Peter M. Groen / libmodbus

Commit a0b122545d625b51814c32b998ab54b23d806820

Authored by Stéphane Raimbault
1 parent 811d1349

Add missing invalid function exception

Inline Side-by-side
Showing 1 changed file with 44 additions and 42 deletions
src/modbus.c
  View file @a0b1225
... ... @@ -920,7 +920,7 @@ int modbus_reply(modbus_t *ctx, const uint8_t *req,
920 920 int function = req[offset];
921 921 uint16_t address = (req[offset + 1] << 8) + req[offset + 2];
922 922 uint8_t rsp[MAX_MESSAGE_LENGTH];
923   - int resp_length = 0;
  923 + int rsp_length = 0;
924 924 sft_t sft;
925 925  
926 926 /* Filter on the Modbus unit identifier (slave) in RTU mode */
... ... @@ -952,15 +952,15 @@ int modbus_reply(modbus_t *ctx, const uint8_t *req,
952 952 fprintf(stderr, "Illegal data address %0X in read_bits\n",
953 953 address + nb);
954 954 }
955   - resp_length = response_exception(
  955 + rsp_length = response_exception(
956 956 ctx, &sft,
957 957 MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp);
958 958 } else {
959   - resp_length = build_response_basis(ctx, &sft, rsp);
960   - rsp[resp_length++] = (nb / 8) + ((nb % 8) ? 1 : 0);
961   - resp_length = response_io_status(address, nb,
962   - mb_mapping->tab_bits,
963   - rsp, resp_length);
  959 + rsp_length = build_response_basis(ctx, &sft, rsp);
  960 + rsp[rsp_length++] = (nb / 8) + ((nb % 8) ? 1 : 0);
  961 + rsp_length = response_io_status(address, nb,
  962 + mb_mapping->tab_bits,
  963 + rsp, rsp_length);
964 964 }
965 965 }
966 966 break;
... ... @@ -974,15 +974,15 @@ int modbus_reply(modbus_t *ctx, const uint8_t *req,
974 974 fprintf(stderr, "Illegal data address %0X in read_input_bits\n",
975 975 address + nb);
976 976 }
977   - resp_length = response_exception(
  977 + rsp_length = response_exception(
978 978 ctx, &sft,
979 979 MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp);
980 980 } else {
981   - resp_length = build_response_basis(ctx, &sft, rsp);
982   - rsp[resp_length++] = (nb / 8) + ((nb % 8) ? 1 : 0);
983   - resp_length = response_io_status(address, nb,
984   - mb_mapping->tab_input_bits,
985   - rsp, resp_length);
  981 + rsp_length = build_response_basis(ctx, &sft, rsp);
  982 + rsp[rsp_length++] = (nb / 8) + ((nb % 8) ? 1 : 0);
  983 + rsp_length = response_io_status(address, nb,
  984 + mb_mapping->tab_input_bits,
  985 + rsp, rsp_length);
986 986 }
987 987 }
988 988 break;
... ... @@ -994,17 +994,17 @@ int modbus_reply(modbus_t *ctx, const uint8_t *req,
994 994 fprintf(stderr, "Illegal data address %0X in read_registers\n",
995 995 address + nb);
996 996 }
997   - resp_length = response_exception(
  997 + rsp_length = response_exception(
998 998 ctx, &sft,
999 999 MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp);
1000 1000 } else {
1001 1001 int i;
1002 1002  
1003   - resp_length = build_response_basis(ctx, &sft, rsp);
1004   - rsp[resp_length++] = nb << 1;
  1003 + rsp_length = build_response_basis(ctx, &sft, rsp);
  1004 + rsp[rsp_length++] = nb << 1;
1005 1005 for (i = address; i < address + nb; i++) {
1006   - rsp[resp_length++] = mb_mapping->tab_registers[i] >> 8;
1007   - rsp[resp_length++] = mb_mapping->tab_registers[i] & 0xFF;
  1006 + rsp[rsp_length++] = mb_mapping->tab_registers[i] >> 8;
  1007 + rsp[rsp_length++] = mb_mapping->tab_registers[i] & 0xFF;
1008 1008 }
1009 1009 }
1010 1010 }
... ... @@ -1019,17 +1019,17 @@ int modbus_reply(modbus_t *ctx, const uint8_t *req,
1019 1019 fprintf(stderr, "Illegal data address %0X in read_input_registers\n",
1020 1020 address + nb);
1021 1021 }
1022   - resp_length = response_exception(
  1022 + rsp_length = response_exception(
1023 1023 ctx, &sft,
1024 1024 MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp);
1025 1025 } else {
1026 1026 int i;
1027 1027  
1028   - resp_length = build_response_basis(ctx, &sft, rsp);
1029   - rsp[resp_length++] = nb << 1;
  1028 + rsp_length = build_response_basis(ctx, &sft, rsp);
  1029 + rsp[rsp_length++] = nb << 1;
1030 1030 for (i = address; i < address + nb; i++) {
1031   - rsp[resp_length++] = mb_mapping->tab_input_registers[i] >> 8;
1032   - rsp[resp_length++] = mb_mapping->tab_input_registers[i] & 0xFF;
  1031 + rsp[rsp_length++] = mb_mapping->tab_input_registers[i] >> 8;
  1032 + rsp[rsp_length++] = mb_mapping->tab_input_registers[i] & 0xFF;
1033 1033 }
1034 1034 }
1035 1035 }
... ... @@ -1040,7 +1040,7 @@ int modbus_reply(modbus_t *ctx, const uint8_t *req,
1040 1040 fprintf(stderr, "Illegal data address %0X in write_bit\n",
1041 1041 address);
1042 1042 }
1043   - resp_length = response_exception(
  1043 + rsp_length = response_exception(
1044 1044 ctx, &sft,
1045 1045 MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp);
1046 1046 } else {
... ... @@ -1054,14 +1054,14 @@ int modbus_reply(modbus_t *ctx, const uint8_t *req,
1054 1054 CRC will be same and optimisation is
1055 1055 possible here (FIXME). */
1056 1056 memcpy(rsp, req, req_length);
1057   - resp_length = req_length;
  1057 + rsp_length = req_length;
1058 1058 } else {
1059 1059 if (ctx->debug) {
1060 1060 fprintf(stderr,
1061 1061 "Illegal data value %0X in write_bit request at address %0X\n",
1062 1062 data, address);
1063 1063 }
1064   - resp_length = response_exception(
  1064 + rsp_length = response_exception(
1065 1065 ctx, &sft,
1066 1066 MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE, rsp);
1067 1067 }
... ... @@ -1073,7 +1073,7 @@ int modbus_reply(modbus_t *ctx, const uint8_t *req,
1073 1073 fprintf(stderr, "Illegal data address %0X in write_register\n",
1074 1074 address);
1075 1075 }
1076   - resp_length = response_exception(
  1076 + rsp_length = response_exception(
1077 1077 ctx, &sft,
1078 1078 MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp);
1079 1079 } else {
... ... @@ -1081,7 +1081,7 @@ int modbus_reply(modbus_t *ctx, const uint8_t *req,
1081 1081  
1082 1082 mb_mapping->tab_registers[address] = data;
1083 1083 memcpy(rsp, req, req_length);
1084   - resp_length = req_length;
  1084 + rsp_length = req_length;
1085 1085 }
1086 1086 break;
1087 1087 case FC_WRITE_MULTIPLE_COILS: {
... ... @@ -1092,17 +1092,17 @@ int modbus_reply(modbus_t *ctx, const uint8_t *req,
1092 1092 fprintf(stderr, "Illegal data address %0X in write_bits\n",
1093 1093 address + nb);
1094 1094 }
1095   - resp_length = response_exception(
  1095 + rsp_length = response_exception(
1096 1096 ctx, &sft,
1097 1097 MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp);
1098 1098 } else {
1099 1099 /* 6 = byte count */
1100 1100 modbus_set_bits_from_bytes(mb_mapping->tab_bits, address, nb, &req[offset + 6]);
1101 1101  
1102   - resp_length = build_response_basis(ctx, &sft, rsp);
  1102 + rsp_length = build_response_basis(ctx, &sft, rsp);
1103 1103 /* 4 to copy the bit address (2) and the quantity of bits */
1104   - memcpy(rsp + resp_length, req + resp_length, 4);
1105   - resp_length += 4;
  1104 + memcpy(rsp + rsp_length, req + rsp_length, 4);
  1105 + rsp_length += 4;
1106 1106 }
1107 1107 }
1108 1108 break;
... ... @@ -1114,7 +1114,7 @@ int modbus_reply(modbus_t *ctx, const uint8_t *req,
1114 1114 fprintf(stderr, "Illegal data address %0X in write_registers\n",
1115 1115 address + nb);
1116 1116 }
1117   - resp_length = response_exception(
  1117 + rsp_length = response_exception(
1118 1118 ctx, &sft,
1119 1119 MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp);
1120 1120 } else {
... ... @@ -1125,20 +1125,20 @@ int modbus_reply(modbus_t *ctx, const uint8_t *req,
1125 1125 (req[offset + j] << 8) + req[offset + j + 1];
1126 1126 }
1127 1127  
1128   - resp_length = build_response_basis(ctx, &sft, rsp);
  1128 + rsp_length = build_response_basis(ctx, &sft, rsp);
1129 1129 /* 4 to copy the address (2) and the no. of registers */
1130   - memcpy(rsp + resp_length, req + resp_length, 4);
1131   - resp_length += 4;
  1130 + memcpy(rsp + rsp_length, req + rsp_length, 4);
  1131 + rsp_length += 4;
1132 1132 }
1133 1133 }
1134 1134 break;
1135 1135 case FC_REPORT_SLAVE_ID:
1136   - resp_length = build_response_basis(ctx, &sft, rsp);
  1136 + rsp_length = build_response_basis(ctx, &sft, rsp);
1137 1137 /* 2 bytes */
1138   - rsp[resp_length++] = 2;
1139   - rsp[resp_length++] = ctx->slave;
  1138 + rsp[rsp_length++] = 2;
  1139 + rsp[rsp_length++] = ctx->slave;
1140 1140 /* Slave is ON */
1141   - rsp[resp_length++] = 0xFF;
  1141 + rsp[rsp_length++] = 0xFF;
1142 1142 break;
1143 1143 case FC_READ_EXCEPTION_STATUS:
1144 1144 if (ctx->debug) {
... ... @@ -1148,11 +1148,13 @@ int modbus_reply(modbus_t *ctx, const uint8_t *req,
1148 1148 return -1;
1149 1149 break;
1150 1150 default:
1151   - /* FIXME Invalid function exception */
  1151 + rsp_length = response_exception(ctx, &sft,
  1152 + MODBUS_EXCEPTION_ILLEGAL_FUNCTION,
  1153 + rsp);
1152 1154 break;
1153 1155 }
1154 1156  
1155   - return send_msg(ctx, rsp, resp_length);
  1157 + return send_msg(ctx, rsp, rsp_length);
1156 1158 }
1157 1159  
1158 1160 /* Reads IO status */
... ...