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,7 +920,7 @@ int modbus_reply(modbus_t *ctx, const uint8_t *req,
920 int function = req[offset]; 920 int function = req[offset];
921 uint16_t address = (req[offset + 1] << 8) + req[offset + 2]; 921 uint16_t address = (req[offset + 1] << 8) + req[offset + 2];
922 uint8_t rsp[MAX_MESSAGE_LENGTH]; 922 uint8_t rsp[MAX_MESSAGE_LENGTH];
923 - int resp_length = 0; 923 + int rsp_length = 0;
924 sft_t sft; 924 sft_t sft;
925 925
926 /* Filter on the Modbus unit identifier (slave) in RTU mode */ 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,15 +952,15 @@ int modbus_reply(modbus_t *ctx, const uint8_t *req,
952 fprintf(stderr, "Illegal data address %0X in read_bits\n", 952 fprintf(stderr, "Illegal data address %0X in read_bits\n",
953 address + nb); 953 address + nb);
954 } 954 }
955 - resp_length = response_exception( 955 + rsp_length = response_exception(
956 ctx, &sft, 956 ctx, &sft,
957 MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp); 957 MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp);
958 } else { 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 break; 966 break;
@@ -974,15 +974,15 @@ int modbus_reply(modbus_t *ctx, const uint8_t *req, @@ -974,15 +974,15 @@ int modbus_reply(modbus_t *ctx, const uint8_t *req,
974 fprintf(stderr, "Illegal data address %0X in read_input_bits\n", 974 fprintf(stderr, "Illegal data address %0X in read_input_bits\n",
975 address + nb); 975 address + nb);
976 } 976 }
977 - resp_length = response_exception( 977 + rsp_length = response_exception(
978 ctx, &sft, 978 ctx, &sft,
979 MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp); 979 MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp);
980 } else { 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 break; 988 break;
@@ -994,17 +994,17 @@ int modbus_reply(modbus_t *ctx, const uint8_t *req, @@ -994,17 +994,17 @@ int modbus_reply(modbus_t *ctx, const uint8_t *req,
994 fprintf(stderr, "Illegal data address %0X in read_registers\n", 994 fprintf(stderr, "Illegal data address %0X in read_registers\n",
995 address + nb); 995 address + nb);
996 } 996 }
997 - resp_length = response_exception( 997 + rsp_length = response_exception(
998 ctx, &sft, 998 ctx, &sft,
999 MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp); 999 MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp);
1000 } else { 1000 } else {
1001 int i; 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 for (i = address; i < address + nb; i++) { 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,17 +1019,17 @@ int modbus_reply(modbus_t *ctx, const uint8_t *req,
1019 fprintf(stderr, "Illegal data address %0X in read_input_registers\n", 1019 fprintf(stderr, "Illegal data address %0X in read_input_registers\n",
1020 address + nb); 1020 address + nb);
1021 } 1021 }
1022 - resp_length = response_exception( 1022 + rsp_length = response_exception(
1023 ctx, &sft, 1023 ctx, &sft,
1024 MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp); 1024 MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp);
1025 } else { 1025 } else {
1026 int i; 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 for (i = address; i < address + nb; i++) { 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,7 +1040,7 @@ int modbus_reply(modbus_t *ctx, const uint8_t *req,
1040 fprintf(stderr, "Illegal data address %0X in write_bit\n", 1040 fprintf(stderr, "Illegal data address %0X in write_bit\n",
1041 address); 1041 address);
1042 } 1042 }
1043 - resp_length = response_exception( 1043 + rsp_length = response_exception(
1044 ctx, &sft, 1044 ctx, &sft,
1045 MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp); 1045 MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp);
1046 } else { 1046 } else {
@@ -1054,14 +1054,14 @@ int modbus_reply(modbus_t *ctx, const uint8_t *req, @@ -1054,14 +1054,14 @@ int modbus_reply(modbus_t *ctx, const uint8_t *req,
1054 CRC will be same and optimisation is 1054 CRC will be same and optimisation is
1055 possible here (FIXME). */ 1055 possible here (FIXME). */
1056 memcpy(rsp, req, req_length); 1056 memcpy(rsp, req, req_length);
1057 - resp_length = req_length; 1057 + rsp_length = req_length;
1058 } else { 1058 } else {
1059 if (ctx->debug) { 1059 if (ctx->debug) {
1060 fprintf(stderr, 1060 fprintf(stderr,
1061 "Illegal data value %0X in write_bit request at address %0X\n", 1061 "Illegal data value %0X in write_bit request at address %0X\n",
1062 data, address); 1062 data, address);
1063 } 1063 }
1064 - resp_length = response_exception( 1064 + rsp_length = response_exception(
1065 ctx, &sft, 1065 ctx, &sft,
1066 MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE, rsp); 1066 MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE, rsp);
1067 } 1067 }
@@ -1073,7 +1073,7 @@ int modbus_reply(modbus_t *ctx, const uint8_t *req, @@ -1073,7 +1073,7 @@ int modbus_reply(modbus_t *ctx, const uint8_t *req,
1073 fprintf(stderr, "Illegal data address %0X in write_register\n", 1073 fprintf(stderr, "Illegal data address %0X in write_register\n",
1074 address); 1074 address);
1075 } 1075 }
1076 - resp_length = response_exception( 1076 + rsp_length = response_exception(
1077 ctx, &sft, 1077 ctx, &sft,
1078 MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp); 1078 MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp);
1079 } else { 1079 } else {
@@ -1081,7 +1081,7 @@ int modbus_reply(modbus_t *ctx, const uint8_t *req, @@ -1081,7 +1081,7 @@ int modbus_reply(modbus_t *ctx, const uint8_t *req,
1081 1081
1082 mb_mapping->tab_registers[address] = data; 1082 mb_mapping->tab_registers[address] = data;
1083 memcpy(rsp, req, req_length); 1083 memcpy(rsp, req, req_length);
1084 - resp_length = req_length; 1084 + rsp_length = req_length;
1085 } 1085 }
1086 break; 1086 break;
1087 case FC_WRITE_MULTIPLE_COILS: { 1087 case FC_WRITE_MULTIPLE_COILS: {
@@ -1092,17 +1092,17 @@ int modbus_reply(modbus_t *ctx, const uint8_t *req, @@ -1092,17 +1092,17 @@ int modbus_reply(modbus_t *ctx, const uint8_t *req,
1092 fprintf(stderr, "Illegal data address %0X in write_bits\n", 1092 fprintf(stderr, "Illegal data address %0X in write_bits\n",
1093 address + nb); 1093 address + nb);
1094 } 1094 }
1095 - resp_length = response_exception( 1095 + rsp_length = response_exception(
1096 ctx, &sft, 1096 ctx, &sft,
1097 MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp); 1097 MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp);
1098 } else { 1098 } else {
1099 /* 6 = byte count */ 1099 /* 6 = byte count */
1100 modbus_set_bits_from_bytes(mb_mapping->tab_bits, address, nb, &req[offset + 6]); 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 /* 4 to copy the bit address (2) and the quantity of bits */ 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 break; 1108 break;
@@ -1114,7 +1114,7 @@ int modbus_reply(modbus_t *ctx, const uint8_t *req, @@ -1114,7 +1114,7 @@ int modbus_reply(modbus_t *ctx, const uint8_t *req,
1114 fprintf(stderr, "Illegal data address %0X in write_registers\n", 1114 fprintf(stderr, "Illegal data address %0X in write_registers\n",
1115 address + nb); 1115 address + nb);
1116 } 1116 }
1117 - resp_length = response_exception( 1117 + rsp_length = response_exception(
1118 ctx, &sft, 1118 ctx, &sft,
1119 MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp); 1119 MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp);
1120 } else { 1120 } else {
@@ -1125,20 +1125,20 @@ int modbus_reply(modbus_t *ctx, const uint8_t *req, @@ -1125,20 +1125,20 @@ int modbus_reply(modbus_t *ctx, const uint8_t *req,
1125 (req[offset + j] << 8) + req[offset + j + 1]; 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 /* 4 to copy the address (2) and the no. of registers */ 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 break; 1134 break;
1135 case FC_REPORT_SLAVE_ID: 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 /* 2 bytes */ 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 /* Slave is ON */ 1140 /* Slave is ON */
1141 - rsp[resp_length++] = 0xFF; 1141 + rsp[rsp_length++] = 0xFF;
1142 break; 1142 break;
1143 case FC_READ_EXCEPTION_STATUS: 1143 case FC_READ_EXCEPTION_STATUS:
1144 if (ctx->debug) { 1144 if (ctx->debug) {
@@ -1148,11 +1148,13 @@ int modbus_reply(modbus_t *ctx, const uint8_t *req, @@ -1148,11 +1148,13 @@ int modbus_reply(modbus_t *ctx, const uint8_t *req,
1148 return -1; 1148 return -1;
1149 break; 1149 break;
1150 default: 1150 default:
1151 - /* FIXME Invalid function exception */ 1151 + rsp_length = response_exception(ctx, &sft,
  1152 + MODBUS_EXCEPTION_ILLEGAL_FUNCTION,
  1153 + rsp);
1152 break; 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 /* Reads IO status */ 1160 /* Reads IO status */