address check in single register / coil responses added (#463)

Address check in single register / coil responses added According to Modbus standard the address of single register / coils request and response must match Co-authored-by: Heinrich Gsponer <heinrich.gsponer@siemens.com>
stephane · Jul 19, 2022 · cfeca26 · cfeca26
1 parent 27b90de
commit cfeca26
Showing 1 changed file with 22 additions and 2 deletions.
diff --git a/src/modbus.c b/src/modbus.c
@@ -555,6 +555,8 @@ static int check_confirmation(modbus_t *ctx, uint8_t *req,
         function < 0x80) {
         int req_nb_value;
         int rsp_nb_value;
+        int resp_addr_ok = TRUE;
+        int resp_data_ok = TRUE;
 
         /* Check function code */
         if (function != req[offset]) {
@@ -591,6 +593,10 @@ static int check_confirmation(modbus_t *ctx, uint8_t *req,
             break;
         case MODBUS_FC_WRITE_MULTIPLE_COILS:
         case MODBUS_FC_WRITE_MULTIPLE_REGISTERS:
+            /* address in request and response must be equal */
+            if ((req[offset + 1] != rsp[offset + 1]) || (req[offset + 2] != rsp[offset + 2])) {
+                resp_addr_ok = FALSE;
+            }
             /* N Write functions */
             req_nb_value = (req[offset + 3] << 8) + req[offset + 4];
             rsp_nb_value = (rsp[offset + 3] << 8) | rsp[offset + 4];
@@ -599,17 +605,31 @@ static int check_confirmation(modbus_t *ctx, uint8_t *req,
             /* Report slave ID (bytes received) */
             req_nb_value = rsp_nb_value = rsp[offset + 1];
             break;
+        case MODBUS_FC_WRITE_SINGLE_COIL:
+        case MODBUS_FC_WRITE_SINGLE_REGISTER:
+            /* address in request and response must be equal */
+            if ((req[offset + 1] != rsp[offset + 1]) || (req[offset + 2] != rsp[offset + 2])) {
+                resp_addr_ok = FALSE;
+            }
+            /* data in request and response must be equal */
+            if ((req[offset + 3] != rsp[offset + 3]) || (req[offset + 4] != rsp[offset + 4])) {
+                resp_data_ok = FALSE;
+            }
+            /* 1 Write functions & others */
+            req_nb_value = rsp_nb_value = 1;
+            break;
         default:
             /* 1 Write functions & others */
             req_nb_value = rsp_nb_value = 1;
+            break;
         }
 
-        if (req_nb_value == rsp_nb_value) {
+        if ((req_nb_value == rsp_nb_value) && (resp_addr_ok == TRUE) && (resp_data_ok == TRUE)) {
             rc = rsp_nb_value;
         } else {
             if (ctx->debug) {
                 fprintf(stderr,
-                        "Quantity not corresponding to the request (%d != %d)\n",
+                        "Received data not corresponding to the request (%d != %d)\n",
                         rsp_nb_value, req_nb_value);
             }