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