?  search:
summary | log | graphiclog1 | graphiclog2 | commit | commitdiff | tree | refs | edit | fork
blame | history | raw | HEAD
doc
[libmodbus.git] / src / modbus.c
blob046ca46f4866a44f1db6adff7d20e82e66bcdc38
1 /*
2 * Copyright © Stéphane Raimbault <stephane.raimbault@gmail.com>
3 *
4 * SPDX-License-Identifier: LGPL-2.1-or-later
5 *
6 * This library implements the Modbus protocol.
7 * http://libmodbus.org/
8 */
9
10 #include <errno.h>
11 #include <limits.h>
12 #include <stdarg.h>
13 #include <stdio.h>
14 #include <stdlib.h>
15 #include <string.h>
16 #include <time.h>
17 #ifndef _MSC_VER
18 #include <unistd.h>
19 #endif
20
21 #include <config.h>
22
23 #include "modbus-private.h"
24 #include "modbus.h"
25
26 /* Internal use */
27 #define MSG_LENGTH_UNDEFINED -1
28
29 /* Exported version */
30 const unsigned int libmodbus_version_major = LIBMODBUS_VERSION_MAJOR;
31 const unsigned int libmodbus_version_minor = LIBMODBUS_VERSION_MINOR;
32 const unsigned int libmodbus_version_micro = LIBMODBUS_VERSION_MICRO;
33
34 /* Max between RTU and TCP max adu length (so TCP) */
35 #define MAX_MESSAGE_LENGTH 260
36
37 /* 3 steps are used to parse the query */
38 typedef enum {
39 _STEP_FUNCTION,
40 _STEP_META,
41 _STEP_DATA
42 } _step_t;
43
44 const char *modbus_strerror(int errnum)
45 {
46 switch (errnum) {
47 case EMBXILFUN:
48 return "Illegal function";
49 case EMBXILADD:
50 return "Illegal data address";
51 case EMBXILVAL:
52 return "Illegal data value";
53 case EMBXSFAIL:
54 return "Slave device or server failure";
55 case EMBXACK:
56 return "Acknowledge";
57 case EMBXSBUSY:
58 return "Slave device or server is busy";
59 case EMBXNACK:
60 return "Negative acknowledge";
61 case EMBXMEMPAR:
62 return "Memory parity error";
63 case EMBXGPATH:
64 return "Gateway path unavailable";
65 case EMBXGTAR:
66 return "Target device failed to respond";
67 case EMBBADCRC:
68 return "Invalid CRC";
69 case EMBBADDATA:
70 return "Invalid data";
71 case EMBBADEXC:
72 return "Invalid exception code";
73 case EMBMDATA:
74 return "Too many data";
75 case EMBBADSLAVE:
76 return "Response not from requested slave";
77 default:
78 return strerror(errnum);
79 }
80 }
81
82 void _error_print(modbus_t *ctx, const char *context)
83 {
84 if (ctx->debug) {
85 fprintf(stderr, "ERROR %s", modbus_strerror(errno));
86 if (context != NULL) {
87 fprintf(stderr, ": %s\n", context);
88 } else {
89 fprintf(stderr, "\n");
90 }
91 }
92 }
93
94 static void _sleep_response_timeout(modbus_t *ctx)
95 {
96 /* Response timeout is always positive */
97 #ifdef _WIN32
98 /* usleep doesn't exist on Windows */
99 Sleep((ctx->response_timeout.tv_sec * 1000) + (ctx->response_timeout.tv_usec / 1000));
100 #else
101 /* usleep source code */
102 struct timespec request, remaining;
103 request.tv_sec = ctx->response_timeout.tv_sec;
104 request.tv_nsec = ((long int) ctx->response_timeout.tv_usec) * 1000;
105 while (nanosleep(&request, &remaining) == -1 && errno == EINTR) {
106 request = remaining;
107 }
108 #endif
109 }
110
111 int modbus_flush(modbus_t *ctx)
112 {
113 int rc;
114
115 if (ctx == NULL) {
116 errno = EINVAL;
117 return -1;
118 }
119
120 rc = ctx->backend->flush(ctx);
121 if (rc != -1 && ctx->debug) {
122 /* Not all backends are able to return the number of bytes flushed */
123 printf("Bytes flushed (%d)\n", rc);
124 }
125 return rc;
126 }
127
128 /* Computes the length of the expected response */
129 static unsigned int compute_response_length_from_request(modbus_t *ctx, uint8_t *req)
130 {
131 int length;
132 const int offset = ctx->backend->header_length;
133
134 switch (req[offset]) {
135 case MODBUS_FC_READ_COILS:
136 case MODBUS_FC_READ_DISCRETE_INPUTS: {
137 /* Header + nb values (code from write_bits) */
138 int nb = (req[offset + 3] << 8) | req[offset + 4];
139 length = 2 + (nb / 8) + ((nb % 8) ? 1 : 0);
140 } break;
141 case MODBUS_FC_WRITE_AND_READ_REGISTERS:
142 case MODBUS_FC_READ_HOLDING_REGISTERS:
143 case MODBUS_FC_READ_INPUT_REGISTERS:
144 /* Header + 2 * nb values */
145 length = 2 + 2 * (req[offset + 3] << 8 | req[offset + 4]);
146 break;
147 case MODBUS_FC_READ_EXCEPTION_STATUS:
148 length = 3;
149 break;
150 case MODBUS_FC_REPORT_SLAVE_ID:
151 /* The response is device specific (the header provides the
152 length) */
153 return MSG_LENGTH_UNDEFINED;
154 case MODBUS_FC_MASK_WRITE_REGISTER:
155 length = 7;
156 break;
157 default:
158 length = 5;
159 }
160
161 return offset + length + ctx->backend->checksum_length;
162 }
163
164 /* Sends a request/response */
165 static int send_msg(modbus_t *ctx, uint8_t *msg, int msg_length)
166 {
167 int rc;
168 int i;
169
170 msg_length = ctx->backend->send_msg_pre(msg, msg_length);
171
172 if (ctx->debug) {
173 for (i = 0; i < msg_length; i++)
174 printf("[%.2X]", msg[i]);
175 printf("\n");
176 }
177
178 /* In recovery mode, the write command will be issued until to be
179 successful! Disabled by default. */
180 do {
181 rc = ctx->backend->send(ctx, msg, msg_length);
182 if (rc == -1) {
183 _error_print(ctx, NULL);
184 if (ctx->error_recovery & MODBUS_ERROR_RECOVERY_LINK) {
185 #ifdef _WIN32
186 const int wsa_err = WSAGetLastError();
187 if (wsa_err == WSAENETRESET || wsa_err == WSAENOTCONN ||
188 wsa_err == WSAENOTSOCK || wsa_err == WSAESHUTDOWN ||
189 wsa_err == WSAEHOSTUNREACH || wsa_err == WSAECONNABORTED ||
190 wsa_err == WSAECONNRESET || wsa_err == WSAETIMEDOUT) {
191 modbus_close(ctx);
192 _sleep_response_timeout(ctx);
193 modbus_connect(ctx);
194 } else {
195 _sleep_response_timeout(ctx);
196 modbus_flush(ctx);
197 }
198 #else
199 int saved_errno = errno;
200
201 if ((errno == EBADF || errno == ECONNRESET || errno == EPIPE)) {
202 modbus_close(ctx);
203 _sleep_response_timeout(ctx);
204 modbus_connect(ctx);
205 } else {
206 _sleep_response_timeout(ctx);
207 modbus_flush(ctx);
208 }
209 errno = saved_errno;
210 #endif
211 }
212 }
213 } while ((ctx->error_recovery & MODBUS_ERROR_RECOVERY_LINK) && rc == -1);
214
215 if (rc > 0 && rc != msg_length) {
216 errno = EMBBADDATA;
217 return -1;
218 }
219
220 return rc;
221 }
222
223 int modbus_send_raw_request_tid(modbus_t *ctx, const uint8_t *raw_req, int raw_req_length, int tid)
224 {
225 sft_t sft;
226 uint8_t req[MAX_MESSAGE_LENGTH];
227 int req_length;
228
229 if (ctx == NULL) {
230 errno = EINVAL;
231 return -1;
232 }
233
234 if (raw_req_length < 2 || raw_req_length > (MODBUS_MAX_PDU_LENGTH + 1)) {
235 /* The raw request must contain function and slave at least and
236 must not be longer than the maximum pdu length plus the slave
237 address. */
238 errno = EINVAL;
239 return -1;
240 }
241
242 sft.slave = raw_req[0];
243 sft.function = raw_req[1];
244 /* The t_id is left to zero */
245 sft.t_id = tid;
246 /* This response function only set the header so it's convenient here */
247 req_length = ctx->backend->build_response_basis(&sft, req);
248
249 if (raw_req_length > 2) {
250 /* Copy data after function code */
251 memcpy(req + req_length, raw_req + 2, raw_req_length - 2);
252 req_length += raw_req_length - 2;
253 }
254
255 return send_msg(ctx, req, req_length);
256 }
257
258 int modbus_send_raw_request(modbus_t *ctx, const uint8_t *raw_req, int raw_req_length)
259 {
260 return modbus_send_raw_request_tid(ctx, raw_req, raw_req_length, 0);
261 }
262
263 /*
264 * ---------- Request Indication ----------
265 * | Client | ---------------------->| Server |
266 * ---------- Confirmation Response ----------
267 */
268
269 /* Computes the length to read after the function received */
270 static uint8_t compute_meta_length_after_function(int function, msg_type_t msg_type)
271 {
272 int length;
273
274 if (msg_type == MSG_INDICATION) {
275 if (function <= MODBUS_FC_WRITE_SINGLE_REGISTER) {
276 length = 4;
277 } else if (function == MODBUS_FC_WRITE_MULTIPLE_COILS ||
278 function == MODBUS_FC_WRITE_MULTIPLE_REGISTERS) {
279 length = 5;
280 } else if (function == MODBUS_FC_MASK_WRITE_REGISTER) {
281 length = 6;
282 } else if (function == MODBUS_FC_WRITE_AND_READ_REGISTERS) {
283 length = 9;
284 } else {
285 /* MODBUS_FC_READ_EXCEPTION_STATUS, MODBUS_FC_REPORT_SLAVE_ID */
286 length = 0;
287 }
288 } else {
289 /* MSG_CONFIRMATION */
290 switch (function) {
291 case MODBUS_FC_WRITE_SINGLE_COIL:
292 case MODBUS_FC_WRITE_SINGLE_REGISTER:
293 case MODBUS_FC_WRITE_MULTIPLE_COILS:
294 case MODBUS_FC_WRITE_MULTIPLE_REGISTERS:
295 length = 4;
296 break;
297 case MODBUS_FC_MASK_WRITE_REGISTER:
298 length = 6;
299 break;
300 default:
301 length = 1;
302 }
303 }
304
305 return length;
306 }
307
308 /* Computes the length to read after the meta information (address, count, etc) */
309 static int
310 compute_data_length_after_meta(modbus_t *ctx, uint8_t *msg, msg_type_t msg_type)
311 {
312 int function = msg[ctx->backend->header_length];
313 int length;
314
315 if (msg_type == MSG_INDICATION) {
316 switch (function) {
317 case MODBUS_FC_WRITE_MULTIPLE_COILS:
318 case MODBUS_FC_WRITE_MULTIPLE_REGISTERS:
319 length = msg[ctx->backend->header_length + 5];
320 break;
321 case MODBUS_FC_WRITE_AND_READ_REGISTERS:
322 length = msg[ctx->backend->header_length + 9];
323 break;
324 default:
325 length = 0;
326 }
327 } else {
328 /* MSG_CONFIRMATION */
329 if (function <= MODBUS_FC_READ_INPUT_REGISTERS ||
330 function == MODBUS_FC_REPORT_SLAVE_ID ||
331 function == MODBUS_FC_WRITE_AND_READ_REGISTERS) {
332 length = msg[ctx->backend->header_length + 1];
333 } else {
334 length = 0;
335 }
336 }
337
338 length += ctx->backend->checksum_length;
339
340 return length;
341 }
342
343 /* Waits a response from a modbus server or a request from a modbus client.
344 This function blocks if there is no replies (3 timeouts).
345
346 The function shall return the number of received characters and the received
347 message in an array of uint8_t if successful. Otherwise it shall return -1
348 and errno is set to one of the values defined below:
349 - ECONNRESET
350 - EMBBADDATA
351 - ETIMEDOUT
352 - read() or recv() error codes
353 */
354
355 int _modbus_receive_msg(modbus_t *ctx, uint8_t *msg, msg_type_t msg_type)
356 {
357 int rc;
358 fd_set rset;
359 struct timeval tv;
360 struct timeval *p_tv;
361 unsigned int length_to_read;
362 int msg_length = 0;
363 _step_t step;
364 #ifdef _WIN32
365 int wsa_err;
366 #endif
367
368 if (ctx->debug) {
369 if (msg_type == MSG_INDICATION) {
370 printf("Waiting for an indication...\n");
371 } else {
372 printf("Waiting for a confirmation...\n");
373 }
374 }
375
376 if (!ctx->backend->is_connected(ctx)) {
377 if (ctx->debug) {
378 fprintf(stderr, "ERROR The connection is not established.\n");
379 }
380 return -1;
381 }
382
383 /* Add a file descriptor to the set */
384 FD_ZERO(&rset);
385 FD_SET(ctx->s, &rset);
386
387 /* We need to analyse the message step by step. At the first step, we want
388 * to reach the function code because all packets contain this
389 * information. */
390 step = _STEP_FUNCTION;
391 length_to_read = ctx->backend->header_length + 1;
392
393 if (msg_type == MSG_INDICATION) {
394 /* Wait for a message, we don't know when the message will be
395 * received */
396 if (ctx->indication_timeout.tv_sec == 0 && ctx->indication_timeout.tv_usec == 0) {
397 /* By default, the indication timeout isn't set */
398 p_tv = NULL;
399 } else {
400 /* Wait for an indication (name of a received request by a server, see schema)
401 */
402 tv.tv_sec = ctx->indication_timeout.tv_sec;
403 tv.tv_usec = ctx->indication_timeout.tv_usec;
404 p_tv = &tv;
405 }
406 } else {
407 tv.tv_sec = ctx->response_timeout.tv_sec;
408 tv.tv_usec = ctx->response_timeout.tv_usec;
409 p_tv = &tv;
410 }
411
412 while (length_to_read != 0) {
413 rc = ctx->backend->select(ctx, &rset, p_tv, length_to_read);
414 if (rc == -1) {
415 _error_print(ctx, "select");
416 if (ctx->error_recovery & MODBUS_ERROR_RECOVERY_LINK) {
417 #ifdef _WIN32
418 wsa_err = WSAGetLastError();
419
420 // no equivalent to ETIMEDOUT when select fails on Windows
421 if (wsa_err == WSAENETDOWN || wsa_err == WSAENOTSOCK) {
422 modbus_close(ctx);
423 modbus_connect(ctx);
424 }
425 #else
426 int saved_errno = errno;
427
428 if (errno == ETIMEDOUT) {
429 _sleep_response_timeout(ctx);
430 modbus_flush(ctx);
431 } else if (errno == EBADF) {
432 modbus_close(ctx);
433 modbus_connect(ctx);
434 }
435 errno = saved_errno;
436 #endif
437 }
438 return -1;
439 }
440
441 rc = ctx->backend->recv(ctx, msg + msg_length, length_to_read);
442 if (rc == 0) {
443 errno = ECONNRESET;
444 rc = -1;
445 }
446
447 if (rc == -1) {
448 _error_print(ctx, "read");
449 #ifdef _WIN32
450 wsa_err = WSAGetLastError();
451 if ((ctx->error_recovery & MODBUS_ERROR_RECOVERY_LINK) &&
452 (ctx->backend->backend_type == _MODBUS_BACKEND_TYPE_TCP) &&
453 (wsa_err == WSAENOTCONN || wsa_err == WSAENETRESET ||
454 wsa_err == WSAENOTSOCK || wsa_err == WSAESHUTDOWN ||
455 wsa_err == WSAECONNABORTED || wsa_err == WSAETIMEDOUT ||
456 wsa_err == WSAECONNRESET)) {
457 modbus_close(ctx);
458 modbus_connect(ctx);
459 }
460 #else
461 if ((ctx->error_recovery & MODBUS_ERROR_RECOVERY_LINK) &&
462 (ctx->backend->backend_type == _MODBUS_BACKEND_TYPE_TCP) &&
463 (errno == ECONNRESET || errno == ECONNREFUSED || errno == EBADF)) {
464 int saved_errno = errno;
465 modbus_close(ctx);
466 modbus_connect(ctx);
467 /* Could be removed by previous calls */
468 errno = saved_errno;
469 }
470 #endif
471 return -1;
472 }
473
474 /* Display the hex code of each character received */
475 if (ctx->debug) {
476 int i;
477 for (i = 0; i < rc; i++)
478 printf("<%.2X>", msg[msg_length + i]);
479 }
480
481 /* Sums bytes received */
482 msg_length += rc;
483 /* Computes remaining bytes */
484 length_to_read -= rc;
485
486 if (length_to_read == 0) {
487 switch (step) {
488 case _STEP_FUNCTION:
489 /* Function code position */
490 length_to_read = compute_meta_length_after_function(
491 msg[ctx->backend->header_length], msg_type);
492 if (length_to_read != 0) {
493 step = _STEP_META;
494 break;
495 } /* else switches straight to the next step */
496 case _STEP_META:
497 length_to_read = compute_data_length_after_meta(ctx, msg, msg_type);
498 if ((msg_length + length_to_read) > ctx->backend->max_adu_length) {
499 errno = EMBBADDATA;
500 _error_print(ctx, "too many data");
501 return -1;
502 }
503 step = _STEP_DATA;
504 break;
505 default:
506 break;
507 }
508 }
509
510 if (length_to_read > 0 &&
511 (ctx->byte_timeout.tv_sec > 0 || ctx->byte_timeout.tv_usec > 0)) {
512 /* If there is no character in the buffer, the allowed timeout
513 interval between two consecutive bytes is defined by
514 byte_timeout */
515 tv.tv_sec = ctx->byte_timeout.tv_sec;
516 tv.tv_usec = ctx->byte_timeout.tv_usec;
517 p_tv = &tv;
518 }
519 /* else timeout isn't set again, the full response must be read before
520 expiration of response timeout (for CONFIRMATION only) */
521 }
522
523 if (ctx->debug)
524 printf("\n");
525
526 return ctx->backend->check_integrity(ctx, msg, msg_length);
527 }
528
529 /* Receive the request from a modbus master */
530 int modbus_receive(modbus_t *ctx, uint8_t *req)
531 {
532 if (ctx == NULL) {
533 errno = EINVAL;
534 return -1;
535 }
536
537 return ctx->backend->receive(ctx, req);
538 }
539
540 /* Receives the confirmation.
541
542 The function shall store the read response in rsp and return the number of
543 values (bits or words). Otherwise, its shall return -1 and errno is set.
544
545 The function doesn't check the confirmation is the expected response to the
546 initial request.
547 */
548 int modbus_receive_confirmation(modbus_t *ctx, uint8_t *rsp)
549 {
550 if (ctx == NULL) {
551 errno = EINVAL;
552 return -1;
553 }
554
555 return _modbus_receive_msg(ctx, rsp, MSG_CONFIRMATION);
556 }
557
558 static int check_confirmation(modbus_t *ctx, uint8_t *req, uint8_t *rsp, int rsp_length)
559 {
560 int rc;
561 int rsp_length_computed;
562 const unsigned int offset = ctx->backend->header_length;
563 const int function = rsp[offset];
564
565 if (ctx->backend->pre_check_confirmation) {
566 rc = ctx->backend->pre_check_confirmation(ctx, req, rsp, rsp_length);
567 if (rc == -1) {
568 if (ctx->error_recovery & MODBUS_ERROR_RECOVERY_PROTOCOL) {
569 _sleep_response_timeout(ctx);
570 modbus_flush(ctx);
571 }
572 return -1;
573 }
574 }
575
576 rsp_length_computed = compute_response_length_from_request(ctx, req);
577
578 /* Exception code */
579 if (function >= 0x80) {
580 if (rsp_length == (int) (offset + 2 + ctx->backend->checksum_length) &&
581 req[offset] == (rsp[offset] - 0x80)) {
582 /* Valid exception code received */
583
584 int exception_code = rsp[offset + 1];
585 if (exception_code < MODBUS_EXCEPTION_MAX) {
586 errno = MODBUS_ENOBASE + exception_code;
587 } else {
588 errno = EMBBADEXC;
589 }
590 _error_print(ctx, NULL);
591 return -1;
592 } else {
593 errno = EMBBADEXC;
594 _error_print(ctx, NULL);
595 return -1;
596 }
597 }
598
599 /* Check length */
600 if ((rsp_length == rsp_length_computed ||
601 rsp_length_computed == MSG_LENGTH_UNDEFINED) &&
602 function < 0x80) {
603 int req_nb_value;
604 int rsp_nb_value;
605 int resp_addr_ok = TRUE;
606 int resp_data_ok = TRUE;
607
608 /* Check function code */
609 if (function != req[offset]) {
610 if (ctx->debug) {
611 fprintf(
612 stderr,
613 "Received function not corresponding to the request (0x%X != 0x%X)\n",
614 function,
615 req[offset]);
616 }
617 if (ctx->error_recovery & MODBUS_ERROR_RECOVERY_PROTOCOL) {
618 _sleep_response_timeout(ctx);
619 modbus_flush(ctx);
620 }
621 errno = EMBBADDATA;
622 return -1;
623 }
624
625 /* Check the number of values is corresponding to the request */
626 switch (function) {
627 case MODBUS_FC_READ_COILS:
628 case MODBUS_FC_READ_DISCRETE_INPUTS:
629 /* Read functions, 8 values in a byte (nb
630 * of values in the request and byte count in
631 * the response. */
632 req_nb_value = (req[offset + 3] << 8) + req[offset + 4];
633 req_nb_value = (req_nb_value / 8) + ((req_nb_value % 8) ? 1 : 0);
634 rsp_nb_value = rsp[offset + 1];
635 break;
636 case MODBUS_FC_WRITE_AND_READ_REGISTERS:
637 case MODBUS_FC_READ_HOLDING_REGISTERS:
638 case MODBUS_FC_READ_INPUT_REGISTERS:
639 /* Read functions 1 value = 2 bytes */
640 req_nb_value = (req[offset + 3] << 8) + req[offset + 4];
641 rsp_nb_value = (rsp[offset + 1] / 2);
642 break;
643 case MODBUS_FC_WRITE_MULTIPLE_COILS:
644 case MODBUS_FC_WRITE_MULTIPLE_REGISTERS:
645 /* address in request and response must be equal */
646 if ((req[offset + 1] != rsp[offset + 1]) ||
647 (req[offset + 2] != rsp[offset + 2])) {
648 resp_addr_ok = FALSE;
649 }
650 /* N Write functions */
651 req_nb_value = (req[offset + 3] << 8) + req[offset + 4];
652 rsp_nb_value = (rsp[offset + 3] << 8) | rsp[offset + 4];
653 break;
654 case MODBUS_FC_REPORT_SLAVE_ID:
655 /* Report slave ID (bytes received) */
656 req_nb_value = rsp_nb_value = rsp[offset + 1];
657 break;
658 case MODBUS_FC_WRITE_SINGLE_COIL:
659 case MODBUS_FC_WRITE_SINGLE_REGISTER:
660 /* address in request and response must be equal */
661 if ((req[offset + 1] != rsp[offset + 1]) ||
662 (req[offset + 2] != rsp[offset + 2])) {
663 resp_addr_ok = FALSE;
664 }
665 /* data in request and response must be equal */
666 if ((req[offset + 3] != rsp[offset + 3]) ||
667 (req[offset + 4] != rsp[offset + 4])) {
668 resp_data_ok = FALSE;
669 }
670 /* 1 Write functions & others */
671 req_nb_value = rsp_nb_value = 1;
672 break;
673 default:
674 /* 1 Write functions & others */
675 req_nb_value = rsp_nb_value = 1;
676 break;
677 }
678
679 if ((req_nb_value == rsp_nb_value) && (resp_addr_ok == TRUE) &&
680 (resp_data_ok == TRUE)) {
681 rc = rsp_nb_value;
682 } else {
683 if (ctx->debug) {
684 fprintf(stderr,
685 "Received data not corresponding to the request (%d != %d)\n",
686 rsp_nb_value,
687 req_nb_value);
688 }
689
690 if (ctx->error_recovery & MODBUS_ERROR_RECOVERY_PROTOCOL) {
691 _sleep_response_timeout(ctx);
692 modbus_flush(ctx);
693 }
694
695 errno = EMBBADDATA;
696 rc = -1;
697 }
698 } else {
699 if (ctx->debug) {
700 fprintf(
701 stderr,
702 "Message length not corresponding to the computed length (%d != %d)\n",
703 rsp_length,
704 rsp_length_computed);
705 }
706 if (ctx->error_recovery & MODBUS_ERROR_RECOVERY_PROTOCOL) {
707 _sleep_response_timeout(ctx);
708 modbus_flush(ctx);
709 }
710 errno = EMBBADDATA;
711 rc = -1;
712 }
713
714 return rc;
715 }
716
717 static int
718 response_io_status(uint8_t *tab_io_status, int address, int nb, uint8_t *rsp, int offset)
719 {
720 int shift = 0;
721 /* Instead of byte (not allowed in Win32) */
722 int one_byte = 0;
723 int i;
724
725 for (i = address; i < address + nb; i++) {
726 one_byte |= tab_io_status[i] << shift;
727 if (shift == 7) {
728 /* Byte is full */
729 rsp[offset++] = one_byte;
730 one_byte = shift = 0;
731 } else {
732 shift++;
733 }
734 }
735
736 if (shift != 0)
737 rsp[offset++] = one_byte;
738
739 return offset;
740 }
741
742 /* Build the exception response */
743 static int response_exception(modbus_t *ctx,
744 sft_t *sft,
745 int exception_code,
746 uint8_t *rsp,
747 unsigned int to_flush,
748 const char *template,
749 ...)
750 {
751 int rsp_length;
752
753 /* Print debug message */
754 if (ctx->debug) {
755 va_list ap;
756
757 va_start(ap, template);
758 vfprintf(stderr, template, ap);
759 va_end(ap);
760 }
761
762 /* Flush if required */
763 if (to_flush) {
764 _sleep_response_timeout(ctx);
765 modbus_flush(ctx);
766 }
767
768 /* Build exception response */
769 sft->function = sft->function + 0x80;
770 rsp_length = ctx->backend->build_response_basis(sft, rsp);
771 rsp[rsp_length++] = exception_code;
772
773 return rsp_length;
774 }
775
776 /* Send a response to the received request.
777 Analyses the request and constructs a response.
778
779 If an error occurs, this function construct the response
780 accordingly.
781 */
782 int modbus_reply(modbus_t *ctx,
783 const uint8_t *req,
784 int req_length,
785 modbus_mapping_t *mb_mapping)
786 {
787 unsigned int offset;
788 int slave;
789 int function;
790 uint16_t address;
791 uint8_t rsp[MAX_MESSAGE_LENGTH];
792 int rsp_length = 0;
793 sft_t sft;
794
795 if (ctx == NULL) {
796 errno = EINVAL;
797 return -1;
798 }
799
800 offset = ctx->backend->header_length;
801 slave = req[offset - 1];
802 function = req[offset];
803 address = (req[offset + 1] << 8) + req[offset + 2];
804
805 sft.slave = slave;
806 sft.function = function;
807 sft.t_id = ctx->backend->prepare_response_tid(req, &req_length);
808
809 /* Data are flushed on illegal number of values errors. */
810 switch (function) {
811 case MODBUS_FC_READ_COILS:
812 case MODBUS_FC_READ_DISCRETE_INPUTS: {
813 unsigned int is_input = (function == MODBUS_FC_READ_DISCRETE_INPUTS);
814 int start_bits = is_input ? mb_mapping->start_input_bits : mb_mapping->start_bits;
815 int nb_bits = is_input ? mb_mapping->nb_input_bits : mb_mapping->nb_bits;
816 uint8_t *tab_bits = is_input ? mb_mapping->tab_input_bits : mb_mapping->tab_bits;
817 const char *const name = is_input ? "read_input_bits" : "read_bits";
818 int nb = (req[offset + 3] << 8) + req[offset + 4];
819 /* The mapping can be shifted to reduce memory consumption and it
820 doesn't always start at address zero. */
821 int mapping_address = address - start_bits;
822
823 if (nb < 1 || MODBUS_MAX_READ_BITS < nb) {
824 rsp_length = response_exception(ctx,
825 &sft,
826 MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE,
827 rsp,
828 TRUE,
829 "Illegal nb of values %d in %s (max %d)\n",
830 nb,
831 name,
832 MODBUS_MAX_READ_BITS);
833 } else if (mapping_address < 0 || (mapping_address + nb) > nb_bits) {
834 rsp_length = response_exception(ctx,
835 &sft,
836 MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS,
837 rsp,
838 FALSE,
839 "Illegal data address 0x%0X in %s\n",
840 mapping_address < 0 ? address : address + nb,
841 name);
842 } else {
843 rsp_length = ctx->backend->build_response_basis(&sft, rsp);
844 rsp[rsp_length++] = (nb / 8) + ((nb % 8) ? 1 : 0);
845 rsp_length =
846 response_io_status(tab_bits, mapping_address, nb, rsp, rsp_length);
847 }
848 } break;
849 case MODBUS_FC_READ_HOLDING_REGISTERS:
850 case MODBUS_FC_READ_INPUT_REGISTERS: {
851 unsigned int is_input = (function == MODBUS_FC_READ_INPUT_REGISTERS);
852 int start_registers =
853 is_input ? mb_mapping->start_input_registers : mb_mapping->start_registers;
854 int nb_registers =
855 is_input ? mb_mapping->nb_input_registers : mb_mapping->nb_registers;
856 uint16_t *tab_registers =
857 is_input ? mb_mapping->tab_input_registers : mb_mapping->tab_registers;
858 const char *const name = is_input ? "read_input_registers" : "read_registers";
859 int nb = (req[offset + 3] << 8) + req[offset + 4];
860 /* The mapping can be shifted to reduce memory consumption and it
861 doesn't always start at address zero. */
862 int mapping_address = address - start_registers;
863
864 if (nb < 1 || MODBUS_MAX_READ_REGISTERS < nb) {
865 rsp_length = response_exception(ctx,
866 &sft,
867 MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE,
868 rsp,
869 TRUE,
870 "Illegal nb of values %d in %s (max %d)\n",
871 nb,
872 name,
873 MODBUS_MAX_READ_REGISTERS);
874 } else if (mapping_address < 0 || (mapping_address + nb) > nb_registers) {
875 rsp_length = response_exception(ctx,
876 &sft,
877 MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS,
878 rsp,
879 FALSE,
880 "Illegal data address 0x%0X in %s\n",
881 mapping_address < 0 ? address : address + nb,
882 name);
883 } else {
884 int i;
885
886 rsp_length = ctx->backend->build_response_basis(&sft, rsp);
887 rsp[rsp_length++] = nb << 1;
888 for (i = mapping_address; i < mapping_address + nb; i++) {
889 rsp[rsp_length++] = tab_registers[i] >> 8;
890 rsp[rsp_length++] = tab_registers[i] & 0xFF;
891 }
892 }
893 } break;
894 case MODBUS_FC_WRITE_SINGLE_COIL: {
895 int mapping_address = address - mb_mapping->start_bits;
896
897 if (mapping_address < 0 || mapping_address >= mb_mapping->nb_bits) {
898 rsp_length = response_exception(ctx,
899 &sft,
900 MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS,
901 rsp,
902 FALSE,
903 "Illegal data address 0x%0X in write_bit\n",
904 address);
905 } else {
906 int data = (req[offset + 3] << 8) + req[offset + 4];
907
908 if (data == 0xFF00 || data == 0x0) {
909 mb_mapping->tab_bits[mapping_address] = data ? ON : OFF;
910 memcpy(rsp, req, req_length);
911 rsp_length = req_length;
912 } else {
913 rsp_length = response_exception(
914 ctx,
915 &sft,
916 MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE,
917 rsp,
918 FALSE,
919 "Illegal data value 0x%0X in write_bit request at address %0X\n",
920 data,
921 address);
922 }
923 }
924 } break;
925 case MODBUS_FC_WRITE_SINGLE_REGISTER: {
926 int mapping_address = address - mb_mapping->start_registers;
927
928 if (mapping_address < 0 || mapping_address >= mb_mapping->nb_registers) {
929 rsp_length =
930 response_exception(ctx,
931 &sft,
932 MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS,
933 rsp,
934 FALSE,
935 "Illegal data address 0x%0X in write_register\n",
936 address);
937 } else {
938 int data = (req[offset + 3] << 8) + req[offset + 4];
939
940 mb_mapping->tab_registers[mapping_address] = data;
941 memcpy(rsp, req, req_length);
942 rsp_length = req_length;
943 }
944 } break;
945 case MODBUS_FC_WRITE_MULTIPLE_COILS: {
946 int nb = (req[offset + 3] << 8) + req[offset + 4];
947 int nb_bits = req[offset + 5];
948 int mapping_address = address - mb_mapping->start_bits;
949
950 if (nb < 1 || MODBUS_MAX_WRITE_BITS < nb || nb_bits * 8 < nb) {
951 /* May be the indication has been truncated on reading because of
952 * invalid address (eg. nb is 0 but the request contains values to
953 * write) so it's necessary to flush. */
954 rsp_length =
955 response_exception(ctx,
956 &sft,
957 MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE,
958 rsp,
959 TRUE,
960 "Illegal number of values %d in write_bits (max %d)\n",
961 nb,
962 MODBUS_MAX_WRITE_BITS);
963 } else if (mapping_address < 0 || (mapping_address + nb) > mb_mapping->nb_bits) {
964 rsp_length = response_exception(ctx,
965 &sft,
966 MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS,
967 rsp,
968 FALSE,
969 "Illegal data address 0x%0X in write_bits\n",
970 mapping_address < 0 ? address : address + nb);
971 } else {
972 /* 6 = byte count */
973 modbus_set_bits_from_bytes(
974 mb_mapping->tab_bits, mapping_address, nb, &req[offset + 6]);
975
976 rsp_length = ctx->backend->build_response_basis(&sft, rsp);
977 /* 4 to copy the bit address (2) and the quantity of bits */
978 memcpy(rsp + rsp_length, req + rsp_length, 4);
979 rsp_length += 4;
980 }
981 } break;
982 case MODBUS_FC_WRITE_MULTIPLE_REGISTERS: {
983 int nb = (req[offset + 3] << 8) + req[offset + 4];
984 int nb_bytes = req[offset + 5];
985 int mapping_address = address - mb_mapping->start_registers;
986
987 if (nb < 1 || MODBUS_MAX_WRITE_REGISTERS < nb || nb_bytes != nb * 2) {
988 rsp_length = response_exception(
989 ctx,
990 &sft,
991 MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE,
992 rsp,
993 TRUE,
994 "Illegal number of values %d in write_registers (max %d)\n",
995 nb,
996 MODBUS_MAX_WRITE_REGISTERS);
997 } else if (mapping_address < 0 ||
998 (mapping_address + nb) > mb_mapping->nb_registers) {
999 rsp_length =
1000 response_exception(ctx,
1001 &sft,
1002 MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS,
1003 rsp,
1004 FALSE,
1005 "Illegal data address 0x%0X in write_registers\n",
1006 mapping_address < 0 ? address : address + nb);
1007 } else {
1008 int i, j;
1009 for (i = mapping_address, j = 6; i < mapping_address + nb; i++, j += 2) {
1010 /* 6 and 7 = first value */
1011 mb_mapping->tab_registers[i] =
1012 (req[offset + j] << 8) + req[offset + j + 1];
1013 }
1014
1015 rsp_length = ctx->backend->build_response_basis(&sft, rsp);
1016 /* 4 to copy the address (2) and the no. of registers */
1017 memcpy(rsp + rsp_length, req + rsp_length, 4);
1018 rsp_length += 4;
1019 }
1020 } break;
1021 case MODBUS_FC_REPORT_SLAVE_ID: {
1022 int str_len;
1023 int byte_count_pos;
1024
1025 rsp_length = ctx->backend->build_response_basis(&sft, rsp);
1026 /* Skip byte count for now */
1027 byte_count_pos = rsp_length++;
1028 rsp[rsp_length++] = _REPORT_SLAVE_ID;
1029 /* Run indicator status to ON */
1030 rsp[rsp_length++] = 0xFF;
1031 /* LMB + length of LIBMODBUS_VERSION_STRING */
1032 str_len = 3 + strlen(LIBMODBUS_VERSION_STRING);
1033 memcpy(rsp + rsp_length, "LMB" LIBMODBUS_VERSION_STRING, str_len);
1034 rsp_length += str_len;
1035 rsp[byte_count_pos] = rsp_length - byte_count_pos - 1;
1036 } break;
1037 case MODBUS_FC_READ_EXCEPTION_STATUS:
1038 if (ctx->debug) {
1039 fprintf(stderr, "FIXME Not implemented\n");
1040 }
1041 errno = ENOPROTOOPT;
1042 return -1;
1043 break;
1044 case MODBUS_FC_MASK_WRITE_REGISTER: {
1045 int mapping_address = address - mb_mapping->start_registers;
1046
1047 if (mapping_address < 0 || mapping_address >= mb_mapping->nb_registers) {
1048 rsp_length =
1049 response_exception(ctx,
1050 &sft,
1051 MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS,
1052 rsp,
1053 FALSE,
1054 "Illegal data address 0x%0X in write_register\n",
1055 address);
1056 } else {
1057 uint16_t data = mb_mapping->tab_registers[mapping_address];
1058 uint16_t and = (req[offset + 3] << 8) + req[offset + 4];
1059 uint16_t or = (req[offset + 5] << 8) + req[offset + 6];
1060
1061 data = (data & and) | (or &(~and));
1062 mb_mapping->tab_registers[mapping_address] = data;
1063 memcpy(rsp, req, req_length);
1064 rsp_length = req_length;
1065 }
1066 } break;
1067 case MODBUS_FC_WRITE_AND_READ_REGISTERS: {
1068 int nb = (req[offset + 3] << 8) + req[offset + 4];
1069 uint16_t address_write = (req[offset + 5] << 8) + req[offset + 6];
1070 int nb_write = (req[offset + 7] << 8) + req[offset + 8];
1071 int nb_write_bytes = req[offset + 9];
1072 int mapping_address = address - mb_mapping->start_registers;
1073 int mapping_address_write = address_write - mb_mapping->start_registers;
1074
1075 if (nb_write < 1 || MODBUS_MAX_WR_WRITE_REGISTERS < nb_write || nb < 1 ||
1076 MODBUS_MAX_WR_READ_REGISTERS < nb || nb_write_bytes != nb_write * 2) {
1077 rsp_length = response_exception(
1078 ctx,
1079 &sft,
1080 MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE,
1081 rsp,
1082 TRUE,
1083 "Illegal nb of values (W%d, R%d) in write_and_read_registers (max W%d, "
1084 "R%d)\n",
1085 nb_write,
1086 nb,
1087 MODBUS_MAX_WR_WRITE_REGISTERS,
1088 MODBUS_MAX_WR_READ_REGISTERS);
1089 } else if (mapping_address < 0 ||
1090 (mapping_address + nb) > mb_mapping->nb_registers ||
1091 mapping_address_write < 0 ||
1092 (mapping_address_write + nb_write) > mb_mapping->nb_registers) {
1093 rsp_length = response_exception(
1094 ctx,
1095 &sft,
1096 MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS,
1097 rsp,
1098 FALSE,
1099 "Illegal data read address 0x%0X or write address 0x%0X "
1100 "write_and_read_registers\n",
1101 mapping_address < 0 ? address : address + nb,
1102 mapping_address_write < 0 ? address_write : address_write + nb_write);
1103 } else {
1104 int i, j;
1105 rsp_length = ctx->backend->build_response_basis(&sft, rsp);
1106 rsp[rsp_length++] = nb << 1;
1107
1108 /* Write first.
1109 10 and 11 are the offset of the first values to write */
1110 for (i = mapping_address_write, j = 10; i < mapping_address_write + nb_write;
1111 i++, j += 2) {
1112 mb_mapping->tab_registers[i] =
1113 (req[offset + j] << 8) + req[offset + j + 1];
1114 }
1115
1116 /* and read the data for the response */
1117 for (i = mapping_address; i < mapping_address + nb; i++) {
1118 rsp[rsp_length++] = mb_mapping->tab_registers[i] >> 8;
1119 rsp[rsp_length++] = mb_mapping->tab_registers[i] & 0xFF;
1120 }
1121 }
1122 } break;
1123
1124 default:
1125 rsp_length = response_exception(ctx,
1126 &sft,
1127 MODBUS_EXCEPTION_ILLEGAL_FUNCTION,
1128 rsp,
1129 TRUE,
1130 "Unknown Modbus function code: 0x%0X\n",
1131 function);
1132 break;
1133 }
1134
1135 /* Suppress any responses in RTU when the request was a broadcast, excepted when quirk
1136 * is enabled. */
1137 if (ctx->backend->backend_type == _MODBUS_BACKEND_TYPE_RTU &&
1138 slave == MODBUS_BROADCAST_ADDRESS &&
1139 !(ctx->quirks & MODBUS_QUIRK_REPLY_TO_BROADCAST)) {
1140 return 0;
1141 }
1142 return send_msg(ctx, rsp, rsp_length);
1143 }
1144
1145 int modbus_reply_exception(modbus_t *ctx, const uint8_t *req, unsigned int exception_code)
1146 {
1147 unsigned int offset;
1148 int slave;
1149 int function;
1150 uint8_t rsp[MAX_MESSAGE_LENGTH];
1151 int rsp_length;
1152 int dummy_length = 99;
1153 sft_t sft;
1154
1155 if (ctx == NULL) {
1156 errno = EINVAL;
1157 return -1;
1158 }
1159
1160 offset = ctx->backend->header_length;
1161 slave = req[offset - 1];
1162 function = req[offset];
1163
1164 sft.slave = slave;
1165 sft.function = function + 0x80;
1166 sft.t_id = ctx->backend->prepare_response_tid(req, &dummy_length);
1167 rsp_length = ctx->backend->build_response_basis(&sft, rsp);
1168
1169 /* Positive exception code */
1170 if (exception_code < MODBUS_EXCEPTION_MAX) {
1171 rsp[rsp_length++] = exception_code;
1172 return send_msg(ctx, rsp, rsp_length);
1173 } else {
1174 errno = EINVAL;
1175 return -1;
1176 }
1177 }
1178
1179 /* Reads IO status */
1180 static int read_io_status(modbus_t *ctx, int function, int addr, int nb, uint8_t *dest)
1181 {
1182 int rc;
1183 int req_length;
1184
1185 uint8_t req[_MIN_REQ_LENGTH];
1186 uint8_t rsp[MAX_MESSAGE_LENGTH];
1187
1188 req_length = ctx->backend->build_request_basis(ctx, function, addr, nb, req);
1189
1190 rc = send_msg(ctx, req, req_length);
1191 if (rc > 0) {
1192 int temp, bit;
1193 int pos = 0;
1194 unsigned int offset;
1195 unsigned int offset_end;
1196
1197 rc = _modbus_receive_msg(ctx, rsp, MSG_CONFIRMATION);
1198 if (rc == -1)
1199 return -1;
1200
1201 rc = check_confirmation(ctx, req, rsp, rc);
1202 if (rc == -1)
1203 return -1;
1204
1205 offset = ctx->backend->header_length + 2;
1206 offset_end = offset + rc;
1207 for (unsigned int i = offset; i < offset_end; i++) {
1208 /* Shift reg hi_byte to temp */
1209 temp = rsp[i];
1210
1211 for (bit = 0x01; (bit & 0xff) && (pos < nb);) {
1212 dest[pos++] = (temp & bit) ? TRUE : FALSE;
1213 bit = bit << 1;
1214 }
1215 }
1216 }
1217
1218 return rc;
1219 }
1220
1221 /* Reads the boolean status of bits and sets the array elements
1222 in the destination to TRUE or FALSE (single bits). */
1223 int modbus_read_bits(modbus_t *ctx, int addr, int nb, uint8_t *dest)
1224 {
1225 int rc;
1226
1227 if (ctx == NULL) {
1228 errno = EINVAL;
1229 return -1;
1230 }
1231
1232 if (nb > MODBUS_MAX_READ_BITS) {
1233 if (ctx->debug) {
1234 fprintf(stderr,
1235 "ERROR Too many bits requested (%d > %d)\n",
1236 nb,
1237 MODBUS_MAX_READ_BITS);
1238 }
1239 errno = EMBMDATA;
1240 return -1;
1241 }
1242
1243 rc = read_io_status(ctx, MODBUS_FC_READ_COILS, addr, nb, dest);
1244
1245 if (rc == -1)
1246 return -1;
1247 else
1248 return nb;
1249 }
1250
1251 /* Same as modbus_read_bits but reads the remote device input table */
1252 int modbus_read_input_bits(modbus_t *ctx, int addr, int nb, uint8_t *dest)
1253 {
1254 int rc;
1255
1256 if (ctx == NULL) {
1257 errno = EINVAL;
1258 return -1;
1259 }
1260
1261 if (nb > MODBUS_MAX_READ_BITS) {
1262 if (ctx->debug) {
1263 fprintf(stderr,
1264 "ERROR Too many discrete inputs requested (%d > %d)\n",
1265 nb,
1266 MODBUS_MAX_READ_BITS);
1267 }
1268 errno = EMBMDATA;
1269 return -1;
1270 }
1271
1272 rc = read_io_status(ctx, MODBUS_FC_READ_DISCRETE_INPUTS, addr, nb, dest);
1273
1274 if (rc == -1)
1275 return -1;
1276 else
1277 return nb;
1278 }
1279
1280 /* Reads the data from a remote device and put that data into an array */
1281 static int read_registers(modbus_t *ctx, int function, int addr, int nb, uint16_t *dest)
1282 {
1283 int rc;
1284 int req_length;
1285 uint8_t req[_MIN_REQ_LENGTH];
1286 uint8_t rsp[MAX_MESSAGE_LENGTH];
1287
1288 if (nb > MODBUS_MAX_READ_REGISTERS) {
1289 if (ctx->debug) {
1290 fprintf(stderr,
1291 "ERROR Too many registers requested (%d > %d)\n",
1292 nb,
1293 MODBUS_MAX_READ_REGISTERS);
1294 }
1295 errno = EMBMDATA;
1296 return -1;
1297 }
1298
1299 req_length = ctx->backend->build_request_basis(ctx, function, addr, nb, req);
1300
1301 rc = send_msg(ctx, req, req_length);
1302 if (rc > 0) {
1303 unsigned int offset;
1304 int i;
1305
1306 rc = _modbus_receive_msg(ctx, rsp, MSG_CONFIRMATION);
1307 if (rc == -1)
1308 return -1;
1309
1310 rc = check_confirmation(ctx, req, rsp, rc);
1311 if (rc == -1)
1312 return -1;
1313
1314 offset = ctx->backend->header_length;
1315
1316 for (i = 0; i < rc; i++) {
1317 /* shift reg hi_byte to temp OR with lo_byte */
1318 dest[i] = (rsp[offset + 2 + (i << 1)] << 8) | rsp[offset + 3 + (i << 1)];
1319 }
1320 }
1321
1322 return rc;
1323 }
1324
1325 /* Reads the holding registers of remote device and put the data into an
1326 array */
1327 int modbus_read_registers(modbus_t *ctx, int addr, int nb, uint16_t *dest)
1328 {
1329 int status;
1330
1331 if (ctx == NULL) {
1332 errno = EINVAL;
1333 return -1;
1334 }
1335
1336 if (nb > MODBUS_MAX_READ_REGISTERS) {
1337 if (ctx->debug) {
1338 fprintf(stderr,
1339 "ERROR Too many registers requested (%d > %d)\n",
1340 nb,
1341 MODBUS_MAX_READ_REGISTERS);
1342 }
1343 errno = EMBMDATA;
1344 return -1;
1345 }
1346
1347 status = read_registers(ctx, MODBUS_FC_READ_HOLDING_REGISTERS, addr, nb, dest);
1348 return status;
1349 }
1350
1351 /* Reads the input registers of remote device and put the data into an array */
1352 int modbus_read_input_registers(modbus_t *ctx, int addr, int nb, uint16_t *dest)
1353 {
1354 int status;
1355
1356 if (ctx == NULL) {
1357 errno = EINVAL;
1358 return -1;
1359 }
1360
1361 if (nb > MODBUS_MAX_READ_REGISTERS) {
1362 if (ctx->debug) {
1363 fprintf(stderr,
1364 "ERROR Too many input registers requested (%d > %d)\n",
1365 nb,
1366 MODBUS_MAX_READ_REGISTERS);
1367 }
1368 errno = EMBMDATA;
1369 return -1;
1370 }
1371
1372 status = read_registers(ctx, MODBUS_FC_READ_INPUT_REGISTERS, addr, nb, dest);
1373
1374 return status;
1375 }
1376
1377 /* Write a value to the specified register of the remote device.
1378 Used by write_bit and write_register */
1379 static int write_single(modbus_t *ctx, int function, int addr, const uint16_t value)
1380 {
1381 int rc;
1382 int req_length;
1383 uint8_t req[_MIN_REQ_LENGTH];
1384
1385 if (ctx == NULL) {
1386 errno = EINVAL;
1387 return -1;
1388 }
1389
1390 req_length = ctx->backend->build_request_basis(ctx, function, addr, (int) value, req);
1391
1392 rc = send_msg(ctx, req, req_length);
1393 if (rc > 0) {
1394 /* Used by write_bit and write_register */
1395 uint8_t rsp[MAX_MESSAGE_LENGTH];
1396
1397 rc = _modbus_receive_msg(ctx, rsp, MSG_CONFIRMATION);
1398 if (rc == -1)
1399 return -1;
1400
1401 rc = check_confirmation(ctx, req, rsp, rc);
1402 }
1403
1404 return rc;
1405 }
1406
1407 /* Turns ON or OFF a single bit of the remote device */
1408 int modbus_write_bit(modbus_t *ctx, int addr, int status)
1409 {
1410 if (ctx == NULL) {
1411 errno = EINVAL;
1412 return -1;
1413 }
1414
1415 return write_single(ctx, MODBUS_FC_WRITE_SINGLE_COIL, addr, status ? 0xFF00 : 0);
1416 }
1417
1418 /* Writes a value in one register of the remote device */
1419 int modbus_write_register(modbus_t *ctx, int addr, const uint16_t value)
1420 {
1421 if (ctx == NULL) {
1422 errno = EINVAL;
1423 return -1;
1424 }
1425
1426 return write_single(ctx, MODBUS_FC_WRITE_SINGLE_REGISTER, addr, value);
1427 }
1428
1429 /* Write the bits of the array in the remote device */
1430 int modbus_write_bits(modbus_t *ctx, int addr, int nb, const uint8_t *src)
1431 {
1432 int rc;
1433 int i;
1434 int byte_count;
1435 int req_length;
1436 int bit_check = 0;
1437 int pos = 0;
1438 uint8_t req[MAX_MESSAGE_LENGTH];
1439
1440 if (ctx == NULL) {
1441 errno = EINVAL;
1442 return -1;
1443 }
1444
1445 if (nb > MODBUS_MAX_WRITE_BITS) {
1446 if (ctx->debug) {
1447 fprintf(stderr,
1448 "ERROR Writing too many bits (%d > %d)\n",
1449 nb,
1450 MODBUS_MAX_WRITE_BITS);
1451 }
1452 errno = EMBMDATA;
1453 return -1;
1454 }
1455
1456 req_length = ctx->backend->build_request_basis(
1457 ctx, MODBUS_FC_WRITE_MULTIPLE_COILS, addr, nb, req);
1458 byte_count = (nb / 8) + ((nb % 8) ? 1 : 0);
1459 req[req_length++] = byte_count;
1460
1461 for (i = 0; i < byte_count; i++) {
1462 int bit;
1463
1464 bit = 0x01;
1465 req[req_length] = 0;
1466
1467 while ((bit & 0xFF) && (bit_check++ < nb)) {
1468 if (src[pos++])
1469 req[req_length] |= bit;
1470 else
1471 req[req_length] &= ~bit;
1472
1473 bit = bit << 1;
1474 }
1475 req_length++;
1476 }
1477
1478 rc = send_msg(ctx, req, req_length);
1479 if (rc > 0) {
1480 uint8_t rsp[MAX_MESSAGE_LENGTH];
1481
1482 rc = _modbus_receive_msg(ctx, rsp, MSG_CONFIRMATION);
1483 if (rc == -1)
1484 return -1;
1485
1486 rc = check_confirmation(ctx, req, rsp, rc);
1487 }
1488
1489 return rc;
1490 }
1491
1492 /* Write the values from the array to the registers of the remote device */
1493 int modbus_write_registers(modbus_t *ctx, int addr, int nb, const uint16_t *src)
1494 {
1495 int rc;
1496 int i;
1497 int req_length;
1498 int byte_count;
1499 uint8_t req[MAX_MESSAGE_LENGTH];
1500
1501 if (ctx == NULL) {
1502 errno = EINVAL;
1503 return -1;
1504 }
1505
1506 if (nb > MODBUS_MAX_WRITE_REGISTERS) {
1507 if (ctx->debug) {
1508 fprintf(stderr,
1509 "ERROR Trying to write to too many registers (%d > %d)\n",
1510 nb,
1511 MODBUS_MAX_WRITE_REGISTERS);
1512 }
1513 errno = EMBMDATA;
1514 return -1;
1515 }
1516
1517 req_length = ctx->backend->build_request_basis(
1518 ctx, MODBUS_FC_WRITE_MULTIPLE_REGISTERS, addr, nb, req);
1519 byte_count = nb * 2;
1520 req[req_length++] = byte_count;
1521
1522 for (i = 0; i < nb; i++) {
1523 req[req_length++] = src[i] >> 8;
1524 req[req_length++] = src[i] & 0x00FF;
1525 }
1526
1527 rc = send_msg(ctx, req, req_length);
1528 if (rc > 0) {
1529 uint8_t rsp[MAX_MESSAGE_LENGTH];
1530
1531 rc = _modbus_receive_msg(ctx, rsp, MSG_CONFIRMATION);
1532 if (rc == -1)
1533 return -1;
1534
1535 rc = check_confirmation(ctx, req, rsp, rc);
1536 }
1537
1538 return rc;
1539 }
1540
1541 int modbus_mask_write_register(modbus_t *ctx,
1542 int addr,
1543 uint16_t and_mask,
1544 uint16_t or_mask)
1545 {
1546 int rc;
1547 int req_length;
1548 /* The request length can not exceed _MIN_REQ_LENGTH - 2 and 4 bytes to
1549 * store the masks. The ugly substraction is there to remove the 'nb' value
1550 * (2 bytes) which is not used. */
1551 uint8_t req[_MIN_REQ_LENGTH + 2];
1552
1553 req_length = ctx->backend->build_request_basis(
1554 ctx, MODBUS_FC_MASK_WRITE_REGISTER, addr, 0, req);
1555
1556 /* HACKISH, count is not used */
1557 req_length -= 2;
1558
1559 req[req_length++] = and_mask >> 8;
1560 req[req_length++] = and_mask & 0x00ff;
1561 req[req_length++] = or_mask >> 8;
1562 req[req_length++] = or_mask & 0x00ff;
1563
1564 rc = send_msg(ctx, req, req_length);
1565 if (rc > 0) {
1566 /* Used by write_bit and write_register */
1567 uint8_t rsp[MAX_MESSAGE_LENGTH];
1568
1569 rc = _modbus_receive_msg(ctx, rsp, MSG_CONFIRMATION);
1570 if (rc == -1)
1571 return -1;
1572
1573 rc = check_confirmation(ctx, req, rsp, rc);
1574 }
1575
1576 return rc;
1577 }
1578
1579 /* Write multiple registers from src array to remote device and read multiple
1580 registers from remote device to dest array. */
1581 int modbus_write_and_read_registers(modbus_t *ctx,
1582 int write_addr,
1583 int write_nb,
1584 const uint16_t *src,
1585 int read_addr,
1586 int read_nb,
1587 uint16_t *dest)
1588
1589 {
1590 int rc;
1591 int req_length;
1592 int i;
1593 int byte_count;
1594 uint8_t req[MAX_MESSAGE_LENGTH];
1595 uint8_t rsp[MAX_MESSAGE_LENGTH];
1596
1597 if (ctx == NULL) {
1598 errno = EINVAL;
1599 return -1;
1600 }
1601
1602 if (write_nb > MODBUS_MAX_WR_WRITE_REGISTERS) {
1603 if (ctx->debug) {
1604 fprintf(stderr,
1605 "ERROR Too many registers to write (%d > %d)\n",
1606 write_nb,
1607 MODBUS_MAX_WR_WRITE_REGISTERS);
1608 }
1609 errno = EMBMDATA;
1610 return -1;
1611 }
1612
1613 if (read_nb > MODBUS_MAX_WR_READ_REGISTERS) {
1614 if (ctx->debug) {
1615 fprintf(stderr,
1616 "ERROR Too many registers requested (%d > %d)\n",
1617 read_nb,
1618 MODBUS_MAX_WR_READ_REGISTERS);
1619 }
1620 errno = EMBMDATA;
1621 return -1;
1622 }
1623 req_length = ctx->backend->build_request_basis(
1624 ctx, MODBUS_FC_WRITE_AND_READ_REGISTERS, read_addr, read_nb, req);
1625
1626 req[req_length++] = write_addr >> 8;
1627 req[req_length++] = write_addr & 0x00ff;
1628 req[req_length++] = write_nb >> 8;
1629 req[req_length++] = write_nb & 0x00ff;
1630 byte_count = write_nb * 2;
1631 req[req_length++] = byte_count;
1632
1633 for (i = 0; i < write_nb; i++) {
1634 req[req_length++] = src[i] >> 8;
1635 req[req_length++] = src[i] & 0x00FF;
1636 }
1637
1638 rc = send_msg(ctx, req, req_length);
1639 if (rc > 0) {
1640 unsigned int offset;
1641
1642 rc = _modbus_receive_msg(ctx, rsp, MSG_CONFIRMATION);
1643 if (rc == -1)
1644 return -1;
1645
1646 rc = check_confirmation(ctx, req, rsp, rc);
1647 if (rc == -1)
1648 return -1;
1649
1650 offset = ctx->backend->header_length;
1651 for (i = 0; i < rc; i++) {
1652 /* shift reg hi_byte to temp OR with lo_byte */
1653 dest[i] = (rsp[offset + 2 + (i << 1)] << 8) | rsp[offset + 3 + (i << 1)];
1654 }
1655 }
1656
1657 return rc;
1658 }
1659
1660 /* Send a request to get the slave ID of the device (only available in serial
1661 communication). */
1662 int modbus_report_slave_id(modbus_t *ctx, int max_dest, uint8_t *dest)
1663 {
1664 int rc;
1665 int req_length;
1666 uint8_t req[_MIN_REQ_LENGTH];
1667
1668 if (ctx == NULL || max_dest <= 0) {
1669 errno = EINVAL;
1670 return -1;
1671 }
1672
1673 req_length =
1674 ctx->backend->build_request_basis(ctx, MODBUS_FC_REPORT_SLAVE_ID, 0, 0, req);
1675
1676 /* HACKISH, addr and count are not used */
1677 req_length -= 4;
1678
1679 rc = send_msg(ctx, req, req_length);
1680 if (rc > 0) {
1681 int i;
1682 unsigned int offset;
1683 uint8_t rsp[MAX_MESSAGE_LENGTH];
1684
1685 rc = _modbus_receive_msg(ctx, rsp, MSG_CONFIRMATION);
1686 if (rc == -1)
1687 return -1;
1688
1689 rc = check_confirmation(ctx, req, rsp, rc);
1690 if (rc == -1)
1691 return -1;
1692
1693 offset = ctx->backend->header_length + 2;
1694
1695 /* Byte count, slave id, run indicator status and
1696 additional data. Truncate copy to max_dest. */
1697 for (i = 0; i < rc && i < max_dest; i++) {
1698 dest[i] = rsp[offset + i];
1699 }
1700 }
1701
1702 return rc;
1703 }
1704
1705 void _modbus_init_common(modbus_t *ctx)
1706 {
1707 /* Slave and socket are initialized to -1 */
1708 ctx->slave = -1;
1709 ctx->s = -1;
1710
1711 ctx->debug = FALSE;
1712 ctx->error_recovery = MODBUS_ERROR_RECOVERY_NONE;
1713 ctx->quirks = MODBUS_QUIRK_NONE;
1714
1715 ctx->response_timeout.tv_sec = 0;
1716 ctx->response_timeout.tv_usec = _RESPONSE_TIMEOUT;
1717
1718 ctx->byte_timeout.tv_sec = 0;
1719 ctx->byte_timeout.tv_usec = _BYTE_TIMEOUT;
1720
1721 ctx->indication_timeout.tv_sec = 0;
1722 ctx->indication_timeout.tv_usec = 0;
1723 }
1724
1725 /* Define the slave number */
1726 int modbus_set_slave(modbus_t *ctx, int slave)
1727 {
1728 if (ctx == NULL) {
1729 errno = EINVAL;
1730 return -1;
1731 }
1732
1733 return ctx->backend->set_slave(ctx, slave);
1734 }
1735
1736 int modbus_get_slave(modbus_t *ctx)
1737 {
1738 if (ctx == NULL) {
1739 errno = EINVAL;
1740 return -1;
1741 }
1742
1743 return ctx->slave;
1744 }
1745
1746 int modbus_set_error_recovery(modbus_t *ctx, modbus_error_recovery_mode error_recovery)
1747 {
1748 if (ctx == NULL) {
1749 errno = EINVAL;
1750 return -1;
1751 }
1752
1753 /* The type of modbus_error_recovery_mode is unsigned enum */
1754 ctx->error_recovery = (uint8_t) error_recovery;
1755 return 0;
1756 }
1757
1758 // FIXME Doesn't work under Windows RTU
1759 int modbus_set_socket(modbus_t *ctx, int s)
1760 {
1761 if (ctx == NULL) {
1762 errno = EINVAL;
1763 return -1;
1764 }
1765
1766 ctx->s = s;
1767 return 0;
1768 }
1769
1770 int modbus_get_socket(modbus_t *ctx)
1771 {
1772 if (ctx == NULL) {
1773 errno = EINVAL;
1774 return -1;
1775 }
1776
1777 return ctx->s;
1778 }
1779
1780 /* Get the timeout interval used to wait for a response */
1781 int modbus_get_response_timeout(modbus_t *ctx, uint32_t *to_sec, uint32_t *to_usec)
1782 {
1783 if (ctx == NULL) {
1784 errno = EINVAL;
1785 return -1;
1786 }
1787
1788 *to_sec = ctx->response_timeout.tv_sec;
1789 *to_usec = ctx->response_timeout.tv_usec;
1790 return 0;
1791 }
1792
1793 int modbus_set_response_timeout(modbus_t *ctx, uint32_t to_sec, uint32_t to_usec)
1794 {
1795 if (ctx == NULL || (to_sec == 0 && to_usec == 0) || to_usec > 999999) {
1796 errno = EINVAL;
1797 return -1;
1798 }
1799
1800 ctx->response_timeout.tv_sec = to_sec;
1801 ctx->response_timeout.tv_usec = to_usec;
1802 return 0;
1803 }
1804
1805 /* Get the timeout interval between two consecutive bytes of a message */
1806 int modbus_get_byte_timeout(modbus_t *ctx, uint32_t *to_sec, uint32_t *to_usec)
1807 {
1808 if (ctx == NULL) {
1809 errno = EINVAL;
1810 return -1;
1811 }
1812
1813 *to_sec = ctx->byte_timeout.tv_sec;
1814 *to_usec = ctx->byte_timeout.tv_usec;
1815 return 0;
1816 }
1817
1818 int modbus_set_byte_timeout(modbus_t *ctx, uint32_t to_sec, uint32_t to_usec)
1819 {
1820 /* Byte timeout can be disabled when both values are zero */
1821 if (ctx == NULL || to_usec > 999999) {
1822 errno = EINVAL;
1823 return -1;
1824 }
1825
1826 ctx->byte_timeout.tv_sec = to_sec;
1827 ctx->byte_timeout.tv_usec = to_usec;
1828 return 0;
1829 }
1830
1831 /* Get the timeout interval used by the server to wait for an indication from a client */
1832 int modbus_get_indication_timeout(modbus_t *ctx, uint32_t *to_sec, uint32_t *to_usec)
1833 {
1834 if (ctx == NULL) {
1835 errno = EINVAL;
1836 return -1;
1837 }
1838
1839 *to_sec = ctx->indication_timeout.tv_sec;
1840 *to_usec = ctx->indication_timeout.tv_usec;
1841 return 0;
1842 }
1843
1844 int modbus_set_indication_timeout(modbus_t *ctx, uint32_t to_sec, uint32_t to_usec)
1845 {
1846 /* Indication timeout can be disabled when both values are zero */
1847 if (ctx == NULL || to_usec > 999999) {
1848 errno = EINVAL;
1849 return -1;
1850 }
1851
1852 ctx->indication_timeout.tv_sec = to_sec;
1853 ctx->indication_timeout.tv_usec = to_usec;
1854 return 0;
1855 }
1856
1857 int modbus_get_header_length(modbus_t *ctx)
1858 {
1859 if (ctx == NULL) {
1860 errno = EINVAL;
1861 return -1;
1862 }
1863
1864 return ctx->backend->header_length;
1865 }
1866
1867 int modbus_enable_quirks(modbus_t *ctx, unsigned int quirks_mask)
1868 {
1869 if (ctx == NULL) {
1870 errno = EINVAL;
1871 return -1;
1872 }
1873
1874 /* Enable quirks that have a true value at their index in the mask */
1875 ctx->quirks |= quirks_mask;
1876 return 0;
1877 }
1878
1879 int modbus_disable_quirks(modbus_t *ctx, unsigned int quirks_mask)
1880 {
1881 if (ctx == NULL) {
1882 errno = EINVAL;
1883 return -1;
1884 }
1885
1886 /* Disable quirks that have a true value at ther index in the mask */
1887 ctx->quirks &= ~quirks_mask;
1888 return 0;
1889 }
1890
1891 int modbus_connect(modbus_t *ctx)
1892 {
1893 if (ctx == NULL) {
1894 errno = EINVAL;
1895 return -1;
1896 }
1897
1898 return ctx->backend->connect(ctx);
1899 }
1900
1901 void modbus_close(modbus_t *ctx)
1902 {
1903 if (ctx == NULL)
1904 return;
1905
1906 ctx->backend->close(ctx);
1907 }
1908
1909 void modbus_free(modbus_t *ctx)
1910 {
1911 if (ctx == NULL)
1912 return;
1913
1914 ctx->backend->free(ctx);
1915 }
1916
1917 int modbus_set_debug(modbus_t *ctx, int flag)
1918 {
1919 if (ctx == NULL) {
1920 errno = EINVAL;
1921 return -1;
1922 }
1923
1924 ctx->debug = flag;
1925 return 0;
1926 }
1927
1928 /* Allocates 4 arrays to store bits, input bits, registers and inputs
1929 registers. The pointers are stored in modbus_mapping structure.
1930
1931 The modbus_mapping_new_start_address() function shall return the new allocated
1932 structure if successful. Otherwise it shall return NULL and set errno to
1933 ENOMEM. */
1934 modbus_mapping_t *modbus_mapping_new_start_address(unsigned int start_bits,
1935 unsigned int nb_bits,
1936 unsigned int start_input_bits,
1937 unsigned int nb_input_bits,
1938 unsigned int start_registers,
1939 unsigned int nb_registers,
1940 unsigned int start_input_registers,
1941 unsigned int nb_input_registers)
1942 {
1943 modbus_mapping_t *mb_mapping;
1944
1945 mb_mapping = (modbus_mapping_t *) malloc(sizeof(modbus_mapping_t));
1946 if (mb_mapping == NULL) {
1947 return NULL;
1948 }
1949
1950 /* 0X */
1951 mb_mapping->nb_bits = nb_bits;
1952 mb_mapping->start_bits = start_bits;
1953 if (nb_bits == 0) {
1954 mb_mapping->tab_bits = NULL;
1955 } else {
1956 /* Negative number raises a POSIX error */
1957 mb_mapping->tab_bits = (uint8_t *) malloc(nb_bits * sizeof(uint8_t));
1958 if (mb_mapping->tab_bits == NULL) {
1959 free(mb_mapping);
1960 return NULL;
1961 }
1962 memset(mb_mapping->tab_bits, 0, nb_bits * sizeof(uint8_t));
1963 }
1964
1965 /* 1X */
1966 mb_mapping->nb_input_bits = nb_input_bits;
1967 mb_mapping->start_input_bits = start_input_bits;
1968 if (nb_input_bits == 0) {
1969 mb_mapping->tab_input_bits = NULL;
1970 } else {
1971 mb_mapping->tab_input_bits = (uint8_t *) malloc(nb_input_bits * sizeof(uint8_t));
1972 if (mb_mapping->tab_input_bits == NULL) {
1973 free(mb_mapping->tab_bits);
1974 free(mb_mapping);
1975 return NULL;
1976 }
1977 memset(mb_mapping->tab_input_bits, 0, nb_input_bits * sizeof(uint8_t));
1978 }
1979
1980 /* 4X */
1981 mb_mapping->nb_registers = nb_registers;
1982 mb_mapping->start_registers = start_registers;
1983 if (nb_registers == 0) {
1984 mb_mapping->tab_registers = NULL;
1985 } else {
1986 mb_mapping->tab_registers = (uint16_t *) malloc(nb_registers * sizeof(uint16_t));
1987 if (mb_mapping->tab_registers == NULL) {
1988 free(mb_mapping->tab_input_bits);
1989 free(mb_mapping->tab_bits);
1990 free(mb_mapping);
1991 return NULL;
1992 }
1993 memset(mb_mapping->tab_registers, 0, nb_registers * sizeof(uint16_t));
1994 }
1995
1996 /* 3X */
1997 mb_mapping->nb_input_registers = nb_input_registers;
1998 mb_mapping->start_input_registers = start_input_registers;
1999 if (nb_input_registers == 0) {
2000 mb_mapping->tab_input_registers = NULL;
2001 } else {
2002 mb_mapping->tab_input_registers =
2003 (uint16_t *) malloc(nb_input_registers * sizeof(uint16_t));
2004 if (mb_mapping->tab_input_registers == NULL) {
2005 free(mb_mapping->tab_registers);
2006 free(mb_mapping->tab_input_bits);
2007 free(mb_mapping->tab_bits);
2008 free(mb_mapping);
2009 return NULL;
2010 }
2011 memset(mb_mapping->tab_input_registers, 0, nb_input_registers * sizeof(uint16_t));
2012 }
2013
2014 return mb_mapping;
2015 }
2016
2017 modbus_mapping_t *modbus_mapping_new(int nb_bits,
2018 int nb_input_bits,
2019 int nb_registers,
2020 int nb_input_registers)
2021 {
2022 return modbus_mapping_new_start_address(
2023 0, nb_bits, 0, nb_input_bits, 0, nb_registers, 0, nb_input_registers);
2024 }
2025
2026 /* Frees the 4 arrays */
2027 void modbus_mapping_free(modbus_mapping_t *mb_mapping)
2028 {
2029 if (mb_mapping == NULL) {
2030 return;
2031 }
2032
2033 free(mb_mapping->tab_input_registers);
2034 free(mb_mapping->tab_registers);
2035 free(mb_mapping->tab_input_bits);
2036 free(mb_mapping->tab_bits);
2037 free(mb_mapping);
2038 }
2039
2040 #ifndef HAVE_STRLCPY
2041 /*
2042 * Function strlcpy was originally developed by
2043 * Todd C. Miller <Todd.Miller@courtesan.com> to simplify writing secure code.
2044 * See ftp://ftp.openbsd.org/pub/OpenBSD/src/lib/libc/string/strlcpy.3
2045 * for more information.
2046 *
2047 * Thank you Ulrich Drepper... not!
2048 *
2049 * Copy src to string dest of size dest_size. At most dest_size-1 characters
2050 * will be copied. Always NUL terminates (unless dest_size == 0). Returns
2051 * strlen(src); if retval >= dest_size, truncation occurred.
2052 */
2053 size_t strlcpy(char *dest, const char *src, size_t dest_size)
2054 {
2055 register char *d = dest;
2056 register const char *s = src;
2057 register size_t n = dest_size;
2058
2059 /* Copy as many bytes as will fit */
2060 if (n != 0 && --n != 0) {
2061 do {
2062 if ((*d++ = *s++) == 0)
2063 break;
2064 } while (--n != 0);
2065 }
2066
2067 /* Not enough room in dest, add NUL and traverse rest of src */
2068 if (n == 0) {
2069 if (dest_size != 0)
2070 *d = '\0'; /* NUL-terminate dest */
2071 while (*s++)
2072 ;
2073 }
2074
2075 return (s - src - 1); /* count does not include NUL */
2076 }
2077 #endif
libmodbus mirror