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.

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.
Stéphane Raimbault
1 parent ac6ba5c1
Showing 7 changed files with 94 additions and 55 deletions
configure.ac
src/modbus-rtu.c
src/modbus-tcp.c
src/modbus.c
src/modbus.h
tests/unit-test-client.c
tests/unit-test-server.c
@@ -66,6 +66,7 @@ LT_INIT([disable-static win32-dll])
 AC_CHECK_HEADERS([ \
     termios.h \
     sys/time.h \
+    time.h \
     unistd.h \
     errno.h \
     limits.h \
@@ -297,7 +297,7 @@ int _modbus_rtu_check_integrity(modbus_t *ctx, uint8_t *msg,
             fprintf(stderr, "ERROR CRC received %0X != CRC calculated %0X\n",
                     crc_received, crc_calculated);
         }
-        if (ctx->error_recovery) {
+        if (ctx->error_recovery & MODBUS_ERROR_RECOVERY_PROTOCOL) {
             _modbus_rtu_flush(ctx);
         }
         errno = EMBBADCRC;
@@ -790,15 +790,7 @@ int _modbus_rtu_select(modbus_t *ctx, fd_set *rfds,
     }
     if (s_rc < 0) {
-        _error_print(ctx, "select");
-        if (ctx->error_recovery && (errno == EBADF)) {
-            modbus_close(ctx);
-            modbus_connect(ctx);
-            errno = EBADF;
-            return -1;
-        } else {
-            return -1;
-        }
+        return -1;
     }
 #else
     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,
             FD_ZERO(rfds);
             FD_SET(ctx->s, rfds);
         } else {
-            _error_print(ctx, "select");
-            if (ctx->error_recovery && (errno == EBADF)) {
-                modbus_close(ctx);
-                modbus_connect(ctx);
-                errno = EBADF;
-                return -1;
-            } else {
-                return -1;
-            }
+            return -1;
         }
     }
     if (s_rc == 0) {
         /* Timeout */
         errno = ETIMEDOUT;
-        _error_print(ctx, "select");
         return -1;
     }
 #endif
@@ -342,6 +342,7 @@ void _modbus_tcp_close(modbus_t *ctx)
 int _modbus_tcp_flush(modbus_t *ctx)
 {
     int rc;
+    int rc_sum = 0;
     do {
         /* Extract the garbage from the socket */
@@ -367,12 +368,12 @@ int _modbus_tcp_flush(modbus_t *ctx)
             rc = recv(ctx->s, devnull, MODBUS_TCP_MAX_ADU_LENGTH, 0);
         }
 #endif
-        if (ctx->debug && rc != -1) {
-            printf("%d bytes flushed\n", rc);
+        if (rc > 0) {
+            rc_sum += rc;
         }
     } while (rc == MODBUS_TCP_MAX_ADU_LENGTH);
-    return rc;
+    return rc_sum;
 }
 /* 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
             FD_ZERO(rfds);
             FD_SET(ctx->s, rfds);
         } else {
-            _error_print(ctx, "select");
-            if (ctx->error_recovery && (errno == EBADF)) {
-                modbus_close(ctx);
-                modbus_connect(ctx);
-                errno = EBADF;
-                return -1;
-            } else {
-                return -1;
-            }
+            return -1;
         }
     }
     if (s_rc == 0) {
         errno = ETIMEDOUT;
-        _error_print(ctx, "select");
         return -1;
     }
@@ -22,11 +22,9 @@
 #include <stdio.h>
 #include <string.h>
 #include <stdlib.h>
-#ifndef _MSC_VER
-#include <unistd.h>
-#endif
 #include <errno.h>
 #include <limits.h>
+#include <time.h>
 #include <config.h>
@@ -98,9 +96,31 @@ void _error_print(modbus_t *ctx, const char *context)
     }
 }
+int _sleep_and_flush(modbus_t *ctx)
+{
+#ifdef _WIN32
+    /* usleep doesn't exist on Windows */
+    Sleep((ctx->response_timeout.tv_sec * 1000) +
+          (ctx->response_timeout.tv_usec / 1000));
+#else
+    /* usleep source code */
+    struct timespec request, remaining;
+    request.tv_sec = ctx->response_timeout.tv_sec;
+    request.tv_nsec = ((long int)ctx->response_timeout.tv_usec % 1000000)
+        * 1000;
+    while (nanosleep(&request, &remaining) == -1 && errno == EINTR)
+        request = remaining;
+#endif
+    return modbus_flush(ctx);
+}
+
 int modbus_flush(modbus_t *ctx)
 {
-    return ctx->backend->flush(ctx);
+    int rc = ctx->backend->flush(ctx);
+    if (rc != -1 && ctx->debug) {
+        printf("%d bytes flushed\n", rc);
+    }
+    return rc;
 }
 /* Computes the length of the expected response */
@@ -157,13 +177,20 @@ static int send_msg(modbus_t *ctx, uint8_t *msg, int msg_length)
         rc = ctx->backend->send(ctx, msg, msg_length);
         if (rc == -1) {
             _error_print(ctx, NULL);
-            if (ctx->error_recovery &&
-                (errno == EBADF || errno == ECONNRESET || errno == EPIPE)) {
-                modbus_close(ctx);
-                modbus_connect(ctx);
+            if (ctx->error_recovery & MODBUS_ERROR_RECOVERY_LINK) {
+                int saved_errno = errno;
+
+                if ((errno == EBADF || errno == ECONNRESET || errno == EPIPE)) {
+                    modbus_close(ctx);
+                    modbus_connect(ctx);
+                } else {
+                    _sleep_and_flush(ctx);
+                }
+                errno = saved_errno;
             }
         }
-    } while (ctx->error_recovery && rc == -1);
+    } while ((ctx->error_recovery & MODBUS_ERROR_RECOVERY_LINK) &&
+             rc == -1);
     if (rc > 0 && rc != msg_length) {
         errno = EMBBADDATA;
@@ -339,6 +366,18 @@ static int receive_msg(modbus_t *ctx, uint8_t *msg, msg_type_t msg_type)
     while (length_to_read != 0) {
         rc = ctx->backend->select(ctx, &rfds, p_tv, length_to_read);
         if (rc == -1) {
+            _error_print(ctx, "select");
+            if (ctx->error_recovery & MODBUS_ERROR_RECOVERY_LINK) {
+                int saved_errno = errno;
+
+                if (errno == ETIMEDOUT) {
+                    _sleep_and_flush(ctx);
+                } else if (errno == EBADF) {
+                    modbus_close(ctx);
+                    modbus_connect(ctx);
+                }
+                errno = saved_errno;
+            }
             return -1;
         }
@@ -350,12 +389,14 @@ static int receive_msg(modbus_t *ctx, uint8_t *msg, msg_type_t msg_type)
         if (rc == -1) {
             _error_print(ctx, "read");
-            if (ctx->error_recovery && (errno == ECONNRESET ||
-                                        errno == ECONNREFUSED)) {
+            if ((ctx->error_recovery & MODBUS_ERROR_RECOVERY_LINK) &&
+                (errno == ECONNRESET || errno == ECONNREFUSED ||
+                 errno == EBADF)) {
+                int saved_errno = errno;
                 modbus_close(ctx);
                 modbus_connect(ctx);
                 /* Could be removed by previous calls */
-                errno = ECONNRESET;
+                errno = saved_errno;
             }
             return -1;
         }
@@ -440,10 +481,12 @@ static int check_confirmation(modbus_t *ctx, uint8_t *req,
     int rsp_length_computed;
     const int offset = ctx->backend->header_length;
-
     if (ctx->backend->pre_check_confirmation) {
         rc = ctx->backend->pre_check_confirmation(ctx, req, rsp, rsp_length);
         if (rc == -1) {
+            if (ctx->error_recovery & MODBUS_ERROR_RECOVERY_PROTOCOL) {
+                _sleep_and_flush(ctx);
+            }
             return -1;
         }
     }
@@ -464,6 +507,9 @@ static int check_confirmation(modbus_t *ctx, uint8_t *req,
                         "Received function not corresponding to the request (%d != %d)\n",
                         function, req[offset]);
             }
+            if (ctx->error_recovery & MODBUS_ERROR_RECOVERY_PROTOCOL) {
+                _sleep_and_flush(ctx);
+            }
             errno = EMBBADDATA;
             return -1;
         }
@@ -509,6 +555,11 @@ static int check_confirmation(modbus_t *ctx, uint8_t *req,
                         "Quantity not corresponding to the request (%d != %d)\n",
                         rsp_nb_value, req_nb_value);
             }
+
+            if (ctx->error_recovery & MODBUS_ERROR_RECOVERY_PROTOCOL) {
+                _sleep_and_flush(ctx);
+            }
+
             errno = EMBBADDATA;
             rc = -1;
         }
@@ -530,6 +581,9 @@ static int check_confirmation(modbus_t *ctx, uint8_t *req,
                     "Message length not corresponding to the computed length (%d != %d)\n",
                     rsp_length, rsp_length_computed);
         }
+        if (ctx->error_recovery & MODBUS_ERROR_RECOVERY_PROTOCOL) {
+            _sleep_and_flush(ctx);
+        }
         errno = EMBBADDATA;
         rc = -1;
     }
@@ -1320,7 +1374,7 @@ void _modbus_init_common(modbus_t *ctx)
     ctx->s = -1;
     ctx->debug = FALSE;
-    ctx->error_recovery = FALSE;
+    ctx->error_recovery = MODBUS_ERROR_RECOVERY_NONE;
     ctx->response_timeout.tv_sec = 0;
     ctx->response_timeout.tv_usec = _RESPONSE_TIMEOUT;
@@ -1335,10 +1389,11 @@ int modbus_set_slave(modbus_t *ctx, int slave)
     return ctx->backend->set_slave(ctx, slave);
 }
-int modbus_set_error_recovery(modbus_t *ctx, int enabled)
+int modbus_set_error_recovery(modbus_t *ctx,
+                              modbus_error_recovery_mode error_recovery)
 {
-    if (enabled == TRUE || enabled == FALSE) {
-        ctx->error_recovery = (uint8_t) enabled;
+    if (error_recovery >= 0) {
+        ctx->error_recovery = (uint8_t) error_recovery;
     } else {
         errno = EINVAL;
         return -1;
@@ -134,8 +134,15 @@ typedef struct {
     uint16_t *tab_registers;
 } modbus_mapping_t;
+typedef enum
+{
+    MODBUS_ERROR_RECOVERY_NONE          = 0,
+    MODBUS_ERROR_RECOVERY_LINK          = (1<<1),
+    MODBUS_ERROR_RECOVERY_PROTOCOL      = (1<<2),
+} modbus_error_recovery_type;
+
 int modbus_set_slave(modbus_t* ctx, int slave);
-int modbus_set_error_recovery(modbus_t *ctx, int enabled);
+int modbus_set_error_recovery(modbus_t *ctx, modbus_error_recovery_type error_recovery);
 void modbus_set_socket(modbus_t *ctx, int socket);
 int modbus_get_socket(modbus_t *ctx);
@@ -73,6 +73,9 @@ int main(int argc, char *argv[])
         return -1;
     }
     modbus_set_debug(ctx, TRUE);
+    modbus_set_error_recovery(ctx,
+                              MODBUS_ERROR_RECOVERY_LINK |
+                              MODBUS_ERROR_RECOVERY_PROTOCOL);
     if (use_backend == RTU) {
           modbus_set_slave(ctx, SERVER_ID);
@@ -587,9 +590,8 @@ int main(int argc, char *argv[])
     /* Restore original timeout */
     modbus_set_response_timeout(ctx, &old_response_timeout);
-    /* Wait for data before flushing */
-    usleep(500000);
-    modbus_flush(ctx);
+    /* A wait and flush operation is done by the error recovery code of
+     * libmodbus */
     /** BAD RESPONSE **/
     printf("\nTEST BAD RESPONSE ERROR:\n");
@@ -71,7 +71,6 @@ int main(int argc, char*argv[])
     header_length = modbus_get_header_length(ctx);
     modbus_set_debug(ctx, TRUE);
-    modbus_set_error_recovery(ctx, TRUE);
     mb_mapping = modbus_mapping_new(
         UT_BITS_ADDRESS + UT_BITS_NB,