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 mbm_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_c, CONFIG_MODBUS_LOG_LEVEL);
28
mbc_validate_response_fc(struct modbus_context * ctx,const uint8_t unit_id,uint8_t fc)29 static int mbc_validate_response_fc(struct modbus_context *ctx,
30 const uint8_t unit_id,
31 uint8_t fc)
32 {
33 uint8_t resp_fc = ctx->rx_adu.fc;
34 uint8_t excep_code = ctx->rx_adu.data[0];
35 const uint8_t excep_bit = BIT(7);
36 const uint8_t excep_mask = BIT_MASK(7);
37
38 if (unit_id != ctx->rx_adu.unit_id) {
39 return -EIO;
40 }
41
42 if (fc != (resp_fc & excep_mask)) {
43 return -EIO;
44 }
45
46 if (resp_fc & excep_bit) {
47 if (excep_code > MODBUS_EXC_NONE) {
48 return excep_code;
49 }
50
51 return -EIO;
52 }
53
54 return 0;
55 }
56
mbc_validate_fc03fp_response(struct modbus_context * ctx,float * ptbl)57 static int mbc_validate_fc03fp_response(struct modbus_context *ctx, float *ptbl)
58 {
59 size_t resp_byte_cnt;
60 size_t req_byte_cnt;
61 uint16_t req_qty;
62 uint8_t *resp_data;
63
64 resp_byte_cnt = ctx->rx_adu.data[0];
65 resp_data = &ctx->rx_adu.data[1];
66 req_qty = sys_get_be16(&ctx->tx_adu.data[2]);
67 req_byte_cnt = req_qty * sizeof(float);
68
69 if (req_byte_cnt != resp_byte_cnt) {
70 LOG_ERR("Mismatch in the number of registers");
71 return -EINVAL;
72 }
73
74 for (uint16_t i = 0; i < req_qty; i++) {
75 uint32_t reg_val = sys_get_be32(resp_data);
76
77 memcpy(&ptbl[i], ®_val, sizeof(float));
78 resp_data += sizeof(uint32_t);
79 }
80
81 return 0;
82 }
83
mbc_validate_rd_response(struct modbus_context * ctx,const uint8_t unit_id,uint8_t fc,uint8_t * data)84 static int mbc_validate_rd_response(struct modbus_context *ctx,
85 const uint8_t unit_id,
86 uint8_t fc,
87 uint8_t *data)
88 {
89 int err;
90 size_t resp_byte_cnt;
91 size_t req_byte_cnt;
92 uint16_t req_qty;
93 uint16_t req_addr;
94 uint8_t *resp_data;
95 uint16_t *data_p16 = (uint16_t *)data;
96
97 if (data == NULL) {
98 return -EINVAL;
99 }
100
101 resp_byte_cnt = ctx->rx_adu.data[0];
102 resp_data = &ctx->rx_adu.data[1];
103 req_qty = sys_get_be16(&ctx->tx_adu.data[2]);
104 req_addr = sys_get_be16(&ctx->tx_adu.data[0]);
105
106 if ((resp_byte_cnt + 1) > sizeof(ctx->rx_adu.data)) {
107 LOG_ERR("Byte count exceeds buffer length");
108 return -EINVAL;
109 }
110
111 switch (fc) {
112 case MODBUS_FC01_COIL_RD:
113 case MODBUS_FC02_DI_RD:
114 req_byte_cnt = ((req_qty - 1) / 8) + 1;
115 if (req_byte_cnt != resp_byte_cnt) {
116 LOG_ERR("Mismatch in the number of coils or inputs");
117 err = -EINVAL;
118 } else {
119 for (uint8_t i = 0; i < resp_byte_cnt; i++) {
120 data[i] = resp_data[i];
121 }
122
123 err = 0;
124 }
125 break;
126
127 case MODBUS_FC03_HOLDING_REG_RD:
128 if (IS_ENABLED(CONFIG_MODBUS_FP_EXTENSIONS) &&
129 (req_addr >= MODBUS_FP_EXTENSIONS_ADDR)) {
130 err = mbc_validate_fc03fp_response(ctx, (float *)data);
131 break;
132 }
133 __fallthrough;
134 case MODBUS_FC04_IN_REG_RD:
135 req_byte_cnt = req_qty * sizeof(uint16_t);
136 if (req_byte_cnt != resp_byte_cnt) {
137 LOG_ERR("Mismatch in the number of registers");
138 err = -EINVAL;
139 } else {
140 for (uint16_t i = 0; i < req_qty; i++) {
141 data_p16[i] = sys_get_be16(resp_data);
142 resp_data += sizeof(uint16_t);
143 }
144
145 err = 0;
146 }
147 break;
148
149 default:
150 LOG_ERR("Validation not implemented for FC 0x%02x", fc);
151 err = -ENOTSUP;
152 }
153
154 return err;
155 }
156
mbc_validate_fc08_response(struct modbus_context * ctx,const uint8_t unit_id,uint16_t * data)157 static int mbc_validate_fc08_response(struct modbus_context *ctx,
158 const uint8_t unit_id,
159 uint16_t *data)
160 {
161 int err;
162 uint16_t resp_sfunc;
163 uint16_t resp_data;
164 uint16_t req_sfunc;
165 uint16_t req_data;
166
167 if (data == NULL) {
168 return -EINVAL;
169 }
170
171 req_sfunc = sys_get_be16(&ctx->tx_adu.data[0]);
172 req_data = sys_get_be16(&ctx->tx_adu.data[2]);
173 resp_sfunc = sys_get_be16(&ctx->rx_adu.data[0]);
174 resp_data = sys_get_be16(&ctx->rx_adu.data[2]);
175
176 if (req_sfunc != resp_sfunc) {
177 LOG_ERR("Mismatch in the sub-function code");
178 return -EINVAL;
179 }
180
181 switch (resp_sfunc) {
182 case MODBUS_FC08_SUBF_QUERY:
183 case MODBUS_FC08_SUBF_CLR_CTR:
184 if (req_data != resp_data) {
185 LOG_ERR("Request and response data are different");
186 err = -EINVAL;
187 } else {
188 *data = resp_data;
189 err = 0;
190 }
191 break;
192
193 case MODBUS_FC08_SUBF_BUS_MSG_CTR:
194 case MODBUS_FC08_SUBF_BUS_CRC_CTR:
195 case MODBUS_FC08_SUBF_BUS_EXCEPT_CTR:
196 case MODBUS_FC08_SUBF_SERVER_MSG_CTR:
197 case MODBUS_FC08_SUBF_SERVER_NO_RESP_CTR:
198 *data = resp_data;
199 err = 0;
200 break;
201
202 default:
203 err = -EINVAL;
204 }
205
206 return err;
207 }
208
mbc_validate_wr_response(struct modbus_context * ctx,const uint8_t unit_id,uint8_t fc)209 static int mbc_validate_wr_response(struct modbus_context *ctx,
210 const uint8_t unit_id,
211 uint8_t fc)
212 {
213 int err;
214 uint16_t req_addr;
215 uint16_t req_value;
216 uint16_t resp_addr;
217 uint16_t resp_value;
218
219 req_addr = sys_get_be16(&ctx->tx_adu.data[0]);
220 req_value = sys_get_be16(&ctx->tx_adu.data[2]);
221 resp_addr = sys_get_be16(&ctx->rx_adu.data[0]);
222 resp_value = sys_get_be16(&ctx->rx_adu.data[2]);
223
224 switch (fc) {
225 case MODBUS_FC05_COIL_WR:
226 case MODBUS_FC06_HOLDING_REG_WR:
227 case MODBUS_FC15_COILS_WR:
228 case MODBUS_FC16_HOLDING_REGS_WR:
229 if (req_addr != resp_addr || req_value != resp_value) {
230 err = ENXIO;
231 } else {
232 err = 0;
233 }
234 break;
235
236 default:
237 LOG_ERR("Validation not implemented for FC 0x%02x", fc);
238 err = -ENOTSUP;
239 }
240
241 return err;
242 }
243
mbc_send_cmd(struct modbus_context * ctx,const uint8_t unit_id,uint8_t fc,void * data)244 static int mbc_send_cmd(struct modbus_context *ctx, const uint8_t unit_id,
245 uint8_t fc, void *data)
246 {
247 int err;
248
249 ctx->tx_adu.unit_id = unit_id;
250 ctx->tx_adu.fc = fc;
251
252 err = modbus_tx_wait_rx_adu(ctx);
253 if (err != 0) {
254 return err;
255 }
256
257 err = mbc_validate_response_fc(ctx, unit_id, fc);
258 if (err < 0) {
259 LOG_ERR("Failed to validate unit ID or function code");
260 return err;
261 } else if (err > 0) {
262 LOG_INF("Modbus FC %u, error code %u", fc, err);
263 return err;
264 }
265
266 switch (fc) {
267 case MODBUS_FC01_COIL_RD:
268 case MODBUS_FC02_DI_RD:
269 case MODBUS_FC03_HOLDING_REG_RD:
270 case MODBUS_FC04_IN_REG_RD:
271 err = mbc_validate_rd_response(ctx, unit_id, fc, data);
272 break;
273
274 case MODBUS_FC08_DIAGNOSTICS:
275 err = mbc_validate_fc08_response(ctx, unit_id, data);
276 break;
277
278 case MODBUS_FC05_COIL_WR:
279 case MODBUS_FC06_HOLDING_REG_WR:
280 case MODBUS_FC15_COILS_WR:
281 case MODBUS_FC16_HOLDING_REGS_WR:
282 err = mbc_validate_wr_response(ctx, unit_id, fc);
283 break;
284
285 default:
286 LOG_ERR("FC 0x%02x not implemented", fc);
287 err = -ENOTSUP;
288 }
289
290 return err;
291 }
292
modbus_read_coils(const int iface,const uint8_t unit_id,const uint16_t start_addr,uint8_t * const coil_tbl,const uint16_t num_coils)293 int modbus_read_coils(const int iface,
294 const uint8_t unit_id,
295 const uint16_t start_addr,
296 uint8_t *const coil_tbl,
297 const uint16_t num_coils)
298 {
299 struct modbus_context *ctx = modbus_get_context(iface);
300 int err;
301
302 if (ctx == NULL) {
303 return -ENODEV;
304 }
305
306 k_mutex_lock(&ctx->iface_lock, K_FOREVER);
307
308 ctx->tx_adu.length = 4;
309 sys_put_be16(start_addr, &ctx->tx_adu.data[0]);
310 sys_put_be16(num_coils, &ctx->tx_adu.data[2]);
311
312 err = mbc_send_cmd(ctx, unit_id, MODBUS_FC01_COIL_RD, coil_tbl);
313 k_mutex_unlock(&ctx->iface_lock);
314
315 return err;
316 }
317
modbus_read_dinputs(const int iface,const uint8_t unit_id,const uint16_t start_addr,uint8_t * const di_tbl,const uint16_t num_di)318 int modbus_read_dinputs(const int iface,
319 const uint8_t unit_id,
320 const uint16_t start_addr,
321 uint8_t *const di_tbl,
322 const uint16_t num_di)
323 {
324 struct modbus_context *ctx = modbus_get_context(iface);
325 int err;
326
327 if (ctx == NULL) {
328 return -ENODEV;
329 }
330
331 k_mutex_lock(&ctx->iface_lock, K_FOREVER);
332
333 ctx->tx_adu.length = 4;
334 sys_put_be16(start_addr, &ctx->tx_adu.data[0]);
335 sys_put_be16(num_di, &ctx->tx_adu.data[2]);
336
337 err = mbc_send_cmd(ctx, unit_id, MODBUS_FC02_DI_RD, di_tbl);
338 k_mutex_unlock(&ctx->iface_lock);
339
340 return err;
341 }
342
modbus_read_holding_regs(const int iface,const uint8_t unit_id,const uint16_t start_addr,uint16_t * const reg_buf,const uint16_t num_regs)343 int modbus_read_holding_regs(const int iface,
344 const uint8_t unit_id,
345 const uint16_t start_addr,
346 uint16_t *const reg_buf,
347 const uint16_t num_regs)
348 {
349 struct modbus_context *ctx = modbus_get_context(iface);
350 int err;
351
352 if (ctx == NULL) {
353 return -ENODEV;
354 }
355
356 k_mutex_lock(&ctx->iface_lock, K_FOREVER);
357
358 ctx->tx_adu.length = 4;
359 sys_put_be16(start_addr, &ctx->tx_adu.data[0]);
360 sys_put_be16(num_regs, &ctx->tx_adu.data[2]);
361
362 err = mbc_send_cmd(ctx, unit_id, MODBUS_FC03_HOLDING_REG_RD, reg_buf);
363 k_mutex_unlock(&ctx->iface_lock);
364
365 return err;
366 }
367
368
369 #ifdef CONFIG_MODBUS_FP_EXTENSIONS
modbus_read_holding_regs_fp(const int iface,const uint8_t unit_id,const uint16_t start_addr,float * const reg_buf,const uint16_t num_regs)370 int modbus_read_holding_regs_fp(const int iface,
371 const uint8_t unit_id,
372 const uint16_t start_addr,
373 float *const reg_buf,
374 const uint16_t num_regs)
375 {
376 struct modbus_context *ctx = modbus_get_context(iface);
377 int err;
378
379 if (ctx == NULL) {
380 return -ENODEV;
381 }
382
383 k_mutex_lock(&ctx->iface_lock, K_FOREVER);
384
385 ctx->tx_adu.length = 4;
386 sys_put_be16(start_addr, &ctx->tx_adu.data[0]);
387 sys_put_be16(num_regs, &ctx->tx_adu.data[2]);
388
389 err = mbc_send_cmd(ctx, unit_id, MODBUS_FC03_HOLDING_REG_RD, reg_buf);
390 k_mutex_unlock(&ctx->iface_lock);
391
392 return err;
393 }
394 #endif
395
modbus_read_input_regs(const int iface,const uint8_t unit_id,const uint16_t start_addr,uint16_t * const reg_buf,const uint16_t num_regs)396 int modbus_read_input_regs(const int iface,
397 const uint8_t unit_id,
398 const uint16_t start_addr,
399 uint16_t *const reg_buf,
400 const uint16_t num_regs)
401 {
402 struct modbus_context *ctx = modbus_get_context(iface);
403 int err;
404
405 if (ctx == NULL) {
406 return -ENODEV;
407 }
408
409 k_mutex_lock(&ctx->iface_lock, K_FOREVER);
410
411 ctx->tx_adu.length = 4;
412 sys_put_be16(start_addr, &ctx->tx_adu.data[0]);
413 sys_put_be16(num_regs, &ctx->tx_adu.data[2]);
414
415 err = mbc_send_cmd(ctx, unit_id, MODBUS_FC04_IN_REG_RD, reg_buf);
416 k_mutex_unlock(&ctx->iface_lock);
417
418 return err;
419 }
420
modbus_write_coil(const int iface,const uint8_t unit_id,const uint16_t coil_addr,const bool coil_state)421 int modbus_write_coil(const int iface,
422 const uint8_t unit_id,
423 const uint16_t coil_addr,
424 const bool coil_state)
425 {
426 struct modbus_context *ctx = modbus_get_context(iface);
427 int err;
428 uint16_t coil_val;
429
430 if (ctx == NULL) {
431 return -ENODEV;
432 }
433
434 k_mutex_lock(&ctx->iface_lock, K_FOREVER);
435
436 if (coil_state == false) {
437 coil_val = MODBUS_COIL_OFF_CODE;
438 } else {
439 coil_val = MODBUS_COIL_ON_CODE;
440 }
441
442 ctx->tx_adu.length = 4;
443 sys_put_be16(coil_addr, &ctx->tx_adu.data[0]);
444 sys_put_be16(coil_val, &ctx->tx_adu.data[2]);
445
446 err = mbc_send_cmd(ctx, unit_id, MODBUS_FC05_COIL_WR, NULL);
447 k_mutex_unlock(&ctx->iface_lock);
448
449 return err;
450 }
451
modbus_write_holding_reg(const int iface,const uint8_t unit_id,const uint16_t start_addr,const uint16_t reg_val)452 int modbus_write_holding_reg(const int iface,
453 const uint8_t unit_id,
454 const uint16_t start_addr,
455 const uint16_t reg_val)
456 {
457 struct modbus_context *ctx = modbus_get_context(iface);
458 int err;
459
460 if (ctx == NULL) {
461 return -ENODEV;
462 }
463
464 k_mutex_lock(&ctx->iface_lock, K_FOREVER);
465
466 ctx->tx_adu.length = 4;
467 sys_put_be16(start_addr, &ctx->tx_adu.data[0]);
468 sys_put_be16(reg_val, &ctx->tx_adu.data[2]);
469
470 err = mbc_send_cmd(ctx, unit_id, MODBUS_FC06_HOLDING_REG_WR, NULL);
471 k_mutex_unlock(&ctx->iface_lock);
472
473 return err;
474 }
475
modbus_request_diagnostic(const int iface,const uint8_t unit_id,const uint16_t sfunc,const uint16_t data,uint16_t * const data_out)476 int modbus_request_diagnostic(const int iface,
477 const uint8_t unit_id,
478 const uint16_t sfunc,
479 const uint16_t data,
480 uint16_t *const data_out)
481 {
482 struct modbus_context *ctx = modbus_get_context(iface);
483 int err;
484
485 if (ctx == NULL) {
486 return -ENODEV;
487 }
488
489 k_mutex_lock(&ctx->iface_lock, K_FOREVER);
490
491 ctx->tx_adu.length = 4;
492 sys_put_be16(sfunc, &ctx->tx_adu.data[0]);
493 sys_put_be16(data, &ctx->tx_adu.data[2]);
494
495 err = mbc_send_cmd(ctx, unit_id, MODBUS_FC08_DIAGNOSTICS, data_out);
496 k_mutex_unlock(&ctx->iface_lock);
497
498 return err;
499 }
500
modbus_write_coils(const int iface,const uint8_t unit_id,const uint16_t start_addr,uint8_t * const coil_tbl,const uint16_t num_coils)501 int modbus_write_coils(const int iface,
502 const uint8_t unit_id,
503 const uint16_t start_addr,
504 uint8_t *const coil_tbl,
505 const uint16_t num_coils)
506 {
507 struct modbus_context *ctx = modbus_get_context(iface);
508 size_t length = 0;
509 uint8_t *data_ptr;
510 size_t num_bytes;
511 int err;
512
513 if (ctx == NULL) {
514 return -ENODEV;
515 }
516
517 k_mutex_lock(&ctx->iface_lock, K_FOREVER);
518
519 sys_put_be16(start_addr, &ctx->tx_adu.data[0]);
520 length += sizeof(start_addr);
521 sys_put_be16(num_coils, &ctx->tx_adu.data[2]);
522 length += sizeof(num_coils);
523
524 num_bytes = (uint8_t)(((num_coils - 1) / 8) + 1);
525 ctx->tx_adu.data[4] = num_bytes;
526 length += num_bytes + 1;
527
528 if (length > sizeof(ctx->tx_adu.data)) {
529 LOG_ERR("Length of data buffer is not sufficient");
530 k_mutex_unlock(&ctx->iface_lock);
531 return -ENOBUFS;
532 }
533
534 ctx->tx_adu.length = length;
535 data_ptr = &ctx->tx_adu.data[5];
536
537 memcpy(data_ptr, coil_tbl, num_bytes);
538
539 err = mbc_send_cmd(ctx, unit_id, MODBUS_FC15_COILS_WR, NULL);
540 k_mutex_unlock(&ctx->iface_lock);
541
542 return err;
543 }
544
modbus_write_holding_regs(const int iface,const uint8_t unit_id,const uint16_t start_addr,uint16_t * const reg_buf,const uint16_t num_regs)545 int modbus_write_holding_regs(const int iface,
546 const uint8_t unit_id,
547 const uint16_t start_addr,
548 uint16_t *const reg_buf,
549 const uint16_t num_regs)
550 {
551 struct modbus_context *ctx = modbus_get_context(iface);
552 size_t length = 0;
553 uint8_t *data_ptr;
554 size_t num_bytes;
555 int err;
556
557 if (ctx == NULL) {
558 return -ENODEV;
559 }
560
561 k_mutex_lock(&ctx->iface_lock, K_FOREVER);
562
563 sys_put_be16(start_addr, &ctx->tx_adu.data[0]);
564 length += sizeof(start_addr);
565 sys_put_be16(num_regs, &ctx->tx_adu.data[2]);
566 length += sizeof(num_regs);
567
568 num_bytes = num_regs * sizeof(uint16_t);
569 ctx->tx_adu.data[4] = num_bytes;
570 length += num_bytes + 1;
571
572 if (length > sizeof(ctx->tx_adu.data)) {
573 LOG_ERR("Length of data buffer is not sufficient");
574 k_mutex_unlock(&ctx->iface_lock);
575 return -ENOBUFS;
576 }
577
578 ctx->tx_adu.length = length;
579 data_ptr = &ctx->tx_adu.data[5];
580
581 for (uint16_t i = 0; i < num_regs; i++) {
582 sys_put_be16(reg_buf[i], data_ptr);
583 data_ptr += sizeof(uint16_t);
584 }
585
586 err = mbc_send_cmd(ctx, unit_id, MODBUS_FC16_HOLDING_REGS_WR, NULL);
587 k_mutex_unlock(&ctx->iface_lock);
588
589 return err;
590 }
591
592 #ifdef CONFIG_MODBUS_FP_EXTENSIONS
modbus_write_holding_regs_fp(const int iface,const uint8_t unit_id,const uint16_t start_addr,float * const reg_buf,const uint16_t num_regs)593 int modbus_write_holding_regs_fp(const int iface,
594 const uint8_t unit_id,
595 const uint16_t start_addr,
596 float *const reg_buf,
597 const uint16_t num_regs)
598 {
599 struct modbus_context *ctx = modbus_get_context(iface);
600 size_t length = 0;
601 uint8_t *data_ptr;
602 size_t num_bytes;
603 int err;
604
605 if (ctx == NULL) {
606 return -ENODEV;
607 }
608
609 k_mutex_lock(&ctx->iface_lock, K_FOREVER);
610
611 sys_put_be16(start_addr, &ctx->tx_adu.data[0]);
612 length += sizeof(start_addr);
613 sys_put_be16(num_regs, &ctx->tx_adu.data[2]);
614 length += sizeof(num_regs);
615
616 num_bytes = num_regs * sizeof(float);
617 ctx->tx_adu.data[4] = num_bytes;
618 length += num_bytes + 1;
619
620 if (length > sizeof(ctx->tx_adu.data)) {
621 LOG_ERR("Length of data buffer is not sufficient");
622 k_mutex_unlock(&ctx->iface_lock);
623 return -ENOBUFS;
624 }
625
626 ctx->tx_adu.length = length;
627 data_ptr = &ctx->tx_adu.data[5];
628
629 for (uint16_t i = 0; i < num_regs; i++) {
630 uint32_t reg_val;
631
632 memcpy(®_val, ®_buf[i], sizeof(reg_val));
633 sys_put_be32(reg_val, data_ptr);
634 data_ptr += sizeof(uint32_t);
635 }
636
637 err = mbc_send_cmd(ctx, unit_id, MODBUS_FC16_HOLDING_REGS_WR, NULL);
638 k_mutex_unlock(&ctx->iface_lock);
639
640 return err;
641 }
642 #endif
643
