Skip to content

Commit 47c8815

Browse files
jfischer-nocarlescufi
jfischer-no
authored and
carlescufi
committed
modbus: fix support for floating point values
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 <[email protected]>
1 parent 7cd87ea commit 47c8815

File tree

4 files changed

+73
-66
lines changed

4 files changed

+73
-66
lines changed

‎subsys/modbus/modbus_client.c‎

Lines changed: 6 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -64,14 +64,14 @@ static int mbc_validate_fc03fp_response(struct modbus_context *ctx, float *ptbl)
6464
resp_byte_cnt = ctx->rx_adu.data[0];
6565
resp_data = &ctx->rx_adu.data[1];
6666
req_qty = sys_get_be16(&ctx->tx_adu.data[2]);
67-
req_byte_cnt = req_qty * sizeof(float);
67+
req_byte_cnt = req_qty * sizeof(uint16_t);
6868

6969
if (req_byte_cnt != resp_byte_cnt) {
7070
LOG_ERR("Mismatch in the number of registers");
7171
return -EINVAL;
7272
}
7373

74-
for (uint16_t i = 0; i < req_qty; i++) {
74+
for (uint16_t i = 0; i < req_qty / 2; i++) {
7575
uint32_t reg_val = sys_get_be32(resp_data);
7676

7777
memcpy(&ptbl[i], &reg_val, sizeof(float));
@@ -384,7 +384,8 @@ int modbus_read_holding_regs_fp(const int iface,
384384

385385
ctx->tx_adu.length = 4;
386386
sys_put_be16(start_addr, &ctx->tx_adu.data[0]);
387-
sys_put_be16(num_regs, &ctx->tx_adu.data[2]);
387+
/* A 32-bit float is mapped to two 16-bit registers */
388+
sys_put_be16(num_regs * 2, &ctx->tx_adu.data[2]);
388389

389390
err = mbc_send_cmd(ctx, unit_id, MODBUS_FC03_HOLDING_REG_RD, reg_buf);
390391
k_mutex_unlock(&ctx->iface_lock);
@@ -610,7 +611,8 @@ int modbus_write_holding_regs_fp(const int iface,
610611

611612
sys_put_be16(start_addr, &ctx->tx_adu.data[0]);
612613
length += sizeof(start_addr);
613-
sys_put_be16(num_regs, &ctx->tx_adu.data[2]);
614+
/* A 32-bit float is mapped to two 16-bit registers */
615+
sys_put_be16(num_regs * 2, &ctx->tx_adu.data[2]);
614616
length += sizeof(num_regs);
615617

616618
num_bytes = num_regs * sizeof(float);

‎subsys/modbus/modbus_server.c‎

Lines changed: 51 additions & 52 deletions
Original file line numberDiff line numberDiff line change
@@ -322,6 +322,16 @@ static bool mbs_fc03_hreg_read(struct modbus_context *ctx)
322322
reg_addr = sys_get_be16(&ctx->rx_adu.data[0]);
323323
reg_qty = sys_get_be16(&ctx->rx_adu.data[2]);
324324

325+
if (reg_qty == 0 || reg_qty > regs_limit) {
326+
LOG_ERR("Wrong register quantity, %u (limit is %u)",
327+
reg_qty, regs_limit);
328+
mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_DATA_VAL);
329+
return true;
330+
}
331+
332+
/* Get number of bytes needed for response. */
333+
num_bytes = (uint8_t)(reg_qty * sizeof(uint16_t));
334+
325335
if ((reg_addr < MODBUS_FP_EXTENSIONS_ADDR) ||
326336
!IS_ENABLED(CONFIG_MODBUS_FP_EXTENSIONS)) {
327337
/* Read integer register */
@@ -330,29 +340,17 @@ static bool mbs_fc03_hreg_read(struct modbus_context *ctx)
330340
return true;
331341
}
332342

333-
if (reg_qty == 0 || reg_qty > regs_limit) {
334-
LOG_ERR("Number of registers limit exceeded");
335-
mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_DATA_VAL);
336-
return true;
337-
}
338-
339-
/* Get number of bytes needed for response. */
340-
num_bytes = (uint8_t)(reg_qty * sizeof(uint16_t));
341343
} else {
342344
/* Read floating-point register */
343345
if (ctx->mbs_user_cb->holding_reg_rd_fp == NULL) {
344346
mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_FC);
345347
return true;
346348
}
347349

348-
if (reg_qty == 0 || reg_qty > (regs_limit / 2)) {
349-
LOG_ERR("Number of registers limit exceeded");
350-
mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_DATA_VAL);
350+
if (num_bytes % sizeof(uint32_t)) {
351+
mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_FC);
351352
return true;
352353
}
353-
354-
/* Get number of bytes needed for response. */
355-
num_bytes = (uint8_t)(reg_qty * sizeof(float));
356354
}
357355

358356
/* Number of data bytes + byte count. */
@@ -374,6 +372,9 @@ static bool mbs_fc03_hreg_read(struct modbus_context *ctx)
374372
presp += sizeof(uint16_t);
375373
}
376374

375+
/* Increment current register address */
376+
reg_addr++;
377+
reg_qty--;
377378
} else if (IS_ENABLED(CONFIG_MODBUS_FP_EXTENSIONS)) {
378379
float fp;
379380
uint32_t reg;
@@ -385,6 +386,10 @@ static bool mbs_fc03_hreg_read(struct modbus_context *ctx)
385386
sys_put_be32(reg, presp);
386387
presp += sizeof(uint32_t);
387388
}
389+
390+
/* Increment current register address */
391+
reg_addr += 2;
392+
reg_qty -= 2;
388393
}
389394

390395
if (err != 0) {
@@ -393,9 +398,6 @@ static bool mbs_fc03_hreg_read(struct modbus_context *ctx)
393398
return true;
394399
}
395400

396-
/* Increment current register address */
397-
reg_addr++;
398-
reg_qty--;
399401
}
400402

401403
return true;
@@ -432,6 +434,16 @@ static bool mbs_fc04_inreg_read(struct modbus_context *ctx)
432434
reg_addr = sys_get_be16(&ctx->rx_adu.data[0]);
433435
reg_qty = sys_get_be16(&ctx->rx_adu.data[2]);
434436

437+
if (reg_qty == 0 || reg_qty > regs_limit) {
438+
LOG_ERR("Wrong register quantity, %u (limit is %u)",
439+
reg_qty, regs_limit);
440+
mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_DATA_VAL);
441+
return true;
442+
}
443+
444+
/* Get number of bytes needed for response. */
445+
num_bytes = (uint8_t)(reg_qty * sizeof(uint16_t));
446+
435447
if ((reg_addr < MODBUS_FP_EXTENSIONS_ADDR) ||
436448
!IS_ENABLED(CONFIG_MODBUS_FP_EXTENSIONS)) {
437449
/* Read integer register */
@@ -440,29 +452,17 @@ static bool mbs_fc04_inreg_read(struct modbus_context *ctx)
440452
return true;
441453
}
442454

443-
if (reg_qty == 0 || reg_qty > regs_limit) {
444-
LOG_ERR("Number of registers limit exceeded");
445-
mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_DATA_VAL);
446-
return true;
447-
}
448-
449-
/* Get number of bytes needed for response. */
450-
num_bytes = (uint8_t)(reg_qty * sizeof(uint16_t));
451455
} else {
452456
/* Read floating-point register */
453457
if (ctx->mbs_user_cb->input_reg_rd_fp == NULL) {
454458
mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_FC);
455459
return true;
456460
}
457461

458-
if (reg_qty == 0 || reg_qty > (regs_limit / 2)) {
459-
LOG_ERR("Number of registers limit exceeded");
460-
mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_DATA_VAL);
462+
if (num_bytes % sizeof(uint32_t)) {
463+
mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_FC);
461464
return true;
462465
}
463-
464-
/* Get number of bytes needed for response. */
465-
num_bytes = (uint8_t)(reg_qty * sizeof(float));
466466
}
467467

468468
/* Number of data bytes + byte count. */
@@ -484,6 +484,9 @@ static bool mbs_fc04_inreg_read(struct modbus_context *ctx)
484484
presp += sizeof(uint16_t);
485485
}
486486

487+
/* Increment current register number */
488+
reg_addr++;
489+
reg_qty--;
487490
} else if (IS_ENABLED(CONFIG_MODBUS_FP_EXTENSIONS)) {
488491
float fp;
489492
uint32_t reg;
@@ -495,17 +498,17 @@ static bool mbs_fc04_inreg_read(struct modbus_context *ctx)
495498
sys_put_be32(reg, presp);
496499
presp += sizeof(uint32_t);
497500
}
501+
502+
/* Increment current register address */
503+
reg_addr += 2;
504+
reg_qty -= 2;
498505
}
499506

500507
if (err != 0) {
501508
LOG_INF("Input register address not supported");
502509
mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_DATA_ADDR);
503510
return true;
504511
}
505-
506-
/* Increment current register number */
507-
reg_addr++;
508-
reg_qty--;
509512
}
510513

511514
return true;
@@ -833,7 +836,6 @@ static bool mbs_fc16_hregs_write(struct modbus_context *ctx)
833836
uint16_t reg_addr;
834837
uint16_t reg_qty;
835838
uint16_t num_bytes;
836-
uint8_t reg_size;
837839

838840
if (ctx->rx_adu.length < request_len) {
839841
LOG_ERR("Wrong request length %u", ctx->rx_adu.length);
@@ -845,35 +847,30 @@ static bool mbs_fc16_hregs_write(struct modbus_context *ctx)
845847
/* Get the byte count for the data. */
846848
num_bytes = ctx->rx_adu.data[4];
847849

850+
if (reg_qty == 0 || reg_qty > regs_limit) {
851+
LOG_ERR("Number of registers limit exceeded");
852+
mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_DATA_VAL);
853+
return true;
854+
}
855+
848856
if ((reg_addr < MODBUS_FP_EXTENSIONS_ADDR) ||
849857
!IS_ENABLED(CONFIG_MODBUS_FP_EXTENSIONS)) {
850858
/* Write integer register */
851859
if (ctx->mbs_user_cb->holding_reg_wr == NULL) {
852860
mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_FC);
853861
return true;
854862
}
855-
856-
if (reg_qty == 0 || reg_qty > regs_limit) {
857-
LOG_ERR("Number of registers limit exceeded");
858-
mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_DATA_VAL);
859-
return true;
860-
}
861-
862-
reg_size = sizeof(uint16_t);
863863
} else {
864864
/* Write floating-point register */
865865
if (ctx->mbs_user_cb->holding_reg_wr_fp == NULL) {
866866
mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_FC);
867867
return true;
868868
}
869869

870-
if (reg_qty == 0 || reg_qty > (regs_limit / 2)) {
871-
LOG_ERR("Number of registers limit exceeded");
872-
mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_DATA_VAL);
870+
if (num_bytes % sizeof(uint32_t)) {
871+
mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_FC);
873872
return true;
874873
}
875-
876-
reg_size = sizeof(float);
877874
}
878875

879876
/* Compare number of bytes and payload length */
@@ -883,7 +880,7 @@ static bool mbs_fc16_hregs_write(struct modbus_context *ctx)
883880
return true;
884881
}
885882

886-
if ((num_bytes / reg_qty) != (uint16_t)reg_size) {
883+
if ((num_bytes / reg_qty) != sizeof(uint16_t)) {
887884
LOG_ERR("Mismatch in the number of registers");
888885
mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_DATA_VAL);
889886
return true;
@@ -892,7 +889,7 @@ static bool mbs_fc16_hregs_write(struct modbus_context *ctx)
892889
/* The 1st registers data byte is 6th element in payload */
893890
prx_data = &ctx->rx_adu.data[5];
894891

895-
for (uint16_t reg_cntr = 0; reg_cntr < reg_qty; reg_cntr++) {
892+
for (uint16_t reg_cntr = 0; reg_cntr < reg_qty;) {
896893
uint16_t addr = reg_addr + reg_cntr;
897894

898895
if ((reg_addr < MODBUS_FP_EXTENSIONS_ADDR) ||
@@ -901,14 +898,16 @@ static bool mbs_fc16_hregs_write(struct modbus_context *ctx)
901898

902899
prx_data += sizeof(uint16_t);
903900
err = ctx->mbs_user_cb->holding_reg_wr(addr, reg_val);
901+
reg_cntr++;
904902
} else {
905903
uint32_t reg_val = sys_get_be32(prx_data);
906904
float fp;
907905

908906
/* Write to floating point register */
909-
memcpy(&fp, &reg_val, sizeof(float));
907+
memcpy(&fp, &reg_val, sizeof(uint32_t));
910908
prx_data += sizeof(uint32_t);
911909
err = ctx->mbs_user_cb->holding_reg_wr_fp(addr, fp);
910+
reg_cntr += 2;
912911
}
913912

914913
if (err != 0) {

‎tests/subsys/modbus/src/test_modbus_client.c‎

Lines changed: 9 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -150,7 +150,7 @@ void test_holding_reg(void)
150150
for (uint16_t idx = 0; idx < ARRAY_SIZE(fhr_wr); idx++) {
151151
err = modbus_write_holding_regs_fp(client_iface,
152152
node,
153-
fp_offset + idx,
153+
fp_offset + idx * 2,
154154
&fhr_wr[0], 1);
155155
zassert_equal(err, 0, "FC16 write request failed");
156156
}
@@ -176,6 +176,14 @@ void test_holding_reg(void)
176176
ARRAY_SIZE(fhr_wr));
177177
zassert_not_equal(err, 0, "FC16 FP out of range request not failed");
178178

179+
err = modbus_write_holding_regs(client_iface, node, fp_offset,
180+
hr_wr, ARRAY_SIZE(hr_wr) - 1);
181+
zassert_not_equal(err, 0, "FC16 write to FP address request not failed");
182+
183+
err = modbus_read_holding_regs(client_iface, node, fp_offset,
184+
hr_rd, ARRAY_SIZE(hr_rd) - 1);
185+
zassert_not_equal(err, 0, "FC16 read from FP address request not failed");
186+
179187
err = modbus_read_holding_regs_fp(client_iface,
180188
node,
181189
fp_offset,

‎tests/subsys/modbus/src/test_modbus_server.c‎

Lines changed: 7 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -6,6 +6,7 @@
66

77
#include "test_modbus.h"
88

9+
#include <zephyr/sys/util.h>
910
#include <zephyr/logging/log.h>
1011
LOG_MODULE_REGISTER(mbs_test, LOG_LEVEL_INF);
1112

@@ -87,12 +88,11 @@ static int input_reg_rd(uint16_t addr, uint16_t *reg)
8788

8889
static int input_reg_rd_fp(uint16_t addr, float *reg)
8990
{
90-
if ((addr < fp_offset) ||
91-
(addr >= (ARRAY_SIZE(holding_fp) + fp_offset))) {
91+
if (!IN_RANGE(addr, fp_offset, sizeof(holding_fp) / 2 + fp_offset)) {
9292
return -ENOTSUP;
9393
}
9494

95-
*reg = holding_fp[addr - fp_offset];
95+
*reg = holding_fp[(addr - fp_offset) / 2];
9696

9797
LOG_DBG("FP input register read, addr %u", addr);
9898

@@ -127,12 +127,11 @@ static int holding_reg_wr(uint16_t addr, uint16_t reg)
127127

128128
static int holding_reg_rd_fp(uint16_t addr, float *reg)
129129
{
130-
if ((addr < fp_offset) ||
131-
(addr >= (ARRAY_SIZE(holding_fp) + fp_offset))) {
130+
if (!IN_RANGE(addr, fp_offset, sizeof(holding_fp) / 2 + fp_offset)) {
132131
return -ENOTSUP;
133132
}
134133

135-
*reg = holding_fp[addr - fp_offset];
134+
*reg = holding_fp[(addr - fp_offset) / 2];
136135

137136
LOG_DBG("FP holding register read, addr %u", addr);
138137

@@ -141,12 +140,11 @@ static int holding_reg_rd_fp(uint16_t addr, float *reg)
141140

142141
static int holding_reg_wr_fp(uint16_t addr, float reg)
143142
{
144-
if ((addr < fp_offset) ||
145-
(addr >= (ARRAY_SIZE(holding_fp) + fp_offset))) {
143+
if (!IN_RANGE(addr, fp_offset, sizeof(holding_fp) / 2 + fp_offset)) {
146144
return -ENOTSUP;
147145
}
148146

149-
holding_fp[addr - fp_offset] = reg;
147+
holding_fp[(addr - fp_offset) / 2] = reg;
150148

151149
LOG_DBG("FP holding register write, addr %u", addr);
152150

0 commit comments

Comments
 (0)