diff --git a/include/zephyr/modbus/modbus.h b/include/zephyr/modbus/modbus.h index 45fc6fd0f21..1fba0ac669d 100644 --- a/include/zephyr/modbus/modbus.h +++ b/include/zephyr/modbus/modbus.h @@ -31,7 +31,7 @@ #define ZEPHYR_INCLUDE_MODBUS_H_ #include - +#include #ifdef __cplusplus extern "C" { #endif @@ -41,6 +41,22 @@ extern "C" { /** Length of MBAP Header plus function code */ #define MODBUS_MBAP_AND_FC_LENGTH (MODBUS_MBAP_LENGTH + 1) +/** @name Modbus exception codes + * @{ + */ +/* Modbus exception codes */ +#define MODBUS_EXC_NONE 0 +#define MODBUS_EXC_ILLEGAL_FC 1 +#define MODBUS_EXC_ILLEGAL_DATA_ADDR 2 +#define MODBUS_EXC_ILLEGAL_DATA_VAL 3 +#define MODBUS_EXC_SERVER_DEVICE_FAILURE 4 +#define MODBUS_EXC_ACK 5 +#define MODBUS_EXC_SERVER_DEVICE_BUSY 6 +#define MODBUS_EXC_MEM_PARITY_ERROR 8 +#define MODBUS_EXC_GW_PATH_UNAVAILABLE 10 +#define MODBUS_EXC_GW_TARGET_FAILED_TO_RESP 11 +/** @} */ + /** * @brief Frame struct used internally and for raw ADU support. */ @@ -384,6 +400,57 @@ int modbus_iface_get_by_name(const char *iface_name); typedef int (*modbus_raw_cb_t)(const int iface, const struct modbus_adu *adu, void *user_data); +/** + * @brief Custom function code handler function signature. + * + * Modbus allows user defined function codes which can be used to extend + * the base protocol. These callbacks can also be used to implement + * function codes currently not supported by Zephyr's Modbus subsystem. + * + * If an error occurs during the handling of the request, the handler should + * signal this by setting excep_code to a modbus exception code. + * + * User data pointer can be used to pass state between subsequent calls to + * the handler. + * + * @param iface Modbus interface index + * @param rx_adu Pointer to the received ADU struct + * @param tx_adu Pointer to the outgoing ADU struct + * @param excep_code Pointer to possible exception code + * @param user_data Pointer to user data + * + * @retval true If response should be sent, false otherwise + */ +typedef bool (*modbus_custom_cb_t)(const int iface, + const struct modbus_adu *const rx_adu, + struct modbus_adu *const tx_adu, + uint8_t *const excep_code, + void *const user_data); + +/** @cond INTERNAL_HIDDEN */ +/** + * @brief Custom function code definition. + */ +struct modbus_custom_fc { + sys_snode_t node; + modbus_custom_cb_t cb; + void *user_data; + uint8_t fc; + uint8_t excep_code; +}; +/** @endcond INTERNAL_HIDDEN */ + +/** + * @brief Helper macro for initializing custom function code structs + */ +#define MODBUS_CUSTOM_FC_DEFINE(name, user_cb, user_fc, userdata) \ + static struct modbus_custom_fc modbus_cfg_##name = { \ + .cb = user_cb, \ + .user_data = userdata, \ + .fc = user_fc, \ + .excep_code = MODBUS_EXC_NONE, \ + } + /** * @brief Modbus interface mode */ @@ -535,6 +602,24 @@ void modbus_raw_set_server_failure(struct modbus_adu *adu); */ int modbus_raw_backend_txn(const int iface, struct modbus_adu *adu); +/** + * @brief Register a user-defined function code handler. + * + * The Modbus specification allows users to define standard function codes + * missing from Zephyr's Modbus implementation as well as add non-standard + * function codes in the ranges 65 to 72 and 100 to 110 (decimal), as per + * specification. + * + * This function registers a new handler at runtime for the given + * function code. + * + * @param iface Modbus client interface index + * @param custom_fc User defined function code and callback pair + * + * @retval 0 on success + */ +int modbus_register_user_fc(const int iface, struct modbus_custom_fc *custom_fc); + #ifdef __cplusplus } #endif diff --git a/subsys/modbus/modbus_core.c b/subsys/modbus/modbus_core.c index 617f5833a40..777887f2275 100644 --- a/subsys/modbus/modbus_core.c +++ b/subsys/modbus/modbus_core.c @@ -211,6 +211,14 @@ static struct modbus_context *modbus_init_iface(const uint8_t iface) return ctx; } +static int modbus_user_fc_init(struct modbus_context *ctx, struct modbus_iface_param param) +{ + sys_slist_init(&ctx->user_defined_cbs); + LOG_DBG("Initializing user-defined function code support."); + + return 0; +} + int modbus_init_server(const int iface, struct modbus_iface_param param) { struct modbus_context *ctx = NULL; @@ -236,6 +244,12 @@ int modbus_init_server(const int iface, struct modbus_iface_param param) ctx->client = false; + if (modbus_user_fc_init(ctx, param) != 0) { + LOG_ERR("Failed to init MODBUS user defined function codes"); + rc = -EINVAL; + goto init_server_error; + } + switch (param.mode) { case MODBUS_MODE_RTU: case MODBUS_MODE_ASCII: @@ -278,6 +292,28 @@ init_server_error: return rc; } +int modbus_register_user_fc(const int iface, struct modbus_custom_fc *custom_fc) +{ + struct modbus_context *ctx = modbus_get_context(iface); + + if (!custom_fc) { + LOG_ERR("Provided function code handler was NULL"); + return -EINVAL; + } + + if (custom_fc->fc & BIT(7)) { + LOG_ERR("Function codes must have MSB of 0"); + return -EINVAL; + } + + custom_fc->excep_code = MODBUS_EXC_NONE; + + LOG_DBG("Registered new custom function code %d", custom_fc->fc); + sys_slist_append(&ctx->user_defined_cbs, &custom_fc->node); + + return 0; +} + int modbus_init_client(const int iface, struct modbus_iface_param param) { struct modbus_context *ctx = NULL; diff --git a/subsys/modbus/modbus_internal.h b/subsys/modbus/modbus_internal.h index f271e08941c..080da58ea09 100644 --- a/subsys/modbus/modbus_internal.h +++ b/subsys/modbus/modbus_internal.h @@ -55,18 +55,6 @@ #define MODBUS_FC08_SUBF_SERVER_MSG_CTR 14 #define MODBUS_FC08_SUBF_SERVER_NO_RESP_CTR 15 -/* Modbus exception codes */ -#define MODBUS_EXC_NONE 0 -#define MODBUS_EXC_ILLEGAL_FC 1 -#define MODBUS_EXC_ILLEGAL_DATA_ADDR 2 -#define MODBUS_EXC_ILLEGAL_DATA_VAL 3 -#define MODBUS_EXC_SERVER_DEVICE_FAILURE 4 -#define MODBUS_EXC_ACK 5 -#define MODBUS_EXC_SERVER_DEVICE_BUSY 6 -#define MODBUS_EXC_MEM_PARITY_ERROR 8 -#define MODBUS_EXC_GW_PATH_UNAVAILABLE 10 -#define MODBUS_EXC_GW_TARGET_FAILED_TO_RESP 11 - /* Modbus RTU (ASCII) constants */ #define MODBUS_COIL_OFF_CODE 0x0000 #define MODBUS_COIL_ON_CODE 0xFF00 @@ -142,6 +130,8 @@ struct modbus_context { uint16_t mbs_server_msg_ctr; uint16_t mbs_noresp_ctr; #endif + /* A linked list of function code, handler pairs */ + sys_slist_t user_defined_cbs; /* Unit ID */ uint8_t unit_id; diff --git a/subsys/modbus/modbus_server.c b/subsys/modbus/modbus_server.c index 58a783864c6..4805526b2d6 100644 --- a/subsys/modbus/modbus_server.c +++ b/subsys/modbus/modbus_server.c @@ -926,6 +926,38 @@ static bool mbs_fc16_hregs_write(struct modbus_context *ctx) return true; } +static bool mbs_try_user_fc(struct modbus_context *ctx, uint8_t fc) +{ + struct modbus_custom_fc *p; + + LOG_DBG("Searching for custom Modbus handlers for code %u", fc); + + SYS_SLIST_FOR_EACH_CONTAINER(&ctx->user_defined_cbs, p, node) { + if (p->fc == fc) { + int iface = modbus_iface_get_by_ctx(ctx); + bool rval; + + LOG_DBG("Found custom handler"); + + p->excep_code = MODBUS_EXC_NONE; + rval = p->cb(iface, &ctx->rx_adu, &ctx->tx_adu, &p->excep_code, + p->user_data); + + if (p->excep_code != MODBUS_EXC_NONE) { + LOG_INF("Custom handler failed with code %d", p->excep_code); + mbs_exception_rsp(ctx, p->excep_code); + } + + return rval; + } + } + + LOG_ERR("Function code 0x%02x not implemented", fc); + mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_FC); + + return true; +} + bool modbus_server_handler(struct modbus_context *ctx) { bool send_reply = false; @@ -945,6 +977,7 @@ bool modbus_server_handler(struct modbus_context *ctx) } if (addr != 0 && addr != ctx->unit_id) { + LOG_DBG("Unit ID doesn't match %u != %u", addr, ctx->unit_id); update_noresp_ctr(ctx); return false; } @@ -995,10 +1028,7 @@ bool modbus_server_handler(struct modbus_context *ctx) break; default: - LOG_ERR("Function code 0x%02x not implemented", fc); - mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_FC); - send_reply = true; - break; + send_reply = mbs_try_user_fc(ctx, fc); } if (addr == 0) {