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