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(uint16_t);
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 / 2; 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 /* A 32-bit float is mapped to two 16-bit registers */
388 sys_put_be16(num_regs * 2, &ctx->tx_adu.data[2]);
389
390 err = mbc_send_cmd(ctx, unit_id, MODBUS_FC03_HOLDING_REG_RD, reg_buf);
391 k_mutex_unlock(&ctx->iface_lock);
392
393 return err;
394 }
395 #endif
396
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)397 int modbus_read_input_regs(const int iface,
398 const uint8_t unit_id,
399 const uint16_t start_addr,
400 uint16_t *const reg_buf,
401 const uint16_t num_regs)
402 {
403 struct modbus_context *ctx = modbus_get_context(iface);
404 int err;
405
406 if (ctx == NULL) {
407 return -ENODEV;
408 }
409
410 k_mutex_lock(&ctx->iface_lock, K_FOREVER);
411
412 ctx->tx_adu.length = 4;
413 sys_put_be16(start_addr, &ctx->tx_adu.data[0]);
414 sys_put_be16(num_regs, &ctx->tx_adu.data[2]);
415
416 err = mbc_send_cmd(ctx, unit_id, MODBUS_FC04_IN_REG_RD, reg_buf);
417 k_mutex_unlock(&ctx->iface_lock);
418
419 return err;
420 }
421
modbus_write_coil(const int iface,const uint8_t unit_id,const uint16_t coil_addr,const bool coil_state)422 int modbus_write_coil(const int iface,
423 const uint8_t unit_id,
424 const uint16_t coil_addr,
425 const bool coil_state)
426 {
427 struct modbus_context *ctx = modbus_get_context(iface);
428 int err;
429 uint16_t coil_val;
430
431 if (ctx == NULL) {
432 return -ENODEV;
433 }
434
435 k_mutex_lock(&ctx->iface_lock, K_FOREVER);
436
437 if (coil_state == false) {
438 coil_val = MODBUS_COIL_OFF_CODE;
439 } else {
440 coil_val = MODBUS_COIL_ON_CODE;
441 }
442
443 ctx->tx_adu.length = 4;
444 sys_put_be16(coil_addr, &ctx->tx_adu.data[0]);
445 sys_put_be16(coil_val, &ctx->tx_adu.data[2]);
446
447 err = mbc_send_cmd(ctx, unit_id, MODBUS_FC05_COIL_WR, NULL);
448 k_mutex_unlock(&ctx->iface_lock);
449
450 return err;
451 }
452
modbus_write_holding_reg(const int iface,const uint8_t unit_id,const uint16_t start_addr,const uint16_t reg_val)453 int modbus_write_holding_reg(const int iface,
454 const uint8_t unit_id,
455 const uint16_t start_addr,
456 const uint16_t reg_val)
457 {
458 struct modbus_context *ctx = modbus_get_context(iface);
459 int err;
460
461 if (ctx == NULL) {
462 return -ENODEV;
463 }
464
465 k_mutex_lock(&ctx->iface_lock, K_FOREVER);
466
467 ctx->tx_adu.length = 4;
468 sys_put_be16(start_addr, &ctx->tx_adu.data[0]);
469 sys_put_be16(reg_val, &ctx->tx_adu.data[2]);
470
471 err = mbc_send_cmd(ctx, unit_id, MODBUS_FC06_HOLDING_REG_WR, NULL);
472 k_mutex_unlock(&ctx->iface_lock);
473
474 return err;
475 }
476
modbus_request_diagnostic(const int iface,const uint8_t unit_id,const uint16_t sfunc,const uint16_t data,uint16_t * const data_out)477 int modbus_request_diagnostic(const int iface,
478 const uint8_t unit_id,
479 const uint16_t sfunc,
480 const uint16_t data,
481 uint16_t *const data_out)
482 {
483 struct modbus_context *ctx = modbus_get_context(iface);
484 int err;
485
486 if (ctx == NULL) {
487 return -ENODEV;
488 }
489
490 k_mutex_lock(&ctx->iface_lock, K_FOREVER);
491
492 ctx->tx_adu.length = 4;
493 sys_put_be16(sfunc, &ctx->tx_adu.data[0]);
494 sys_put_be16(data, &ctx->tx_adu.data[2]);
495
496 err = mbc_send_cmd(ctx, unit_id, MODBUS_FC08_DIAGNOSTICS, data_out);
497 k_mutex_unlock(&ctx->iface_lock);
498
499 return err;
500 }
501
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)502 int modbus_write_coils(const int iface,
503 const uint8_t unit_id,
504 const uint16_t start_addr,
505 uint8_t *const coil_tbl,
506 const uint16_t num_coils)
507 {
508 struct modbus_context *ctx = modbus_get_context(iface);
509 size_t length = 0;
510 uint8_t *data_ptr;
511 size_t num_bytes;
512 int err;
513
514 if (ctx == NULL) {
515 return -ENODEV;
516 }
517
518 k_mutex_lock(&ctx->iface_lock, K_FOREVER);
519
520 sys_put_be16(start_addr, &ctx->tx_adu.data[0]);
521 length += sizeof(start_addr);
522 sys_put_be16(num_coils, &ctx->tx_adu.data[2]);
523 length += sizeof(num_coils);
524
525 num_bytes = (uint8_t)(((num_coils - 1) / 8) + 1);
526 ctx->tx_adu.data[4] = num_bytes;
527 length += num_bytes + 1;
528
529 if (length > sizeof(ctx->tx_adu.data)) {
530 LOG_ERR("Length of data buffer is not sufficient");
531 k_mutex_unlock(&ctx->iface_lock);
532 return -ENOBUFS;
533 }
534
535 ctx->tx_adu.length = length;
536 data_ptr = &ctx->tx_adu.data[5];
537
538 memcpy(data_ptr, coil_tbl, num_bytes);
539
540 err = mbc_send_cmd(ctx, unit_id, MODBUS_FC15_COILS_WR, NULL);
541 k_mutex_unlock(&ctx->iface_lock);
542
543 return err;
544 }
545
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)546 int modbus_write_holding_regs(const int iface,
547 const uint8_t unit_id,
548 const uint16_t start_addr,
549 uint16_t *const reg_buf,
550 const uint16_t num_regs)
551 {
552 struct modbus_context *ctx = modbus_get_context(iface);
553 size_t length = 0;
554 uint8_t *data_ptr;
555 size_t num_bytes;
556 int err;
557
558 if (ctx == NULL) {
559 return -ENODEV;
560 }
561
562 k_mutex_lock(&ctx->iface_lock, K_FOREVER);
563
564 sys_put_be16(start_addr, &ctx->tx_adu.data[0]);
565 length += sizeof(start_addr);
566 sys_put_be16(num_regs, &ctx->tx_adu.data[2]);
567 length += sizeof(num_regs);
568
569 num_bytes = num_regs * sizeof(uint16_t);
570 ctx->tx_adu.data[4] = num_bytes;
571 length += num_bytes + 1;
572
573 if (length > sizeof(ctx->tx_adu.data)) {
574 LOG_ERR("Length of data buffer is not sufficient");
575 k_mutex_unlock(&ctx->iface_lock);
576 return -ENOBUFS;
577 }
578
579 ctx->tx_adu.length = length;
580 data_ptr = &ctx->tx_adu.data[5];
581
582 for (uint16_t i = 0; i < num_regs; i++) {
583 sys_put_be16(reg_buf[i], data_ptr);
584 data_ptr += sizeof(uint16_t);
585 }
586
587 err = mbc_send_cmd(ctx, unit_id, MODBUS_FC16_HOLDING_REGS_WR, NULL);
588 k_mutex_unlock(&ctx->iface_lock);
589
590 return err;
591 }
592
593 #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)594 int modbus_write_holding_regs_fp(const int iface,
595 const uint8_t unit_id,
596 const uint16_t start_addr,
597 float *const reg_buf,
598 const uint16_t num_regs)
599 {
600 struct modbus_context *ctx = modbus_get_context(iface);
601 size_t length = 0;
602 uint8_t *data_ptr;
603 size_t num_bytes;
604 int err;
605
606 if (ctx == NULL) {
607 return -ENODEV;
608 }
609
610 k_mutex_lock(&ctx->iface_lock, K_FOREVER);
611
612 sys_put_be16(start_addr, &ctx->tx_adu.data[0]);
613 length += sizeof(start_addr);
614 /* A 32-bit float is mapped to two 16-bit registers */
615 sys_put_be16(num_regs * 2, &ctx->tx_adu.data[2]);
616 length += sizeof(num_regs);
617
618 num_bytes = num_regs * sizeof(float);
619 ctx->tx_adu.data[4] = num_bytes;
620 length += num_bytes + 1;
621
622 if (length > sizeof(ctx->tx_adu.data)) {
623 LOG_ERR("Length of data buffer is not sufficient");
624 k_mutex_unlock(&ctx->iface_lock);
625 return -ENOBUFS;
626 }
627
628 ctx->tx_adu.length = length;
629 data_ptr = &ctx->tx_adu.data[5];
630
631 for (uint16_t i = 0; i < num_regs; i++) {
632 uint32_t reg_val;
633
634 memcpy(®_val, ®_buf[i], sizeof(reg_val));
635 sys_put_be32(reg_val, data_ptr);
636 data_ptr += sizeof(uint32_t);
637 }
638
639 err = mbc_send_cmd(ctx, unit_id, MODBUS_FC16_HOLDING_REGS_WR, NULL);
640 k_mutex_unlock(&ctx->iface_lock);
641
642 return err;
643 }
644 #endif
645
