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