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