1 /*
2 * Copyright (c) 2020 PHYTEC Messtechnik GmbH
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 /*
8 * This file is based on mbs_core.c from uC/Modbus Stack.
9 *
10 * uC/Modbus
11 * The Embedded Modbus Stack
12 *
13 * Copyright 2003-2020 Silicon Laboratories Inc. www.silabs.com
14 *
15 * SPDX-License-Identifier: APACHE-2.0
16 *
17 * This software is subject to an open source license and is distributed by
18 * Silicon Laboratories Inc. pursuant to the terms of the Apache License,
19 * Version 2.0 available at www.apache.org/licenses/LICENSE-2.0.
20 */
21
22 #include <string.h>
23 #include <zephyr/sys/byteorder.h>
24 #include <modbus_internal.h>
25
26 #include <zephyr/logging/log.h>
27 LOG_MODULE_REGISTER(modbus_s, CONFIG_MODBUS_LOG_LEVEL);
28
29 /*
30 * This functions are used to reset and update server's
31 * statistics and communications counters.
32 */
33 #ifdef CONFIG_MODBUS_FC08_DIAGNOSTIC
modbus_reset_stats(struct modbus_context * ctx)34 void modbus_reset_stats(struct modbus_context *ctx)
35 {
36 /* Initialize all MODBUS event counters. */
37 ctx->mbs_msg_ctr = 0;
38 ctx->mbs_crc_err_ctr = 0;
39 ctx->mbs_except_ctr = 0;
40 ctx->mbs_server_msg_ctr = 0;
41 ctx->mbs_noresp_ctr = 0;
42 }
43
update_msg_ctr(struct modbus_context * ctx)44 static void update_msg_ctr(struct modbus_context *ctx)
45 {
46 ctx->mbs_msg_ctr++;
47 }
48
update_crcerr_ctr(struct modbus_context * ctx)49 static void update_crcerr_ctr(struct modbus_context *ctx)
50 {
51 ctx->mbs_crc_err_ctr++;
52 }
53
update_excep_ctr(struct modbus_context * ctx)54 static void update_excep_ctr(struct modbus_context *ctx)
55 {
56 ctx->mbs_except_ctr++;
57 }
58
update_server_msg_ctr(struct modbus_context * ctx)59 static void update_server_msg_ctr(struct modbus_context *ctx)
60 {
61 ctx->mbs_server_msg_ctr++;
62 }
63
update_noresp_ctr(struct modbus_context * ctx)64 static void update_noresp_ctr(struct modbus_context *ctx)
65 {
66 ctx->mbs_noresp_ctr++;
67 }
68 #else
69 #define modbus_reset_stats(...)
70 #define update_msg_ctr(...)
71 #define update_crcerr_ctr(...)
72 #define update_excep_ctr(...)
73 #define update_server_msg_ctr(...)
74 #define update_noresp_ctr(...)
75 #endif /* CONFIG_MODBUS_FC08_DIAGNOSTIC */
76
77 /*
78 * This function sets the indicated error response code into the response frame.
79 * Then the routine is called to calculate the error check value.
80 */
mbs_exception_rsp(struct modbus_context * ctx,uint8_t excep_code)81 static void mbs_exception_rsp(struct modbus_context *ctx, uint8_t excep_code)
82 {
83 const uint8_t excep_bit = BIT(7);
84
85 LOG_INF("FC 0x%02x Error 0x%02x", ctx->rx_adu.fc, excep_code);
86
87 update_excep_ctr(ctx);
88
89 ctx->tx_adu.fc |= excep_bit;
90 ctx->tx_adu.data[0] = excep_code;
91 ctx->tx_adu.length = 1;
92 }
93
94 /*
95 * FC 01 (0x01) Read Coils
96 *
97 * Request Payload:
98 * Function code 1 Byte
99 * Starting Address 2 Bytes
100 * Quantity of Coils 2 Bytes
101 *
102 * Response Payload:
103 * Function code 1 Byte
104 * Byte count 1 Bytes
105 * Coil status N * 1 Byte
106 */
mbs_fc01_coil_read(struct modbus_context * ctx)107 static bool mbs_fc01_coil_read(struct modbus_context *ctx)
108 {
109 const uint16_t coils_limit = 2000;
110 const uint8_t request_len = 4;
111 uint8_t *presp;
112 bool coil_state;
113 int err;
114 uint16_t coil_addr;
115 uint16_t coil_qty;
116 uint16_t num_bytes;
117 uint8_t bit_mask;
118 uint16_t coil_cntr;
119
120 if (ctx->rx_adu.length != request_len) {
121 LOG_ERR("Wrong request length");
122 return false;
123 }
124
125 if (ctx->mbs_user_cb->coil_rd == NULL) {
126 mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_FC);
127 return true;
128 }
129
130 coil_addr = sys_get_be16(&ctx->rx_adu.data[0]);
131 coil_qty = sys_get_be16(&ctx->rx_adu.data[2]);
132
133 /* Make sure we don't exceed the allowed limit per request */
134 if (coil_qty == 0 || coil_qty > coils_limit) {
135 LOG_ERR("Number of coils limit exceeded");
136 mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_DATA_VAL);
137 return true;
138 }
139
140 /* Calculate byte count for response. */
141 num_bytes = ((coil_qty - 1) / 8) + 1;
142 /* Number of data bytes + byte count. */
143 ctx->tx_adu.length = num_bytes + 1;
144 /* Set number of data bytes in response message. */
145 ctx->tx_adu.data[0] = (uint8_t)num_bytes;
146
147 /* Clear bytes in response */
148 presp = &ctx->tx_adu.data[1];
149 memset(presp, 0, num_bytes);
150
151 /* Reset the pointer to the start of the response payload */
152 presp = &ctx->tx_adu.data[1];
153 /* Start with bit 0 in response byte data mask. */
154 bit_mask = BIT(0);
155 /* Initialize loop counter. */
156 coil_cntr = 0;
157
158 /* Loop through each coil requested. */
159 while (coil_cntr < coil_qty) {
160
161 err = ctx->mbs_user_cb->coil_rd(coil_addr, &coil_state);
162 if (err != 0) {
163 LOG_INF("Coil address not supported");
164 mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_DATA_ADDR);
165 return true;
166 }
167
168 if (coil_state) {
169 *presp |= bit_mask;
170 }
171
172 coil_addr++;
173 /* Increment coil counter. */
174 coil_cntr++;
175 /* Determine if 8 data bits have been filled. */
176 if ((coil_cntr % 8) == 0) {
177 /* Reset the data mask. */
178 bit_mask = BIT(0);
179 /* Increment frame data index. */
180 presp++;
181 } else {
182 /*
183 * Still in same data byte, so shift the data mask
184 * to the next higher bit position.
185 */
186 bit_mask <<= 1;
187 }
188 }
189
190 return true;
191 }
192
193 /*
194 * FC 02 (0x02) Read Discrete Inputs
195 *
196 * Request Payload:
197 * Function code 1 Byte
198 * Starting Address 2 Bytes
199 * Quantity of Inputs 2 Bytes
200 *
201 * Response Payload:
202 * Function code 1 Byte
203 * Byte count 1 Bytes
204 * Input status N * 1 Byte
205 */
mbs_fc02_di_read(struct modbus_context * ctx)206 static bool mbs_fc02_di_read(struct modbus_context *ctx)
207 {
208 const uint16_t di_limit = 2000;
209 const uint8_t request_len = 4;
210 uint8_t *presp;
211 bool di_state;
212 int err;
213 uint16_t di_addr;
214 uint16_t di_qty;
215 uint16_t num_bytes;
216 uint8_t bit_mask;
217 uint16_t di_cntr;
218
219 if (ctx->rx_adu.length != request_len) {
220 LOG_ERR("Wrong request length");
221 return false;
222 }
223
224 if (ctx->mbs_user_cb->discrete_input_rd == NULL) {
225 mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_FC);
226 return true;
227 }
228
229 di_addr = sys_get_be16(&ctx->rx_adu.data[0]);
230 di_qty = sys_get_be16(&ctx->rx_adu.data[2]);
231
232 /* Make sure we don't exceed the allowed limit per request */
233 if (di_qty == 0 || di_qty > di_limit) {
234 LOG_ERR("Number of inputs limit exceeded");
235 mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_DATA_VAL);
236 return true;
237 }
238
239 /* Get number of bytes needed for response. */
240 num_bytes = ((di_qty - 1) / 8) + 1;
241 /* Number of data bytes + byte count. */
242 ctx->tx_adu.length = num_bytes + 1;
243 /* Set number of data bytes in response message. */
244 ctx->tx_adu.data[0] = (uint8_t)num_bytes;
245
246 /* Clear bytes in response */
247 presp = &ctx->tx_adu.data[1];
248 for (di_cntr = 0; di_cntr < num_bytes; di_cntr++) {
249 *presp++ = 0x00;
250 }
251
252 /* Reset the pointer to the start of the response payload */
253 presp = &ctx->tx_adu.data[1];
254 /* Start with bit 0 in response byte data mask. */
255 bit_mask = BIT(0);
256 /* Initialize loop counter. */
257 di_cntr = 0;
258
259 /* Loop through each DI requested. */
260 while (di_cntr < di_qty) {
261
262 err = ctx->mbs_user_cb->discrete_input_rd(di_addr, &di_state);
263 if (err != 0) {
264 LOG_INF("Discrete input address not supported");
265 mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_DATA_ADDR);
266 return true;
267 }
268
269 if (di_state) {
270 *presp |= bit_mask;
271 }
272
273 di_addr++;
274 /* Increment DI counter. */
275 di_cntr++;
276 /* Determine if 8 data bits have been filled. */
277 if ((di_cntr % 8) == 0) {
278 /* Reset the data mask. */
279 bit_mask = BIT(0);
280 /* Increment data frame index. */
281 presp++;
282 } else {
283 /*
284 * Still in same data byte, so shift the data mask
285 * to the next higher bit position.
286 */
287 bit_mask <<= 1;
288 }
289 }
290
291 return true;
292 }
293
294 /*
295 * 03 (0x03) Read Holding Registers
296 *
297 * Request Payload:
298 * Function code 1 Byte
299 * Starting Address 2 Bytes
300 * Quantity of Registers 2 Bytes
301 *
302 * Response Payload:
303 * Function code 1 Byte
304 * Byte count 1 Bytes
305 * Register Value N * 2 Byte
306 */
mbs_fc03_hreg_read(struct modbus_context * ctx)307 static bool mbs_fc03_hreg_read(struct modbus_context *ctx)
308 {
309 const uint16_t regs_limit = 125;
310 const uint8_t request_len = 4;
311 uint8_t *presp;
312 uint16_t err;
313 uint16_t reg_addr;
314 uint16_t reg_qty;
315 uint16_t num_bytes;
316
317 if (ctx->rx_adu.length != request_len) {
318 LOG_ERR("Wrong request length");
319 return false;
320 }
321
322 reg_addr = sys_get_be16(&ctx->rx_adu.data[0]);
323 reg_qty = sys_get_be16(&ctx->rx_adu.data[2]);
324
325 if ((reg_addr < MODBUS_FP_EXTENSIONS_ADDR) ||
326 !IS_ENABLED(CONFIG_MODBUS_FP_EXTENSIONS)) {
327 /* Read integer register */
328 if (ctx->mbs_user_cb->holding_reg_rd == NULL) {
329 mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_FC);
330 return true;
331 }
332
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));
341 } else {
342 /* Read floating-point register */
343 if (ctx->mbs_user_cb->holding_reg_rd_fp == NULL) {
344 mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_FC);
345 return true;
346 }
347
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);
351 return true;
352 }
353
354 /* Get number of bytes needed for response. */
355 num_bytes = (uint8_t)(reg_qty * sizeof(float));
356 }
357
358 /* Number of data bytes + byte count. */
359 ctx->tx_adu.length = num_bytes + 1;
360 /* Set number of data bytes in response message. */
361 ctx->tx_adu.data[0] = (uint8_t)num_bytes;
362
363 /* Reset the pointer to the start of the response payload */
364 presp = &ctx->tx_adu.data[1];
365 /* Loop through each register requested. */
366 while (reg_qty > 0) {
367 if (reg_addr < MODBUS_FP_EXTENSIONS_ADDR) {
368 uint16_t reg;
369
370 /* Read integer register */
371 err = ctx->mbs_user_cb->holding_reg_rd(reg_addr, ®);
372 if (err == 0) {
373 sys_put_be16(reg, presp);
374 presp += sizeof(uint16_t);
375 }
376
377 } else if (IS_ENABLED(CONFIG_MODBUS_FP_EXTENSIONS)) {
378 float fp;
379 uint32_t reg;
380
381 /* Read floating-point register */
382 err = ctx->mbs_user_cb->holding_reg_rd_fp(reg_addr, &fp);
383 if (err == 0) {
384 memcpy(®, &fp, sizeof(reg));
385 sys_put_be32(reg, presp);
386 presp += sizeof(uint32_t);
387 }
388 }
389
390 if (err != 0) {
391 LOG_INF("Holding register address not supported");
392 mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_DATA_ADDR);
393 return true;
394 }
395
396 /* Increment current register address */
397 reg_addr++;
398 reg_qty--;
399 }
400
401 return true;
402 }
403
404 /*
405 * 04 (0x04) Read Input Registers
406 *
407 * Request Payload:
408 * Function code 1 Byte
409 * Starting Address 2 Bytes
410 * Quantity of Registers 2 Bytes
411 *
412 * Response Payload:
413 * Function code 1 Byte
414 * Byte count 1 Bytes
415 * Register Value N * 2 Byte
416 */
mbs_fc04_inreg_read(struct modbus_context * ctx)417 static bool mbs_fc04_inreg_read(struct modbus_context *ctx)
418 {
419 const uint16_t regs_limit = 125;
420 const uint8_t request_len = 4;
421 uint8_t *presp;
422 int err;
423 uint16_t reg_addr;
424 uint16_t reg_qty;
425 uint16_t num_bytes;
426
427 if (ctx->rx_adu.length != request_len) {
428 LOG_ERR("Wrong request length");
429 return false;
430 }
431
432 reg_addr = sys_get_be16(&ctx->rx_adu.data[0]);
433 reg_qty = sys_get_be16(&ctx->rx_adu.data[2]);
434
435 if ((reg_addr < MODBUS_FP_EXTENSIONS_ADDR) ||
436 !IS_ENABLED(CONFIG_MODBUS_FP_EXTENSIONS)) {
437 /* Read integer register */
438 if (ctx->mbs_user_cb->input_reg_rd == NULL) {
439 mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_FC);
440 return true;
441 }
442
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));
451 } else {
452 /* Read floating-point register */
453 if (ctx->mbs_user_cb->input_reg_rd_fp == NULL) {
454 mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_FC);
455 return true;
456 }
457
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);
461 return true;
462 }
463
464 /* Get number of bytes needed for response. */
465 num_bytes = (uint8_t)(reg_qty * sizeof(float));
466 }
467
468 /* Number of data bytes + byte count. */
469 ctx->tx_adu.length = num_bytes + 1;
470 /* Set number of data bytes in response message. */
471 ctx->tx_adu.data[0] = (uint8_t)num_bytes;
472
473 /* Reset the pointer to the start of the response payload */
474 presp = &ctx->tx_adu.data[1];
475 /* Loop through each register requested. */
476 while (reg_qty > 0) {
477 if (reg_addr < MODBUS_FP_EXTENSIONS_ADDR) {
478 uint16_t reg;
479
480 /* Read integer register */
481 err = ctx->mbs_user_cb->input_reg_rd(reg_addr, ®);
482 if (err == 0) {
483 sys_put_be16(reg, presp);
484 presp += sizeof(uint16_t);
485 }
486
487 } else if (IS_ENABLED(CONFIG_MODBUS_FP_EXTENSIONS)) {
488 float fp;
489 uint32_t reg;
490
491 /* Read floating-point register */
492 err = ctx->mbs_user_cb->input_reg_rd_fp(reg_addr, &fp);
493 if (err == 0) {
494 memcpy(®, &fp, sizeof(reg));
495 sys_put_be32(reg, presp);
496 presp += sizeof(uint32_t);
497 }
498 }
499
500 if (err != 0) {
501 LOG_INF("Input register address not supported");
502 mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_DATA_ADDR);
503 return true;
504 }
505
506 /* Increment current register number */
507 reg_addr++;
508 reg_qty--;
509 }
510
511 return true;
512 }
513
514 /*
515 * FC 05 (0x05) Write Single Coil
516 *
517 * Request Payload:
518 * Function code 1 Byte
519 * Output Address 2 Bytes
520 * Output Value 2 Bytes
521 *
522 * Response Payload:
523 * Function code 1 Byte
524 * Output Address 2 Bytes
525 * Output Value 2 Bytes
526 */
mbs_fc05_coil_write(struct modbus_context * ctx)527 static bool mbs_fc05_coil_write(struct modbus_context *ctx)
528 {
529 const uint8_t request_len = 4;
530 const uint8_t response_len = 4;
531 int err;
532 uint16_t coil_addr;
533 uint16_t coil_val;
534 bool coil_state;
535
536 if (ctx->rx_adu.length != request_len) {
537 LOG_ERR("Wrong request length %u", ctx->rx_adu.length);
538 return false;
539 }
540
541 if (ctx->mbs_user_cb->coil_wr == NULL) {
542 mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_FC);
543 return true;
544 }
545
546 /* Get the desired coil address and coil value */
547 coil_addr = sys_get_be16(&ctx->rx_adu.data[0]);
548 coil_val = sys_get_be16(&ctx->rx_adu.data[2]);
549
550 /* See if coil needs to be OFF? */
551 if (coil_val == MODBUS_COIL_OFF_CODE) {
552 coil_state = false;
553 } else {
554 coil_state = true;
555 }
556
557 err = ctx->mbs_user_cb->coil_wr(coil_addr, coil_state);
558
559 if (err != 0) {
560 LOG_INF("Coil address not supported");
561 mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_DATA_ADDR);
562 return true;
563 }
564
565 /* Assemble response payload */
566 ctx->tx_adu.length = response_len;
567 sys_put_be16(coil_addr, &ctx->tx_adu.data[0]);
568 sys_put_be16(coil_val, &ctx->tx_adu.data[2]);
569
570 return true;
571 }
572
573 /*
574 * 06 (0x06) Write Single Register
575 *
576 * Request Payload:
577 * Function code 1 Byte
578 * Register Address 2 Bytes
579 * Register Value 2 Bytes
580 *
581 * Response Payload:
582 * Function code 1 Byte
583 * Register Address 2 Bytes
584 * Register Value 2 Bytes
585 */
mbs_fc06_hreg_write(struct modbus_context * ctx)586 static bool mbs_fc06_hreg_write(struct modbus_context *ctx)
587 {
588 const uint8_t request_len = 4;
589 const uint8_t response_len = 4;
590 int err;
591 uint16_t reg_addr;
592 uint16_t reg_val;
593
594 if (ctx->rx_adu.length != request_len) {
595 LOG_ERR("Wrong request length %u", ctx->rx_adu.length);
596 return false;
597 }
598
599 if (ctx->mbs_user_cb->holding_reg_wr == NULL) {
600 mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_FC);
601 return true;
602 }
603
604 reg_addr = sys_get_be16(&ctx->rx_adu.data[0]);
605 reg_val = sys_get_be16(&ctx->rx_adu.data[2]);
606
607 err = ctx->mbs_user_cb->holding_reg_wr(reg_addr, reg_val);
608
609 if (err != 0) {
610 LOG_INF("Register address not supported");
611 mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_DATA_ADDR);
612 return true;
613 }
614
615 /* Assemble response payload */
616 ctx->tx_adu.length = response_len;
617 sys_put_be16(reg_addr, &ctx->tx_adu.data[0]);
618 sys_put_be16(reg_val, &ctx->tx_adu.data[2]);
619
620 return true;
621 }
622
623 /*
624 * 08 (0x08) Diagnostics
625 *
626 * Request Payload:
627 * Function code 1 Byte
628 * Sub-function code 2 Bytes
629 * Data N * 2 Byte
630 *
631 * Response Payload:
632 * Function code 1 Byte
633 * Sub-function code 2 Bytes
634 * Data N * 2 Byte
635 */
636 #ifdef CONFIG_MODBUS_FC08_DIAGNOSTIC
mbs_fc08_diagnostics(struct modbus_context * ctx)637 static bool mbs_fc08_diagnostics(struct modbus_context *ctx)
638 {
639 const uint8_t request_len = 4;
640 const uint8_t response_len = 4;
641 uint16_t sfunc;
642 uint16_t data;
643
644 if (ctx->rx_adu.length != request_len) {
645 LOG_ERR("Wrong request length %u", ctx->rx_adu.length);
646 return false;
647 }
648
649 sfunc = sys_get_be16(&ctx->rx_adu.data[0]);
650 data = sys_get_be16(&ctx->rx_adu.data[2]);
651
652 switch (sfunc) {
653 case MODBUS_FC08_SUBF_QUERY:
654 /* Sub-function 0x00 return Query Data */
655 break;
656
657 case MODBUS_FC08_SUBF_CLR_CTR:
658 /* Sub-function 0x0A clear Counters and Diagnostic */
659 modbus_reset_stats(ctx);
660 break;
661
662 case MODBUS_FC08_SUBF_BUS_MSG_CTR:
663 /* Sub-function 0x0B return Bus Message Count */
664 data = ctx->mbs_msg_ctr;
665 break;
666
667 case MODBUS_FC08_SUBF_BUS_CRC_CTR:
668 /* Sub-function 0x0C return Bus Communication Error Count */
669 data = ctx->mbs_crc_err_ctr;
670 break;
671
672 case MODBUS_FC08_SUBF_BUS_EXCEPT_CTR:
673 /* Sub-function 0x0D return Bus Exception Error Count */
674 data = ctx->mbs_except_ctr;
675 break;
676
677 case MODBUS_FC08_SUBF_SERVER_MSG_CTR:
678 /* Sub-function 0x0E return Server Message Count */
679 data = ctx->mbs_server_msg_ctr;
680 break;
681
682 case MODBUS_FC08_SUBF_SERVER_NO_RESP_CTR:
683 /* Sub-function 0x0F return Server No Response Count */
684 data = ctx->mbs_noresp_ctr;
685 break;
686
687 default:
688 LOG_INF("Sub-function not supported");
689 mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_FC);
690 return true;
691 }
692
693 /* Assemble response payload */
694 ctx->tx_adu.length = response_len;
695 sys_put_be16(sfunc, &ctx->tx_adu.data[0]);
696 sys_put_be16(data, &ctx->tx_adu.data[2]);
697
698 return true;
699 }
700 #else
mbs_fc08_diagnostics(struct modbus_context * ctx)701 static bool mbs_fc08_diagnostics(struct modbus_context *ctx)
702 {
703 mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_FC);
704
705 return true;
706 }
707 #endif
708
709 /*
710 * FC 15 (0x0F) Write Multiple Coils
711 *
712 * Request Payload:
713 * Function code 1 Byte
714 * Starting Address 2 Bytes
715 * Quantity of Outputs 2 Bytes
716 * Byte Count 1 Byte
717 * Outputs Value N * 1 Byte
718 *
719 * Response Payload:
720 * Function code 1 Byte
721 * Starting Address 2 Bytes
722 * Quantity of Outputs 2 Bytes
723 */
mbs_fc15_coils_write(struct modbus_context * ctx)724 static bool mbs_fc15_coils_write(struct modbus_context *ctx)
725 {
726 const uint16_t coils_limit = 2000;
727 const uint8_t request_len = 6;
728 const uint8_t response_len = 4;
729 uint8_t temp = 0;
730 int err;
731 uint16_t coil_addr;
732 uint16_t coil_qty;
733 uint16_t num_bytes;
734 uint16_t coil_cntr;
735 uint8_t data_ix;
736 bool coil_state;
737
738 if (ctx->rx_adu.length < request_len) {
739 LOG_ERR("Wrong request length %u", ctx->rx_adu.length);
740 return false;
741 }
742
743 if (ctx->mbs_user_cb->coil_wr == NULL) {
744 mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_FC);
745 return true;
746 }
747
748 coil_addr = sys_get_be16(&ctx->rx_adu.data[0]);
749 coil_qty = sys_get_be16(&ctx->rx_adu.data[2]);
750 /* Get the byte count for the data. */
751 num_bytes = ctx->rx_adu.data[4];
752
753 if (coil_qty == 0 || coil_qty > coils_limit) {
754 LOG_ERR("Number of coils limit exceeded");
755 mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_DATA_VAL);
756 return true;
757 }
758
759 /* Be sure byte count is valid for quantity of coils. */
760 if (((((coil_qty - 1) / 8) + 1) != num_bytes) ||
761 (ctx->rx_adu.length != (num_bytes + 5))) {
762 LOG_ERR("Mismatch in the number of coils");
763 mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_DATA_VAL);
764 return true;
765 }
766
767 coil_cntr = 0;
768 /* The 1st coil data byte is 6th element in payload */
769 data_ix = 5;
770 /* Loop through each coil to be forced. */
771 while (coil_cntr < coil_qty) {
772 /* Move to the next data byte after every eight bits. */
773 if ((coil_cntr % 8) == 0) {
774 temp = ctx->rx_adu.data[data_ix++];
775 }
776
777 if (temp & BIT(0)) {
778 coil_state = true;
779 } else {
780 coil_state = false;
781 }
782
783 err = ctx->mbs_user_cb->coil_wr(coil_addr + coil_cntr,
784 coil_state);
785
786 if (err != 0) {
787 LOG_INF("Coil address not supported");
788 mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_DATA_ADDR);
789 return true;
790 }
791
792 /* Shift the data one bit position * to the right. */
793 temp >>= 1;
794 /* Increment the COIL counter. */
795 coil_cntr++;
796 }
797
798 /* Assemble response payload */
799 ctx->tx_adu.length = response_len;
800 sys_put_be16(coil_addr, &ctx->tx_adu.data[0]);
801 sys_put_be16(coil_qty, &ctx->tx_adu.data[2]);
802
803 return true;
804 }
805
806 /*
807 * FC16 (0x10) Write Multiple registers
808 *
809 * Request Payload:
810 * Function code 1 Byte
811 * Starting Address 2 Bytes
812 * Quantity of Registers 2 Bytes
813 * Byte Count 1 Byte
814 * Registers Value N * 1 Byte
815 *
816 * Response Payload:
817 * Function code 1 Byte
818 * Starting Address 2 Bytes
819 * Quantity of Registers 2 Bytes
820 *
821 * If the address of the request exceeds or is equal to MODBUS_FP_EXTENSIONS_ADDR,
822 * then the function would write to multiple 'floating-point' according to
823 * the 'Daniels Flow Meter' extensions. This means that each register
824 * requested is considered as a 32-bit IEEE-754 floating-point format.
825 */
mbs_fc16_hregs_write(struct modbus_context * ctx)826 static bool mbs_fc16_hregs_write(struct modbus_context *ctx)
827 {
828 const uint16_t regs_limit = 125;
829 const uint8_t request_len = 6;
830 const uint8_t response_len = 4;
831 uint8_t *prx_data;
832 int err;
833 uint16_t reg_addr;
834 uint16_t reg_qty;
835 uint16_t num_bytes;
836 uint8_t reg_size;
837
838 if (ctx->rx_adu.length < request_len) {
839 LOG_ERR("Wrong request length %u", ctx->rx_adu.length);
840 return false;
841 }
842
843 reg_addr = sys_get_be16(&ctx->rx_adu.data[0]);
844 reg_qty = sys_get_be16(&ctx->rx_adu.data[2]);
845 /* Get the byte count for the data. */
846 num_bytes = ctx->rx_adu.data[4];
847
848 if ((reg_addr < MODBUS_FP_EXTENSIONS_ADDR) ||
849 !IS_ENABLED(CONFIG_MODBUS_FP_EXTENSIONS)) {
850 /* Write integer register */
851 if (ctx->mbs_user_cb->holding_reg_wr == NULL) {
852 mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_FC);
853 return true;
854 }
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);
863 } else {
864 /* Write floating-point register */
865 if (ctx->mbs_user_cb->holding_reg_wr_fp == NULL) {
866 mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_FC);
867 return true;
868 }
869
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);
873 return true;
874 }
875
876 reg_size = sizeof(float);
877 }
878
879 /* Compare number of bytes and payload length */
880 if ((ctx->rx_adu.length - 5) != num_bytes) {
881 LOG_ERR("Mismatch in the number of bytes");
882 mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_DATA_VAL);
883 return true;
884 }
885
886 if ((num_bytes / reg_qty) != (uint16_t)reg_size) {
887 LOG_ERR("Mismatch in the number of registers");
888 mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_DATA_VAL);
889 return true;
890 }
891
892 /* The 1st registers data byte is 6th element in payload */
893 prx_data = &ctx->rx_adu.data[5];
894
895 for (uint16_t reg_cntr = 0; reg_cntr < reg_qty; reg_cntr++) {
896 uint16_t addr = reg_addr + reg_cntr;
897
898 if ((reg_addr < MODBUS_FP_EXTENSIONS_ADDR) ||
899 !IS_ENABLED(CONFIG_MODBUS_FP_EXTENSIONS)) {
900 uint16_t reg_val = sys_get_be16(prx_data);
901
902 prx_data += sizeof(uint16_t);
903 err = ctx->mbs_user_cb->holding_reg_wr(addr, reg_val);
904 } else {
905 uint32_t reg_val = sys_get_be32(prx_data);
906 float fp;
907
908 /* Write to floating point register */
909 memcpy(&fp, ®_val, sizeof(float));
910 prx_data += sizeof(uint32_t);
911 err = ctx->mbs_user_cb->holding_reg_wr_fp(addr, fp);
912 }
913
914 if (err != 0) {
915 LOG_INF("Register address not supported");
916 mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_DATA_ADDR);
917 return true;
918 }
919 }
920
921 /* Assemble response payload */
922 ctx->tx_adu.length = response_len;
923 sys_put_be16(reg_addr, &ctx->tx_adu.data[0]);
924 sys_put_be16(reg_qty, &ctx->tx_adu.data[2]);
925
926 return true;
927 }
928
mbs_try_user_fc(struct modbus_context * ctx,uint8_t fc)929 static bool mbs_try_user_fc(struct modbus_context *ctx, uint8_t fc)
930 {
931 struct modbus_custom_fc *p;
932
933 LOG_DBG("Searching for custom Modbus handlers for code %u", fc);
934
935 SYS_SLIST_FOR_EACH_CONTAINER(&ctx->user_defined_cbs, p, node) {
936 if (p->fc == fc) {
937 int iface = modbus_iface_get_by_ctx(ctx);
938 bool rval;
939
940 LOG_DBG("Found custom handler");
941
942 p->excep_code = MODBUS_EXC_NONE;
943 rval = p->cb(iface, &ctx->rx_adu, &ctx->tx_adu, &p->excep_code,
944 p->user_data);
945
946 if (p->excep_code != MODBUS_EXC_NONE) {
947 LOG_INF("Custom handler failed with code %d", p->excep_code);
948 mbs_exception_rsp(ctx, p->excep_code);
949 }
950
951 return rval;
952 }
953 }
954
955 LOG_ERR("Function code 0x%02x not implemented", fc);
956 mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_FC);
957
958 return true;
959 }
960
modbus_server_handler(struct modbus_context * ctx)961 bool modbus_server_handler(struct modbus_context *ctx)
962 {
963 bool send_reply = false;
964 uint8_t addr = ctx->rx_adu.unit_id;
965 uint8_t fc = ctx->rx_adu.fc;
966
967 LOG_DBG("Server RX handler %p", ctx);
968 update_msg_ctr(ctx);
969
970 if (ctx->rx_adu_err != 0) {
971 update_noresp_ctr(ctx);
972 if (ctx->rx_adu_err == -EIO) {
973 update_crcerr_ctr(ctx);
974 }
975
976 return false;
977 }
978
979 if (addr != 0 && addr != ctx->unit_id) {
980 LOG_DBG("Unit ID doesn't match %u != %u", addr, ctx->unit_id);
981 update_noresp_ctr(ctx);
982 return false;
983 }
984
985 /* Prepare response header */
986 ctx->tx_adu.trans_id = ctx->rx_adu.trans_id;
987 ctx->tx_adu.proto_id = ctx->rx_adu.proto_id;
988 ctx->tx_adu.unit_id = addr;
989 ctx->tx_adu.fc = fc;
990
991 update_server_msg_ctr(ctx);
992
993 switch (fc) {
994 case MODBUS_FC01_COIL_RD:
995 send_reply = mbs_fc01_coil_read(ctx);
996 break;
997
998 case MODBUS_FC02_DI_RD:
999 send_reply = mbs_fc02_di_read(ctx);
1000 break;
1001
1002 case MODBUS_FC03_HOLDING_REG_RD:
1003 send_reply = mbs_fc03_hreg_read(ctx);
1004 break;
1005
1006 case MODBUS_FC04_IN_REG_RD:
1007 send_reply = mbs_fc04_inreg_read(ctx);
1008 break;
1009
1010 case MODBUS_FC05_COIL_WR:
1011 send_reply = mbs_fc05_coil_write(ctx);
1012 break;
1013
1014 case MODBUS_FC06_HOLDING_REG_WR:
1015 send_reply = mbs_fc06_hreg_write(ctx);
1016 break;
1017
1018 case MODBUS_FC08_DIAGNOSTICS:
1019 send_reply = mbs_fc08_diagnostics(ctx);
1020 break;
1021
1022 case MODBUS_FC15_COILS_WR:
1023 send_reply = mbs_fc15_coils_write(ctx);
1024 break;
1025
1026 case MODBUS_FC16_HOLDING_REGS_WR:
1027 send_reply = mbs_fc16_hregs_write(ctx);
1028 break;
1029
1030 default:
1031 send_reply = mbs_try_user_fc(ctx, fc);
1032 }
1033
1034 if (addr == 0) {
1035 /* Broadcast address, do not reply */
1036 send_reply = false;
1037 }
1038
1039 if (send_reply == false) {
1040 update_noresp_ctr(ctx);
1041 }
1042
1043 return send_reply;
1044 }
1045
