From b4330d2143163bd92bbb10ac3b0705eb521fe3c3 Mon Sep 17 00:00:00 2001
From: Stéphane Raimbault <stephane.raimbault@gmail.com>
Date: Tue, 10 May 2011 12:31:27 +0200
Subject: [PATCH] Add check on transaction ID for Modbus TCP

---
 src/modbus-private.h |  2 ++
 src/modbus-rtu.c     |  1 +
 src/modbus-tcp.c     | 20 +++++++++++++++++++-
 src/modbus.c         |  8 ++++++++
 4 files changed, 30 insertions(+), 1 deletion(-)

diff --git a/src/modbus-private.h b/src/modbus-private.h
index 395282f..01f861b 100644
--- a/src/modbus-private.h
+++ b/src/modbus-private.h
@@ -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);
diff --git a/src/modbus-rtu.c b/src/modbus-rtu.c
index a21ea28..983e9c1 100644
--- a/src/modbus-rtu.c
+++ b/src/modbus-rtu.c
@@ -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,
diff --git a/src/modbus-tcp.c b/src/modbus-tcp.c
index eb42f49..0db9c9e 100644
--- a/src/modbus-tcp.c
+++ b/src/modbus-tcp.c
@@ -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,
diff --git a/src/modbus.c b/src/modbus.c
index 51d6d73..131b903 100644
--- a/src/modbus.c
+++ b/src/modbus.c
@@ -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 */
--
libgit2 0.21.4