1 /*
2 * Copyright (c) 2020 PHYTEC Messtechnik GmbH
3 * Copyright (c) 2021 Nordic Semiconductor ASA
4 *
5 * SPDX-License-Identifier: Apache-2.0
6 */
7
8 /*
9 * This file is based on mb.c and mb_util.c from uC/Modbus Stack.
10 *
11 * uC/Modbus
12 * The Embedded Modbus Stack
13 *
14 * Copyright 2003-2020 Silicon Laboratories Inc. www.silabs.com
15 *
16 * SPDX-License-Identifier: Apache-2.0
17 *
18 * This software is subject to an open source license and is distributed by
19 * Silicon Laboratories Inc. pursuant to the terms of the Apache License,
20 * Version 2.0 available at www.apache.org/licenses/LICENSE-2.0.
21 */
22
23 #include <zephyr/logging/log.h>
24 LOG_MODULE_REGISTER(modbus_serial, CONFIG_MODBUS_LOG_LEVEL);
25
26 #include <zephyr/kernel.h>
27 #include <string.h>
28 #include <zephyr/sys/byteorder.h>
29 #include <zephyr/sys/crc.h>
30 #include <modbus_internal.h>
31
modbus_serial_tx_on(struct modbus_context * ctx)32 static void modbus_serial_tx_on(struct modbus_context *ctx)
33 {
34 struct modbus_serial_config *cfg = ctx->cfg;
35
36 if (cfg->de != NULL) {
37 gpio_pin_set(cfg->de->port, cfg->de->pin, 1);
38 }
39
40 uart_irq_tx_enable(cfg->dev);
41 }
42
modbus_serial_tx_off(struct modbus_context * ctx)43 static void modbus_serial_tx_off(struct modbus_context *ctx)
44 {
45 struct modbus_serial_config *cfg = ctx->cfg;
46
47 uart_irq_tx_disable(cfg->dev);
48 if (cfg->de != NULL) {
49 gpio_pin_set(cfg->de->port, cfg->de->pin, 0);
50 }
51 }
52
modbus_serial_rx_fifo_drain(struct modbus_context * ctx)53 static void modbus_serial_rx_fifo_drain(struct modbus_context *ctx)
54 {
55 struct modbus_serial_config *cfg = ctx->cfg;
56 uint8_t buf[8];
57 int n;
58
59 do {
60 n = uart_fifo_read(cfg->dev, buf, sizeof(buf));
61 } while (n == sizeof(buf));
62 }
63
modbus_serial_rx_on(struct modbus_context * ctx)64 static void modbus_serial_rx_on(struct modbus_context *ctx)
65 {
66 struct modbus_serial_config *cfg = ctx->cfg;
67
68 if (cfg->re != NULL) {
69 gpio_pin_set(cfg->re->port, cfg->re->pin, 1);
70 }
71
72 atomic_set_bit(&ctx->state, MODBUS_STATE_RX_ENABLED);
73 uart_irq_rx_enable(cfg->dev);
74 }
75
modbus_serial_rx_off(struct modbus_context * ctx)76 static void modbus_serial_rx_off(struct modbus_context *ctx)
77 {
78 struct modbus_serial_config *cfg = ctx->cfg;
79
80 uart_irq_rx_disable(cfg->dev);
81 atomic_clear_bit(&ctx->state, MODBUS_STATE_RX_ENABLED);
82
83 if (cfg->re != NULL) {
84 gpio_pin_set(cfg->re->port, cfg->re->pin, 0);
85 }
86 }
87
88 #ifdef CONFIG_MODBUS_ASCII_MODE
89 /* The function calculates an 8-bit Longitudinal Redundancy Check. */
modbus_ascii_get_lrc(uint8_t * src,size_t length)90 static uint8_t modbus_ascii_get_lrc(uint8_t *src, size_t length)
91 {
92 uint8_t lrc = 0;
93 uint8_t tmp;
94 uint8_t *pblock = src;
95
96 while (length-- > 0) {
97 /* Add the data byte to LRC, increment data pointer. */
98 if (hex2bin(pblock, 2, &tmp, sizeof(tmp)) != sizeof(tmp)) {
99 return 0;
100 }
101
102 lrc += tmp;
103 pblock += 2;
104 }
105
106 /* Two complement the binary sum */
107 lrc = ~lrc + 1;
108
109 return lrc;
110 }
111
112 /* Parses and converts an ASCII mode frame into a Modbus RTU frame. */
modbus_ascii_rx_adu(struct modbus_context * ctx)113 static int modbus_ascii_rx_adu(struct modbus_context *ctx)
114 {
115 struct modbus_serial_config *cfg = ctx->cfg;
116 uint8_t *pmsg;
117 uint8_t *prx_data;
118 uint16_t rx_size;
119 uint8_t frame_lrc;
120 uint8_t calc_lrc;
121
122 rx_size = cfg->uart_buf_ctr;
123 prx_data = &ctx->rx_adu.data[0];
124
125 if (!(rx_size & 0x01)) {
126 LOG_WRN("Message should have an odd number of bytes");
127 return -EMSGSIZE;
128 }
129
130 if (rx_size < MODBUS_ASCII_MIN_MSG_SIZE) {
131 LOG_WRN("Frame length error");
132 return -EMSGSIZE;
133 }
134
135 if ((cfg->uart_buf[0] != MODBUS_ASCII_START_FRAME_CHAR) ||
136 (cfg->uart_buf[rx_size - 2] != MODBUS_ASCII_END_FRAME_CHAR1) ||
137 (cfg->uart_buf[rx_size - 1] != MODBUS_ASCII_END_FRAME_CHAR2)) {
138 LOG_WRN("Frame character error");
139 return -EMSGSIZE;
140 }
141
142 /* Take away for the ':', CR, and LF */
143 rx_size -= 3;
144 /* Point past the ':' to the address. */
145 pmsg = &cfg->uart_buf[1];
146
147 hex2bin(pmsg, 2, &ctx->rx_adu.unit_id, 1);
148 pmsg += 2;
149 rx_size -= 2;
150 hex2bin(pmsg, 2, &ctx->rx_adu.fc, 1);
151 pmsg += 2;
152 rx_size -= 2;
153
154 /* Get the data from the message */
155 ctx->rx_adu.length = 0;
156 while (rx_size > 2) {
157 hex2bin(pmsg, 2, prx_data, 1);
158 prx_data++;
159 pmsg += 2;
160 rx_size -= 2;
161 /* Increment the number of Modbus packets received */
162 ctx->rx_adu.length++;
163 }
164
165 /* Extract the message's LRC */
166 hex2bin(pmsg, 2, &frame_lrc, 1);
167 ctx->rx_adu.crc = frame_lrc;
168
169 /*
170 * The LRC is calculated on the ADDR, FC and Data fields,
171 * not the ':', CR/LF and LRC placed in the message
172 * by the sender. We thus need to subtract 5 'ASCII' characters
173 * from the received message to exclude these.
174 */
175 calc_lrc = modbus_ascii_get_lrc(&cfg->uart_buf[1],
176 (cfg->uart_buf_ctr - 5) / 2);
177
178 if (calc_lrc != frame_lrc) {
179 LOG_ERR("Calculated LRC does not match received LRC");
180 return -EIO;
181 }
182
183 return 0;
184 }
185
modbus_ascii_bin2hex(uint8_t value,uint8_t * pbuf)186 static uint8_t *modbus_ascii_bin2hex(uint8_t value, uint8_t *pbuf)
187 {
188 uint8_t u_nibble = (value >> 4) & 0x0F;
189 uint8_t l_nibble = value & 0x0F;
190
191 hex2char(u_nibble, pbuf);
192 pbuf++;
193 hex2char(l_nibble, pbuf);
194 pbuf++;
195
196 return pbuf;
197 }
198
modbus_ascii_tx_adu(struct modbus_context * ctx)199 static void modbus_ascii_tx_adu(struct modbus_context *ctx)
200 {
201 struct modbus_serial_config *cfg = ctx->cfg;
202 uint16_t tx_bytes = 0;
203 uint8_t lrc;
204 uint8_t *pbuf;
205
206 /* Place the start-of-frame character into output buffer */
207 cfg->uart_buf[0] = MODBUS_ASCII_START_FRAME_CHAR;
208 tx_bytes = 1;
209
210 pbuf = &cfg->uart_buf[1];
211 pbuf = modbus_ascii_bin2hex(ctx->tx_adu.unit_id, pbuf);
212 tx_bytes += 2;
213 pbuf = modbus_ascii_bin2hex(ctx->tx_adu.fc, pbuf);
214 tx_bytes += 2;
215
216 for (int i = 0; i < ctx->tx_adu.length; i++) {
217 pbuf = modbus_ascii_bin2hex(ctx->tx_adu.data[i], pbuf);
218 tx_bytes += 2;
219 }
220
221 /*
222 * Add the LRC checksum in the packet.
223 *
224 * The LRC is calculated on the ADDR, FC and Data fields,
225 * not the ':' which was inserted in the uart_buf[].
226 * Thus we subtract 1 ASCII character from the LRC.
227 * The LRC and CR/LF bytes are not YET in the .uart_buf[].
228 */
229 lrc = modbus_ascii_get_lrc(&cfg->uart_buf[1], (tx_bytes - 1) / 2);
230 pbuf = modbus_ascii_bin2hex(lrc, pbuf);
231 tx_bytes += 2;
232
233 *pbuf++ = MODBUS_ASCII_END_FRAME_CHAR1;
234 *pbuf++ = MODBUS_ASCII_END_FRAME_CHAR2;
235 tx_bytes += 2;
236
237 /* Update the total number of bytes to send */
238 cfg->uart_buf_ctr = tx_bytes;
239 cfg->uart_buf_ptr = &cfg->uart_buf[0];
240
241 LOG_DBG("Start frame transmission");
242 modbus_serial_rx_off(ctx);
243 modbus_serial_tx_on(ctx);
244 }
245 #else
modbus_ascii_rx_adu(struct modbus_context * ctx)246 static int modbus_ascii_rx_adu(struct modbus_context *ctx)
247 {
248 return 0;
249 }
250
modbus_ascii_tx_adu(struct modbus_context * ctx)251 static void modbus_ascii_tx_adu(struct modbus_context *ctx)
252 {
253 }
254 #endif
255
256 /* Copy Modbus RTU frame and check if the CRC is valid. */
modbus_rtu_rx_adu(struct modbus_context * ctx)257 static int modbus_rtu_rx_adu(struct modbus_context *ctx)
258 {
259 struct modbus_serial_config *cfg = ctx->cfg;
260 uint16_t calc_crc;
261 uint16_t crc_idx;
262 uint8_t *data_ptr;
263
264 /* Is the message long enough? */
265 if ((cfg->uart_buf_ctr < MODBUS_RTU_MIN_MSG_SIZE) ||
266 (cfg->uart_buf_ctr > CONFIG_MODBUS_BUFFER_SIZE)) {
267 LOG_WRN("Frame length error");
268 return -EMSGSIZE;
269 }
270
271 ctx->rx_adu.unit_id = cfg->uart_buf[0];
272 ctx->rx_adu.fc = cfg->uart_buf[1];
273 data_ptr = &cfg->uart_buf[2];
274 /* Payload length without node address, function code, and CRC */
275 ctx->rx_adu.length = cfg->uart_buf_ctr - 4;
276 /* CRC index */
277 crc_idx = cfg->uart_buf_ctr - sizeof(uint16_t);
278
279 memcpy(ctx->rx_adu.data, data_ptr, ctx->rx_adu.length);
280
281 ctx->rx_adu.crc = sys_get_le16(&cfg->uart_buf[crc_idx]);
282 /* Calculate CRC over address, function code, and payload */
283 calc_crc = crc16_ansi(&cfg->uart_buf[0],
284 cfg->uart_buf_ctr - sizeof(ctx->rx_adu.crc));
285
286 if (ctx->rx_adu.crc != calc_crc) {
287 LOG_WRN("Calculated CRC does not match received CRC");
288 return -EIO;
289 }
290
291 return 0;
292 }
293
rtu_tx_adu(struct modbus_context * ctx)294 static void rtu_tx_adu(struct modbus_context *ctx)
295 {
296 struct modbus_serial_config *cfg = ctx->cfg;
297 uint16_t tx_bytes = 0;
298 uint8_t *data_ptr;
299
300 cfg->uart_buf[0] = ctx->tx_adu.unit_id;
301 cfg->uart_buf[1] = ctx->tx_adu.fc;
302 tx_bytes = 2 + ctx->tx_adu.length;
303 data_ptr = &cfg->uart_buf[2];
304
305 memcpy(data_ptr, ctx->tx_adu.data, ctx->tx_adu.length);
306
307 ctx->tx_adu.crc = crc16_ansi(&cfg->uart_buf[0], ctx->tx_adu.length + 2);
308 sys_put_le16(ctx->tx_adu.crc,
309 &cfg->uart_buf[ctx->tx_adu.length + 2]);
310 tx_bytes += 2;
311
312 cfg->uart_buf_ctr = tx_bytes;
313 cfg->uart_buf_ptr = &cfg->uart_buf[0];
314
315 LOG_HEXDUMP_DBG(cfg->uart_buf, cfg->uart_buf_ctr, "uart_buf");
316 LOG_DBG("Start frame transmission");
317 modbus_serial_rx_off(ctx);
318 modbus_serial_tx_on(ctx);
319 }
320
321 /*
322 * A byte has been received from a serial port. We just store it in the buffer
323 * for processing when a complete packet has been received.
324 */
cb_handler_rx(struct modbus_context * ctx)325 static void cb_handler_rx(struct modbus_context *ctx)
326 {
327 struct modbus_serial_config *cfg = ctx->cfg;
328
329 if (!atomic_test_bit(&ctx->state, MODBUS_STATE_RX_ENABLED)) {
330 modbus_serial_rx_fifo_drain(ctx);
331 return;
332 }
333
334 if ((ctx->mode == MODBUS_MODE_ASCII) &&
335 IS_ENABLED(CONFIG_MODBUS_ASCII_MODE)) {
336 uint8_t c;
337
338 if (uart_fifo_read(cfg->dev, &c, 1) != 1) {
339 LOG_ERR("Failed to read UART");
340 return;
341 }
342
343 if (c == MODBUS_ASCII_START_FRAME_CHAR) {
344 /* Restart a new frame */
345 cfg->uart_buf_ptr = &cfg->uart_buf[0];
346 cfg->uart_buf_ctr = 0;
347 }
348
349 if (cfg->uart_buf_ctr < CONFIG_MODBUS_BUFFER_SIZE) {
350 *cfg->uart_buf_ptr++ = c;
351 cfg->uart_buf_ctr++;
352 }
353
354 if (c == MODBUS_ASCII_END_FRAME_CHAR2) {
355 k_work_submit(&ctx->server_work);
356 }
357
358 } else {
359 int n;
360
361 if (cfg->uart_buf_ctr == CONFIG_MODBUS_BUFFER_SIZE) {
362 /* Buffer full. Disable interrupt until timeout. */
363 modbus_serial_rx_disable(ctx);
364 return;
365 }
366
367 /* Restart timer on a new character */
368 k_timer_start(&cfg->rtu_timer,
369 K_USEC(cfg->rtu_timeout), K_NO_WAIT);
370
371 n = uart_fifo_read(cfg->dev, cfg->uart_buf_ptr,
372 (CONFIG_MODBUS_BUFFER_SIZE -
373 cfg->uart_buf_ctr));
374
375 cfg->uart_buf_ptr += n;
376 cfg->uart_buf_ctr += n;
377 }
378 }
379
cb_handler_tx(struct modbus_context * ctx)380 static void cb_handler_tx(struct modbus_context *ctx)
381 {
382 struct modbus_serial_config *cfg = ctx->cfg;
383 int n;
384
385 if (cfg->uart_buf_ctr > 0) {
386 n = uart_fifo_fill(cfg->dev, cfg->uart_buf_ptr,
387 cfg->uart_buf_ctr);
388 cfg->uart_buf_ctr -= n;
389 cfg->uart_buf_ptr += n;
390 return;
391 }
392
393 /* Must wait till the transmission is complete or
394 * RS-485 transceiver could be disabled before all data has
395 * been transmitted and message will be corrupted.
396 */
397 if (uart_irq_tx_complete(cfg->dev)) {
398 /* Disable transmission */
399 cfg->uart_buf_ptr = &cfg->uart_buf[0];
400 modbus_serial_tx_off(ctx);
401 modbus_serial_rx_fifo_drain(ctx);
402 modbus_serial_rx_on(ctx);
403 }
404 }
405
uart_cb_handler(const struct device * dev,void * app_data)406 static void uart_cb_handler(const struct device *dev, void *app_data)
407 {
408 struct modbus_context *ctx = (struct modbus_context *)app_data;
409
410 if (ctx == NULL) {
411 LOG_ERR("Modbus hardware is not properly initialized");
412 return;
413 }
414
415 if (uart_irq_update(dev) && uart_irq_is_pending(dev)) {
416
417 if (uart_irq_rx_ready(dev)) {
418 cb_handler_rx(ctx);
419 }
420
421 if (uart_irq_tx_ready(dev)) {
422 cb_handler_tx(ctx);
423 }
424 }
425 }
426
427 /* This function is called when the RTU framing timer expires. */
rtu_tmr_handler(struct k_timer * t_id)428 static void rtu_tmr_handler(struct k_timer *t_id)
429 {
430 struct modbus_context *ctx;
431
432 ctx = (struct modbus_context *)k_timer_user_data_get(t_id);
433
434 if (ctx == NULL) {
435 LOG_ERR("Failed to get Modbus context");
436 return;
437 }
438
439 k_work_submit(&ctx->server_work);
440 }
441
configure_gpio(struct modbus_context * ctx)442 static int configure_gpio(struct modbus_context *ctx)
443 {
444 struct modbus_serial_config *cfg = ctx->cfg;
445
446 if (cfg->de != NULL) {
447 if (!device_is_ready(cfg->de->port)) {
448 return -ENODEV;
449 }
450
451 if (gpio_pin_configure_dt(cfg->de, GPIO_OUTPUT_INACTIVE)) {
452 return -EIO;
453 }
454 }
455
456
457 if (cfg->re != NULL) {
458 if (!device_is_ready(cfg->re->port)) {
459 return -ENODEV;
460 }
461
462 if (gpio_pin_configure_dt(cfg->re, GPIO_OUTPUT_INACTIVE)) {
463 return -EIO;
464 }
465 }
466
467 return 0;
468 }
469
configure_uart(struct modbus_context * ctx,struct modbus_iface_param * param)470 static inline int configure_uart(struct modbus_context *ctx,
471 struct modbus_iface_param *param)
472 {
473 struct modbus_serial_config *cfg = ctx->cfg;
474 struct uart_config uart_cfg = {
475 .baudrate = param->serial.baud,
476 .flow_ctrl = UART_CFG_FLOW_CTRL_NONE,
477 };
478
479 if (ctx->mode == MODBUS_MODE_ASCII) {
480 uart_cfg.data_bits = UART_CFG_DATA_BITS_7;
481 } else {
482 uart_cfg.data_bits = UART_CFG_DATA_BITS_8;
483 }
484
485 switch (param->serial.parity) {
486 case UART_CFG_PARITY_ODD:
487 case UART_CFG_PARITY_EVEN:
488 uart_cfg.parity = param->serial.parity;
489 uart_cfg.stop_bits = UART_CFG_STOP_BITS_1;
490 break;
491 case UART_CFG_PARITY_NONE:
492 /* Use of no parity requires 2 stop bits */
493 uart_cfg.parity = param->serial.parity;
494 uart_cfg.stop_bits = UART_CFG_STOP_BITS_2;
495 break;
496 default:
497 return -EINVAL;
498 }
499
500 if (IS_ENABLED(CONFIG_MODBUS_NONCOMPLIANT_SERIAL_MODE)) {
501 /* Allow custom stop bit settings only in non-compliant mode */
502 switch (param->serial.stop_bits) {
503 case UART_CFG_STOP_BITS_0_5:
504 case UART_CFG_STOP_BITS_1:
505 case UART_CFG_STOP_BITS_1_5:
506 case UART_CFG_STOP_BITS_2:
507 uart_cfg.stop_bits = param->serial.stop_bits;
508 break;
509 default:
510 return -EINVAL;
511 }
512 }
513
514 if (uart_configure(cfg->dev, &uart_cfg) != 0) {
515 LOG_ERR("Failed to configure UART");
516 return -EINVAL;
517 }
518
519 return 0;
520 }
521
modbus_serial_rx_disable(struct modbus_context * ctx)522 void modbus_serial_rx_disable(struct modbus_context *ctx)
523 {
524 modbus_serial_rx_off(ctx);
525 }
526
modbus_serial_rx_enable(struct modbus_context * ctx)527 void modbus_serial_rx_enable(struct modbus_context *ctx)
528 {
529 modbus_serial_rx_on(ctx);
530 }
531
modbus_serial_rx_adu(struct modbus_context * ctx)532 int modbus_serial_rx_adu(struct modbus_context *ctx)
533 {
534 struct modbus_serial_config *cfg = ctx->cfg;
535 int rc = 0;
536
537 switch (ctx->mode) {
538 case MODBUS_MODE_RTU:
539 rc = modbus_rtu_rx_adu(ctx);
540 break;
541 case MODBUS_MODE_ASCII:
542 if (!IS_ENABLED(CONFIG_MODBUS_ASCII_MODE)) {
543 return -ENOTSUP;
544 }
545
546 rc = modbus_ascii_rx_adu(ctx);
547 break;
548 default:
549 LOG_ERR("Unsupported MODBUS mode");
550 return -ENOTSUP;
551 }
552
553 cfg->uart_buf_ctr = 0;
554 cfg->uart_buf_ptr = &cfg->uart_buf[0];
555
556 return rc;
557 }
558
modbus_serial_tx_adu(struct modbus_context * ctx)559 int modbus_serial_tx_adu(struct modbus_context *ctx)
560 {
561 switch (ctx->mode) {
562 case MODBUS_MODE_RTU:
563 rtu_tx_adu(ctx);
564 return 0;
565 case MODBUS_MODE_ASCII:
566 if (IS_ENABLED(CONFIG_MODBUS_ASCII_MODE)) {
567 modbus_ascii_tx_adu(ctx);
568 return 0;
569 }
570 default:
571 break;
572 }
573
574 return -ENOTSUP;
575 }
576
modbus_serial_init(struct modbus_context * ctx,struct modbus_iface_param param)577 int modbus_serial_init(struct modbus_context *ctx,
578 struct modbus_iface_param param)
579 {
580 struct modbus_serial_config *cfg = ctx->cfg;
581 const uint32_t if_delay_max = 3500000;
582 const uint32_t numof_bits = 11;
583 int err;
584
585 switch (param.mode) {
586 case MODBUS_MODE_RTU:
587 case MODBUS_MODE_ASCII:
588 ctx->mode = param.mode;
589 break;
590 default:
591 return -ENOTSUP;
592 }
593
594 if (!device_is_ready(cfg->dev)) {
595 LOG_ERR("Bus device %s is not ready", cfg->dev->name);
596 return -ENODEV;
597 }
598
599 if (IS_ENABLED(CONFIG_UART_USE_RUNTIME_CONFIGURE)) {
600 if (configure_uart(ctx, ¶m) != 0) {
601 return -EINVAL;
602 }
603 }
604
605 if (param.serial.baud == 0) {
606 LOG_ERR("Baudrate is 0");
607 return -EINVAL;
608 }
609
610 if (param.serial.baud <= 38400) {
611 cfg->rtu_timeout = (numof_bits * if_delay_max) /
612 param.serial.baud;
613 } else {
614 cfg->rtu_timeout = (numof_bits * if_delay_max) / 38400;
615 }
616
617 if (configure_gpio(ctx) != 0) {
618 return -EIO;
619 }
620
621 cfg->uart_buf_ctr = 0;
622 cfg->uart_buf_ptr = &cfg->uart_buf[0];
623
624 err = uart_irq_callback_user_data_set(cfg->dev, uart_cb_handler, ctx);
625 if (err != 0) {
626 return err;
627 };
628
629 k_timer_init(&cfg->rtu_timer, rtu_tmr_handler, NULL);
630 k_timer_user_data_set(&cfg->rtu_timer, ctx);
631
632 modbus_serial_rx_on(ctx);
633 LOG_INF("RTU timeout %u us", cfg->rtu_timeout);
634
635 return 0;
636 }
637
modbus_serial_disable(struct modbus_context * ctx)638 void modbus_serial_disable(struct modbus_context *ctx)
639 {
640 modbus_serial_tx_off(ctx);
641 modbus_serial_rx_off(ctx);
642 k_timer_stop(&ctx->cfg->rtu_timer);
643 }
644