diff --git a/configure.ac b/configure.ac index 810d5a8..06ae00c 100644 --- a/configure.ac +++ b/configure.ac @@ -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 \ diff --git a/src/modbus-rtu.c b/src/modbus-rtu.c index 0b6eeac..2cbff02 100644 --- a/src/modbus-rtu.c +++ b/src/modbus-rtu.c @@ -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 diff --git a/src/modbus-tcp.c b/src/modbus-tcp.c index 0db9c9e..efd4658 100644 --- a/src/modbus-tcp.c +++ b/src/modbus-tcp.c @@ -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; } diff --git a/src/modbus.c b/src/modbus.c index 131b903..0c3f35c 100644 --- a/src/modbus.c +++ b/src/modbus.c @@ -22,11 +22,9 @@ #include #include #include -#ifndef _MSC_VER -#include -#endif #include #include +#include #include @@ -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; diff --git a/src/modbus.h b/src/modbus.h index f686094..75b0b73 100644 --- a/src/modbus.h +++ b/src/modbus.h @@ -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); diff --git a/tests/unit-test-client.c b/tests/unit-test-client.c index f043235..90128e5 100644 --- a/tests/unit-test-client.c +++ b/tests/unit-test-client.c @@ -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"); diff --git a/tests/unit-test-server.c b/tests/unit-test-server.c index 5a2d5eb..9a986c5 100644 --- a/tests/unit-test-server.c +++ b/tests/unit-test-server.c @@ -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,