diff --git a/include/modbus/modbus.h b/include/modbus/modbus.h index 6a8be81fcba..400dc8b9d9e 100644 --- a/include/modbus/modbus.h +++ b/include/modbus/modbus.h @@ -36,6 +36,31 @@ extern "C" { #endif +/** Length of MBAP Header */ +#define MODBUS_MBAP_LENGTH 7 +/** Length of MBAP Header plus function code */ +#define MODBUS_MBAP_AND_FC_LENGTH (MODBUS_MBAP_LENGTH + 1) + +/** + * @brief Frame struct used internally and for raw ADU support. + */ +struct modbus_adu { + /** Transaction Identifier */ + uint16_t trans_id; + /** Protocol Identifier */ + uint16_t proto_id; + /** Length of the data only (not the length of unit ID + PDU) */ + uint16_t length; + /** Unit Identifier */ + uint8_t unit_id; + /** Function Code */ + uint8_t fc; + /** Transaction Data */ + uint8_t data[CONFIG_MODBUS_BUFFER_SIZE - 4]; + /** RTU CRC */ + uint16_t crc; +}; + /** * @brief Coil read (FC01) * @@ -347,6 +372,16 @@ struct modbus_user_callbacks { */ int modbus_iface_get_by_name(const char *iface_name); +/** + * @brief ADU raw callback function signature + * + * @param iface Modbus RTU interface index + * @param adu Pointer to the RAW ADU struct to send + * + * @retval 0 If transfer was successful + */ +typedef int (*modbus_raw_cb_t)(const int iface, const struct modbus_adu *adu); + /** * @brief Modbus interface mode */ @@ -355,6 +390,8 @@ enum modbus_mode { MODBUS_MODE_RTU, /** Modbus over serial line ASCII mode */ MODBUS_MODE_ASCII, + /** Modbus raw ADU mode */ + MODBUS_MODE_RAW, }; /** @@ -398,6 +435,8 @@ struct modbus_iface_param { union { /** Serial support parameter of the interface */ struct modbus_serial_param serial; + /** Pointer to raw ADU callback function */ + modbus_raw_cb_t raw_tx_cb; }; }; @@ -432,6 +471,60 @@ int modbus_init_client(const int iface, struct modbus_iface_param param); */ int modbus_disable(const uint8_t iface); +/** + * @brief Submit raw ADU + * + * @param iface Modbus RTU interface index + * @param adu Pointer to the RAW ADU struct that is received + * + * @retval 0 If transfer was successful + */ +int modbus_raw_submit_rx(const int iface, const struct modbus_adu *adu); + +/** + * @brief Put MBAP header into a buffer + * + * @param adu Pointer to the RAW ADU struct + * @param header Pointer to the buffer in which MBAP header + * will be placed. + * + * @retval 0 If transfer was successful + */ +void modbus_raw_put_header(const struct modbus_adu *adu, uint8_t *header); + +/** + * @brief Get MBAP header from a buffer + * + * @param adu Pointer to the RAW ADU struct + * @param header Pointer to the buffer containing MBAP header + * + * @retval 0 If transfer was successful + */ +void modbus_raw_get_header(struct modbus_adu *adu, const uint8_t *header); + +/** + * @brief Set Server Device Failure exception + * + * This function modifies ADU passed by the pointer. + * + * @param adu Pointer to the RAW ADU struct + */ +void modbus_raw_set_server_failure(struct modbus_adu *adu); + +/** + * @brief Use interface as backend to send and receive ADU + * + * This function overwrites ADU passed by the pointer and generates + * exception responses if backend interface is misconfigured or + * target device is unreachable. + * + * @param iface Modbus client interface index + * @param adu Pointer to the RAW ADU struct + * + * @retval 0 If transfer was successful + */ +int modbus_raw_backend_txn(const int iface, struct modbus_adu *adu); + #ifdef __cplusplus } #endif diff --git a/subsys/modbus/CMakeLists.txt b/subsys/modbus/CMakeLists.txt index 1ac2076c556..e603f8f5706 100644 --- a/subsys/modbus/CMakeLists.txt +++ b/subsys/modbus/CMakeLists.txt @@ -15,6 +15,11 @@ if(CONFIG_MODBUS) modbus_serial.c ) + zephyr_library_sources_ifdef( + CONFIG_MODBUS_RAW_ADU + modbus_raw.c + ) + zephyr_library_sources_ifdef( CONFIG_MODBUS_SERVER modbus_server.c diff --git a/subsys/modbus/Kconfig b/subsys/modbus/Kconfig index d0d54a783bb..58ab15798dd 100644 --- a/subsys/modbus/Kconfig +++ b/subsys/modbus/Kconfig @@ -53,6 +53,18 @@ config MODBUS_ASCII_MODE help Enable ASCII transmission mode. +config MODBUS_RAW_ADU + bool "Modbus raw ADU support" + help + Enable Modbus raw ADU support. + +config MODBUS_NUMOF_RAW_ADU + int "Number of raw ADU instances" + depends on MODBUS_RAW_ADU + range 1 4 + help + Number of raw ADU instances. + config MODBUS_FP_EXTENSIONS bool "Floating-Point extensions" default y diff --git a/subsys/modbus/modbus_core.c b/subsys/modbus/modbus_core.c index ee3abb514e5..c1a89788fe2 100644 --- a/subsys/modbus/modbus_core.c +++ b/subsys/modbus/modbus_core.c @@ -51,8 +51,18 @@ static struct modbus_serial_config modbus_serial_cfg[] = { .cfg = &modbus_serial_cfg[n], \ }, +#define DEFINE_MODBUS_RAW_ADU(x, _) { \ + .iface_name = "RAW_"#x, \ + .raw_tx_cb = NULL, \ + .mode = MODBUS_MODE_RAW, \ + }, + + static struct modbus_context mb_ctx_tbl[] = { DT_INST_FOREACH_STATUS_OKAY(MODBUS_DT_GET_DEV) +#ifdef CONFIG_MODBUS_RAW_ADU + UTIL_LISTIFY(CONFIG_MODBUS_NUMOF_RAW_ADU, DEFINE_MODBUS_RAW_ADU, _) +#endif }; static void modbus_rx_handler(struct k_work *item) @@ -73,6 +83,11 @@ static void modbus_rx_handler(struct k_work *item) ctx->rx_adu_err = modbus_serial_rx_adu(ctx); } break; + case MODBUS_MODE_RAW: + if (IS_ENABLED(CONFIG_MODBUS_RAW_ADU)) { + ctx->rx_adu_err = modbus_raw_rx_adu(ctx); + } + break; default: LOG_ERR("Unknown MODBUS mode"); return; @@ -113,6 +128,12 @@ void modbus_tx_adu(struct modbus_context *ctx) LOG_ERR("Unsupported MODBUS serial mode"); } break; + case MODBUS_MODE_RAW: + if (IS_ENABLED(CONFIG_MODBUS_RAW_ADU) && + modbus_raw_tx_adu(ctx)) { + LOG_ERR("Unsupported MODBUS raw mode"); + } + break; default: LOG_ERR("Unknown MODBUS mode"); } @@ -149,6 +170,17 @@ struct modbus_context *modbus_get_context(const uint8_t iface) return ctx; } +int modbus_iface_get_by_ctx(const struct modbus_context *ctx) +{ + for (int i = 0; i < ARRAY_SIZE(mb_ctx_tbl); i++) { + if (&mb_ctx_tbl[i] == ctx) { + return i; + } + } + + return -ENODEV; +} + int modbus_iface_get_by_name(const char *iface_name) { for (int i = 0; i < ARRAY_SIZE(mb_ctx_tbl); i++) { @@ -216,6 +248,14 @@ int modbus_init_server(const int iface, struct modbus_iface_param param) goto init_server_error; } break; + case MODBUS_MODE_RAW: + if (IS_ENABLED(CONFIG_MODBUS_RAW_ADU) && + modbus_raw_init(ctx, param) != 0) { + LOG_ERR("Failed to init MODBUS raw ADU support"); + rc = -EINVAL; + goto init_server_error; + } + break; default: LOG_ERR("Unknown MODBUS mode"); rc = -ENOTSUP; @@ -268,6 +308,14 @@ int modbus_init_client(const int iface, struct modbus_iface_param param) goto init_client_error; } break; + case MODBUS_MODE_RAW: + if (IS_ENABLED(CONFIG_MODBUS_RAW_ADU) && + modbus_raw_init(ctx, param) != 0) { + LOG_ERR("Failed to init MODBUS raw ADU support"); + rc = -EINVAL; + goto init_client_error; + } + break; default: LOG_ERR("Unknown MODBUS mode"); rc = -ENOTSUP; @@ -306,6 +354,8 @@ int modbus_disable(const uint8_t iface) modbus_serial_disable(ctx); } break; + case MODBUS_MODE_RAW: + break; default: LOG_ERR("Unknown MODBUS mode"); } diff --git a/subsys/modbus/modbus_internal.h b/subsys/modbus/modbus_internal.h index 12a607c6cc1..c6979934cb7 100644 --- a/subsys/modbus/modbus_internal.h +++ b/subsys/modbus/modbus_internal.h @@ -61,6 +61,11 @@ #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 @@ -72,13 +77,8 @@ #define MODBUS_ASCII_END_FRAME_CHAR1 '\r' #define MODBUS_ASCII_END_FRAME_CHAR2 '\n' -struct modbus_adu { - uint16_t length; - uint8_t unit_id; - uint8_t fc; - uint8_t data[CONFIG_MODBUS_BUFFER_SIZE - 4]; - uint16_t crc; -}; +/* Modbus ADU constants */ +#define MODBUS_ADU_PROTO_ID 0x0000 struct mb_rtu_gpio_config { const char *name; @@ -113,8 +113,12 @@ struct modbus_serial_config { struct modbus_context { /* Interface name */ const char *iface_name; - /* Serial line configuration */ - struct modbus_serial_config *cfg; + union { + /* Serial line configuration */ + struct modbus_serial_config *cfg; + /* RAW TX callback */ + modbus_raw_cb_t raw_tx_cb; + }; /* MODBUS mode */ enum modbus_mode mode; /* True if interface is configured as client */ @@ -162,6 +166,15 @@ struct modbus_context { */ struct modbus_context *modbus_get_context(const uint8_t iface); +/** + * @brief Get Modbus interface index. + * + * @param ctx Pointer to Modbus interface context + * + * @retval Interface index or negative error value. + */ +int modbus_iface_get_by_ctx(const struct modbus_context *ctx); + /** * @brief Send ADU. * @@ -253,4 +266,9 @@ int modbus_serial_init(struct modbus_context *ctx, */ void modbus_serial_disable(struct modbus_context *ctx); +int modbus_raw_rx_adu(struct modbus_context *ctx); +int modbus_raw_tx_adu(struct modbus_context *ctx); +int modbus_raw_init(struct modbus_context *ctx, + struct modbus_iface_param param); + #endif /* ZEPHYR_INCLUDE_MODBUS_INTERNAL_H_ */ diff --git a/subsys/modbus/modbus_raw.c b/subsys/modbus/modbus_raw.c new file mode 100644 index 00000000000..7f98a54d288 --- /dev/null +++ b/subsys/modbus/modbus_raw.c @@ -0,0 +1,172 @@ +/* + * Copyright (c) 2021 Nordic Semiconductor ASA + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#include +LOG_MODULE_REGISTER(modbus_raw, CONFIG_MODBUS_LOG_LEVEL); + +#include +#include +#include + +#define MODBUS_ADU_LENGTH_DEVIATION 2 +#define MODBUS_RAW_MIN_MSG_SIZE (MODBUS_RTU_MIN_MSG_SIZE - 2) +#define MODBUS_RAW_BUFFER_SIZE (CONFIG_MODBUS_BUFFER_SIZE - 2) + +int modbus_raw_rx_adu(struct modbus_context *ctx) +{ + if (ctx->rx_adu.length < MODBUS_RAW_MIN_MSG_SIZE || + ctx->rx_adu.length > MODBUS_RAW_BUFFER_SIZE) { + LOG_WRN("Frame length error"); + return -EMSGSIZE; + } + + if (ctx->rx_adu.proto_id != MODBUS_ADU_PROTO_ID) { + LOG_ERR("MODBUS protocol not supported"); + return -ENOTSUP; + } + + return 0; +} + +int modbus_raw_tx_adu(struct modbus_context *ctx) +{ + int iface = modbus_iface_get_by_ctx(ctx); + + if (ctx->mode != MODBUS_MODE_RAW) { + return -ENOTSUP; + } + + if (iface < 0) { + return -ENODEV; + } + + ctx->raw_tx_cb(iface, &ctx->tx_adu); + + return 0; +} + +int modbus_raw_submit_rx(const int iface, const struct modbus_adu *adu) +{ + struct modbus_context *ctx; + + ctx = modbus_get_context(iface); + + if (ctx == NULL) { + LOG_ERR("Interface not available"); + return -ENODEV; + } + + if (ctx->mode != MODBUS_MODE_RAW) { + LOG_ERR("Interface not in RAW mode"); + return -ENOTSUP; + } + + ctx->rx_adu.trans_id = adu->trans_id; + ctx->rx_adu.proto_id = adu->proto_id; + ctx->rx_adu.length = adu->length; + ctx->rx_adu.unit_id = adu->unit_id; + ctx->rx_adu.fc = adu->fc; + memcpy(ctx->rx_adu.data, adu->data, + MIN(adu->length, CONFIG_MODBUS_BUFFER_SIZE)); + k_work_submit(&ctx->server_work); + + return 0; +} + +void modbus_raw_put_header(const struct modbus_adu *adu, uint8_t *header) +{ + uint16_t length = MIN(adu->length, CONFIG_MODBUS_BUFFER_SIZE); + + sys_put_be16(adu->trans_id, &header[0]); + sys_put_be16(adu->proto_id, &header[2]); + sys_put_be16(length + MODBUS_ADU_LENGTH_DEVIATION, &header[4]); + header[6] = adu->unit_id; + header[7] = adu->fc; +} + +void modbus_raw_get_header(struct modbus_adu *adu, const uint8_t *header) +{ + adu->trans_id = sys_get_be16(&header[0]); + adu->proto_id = sys_get_be16(&header[2]); + adu->length = MIN(sys_get_be16(&header[4]), CONFIG_MODBUS_BUFFER_SIZE); + adu->unit_id = header[6]; + adu->fc = header[7]; + + if (adu->length >= MODBUS_ADU_LENGTH_DEVIATION) { + adu->length -= MODBUS_ADU_LENGTH_DEVIATION; + } +} + +static void modbus_set_exception(struct modbus_adu *adu, + const uint8_t excep_code) +{ + const uint8_t excep_bit = BIT(7); + + adu->fc |= excep_bit; + adu->data[0] = excep_code; + adu->length = 1; +} + +void modbus_raw_set_server_failure(struct modbus_adu *adu) +{ + const uint8_t excep_bit = BIT(7); + + adu->fc |= excep_bit; + adu->data[0] = MODBUS_EXC_SERVER_DEVICE_FAILURE; + adu->length = 1; +} + +int modbus_raw_backend_txn(const int iface, struct modbus_adu *adu) +{ + struct modbus_context *ctx; + int err; + + ctx = modbus_get_context(iface); + if (ctx == NULL) { + LOG_ERR("Interface %d not available", iface); + modbus_set_exception(adu, MODBUS_EXC_GW_PATH_UNAVAILABLE); + return -ENODEV; + } + + /* + * This is currently only possible over serial line + * since no other medium is directly supported. + */ + if (ctx->client == false || + (ctx->mode != MODBUS_MODE_RTU && ctx->mode != MODBUS_MODE_ASCII)) { + LOG_ERR("Interface %d has wrong configuration", iface); + modbus_set_exception(adu, MODBUS_EXC_GW_PATH_UNAVAILABLE); + return -ENOTSUP; + } + + LOG_DBG("Use backend interface %d", iface); + memcpy(&ctx->tx_adu, adu, sizeof(struct modbus_adu)); + err = modbus_tx_wait_rx_adu(ctx); + + if (err == 0) { + memcpy(adu, &ctx->rx_adu, sizeof(struct modbus_adu)); + } else { + modbus_set_exception(adu, MODBUS_EXC_GW_TARGET_FAILED_TO_RESP); + } + + return err; +} + +int modbus_raw_init(struct modbus_context *ctx, + struct modbus_iface_param param) +{ + if (ctx->mode != MODBUS_MODE_RAW) { + return -ENOTSUP; + } + + ctx->raw_tx_cb = param.raw_tx_cb; + + return 0; +} + +void modbus_raw_disable(struct modbus_context *ctx) +{ +}