modbus: add modbus_reply_router for per-slave dispatch

.gitignore

@@ -48,6 +48,7 @@ tests/random-test-server
 tests/proxy-test-client
 tests/proxy-test-server
 tests/unit-test-client
+tests/unit-test-reply-router
 tests/unit-test-server
 tests/version
 tests/stamp-h2

docs/index.md

@@ -178,11 +178,13 @@ Data mapping:
 Receive:
 
 - [modbus_receive](modbus_receive.md)
+- [modbus_get_request_slave](modbus_get_request_slave.md)
 
 Reply:
 
 - [modbus_reply](modbus_reply.md)
 - [modbus_reply_exception](modbus_reply_exception.md)
+- [modbus_reply_router](modbus_reply_router.md)
 
 Proxy:
 

docs/modbus_get_request_slave.md

@@ -0,0 +1,28 @@
+# modbus_get_request_slave
+
+## Name
+
+modbus_get_request_slave - get the unit identifier addressed by a request
+
+## Synopsis
+
+```c
+int modbus_get_request_slave(modbus_t *ctx, const uint8_t *req);
+```
+
+## Description
+
+The *modbus_get_request_slave()* function shall return the Modbus unit identifier
+(slave) addressed by the request or indication *req*. The identifier is located
+just before the function code, at the end of the backend header (offset 0 in RTU,
+6 in TCP). This is useful to dispatch an indication to a per-slave data mapping.
+
+## Return value
+
+The *modbus_get_request_slave()* function shall return the unit identifier.
+Otherwise it shall return -1 and set errno to EINVAL if *ctx* or *req* is NULL.
+
+## See also
+
+- [modbus_reply_router](modbus_reply_router.md)
+- [modbus_receive](modbus_receive.md)

docs/modbus_reply_router.md

@@ -0,0 +1,57 @@
+# modbus_reply_router
+
+## Name
+
+modbus_reply_router - reply to an indication using a per-slave data mapping
+
+## Synopsis
+
+```c
+typedef modbus_mapping_t *(*modbus_mapping_resolver_t)(int slave, void *user);
+
+int modbus_reply_router(modbus_t *ctx, const uint8_t *req, int req_length, modbus_mapping_resolver_t resolve, void *user);
+```
+
+## Description
+
+The *modbus_reply_router()* function shall answer the indication *req* of length
+*req_length* using the data mapping returned by the *resolve* callback for the
+addressed unit identifier. It is a convenience over
+[modbus_reply](modbus_reply.md) for a server that handles several slaves on one
+context, each backed by its own mapping.
+
+The *resolve* callback receives the unit identifier and the opaque *user* pointer
+passed to *modbus_reply_router()*, and shall return the *modbus_mapping_t* serving
+that slave, or NULL if the slave is not served. When it returns NULL, a gateway
+path exception (MODBUS_EXCEPTION_GATEWAY_PATH) is sent so the client learns the
+unit is unavailable.
+
+## Return value
+
+The *modbus_reply_router()* function shall return the length of the response sent
+if successful. Otherwise it shall return -1 and set errno.
+
+## Example
+
+```c
+static modbus_mapping_t *resolve(int slave, void *user)
+{
+    modbus_mapping_t **maps = user; /* indexed by unit id */
+    return maps[slave];
+}
+
+for (;;) {
+    uint8_t req[MODBUS_TCP_MAX_ADU_LENGTH];
+    int rc = modbus_receive(ctx, req);
+    if (rc > 0) {
+        modbus_reply_router(ctx, req, rc, resolve, maps);
+    }
+}
+```
+
+## See also
+
+- [modbus_reply](modbus_reply.md)
+- [modbus_get_request_slave](modbus_get_request_slave.md)
+- [modbus_mapping_new](modbus_mapping_new.md)
+- [modbus_receive](modbus_receive.md)

src/modbus.c

@@ -1297,6 +1297,52 @@ int modbus_reply_exception(modbus_t *ctx, const uint8_t *req, unsigned int excep
     }
 }
 
+/* Return the unit identifier (slave) addressed by a request or indication.
+   The identifier sits just before the function code, at the end of the backend
+   header (offset 0 in RTU, 6 in TCP). */
+int modbus_get_request_slave(modbus_t *ctx, const uint8_t *req)
+{
+    if (ctx == NULL || req == NULL) {
+        errno = EINVAL;
+        return -1;
+    }
+
+    return req[ctx->backend->header_length - 1];
+}
+
+/* Reply to an indication using the mapping returned by `resolve` for the
+   addressed unit identifier. This is a convenience over modbus_reply() for a
+   server that handles several slaves, each with its own mapping. When `resolve`
+   returns NULL, a gateway path exception is sent so the client learns the unit
+   is unavailable. */
+int modbus_reply_router(modbus_t *ctx,
+                        const uint8_t *req,
+                        int req_length,
+                        modbus_mapping_resolver_t resolve,
+                        void *user)
+{
+    int slave;
+    modbus_mapping_t *mb_mapping;
+
+    if (ctx == NULL || req == NULL || resolve == NULL) {
+        errno = EINVAL;
+        return -1;
+    }
+
+    if (req_length < (int) (ctx->backend->header_length + 1)) {
+        errno = EMBBADDATA;
+        return -1;
+    }
+
+    slave = req[ctx->backend->header_length - 1];
+    mb_mapping = resolve(slave, user);
+    if (mb_mapping == NULL) {
+        return modbus_reply_exception(ctx, req, MODBUS_EXCEPTION_GATEWAY_PATH);
+    }
+
+    return modbus_reply(ctx, req, req_length, mb_mapping);
+}
+
 /* Forward a request received on one context to another and relay the response back.
    This function is useful to implement a Modbus gateway/proxy that bridges
    two different backends (eg. TCP to RTU). */

src/modbus.h

@@ -276,6 +276,15 @@ MODBUS_API int modbus_reply(modbus_t *ctx,
                             modbus_mapping_t *mb_mapping);
 MODBUS_API int
 modbus_reply_exception(modbus_t *ctx, const uint8_t *req, unsigned int exception_code);
+
+/* Resolve the data mapping serving a unit identifier, or NULL if none. */
+typedef modbus_mapping_t *(*modbus_mapping_resolver_t)(int slave, void *user);
+MODBUS_API int modbus_reply_router(modbus_t *ctx,
+                                   const uint8_t *req,
+                                   int req_length,
+                                   modbus_mapping_resolver_t resolve,
+                                   void *user);
+MODBUS_API int modbus_get_request_slave(modbus_t *ctx, const uint8_t *req);
 MODBUS_API int modbus_proxy(modbus_t *frontend_ctx,
                             modbus_t *backend_ctx,
                             const uint8_t *req,

tests/Makefile.am

@@ -8,6 +8,7 @@ noinst_PROGRAMS = \
 	random-test-client \
 	unit-test-server \
 	unit-test-client \
+	unit-test-reply-router \
 	proxy-test-server \
 	proxy-test-client \
 	version
@@ -36,6 +37,9 @@ unit_test_server_LDADD = $(common_ldflags)
 unit_test_client_SOURCES = unit-test-client.c unit-test.h
 unit_test_client_LDADD = $(common_ldflags)
 
+unit_test_reply_router_SOURCES = unit-test-reply-router.c
+unit_test_reply_router_LDADD = $(common_ldflags)
+
 proxy_test_server_SOURCES = proxy-test-server.c
 proxy_test_server_LDADD = $(common_ldflags)
 
@@ -57,4 +61,4 @@ AM_CFLAGS = $(LIBMODBUSCFLAGS) $(WARNING_CFLAGS)
 CLEANFILES = *~ *.log
 
 noinst_SCRIPTS=unit-tests.sh
-TESTS=./unit-tests.sh
+TESTS=./unit-tests.sh unit-test-reply-router