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_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
335 if ((reg_addr < MODBUS_FP_EXTENSIONS_ADDR) ||
336 !IS_ENABLED(CONFIG_MODBUS_FP_EXTENSIONS)) {
337 /* Read integer register */
338 if (ctx->mbs_user_cb->holding_reg_rd == NULL) {
339 mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_FC);
340 return true;
341 }
342
343 } else {
344 /* Read floating-point register */
345 if (ctx->mbs_user_cb->holding_reg_rd_fp == NULL) {
346 mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_FC);
347 return true;
348 }
349
350 if (num_bytes % sizeof(uint32_t)) {
351 mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_FC);
352 return true;
353 }
354 }
355
356 /* Number of data bytes + byte count. */
357 ctx->tx_adu.length = num_bytes + 1;
358 /* Set number of data bytes in response message. */
359 ctx->tx_adu.data[0] = (uint8_t)num_bytes;
360
361 /* Reset the pointer to the start of the response payload */
362 presp = &ctx->tx_adu.data[1];
363 /* Loop through each register requested. */
364 while (reg_qty > 0) {
365 if (reg_addr < MODBUS_FP_EXTENSIONS_ADDR) {
366 uint16_t reg;
367
368 /* Read integer register */
369 err = ctx->mbs_user_cb->holding_reg_rd(reg_addr, ®);
370 if (err == 0) {
371 sys_put_be16(reg, presp);
372 presp += sizeof(uint16_t);
373 }
374
375 /* Increment current register address */
376 reg_addr++;
377 reg_qty--;
378 } else if (IS_ENABLED(CONFIG_MODBUS_FP_EXTENSIONS)) {
379 float fp;
380 uint32_t reg;
381
382 /* Read floating-point register */
383 err = ctx->mbs_user_cb->holding_reg_rd_fp(reg_addr, &fp);
384 if (err == 0) {
385 memcpy(®, &fp, sizeof(reg));
386 sys_put_be32(reg, presp);
387 presp += sizeof(uint32_t);
388 }
389
390 /* Increment current register address */
391 reg_addr += 2;
392 reg_qty -= 2;
393 }
394
395 if (err != 0) {
396 LOG_INF("Holding register address not supported");
397 mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_DATA_ADDR);
398 return true;
399 }
400
401 }
402
403 return true;
404 }
405
406 /*
407 * 04 (0x04) Read Input Registers
408 *
409 * Request Payload:
410 * Function code 1 Byte
411 * Starting Address 2 Bytes
412 * Quantity of Registers 2 Bytes
413 *
414 * Response Payload:
415 * Function code 1 Byte
416 * Byte count 1 Bytes
417 * Register Value N * 2 Byte
418 */
mbs_fc04_inreg_read(struct modbus_context * ctx)419 static bool mbs_fc04_inreg_read(struct modbus_context *ctx)
420 {
421 const uint16_t regs_limit = 125;
422 const uint8_t request_len = 4;
423 uint8_t *presp;
424 int err;
425 uint16_t reg_addr;
426 uint16_t reg_qty;
427 uint16_t num_bytes;
428
429 if (ctx->rx_adu.length != request_len) {
430 LOG_ERR("Wrong request length");
431 return false;
432 }
433
434 reg_addr = sys_get_be16(&ctx->rx_adu.data[0]);
435 reg_qty = sys_get_be16(&ctx->rx_adu.data[2]);
436
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
447 if ((reg_addr < MODBUS_FP_EXTENSIONS_ADDR) ||
448 !IS_ENABLED(CONFIG_MODBUS_FP_EXTENSIONS)) {
449 /* Read integer register */
450 if (ctx->mbs_user_cb->input_reg_rd == NULL) {
451 mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_FC);
452 return true;
453 }
454
455 } else {
456 /* Read floating-point register */
457 if (ctx->mbs_user_cb->input_reg_rd_fp == NULL) {
458 mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_FC);
459 return true;
460 }
461
462 if (num_bytes % sizeof(uint32_t)) {
463 mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_FC);
464 return true;
465 }
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 /* Increment current register number */
488 reg_addr++;
489 reg_qty--;
490 } else if (IS_ENABLED(CONFIG_MODBUS_FP_EXTENSIONS)) {
491 float fp;
492 uint32_t reg;
493
494 /* Read floating-point register */
495 err = ctx->mbs_user_cb->input_reg_rd_fp(reg_addr, &fp);
496 if (err == 0) {
497 memcpy(®, &fp, sizeof(reg));
498 sys_put_be32(reg, presp);
499 presp += sizeof(uint32_t);
500 }
501
502 /* Increment current register address */
503 reg_addr += 2;
504 reg_qty -= 2;
505 }
506
507 if (err != 0) {
508 LOG_INF("Input register address not supported");
509 mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_DATA_ADDR);
510 return true;
511 }
512 }
513
514 return true;
515 }
516
517 /*
518 * FC 05 (0x05) Write Single Coil
519 *
520 * Request Payload:
521 * Function code 1 Byte
522 * Output Address 2 Bytes
523 * Output Value 2 Bytes
524 *
525 * Response Payload:
526 * Function code 1 Byte
527 * Output Address 2 Bytes
528 * Output Value 2 Bytes
529 */
mbs_fc05_coil_write(struct modbus_context * ctx)530 static bool mbs_fc05_coil_write(struct modbus_context *ctx)
531 {
532 const uint8_t request_len = 4;
533 const uint8_t response_len = 4;
534 int err;
535 uint16_t coil_addr;
536 uint16_t coil_val;
537 bool coil_state;
538
539 if (ctx->rx_adu.length != request_len) {
540 LOG_ERR("Wrong request length %u", ctx->rx_adu.length);
541 return false;
542 }
543
544 if (ctx->mbs_user_cb->coil_wr == NULL) {
545 mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_FC);
546 return true;
547 }
548
549 /* Get the desired coil address and coil value */
550 coil_addr = sys_get_be16(&ctx->rx_adu.data[0]);
551 coil_val = sys_get_be16(&ctx->rx_adu.data[2]);
552
553 /* See if coil needs to be OFF? */
554 if (coil_val == MODBUS_COIL_OFF_CODE) {
555 coil_state = false;
556 } else {
557 coil_state = true;
558 }
559
560 err = ctx->mbs_user_cb->coil_wr(coil_addr, coil_state);
561
562 if (err != 0) {
563 LOG_INF("Coil address not supported");
564 mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_DATA_ADDR);
565 return true;
566 }
567
568 /* Assemble response payload */
569 ctx->tx_adu.length = response_len;
570 sys_put_be16(coil_addr, &ctx->tx_adu.data[0]);
571 sys_put_be16(coil_val, &ctx->tx_adu.data[2]);
572
573 return true;
574 }
575
576 /*
577 * 06 (0x06) Write Single Register
578 *
579 * Request Payload:
580 * Function code 1 Byte
581 * Register Address 2 Bytes
582 * Register Value 2 Bytes
583 *
584 * Response Payload:
585 * Function code 1 Byte
586 * Register Address 2 Bytes
587 * Register Value 2 Bytes
588 */
mbs_fc06_hreg_write(struct modbus_context * ctx)589 static bool mbs_fc06_hreg_write(struct modbus_context *ctx)
590 {
591 const uint8_t request_len = 4;
592 const uint8_t response_len = 4;
593 int err;
594 uint16_t reg_addr;
595 uint16_t reg_val;
596
597 if (ctx->rx_adu.length != request_len) {
598 LOG_ERR("Wrong request length %u", ctx->rx_adu.length);
599 return false;
600 }
601
602 if (ctx->mbs_user_cb->holding_reg_wr == NULL) {
603 mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_FC);
604 return true;
605 }
606
607 reg_addr = sys_get_be16(&ctx->rx_adu.data[0]);
608 reg_val = sys_get_be16(&ctx->rx_adu.data[2]);
609
610 err = ctx->mbs_user_cb->holding_reg_wr(reg_addr, reg_val);
611
612 if (err != 0) {
613 LOG_INF("Register address not supported");
614 mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_DATA_ADDR);
615 return true;
616 }
617
618 /* Assemble response payload */
619 ctx->tx_adu.length = response_len;
620 sys_put_be16(reg_addr, &ctx->tx_adu.data[0]);
621 sys_put_be16(reg_val, &ctx->tx_adu.data[2]);
622
623 return true;
624 }
625
626 /*
627 * 08 (0x08) Diagnostics
628 *
629 * Request Payload:
630 * Function code 1 Byte
631 * Sub-function code 2 Bytes
632 * Data N * 2 Byte
633 *
634 * Response Payload:
635 * Function code 1 Byte
636 * Sub-function code 2 Bytes
637 * Data N * 2 Byte
638 */
639 #ifdef CONFIG_MODBUS_FC08_DIAGNOSTIC
mbs_fc08_diagnostics(struct modbus_context * ctx)640 static bool mbs_fc08_diagnostics(struct modbus_context *ctx)
641 {
642 const uint8_t request_len = 4;
643 const uint8_t response_len = 4;
644 uint16_t sfunc;
645 uint16_t data;
646
647 if (ctx->rx_adu.length != request_len) {
648 LOG_ERR("Wrong request length %u", ctx->rx_adu.length);
649 return false;
650 }
651
652 sfunc = sys_get_be16(&ctx->rx_adu.data[0]);
653 data = sys_get_be16(&ctx->rx_adu.data[2]);
654
655 switch (sfunc) {
656 case MODBUS_FC08_SUBF_QUERY:
657 /* Sub-function 0x00 return Query Data */
658 break;
659
660 case MODBUS_FC08_SUBF_CLR_CTR:
661 /* Sub-function 0x0A clear Counters and Diagnostic */
662 modbus_reset_stats(ctx);
663 break;
664
665 case MODBUS_FC08_SUBF_BUS_MSG_CTR:
666 /* Sub-function 0x0B return Bus Message Count */
667 data = ctx->mbs_msg_ctr;
668 break;
669
670 case MODBUS_FC08_SUBF_BUS_CRC_CTR:
671 /* Sub-function 0x0C return Bus Communication Error Count */
672 data = ctx->mbs_crc_err_ctr;
673 break;
674
675 case MODBUS_FC08_SUBF_BUS_EXCEPT_CTR:
676 /* Sub-function 0x0D return Bus Exception Error Count */
677 data = ctx->mbs_except_ctr;
678 break;
679
680 case MODBUS_FC08_SUBF_SERVER_MSG_CTR:
681 /* Sub-function 0x0E return Server Message Count */
682 data = ctx->mbs_server_msg_ctr;
683 break;
684
685 case MODBUS_FC08_SUBF_SERVER_NO_RESP_CTR:
686 /* Sub-function 0x0F return Server No Response Count */
687 data = ctx->mbs_noresp_ctr;
688 break;
689
690 default:
691 LOG_INF("Sub-function not supported");
692 mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_FC);
693 return true;
694 }
695
696 /* Assemble response payload */
697 ctx->tx_adu.length = response_len;
698 sys_put_be16(sfunc, &ctx->tx_adu.data[0]);
699 sys_put_be16(data, &ctx->tx_adu.data[2]);
700
701 return true;
702 }
703 #else
mbs_fc08_diagnostics(struct modbus_context * ctx)704 static bool mbs_fc08_diagnostics(struct modbus_context *ctx)
705 {
706 mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_FC);
707
708 return true;
709 }
710 #endif
711
712 /*
713 * FC 15 (0x0F) Write Multiple Coils
714 *
715 * Request Payload:
716 * Function code 1 Byte
717 * Starting Address 2 Bytes
718 * Quantity of Outputs 2 Bytes
719 * Byte Count 1 Byte
720 * Outputs Value N * 1 Byte
721 *
722 * Response Payload:
723 * Function code 1 Byte
724 * Starting Address 2 Bytes
725 * Quantity of Outputs 2 Bytes
726 */
mbs_fc15_coils_write(struct modbus_context * ctx)727 static bool mbs_fc15_coils_write(struct modbus_context *ctx)
728 {
729 const uint16_t coils_limit = 2000;
730 const uint8_t request_len = 6;
731 const uint8_t response_len = 4;
732 uint8_t temp = 0;
733 int err;
734 uint16_t coil_addr;
735 uint16_t coil_qty;
736 uint16_t num_bytes;
737 uint16_t coil_cntr;
738 uint8_t data_ix;
739 bool coil_state;
740
741 if (ctx->rx_adu.length < request_len) {
742 LOG_ERR("Wrong request length %u", ctx->rx_adu.length);
743 return false;
744 }
745
746 if (ctx->mbs_user_cb->coil_wr == NULL) {
747 mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_FC);
748 return true;
749 }
750
751 coil_addr = sys_get_be16(&ctx->rx_adu.data[0]);
752 coil_qty = sys_get_be16(&ctx->rx_adu.data[2]);
753 /* Get the byte count for the data. */
754 num_bytes = ctx->rx_adu.data[4];
755
756 if (coil_qty == 0 || coil_qty > coils_limit) {
757 LOG_ERR("Number of coils limit exceeded");
758 mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_DATA_VAL);
759 return true;
760 }
761
762 /* Be sure byte count is valid for quantity of coils. */
763 if (((((coil_qty - 1) / 8) + 1) != num_bytes) ||
764 (ctx->rx_adu.length != (num_bytes + 5))) {
765 LOG_ERR("Mismatch in the number of coils");
766 mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_DATA_VAL);
767 return true;
768 }
769
770 coil_cntr = 0;
771 /* The 1st coil data byte is 6th element in payload */
772 data_ix = 5;
773 /* Loop through each coil to be forced. */
774 while (coil_cntr < coil_qty) {
775 /* Move to the next data byte after every eight bits. */
776 if ((coil_cntr % 8) == 0) {
777 temp = ctx->rx_adu.data[data_ix++];
778 }
779
780 if (temp & BIT(0)) {
781 coil_state = true;
782 } else {
783 coil_state = false;
784 }
785
786 err = ctx->mbs_user_cb->coil_wr(coil_addr + coil_cntr,
787 coil_state);
788
789 if (err != 0) {
790 LOG_INF("Coil address not supported");
791 mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_DATA_ADDR);
792 return true;
793 }
794
795 /* Shift the data one bit position * to the right. */
796 temp >>= 1;
797 /* Increment the COIL counter. */
798 coil_cntr++;
799 }
800
801 /* Assemble response payload */
802 ctx->tx_adu.length = response_len;
803 sys_put_be16(coil_addr, &ctx->tx_adu.data[0]);
804 sys_put_be16(coil_qty, &ctx->tx_adu.data[2]);
805
806 return true;
807 }
808
809 /*
810 * FC16 (0x10) Write Multiple registers
811 *
812 * Request Payload:
813 * Function code 1 Byte
814 * Starting Address 2 Bytes
815 * Quantity of Registers 2 Bytes
816 * Byte Count 1 Byte
817 * Registers Value N * 1 Byte
818 *
819 * Response Payload:
820 * Function code 1 Byte
821 * Starting Address 2 Bytes
822 * Quantity of Registers 2 Bytes
823 *
824 * If the address of the request exceeds or is equal to MODBUS_FP_EXTENSIONS_ADDR,
825 * then the function would write to multiple 'floating-point' according to
826 * the 'Daniels Flow Meter' extensions. This means that each register
827 * requested is considered as a 32-bit IEEE-754 floating-point format.
828 */
mbs_fc16_hregs_write(struct modbus_context * ctx)829 static bool mbs_fc16_hregs_write(struct modbus_context *ctx)
830 {
831 const uint16_t regs_limit = 125;
832 const uint8_t request_len = 6;
833 const uint8_t response_len = 4;
834 uint8_t *prx_data;
835 int err;
836 uint16_t reg_addr;
837 uint16_t reg_qty;
838 uint16_t num_bytes;
839
840 if (ctx->rx_adu.length < request_len) {
841 LOG_ERR("Wrong request length %u", ctx->rx_adu.length);
842 return false;
843 }
844
845 reg_addr = sys_get_be16(&ctx->rx_adu.data[0]);
846 reg_qty = sys_get_be16(&ctx->rx_adu.data[2]);
847 /* Get the byte count for the data. */
848 num_bytes = ctx->rx_adu.data[4];
849
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
856 if ((reg_addr < MODBUS_FP_EXTENSIONS_ADDR) ||
857 !IS_ENABLED(CONFIG_MODBUS_FP_EXTENSIONS)) {
858 /* Write integer register */
859 if (ctx->mbs_user_cb->holding_reg_wr == NULL) {
860 mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_FC);
861 return true;
862 }
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 (num_bytes % sizeof(uint32_t)) {
871 mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_FC);
872 return true;
873 }
874 }
875
876 /* Compare number of bytes and payload length */
877 if ((ctx->rx_adu.length - 5) != num_bytes) {
878 LOG_ERR("Mismatch in the number of bytes");
879 mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_DATA_VAL);
880 return true;
881 }
882
883 if ((num_bytes / reg_qty) != sizeof(uint16_t)) {
884 LOG_ERR("Mismatch in the number of registers");
885 mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_DATA_VAL);
886 return true;
887 }
888
889 /* The 1st registers data byte is 6th element in payload */
890 prx_data = &ctx->rx_adu.data[5];
891
892 for (uint16_t reg_cntr = 0; reg_cntr < reg_qty;) {
893 uint16_t addr = reg_addr + reg_cntr;
894
895 if ((reg_addr < MODBUS_FP_EXTENSIONS_ADDR) ||
896 !IS_ENABLED(CONFIG_MODBUS_FP_EXTENSIONS)) {
897 uint16_t reg_val = sys_get_be16(prx_data);
898
899 prx_data += sizeof(uint16_t);
900 err = ctx->mbs_user_cb->holding_reg_wr(addr, reg_val);
901 reg_cntr++;
902 } else {
903 uint32_t reg_val = sys_get_be32(prx_data);
904 float fp;
905
906 /* Write to floating point register */
907 memcpy(&fp, ®_val, sizeof(uint32_t));
908 prx_data += sizeof(uint32_t);
909 err = ctx->mbs_user_cb->holding_reg_wr_fp(addr, fp);
910 reg_cntr += 2;
911 }
912
913 if (err != 0) {
914 LOG_INF("Register address not supported");
915 mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_DATA_ADDR);
916 return true;
917 }
918 }
919
920 /* Assemble response payload */
921 ctx->tx_adu.length = response_len;
922 sys_put_be16(reg_addr, &ctx->tx_adu.data[0]);
923 sys_put_be16(reg_qty, &ctx->tx_adu.data[2]);
924
925 return true;
926 }
927
mbs_try_user_fc(struct modbus_context * ctx,uint8_t fc)928 static bool mbs_try_user_fc(struct modbus_context *ctx, uint8_t fc)
929 {
930 struct modbus_custom_fc *p;
931
932 LOG_DBG("Searching for custom Modbus handlers for code %u", fc);
933
934 SYS_SLIST_FOR_EACH_CONTAINER(&ctx->user_defined_cbs, p, node) {
935 if (p->fc == fc) {
936 int iface = modbus_iface_get_by_ctx(ctx);
937 bool rval;
938
939 LOG_DBG("Found custom handler");
940
941 p->excep_code = MODBUS_EXC_NONE;
942 rval = p->cb(iface, &ctx->rx_adu, &ctx->tx_adu, &p->excep_code,
943 p->user_data);
944
945 if (p->excep_code != MODBUS_EXC_NONE) {
946 LOG_INF("Custom handler failed with code %d", p->excep_code);
947 mbs_exception_rsp(ctx, p->excep_code);
948 }
949
950 return rval;
951 }
952 }
953
954 LOG_ERR("Function code 0x%02x not implemented", fc);
955 mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_FC);
956
957 return true;
958 }
959
modbus_server_handler(struct modbus_context * ctx)960 bool modbus_server_handler(struct modbus_context *ctx)
961 {
962 bool send_reply = false;
963 uint8_t addr = ctx->rx_adu.unit_id;
964 uint8_t fc = ctx->rx_adu.fc;
965
966 LOG_DBG("Server RX handler %p", ctx);
967 update_msg_ctr(ctx);
968
969 if (ctx->rx_adu_err != 0) {
970 update_noresp_ctr(ctx);
971 if (ctx->rx_adu_err == -EIO) {
972 update_crcerr_ctr(ctx);
973 }
974
975 return false;
976 }
977
978 if (addr != 0 && addr != ctx->unit_id) {
979 LOG_DBG("Unit ID doesn't match %u != %u", addr, ctx->unit_id);
980 update_noresp_ctr(ctx);
981 return false;
982 }
983
984 /* Prepare response header */
985 ctx->tx_adu.trans_id = ctx->rx_adu.trans_id;
986 ctx->tx_adu.proto_id = ctx->rx_adu.proto_id;
987 ctx->tx_adu.unit_id = addr;
988 ctx->tx_adu.fc = fc;
989
990 update_server_msg_ctr(ctx);
991
992 switch (fc) {
993 case MODBUS_FC01_COIL_RD:
994 send_reply = mbs_fc01_coil_read(ctx);
995 break;
996
997 case MODBUS_FC02_DI_RD:
998 send_reply = mbs_fc02_di_read(ctx);
999 break;
1000
1001 case MODBUS_FC03_HOLDING_REG_RD:
1002 send_reply = mbs_fc03_hreg_read(ctx);
1003 break;
1004
1005 case MODBUS_FC04_IN_REG_RD:
1006 send_reply = mbs_fc04_inreg_read(ctx);
1007 break;
1008
1009 case MODBUS_FC05_COIL_WR:
1010 send_reply = mbs_fc05_coil_write(ctx);
1011 break;
1012
1013 case MODBUS_FC06_HOLDING_REG_WR:
1014 send_reply = mbs_fc06_hreg_write(ctx);
1015 break;
1016
1017 case MODBUS_FC08_DIAGNOSTICS:
1018 send_reply = mbs_fc08_diagnostics(ctx);
1019 break;
1020
1021 case MODBUS_FC15_COILS_WR:
1022 send_reply = mbs_fc15_coils_write(ctx);
1023 break;
1024
1025 case MODBUS_FC16_HOLDING_REGS_WR:
1026 send_reply = mbs_fc16_hregs_write(ctx);
1027 break;
1028
1029 default:
1030 send_reply = mbs_try_user_fc(ctx, fc);
1031 }
1032
1033 if (addr == 0) {
1034 /* Broadcast address, do not reply */
1035 send_reply = false;
1036 }
1037
1038 if (send_reply == false) {
1039 update_noresp_ctr(ctx);
1040 }
1041
1042 return send_reply;
1043 }
1044
