adafruit-mirror/zephyr
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions 9

modbus: fix support for floating point values

Browse source 
The Modbus protocol object types are either single-bit or 16-bit word.
Other types are not defined in the specification. Types such as float
are typically mapped to two 16-bit registers. Current implementaiton
does not maps correctly to the 16-bit word addresses. On the client
side, the implementation must take into account that the number of
requested registers (Quantity of Registers) is double that of a "float"
register.  The server side should not treat "Quantity of Registers" and
"Byte count" differently for addresses reserved for floating values,
only in the user callback the two 16-bit registers are mapped to a float
value but still aligned to 16-bit register addresses.

Signed-off-by: Johann Fischer <johann.fischer@nordicsemi.no>
This commit is contained in:
Johann Fischer 2024-09-16 23:54:10 +02:00 committed by Carles Cufí
parent 7cd87ea1f7
commit 47c8815253
4 changed files with 73 additions and 66 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

10
subsys/modbus/modbus_client.c
View file

@ -64,14 +64,14 @@ static int mbc_validate_fc03fp_response(struct modbus_context *ctx, float *ptbl)
resp_byte_cnt = ctx->rx_adu.data[0];
resp_data = &ctx->rx_adu.data[1];
req_qty = sys_get_be16(&ctx->tx_adu.data[2]);
req_byte_cnt = req_qty * sizeof(float);
req_byte_cnt = req_qty * sizeof(uint16_t);
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++) {
for (uint16_t i = 0; i < req_qty / 2; i++) {
uint32_t reg_val = sys_get_be32(resp_data);
memcpy(&ptbl[i], &reg_val, sizeof(float));
@ -384,7 +384,8 @@ int modbus_read_holding_regs_fp(const int iface,
ctx->tx_adu.length = 4;
sys_put_be16(start_addr, &ctx->tx_adu.data[0]);
sys_put_be16(num_regs, &ctx->tx_adu.data[2]);
/* A 32-bit float is mapped to two 16-bit registers */
sys_put_be16(num_regs * 2, &ctx->tx_adu.data[2]);
err = mbc_send_cmd(ctx, unit_id, MODBUS_FC03_HOLDING_REG_RD, reg_buf);
k_mutex_unlock(&ctx->iface_lock);
@ -610,7 +611,8 @@ int modbus_write_holding_regs_fp(const int iface,
sys_put_be16(start_addr, &ctx->tx_adu.data[0]);
length += sizeof(start_addr);
sys_put_be16(num_regs, &ctx->tx_adu.data[2]);
/* A 32-bit float is mapped to two 16-bit registers */
sys_put_be16(num_regs * 2, &ctx->tx_adu.data[2]);
length += sizeof(num_regs);
num_bytes = num_regs * sizeof(float);

103
subsys/modbus/modbus_server.c
View file

@ -322,6 +322,16 @@ static bool mbs_fc03_hreg_read(struct modbus_context *ctx)
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 */
@ -330,14 +340,6 @@ static bool mbs_fc03_hreg_read(struct modbus_context *ctx)
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) {
@ -345,14 +347,10 @@ static bool mbs_fc03_hreg_read(struct modbus_context *ctx)
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);
if (num_bytes % sizeof(uint32_t)) {
mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_FC);
return true;
}
/* Get number of bytes needed for response. */
num_bytes = (uint8_t)(reg_qty * sizeof(float));
}
/* Number of data bytes + byte count. */
@ -374,6 +372,9 @@ static bool mbs_fc03_hreg_read(struct modbus_context *ctx)
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;
@ -385,6 +386,10 @@ static bool mbs_fc03_hreg_read(struct modbus_context *ctx)
sys_put_be32(reg, presp);
presp += sizeof(uint32_t);
}
/* Increment current register address */
reg_addr += 2;
reg_qty -= 2;
}
if (err != 0) {
@ -393,9 +398,6 @@ static bool mbs_fc03_hreg_read(struct modbus_context *ctx)
return true;
}
/* Increment current register address */
reg_addr++;
reg_qty--;
}
return true;
@ -432,6 +434,16 @@ static bool mbs_fc04_inreg_read(struct modbus_context *ctx)
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 */
@ -440,14 +452,6 @@ static bool mbs_fc04_inreg_read(struct modbus_context *ctx)
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) {
@ -455,14 +459,10 @@ static bool mbs_fc04_inreg_read(struct modbus_context *ctx)
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);
if (num_bytes % sizeof(uint32_t)) {
mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_FC);
return true;
}
/* Get number of bytes needed for response. */
num_bytes = (uint8_t)(reg_qty * sizeof(float));
}
/* Number of data bytes + byte count. */
@ -484,6 +484,9 @@ static bool mbs_fc04_inreg_read(struct modbus_context *ctx)
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;
@ -495,6 +498,10 @@ static bool mbs_fc04_inreg_read(struct modbus_context *ctx)
sys_put_be32(reg, presp);
presp += sizeof(uint32_t);
}
/* Increment current register address */
reg_addr += 2;
reg_qty -= 2;
}
if (err != 0) {
@ -502,10 +509,6 @@ static bool mbs_fc04_inreg_read(struct modbus_context *ctx)
mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_DATA_ADDR);
return true;
}
/* Increment current register number */
reg_addr++;
reg_qty--;
}
return true;
@ -833,7 +836,6 @@ static bool mbs_fc16_hregs_write(struct modbus_context *ctx)
uint16_t reg_addr;
uint16_t reg_qty;
uint16_t num_bytes;
uint8_t reg_size;
if (ctx->rx_adu.length < request_len) {
LOG_ERR("Wrong request length %u", ctx->rx_adu.length);
@ -845,6 +847,12 @@ static bool mbs_fc16_hregs_write(struct modbus_context *ctx)
/* 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 */
@ -852,14 +860,6 @@ static bool mbs_fc16_hregs_write(struct modbus_context *ctx)
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) {
@ -867,13 +867,10 @@ static bool mbs_fc16_hregs_write(struct modbus_context *ctx)
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);
if (num_bytes % sizeof(uint32_t)) {
mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_FC);
return true;
}
reg_size = sizeof(float);
}
/* Compare number of bytes and payload length */
@ -883,7 +880,7 @@ static bool mbs_fc16_hregs_write(struct modbus_context *ctx)
return true;
}
if ((num_bytes / reg_qty) != (uint16_t)reg_size) {
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;
@ -892,7 +889,7 @@ static bool mbs_fc16_hregs_write(struct modbus_context *ctx)
/* 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; reg_cntr++) {
for (uint16_t reg_cntr = 0; reg_cntr < reg_qty;) {
uint16_t addr = reg_addr + reg_cntr;
if ((reg_addr < MODBUS_FP_EXTENSIONS_ADDR) ||
@ -901,14 +898,16 @@ static bool mbs_fc16_hregs_write(struct modbus_context *ctx)
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(float));
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) {

10
tests/subsys/modbus/src/test_modbus_client.c
View file

@ -150,7 +150,7 @@ void test_holding_reg(void)
for (uint16_t idx = 0; idx < ARRAY_SIZE(fhr_wr); idx++) {
err = modbus_write_holding_regs_fp(client_iface,
node,
fp_offset + idx,
fp_offset + idx * 2,
&fhr_wr[0], 1);
zassert_equal(err, 0, "FC16 write request failed");
}
@ -176,6 +176,14 @@ void test_holding_reg(void)
ARRAY_SIZE(fhr_wr));
zassert_not_equal(err, 0, "FC16 FP out of range request not failed");
err = modbus_write_holding_regs(client_iface, node, fp_offset,
hr_wr, ARRAY_SIZE(hr_wr) - 1);
zassert_not_equal(err, 0, "FC16 write to FP address request not failed");
err = modbus_read_holding_regs(client_iface, node, fp_offset,
hr_rd, ARRAY_SIZE(hr_rd) - 1);
zassert_not_equal(err, 0, "FC16 read from FP address request not failed");
err = modbus_read_holding_regs_fp(client_iface,
node,
fp_offset,

16
tests/subsys/modbus/src/test_modbus_server.c
View file

@ -6,6 +6,7 @@
#include "test_modbus.h"
#include <zephyr/sys/util.h>
#include <zephyr/logging/log.h>
LOG_MODULE_REGISTER(mbs_test, LOG_LEVEL_INF);
@ -87,12 +88,11 @@ static int input_reg_rd(uint16_t addr, uint16_t *reg)
static int input_reg_rd_fp(uint16_t addr, float *reg)
{
if ((addr < fp_offset) ||
(addr >= (ARRAY_SIZE(holding_fp) + fp_offset))) {
if (!IN_RANGE(addr, fp_offset, sizeof(holding_fp) / 2 + fp_offset)) {
return -ENOTSUP;
}
*reg = holding_fp[addr - fp_offset];
*reg = holding_fp[(addr - fp_offset) / 2];
LOG_DBG("FP input register read, addr %u", addr);
@ -127,12 +127,11 @@ static int holding_reg_wr(uint16_t addr, uint16_t reg)
static int holding_reg_rd_fp(uint16_t addr, float *reg)
{
if ((addr < fp_offset) ||
(addr >= (ARRAY_SIZE(holding_fp) + fp_offset))) {
if (!IN_RANGE(addr, fp_offset, sizeof(holding_fp) / 2 + fp_offset)) {
return -ENOTSUP;
}
*reg = holding_fp[addr - fp_offset];
*reg = holding_fp[(addr - fp_offset) / 2];
LOG_DBG("FP holding register read, addr %u", addr);
@ -141,12 +140,11 @@ static int holding_reg_rd_fp(uint16_t addr, float *reg)
static int holding_reg_wr_fp(uint16_t addr, float reg)
{
if ((addr < fp_offset) ||
(addr >= (ARRAY_SIZE(holding_fp) + fp_offset))) {
if (!IN_RANGE(addr, fp_offset, sizeof(holding_fp) / 2 + fp_offset)) {
return -ENOTSUP;
}
holding_fp[addr - fp_offset] = reg;
holding_fp[(addr - fp_offset) / 2] = reg;
LOG_DBG("FP holding register write, addr %u", addr);