Skip to content

fix: keep waiting after unexpected RTU slave response #860

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
MrAlaskan wants to merge 1 commit into
base: master
Choose a base branch
from
Open

fix: keep waiting after unexpected RTU slave response #860

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
150 changes: 81 additions & 69 deletions src/modbus.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -108,6 +108,29 @@ static void _sleep_response_timeout(modbus_t *ctx)
#endif
}

static uint64_t _timeval_to_usec(const struct timeval *tv)
{
return ((uint64_t) tv->tv_sec * 1000000U) + (uint64_t) tv->tv_usec;
}

static void _timeval_from_usec(uint64_t usec, struct timeval *tv)
{
tv->tv_sec = (long) (usec / 1000000U);
tv->tv_usec = (long) (usec % 1000000U);
}

static uint64_t _get_monotonic_time_usec(void)
{
#if defined(HAVE_GETTIMEOFDAY)
struct timeval tv;

gettimeofday(&tv, NULL);
return _timeval_to_usec(&tv);
#else
return (uint64_t) time(NULL) * 1000000U;
#endif
}

int modbus_flush(modbus_t *ctx)
{
int rc;
Expand Down Expand Up @@ -572,6 +595,9 @@ static int check_confirmation(modbus_t *ctx, uint8_t *req, uint8_t *rsp, int rsp
if (ctx->backend->pre_check_confirmation) {
rc = ctx->backend->pre_check_confirmation(ctx, req, rsp, rsp_length);
if (rc == -1) {
if (errno == EMBBADSLAVE) {
return -1;
}
if (ctx->error_recovery & MODBUS_ERROR_RECOVERY_PROTOCOL) {
_sleep_response_timeout(ctx);
modbus_flush(ctx);
Expand Down Expand Up @@ -721,6 +747,51 @@ static int check_confirmation(modbus_t *ctx, uint8_t *req, uint8_t *rsp, int rsp
return rc;
}

static int receive_confirmation(modbus_t *ctx, uint8_t *req, uint8_t *rsp)
{
int rc;
int saved_errno = 0;
const struct timeval original_response_timeout = ctx->response_timeout;
const uint64_t deadline =
_get_monotonic_time_usec() + _timeval_to_usec(&original_response_timeout);

for (;;) {
rc = _modbus_receive_msg(ctx, rsp, MSG_CONFIRMATION);
if (rc == -1) {
saved_errno = errno;
break;
}

rc = check_confirmation(ctx, req, rsp, rc);
if (rc != -1) {
break;
}

if (errno != EMBBADSLAVE) {
saved_errno = errno;
break;
}

{
const uint64_t now = _get_monotonic_time_usec();

if (now >= deadline) {
saved_errno = ETIMEDOUT;
rc = -1;
break;
}

_timeval_from_usec(deadline - now, &ctx->response_timeout);
}
}

ctx->response_timeout = original_response_timeout;
if (rc == -1) {
errno = saved_errno;
}
return rc;
}

static int
response_io_status(uint8_t *tab_io_status, int address, int nb, uint8_t *rsp, int offset)
{
Expand Down Expand Up @@ -1359,8 +1430,8 @@ int modbus_proxy(modbus_t *frontend_ctx,
return -1;
}

/* Receive the response from the backend */
backend_rsp_length = _modbus_receive_msg(backend_ctx, backend_rsp, MSG_CONFIRMATION);
/* Receive and validate the response from the backend. */
backend_rsp_length = receive_confirmation(backend_ctx, backend_req, backend_rsp);
if (backend_rsp_length == -1) {
if (errno == ETIMEDOUT)
modbus_reply_exception(
Expand All @@ -1371,33 +1442,6 @@ int modbus_proxy(modbus_t *frontend_ctx,
return -1;
}

/* Check the backend response integrity */
if (backend_ctx->backend->pre_check_confirmation) {
/* Build a minimal req for the pre_check (only header matters for TID/slave check).
We reuse the backend_req buffer which still holds our sent data. */
uint8_t check_req[MAX_MESSAGE_LENGTH];
int check_req_length;
sft_t check_sft;

check_sft.slave = sft.slave;
check_sft.function = sft.function;
check_sft.t_id = 0;
check_req_length = backend_ctx->backend->build_response_basis(&check_sft, check_req);
/* Append enough data so the length is valid */
if (backend_req_length > 2) {
memcpy(check_req + check_req_length, backend_req + 2, backend_req_length - 2);
check_req_length += backend_req_length - 2;
}

rc = backend_ctx->backend->pre_check_confirmation(
backend_ctx, check_req, backend_rsp, backend_rsp_length);
if (rc == -1) {
modbus_reply_exception(
frontend_ctx, req, MODBUS_EXCEPTION_GATEWAY_TARGET);
return -1;
}
}

/* Extract the PDU from the backend response and wrap it for the frontend.
Backend response layout: [header][function][data...][checksum] */
pdu_length =
Expand Down Expand Up @@ -1448,11 +1492,7 @@ static int read_io_status(modbus_t *ctx, int function, int addr, int nb, uint8_t
unsigned int offset_end;
unsigned int i;

rc = _modbus_receive_msg(ctx, rsp, MSG_CONFIRMATION);
if (rc == -1)
return -1;

rc = check_confirmation(ctx, req, rsp, rc);
rc = receive_confirmation(ctx, req, rsp);
if (rc == -1)
return -1;

Expand Down Expand Up @@ -1557,11 +1597,7 @@ static int read_registers(modbus_t *ctx, int function, int addr, int nb, uint16_
unsigned int offset;
int i;

rc = _modbus_receive_msg(ctx, rsp, MSG_CONFIRMATION);
if (rc == -1)
return -1;

rc = check_confirmation(ctx, req, rsp, rc);
rc = receive_confirmation(ctx, req, rsp);
if (rc == -1)
return -1;

Expand Down Expand Up @@ -1648,11 +1684,7 @@ static int write_single(modbus_t *ctx, int function, int addr, const uint16_t va
/* Used by write_bit and write_register */
uint8_t rsp[MAX_MESSAGE_LENGTH];

rc = _modbus_receive_msg(ctx, rsp, MSG_CONFIRMATION);
if (rc == -1)
return -1;

rc = check_confirmation(ctx, req, rsp, rc);
rc = receive_confirmation(ctx, req, rsp);
}

return rc;
Expand Down Expand Up @@ -1733,11 +1765,7 @@ int modbus_write_bits(modbus_t *ctx, int addr, int nb, const uint8_t *src)
if (rc > 0) {
uint8_t rsp[MAX_MESSAGE_LENGTH];

rc = _modbus_receive_msg(ctx, rsp, MSG_CONFIRMATION);
if (rc == -1)
return -1;

rc = check_confirmation(ctx, req, rsp, rc);
rc = receive_confirmation(ctx, req, rsp);
}

return rc;
Expand Down Expand Up @@ -1782,11 +1810,7 @@ int modbus_write_registers(modbus_t *ctx, int addr, int nb, const uint16_t *src)
if (rc > 0) {
uint8_t rsp[MAX_MESSAGE_LENGTH];

rc = _modbus_receive_msg(ctx, rsp, MSG_CONFIRMATION);
if (rc == -1)
return -1;

rc = check_confirmation(ctx, req, rsp, rc);
rc = receive_confirmation(ctx, req, rsp);
}

return rc;
Expand Down Expand Up @@ -1820,11 +1844,7 @@ int modbus_mask_write_register(modbus_t *ctx,
/* Used by write_bit and write_register */
uint8_t rsp[MAX_MESSAGE_LENGTH];

rc = _modbus_receive_msg(ctx, rsp, MSG_CONFIRMATION);
if (rc == -1)
return -1;

rc = check_confirmation(ctx, req, rsp, rc);
rc = receive_confirmation(ctx, req, rsp);
}

return rc;
Expand Down Expand Up @@ -1893,11 +1913,7 @@ int modbus_write_and_read_registers(modbus_t *ctx,
if (rc > 0) {
unsigned int offset;

rc = _modbus_receive_msg(ctx, rsp, MSG_CONFIRMATION);
if (rc == -1)
return -1;

rc = check_confirmation(ctx, req, rsp, rc);
rc = receive_confirmation(ctx, req, rsp);
if (rc == -1)
return -1;

Expand Down Expand Up @@ -1936,11 +1952,7 @@ int modbus_report_slave_id(modbus_t *ctx, int max_dest, uint8_t *dest)
unsigned int offset;
uint8_t rsp[MAX_MESSAGE_LENGTH];

rc = _modbus_receive_msg(ctx, rsp, MSG_CONFIRMATION);
if (rc == -1)
return -1;

rc = check_confirmation(ctx, req, rsp, rc);
rc = receive_confirmation(ctx, req, rsp);
if (rc == -1)
return -1;

Expand Down