Add check on transaction ID for Modbus TCP

Stéphane Raimbault
1 parent 1a45c24d
Showing 4 changed files with 30 additions and 1 deletions
src/modbus-private.h
src/modbus-rtu.c
src/modbus-tcp.c
src/modbus.c
@@ -90,6 +90,8 @@ typedef struct _modbus_backend {
     ssize_t (*recv) (modbus_t *ctx, uint8_t *rsp, int rsp_length);
     int (*check_integrity) (modbus_t *ctx, uint8_t *msg,
                             const int msg_length);
+    int (*pre_check_confirmation) (modbus_t *ctx, const uint8_t *req,
+                                   const uint8_t *rsp, int rsp_length);
     int (*connect) (modbus_t *ctx);
     void (*close) (modbus_t *ctx);
     int (*flush) (modbus_t *ctx);
@@ -812,6 +812,7 @@ const modbus_backend_t _modbus_rtu_backend = {
     _modbus_rtu_send,
     _modbus_rtu_recv,
     _modbus_rtu_check_integrity,
+    NULL,
     _modbus_rtu_connect,
     _modbus_rtu_close,
     _modbus_rtu_flush,
@@ -190,6 +190,22 @@ int _modbus_tcp_check_integrity(modbus_t *ctx, uint8_t *msg, const int msg_lengt
     return msg_length;
 }
+int _modbus_tcp_pre_check_confirmation(modbus_t *ctx, const uint8_t *req,
+                                       const uint8_t *rsp, int rsp_length)
+{
+    /* Check TID */
+    if (req[0] != rsp[0] || req[1] != rsp[1]) {
+        if (ctx->debug) {
+            fprintf(stderr, "Invalid TID received 0x%X (not 0x%X)\n",
+                    (rsp[0] << 8) + rsp[1], (req[0] << 8) + req[1]);
+        }
+        errno = EMBBADDATA;
+        return -1;
+    } else {
+        return 0;
+    }
+}
+
 static int _modbus_tcp_set_ipv4_options(int s)
 {
     int rc;
@@ -352,7 +368,7 @@ int _modbus_tcp_flush(modbus_t *ctx)
         }
 #endif
         if (ctx->debug && rc != -1) {
-            printf("\n%d bytes flushed\n", rc);
+            printf("%d bytes flushed\n", rc);
         }
     } while (rc == MODBUS_TCP_MAX_ADU_LENGTH);
@@ -588,6 +604,7 @@ const modbus_backend_t _modbus_tcp_backend = {
     _modbus_tcp_send,
     _modbus_tcp_recv,
     _modbus_tcp_check_integrity,
+    _modbus_tcp_pre_check_confirmation,
     _modbus_tcp_connect,
     _modbus_tcp_close,
     _modbus_tcp_flush,
@@ -609,6 +626,7 @@ const modbus_backend_t _modbus_tcp_pi_backend = {
     _modbus_tcp_send,
     _modbus_tcp_recv,
     _modbus_tcp_check_integrity,
+    _modbus_tcp_pre_check_confirmation,
     _modbus_tcp_pi_connect,
     _modbus_tcp_close,
     _modbus_tcp_flush,
@@ -440,6 +440,14 @@ static int check_confirmation(modbus_t *ctx, uint8_t *req,
     int rsp_length_computed;
     const int offset = ctx->backend->header_length;
+
+    if (ctx->backend->pre_check_confirmation) {
+        rc = ctx->backend->pre_check_confirmation(ctx, req, rsp, rsp_length);
+        if (rc == -1) {
+            return -1;
+        }
+    }
+
     rsp_length_computed = compute_response_length_from_request(ctx, req);
     /* Check length */