Peter M. Groen / libmodbus

Commit d1f1854338bbf480356fd074f4ca14b51d88f224

Authored by Stéphane Raimbault
1 parent ac6ba5c1

New error recovery modes: link and protocol 

The two modes are complementary, MODBUS_ERROR_RECOVERY_LINK handles
errors at data link level (bad file descriptor, timeout, etc) and
MODBUS_ERROR_RECOVERY_PROTOCOL checks Modbus error (eg. invalid
function code or trame length).

This change introduces the use of the Sleep function for Windows.
Some duplicated code has been moved from backends to modbus core.
A new debug message is now available when a flush occurs.

The unit tests are now based on this error recovery code.
Inline Side-by-side
Showing 7 changed files with 94 additions and 55 deletions
configure.ac
  View file @d1f1854
... ... @@ -66,6 +66,7 @@ LT_INIT([disable-static win32-dll])
66 66 AC_CHECK_HEADERS([ \
67 67 termios.h \
68 68 sys/time.h \
  69 + time.h \
69 70 unistd.h \
70 71 errno.h \
71 72 limits.h \
... ...
src/modbus-rtu.c
  View file @d1f1854
... ... @@ -297,7 +297,7 @@ int _modbus_rtu_check_integrity(modbus_t *ctx, uint8_t *msg,
297 297 fprintf(stderr, "ERROR CRC received %0X != CRC calculated %0X\n",
298 298 crc_received, crc_calculated);
299 299 }
300   - if (ctx->error_recovery) {
  300 + if (ctx->error_recovery & MODBUS_ERROR_RECOVERY_PROTOCOL) {
301 301 _modbus_rtu_flush(ctx);
302 302 }
303 303 errno = EMBBADCRC;
... ... @@ -790,15 +790,7 @@ int _modbus_rtu_select(modbus_t *ctx, fd_set *rfds,
790 790 }
791 791  
792 792 if (s_rc < 0) {
793   - _error_print(ctx, "select");
794   - if (ctx->error_recovery && (errno == EBADF)) {
795   - modbus_close(ctx);
796   - modbus_connect(ctx);
797   - errno = EBADF;
798   - return -1;
799   - } else {
800   - return -1;
801   - }
  793 + return -1;
802 794 }
803 795 #else
804 796 while ((s_rc = select(ctx->s+1, rfds, NULL, NULL, tv)) == -1) {
... ... @@ -810,22 +802,13 @@ int _modbus_rtu_select(modbus_t *ctx, fd_set *rfds,
810 802 FD_ZERO(rfds);
811 803 FD_SET(ctx->s, rfds);
812 804 } else {
813   - _error_print(ctx, "select");
814   - if (ctx->error_recovery && (errno == EBADF)) {
815   - modbus_close(ctx);
816   - modbus_connect(ctx);
817   - errno = EBADF;
818   - return -1;
819   - } else {
820   - return -1;
821   - }
  805 + return -1;
822 806 }
823 807 }
824 808  
825 809 if (s_rc == 0) {
826 810 /* Timeout */
827 811 errno = ETIMEDOUT;
828   - _error_print(ctx, "select");
829 812 return -1;
830 813 }
831 814 #endif
... ...
src/modbus-tcp.c
  View file @d1f1854
... ... @@ -342,6 +342,7 @@ void _modbus_tcp_close(modbus_t *ctx)
342 342 int _modbus_tcp_flush(modbus_t *ctx)
343 343 {
344 344 int rc;
  345 + int rc_sum = 0;
345 346  
346 347 do {
347 348 /* Extract the garbage from the socket */
... ... @@ -367,12 +368,12 @@ int _modbus_tcp_flush(modbus_t *ctx)
367 368 rc = recv(ctx->s, devnull, MODBUS_TCP_MAX_ADU_LENGTH, 0);
368 369 }
369 370 #endif
370   - if (ctx->debug && rc != -1) {
371   - printf("%d bytes flushed\n", rc);
  371 + if (rc > 0) {
  372 + rc_sum += rc;
372 373 }
373 374 } while (rc == MODBUS_TCP_MAX_ADU_LENGTH);
374 375  
375   - return rc;
  376 + return rc_sum;
376 377 }
377 378  
378 379 /* Listens for any request from one or many modbus masters in TCP */
... ... @@ -565,21 +566,12 @@ int _modbus_tcp_select(modbus_t *ctx, fd_set *rfds, struct timeval *tv, int leng
565 566 FD_ZERO(rfds);
566 567 FD_SET(ctx->s, rfds);
567 568 } else {
568   - _error_print(ctx, "select");
569   - if (ctx->error_recovery && (errno == EBADF)) {
570   - modbus_close(ctx);
571   - modbus_connect(ctx);
572   - errno = EBADF;
573   - return -1;
574   - } else {
575   - return -1;
576   - }
  569 + return -1;
577 570 }
578 571 }
579 572  
580 573 if (s_rc == 0) {
581 574 errno = ETIMEDOUT;
582   - _error_print(ctx, "select");
583 575 return -1;
584 576 }
585 577  
... ...
src/modbus.c
  View file @d1f1854
... ... @@ -22,11 +22,9 @@
22 22 #include <stdio.h>
23 23 #include <string.h>
24 24 #include <stdlib.h>
25   -#ifndef _MSC_VER
26   -#include <unistd.h>
27   -#endif
28 25 #include <errno.h>
29 26 #include <limits.h>
  27 +#include <time.h>
30 28  
31 29 #include <config.h>
32 30  
... ... @@ -98,9 +96,31 @@ void _error_print(modbus_t *ctx, const char *context)
98 96 }
99 97 }
100 98  
  99 +int _sleep_and_flush(modbus_t *ctx)
  100 +{
  101 +#ifdef _WIN32
  102 + /* usleep doesn't exist on Windows */
  103 + Sleep((ctx->response_timeout.tv_sec * 1000) +
  104 + (ctx->response_timeout.tv_usec / 1000));
  105 +#else
  106 + /* usleep source code */
  107 + struct timespec request, remaining;
  108 + request.tv_sec = ctx->response_timeout.tv_sec;
  109 + request.tv_nsec = ((long int)ctx->response_timeout.tv_usec % 1000000)
  110 + * 1000;
  111 + while (nanosleep(&request, &remaining) == -1 && errno == EINTR)
  112 + request = remaining;
  113 +#endif
  114 + return modbus_flush(ctx);
  115 +}
  116 +
101 117 int modbus_flush(modbus_t *ctx)
102 118 {
103   - return ctx->backend->flush(ctx);
  119 + int rc = ctx->backend->flush(ctx);
  120 + if (rc != -1 && ctx->debug) {
  121 + printf("%d bytes flushed\n", rc);
  122 + }
  123 + return rc;
104 124 }
105 125  
106 126 /* Computes the length of the expected response */
... ... @@ -157,13 +177,20 @@ static int send_msg(modbus_t *ctx, uint8_t *msg, int msg_length)
157 177 rc = ctx->backend->send(ctx, msg, msg_length);
158 178 if (rc == -1) {
159 179 _error_print(ctx, NULL);
160   - if (ctx->error_recovery &&
161   - (errno == EBADF || errno == ECONNRESET || errno == EPIPE)) {
162   - modbus_close(ctx);
163   - modbus_connect(ctx);
  180 + if (ctx->error_recovery & MODBUS_ERROR_RECOVERY_LINK) {
  181 + int saved_errno = errno;
  182 +
  183 + if ((errno == EBADF || errno == ECONNRESET || errno == EPIPE)) {
  184 + modbus_close(ctx);
  185 + modbus_connect(ctx);
  186 + } else {
  187 + _sleep_and_flush(ctx);
  188 + }
  189 + errno = saved_errno;
164 190 }
165 191 }
166   - } while (ctx->error_recovery && rc == -1);
  192 + } while ((ctx->error_recovery & MODBUS_ERROR_RECOVERY_LINK) &&
  193 + rc == -1);
167 194  
168 195 if (rc > 0 && rc != msg_length) {
169 196 errno = EMBBADDATA;
... ... @@ -339,6 +366,18 @@ static int receive_msg(modbus_t *ctx, uint8_t *msg, msg_type_t msg_type)
339 366 while (length_to_read != 0) {
340 367 rc = ctx->backend->select(ctx, &rfds, p_tv, length_to_read);
341 368 if (rc == -1) {
  369 + _error_print(ctx, "select");
  370 + if (ctx->error_recovery & MODBUS_ERROR_RECOVERY_LINK) {
  371 + int saved_errno = errno;
  372 +
  373 + if (errno == ETIMEDOUT) {
  374 + _sleep_and_flush(ctx);
  375 + } else if (errno == EBADF) {
  376 + modbus_close(ctx);
  377 + modbus_connect(ctx);
  378 + }
  379 + errno = saved_errno;
  380 + }
342 381 return -1;
343 382 }
344 383  
... ... @@ -350,12 +389,14 @@ static int receive_msg(modbus_t *ctx, uint8_t *msg, msg_type_t msg_type)
350 389  
351 390 if (rc == -1) {
352 391 _error_print(ctx, "read");
353   - if (ctx->error_recovery && (errno == ECONNRESET ||
354   - errno == ECONNREFUSED)) {
  392 + if ((ctx->error_recovery & MODBUS_ERROR_RECOVERY_LINK) &&
  393 + (errno == ECONNRESET || errno == ECONNREFUSED ||
  394 + errno == EBADF)) {
  395 + int saved_errno = errno;
355 396 modbus_close(ctx);
356 397 modbus_connect(ctx);
357 398 /* Could be removed by previous calls */
358   - errno = ECONNRESET;
  399 + errno = saved_errno;
359 400 }
360 401 return -1;
361 402 }
... ... @@ -440,10 +481,12 @@ static int check_confirmation(modbus_t *ctx, uint8_t *req,
440 481 int rsp_length_computed;
441 482 const int offset = ctx->backend->header_length;
442 483  
443   -
444 484 if (ctx->backend->pre_check_confirmation) {
445 485 rc = ctx->backend->pre_check_confirmation(ctx, req, rsp, rsp_length);
446 486 if (rc == -1) {
  487 + if (ctx->error_recovery & MODBUS_ERROR_RECOVERY_PROTOCOL) {
  488 + _sleep_and_flush(ctx);
  489 + }
447 490 return -1;
448 491 }
449 492 }
... ... @@ -464,6 +507,9 @@ static int check_confirmation(modbus_t *ctx, uint8_t *req,
464 507 "Received function not corresponding to the request (%d != %d)\n",
465 508 function, req[offset]);
466 509 }
  510 + if (ctx->error_recovery & MODBUS_ERROR_RECOVERY_PROTOCOL) {
  511 + _sleep_and_flush(ctx);
  512 + }
467 513 errno = EMBBADDATA;
468 514 return -1;
469 515 }
... ... @@ -509,6 +555,11 @@ static int check_confirmation(modbus_t *ctx, uint8_t *req,
509 555 "Quantity not corresponding to the request (%d != %d)\n",
510 556 rsp_nb_value, req_nb_value);
511 557 }
  558 +
  559 + if (ctx->error_recovery & MODBUS_ERROR_RECOVERY_PROTOCOL) {
  560 + _sleep_and_flush(ctx);
  561 + }
  562 +
512 563 errno = EMBBADDATA;
513 564 rc = -1;
514 565 }
... ... @@ -530,6 +581,9 @@ static int check_confirmation(modbus_t *ctx, uint8_t *req,
530 581 "Message length not corresponding to the computed length (%d != %d)\n",
531 582 rsp_length, rsp_length_computed);
532 583 }
  584 + if (ctx->error_recovery & MODBUS_ERROR_RECOVERY_PROTOCOL) {
  585 + _sleep_and_flush(ctx);
  586 + }
533 587 errno = EMBBADDATA;
534 588 rc = -1;
535 589 }
... ... @@ -1320,7 +1374,7 @@ void _modbus_init_common(modbus_t *ctx)
1320 1374 ctx->s = -1;
1321 1375  
1322 1376 ctx->debug = FALSE;
1323   - ctx->error_recovery = FALSE;
  1377 + ctx->error_recovery = MODBUS_ERROR_RECOVERY_NONE;
1324 1378  
1325 1379 ctx->response_timeout.tv_sec = 0;
1326 1380 ctx->response_timeout.tv_usec = _RESPONSE_TIMEOUT;
... ... @@ -1335,10 +1389,11 @@ int modbus_set_slave(modbus_t *ctx, int slave)
1335 1389 return ctx->backend->set_slave(ctx, slave);
1336 1390 }
1337 1391  
1338   -int modbus_set_error_recovery(modbus_t *ctx, int enabled)
  1392 +int modbus_set_error_recovery(modbus_t *ctx,
  1393 + modbus_error_recovery_mode error_recovery)
1339 1394 {
1340   - if (enabled == TRUE || enabled == FALSE) {
1341   - ctx->error_recovery = (uint8_t) enabled;
  1395 + if (error_recovery >= 0) {
  1396 + ctx->error_recovery = (uint8_t) error_recovery;
1342 1397 } else {
1343 1398 errno = EINVAL;
1344 1399 return -1;
... ...
src/modbus.h
  View file @d1f1854
... ... @@ -134,8 +134,15 @@ typedef struct {
134 134 uint16_t *tab_registers;
135 135 } modbus_mapping_t;
136 136  
  137 +typedef enum
  138 +{
  139 + MODBUS_ERROR_RECOVERY_NONE = 0,
  140 + MODBUS_ERROR_RECOVERY_LINK = (1<<1),
  141 + MODBUS_ERROR_RECOVERY_PROTOCOL = (1<<2),
  142 +} modbus_error_recovery_type;
  143 +
137 144 int modbus_set_slave(modbus_t* ctx, int slave);
138   -int modbus_set_error_recovery(modbus_t *ctx, int enabled);
  145 +int modbus_set_error_recovery(modbus_t *ctx, modbus_error_recovery_type error_recovery);
139 146 void modbus_set_socket(modbus_t *ctx, int socket);
140 147 int modbus_get_socket(modbus_t *ctx);
141 148  
... ...
tests/unit-test-client.c
  View file @d1f1854
... ... @@ -73,6 +73,9 @@ int main(int argc, char *argv[])
73 73 return -1;
74 74 }
75 75 modbus_set_debug(ctx, TRUE);
  76 + modbus_set_error_recovery(ctx,
  77 + MODBUS_ERROR_RECOVERY_LINK |
  78 + MODBUS_ERROR_RECOVERY_PROTOCOL);
76 79  
77 80 if (use_backend == RTU) {
78 81 modbus_set_slave(ctx, SERVER_ID);
... ... @@ -587,9 +590,8 @@ int main(int argc, char *argv[])
587 590 /* Restore original timeout */
588 591 modbus_set_response_timeout(ctx, &old_response_timeout);
589 592  
590   - /* Wait for data before flushing */
591   - usleep(500000);
592   - modbus_flush(ctx);
  593 + /* A wait and flush operation is done by the error recovery code of
  594 + * libmodbus */
593 595  
594 596 /** BAD RESPONSE **/
595 597 printf("\nTEST BAD RESPONSE ERROR:\n");
... ...
tests/unit-test-server.c
  View file @d1f1854
... ... @@ -71,7 +71,6 @@ int main(int argc, char*argv[])
71 71 header_length = modbus_get_header_length(ctx);
72 72  
73 73 modbus_set_debug(ctx, TRUE);
74   - modbus_set_error_recovery(ctx, TRUE);
75 74  
76 75 mb_mapping = modbus_mapping_new(
77 76 UT_BITS_ADDRESS + UT_BITS_NB,
... ...