diff --git a/dts/bindings/misc/zephyr,modbus-serial.yaml b/dts/bindings/misc/zephyr,modbus-serial.yaml new file mode 100644 index 00000000000..e761f220e6a --- /dev/null +++ b/dts/bindings/misc/zephyr,modbus-serial.yaml @@ -0,0 +1,8 @@ +# Copyright (c) 2020 PHYTEC Messtechnik GmbH +# SPDX-License-Identifier: Apache-2.0 + +description: Modbus over serial line device + +compatible: "zephyr,modbus-serial" + +include: uart-device.yaml diff --git a/include/modbus/modbus_rtu.h b/include/modbus/modbus_rtu.h new file mode 100644 index 00000000000..cbe1136d873 --- /dev/null +++ b/include/modbus/modbus_rtu.h @@ -0,0 +1,395 @@ +/* + * Copyright (c) 2020 PHYTEC Messtechnik GmbH + * + * SPDX-License-Identifier: Apache-2.0 + */ + +/* + * Client API in this file is based on mbm_core.c from uC/Modbus Stack. + * + * uC/Modbus + * The Embedded Modbus Stack + * + * Copyright 2003-2020 Silicon Laboratories Inc. www.silabs.com + * + * SPDX-License-Identifier: APACHE-2.0 + * + * This software is subject to an open source license and is distributed by + * Silicon Laboratories Inc. pursuant to the terms of the Apache License, + * Version 2.0 available at www.apache.org/licenses/LICENSE-2.0. + */ + +/** + * @brief MODBUS RTU transport protocol over Serial Line + * @defgroup modbus MODBUS + * @ingroup io_interfaces + * @{ + */ + +#ifndef ZEPHYR_INCLUDE_MODBUS_RTU_H_ +#define ZEPHYR_INCLUDE_MODBUS_RTU_H_ + +#include + +#ifdef __cplusplus +extern "C" { +#endif + +/** + * @brief Coil read (FC01) + * + * Sends a Modbus message to read the status of coils from a server. + * + * @param iface Modbus RTU interface index + * @param node_addr Modbus RTU address of the server + * @param start_addr Coil starting address + * @param coil_tbl Pointer to an array of bytes containing the value + * of the coils read. + * The format is: + * + * MSB LSB + * B7 B6 B5 B4 B3 B2 B1 B0 + * ------------------------------------- + * coil_tbl[0] #8 #7 #1 + * coil_tbl[1] #16 #15 #9 + * : + * : + * + * Note that the array that will be receiving the coil + * values must be greater than or equal to: + * (num_coils - 1) / 8 + 1 + * @param num_coils Quantity of coils to read + * + * @retval 0 If the function was successful + */ +int mb_rtu_read_coils(const int iface, + const uint8_t node_addr, + const uint16_t start_addr, + uint8_t *const coil_tbl, + const uint16_t num_coils); + +/** + * @brief Read discrete inputs (FC02) + * + * Sends a Modbus message to read the status of discrete inputs from + * a server. + * + * @param iface Modbus RTU interface index + * @param node_addr Modbus RTU address of the server + * @param start_addr Discrete input starting address + * @param di_tbl Pointer to an array that will receive the state + * of the discrete inputs. + * The format of the array is as follows: + * + * MSB LSB + * B7 B6 B5 B4 B3 B2 B1 B0 + * ------------------------------------- + * di_tbl[0] #8 #7 #1 + * di_tbl[1] #16 #15 #9 + * : + * : + * + * Note that the array that will be receiving the discrete + * input values must be greater than or equal to: + * (num_di - 1) / 8 + 1 + * @param num_di Quantity of discrete inputs to read + * + * @retval 0 If the function was successful + */ +int mb_rtu_read_dinputs(const int iface, + const uint8_t node_addr, + const uint16_t start_addr, + uint8_t *const di_tbl, + const uint16_t num_di); + +/** + * @brief Read holding registers (FC03) + * + * Sends a Modbus message to read the value of holding registers + * from a server. + * + * @param iface Modbus RTU interface index + * @param node_addr Modbus RTU address of the server + * @param start_addr Register starting address + * @param reg_buf Is a pointer to an array that will receive + * the current values of the holding registers from + * the server. The array pointed to by 'reg_buf' needs + * to be able to hold at least 'num_regs' entries. + * @param num_regs Quantity of registers to read + * + * @retval 0 If the function was successful + */ +int mb_rtu_read_holding_regs(const int iface, + const uint8_t node_addr, + const uint16_t start_addr, + uint16_t *const reg_buf, + const uint16_t num_regs); + +/** + * @brief Read input registers (FC04) + * + * Sends a Modbus message to read the value of input registers from + * a server. + * + * @param iface Modbus RTU interface index + * @param node_addr Modbus RTU address of the server + * @param start_addr Register starting address + * @param reg_buf Is a pointer to an array that will receive + * the current value of the holding registers + * from the server. The array pointed to by 'reg_buf' + * needs to be able to hold at least 'num_regs' entries. + * @param num_regs Quantity of registers to read + * + * @retval 0 If the function was successful + */ +int mb_rtu_read_input_regs(const int iface, + const uint8_t node_addr, + const uint16_t start_addr, + uint16_t *const reg_buf, + const uint16_t num_regs); + +/** + * @brief Write single coil (FC05) + * + * Sends a Modbus message to write the value of single coil to a server. + * + * @param iface Modbus RTU interface index + * @param node_addr Modbus RTU address of the server + * @param coil_addr Coils starting address + * @param coil_state Is the desired state of the coil + * + * @retval 0 If the function was successful + */ +int mb_rtu_write_coil(const int iface, + const uint8_t node_addr, + const uint16_t coil_addr, + const bool coil_state); + +/** + * @brief Write single holding register (FC06) + * + * Sends a Modbus message to write the value of single holding register + * to a server unit. + * + * @param iface Modbus RTU interface index + * @param node_addr Modbus RTU address of the server + * @param start_addr Coils starting address + * @param reg_val Desired value of the holding register + * + * @retval 0 If the function was successful + */ +int mb_rtu_write_holding_reg(const int iface, + const uint8_t node_addr, + const uint16_t start_addr, + const uint16_t reg_val); + +/** + * @brief Read diagnostic (FC08) + * + * Sends a Modbus message to perform a diagnostic function of a server unit. + * + * @param iface Modbus RTU interface index + * @param node_addr Modbus RTU address of the server + * @param sfunc Diagnostic sub-function code + * @param data Sub-function data + * @param data_out Pointer to the data value + * + * @retval 0 If the function was successful + */ +int mb_rtu_request_diagnostic(const int iface, + const uint8_t node_addr, + const uint16_t sfunc, + const uint16_t data, + uint16_t *const data_out); + +/** + * @brief Write coils (FC15) + * + * Sends a Modbus message to write to coils on a server unit. + * + * @param iface Modbus RTU interface index + * @param node_addr Modbus RTU address of the server + * @param start_addr Coils starting address + * @param coil_tbl Pointer to an array of bytes containing the value + * of the coils to write. + * The format is: + * + * MSB LSB + * B7 B6 B5 B4 B3 B2 B1 B0 + * ------------------------------------- + * coil_tbl[0] #8 #7 #1 + * coil_tbl[1] #16 #15 #9 + * : + * : + * + * Note that the array that will be receiving the coil + * values must be greater than or equal to: + * (num_coils - 1) / 8 + 1 + * @param num_coils Quantity of coils to write + * + * @retval 0 If the function was successful + */ +int mb_rtu_write_coils(const int iface, + const uint8_t node_addr, + const uint16_t start_addr, + uint8_t *const coil_tbl, + const uint16_t num_coils); + +/** + * @brief Write holding registers (FC16) + * + * Sends a Modbus message to write to integer holding registers + * to a server unit. + * + * @param iface Modbus RTU interface index + * @param node_addr Modbus RTU address of the server + * @param start_addr Register starting address + * @param reg_buf Is a pointer to an array containing + * the value of the holding registers to write. + * Note that the array containing the register values must + * be greater than or equal to 'num_regs' + * @param num_regs Quantity of registers to write + * + * @retval 0 If the function was successful + */ +int mb_rtu_write_holding_regs(const int iface, + const uint8_t node_addr, + const uint16_t start_addr, + uint16_t *const reg_buf, + const uint16_t num_regs); + +/** + * @brief Read floating-point holding registers (FC03) + * + * Sends a Modbus message to read the value of floating-point + * holding registers from a server unit. + * + * @param iface Modbus RTU interface index + * @param node_addr Modbus RTU address of the server + * @param start_addr Register starting address + * @param reg_buf Is a pointer to an array that will receive + * the current values of the holding registers from + * the server. The array pointed to by 'reg_buf' needs + * to be able to hold at least 'num_regs' entries. + * @param num_regs Quantity of registers to read + * + * @retval 0 If the function was successful + */ +int mb_rtu_read_holding_regs_fp(const int iface, + const uint8_t node_addr, + const uint16_t start_addr, + float *const reg_buf, + const uint16_t num_regs); + +/** + * @brief Write floating-point holding registers (FC16) + * + * Sends a Modbus message to write to floating-point holding registers + * to a server unit. + * + * @param iface Modbus RTU interface index + * @param node_addr Modbus RTU address of the server + * @param start_addr Register starting address + * @param reg_buf Is a pointer to an array containing + * the value of the holding registers to write. + * Note that the array containing the register values must + * be greater than or equal to 'num_regs' + * @param num_regs Quantity of registers to write + * + * @retval 0 If the function was successful + */ +int mb_rtu_write_holding_regs_fp(const int iface, + const uint8_t node_addr, + const uint16_t start_addr, + float *const reg_buf, + const uint16_t num_regs); + +/** Modbus Server User Callback structure */ +struct mbs_rtu_user_callbacks { + /** Coil read callback */ + int (*coil_rd)(uint16_t addr, bool *state); + + /** Coil write callback */ + int (*coil_wr)(uint16_t addr, bool state); + + /** Discrete Input read callback */ + int (*discrete_input_rd)(uint16_t addr, bool *state); + + /** Input Register read callback */ + int (*input_reg_rd)(uint16_t addr, uint16_t *reg); + + /** Floating Point Input Register read callback */ + int (*input_reg_rd_fp)(uint16_t addr, float *reg); + + /** Holding Register read callback */ + int (*holding_reg_rd)(uint16_t addr, uint16_t *reg); + + /** Holding Register write callback */ + int (*holding_reg_wr)(uint16_t addr, uint16_t reg); + + /** Floating Point Holding Register read callback */ + int (*holding_reg_rd_fp)(uint16_t addr, float *reg); + + /** Floating Point Holding Register write callback */ + int (*holding_reg_wr_fp)(uint16_t addr, float reg); +}; + +/** + * @brief Configure Modbus Interface as server + * + * @param iface Modbus RTU interface index + * @param node_addr Modbus RTU address of the server + * @param baud Baudrate of the serial line + * @param parity UART's parity setting: + * UART_CFG_PARITY_NONE, + * UART_CFG_PARITY_EVEN, + * UART_CFG_PARITY_ODD + * @param cb Pointer to the User Callback structure + * @param ascii_mode Enable ASCII Transfer Mode + * + * @retval 0 If the function was successful + */ +int mb_rtu_cfg_server(const uint8_t iface, const uint8_t node_addr, + const uint32_t baud, enum uart_config_parity parity, + struct mbs_rtu_user_callbacks *const cb, + bool ascii_mode); + +/** + * @brief Configure Modbus Interface as client + * + * @param iface Modbus RTU interface index + * @param baud Baudrate of the serial line + * @param parity UART's parity setting: + * UART_CFG_PARITY_NONE, + * UART_CFG_PARITY_EVEN, + * UART_CFG_PARITY_ODD + * @param rx_timeout Amount of time client will wait for a response + * from the server. + * @param ascii_mode Enable ASCII Transfer Mode + * + * @retval 0 If the function was successful + */ +int mb_rtu_cfg_client(const uint8_t iface, + const uint32_t baud, enum uart_config_parity parity, + uint32_t rx_timeout, + bool ascii_mode); +/** + * @brief Disable Modbus Interface + * + * This function is called to disable Modbus interface. + * + * @param iface Modbus RTU interface index + * + * @retval 0 If the function was successful + */ +int mb_rtu_disable_iface(const uint8_t iface); + +#ifdef __cplusplus +} +#endif + +/** + * @} + */ + +#endif /* ZEPHYR_INCLUDE_MODBUS_RTU_H_ */ diff --git a/subsys/CMakeLists.txt b/subsys/CMakeLists.txt index fc18f2c1006..6fd7c8bcd07 100644 --- a/subsys/CMakeLists.txt +++ b/subsys/CMakeLists.txt @@ -28,3 +28,4 @@ add_subdirectory_ifdef(CONFIG_JWT jwt) add_subdirectory(canbus) add_subdirectory_ifdef(CONFIG_TIMING_FUNCTIONS timing) add_subdirectory_ifdef(CONFIG_DEMAND_PAGING demand_paging) +add_subdirectory(modbus) diff --git a/subsys/Kconfig b/subsys/Kconfig index b3a9661811a..cd5584b2a55 100644 --- a/subsys/Kconfig +++ b/subsys/Kconfig @@ -34,6 +34,8 @@ source "subsys/lorawan/Kconfig" source "subsys/mgmt/Kconfig" +source "subsys/modbus/Kconfig" + source "subsys/net/Kconfig" source "subsys/power/Kconfig" diff --git a/subsys/modbus/CMakeLists.txt b/subsys/modbus/CMakeLists.txt new file mode 100644 index 00000000000..5527bc23433 --- /dev/null +++ b/subsys/modbus/CMakeLists.txt @@ -0,0 +1,22 @@ +# Copyright (c) 2020 PHYTEC Messtechnik GmbH +# SPDX-License-Identifier: Apache-2.0 + +if(CONFIG_MODBUS_RTU) + zephyr_library() + + zephyr_include_directories(${ZEPHYR_BASE}/subsys/modbus) + + zephyr_library_sources( + mb_rtu_core.c + ) + + zephyr_library_sources_ifdef( + CONFIG_MODBUS_RTU_SERVER + mb_rtu_server.c + ) + + zephyr_library_sources_ifdef( + CONFIG_MODBUS_RTU_CLIENT + mb_rtu_client.c + ) +endif() diff --git a/subsys/modbus/Kconfig b/subsys/modbus/Kconfig new file mode 100644 index 00000000000..5b60df322f2 --- /dev/null +++ b/subsys/modbus/Kconfig @@ -0,0 +1,65 @@ +# Copyright (c) 2020 PHYTEC Messtechnik GmbH +# SPDX-License-Identifier: Apache-2.0 + +config MODBUS_RTU + bool "Modbus RTU support" + depends on SERIAL && SERIAL_HAS_DRIVER + +if MODBUS_RTU + +config MODBUS_RTU_BUFFER_SIZE + int "Modbus buffer size" + default 256 + range 64 256 + help + Modbus buffer size. + +choice + prompt "Supported node roles" + default MODBUS_RTU_ROLE_CLIENT_SERVER + help + Specify the type of supported node roles. + +config MODBUS_RTU_ROLE_CLIENT + bool "Client support" + +config MODBUS_RTU_ROLE_SERVER + bool "Server support" + +config MODBUS_RTU_ROLE_CLIENT_SERVER + bool "Client and server support" + +endchoice + +config MODBUS_RTU_SERVER + bool + default y if MODBUS_RTU_ROLE_SERVER || MODBUS_RTU_ROLE_CLIENT_SERVER + +config MODBUS_RTU_CLIENT + bool + default y if MODBUS_RTU_ROLE_CLIENT || MODBUS_RTU_ROLE_CLIENT_SERVER + +config MODBUS_RTU_ASCII_MODE + bool "Modbus transmission mode ASCII" + help + Enable ASCII transmission mode. + +config MODBUS_RTU_FP_EXTENSIONS + bool "Floating-Point extensions" + default y + help + Enable Floating-Point extensions + +config MODBUS_RTU_FC08_DIAGNOSTIC + bool "Enable FC08 Diagnostic support" + depends on MODBUS_RTU_SERVER + default y + help + Enable function code 08 Diagnostic support + +module = MODBUS_RTU +module-str = Modbus RTU Support +module-help = Sets log level for Modbus RTU support +source "subsys/logging/Kconfig.template.log_config" + +endif diff --git a/subsys/modbus/mb_rtu_client.c b/subsys/modbus/mb_rtu_client.c new file mode 100644 index 00000000000..4987041f2ee --- /dev/null +++ b/subsys/modbus/mb_rtu_client.c @@ -0,0 +1,615 @@ +/* + * Copyright (c) 2020 PHYTEC Messtechnik GmbH + * + * SPDX-License-Identifier: Apache-2.0 + */ + +/* + * This file is based on mbm_core.c from uC/Modbus Stack. + * + * uC/Modbus + * The Embedded Modbus Stack + * + * Copyright 2003-2020 Silicon Laboratories Inc. www.silabs.com + * + * SPDX-License-Identifier: APACHE-2.0 + * + * This software is subject to an open source license and is distributed by + * Silicon Laboratories Inc. pursuant to the terms of the Apache License, + * Version 2.0 available at www.apache.org/licenses/LICENSE-2.0. + */ + +#include +#include +#include + +#include +LOG_MODULE_REGISTER(mb_rtu_c, CONFIG_MODBUS_RTU_LOG_LEVEL); + +static int mbm_validate_response_fc(struct mb_rtu_context *ctx, + const uint8_t node_addr, + uint8_t fc) +{ + uint8_t resp_fc = ctx->rx_frame.fc; + uint8_t excep_code = ctx->rx_frame.data[0]; + const uint8_t excep_bit = BIT(7); + const uint8_t excep_mask = BIT_MASK(7); + + if (node_addr != ctx->rx_frame.addr) { + return -EIO; + } + + if (fc != (resp_fc & excep_mask)) { + return -EIO; + } + + if (resp_fc & excep_bit) { + if (excep_code > MODBUS_EXC_NONE) { + return excep_code; + } + + return -EIO; + } + + return 0; +} + +#ifdef CONFIG_MODBUS_RTU_FP_EXTENSIONS +static int mbm_validate_fc03fp_response(struct mb_rtu_context *ctx, float *ptbl) +{ + uint8_t resp_byte_cnt; + uint8_t req_byte_cnt; + uint16_t req_qty; + uint8_t *resp_data; + + resp_byte_cnt = ctx->rx_frame.data[0]; + resp_data = &ctx->rx_frame.data[1]; + req_qty = sys_get_be16(&ctx->tx_frame.data[2]); + req_byte_cnt = req_qty * sizeof(float); + + if (req_byte_cnt != resp_byte_cnt) { + LOG_ERR("Mismatch in the number of registers"); + return -EINVAL; + } + + for (uint16_t i = 0; i < req_qty; i++) { + uint32_t reg_val = sys_get_be32(resp_data); + + memcpy(&ptbl[i], ®_val, sizeof(float)); + resp_data += sizeof(uint32_t); + } + + return 0; +} +#endif + +static int mbm_validate_rd_response(struct mb_rtu_context *ctx, + const uint8_t node_addr, + uint8_t fc, + uint8_t *data) +{ + int err; + uint8_t resp_byte_cnt; + uint8_t req_byte_cnt; + uint16_t req_qty; + uint16_t req_addr; + uint8_t *resp_data; + uint16_t *data_p16 = (uint16_t *)data; + + if (data == NULL) { + return -EINVAL; + } + + resp_byte_cnt = ctx->rx_frame.data[0]; + resp_data = &ctx->rx_frame.data[1]; + req_qty = sys_get_be16(&ctx->tx_frame.data[2]); + req_addr = sys_get_be16(&ctx->tx_frame.data[0]); + + switch (fc) { + case MODBUS_FC01_COIL_RD: + case MODBUS_FC02_DI_RD: + req_byte_cnt = (uint8_t)((req_qty - 1) / 8) + 1; + if (req_byte_cnt != resp_byte_cnt) { + LOG_ERR("Mismatch in the number of coils or inputs"); + err = -EINVAL; + } else { + for (uint8_t i = 0; i < resp_byte_cnt; i++) { + data[i] = resp_data[i]; + } + + err = 0; + } + break; + + case MODBUS_FC03_HOLDING_REG_RD: + if (IS_ENABLED(CONFIG_MODBUS_RTU_FP_EXTENSIONS) && + (req_addr >= MODBUS_RTU_FP_ADDR)) { + err = mbm_validate_fc03fp_response(ctx, (float *)data); + break; + } + /* fallthrough */ + case MODBUS_FC04_IN_REG_RD: + req_byte_cnt = req_qty * sizeof(uint16_t); + if (req_byte_cnt != resp_byte_cnt) { + LOG_ERR("Mismatch in the number of registers"); + err = -EINVAL; + } else { + for (uint16_t i = 0; i < req_qty; i++) { + data_p16[i] = sys_get_be16(resp_data); + resp_data += sizeof(uint16_t); + } + + err = 0; + } + break; + + default: + LOG_ERR("Validation not implemented for FC 0x%02x", fc); + err = -ENOTSUP; + } + + return err; +} + +static int mbm_validate_fc08_response(struct mb_rtu_context *ctx, + const uint8_t node_addr, + uint16_t *data) +{ + int err; + uint16_t resp_sfunc; + uint16_t resp_data; + uint16_t req_sfunc; + uint16_t req_data; + + if (data == NULL) { + return -EINVAL; + } + + req_sfunc = sys_get_be16(&ctx->tx_frame.data[0]); + req_data = sys_get_be16(&ctx->tx_frame.data[2]); + resp_sfunc = sys_get_be16(&ctx->rx_frame.data[0]); + resp_data = sys_get_be16(&ctx->rx_frame.data[2]); + + if (req_sfunc != resp_sfunc) { + LOG_ERR("Mismatch in the sub-function code"); + return -EINVAL; + } + + switch (resp_sfunc) { + case MODBUS_FC08_SUBF_QUERY: + case MODBUS_FC08_SUBF_CLR_CTR: + if (req_data != resp_data) { + LOG_ERR("Request and response data are different"); + err = -EINVAL; + } else { + *data = resp_data; + err = 0; + } + break; + + case MODBUS_FC08_SUBF_BUS_MSG_CTR: + case MODBUS_FC08_SUBF_BUS_CRC_CTR: + case MODBUS_FC08_SUBF_BUS_EXCEPT_CTR: + case MODBUS_FC08_SUBF_SERVER_MSG_CTR: + case MODBUS_FC08_SUBF_SERVER_NO_RESP_CTR: + *data = resp_data; + err = 0; + break; + + default: + err = -EINVAL; + } + + return err; +} + +static int mbm_validate_wr_response(struct mb_rtu_context *ctx, + const uint8_t node_addr, + uint8_t fc) +{ + int err; + uint16_t req_addr; + uint16_t req_value; + uint16_t resp_addr; + uint16_t resp_value; + + req_addr = sys_get_be16(&ctx->tx_frame.data[0]); + req_value = sys_get_be16(&ctx->tx_frame.data[2]); + resp_addr = sys_get_be16(&ctx->rx_frame.data[0]); + resp_value = sys_get_be16(&ctx->rx_frame.data[2]); + + switch (fc) { + case MODBUS_FC05_COIL_WR: + case MODBUS_FC06_HOLDING_REG_WR: + case MODBUS_FC15_COILS_WR: + case MODBUS_FC16_HOLDING_REGS_WR: + if (req_addr != resp_addr || req_value != resp_value) { + err = ENXIO; + } else { + err = 0; + } + break; + + default: + LOG_ERR("Validation not implemented for FC 0x%02x", fc); + err = -ENOTSUP; + } + + return err; +} + +static int mbm_send_cmd(struct mb_rtu_context *ctx, const uint8_t node_addr, + uint8_t fc, void *data) +{ + int err; + + ctx->tx_frame.addr = node_addr; + ctx->tx_frame.fc = fc; + + mb_tx_frame(ctx); + + if (k_sem_take(&ctx->client_wait_sem, K_USEC(ctx->rxwait_to)) != 0) { + LOG_WRN("Client wait-for-RX timeout"); + err = -EIO; + goto exit_error; + } + + if (ctx->rx_frame_err != 0) { + err = ctx->rx_frame_err; + goto exit_error; + } + + err = mbm_validate_response_fc(ctx, node_addr, fc); + if (err < 0) { + LOG_ERR("Failed to validate address or function code"); + goto exit_error; + } else if (err > 0) { + LOG_INF("Modbus FC %u, error code %u", fc, err); + goto exit_error; + } + + switch (fc) { + case MODBUS_FC01_COIL_RD: + case MODBUS_FC02_DI_RD: + case MODBUS_FC03_HOLDING_REG_RD: + case MODBUS_FC04_IN_REG_RD: + err = mbm_validate_rd_response(ctx, node_addr, fc, data); + break; + + case MODBUS_FC08_DIAGNOSTICS: + err = mbm_validate_fc08_response(ctx, node_addr, data); + break; + + case MODBUS_FC05_COIL_WR: + case MODBUS_FC06_HOLDING_REG_WR: + case MODBUS_FC15_COILS_WR: + case MODBUS_FC16_HOLDING_REGS_WR: + err = mbm_validate_wr_response(ctx, node_addr, fc); + break; + + default: + LOG_ERR("FC 0x%02x not implemented", fc); + err = -ENOTSUP; + } + +exit_error: + return err; +} + +int mb_rtu_read_coils(const int iface, + const uint8_t node_addr, + const uint16_t start_addr, + uint8_t *const coil_tbl, + const uint16_t num_coils) +{ + struct mb_rtu_context *ctx = mb_get_context(iface); + int err; + + if (ctx == NULL) { + return -ENODEV; + } + + k_mutex_lock(&ctx->iface_lock, K_FOREVER); + + ctx->tx_frame.length = 4; + sys_put_be16(start_addr, &ctx->tx_frame.data[0]); + sys_put_be16(num_coils, &ctx->tx_frame.data[2]); + + err = mbm_send_cmd(ctx, node_addr, MODBUS_FC01_COIL_RD, coil_tbl); + k_mutex_unlock(&ctx->iface_lock); + + return err; +} + +int mb_rtu_read_dinputs(const int iface, + const uint8_t node_addr, + const uint16_t start_addr, + uint8_t *const di_tbl, + const uint16_t num_di) +{ + struct mb_rtu_context *ctx = mb_get_context(iface); + int err; + + if (ctx == NULL) { + return -ENODEV; + } + + k_mutex_lock(&ctx->iface_lock, K_FOREVER); + + ctx->tx_frame.length = 4; + sys_put_be16(start_addr, &ctx->tx_frame.data[0]); + sys_put_be16(num_di, &ctx->tx_frame.data[2]); + + err = mbm_send_cmd(ctx, node_addr, MODBUS_FC02_DI_RD, di_tbl); + k_mutex_unlock(&ctx->iface_lock); + + return err; +} + +int mb_rtu_read_holding_regs(const int iface, + const uint8_t node_addr, + const uint16_t start_addr, + uint16_t *const reg_buf, + const uint16_t num_regs) +{ + struct mb_rtu_context *ctx = mb_get_context(iface); + int err; + + if (ctx == NULL) { + return -ENODEV; + } + + k_mutex_lock(&ctx->iface_lock, K_FOREVER); + + ctx->tx_frame.length = 4; + sys_put_be16(start_addr, &ctx->tx_frame.data[0]); + sys_put_be16(num_regs, &ctx->tx_frame.data[2]); + + err = mbm_send_cmd(ctx, node_addr, MODBUS_FC03_HOLDING_REG_RD, reg_buf); + k_mutex_unlock(&ctx->iface_lock); + + return err; +} + + +#ifdef CONFIG_MODBUS_RTU_FP_EXTENSIONS +int mb_rtu_read_holding_regs_fp(const int iface, + const uint8_t node_addr, + const uint16_t start_addr, + float *const reg_buf, + const uint16_t num_regs) +{ + struct mb_rtu_context *ctx = mb_get_context(iface); + int err; + + if (ctx == NULL) { + return -ENODEV; + } + + k_mutex_lock(&ctx->iface_lock, K_FOREVER); + + ctx->tx_frame.length = 4; + sys_put_be16(start_addr, &ctx->tx_frame.data[0]); + sys_put_be16(num_regs, &ctx->tx_frame.data[2]); + + err = mbm_send_cmd(ctx, node_addr, MODBUS_FC03_HOLDING_REG_RD, reg_buf); + k_mutex_unlock(&ctx->iface_lock); + + return err; +} +#endif + +int mb_rtu_read_input_regs(const int iface, + const uint8_t node_addr, + const uint16_t start_addr, + uint16_t *const reg_buf, + const uint16_t num_regs) +{ + struct mb_rtu_context *ctx = mb_get_context(iface); + int err; + + if (ctx == NULL) { + return -ENODEV; + } + + k_mutex_lock(&ctx->iface_lock, K_FOREVER); + + ctx->tx_frame.length = 4; + sys_put_be16(start_addr, &ctx->tx_frame.data[0]); + sys_put_be16(num_regs, &ctx->tx_frame.data[2]); + + err = mbm_send_cmd(ctx, node_addr, 4, reg_buf); + k_mutex_unlock(&ctx->iface_lock); + + return err; +} + +int mb_rtu_write_coil(const int iface, + const uint8_t node_addr, + const uint16_t coil_addr, + const bool coil_state) +{ + struct mb_rtu_context *ctx = mb_get_context(iface); + int err; + uint16_t coil_val; + + if (ctx == NULL) { + return -ENODEV; + } + + k_mutex_lock(&ctx->iface_lock, K_FOREVER); + + if (coil_state == false) { + coil_val = MODBUS_COIL_OFF_CODE; + } else { + coil_val = MODBUS_COIL_ON_CODE; + } + + ctx->tx_frame.length = 4; + sys_put_be16(coil_addr, &ctx->tx_frame.data[0]); + sys_put_be16(coil_val, &ctx->tx_frame.data[2]); + + err = mbm_send_cmd(ctx, node_addr, MODBUS_FC05_COIL_WR, NULL); + k_mutex_unlock(&ctx->iface_lock); + + return err; +} + +int mb_rtu_write_holding_reg(const int iface, + const uint8_t node_addr, + const uint16_t start_addr, + const uint16_t reg_val) +{ + struct mb_rtu_context *ctx = mb_get_context(iface); + int err; + + if (ctx == NULL) { + return -ENODEV; + } + + k_mutex_lock(&ctx->iface_lock, K_FOREVER); + + ctx->tx_frame.length = 4; + sys_put_be16(start_addr, &ctx->tx_frame.data[0]); + sys_put_be16(reg_val, &ctx->tx_frame.data[2]); + + err = mbm_send_cmd(ctx, node_addr, MODBUS_FC06_HOLDING_REG_WR, NULL); + k_mutex_unlock(&ctx->iface_lock); + + return err; +} + +int mb_rtu_request_diagnostic(const int iface, + const uint8_t node_addr, + const uint16_t sfunc, + const uint16_t data, + uint16_t *const data_out) +{ + struct mb_rtu_context *ctx = mb_get_context(iface); + int err; + + if (ctx == NULL) { + return -ENODEV; + } + + k_mutex_lock(&ctx->iface_lock, K_FOREVER); + + ctx->tx_frame.length = 4; + sys_put_be16(sfunc, &ctx->tx_frame.data[0]); + sys_put_be16(data, &ctx->tx_frame.data[2]); + + err = mbm_send_cmd(ctx, node_addr, MODBUS_FC08_DIAGNOSTICS, data_out); + k_mutex_unlock(&ctx->iface_lock); + + return err; +} + +int mb_rtu_write_coils(const int iface, + const uint8_t node_addr, + const uint16_t start_addr, + uint8_t *const coil_tbl, + const uint16_t num_coils) +{ + struct mb_rtu_context *ctx = mb_get_context(iface); + uint8_t num_bytes; + int err; + uint8_t *data_ptr; + + if (ctx == NULL) { + return -ENODEV; + } + + k_mutex_lock(&ctx->iface_lock, K_FOREVER); + + sys_put_be16(start_addr, &ctx->tx_frame.data[0]); + sys_put_be16(num_coils, &ctx->tx_frame.data[2]); + + num_bytes = (uint8_t)(((num_coils - 1) / 8) + 1); + ctx->tx_frame.data[4] = num_bytes; + data_ptr = &ctx->tx_frame.data[5]; + ctx->tx_frame.length = 5 + num_bytes; + + memcpy(data_ptr, coil_tbl, num_bytes); + + err = mbm_send_cmd(ctx, node_addr, MODBUS_FC15_COILS_WR, NULL); + k_mutex_unlock(&ctx->iface_lock); + + return err; +} + +int mb_rtu_write_holding_regs(const int iface, + const uint8_t node_addr, + const uint16_t start_addr, + uint16_t *const reg_buf, + const uint16_t num_regs) +{ + struct mb_rtu_context *ctx = mb_get_context(iface); + uint8_t num_bytes; + int err; + uint8_t *data_ptr; + + if (ctx == NULL) { + return -ENODEV; + } + + k_mutex_lock(&ctx->iface_lock, K_FOREVER); + + sys_put_be16(start_addr, &ctx->tx_frame.data[0]); + sys_put_be16(num_regs, &ctx->tx_frame.data[2]); + + num_bytes = (uint8_t) (num_regs * sizeof(uint16_t)); + ctx->tx_frame.length = num_bytes + 5; + ctx->tx_frame.data[4] = num_bytes; + data_ptr = &ctx->tx_frame.data[5]; + + for (uint16_t i = 0; i < num_regs; i++) { + sys_put_be16(reg_buf[i], data_ptr); + data_ptr += sizeof(uint16_t); + } + + err = mbm_send_cmd(ctx, node_addr, MODBUS_FC16_HOLDING_REGS_WR, NULL); + k_mutex_unlock(&ctx->iface_lock); + + return err; +} + +#ifdef CONFIG_MODBUS_RTU_FP_EXTENSIONS +int mb_rtu_write_holding_regs_fp(const int iface, + const uint8_t node_addr, + const uint16_t start_addr, + float *const reg_buf, + const uint16_t num_regs) +{ + struct mb_rtu_context *ctx = mb_get_context(iface); + uint8_t num_bytes; + int err; + uint8_t *data_ptr; + + if (ctx == NULL) { + return -ENODEV; + } + + k_mutex_lock(&ctx->iface_lock, K_FOREVER); + + sys_put_be16(start_addr, &ctx->tx_frame.data[0]); + sys_put_be16(num_regs, &ctx->tx_frame.data[2]); + + num_bytes = (uint8_t) (num_regs * sizeof(float)); + ctx->tx_frame.length = num_bytes + 5; + ctx->tx_frame.data[4] = num_bytes; + data_ptr = &ctx->tx_frame.data[5]; + + for (uint16_t i = 0; i < num_regs; i++) { + uint32_t reg_val; + + memcpy(®_val, ®_buf[i], sizeof(reg_val)); + sys_put_be32(reg_val, data_ptr); + data_ptr += sizeof(uint32_t); + } + + err = mbm_send_cmd(ctx, node_addr, MODBUS_FC16_HOLDING_REGS_WR, NULL); + k_mutex_unlock(&ctx->iface_lock); + + return err; +} +#endif diff --git a/subsys/modbus/mb_rtu_core.c b/subsys/modbus/mb_rtu_core.c new file mode 100644 index 00000000000..5a7e4c70a36 --- /dev/null +++ b/subsys/modbus/mb_rtu_core.c @@ -0,0 +1,654 @@ +/* + * Copyright (c) 2020 PHYTEC Messtechnik GmbH + * + * SPDX-License-Identifier: Apache-2.0 + */ + +/* + * This file is based on mb.c and mb_util.c from uC/Modbus Stack. + * + * uC/Modbus + * The Embedded Modbus Stack + * + * Copyright 2003-2020 Silicon Laboratories Inc. www.silabs.com + * + * SPDX-License-Identifier: APACHE-2.0 + * + * This software is subject to an open source license and is distributed by + * Silicon Laboratories Inc. pursuant to the terms of the Apache License, + * Version 2.0 available at www.apache.org/licenses/LICENSE-2.0. + */ + +#include +LOG_MODULE_REGISTER(mb_rtu, CONFIG_MODBUS_RTU_LOG_LEVEL); + +#include +#include +#include +#include + +#define DT_DRV_COMPAT zephyr_modbus_serial + +#define MODBUS_DT_GET_DEV(port) {.dev_name = DT_INST_BUS_LABEL(port),}, + +static struct mb_rtu_context mb_ctx_tbl[] = { + DT_INST_FOREACH_STATUS_OKAY(MODBUS_DT_GET_DEV) +}; + +#ifdef CONFIG_MODBUS_RTU_ASCII_MODE +/* The function calculates an 8-bit Longitudinal Redundancy Check. */ +static uint8_t mb_ascii_get_lrc(uint8_t *src, size_t length) +{ + uint8_t lrc = 0; + uint8_t tmp; + uint8_t *pblock = src; + + while (length-- > 0) { + /* Add the data byte to LRC, increment data pointer. */ + if (hex2bin(pblock, 2, &tmp, sizeof(tmp)) != sizeof(tmp)) { + return 0; + } + + lrc += tmp; + pblock += 2; + } + + /* Two complement the binary sum */ + lrc = ~lrc + 1; + + return lrc; +} + +/* Parses and converts an ASCII mode frame into a Modbus RTU frame. */ +static int mb_rx_ascii_frame(struct mb_rtu_context *ctx) +{ + uint8_t *pmsg; + uint8_t *prx_data; + uint16_t rx_size; + uint8_t frame_lrc; + uint8_t calc_lrc; + + rx_size = ctx->uart_buf_ctr; + prx_data = &ctx->rx_frame.data[0]; + + if (!(rx_size & 0x01)) { + LOG_WRN("Message should have an odd number of bytes"); + return -EMSGSIZE; + } + + if (rx_size > MODBUS_ASCII_MIN_MSG_SIZE) { + LOG_WRN("Frame length error"); + return -EMSGSIZE; + } + + if ((ctx->uart_buf[0] != MODBUS_ASCII_START_FRAME_CHAR) || + (ctx->uart_buf[rx_size - 2] != MODBUS_ASCII_END_FRAME_CHAR1) || + (ctx->uart_buf[rx_size - 1] != MODBUS_ASCII_END_FRAME_CHAR2)) { + LOG_WRN("Frame character error"); + return -EMSGSIZE; + } + + /* Take away for the ':', CR, and LF */ + rx_size -= 3; + /* Point past the ':' to the address. */ + pmsg = &ctx->uart_buf[1]; + + hex2bin(pmsg, 2, &ctx->rx_frame.addr, 1); + pmsg += 2; + rx_size -= 2; + hex2bin(pmsg, 2, &ctx->rx_frame.fc, 1); + pmsg += 2; + rx_size -= 2; + + /* Get the data from the message */ + ctx->rx_frame.length = 0; + while (rx_size > 2) { + hex2bin(pmsg, 2, prx_data, 1); + prx_data++; + pmsg += 2; + rx_size -= 2; + /* Increment the number of Modbus packets received */ + ctx->rx_frame.length++; + } + + /* Subtract the Address and function code */ + ctx->rx_frame.length -= 2; + /* Extract the message's LRC */ + hex2bin(pmsg, 2, &frame_lrc, 1); + ctx->rx_frame.crc = frame_lrc; + + /* + * The LRC is calculated on the ADDR, FC and Data fields, + * not the ':', CR/LF and LRC placed in the message + * by the sender. We thus need to subtract 5 'ASCII' characters + * from the received message to exclude these. + */ + calc_lrc = mb_ascii_get_lrc(&ctx->uart_buf[1], + (ctx->uart_buf_ctr - 5) / 2); + + ctx->uart_buf_ctr = 0; + ctx->uart_buf_ptr = &ctx->uart_buf[0]; + + if (calc_lrc != frame_lrc) { + LOG_ERR("Calculated LRC does not match received LRC"); + return -EIO; + } + + return 0; +} + +static uint8_t *mb_bin2hex(uint8_t value, uint8_t *pbuf) +{ + uint8_t u_nibble = (value >> 4) & 0x0F; + uint8_t l_nibble = value & 0x0F; + + hex2char(u_nibble, pbuf); + pbuf++; + hex2char(l_nibble, pbuf); + pbuf++; + + return pbuf; +} + +static void mb_tx_ascii_frame(struct mb_rtu_context *ctx) +{ + uint16_t tx_bytes = 0; + uint8_t lrc; + uint8_t *pbuf; + + /* Place the start-of-frame character into output buffer */ + ctx->uart_buf[0] = MODBUS_ASCII_START_FRAME_CHAR; + tx_bytes = 1; + + pbuf = &ctx->uart_buf[1]; + pbuf = mb_bin2hex(ctx->tx_frame.addr, pbuf); + tx_bytes += 2; + pbuf = mb_bin2hex(ctx->tx_frame.fc, pbuf); + tx_bytes += 2; + + for (int i = 0; i < ctx->tx_frame.length; i++) { + pbuf = mb_bin2hex(ctx->tx_frame.data[i], pbuf); + tx_bytes += 2; + } + + /* + * Add the LRC checksum in the packet. + * + * The LRC is calculated on the ADDR, FC and Data fields, + * not the ':' which was inserted in the uart_buf[]. + * Thus we subtract 1 ASCII character from the LRC. + * The LRC and CR/LF bytes are not YET in the .uart_buf[]. + */ + lrc = mb_ascii_get_lrc(&ctx->uart_buf[1], (tx_bytes - 1) / 2); + pbuf = mb_bin2hex(lrc, pbuf); + tx_bytes += 2; + + *pbuf++ = MODBUS_ASCII_END_FRAME_CHAR1; + *pbuf++ = MODBUS_ASCII_END_FRAME_CHAR2; + tx_bytes += 2; + + /* Update the total number of bytes to send */ + ctx->uart_buf_ctr = tx_bytes; + ctx->uart_buf_ptr = &ctx->uart_buf[0]; + + LOG_DBG("Start frame transmission"); + uart_irq_rx_disable(ctx->dev); + uart_irq_tx_enable(ctx->dev); +} +#else +static int mb_rx_ascii_frame(struct mb_rtu_context *ctx) +{ + return 0; +} + +static void mb_tx_ascii_frame(struct mb_rtu_context *ctx) +{ +} +#endif + +static uint16_t mb_rtu_crc16(uint8_t *src, size_t length) +{ + uint16_t crc = 0xFFFF; + uint8_t shiftctr; + bool flag; + uint8_t *pblock = src; + + while (length > 0) { + length--; + crc ^= (uint16_t)*pblock++; + shiftctr = 8; + do { + /* Determine if the shift out of rightmost bit is 1 */ + flag = (crc & 0x0001) ? true : false; + /* Shift CRC to the right one bit. */ + crc >>= 1; + /* + * If bit shifted out of rightmost bit was a 1 + * exclusive OR the CRC with the generating polynomial. + */ + if (flag == true) { + crc ^= MODBUS_CRC16_POLY; + } + + shiftctr--; + } while (shiftctr > 0); + } + + return crc; +} + +/* Copy Modbus RTU frame and check if the CRC is valid. */ +static int mb_rx_rtu_frame(struct mb_rtu_context *ctx) +{ + uint16_t calc_crc; + uint16_t crc_idx; + uint8_t *data_ptr; + + /* Is the message long enough? */ + if ((ctx->uart_buf_ctr < MODBUS_RTU_MIN_MSG_SIZE) || + (ctx->uart_buf_ctr > CONFIG_MODBUS_RTU_BUFFER_SIZE)) { + LOG_WRN("Frame length error"); + return -EMSGSIZE; + } + + ctx->rx_frame.addr = ctx->uart_buf[0]; + ctx->rx_frame.fc = ctx->uart_buf[1]; + data_ptr = &ctx->uart_buf[2]; + /* Payload length without node address, function code, and CRC */ + ctx->rx_frame.length = ctx->uart_buf_ctr - 4; + /* CRC index */ + crc_idx = ctx->uart_buf_ctr - sizeof(uint16_t); + + memcpy(ctx->rx_frame.data, data_ptr, ctx->rx_frame.length); + + ctx->rx_frame.crc = sys_get_le16(&ctx->uart_buf[crc_idx]); + /* Calculate CRC over address, function code, and payload */ + calc_crc = mb_rtu_crc16(&ctx->uart_buf[0], + ctx->uart_buf_ctr - sizeof(ctx->rx_frame.crc)); + + ctx->uart_buf_ctr = 0; + ctx->uart_buf_ptr = &ctx->uart_buf[0]; + + if (ctx->rx_frame.crc != calc_crc) { + LOG_WRN("Calculated CRC does not match received CRC"); + return -EIO; + } + + return 0; +} + +static void mb_tx_rtu_frame(struct mb_rtu_context *ctx) +{ + uint16_t tx_bytes = 0; + uint8_t *data_ptr; + + ctx->uart_buf[0] = ctx->tx_frame.addr; + ctx->uart_buf[1] = ctx->tx_frame.fc; + tx_bytes = 2 + ctx->tx_frame.length; + data_ptr = &ctx->uart_buf[2]; + + memcpy(data_ptr, ctx->tx_frame.data, ctx->tx_frame.length); + + ctx->tx_frame.crc = mb_rtu_crc16(&ctx->uart_buf[0], + ctx->tx_frame.length + 2); + sys_put_le16(ctx->tx_frame.crc, + &ctx->uart_buf[ctx->tx_frame.length + 2]); + tx_bytes += 2; + + ctx->uart_buf_ctr = tx_bytes; + ctx->uart_buf_ptr = &ctx->uart_buf[0]; + + LOG_HEXDUMP_DBG(ctx->uart_buf, ctx->uart_buf_ctr, "uart_buf"); + LOG_DBG("Start frame transmission"); + uart_irq_rx_disable(ctx->dev); + uart_irq_tx_enable(ctx->dev); +} + +void mb_tx_frame(struct mb_rtu_context *ctx) +{ + if (IS_ENABLED(CONFIG_MODBUS_RTU_ASCII_MODE) && + (ctx->ascii_mode == true)) { + mb_tx_ascii_frame(ctx); + } else { + mb_tx_rtu_frame(ctx); + } +} + +/* + * A byte has been received from a serial port. We just store it in the buffer + * for processing when a complete packet has been received. + */ +static void mb_cb_handler_rx(struct mb_rtu_context *ctx) +{ + if ((ctx->ascii_mode == true) && + IS_ENABLED(CONFIG_MODBUS_RTU_ASCII_MODE)) { + uint8_t c; + + if (uart_fifo_read(ctx->dev, &c, 1) != 1) { + LOG_ERR("Failed to read UART"); + return; + } + + if (c == MODBUS_ASCII_START_FRAME_CHAR) { + /* Restart a new frame */ + ctx->uart_buf_ptr = &ctx->uart_buf[0]; + ctx->uart_buf_ctr = 0; + } + + if (ctx->uart_buf_ctr < CONFIG_MODBUS_RTU_BUFFER_SIZE) { + *ctx->uart_buf_ptr++ = c; + ctx->uart_buf_ctr++; + } + + if (c == MODBUS_ASCII_END_FRAME_CHAR2) { + k_work_submit(&ctx->server_work); + } + + } else { + int n; + + /* Restart timer on a new character */ + k_timer_start(&ctx->rtu_timer, + K_USEC(ctx->rtu_timeout), K_NO_WAIT); + + n = uart_fifo_read(ctx->dev, ctx->uart_buf_ptr, + (CONFIG_MODBUS_RTU_BUFFER_SIZE - + ctx->uart_buf_ctr)); + + ctx->uart_buf_ptr += n; + ctx->uart_buf_ctr += n; + } +} + +static void mb_cb_handler_tx(struct mb_rtu_context *ctx) +{ + int n; + + if (ctx->uart_buf_ctr > 0) { + n = uart_fifo_fill(ctx->dev, ctx->uart_buf_ptr, + ctx->uart_buf_ctr); + ctx->uart_buf_ctr -= n; + ctx->uart_buf_ptr += n; + } else { + /* Disable transmission */ + ctx->uart_buf_ptr = &ctx->uart_buf[0]; + uart_irq_tx_disable(ctx->dev); + uart_irq_rx_enable(ctx->dev); + } +} + +static void mb_uart_cb_handler(const struct device *dev, void *app_data) +{ + struct mb_rtu_context *ctx = (struct mb_rtu_context *)app_data; + + if (ctx == NULL) { + LOG_ERR("Modbus hardware is not properly initialized"); + return; + } + + while (uart_irq_update(ctx->dev) && uart_irq_is_pending(ctx->dev)) { + + if (uart_irq_rx_ready(ctx->dev)) { + mb_cb_handler_rx(ctx); + } + + if (uart_irq_tx_ready(ctx->dev)) { + mb_cb_handler_tx(ctx); + } + } +} + +static void mb_rx_handler(struct k_work *item) +{ + struct mb_rtu_context *ctx = + CONTAINER_OF(item, struct mb_rtu_context, server_work); + + if (ctx == NULL) { + LOG_ERR("Where is my pointer?"); + return; + } + + uart_irq_rx_disable(ctx->dev); + + if (IS_ENABLED(CONFIG_MODBUS_RTU_ASCII_MODE) && + (ctx->ascii_mode == true)) { + ctx->rx_frame_err = mb_rx_ascii_frame(ctx); + } else { + ctx->rx_frame_err = mb_rx_rtu_frame(ctx); + } + + if (ctx->client == true) { + k_sem_give(&ctx->client_wait_sem); + } else if (IS_ENABLED(CONFIG_MODBUS_RTU_SERVER)) { + if (mbs_rx_handler(ctx) == false) { + /* Server does not send response, re-enable RX */ + uart_irq_rx_enable(ctx->dev); + } + } +} + +/* This function is called when the RTU framing timer expires. */ +static void mb_rtu_tmr_handler(struct k_timer *t_id) +{ + struct mb_rtu_context *ctx; + + ctx = (struct mb_rtu_context *)k_timer_user_data_get(t_id); + + if (ctx == NULL) { + LOG_ERR("Failed to get Modbus context"); + return; + } + + k_work_submit(&ctx->server_work); +} + +static int mb_configure_uart(struct mb_rtu_context *ctx, + uint32_t baudrate, + enum uart_config_parity parity) +{ + const uint32_t if_delay_max = 3500000; + const uint32_t numof_bits = 11; + struct uart_config uart_cfg; + + ctx->dev = device_get_binding(ctx->dev_name); + if (ctx->dev == NULL) { + LOG_ERR("Failed to get UART device %s", + log_strdup(ctx->dev_name)); + return -ENODEV; + } + + uart_cfg.baudrate = baudrate, + uart_cfg.flow_ctrl = UART_CFG_FLOW_CTRL_NONE; + + if (ctx->ascii_mode == true) { + uart_cfg.data_bits = UART_CFG_DATA_BITS_7; + } else { + uart_cfg.data_bits = UART_CFG_DATA_BITS_8; + } + + switch (parity) { + case UART_CFG_PARITY_ODD: + case UART_CFG_PARITY_EVEN: + uart_cfg.parity = parity; + uart_cfg.stop_bits = UART_CFG_STOP_BITS_1; + break; + case UART_CFG_PARITY_NONE: + /* Use of no parity requires 2 stop bits */ + uart_cfg.parity = parity; + uart_cfg.stop_bits = UART_CFG_STOP_BITS_2; + break; + default: + return -EINVAL; + } + + if (uart_configure(ctx->dev, &uart_cfg) != 0) { + LOG_ERR("Failed to configure UART"); + return -EINVAL; + } + + uart_irq_callback_user_data_set(ctx->dev, mb_uart_cb_handler, ctx); + uart_irq_rx_enable(ctx->dev); + + if (baudrate <= 38400) { + ctx->rtu_timeout = (numof_bits * if_delay_max) / baudrate; + } else { + ctx->rtu_timeout = (numof_bits * if_delay_max) / 38400; + } + + LOG_INF("RTU timeout %u us", ctx->rtu_timeout); + + return 0; +} + +struct mb_rtu_context *mb_get_context(const uint8_t iface) +{ + struct mb_rtu_context *ctx; + + if (iface >= ARRAY_SIZE(mb_ctx_tbl)) { + LOG_ERR("Interface %u not available", iface); + return NULL; + } + + ctx = &mb_ctx_tbl[iface]; + + if (!atomic_test_bit(&ctx->state, MB_RTU_STATE_CONFIGURED)) { + LOG_ERR("Interface not configured"); + return NULL; + } + + return ctx; +} + +static struct mb_rtu_context *mb_cfg_iface(const uint8_t iface, + const uint8_t node_addr, + const uint32_t baud, + const enum uart_config_parity parity, + const uint32_t rx_timeout, + const bool client, + const bool ascii_mode) +{ + struct mb_rtu_context *ctx; + + if (iface >= ARRAY_SIZE(mb_ctx_tbl)) { + LOG_ERR("Interface %u not available", iface); + return NULL; + } + + ctx = &mb_ctx_tbl[iface]; + + if (atomic_test_and_set_bit(&ctx->state, MB_RTU_STATE_CONFIGURED)) { + LOG_ERR("Interface allready used"); + return NULL; + } + + if ((client == true) && + !IS_ENABLED(CONFIG_MODBUS_RTU_CLIENT)) { + LOG_ERR("Modbus client support is not enabled"); + ctx->client = false; + return NULL; + } + + ctx->rxwait_to = rx_timeout; + ctx->node_addr = node_addr; + ctx->client = client; + ctx->ascii_mode = ascii_mode; + ctx->mbs_user_cb = NULL; + k_mutex_init(&ctx->iface_lock); + + ctx->uart_buf_ctr = 0; + ctx->uart_buf_ptr = &ctx->uart_buf[0]; + + k_sem_init(&ctx->client_wait_sem, 0, 1); + k_work_init(&ctx->server_work, mb_rx_handler); + + if (IS_ENABLED(CONFIG_MODBUS_RTU_FC08_DIAGNOSTIC)) { + mbs_reset_statistics(ctx); + } + + if (mb_configure_uart(ctx, baud, parity) != 0) { + LOG_ERR("Failed to configure UART"); + return NULL; + } + + k_timer_init(&ctx->rtu_timer, mb_rtu_tmr_handler, NULL); + k_timer_user_data_set(&ctx->rtu_timer, ctx); + + return ctx; +} + +int mb_rtu_cfg_server(const uint8_t iface, const uint8_t node_addr, + const uint32_t baud, const enum uart_config_parity parity, + struct mbs_rtu_user_callbacks *const cb, + const bool ascii_mode) +{ + struct mb_rtu_context *ctx; + + if (!IS_ENABLED(CONFIG_MODBUS_RTU_SERVER)) { + LOG_ERR("Modbus server support is not enabled"); + return -ENOTSUP; + } + + if (cb == NULL) { + LOG_ERR("User callbacks should be available"); + return -EINVAL; + } + + ctx = mb_cfg_iface(iface, node_addr, baud, + parity, 0, false, ascii_mode); + + if (ctx == NULL) { + return -EINVAL; + } + + ctx->mbs_user_cb = cb; + + return 0; +} + +int mb_rtu_cfg_client(const uint8_t iface, + const uint32_t baud, const enum uart_config_parity parity, + const uint32_t rx_timeout, + const bool ascii_mode) +{ + struct mb_rtu_context *ctx; + + if (!IS_ENABLED(CONFIG_MODBUS_RTU_CLIENT)) { + LOG_ERR("Modbus client support is not enabled"); + return -ENOTSUP; + } + + ctx = mb_cfg_iface(iface, 0, baud, + parity, rx_timeout, true, ascii_mode); + + if (ctx == NULL) { + return -EINVAL; + } + + return 0; +} + +int mb_rtu_disable_iface(const uint8_t iface) +{ + struct mb_rtu_context *ctx; + + if (iface >= ARRAY_SIZE(mb_ctx_tbl)) { + LOG_ERR("Interface %u not available", iface); + return -EINVAL; + } + + ctx = &mb_ctx_tbl[iface]; + + uart_irq_tx_disable(ctx->dev); + uart_irq_rx_disable(ctx->dev); + k_timer_stop(&ctx->rtu_timer); + + ctx->rxwait_to = 0; + ctx->node_addr = 0; + ctx->ascii_mode = false; + ctx->mbs_user_cb = NULL; + atomic_clear_bit(&ctx->state, MB_RTU_STATE_CONFIGURED); + + LOG_INF("Disable Modbus interface"); + + return 0; +} diff --git a/subsys/modbus/mb_rtu_internal.h b/subsys/modbus/mb_rtu_internal.h new file mode 100644 index 00000000000..b3a77d3fd61 --- /dev/null +++ b/subsys/modbus/mb_rtu_internal.h @@ -0,0 +1,142 @@ +/* + * Copyright (c) 2020 PHYTEC Messtechnik GmbH + * + * SPDX-License-Identifier: Apache-2.0 + */ + +/* + * Parts of this file are based on mb.h from uC/Modbus Stack. + * + * uC/Modbus + * The Embedded Modbus Stack + * + * Copyright 2003-2020 Silicon Laboratories Inc. www.silabs.com + * + * SPDX-License-Identifier: APACHE-2.0 + * + * This software is subject to an open source license and is distributed by + * Silicon Laboratories Inc. pursuant to the terms of the Apache License, + * Version 2.0 available at www.apache.org/licenses/LICENSE-2.0. + */ + +#ifndef ZEPHYR_INCLUDE_MODBUS_RTU_INTERNAL_H_ +#define ZEPHYR_INCLUDE_MODBUS_RTU_INTERNAL_H_ + +#include +#include + +#ifdef CONFIG_MODBUS_RTU_FP_EXTENSIONS +#define MODBUS_RTU_FP_ADDR 5000 +#else +#define MODBUS_RTU_FP_ADDR UINT16_MAX +#endif + +#define MODBUS_RTU_MTU 256 + +/* Modbus function codes */ +#define MODBUS_FC01_COIL_RD 1 +#define MODBUS_FC02_DI_RD 2 +#define MODBUS_FC03_HOLDING_REG_RD 3 +#define MODBUS_FC04_IN_REG_RD 4 +#define MODBUS_FC05_COIL_WR 5 +#define MODBUS_FC06_HOLDING_REG_WR 6 +#define MODBUS_FC08_DIAGNOSTICS 8 +#define MODBUS_FC15_COILS_WR 15 +#define MODBUS_FC16_HOLDING_REGS_WR 16 + +/* Diagnostic sub-function codes */ +#define MODBUS_FC08_SUBF_QUERY 0 +#define MODBUS_FC08_SUBF_CLR_CTR 10 +#define MODBUS_FC08_SUBF_BUS_MSG_CTR 11 +#define MODBUS_FC08_SUBF_BUS_CRC_CTR 12 +#define MODBUS_FC08_SUBF_BUS_EXCEPT_CTR 13 +#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 + +/* Modbus RTU (ASCII) constants */ +#define MODBUS_COIL_OFF_CODE 0x0000 +#define MODBUS_COIL_ON_CODE 0xFF00 +#define MODBUS_RTU_MIN_MSG_SIZE 4 +#define MODBUS_CRC16_POLY 0xA001 +#define MODBUS_ASCII_MIN_MSG_SIZE 11 +#define MODBUS_ASCII_START_FRAME_CHAR ':' +#define MODBUS_ASCII_END_FRAME_CHAR1 '\r' +#define MODBUS_ASCII_END_FRAME_CHAR2 '\n' + +struct mb_rtu_frame { + uint16_t length; + uint8_t addr; + uint8_t fc; + uint8_t data[CONFIG_MODBUS_RTU_BUFFER_SIZE - 4]; + uint16_t crc; +}; + +#define MB_RTU_STATE_CONFIGURED 0 + +struct mb_rtu_context { + /* UART device name */ + const char *dev_name; + /* UART device */ + const struct device *dev; + /* True if ASCII mode is enabled */ + bool ascii_mode; + /* True if interface is configured as client */ + bool client; + /* Amount of time client is willing to wait for response from server */ + uint32_t rxwait_to; + /* RTU timeout (maximum inter-frame delay) */ + uint32_t rtu_timeout; + /* Pointer to user server callbacks */ + struct mbs_rtu_user_callbacks *mbs_user_cb; + /* Interface state */ + atomic_t state; + /* Pointer to current position in buffer */ + uint8_t *uart_buf_ptr; + + /* Client's mutually exclusive access */ + struct k_mutex iface_lock; + /* Wait for response semaphore */ + struct k_sem client_wait_sem; + /* Server work item */ + struct k_work server_work; + /* RTU timer to detect frame end point */ + struct k_timer rtu_timer; + /* Received frame */ + struct mb_rtu_frame rx_frame; + /* Frame to transmit */ + struct mb_rtu_frame tx_frame; + + /* Number of bytes received or to send */ + uint16_t uart_buf_ctr; + /* Records error from frame reception, e.g. CRC error */ + uint16_t rx_frame_err; + +#ifdef CONFIG_MODBUS_RTU_FC08_DIAGNOSTIC + uint16_t mbs_msg_ctr; + uint16_t mbs_crc_err_ctr; + uint16_t mbs_except_ctr; + uint16_t mbs_server_msg_ctr; + uint16_t mbs_noresp_ctr; +#endif + /* Node address */ + uint8_t node_addr; + /* Storage of received characters or characters to send */ + uint8_t uart_buf[CONFIG_MODBUS_RTU_BUFFER_SIZE]; + +}; + +struct mb_rtu_context *mb_get_context(const uint8_t iface); +int mb_rx_frame(struct mb_rtu_context *ctx); +void mb_tx_frame(struct mb_rtu_context *ctx); + +bool mbs_rx_handler(struct mb_rtu_context *ctx); +void mbs_reset_statistics(struct mb_rtu_context *pch); + +#endif /* ZEPHYR_INCLUDE_MODBUS_RTU_INTERNAL_H_ */ diff --git a/subsys/modbus/mb_rtu_server.c b/subsys/modbus/mb_rtu_server.c new file mode 100644 index 00000000000..518878028c6 --- /dev/null +++ b/subsys/modbus/mb_rtu_server.c @@ -0,0 +1,1018 @@ +/* + * Copyright (c) 2020 PHYTEC Messtechnik GmbH + * + * SPDX-License-Identifier: Apache-2.0 + */ + +/* + * This file is based on mbs_core.c from uC/Modbus Stack. + * + * uC/Modbus + * The Embedded Modbus Stack + * + * Copyright 2003-2020 Silicon Laboratories Inc. www.silabs.com + * + * SPDX-License-Identifier: APACHE-2.0 + * + * This software is subject to an open source license and is distributed by + * Silicon Laboratories Inc. pursuant to the terms of the Apache License, + * Version 2.0 available at www.apache.org/licenses/LICENSE-2.0. + */ + +#include +#include +#include + +#include +LOG_MODULE_REGISTER(mb_rtu_s, CONFIG_MODBUS_RTU_LOG_LEVEL); + +/* + * This functions are used to reset and update server's + * statistics and communications counters. + */ +#ifdef CONFIG_MODBUS_RTU_FC08_DIAGNOSTIC +void mbs_reset_statistics(struct mb_rtu_context *ctx) +{ + /* Initialize all MODBUS event counters. */ + ctx->mbs_msg_ctr = 0; + ctx->mbs_crc_err_ctr = 0; + ctx->mbs_except_ctr = 0; + ctx->mbs_server_msg_ctr = 0; + ctx->mbs_noresp_ctr = 0; +} + +static void update_msg_ctr(struct mb_rtu_context *ctx) +{ + ctx->mbs_msg_ctr++; +} + +static void update_crcerr_ctr(struct mb_rtu_context *ctx) +{ + ctx->mbs_crc_err_ctr++; +} + +static void update_excep_ctr(struct mb_rtu_context *ctx) +{ + ctx->mbs_except_ctr++; +} + +static void update_server_msg_ctr(struct mb_rtu_context *ctx) +{ + ctx->mbs_server_msg_ctr++; +} + +static void update_noresp_ctr(struct mb_rtu_context *ctx) +{ + ctx->mbs_noresp_ctr++; +} +#else +#define mbs_reset_statistics(...) +#define update_msg_ctr(...) +#define update_crcerr_ctr(...) +#define update_excep_ctr(...) +#define update_server_msg_ctr(...) +#define update_noresp_ctr(...) +#endif /* CONFIG_MODBUS_RTU_FC08_DIAGNOSTIC */ + +/* + * This function sets the indicated error response code into the response frame. + * Then the routine is called to calculate the error check value. + */ +static void mbs_exception_rsp(struct mb_rtu_context *ctx, uint8_t excep_code) +{ + const uint8_t excep_bit = BIT(7); + + LOG_INF("FC 0x%02x Error 0x%02x", ctx->rx_frame.fc, excep_code); + + update_excep_ctr(ctx); + + ctx->tx_frame.fc |= excep_bit; + ctx->tx_frame.data[0] = excep_code; + ctx->tx_frame.length = 1; +} + +/* + * FC 01 (0x01) Read Coils + * + * Request Payload: + * Function code 1 Byte + * Starting Address 2 Bytes + * Quantity of Coils 2 Bytes + * + * Response Payload: + * Function code 1 Byte + * Byte count 1 Bytes + * Coil status N * 1 Byte + */ +static bool mbs_fc01_coil_read(struct mb_rtu_context *ctx) +{ + const uint16_t coils_limit = 2000; + const uint8_t request_len = 4; + uint8_t *presp; + bool coil_state; + int err; + uint16_t coil_addr; + uint16_t coil_qty; + uint16_t num_bytes; + uint8_t bit_mask; + uint16_t coil_cntr; + + if (ctx->rx_frame.length != request_len) { + LOG_ERR("Wrong request length"); + return false; + } + + if (ctx->mbs_user_cb->coil_rd == NULL) { + mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_FC); + return true; + } + + coil_addr = sys_get_be16(&ctx->rx_frame.data[0]); + coil_qty = sys_get_be16(&ctx->rx_frame.data[2]); + + /* Make sure we don't exceed the allowed limit per request */ + if (coil_qty == 0 || coil_qty > coils_limit) { + LOG_ERR("Number of coils limit exceeded"); + mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_DATA_VAL); + return true; + } + + /* Calculate byte count for response. */ + num_bytes = ((coil_qty - 1) / 8) + 1; + /* Number of data bytes + byte count. */ + ctx->tx_frame.length = num_bytes + 1; + /* Set number of data bytes in response message. */ + ctx->tx_frame.data[0] = (uint8_t)num_bytes; + + /* Clear bytes in response */ + presp = &ctx->tx_frame.data[1]; + memset(presp, 0, num_bytes); + + /* Reset the pointer to the start of the response payload */ + presp = &ctx->tx_frame.data[1]; + /* Start with bit 0 in response byte data mask. */ + bit_mask = BIT(0); + /* Initialize loop counter. */ + coil_cntr = 0; + + /* Loop through each coil requested. */ + while (coil_cntr < coil_qty) { + + err = ctx->mbs_user_cb->coil_rd(coil_addr, &coil_state); + if (err != 0) { + LOG_INF("Coil address not supported"); + mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_DATA_ADDR); + return true; + } + + if (coil_state) { + *presp |= bit_mask; + } + + coil_addr++; + /* Increment coil counter. */ + coil_cntr++; + /* Determine if 8 data bits have been filled. */ + if ((coil_cntr % 8) == 0) { + /* Reset the data mask. */ + bit_mask = BIT(0); + /* Increment frame data index. */ + presp++; + } else { + /* + * Still in same data byte, so shift the data mask + * to the next higher bit position. + */ + bit_mask <<= 1; + } + } + + return true; +} + +/* + * FC 02 (0x02) Read Discrete Inputs + * + * Request Payload: + * Function code 1 Byte + * Starting Address 2 Bytes + * Quantity of Inputs 2 Bytes + * + * Response Payload: + * Function code 1 Byte + * Byte count 1 Bytes + * Input status N * 1 Byte + */ +static bool mbs_fc02_di_read(struct mb_rtu_context *ctx) +{ + const uint16_t di_limit = 2000; + const uint8_t request_len = 4; + uint8_t *presp; + bool di_state; + int err; + uint16_t di_addr; + uint16_t di_qty; + uint16_t num_bytes; + uint8_t bit_mask; + uint16_t di_cntr; + + if (ctx->rx_frame.length != request_len) { + LOG_ERR("Wrong request length"); + return false; + } + + if (ctx->mbs_user_cb->discrete_input_rd == NULL) { + mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_FC); + return true; + } + + di_addr = sys_get_be16(&ctx->rx_frame.data[0]); + di_qty = sys_get_be16(&ctx->rx_frame.data[2]); + + /* Make sure we don't exceed the allowed limit per request */ + if (di_qty == 0 || di_qty > di_limit) { + LOG_ERR("Number of inputs limit exceeded"); + mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_DATA_VAL); + return true; + } + + /* Get number of bytes needed for response. */ + num_bytes = ((di_qty - 1) / 8) + 1; + /* Number of data bytes + byte count. */ + ctx->tx_frame.length = num_bytes + 1; + /* Set number of data bytes in response message. */ + ctx->tx_frame.data[0] = (uint8_t)num_bytes; + + /* Clear bytes in response */ + presp = &ctx->tx_frame.data[1]; + for (di_cntr = 0; di_cntr < num_bytes; di_cntr++) { + *presp++ = 0x00; + } + + /* Reset the pointer to the start of the response payload */ + presp = &ctx->tx_frame.data[1]; + /* Start with bit 0 in response byte data mask. */ + bit_mask = BIT(0); + /* Initialize loop counter. */ + di_cntr = 0; + + /* Loop through each DI requested. */ + while (di_cntr < di_qty) { + + err = ctx->mbs_user_cb->discrete_input_rd(di_addr, &di_state); + if (err != 0) { + LOG_INF("Discrete input address not supported"); + mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_DATA_ADDR); + return true; + } + + if (di_state) { + *presp |= bit_mask; + } + + di_addr++; + /* Increment DI counter. */ + di_cntr++; + /* Determine if 8 data bits have been filled. */ + if ((di_cntr % 8) == 0) { + /* Reset the data mask. */ + bit_mask = BIT(0); + /* Increment data frame index. */ + presp++; + } else { + /* + * Still in same data byte, so shift the data mask + * to the next higher bit position. + */ + bit_mask <<= 1; + } + } + + return true; +} + +/* + * 03 (0x03) Read Holding Registers + * + * Request Payload: + * Function code 1 Byte + * Starting Address 2 Bytes + * Quantity of Registers 2 Bytes + * + * Response Payload: + * Function code 1 Byte + * Byte count 1 Bytes + * Register Value N * 2 Byte + */ +static bool mbs_fc03_hreg_read(struct mb_rtu_context *ctx) +{ + const uint16_t regs_limit = 125; + const uint8_t request_len = 4; + uint8_t *presp; + uint16_t err; + uint16_t reg_addr; + uint16_t reg_qty; + uint16_t num_bytes; + + if (ctx->rx_frame.length != request_len) { + LOG_ERR("Wrong request length"); + return false; + } + + reg_addr = sys_get_be16(&ctx->rx_frame.data[0]); + reg_qty = sys_get_be16(&ctx->rx_frame.data[2]); + + if ((reg_addr < MODBUS_RTU_FP_ADDR) || + !IS_ENABLED(CONFIG_MODBUS_RTU_FP_EXTENSIONS)) { + /* Read integer register */ + if (ctx->mbs_user_cb->holding_reg_rd == NULL) { + mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_FC); + return true; + } + + if (reg_qty == 0 || reg_qty > regs_limit) { + LOG_ERR("Number of registers limit exceeded"); + mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_DATA_VAL); + return true; + } + + /* Get number of bytes needed for response. */ + num_bytes = (uint8_t)(reg_qty * sizeof(uint16_t)); + } else { + /* Read floating-point register */ + if (ctx->mbs_user_cb->holding_reg_rd_fp == NULL) { + mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_FC); + return true; + } + + if (reg_qty == 0 || reg_qty > (regs_limit / 2)) { + LOG_ERR("Number of registers limit exceeded"); + mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_DATA_VAL); + return true; + } + + /* Get number of bytes needed for response. */ + num_bytes = (uint8_t)(reg_qty * sizeof(float)); + } + + /* Number of data bytes + byte count. */ + ctx->tx_frame.length = num_bytes + 1; + /* Set number of data bytes in response message. */ + ctx->tx_frame.data[0] = (uint8_t)num_bytes; + + /* Reset the pointer to the start of the response payload */ + presp = &ctx->tx_frame.data[1]; + /* Loop through each register requested. */ + while (reg_qty > 0) { + if (reg_addr < MODBUS_RTU_FP_ADDR) { + uint16_t reg; + + /* Read integer register */ + err = ctx->mbs_user_cb->holding_reg_rd(reg_addr, ®); + if (err == 0) { + sys_put_be16(reg, presp); + presp += sizeof(uint16_t); + } + + } else if (IS_ENABLED(CONFIG_MODBUS_RTU_FP_EXTENSIONS)) { + float fp; + uint32_t reg; + + /* Read floating-point register */ + err = ctx->mbs_user_cb->holding_reg_rd_fp(reg_addr, &fp); + if (err == 0) { + memcpy(®, &fp, sizeof(reg)); + sys_put_be32(reg, presp); + presp += sizeof(uint32_t); + } + } + + if (err != 0) { + LOG_INF("Holding register address not supported"); + mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_DATA_ADDR); + return true; + } + + /* Increment current register address */ + reg_addr++; + reg_qty--; + } + + return true; +} + +/* + * 04 (0x04) Read Input Registers + * + * Request Payload: + * Function code 1 Byte + * Starting Address 2 Bytes + * Quantity of Registers 2 Bytes + * + * Response Payload: + * Function code 1 Byte + * Byte count 1 Bytes + * Register Value N * 2 Byte + */ +static bool mbs_fc04_inreg_read(struct mb_rtu_context *ctx) +{ + const uint16_t regs_limit = 125; + const uint8_t request_len = 4; + uint8_t *presp; + int err; + uint16_t reg_addr; + uint16_t reg_qty; + uint16_t num_bytes; + + if (ctx->rx_frame.length != request_len) { + LOG_ERR("Wrong request length"); + return false; + } + + reg_addr = sys_get_be16(&ctx->rx_frame.data[0]); + reg_qty = sys_get_be16(&ctx->rx_frame.data[2]); + + if ((reg_addr < MODBUS_RTU_FP_ADDR) || + !IS_ENABLED(CONFIG_MODBUS_RTU_FP_EXTENSIONS)) { + /* Read integer register */ + if (ctx->mbs_user_cb->input_reg_rd == NULL) { + mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_FC); + return true; + } + + if (reg_qty == 0 || reg_qty > regs_limit) { + LOG_ERR("Number of registers limit exceeded"); + mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_DATA_VAL); + return true; + } + + /* Get number of bytes needed for response. */ + num_bytes = (uint8_t)(reg_qty * sizeof(uint16_t)); + } else { + /* Read floating-point register */ + if (ctx->mbs_user_cb->input_reg_rd_fp == NULL) { + mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_FC); + return true; + } + + if (reg_qty == 0 || reg_qty > (regs_limit / 2)) { + LOG_ERR("Number of registers limit exceeded"); + mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_DATA_VAL); + return true; + } + + /* Get number of bytes needed for response. */ + num_bytes = (uint8_t)(reg_qty * sizeof(float)); + } + + /* Number of data bytes + byte count. */ + ctx->tx_frame.length = num_bytes + 1; + /* Set number of data bytes in response message. */ + ctx->tx_frame.data[0] = (uint8_t)num_bytes; + + /* Reset the pointer to the start of the response payload */ + presp = &ctx->tx_frame.data[1]; + /* Loop through each register requested. */ + while (reg_qty > 0) { + if (reg_addr < MODBUS_RTU_FP_ADDR) { + uint16_t reg; + + /* Read integer register */ + err = ctx->mbs_user_cb->input_reg_rd(reg_addr, ®); + if (err == 0) { + sys_put_be16(reg, presp); + presp += sizeof(uint16_t); + } + + } else if (IS_ENABLED(CONFIG_MODBUS_RTU_FP_EXTENSIONS)) { + float fp; + uint32_t reg; + + /* Read floating-point register */ + err = ctx->mbs_user_cb->input_reg_rd_fp(reg_addr, &fp); + if (err == 0) { + memcpy(®, &fp, sizeof(reg)); + sys_put_be32(reg, presp); + presp += sizeof(uint32_t); + } + } + + if (err != 0) { + LOG_INF("Input register address not supported"); + mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_DATA_ADDR); + return true; + } + + /* Increment current register number */ + reg_addr++; + reg_qty--; + } + + return true; +} + +/* + * FC 05 (0x05) Write Single Coil + * + * Request Payload: + * Function code 1 Byte + * Output Address 2 Bytes + * Output Value 2 Bytes + * + * Response Payload: + * Function code 1 Byte + * Output Address 2 Bytes + * Output Value 2 Bytes + */ +static bool mbs_fc05_coil_write(struct mb_rtu_context *ctx) +{ + const uint8_t request_len = 4; + const uint8_t response_len = 4; + int err; + uint16_t coil_addr; + uint16_t coil_val; + bool coil_state; + + if (ctx->rx_frame.length != request_len) { + LOG_ERR("Wrong request length %u", ctx->rx_frame.length); + return false; + } + + if (ctx->mbs_user_cb->coil_wr == NULL) { + mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_FC); + return true; + } + + /* Get the desired coil address and coil value */ + coil_addr = sys_get_be16(&ctx->rx_frame.data[0]); + coil_val = sys_get_be16(&ctx->rx_frame.data[2]); + + /* See if coil needs to be OFF? */ + if (coil_val == MODBUS_COIL_OFF_CODE) { + coil_state = false; + } else { + coil_state = true; + } + + err = ctx->mbs_user_cb->coil_wr(coil_addr, coil_state); + + if (err != 0) { + LOG_INF("Coil address not supported"); + mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_DATA_ADDR); + return true; + } + + /* Assemble response payload */ + ctx->tx_frame.length = response_len; + sys_put_be16(coil_addr, &ctx->tx_frame.data[0]); + sys_put_be16(coil_val, &ctx->tx_frame.data[2]); + + return true; +} + +/* + * 06 (0x06) Write Single Register + * + * Request Payload: + * Function code 1 Byte + * Register Address 2 Bytes + * Register Value 2 Bytes + * + * Response Payload: + * Function code 1 Byte + * Register Address 2 Bytes + * Register Value 2 Bytes + */ +static bool mbs_fc06_hreg_write(struct mb_rtu_context *ctx) +{ + const uint8_t request_len = 4; + const uint8_t response_len = 4; + int err; + uint16_t reg_addr; + uint16_t reg_val; + + if (ctx->rx_frame.length != request_len) { + LOG_ERR("Wrong request length %u", ctx->rx_frame.length); + return false; + } + + if (ctx->mbs_user_cb->holding_reg_wr == NULL) { + mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_FC); + return true; + } + + reg_addr = sys_get_be16(&ctx->rx_frame.data[0]); + reg_val = sys_get_be16(&ctx->rx_frame.data[2]); + + err = ctx->mbs_user_cb->holding_reg_wr(reg_addr, reg_val); + + if (err != 0) { + LOG_INF("Register address not supported"); + mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_DATA_ADDR); + return true; + } + + /* Assemble response payload */ + ctx->tx_frame.length = response_len; + sys_put_be16(reg_addr, &ctx->tx_frame.data[0]); + sys_put_be16(reg_val, &ctx->tx_frame.data[2]); + + return true; +} + +/* + * 08 (0x08) Diagnostics + * + * Request Payload: + * Function code 1 Byte + * Sub-function code 2 Bytes + * Data N * 2 Byte + * + * Response Payload: + * Function code 1 Byte + * Sub-function code 2 Bytes + * Data N * 2 Byte + */ +#ifdef CONFIG_MODBUS_RTU_FC08_DIAGNOSTIC +static bool mbs_fc08_diagnostics(struct mb_rtu_context *ctx) +{ + const uint8_t request_len = 4; + const uint8_t response_len = 4; + uint16_t sfunc; + uint16_t data; + + if (ctx->rx_frame.length != request_len) { + LOG_ERR("Wrong request length %u", ctx->rx_frame.length); + return false; + } + + sfunc = sys_get_be16(&ctx->rx_frame.data[0]); + data = sys_get_be16(&ctx->rx_frame.data[2]); + + switch (sfunc) { + case MODBUS_FC08_SUBF_QUERY: + /* Sub-function 0x00 return Query Data */ + break; + + case MODBUS_FC08_SUBF_CLR_CTR: + /* Sub-function 0x0A clear Counters and Diagnostic */ + mbs_reset_statistics(ctx); + break; + + case MODBUS_FC08_SUBF_BUS_MSG_CTR: + /* Sub-function 0x0B return Bus Message Count */ + data = ctx->mbs_msg_ctr; + break; + + case MODBUS_FC08_SUBF_BUS_CRC_CTR: + /* Sub-function 0x0C return Bus Communication Error Count */ + data = ctx->mbs_crc_err_ctr; + break; + + case MODBUS_FC08_SUBF_BUS_EXCEPT_CTR: + /* Sub-function 0x0D return Bus Exception Error Count */ + data = ctx->mbs_except_ctr; + break; + + case MODBUS_FC08_SUBF_SERVER_MSG_CTR: + /* Sub-function 0x0E return Server Message Count */ + data = ctx->mbs_server_msg_ctr; + break; + + case MODBUS_FC08_SUBF_SERVER_NO_RESP_CTR: + /* Sub-function 0x0F return Server No Response Count */ + data = ctx->mbs_noresp_ctr; + break; + + default: + LOG_INF("Sub-function not supported"); + mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_FC); + return true; + } + + /* Assemble response payload */ + ctx->tx_frame.length = response_len; + sys_put_be16(sfunc, &ctx->tx_frame.data[0]); + sys_put_be16(data, &ctx->tx_frame.data[2]); + + return true; +} +#else +static bool mbs_fc08_diagnostics(struct mb_rtu_context *ctx) +{ + mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_FC); + + return true; +} +#endif + +/* + * FC 15 (0x0F) Write Multiple Coils + * + * Request Payload: + * Function code 1 Byte + * Starting Address 2 Bytes + * Quantity of Outputs 2 Bytes + * Byte Count 1 Byte + * Outputs Value N * 1 Byte + * + * Response Payload: + * Function code 1 Byte + * Starting Address 2 Bytes + * Quantity of Outputs 2 Bytes + */ +static bool mbs_fc15_coils_write(struct mb_rtu_context *ctx) +{ + const uint16_t coils_limit = 2000; + const uint8_t request_len = 6; + const uint8_t response_len = 4; + uint8_t temp = 0; + int err; + uint16_t coil_addr; + uint16_t coil_qty; + uint16_t num_bytes; + uint16_t coil_cntr; + uint8_t data_ix; + bool coil_state; + + if (ctx->rx_frame.length < request_len) { + LOG_ERR("Wrong request length %u", ctx->rx_frame.length); + return false; + } + + if (ctx->mbs_user_cb->coil_wr == NULL) { + mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_FC); + return true; + } + + coil_addr = sys_get_be16(&ctx->rx_frame.data[0]); + coil_qty = sys_get_be16(&ctx->rx_frame.data[2]); + /* Get the byte count for the data. */ + num_bytes = ctx->rx_frame.data[4]; + + if (coil_qty == 0 || coil_qty > coils_limit) { + LOG_ERR("Number of coils limit exceeded"); + mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_DATA_VAL); + return true; + } + + /* Be sure byte count is valid for quantity of coils. */ + if (((((coil_qty - 1) / 8) + 1) != num_bytes) || + (ctx->rx_frame.length != (num_bytes + 5))) { + LOG_ERR("Mismatch in the number of coils"); + mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_DATA_VAL); + return true; + } + + coil_cntr = 0; + /* The 1st coil data byte is 6th element in payload */ + data_ix = 5; + /* Loop through each coil to be forced. */ + while (coil_cntr < coil_qty) { + /* Move to the next data byte after every eight bits. */ + if ((coil_cntr % 8) == 0) { + temp = ctx->rx_frame.data[data_ix++]; + } + + if (temp & BIT(0)) { + coil_state = true; + } else { + coil_state = false; + } + + err = ctx->mbs_user_cb->coil_wr(coil_addr + coil_cntr, + coil_state); + + if (err != 0) { + LOG_INF("Coil address not supported"); + mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_DATA_ADDR); + return true; + } + + /* Shift the data one bit position * to the right. */ + temp >>= 1; + /* Increment the COIL counter. */ + coil_cntr++; + } + + /* Assemble response payload */ + ctx->tx_frame.length = response_len; + sys_put_be16(coil_addr, &ctx->tx_frame.data[0]); + sys_put_be16(coil_qty, &ctx->tx_frame.data[2]); + + return true; +} + +/* + * FC16 (0x10) Write Multiple registers + * + * Request Payload: + * Function code 1 Byte + * Starting Address 2 Bytes + * Quantity of Registers 2 Bytes + * Byte Count 1 Byte + * Registers Value N * 1 Byte + * + * Response Payload: + * Function code 1 Byte + * Starting Address 2 Bytes + * Quantity of Registers 2 Bytes + * + * If the address of the request exceeds or is equal to MODBUS_RTU_FP_ADDR, + * then the function would write to multiple 'floating-point' according to + * the 'Daniels Flow Meter' extensions. This means that each register + * requested is considered as a 32-bit IEEE-754 floating-point format. + */ +static bool mbs_fc16_hregs_write(struct mb_rtu_context *ctx) +{ + const uint16_t regs_limit = 125; + const uint8_t request_len = 6; + const uint8_t response_len = 4; + uint8_t *prx_data; + int err; + uint16_t reg_addr; + uint16_t reg_qty; + uint16_t num_bytes; + uint8_t reg_size; + + if (ctx->rx_frame.length < request_len) { + LOG_ERR("Wrong request length %u", ctx->rx_frame.length); + return false; + } + + reg_addr = sys_get_be16(&ctx->rx_frame.data[0]); + reg_qty = sys_get_be16(&ctx->rx_frame.data[2]); + /* Get the byte count for the data. */ + num_bytes = ctx->rx_frame.data[4]; + + if ((reg_addr < MODBUS_RTU_FP_ADDR) || + !IS_ENABLED(CONFIG_MODBUS_RTU_FP_EXTENSIONS)) { + /* Write integer register */ + if (ctx->mbs_user_cb->holding_reg_wr == NULL) { + mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_FC); + return true; + } + + if (reg_qty == 0 || reg_qty > regs_limit) { + LOG_ERR("Number of registers limit exceeded"); + mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_DATA_VAL); + return true; + } + + reg_size = sizeof(uint16_t); + } else { + /* Write floating-point register */ + if (ctx->mbs_user_cb->holding_reg_wr_fp == NULL) { + mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_FC); + return true; + } + + if (reg_qty == 0 || reg_qty > (regs_limit / 2)) { + LOG_ERR("Number of registers limit exceeded"); + mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_DATA_VAL); + return true; + } + + reg_size = sizeof(float); + } + + /* Compare number of bytes and payload length */ + if ((ctx->rx_frame.length - 5) != num_bytes) { + LOG_ERR("Mismatch in the number of bytes"); + mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_DATA_VAL); + return true; + } + + if ((num_bytes / reg_qty) != (uint16_t)reg_size) { + LOG_ERR("Mismatch in the number of registers"); + mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_DATA_VAL); + return true; + } + + /* The 1st registers data byte is 6th element in payload */ + prx_data = &ctx->rx_frame.data[5]; + + for (uint16_t reg_cntr = 0; reg_cntr < reg_qty; reg_cntr++) { + uint16_t addr = reg_addr + reg_cntr; + + if ((reg_addr < MODBUS_RTU_FP_ADDR) || + !IS_ENABLED(CONFIG_MODBUS_RTU_FP_EXTENSIONS)) { + uint16_t reg_val = sys_get_be16(prx_data); + + prx_data += sizeof(uint16_t); + err = ctx->mbs_user_cb->holding_reg_wr(addr, reg_val); + } else { + uint32_t reg_val = sys_get_be32(prx_data); + float fp; + + /* Write to floating point register */ + memcpy(&fp, ®_val, sizeof(float)); + prx_data += sizeof(uint32_t); + err = ctx->mbs_user_cb->holding_reg_wr_fp(addr, fp); + } + + if (err != 0) { + LOG_INF("Register address not supported"); + mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_DATA_ADDR); + return true; + } + } + + /* Assemble response payload */ + ctx->tx_frame.length = response_len; + sys_put_be16(reg_addr, &ctx->tx_frame.data[0]); + sys_put_be16(reg_qty, &ctx->tx_frame.data[2]); + + return true; +} + +static bool mbs_fc_handler(struct mb_rtu_context *ctx) +{ + bool send_reply = false; + uint8_t addr = ctx->rx_frame.addr; + uint8_t fc = ctx->rx_frame.fc; + + if (addr != 0 && addr != ctx->node_addr) { + return false; + } + + /* Prepare response header */ + ctx->tx_frame.addr = addr; + ctx->tx_frame.fc = fc; + + update_server_msg_ctr(ctx); + + switch (fc) { + case MODBUS_FC01_COIL_RD: + send_reply = mbs_fc01_coil_read(ctx); + break; + + case MODBUS_FC02_DI_RD: + send_reply = mbs_fc02_di_read(ctx); + break; + + case MODBUS_FC03_HOLDING_REG_RD: + send_reply = mbs_fc03_hreg_read(ctx); + break; + + case MODBUS_FC04_IN_REG_RD: + send_reply = mbs_fc04_inreg_read(ctx); + break; + + case MODBUS_FC05_COIL_WR: + send_reply = mbs_fc05_coil_write(ctx); + break; + + case MODBUS_FC06_HOLDING_REG_WR: + send_reply = mbs_fc06_hreg_write(ctx); + break; + + case MODBUS_FC08_DIAGNOSTICS: + send_reply = mbs_fc08_diagnostics(ctx); + break; + + case MODBUS_FC15_COILS_WR: + send_reply = mbs_fc15_coils_write(ctx); + break; + + case MODBUS_FC16_HOLDING_REGS_WR: + send_reply = mbs_fc16_hregs_write(ctx); + break; + + default: + LOG_ERR("Function code 0x%02x not implemented", fc); + mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_FC); + send_reply = true; + break; + } + + if (addr == 0) { + /* Broadcast address, do not reply */ + return false; + } else { + return send_reply; + } +} + +bool mbs_rx_handler(struct mb_rtu_context *ctx) +{ + LOG_DBG("Server RX handler %p", ctx); + + update_msg_ctr(ctx); + + if (ctx->rx_frame_err == -EIO) { + update_noresp_ctr(ctx); + update_crcerr_ctr(ctx); + } else if (ctx->rx_frame_err != 0) { + update_noresp_ctr(ctx); + } else { + if (mbs_fc_handler(ctx) == true) { + mb_tx_frame(ctx); + return true; + } + + update_noresp_ctr(ctx); + } + + return false; +}