int _modbus_rtu_select(modbus_t *ctx, fd_set *rfds, struct timeval *tv, int length_to_read)
{
int s_rc;
#if defined(_WIN32)
s_rc = win32_ser_select(&(((modbus_rtu_t*)ctx->backend_data)->w_ser), length_to_read, tv);
if (s_rc == 0) {
errno = ETIMEDOUT;
return -1;
}
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;
}
}
#else
while ((s_rc = select(ctx->s+1, rfds, NULL, NULL, tv)) == -1) {
if (errno == EINTR) {
if (ctx->debug) {
fprintf(stderr, "A non blocked signal was caught\n");
}
/* Necessary after an error */
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;
}
}
}
if (s_rc == 0) {
/* Timeout */
errno = ETIMEDOUT;
_error_print(ctx, "select");
return -1;
}
#endif
return s_rc;
}
static int check_confirmation(tta_t *ctx, uint8_t *req,
uint8_t *buf, int rsp_length)
{
int rc = 0;
if (ctx->backend->pre_check_confirmation) {
rc = ctx->backend->pre_check_confirmation(ctx, req, buf, rsp_length);
if (rc == -1) {
if (ctx->error_recovery & TTA_ERROR_RECOVERY_PROTOCOL) {
_sleep_and_flush(ctx);
}
return -1;
}
}
#if 0
/* Exception code */
if (function >= 0x80) {
if (rsp_length == (offset + 2 + ctx->backend->checksum_length) &&
req[offset] == (buf[offset] - 0x80)) {
/* Valid exception code received */
int exception_code = buf[offset + 1];
if (exception_code < TTA_EXCEPTION_MAX) {
errno = TTA_ENOBASE + exception_code;
} else {
errno = EMBBADEXC;
}
_error_print(ctx, NULL);
return -1;
} else {
errno = EMBBADEXC;
_error_print(ctx, NULL);
return -1;
}
}
#endif
return rc;
}
static int _send_pkt(tta_t *ctx, uint8_t *pkt, int pktlen)
{
int rc, i;
#ifdef DUMP_PKT
#if 0
if (ctx->debug)
{
for (i = 0; i < pktlen; i++)
LOGD("[%.2X]", pkt[i]);
LOGD("\n");
}
#endif
#endif
/* In recovery mode, the write command will be issued until to be
successful! Disabled by default. */
do {
rc = ctx->backend->send(ctx, pkt, pktlen);
if (rc == -1) {
_error_print(ctx, NULL);
if (ctx->error_recovery & TTA_ERROR_RECOVERY_LINK) {
int saved_errno = errno;
if ((errno == EBADF || errno == ECONNRESET || errno == EPIPE)) {
tta_close(ctx);
tta_connect(ctx);
} else {
_sleep_and_flush(ctx);
}
errno = saved_errno;
}
}
} while ((ctx->error_recovery & TTA_ERROR_RECOVERY_LINK) &&
rc == -1);
if (rc > 0 && rc != pktlen)
{
errno = EMBBADDATA;
return -1;
}
return rc;
}
/* Sends a request/response */
static int send_msg(modbus_t *ctx, uint8_t *msg, int msg_length)
{
int rc;
int i;
msg_length = ctx->backend->send_msg_pre(msg, msg_length);
if (ctx->debug) {
for (i = 0; i < msg_length; i++)
printf("[%.2X]", msg[i]);
printf("\n");
}
/* In recovery mode, the write command will be issued until to be
successful! Disabled by default. */
do {
rc = ctx->backend->send(ctx, msg, msg_length);
if (rc == -1) {
_error_print(ctx, NULL);
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 & MODBUS_ERROR_RECOVERY_LINK) &&
rc == -1);
if (rc > 0 && rc != msg_length) {
errno = EMBBADDATA;
return -1;
}
return rc;
}
int _modbus_rtu_select(modbus_t *ctx, fd_set *rfds, struct timeval *tv, int msg_length_computed, int msg_length)
{
int s_rc;
#if defined(_WIN32)
s_rc = win32_ser_select(&(((modbus_rtu_t*)ctx->backend_data)->w_ser), msg_length_computed, tv);
if (s_rc == 0) {
errno = ETIMEDOUT;
return -1;
}
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;
}
}
#else
while ((s_rc = select(ctx->s+1, rfds, NULL, NULL, tv)) == -1) {
if (errno == EINTR) {
if (ctx->debug) {
fprintf(stderr, "A non blocked signal was caught\n");
}
/* Necessary after an error */
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;
}
}
}
if (s_rc == 0) {
/* Timeout */
if (msg_length == (ctx->backend->header_length + 2 +
ctx->backend->checksum_length)) {
/* Optimization allowed because exception response is
the smallest trame in modbus protocol (3) so always
raise a timeout error.
Temporary error before exception analyze. */
errno = EMBUNKEXC;
} else {
errno = ETIMEDOUT;
_error_print(ctx, "select");
}
return -1;
}
#endif
return s_rc;
}
static int check_confirmation(modbus_t *ctx, uint8_t *req,
uint8_t *rsp, int rsp_length)
{
int rc;
int rsp_length_computed;
const int offset = ctx->backend->header_length;
const int function = rsp[offset];
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;
}
}
rsp_length_computed = compute_response_length_from_request(ctx, req);
/* Exception code */
if (function >= 0x80) {
if (rsp_length == (offset + 2 + ctx->backend->checksum_length) &&
req[offset] == (rsp[offset] - 0x80)) {
/* Valid exception code received */
int exception_code = rsp[offset + 1];
if (exception_code < MODBUS_EXCEPTION_MAX) {
errno = MODBUS_ENOBASE + exception_code;
} else {
errno = EMBBADEXC;
}
_error_print(ctx, NULL);
return -1;
} else {
errno = EMBBADEXC;
_error_print(ctx, NULL);
return -1;
}
}
/* Check length */
if ((rsp_length == rsp_length_computed ||
rsp_length_computed == MSG_LENGTH_UNDEFINED) &&
function < 0x80) {
int req_nb_value;
int rsp_nb_value;
/* Check function code */
if (function != req[offset]) {
if (ctx->debug) {
fprintf(stderr,
"Received function not corresponding to the requestd (0x%X != 0x%X)\n",
function, req[offset]);
}
if (ctx->error_recovery & MODBUS_ERROR_RECOVERY_PROTOCOL) {
_sleep_and_flush(ctx);
}
errno = EMBBADDATA;
return -1;
}
/* Check the number of values is corresponding to the request */
switch (function) {
case _FC_READ_COILS:
case _FC_READ_DISCRETE_INPUTS:
/* Read functions, 8 values in a byte (nb
* of values in the request and byte count in
* the response. */
req_nb_value = (req[offset + 3] << 8) + req[offset + 4];
req_nb_value = (req_nb_value / 8) + ((req_nb_value % 8) ? 1 : 0);
rsp_nb_value = rsp[offset + 1];
break;
case _FC_WRITE_AND_READ_REGISTERS:
case _FC_READ_HOLDING_REGISTERS:
case _FC_READ_INPUT_REGISTERS:
/* Read functions 1 value = 2 bytes */
req_nb_value = (req[offset + 3] << 8) + req[offset + 4];
rsp_nb_value = (rsp[offset + 1] / 2);
break;
case _FC_WRITE_MULTIPLE_COILS:
case _FC_WRITE_MULTIPLE_REGISTERS:
/* N Write functions */
req_nb_value = (req[offset + 3] << 8) + req[offset + 4];
rsp_nb_value = (rsp[offset + 3] << 8) | rsp[offset + 4];
break;
case _FC_REPORT_SLAVE_ID:
/* Report slave ID (bytes received) */
req_nb_value = rsp_nb_value = rsp[offset + 1];
break;
default:
/* 1 Write functions & others */
req_nb_value = rsp_nb_value = 1;
}
if (req_nb_value == rsp_nb_value) {
rc = rsp_nb_value;
} else {
if (ctx->debug) {
fprintf(stderr,
"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;
}
} else {
if (ctx->debug) {
fprintf(stderr,
"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;
}
return rc;
}
int _modbus_receive_msg(modbus_t *ctx, uint8_t *msg, msg_type_t msg_type)
{
int rc;
fd_set rset;
struct timeval tv;
struct timeval *p_tv;
int length_to_read;
int msg_length = 0;
_step_t step;
if (ctx->debug) {
if (msg_type == MSG_INDICATION) {
printf("Waiting for a indication...\n");
} else {
printf("Waiting for a confirmation...\n");
}
}
/* Add a file descriptor to the set */
FD_ZERO(&rset);
FD_SET(ctx->s, &rset);
/* We need to analyse the message step by step. At the first step, we want
* to reach the function code because all packets contain this
* information. */
step = _STEP_FUNCTION;
length_to_read = ctx->backend->header_length + 1;
if (msg_type == MSG_INDICATION) {
/* Wait for a message, we don't know when the message will be
* received */
p_tv = NULL;
} else {
tv.tv_sec = ctx->response_timeout.tv_sec;
tv.tv_usec = ctx->response_timeout.tv_usec;
p_tv = &tv;
}
while (length_to_read != 0) {
rc = ctx->backend->select(ctx, &rset, 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;
}
rc = ctx->backend->recv(ctx, msg + msg_length, length_to_read);
if (rc == 0) {
errno = ECONNRESET;
rc = -1;
}
if (rc == -1) {
_error_print(ctx, "read");
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 = saved_errno;
}
return -1;
}
/* Display the hex code of each character received */
if (ctx->debug) {
int i;
for (i=0; i < rc; i++)
printf("<%.2X>", msg[msg_length + i]);
}
/* Sums bytes received */
msg_length += rc;
/* Computes remaining bytes */
length_to_read -= rc;
if (length_to_read == 0) {
switch (step) {
case _STEP_FUNCTION:
/* Function code position */
length_to_read = compute_meta_length_after_function(
msg[ctx->backend->header_length],
msg_type);
if (length_to_read != 0) {
step = _STEP_META;
break;
} /* else switches straight to the next step */
case _STEP_META:
length_to_read = compute_data_length_after_meta(
ctx, msg, msg_type);
if ((msg_length + length_to_read) > ctx->backend->max_adu_length) {
errno = EMBBADDATA;
_error_print(ctx, "too many data");
return -1;
}
step = _STEP_DATA;
break;
default:
break;
}
}
if (length_to_read > 0 && ctx->byte_timeout.tv_sec != -1) {
/* If there is no character in the buffer, the allowed timeout
interval between two consecutive bytes is defined by
byte_timeout */
tv.tv_sec = ctx->byte_timeout.tv_sec;
tv.tv_usec = ctx->byte_timeout.tv_usec;
p_tv = &tv;
}
}
if (ctx->debug)
printf("\n");
return ctx->backend->check_integrity(ctx, msg, msg_length);
}
/* Waits a response from a tta server or a request from a tta client.
This function blocks if there is no replies (3 timeouts).
The function shall return the number of received characters and the received
message in an array of uint8_t if successful. Otherwise it shall return -1
and errno is set to one of the values defined below:
- ECONNRESET
- EMBBADDATA
- EMBUNKEXC
- ETIMEDOUT
- read() or recv() error codes
*/
int _tta_receive_pkt(tta_t *ctx, uint8_t *pkt, int fgtimeout)
{
fd_set rfds;
struct timeval tv;
struct timeval *p_tv;
int length_to_read;
int pktlen = 0;
_step_t step;
int rc;
if (ctx->debug) {
if (fgtimeout) {
LOGD("Waiting until timeout...\n");
} else {
LOGD("Waiting without timeout...\n");
}
}
/* Add a file descriptor to the set */
FD_ZERO(&rfds);
FD_SET(ctx->s, &rfds);
step = _STEP_HD;
length_to_read = ctx->backend->header_length;
if (fgtimeout) {
tv.tv_sec = ctx->response_timeout.tv_sec;
tv.tv_usec = ctx->response_timeout.tv_usec;
p_tv = &tv;
} else {
/* Wait for a message, we don't know when the message will be
* received */
p_tv = NULL;
}
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 & TTA_ERROR_RECOVERY_LINK)
{
int saved_errno = errno;
if (errno == ETIMEDOUT)
{
_sleep_and_flush(ctx);
}
else if (errno == EBADF)
{
tta_close(ctx);
tta_connect(ctx);
}
errno = saved_errno;
}
return -1;
}
rc = ctx->backend->recv(ctx, pkt+pktlen, length_to_read);
if (rc == 0)
{
errno = ECONNRESET;
rc = -1;
}
if (rc == -1)
{
_error_print(ctx, "read");
if ((ctx->error_recovery & TTA_ERROR_RECOVERY_LINK) &&
(errno == ECONNRESET || errno == ECONNREFUSED ||
errno == EBADF))
{
int saved_errno = errno;
tta_close(ctx);
tta_connect(ctx);
/* Could be removed by previous calls */
errno = saved_errno;
}
return -1;
}
/* Display the hex code of each character received */
#ifdef DUMP_PKT
#if 0
if (ctx->debug)
{
int i;
for (i=0; i < rc; i++)
LOGD("<%.2X>", pkt[pktlen + i]);
}
#endif
#endif
/* Sums bytes received */
pktlen += rc;
/* Computes remaining bytes */
length_to_read -= rc;
if (length_to_read == 0)
{
switch (step)
{
case _STEP_HD:
/* Function code position */
length_to_read = compute_data_length_after_hd(ctx, pkt);
if((pktlen+length_to_read) > ctx->backend->max_adu_length)
{
errno = EMBBADDATA;
_error_print(ctx, "too many data");
return -1;
}
step = _STEP_DATA;
break;
case _STEP_DATA:
case _STEP_TAIL:
default:
break;
}
}
if (length_to_read > 0)
{
/* If there is no character in the buffer, the allowed timeout
interval between two consecutive bytes is defined by
byte_timeout */
tv.tv_sec = ctx->byte_timeout.tv_sec;
tv.tv_usec = ctx->byte_timeout.tv_usec;
p_tv = &tv;
}
}
/* Display the hex code of each character received */
if (ctx->debug)
LOGD("\n");
return ctx->backend->check_integrity(ctx, pkt, pktlen);
}
