Exception response on report slave ID wasn't detected (closes #27)

Stéphane Raimbault
1 parent 7fed3ff9
Showing 1 changed file with 25 additions and 15 deletions
src/modbus.c
@@ -483,6 +483,7 @@ static int check_confirmation(modbus_t *ctx, uint8_t *req,
     int rc;
     int rsp_length_computed;
     const int offset = ctx->backend->header_length;
+    const int function = rsp[offset];
     if (ctx->backend->pre_check_confirmation) {
         rc = ctx->backend->pre_check_confirmation(ctx, req, rsp, rsp_length);
@@ -496,12 +497,33 @@ static int check_confirmation(modbus_t *ctx, uint8_t *req,
     rsp_length_computed = compute_response_length_from_request(ctx, req);
+    /* Exception code */
+    if (function >= 0x80) {
+        if (rsp_length == (offset + 2 + ctx->backend->checksum_length) &&
+            req[offset] == (rsp[offset] - 0x80)) {
+            /* Valid exception code received */
+
+            int exception_code = rsp[offset + 1];
+            if (exception_code < MODBUS_EXCEPTION_MAX) {
+                errno = MODBUS_ENOBASE + exception_code;
+            } else {
+                errno = EMBBADEXC;
+            }
+            _error_print(ctx, NULL);
+            return -1;
+        } else {
+            errno = EMBBADEXC;
+            _error_print(ctx, NULL);
+            return -1;
+        }
+    }
+
     /* Check length */
-    if (rsp_length == rsp_length_computed ||
-        rsp_length_computed == MSG_LENGTH_UNDEFINED) {
+    if ((rsp_length == rsp_length_computed ||
+         rsp_length_computed == MSG_LENGTH_UNDEFINED) &&
+        function < 0x80) {
         int req_nb_value;
         int rsp_nb_value;
-        const int function = rsp[offset];
         /* Check function code */
         if (function != req[offset]) {
@@ -566,18 +588,6 @@ static int check_confirmation(modbus_t *ctx, uint8_t *req,
             errno = EMBBADDATA;
             rc = -1;
         }
-    } else if (rsp_length == (offset + 2 + ctx->backend->checksum_length) &&
-               req[offset] == (rsp[offset] - 0x80)) {
-        /* EXCEPTION CODE RECEIVED */
-
-        int exception_code = rsp[offset + 1];
-        if (exception_code < MODBUS_EXCEPTION_MAX) {
-            errno = MODBUS_ENOBASE + exception_code;
-        } else {
-            errno = EMBBADEXC;
-        }
-        _error_print(ctx, NULL);
-        rc = -1;
     } else {
         if (ctx->debug) {
             fprintf(stderr,