From a4f7996eafbf701371db7c9fa826987fed119188 Mon Sep 17 00:00:00 2001
From: Stéphane Raimbault <stephane.raimbault@gmail.com>
Date: Wed, 8 Dec 2010 00:09:43 +0100
Subject: [PATCH] New function to reply to an indication with an exception message

---
 src/modbus.c             | 31 +++++++++++++++++++++++++++++++
 src/modbus.h             |  2 ++
 tests/unit-test-client.c | 15 +++++++++++++++
 tests/unit-test-server.c |  5 +++++
 tests/unit-test.h        |  2 ++
 5 files changed, 55 insertions(+), 0 deletions(-)

diff --git a/src/modbus.c b/src/modbus.c
index 52e6ada..3d8756d 100644
--- a/src/modbus.c
+++ b/src/modbus.c
@@ -818,6 +818,37 @@ int modbus_reply(modbus_t *ctx, const uint8_t *req,
     return send_msg(ctx, rsp, rsp_length);
 }
 
+int modbus_reply_exception(modbus_t *ctx, const uint8_t *req,
+                           unsigned int exception_code)
+{
+    int offset = ctx->backend->header_length;
+    int slave = req[offset - 1];
+    int function = req[offset];
+    uint8_t rsp[MAX_MESSAGE_LENGTH];
+    int rsp_length;
+    int dummy_length = 99;
+    sft_t sft;
+
+    if (ctx->backend->filter_request(ctx, slave) == 1) {
+        /* Filtered */
+        return 0;
+    }
+
+    sft.slave = slave;
+    sft.function = function + 0x80;;
+    sft.t_id = ctx->backend->prepare_response_tid(req, &dummy_length);
+    rsp_length = ctx->backend->build_response_basis(&sft, rsp);
+
+    /* Positive exception code */
+    if (exception_code < MODBUS_EXCEPTION_MAX) {
+        rsp[rsp_length++] = exception_code;
+        return send_msg(ctx, rsp, rsp_length);
+    } else {
+        errno = EINVAL;
+        return -1;
+    }
+}
+
 /* Reads IO status */
 static int read_io_status(modbus_t *ctx, int function,
                           int addr, int nb, uint8_t *data_dest)
diff --git a/src/modbus.h b/src/modbus.h
index 37d6018..8c87f4e 100644
--- a/src/modbus.h
+++ b/src/modbus.h
@@ -171,6 +171,8 @@ void modbus_mapping_free(modbus_mapping_t *mb_mapping);
 int modbus_receive(modbus_t *ctx, int sockfd, uint8_t *req);
 int modbus_reply(modbus_t *ctx, const uint8_t *req,
                  int req_length, modbus_mapping_t *mb_mapping);
+int modbus_reply_exception(modbus_t *ctx, const uint8_t *req,
+                           unsigned int exception_code);
 
 /**
  * UTILS FUNCTIONS
diff --git a/tests/unit-test-client.c b/tests/unit-test-client.c
index 0829739..a06a16a 100644
--- a/tests/unit-test-client.c
+++ b/tests/unit-test-client.c
@@ -618,8 +618,23 @@ int main(int argc, char *argv[])
         printf("FAILED\n");
         goto close;
     }
+
     free(tab_rp_registers_bad);
 
+    /** MANUAL EXCEPTION **/
+    printf("\nTEST MANUAL EXCEPTION:\n");
+
+    rc = modbus_read_registers(ctx, UT_REGISTERS_ADDRESS_SPECIAL,
+                               UT_REGISTERS_NB_POINTS,
+                               tab_rp_registers);
+    printf("* modbus_read_registers at special address: ");
+    if (rc == -1 && errno == EMBXSBUSY) {
+        printf("OK\n");
+    } else {
+        printf("FAILED\n");
+        goto close;
+    }
+
     printf("\nALL TESTS PASS WITH SUCCESS.\n");
 
 close:
diff --git a/tests/unit-test-server.c b/tests/unit-test-server.c
index 27cc4b4..773ba35 100644
--- a/tests/unit-test-server.c
+++ b/tests/unit-test-server.c
@@ -119,6 +119,11 @@ int main(int argc, char*argv[])
                 printf("Set an incorrect number of values\n");
                 MODBUS_SET_INT16_TO_INT8(query, header_length + 3,
                                          UT_REGISTERS_NB_POINTS);
+            } else if (MODBUS_GET_INT16_FROM_INT8(query, header_length + 1)
+                == UT_REGISTERS_ADDRESS_SPECIAL) {
+                modbus_reply_exception(ctx, query,
+                                       MODBUS_EXCEPTION_SLAVE_OR_SERVER_BUSY);
+                continue;
             }
         }
 
diff --git a/tests/unit-test.h b/tests/unit-test.h
index e084a8e..e65c08d 100644
--- a/tests/unit-test.h
+++ b/tests/unit-test.h
@@ -38,6 +38,8 @@ const uint16_t UT_INPUT_BITS_NB_POINTS = 0x16;
 const uint8_t UT_INPUT_BITS_TAB[] = { 0xAC, 0xDB, 0x35 };
 
 const uint16_t UT_REGISTERS_ADDRESS = 0x6B;
+/* Raise a manual exception when this adress is used for the first byte */
+const uint16_t UT_REGISTERS_ADDRESS_SPECIAL = 0x6C;
 const uint16_t UT_REGISTERS_NB_POINTS = 0x3;
 const uint16_t UT_REGISTERS_TAB[] = { 0x022B, 0x0000, 0x0064 };
 /* If the following value is used, a bad response is sent.
--
libgit2 0.21.4