OrgZephyr/zephyr
OrgZephyr
/
zephyr
mirror of https://github.com/zephyrproject-rtos/zephyr.git
1
0
Fork 0
Code Issues Releases Wiki Activity
zephyr/subsys/modbus/modbus_server.c

1044 lines
25 KiB
C
Raw Blame History

/*
* 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 <string.h>
#include <zephyr/sys/byteorder.h>
#include <modbus_internal.h>
#include <zephyr/logging/log.h>
LOG_MODULE_REGISTER(modbus_s, CONFIG_MODBUS_LOG_LEVEL);
/*
* This functions are used to reset and update server's
* statistics and communications counters.
*/
#ifdef CONFIG_MODBUS_FC08_DIAGNOSTIC
void modbus_reset_stats(struct modbus_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 modbus_context *ctx)
{
ctx->mbs_msg_ctr++;
}
static void update_crcerr_ctr(struct modbus_context *ctx)
{
ctx->mbs_crc_err_ctr++;
}
static void update_excep_ctr(struct modbus_context *ctx)
{
ctx->mbs_except_ctr++;
}
static void update_server_msg_ctr(struct modbus_context *ctx)
{
ctx->mbs_server_msg_ctr++;
}
static void update_noresp_ctr(struct modbus_context *ctx)
{
ctx->mbs_noresp_ctr++;
}
#else
#define modbus_reset_stats(...)
#define update_msg_ctr(...)
#define update_crcerr_ctr(...)
#define update_excep_ctr(...)
#define update_server_msg_ctr(...)
#define update_noresp_ctr(...)
#endif /* CONFIG_MODBUS_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 modbus_context *ctx, uint8_t excep_code)
{
const uint8_t excep_bit = BIT(7);
LOG_INF("FC 0x%02x Error 0x%02x", ctx->rx_adu.fc, excep_code);
update_excep_ctr(ctx);
ctx->tx_adu.fc |= excep_bit;
ctx->tx_adu.data[0] = excep_code;
ctx->tx_adu.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 modbus_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_adu.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_adu.data[0]);
coil_qty = sys_get_be16(&ctx->rx_adu.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_adu.length = num_bytes + 1;
/* Set number of data bytes in response message. */
ctx->tx_adu.data[0] = (uint8_t)num_bytes;
/* Clear bytes in response */
presp = &ctx->tx_adu.data[1];
memset(presp, 0, num_bytes);
/* Reset the pointer to the start of the response payload */
presp = &ctx->tx_adu.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 modbus_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_adu.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_adu.data[0]);
di_qty = sys_get_be16(&ctx->rx_adu.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_adu.length = num_bytes + 1;
/* Set number of data bytes in response message. */
ctx->tx_adu.data[0] = (uint8_t)num_bytes;
/* Clear bytes in response */
presp = &ctx->tx_adu.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_adu.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 modbus_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_adu.length != request_len) {
LOG_ERR("Wrong request length");
return false;
}
reg_addr = sys_get_be16(&ctx->rx_adu.data[0]);
reg_qty = sys_get_be16(&ctx->rx_adu.data[2]);
if (reg_qty == 0 || reg_qty > regs_limit) {
LOG_ERR("Wrong register quantity, %u (limit is %u)",
reg_qty, regs_limit);
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));
if ((reg_addr < MODBUS_FP_EXTENSIONS_ADDR) ||
!IS_ENABLED(CONFIG_MODBUS_FP_EXTENSIONS)) {
/* Read integer register */
if (ctx->mbs_user_cb->holding_reg_rd == NULL) {
mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_FC);
return true;
}
} 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 (num_bytes % sizeof(uint32_t)) {
mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_FC);
return true;
}
}
/* Number of data bytes + byte count. */
ctx->tx_adu.length = num_bytes + 1;
/* Set number of data bytes in response message. */
ctx->tx_adu.data[0] = (uint8_t)num_bytes;
/* Reset the pointer to the start of the response payload */
presp = &ctx->tx_adu.data[1];
/* Loop through each register requested. */
while (reg_qty > 0) {
if (reg_addr < MODBUS_FP_EXTENSIONS_ADDR) {
uint16_t reg;
/* Read integer register */
err = ctx->mbs_user_cb->holding_reg_rd(reg_addr, &reg);
if (err == 0) {
sys_put_be16(reg, presp);
presp += sizeof(uint16_t);
}
/* Increment current register address */
reg_addr++;
reg_qty--;
} else if (IS_ENABLED(CONFIG_MODBUS_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(&reg, &fp, sizeof(reg));
sys_put_be32(reg, presp);
presp += sizeof(uint32_t);
}
/* Increment current register address */
reg_addr += 2;
reg_qty -= 2;
}
if (err != 0) {
LOG_INF("Holding register address not supported");
mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_DATA_ADDR);
return true;
}
}
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 modbus_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_adu.length != request_len) {
LOG_ERR("Wrong request length");
return false;
}
reg_addr = sys_get_be16(&ctx->rx_adu.data[0]);
reg_qty = sys_get_be16(&ctx->rx_adu.data[2]);
if (reg_qty == 0 || reg_qty > regs_limit) {
LOG_ERR("Wrong register quantity, %u (limit is %u)",
reg_qty, regs_limit);
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));
if ((reg_addr < MODBUS_FP_EXTENSIONS_ADDR) ||
!IS_ENABLED(CONFIG_MODBUS_FP_EXTENSIONS)) {
/* Read integer register */
if (ctx->mbs_user_cb->input_reg_rd == NULL) {
mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_FC);
return true;
}
} 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 (num_bytes % sizeof(uint32_t)) {
mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_FC);
return true;
}
}
/* Number of data bytes + byte count. */
ctx->tx_adu.length = num_bytes + 1;
/* Set number of data bytes in response message. */
ctx->tx_adu.data[0] = (uint8_t)num_bytes;
/* Reset the pointer to the start of the response payload */
presp = &ctx->tx_adu.data[1];
/* Loop through each register requested. */
while (reg_qty > 0) {
if (reg_addr < MODBUS_FP_EXTENSIONS_ADDR) {
uint16_t reg;
/* Read integer register */
err = ctx->mbs_user_cb->input_reg_rd(reg_addr, &reg);
if (err == 0) {
sys_put_be16(reg, presp);
presp += sizeof(uint16_t);
}
/* Increment current register number */
reg_addr++;
reg_qty--;
} else if (IS_ENABLED(CONFIG_MODBUS_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(&reg, &fp, sizeof(reg));
sys_put_be32(reg, presp);
presp += sizeof(uint32_t);
}
/* Increment current register address */
reg_addr += 2;
reg_qty -= 2;
}
if (err != 0) {
LOG_INF("Input register address not supported");
mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_DATA_ADDR);
return true;
}
}
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 modbus_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_adu.length != request_len) {
LOG_ERR("Wrong request length %u", ctx->rx_adu.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_adu.data[0]);
coil_val = sys_get_be16(&ctx->rx_adu.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_adu.length = response_len;
sys_put_be16(coil_addr, &ctx->tx_adu.data[0]);
sys_put_be16(coil_val, &ctx->tx_adu.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 modbus_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_adu.length != request_len) {
LOG_ERR("Wrong request length %u", ctx->rx_adu.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_adu.data[0]);
reg_val = sys_get_be16(&ctx->rx_adu.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_adu.length = response_len;
sys_put_be16(reg_addr, &ctx->tx_adu.data[0]);
sys_put_be16(reg_val, &ctx->tx_adu.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_FC08_DIAGNOSTIC
static bool mbs_fc08_diagnostics(struct modbus_context *ctx)
{
const uint8_t request_len = 4;
const uint8_t response_len = 4;
uint16_t sfunc;
uint16_t data;
if (ctx->rx_adu.length != request_len) {
LOG_ERR("Wrong request length %u", ctx->rx_adu.length);
return false;
}
sfunc = sys_get_be16(&ctx->rx_adu.data[0]);
data = sys_get_be16(&ctx->rx_adu.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 */
modbus_reset_stats(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_adu.length = response_len;
sys_put_be16(sfunc, &ctx->tx_adu.data[0]);
sys_put_be16(data, &ctx->tx_adu.data[2]);
return true;
}
#else
static bool mbs_fc08_diagnostics(struct modbus_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 modbus_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_adu.length < request_len) {
LOG_ERR("Wrong request length %u", ctx->rx_adu.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_adu.data[0]);
coil_qty = sys_get_be16(&ctx->rx_adu.data[2]);
/* Get the byte count for the data. */
num_bytes = ctx->rx_adu.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_adu.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_adu.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_adu.length = response_len;
sys_put_be16(coil_addr, &ctx->tx_adu.data[0]);
sys_put_be16(coil_qty, &ctx->tx_adu.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_FP_EXTENSIONS_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 modbus_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;
if (ctx->rx_adu.length < request_len) {
LOG_ERR("Wrong request length %u", ctx->rx_adu.length);
return false;
}
reg_addr = sys_get_be16(&ctx->rx_adu.data[0]);
reg_qty = sys_get_be16(&ctx->rx_adu.data[2]);
/* Get the byte count for the data. */
num_bytes = ctx->rx_adu.data[4];
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;
}
if ((reg_addr < MODBUS_FP_EXTENSIONS_ADDR) ||
!IS_ENABLED(CONFIG_MODBUS_FP_EXTENSIONS)) {
/* Write integer register */
if (ctx->mbs_user_cb->holding_reg_wr == NULL) {
mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_FC);
return true;
}
} 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 (num_bytes % sizeof(uint32_t)) {
mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_FC);
return true;
}
}
/* Compare number of bytes and payload length */
if ((ctx->rx_adu.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) != sizeof(uint16_t)) {
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_adu.data[5];
for (uint16_t reg_cntr = 0; reg_cntr < reg_qty;) {
uint16_t addr = reg_addr + reg_cntr;
if ((reg_addr < MODBUS_FP_EXTENSIONS_ADDR) ||
!IS_ENABLED(CONFIG_MODBUS_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);
reg_cntr++;
} else {
uint32_t reg_val = sys_get_be32(prx_data);
float fp;
/* Write to floating point register */
memcpy(&fp, &reg_val, sizeof(uint32_t));
prx_data += sizeof(uint32_t);
err = ctx->mbs_user_cb->holding_reg_wr_fp(addr, fp);
reg_cntr += 2;
}
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_adu.length = response_len;
sys_put_be16(reg_addr, &ctx->tx_adu.data[0]);
sys_put_be16(reg_qty, &ctx->tx_adu.data[2]);
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;
uint8_t addr = ctx->rx_adu.unit_id;
uint8_t fc = ctx->rx_adu.fc;
LOG_DBG("Server RX handler %p", ctx);
update_msg_ctr(ctx);
if (ctx->rx_adu_err != 0) {
update_noresp_ctr(ctx);
if (ctx->rx_adu_err == -EIO) {
update_crcerr_ctr(ctx);
}
return false;
}
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;
}
/* Prepare response header */
ctx->tx_adu.trans_id = ctx->rx_adu.trans_id;
ctx->tx_adu.proto_id = ctx->rx_adu.proto_id;
ctx->tx_adu.unit_id = addr;
ctx->tx_adu.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:
send_reply = mbs_try_user_fc(ctx, fc);
}
if (addr == 0) {
/* Broadcast address, do not reply */
send_reply = false;
}
if (send_reply == false) {
update_noresp_ctr(ctx);
}
return send_reply;
}
Reference in New Issue View Git Blame Copy Permalink