Changes
This commit is contained in:
Valerio De Benedetto
parent
294c14b137
commit
4605782fc7
464
modbusino.c
464
modbusino.c
@ -12,16 +12,30 @@
|
||||
defined(__THUMBEL__) || defined(__AARCH64EL__) || defined(_MIPSEL) || defined(__MIPSEL) || defined(__MIPSEL__)
|
||||
#define MBSN_LITTLE_ENDIAN
|
||||
#else
|
||||
#error "Failed to automatically detect system endianness. Please define either MBSN_BIG_ENDIAN or MBSN_LITTLE_ENDIAN"
|
||||
#error "Failed to automatically detect platform endianness. Please define either MBSN_BIG_ENDIAN or MBSN_LITTLE_ENDIAN"
|
||||
#endif
|
||||
#endif
|
||||
|
||||
|
||||
#define get_1(m) \
|
||||
(m)->msg_buf[(m)->msg_buf_idx]; \
|
||||
(m)->msg_buf_idx++
|
||||
#define put_1(m, b) \
|
||||
(m)->msg_buf[(m)->msg_buf_idx] = (b); \
|
||||
(m)->msg_buf_idx++
|
||||
|
||||
#ifdef MBSN_BIG_ENDIAN
|
||||
#define HTONS(x) (x)
|
||||
#define NTOHS(x) (x)
|
||||
#define get_2(m) ((m)->msg_buf[(m)->msg_buf[(m)->msg_buf_idx]); \
|
||||
(m)->msg_buf_idx += 2
|
||||
#define put_2(m, w) (m)->msg_buf(m)[(m)->msg_buf_idx]] = (w); \
|
||||
(m)->msg_buf_idx += 2
|
||||
#else
|
||||
#define HTONS(x) (((x) >> 8) | ((x) << 8))
|
||||
#define NTOHS(x) (((x) >> 8) | ((x) << 8))
|
||||
#define get_2(m) \
|
||||
((m)->msg_buf[(m)->msg_buf_idx] >> 8 | (m)->msg_buf[(m)->msg_buf_idx] << 8); \
|
||||
(m)->msg_buf_idx += 2
|
||||
#define put_2(m, w) \
|
||||
(m)->msg_buf[(m)->msg_buf_idx] = ((w) >> 8 | (w) << 8); \
|
||||
(m)->msg_buf_idx += 2
|
||||
#endif
|
||||
|
||||
|
||||
@ -31,24 +45,22 @@ typedef struct msg_state {
|
||||
uint16_t transaction_id;
|
||||
bool broadcast;
|
||||
bool ignored;
|
||||
uint16_t crc;
|
||||
} msg_state;
|
||||
|
||||
|
||||
static msg_state msg_state_create() {
|
||||
msg_state s;
|
||||
memset(&s, 0, sizeof(msg_state));
|
||||
s.crc = 0xFFFF;
|
||||
return s;
|
||||
}
|
||||
|
||||
|
||||
static msg_state res_from_req(msg_state* req) {
|
||||
msg_state res = *req;
|
||||
res.crc = 0xFFFF;
|
||||
return res;
|
||||
}
|
||||
|
||||
|
||||
static msg_state req_create(uint8_t unit_id, uint8_t fc) {
|
||||
static uint16_t TID = 0;
|
||||
|
||||
@ -63,6 +75,7 @@ static msg_state req_create(uint8_t unit_id, uint8_t fc) {
|
||||
return req;
|
||||
}
|
||||
|
||||
|
||||
int mbsn_create(mbsn_t* mbsn, mbsn_transport_conf transport_conf) {
|
||||
if (!mbsn)
|
||||
return MBSN_ERROR_INVALID_ARGUMENT;
|
||||
@ -149,16 +162,20 @@ static uint16_t crc_add_2(uint16_t crc, const uint8_t w) {
|
||||
}
|
||||
|
||||
|
||||
static mbsn_error recv_n(mbsn_t* mbsn, uint8_t* buf, uint32_t count, uint16_t* crc) {
|
||||
static void reset_msg_buf(mbsn_t* mbsn) {
|
||||
mbsn->msg_buf_idx = 0;
|
||||
}
|
||||
|
||||
|
||||
static mbsn_error recv(mbsn_t* mbsn, uint32_t count) {
|
||||
int r = 0;
|
||||
while (r != count) {
|
||||
int ret = mbsn->transport_read_byte(buf + r, mbsn->byte_timeout_ms);
|
||||
int ret = mbsn->transport_read_byte(mbsn->msg_buf + mbsn->msg_buf_idx + r, mbsn->byte_timeout_ms);
|
||||
if (ret == 0)
|
||||
return MBSN_ERROR_TIMEOUT;
|
||||
else if (ret != 1)
|
||||
return MBSN_ERROR_TRANSPORT;
|
||||
|
||||
*crc = crc_add_1(*crc, buf[r]);
|
||||
r++;
|
||||
}
|
||||
|
||||
@ -166,70 +183,41 @@ static mbsn_error recv_n(mbsn_t* mbsn, uint8_t* buf, uint32_t count, uint16_t* c
|
||||
}
|
||||
|
||||
|
||||
static mbsn_error recv_1(mbsn_t* mbsn, uint8_t* b, uint16_t* crc) {
|
||||
return recv_n(mbsn, b, 1, crc);
|
||||
}
|
||||
|
||||
|
||||
static mbsn_error recv_2(mbsn_t* mbsn, uint16_t* w, uint16_t* crc) {
|
||||
mbsn_error err = recv_n(mbsn, (uint8_t*) w, 2, crc);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
*w = NTOHS(*w);
|
||||
return MBSN_ERROR_NONE;
|
||||
}
|
||||
|
||||
|
||||
static mbsn_error send_n(mbsn_t* mbsn, uint8_t* buf, uint32_t count, uint16_t* crc) {
|
||||
int w = 0;
|
||||
while (w != count) {
|
||||
int ret = mbsn->transport_write_byte(buf[w], mbsn->read_timeout_ms);
|
||||
static mbsn_error send(mbsn_t* mbsn) {
|
||||
for (int i = 0; i < mbsn->msg_buf_idx; i++) {
|
||||
int ret = mbsn->transport_write_byte(mbsn->msg_buf[i], mbsn->read_timeout_ms);
|
||||
if (ret == 0)
|
||||
return MBSN_ERROR_TIMEOUT;
|
||||
else if (ret != 1)
|
||||
return MBSN_ERROR_TRANSPORT;
|
||||
|
||||
w++;
|
||||
}
|
||||
|
||||
if (crc)
|
||||
*crc = crc_add_n(*crc, buf, count);
|
||||
|
||||
return MBSN_ERROR_NONE;
|
||||
}
|
||||
|
||||
|
||||
static mbsn_error send_1(mbsn_t* mbsn, uint8_t b, uint16_t* crc) {
|
||||
return send_n(mbsn, &b, 1, crc);
|
||||
}
|
||||
|
||||
|
||||
static mbsn_error send_2(mbsn_t* mbsn, uint16_t w, uint16_t* crc) {
|
||||
w = HTONS(w);
|
||||
return send_n(mbsn, (uint8_t*) &w, 2, crc);
|
||||
}
|
||||
|
||||
|
||||
static mbsn_error recv_msg_header(mbsn_t* mbsn, msg_state* s_out) {
|
||||
static mbsn_error recv_msg_header(mbsn_t* mbsn, msg_state* s_out, bool* first_byte_received) {
|
||||
// We wait for the read timeout here, just for the first message byte
|
||||
int32_t old_byte_timeout = mbsn->byte_timeout_ms;
|
||||
mbsn->byte_timeout_ms = mbsn->read_timeout_ms;
|
||||
|
||||
*s_out = msg_state_create();
|
||||
reset_msg_buf(mbsn);
|
||||
if (first_byte_received)
|
||||
*first_byte_received = false;
|
||||
|
||||
if (mbsn->transport == MBSN_TRANSPORT_RTU) {
|
||||
uint8_t unit_id;
|
||||
mbsn_error err = recv_1(mbsn, &unit_id, &s_out->crc);
|
||||
mbsn_error err = recv(mbsn, 1);
|
||||
|
||||
mbsn->byte_timeout_ms = old_byte_timeout;
|
||||
|
||||
if (err != 0) {
|
||||
if (err == MBSN_ERROR_TIMEOUT)
|
||||
return MBSN_ERROR_NONE;
|
||||
else
|
||||
return err;
|
||||
}
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
if (first_byte_received)
|
||||
*first_byte_received = true;
|
||||
|
||||
uint8_t unit_id = get_1(mbsn);
|
||||
|
||||
// Check if message is for us
|
||||
if (unit_id == 0)
|
||||
@ -239,59 +227,55 @@ static mbsn_error recv_msg_header(mbsn_t* mbsn, msg_state* s_out) {
|
||||
else
|
||||
s_out->ignored = false;
|
||||
|
||||
err = recv_1(mbsn, &s_out->fc, &s_out->crc);
|
||||
err = recv(mbsn, 1);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
s_out->fc = get_1(mbsn);
|
||||
}
|
||||
else if (mbsn->transport == MBSN_TRANSPORT_TCP) {
|
||||
mbsn_error err = recv_2(mbsn, &s_out->transaction_id, NULL);
|
||||
mbsn_error err = recv(mbsn, 1);
|
||||
|
||||
mbsn->byte_timeout_ms = old_byte_timeout;
|
||||
|
||||
if (err != 0) {
|
||||
if (err == MBSN_ERROR_TIMEOUT)
|
||||
return MBSN_ERROR_NONE;
|
||||
else
|
||||
return err;
|
||||
}
|
||||
|
||||
uint16_t protocol_id = 0xFFFF;
|
||||
err = recv_2(mbsn, &protocol_id, NULL);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
uint16_t length = 0xFFFF;
|
||||
err = recv_2(mbsn, &length, NULL);
|
||||
if (first_byte_received)
|
||||
*first_byte_received = true;
|
||||
|
||||
err = recv(mbsn, 7);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
err = recv_1(mbsn, &s_out->unit_id, NULL);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
reset_msg_buf(mbsn);
|
||||
|
||||
err = recv_1(mbsn, &s_out->fc, NULL);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
uint16_t protocol_id = get_2(mbsn);
|
||||
// TODO maybe we should actually check the length of the request against this value
|
||||
uint16_t length = get_2(mbsn);
|
||||
s_out->unit_id = get_1(mbsn);
|
||||
s_out->fc = get_1(mbsn);
|
||||
|
||||
if (protocol_id != 0)
|
||||
return MBSN_ERROR_TRANSPORT;
|
||||
|
||||
// TODO maybe we should actually check the length of the request against this value
|
||||
if (length == 0xFFFF)
|
||||
if (length > 255)
|
||||
return MBSN_ERROR_TRANSPORT;
|
||||
}
|
||||
|
||||
return MBSN_ERROR_NONE;
|
||||
}
|
||||
|
||||
|
||||
static mbsn_error recv_msg_footer(mbsn_t* mbsn, msg_state* s) {
|
||||
if (mbsn->transport == MBSN_TRANSPORT_RTU) {
|
||||
uint16_t recv_crc;
|
||||
mbsn_error err = recv_2(mbsn, &recv_crc, NULL);
|
||||
mbsn_error err = recv(mbsn, 2);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
if (recv_crc != s->crc)
|
||||
uint16_t recv_crc = get_2(mbsn);
|
||||
uint16_t crc = crc_add_n(0xFFFF, mbsn->msg_buf, mbsn->msg_buf_idx);
|
||||
if (recv_crc != crc)
|
||||
return MBSN_ERROR_TRANSPORT;
|
||||
}
|
||||
|
||||
@ -301,43 +285,29 @@ static mbsn_error recv_msg_footer(mbsn_t* mbsn, msg_state* s) {
|
||||
|
||||
static mbsn_error send_msg_header(mbsn_t* mbsn, msg_state* s, uint8_t data_length) {
|
||||
if (mbsn->transport == MBSN_TRANSPORT_RTU) {
|
||||
s->crc = 0xFFFF;
|
||||
mbsn_error err = send_1(mbsn, s->unit_id, &s->crc);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
put_1(mbsn, s->unit_id);
|
||||
}
|
||||
else if (mbsn->transport == MBSN_TRANSPORT_TCP) {
|
||||
mbsn_error err = send_2(mbsn, s->transaction_id, NULL);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
const uint16_t protocol_id = 0;
|
||||
err = send_2(mbsn, protocol_id, NULL);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
err = send_2(mbsn, 1 + 1 + data_length, NULL);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
err = send_1(mbsn, s->unit_id, NULL);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
put_2(mbsn, s->transaction_id);
|
||||
put_2(mbsn, 0);
|
||||
put_2(mbsn, 1 + 1 + data_length);
|
||||
put_2(mbsn, s->unit_id);
|
||||
}
|
||||
|
||||
mbsn_error err = send_1(mbsn, s->fc, &s->crc);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
put_1(mbsn, s->fc);
|
||||
return MBSN_ERROR_NONE;
|
||||
}
|
||||
|
||||
|
||||
static mbsn_error send_msg_footer(mbsn_t* mbsn, msg_state* s) {
|
||||
if (mbsn->transport == MBSN_TRANSPORT_RTU)
|
||||
return send_2(mbsn, s->crc, NULL);
|
||||
if (mbsn->transport == MBSN_TRANSPORT_RTU) {
|
||||
uint16_t crc = crc_add_n(0xFFFF, mbsn->msg_buf, mbsn->msg_buf_idx);
|
||||
put_2(mbsn, crc);
|
||||
}
|
||||
|
||||
return MBSN_ERROR_NONE;
|
||||
mbsn_error err = send(mbsn);
|
||||
reset_msg_buf(mbsn);
|
||||
return err;
|
||||
}
|
||||
|
||||
|
||||
@ -349,9 +319,7 @@ static mbsn_error handle_exception(mbsn_t* mbsn, msg_state* req, uint8_t excepti
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
err = send_1(mbsn, exception, &res.crc);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
put_1(mbsn, exception);
|
||||
|
||||
return send_msg_footer(mbsn, &res);
|
||||
}
|
||||
@ -359,13 +327,7 @@ static mbsn_error handle_exception(mbsn_t* mbsn, msg_state* req, uint8_t excepti
|
||||
|
||||
static mbsn_error handle_read_discrete(mbsn_t* mbsn, msg_state* req,
|
||||
mbsn_error (*callback)(uint16_t, uint16_t, mbsn_bitfield)) {
|
||||
uint16_t addr;
|
||||
mbsn_error err = recv_2(mbsn, &addr, &req->crc);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
uint16_t quantity;
|
||||
err = recv_2(mbsn, &quantity, &req->crc);
|
||||
mbsn_error err = recv(mbsn, 4);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
@ -374,6 +336,9 @@ static mbsn_error handle_read_discrete(mbsn_t* mbsn, msg_state* req,
|
||||
return err;
|
||||
|
||||
if (!req->ignored) {
|
||||
uint16_t addr = get_2(mbsn);
|
||||
uint16_t quantity = get_2(mbsn);
|
||||
|
||||
if (quantity < 1 || quantity > 2000)
|
||||
return handle_exception(mbsn, req, MBSN_EXCEPTION_ILLEGAL_DATA_VALUE);
|
||||
|
||||
@ -398,13 +363,11 @@ static mbsn_error handle_read_discrete(mbsn_t* mbsn, msg_state* req,
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
err = send_1(mbsn, discrete_bytes, &res.crc);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
put_1(mbsn, discrete_bytes);
|
||||
|
||||
err = send_n(mbsn, bf, discrete_bytes, &res.crc);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
for (int i = 0; i < discrete_bytes; i++) {
|
||||
put_1(mbsn, bf[i]);
|
||||
}
|
||||
|
||||
err = send_msg_footer(mbsn, &res);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
@ -422,13 +385,7 @@ static mbsn_error handle_read_discrete(mbsn_t* mbsn, msg_state* req,
|
||||
|
||||
static mbsn_error handle_read_registers(mbsn_t* mbsn, msg_state* req,
|
||||
mbsn_error (*callback)(uint16_t, uint16_t, uint16_t*)) {
|
||||
uint16_t addr;
|
||||
mbsn_error err = recv_2(mbsn, &addr, &req->crc);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
uint16_t quantity;
|
||||
err = recv_2(mbsn, &quantity, &req->crc);
|
||||
mbsn_error err = recv(mbsn, 4);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
@ -437,6 +394,9 @@ static mbsn_error handle_read_registers(mbsn_t* mbsn, msg_state* req,
|
||||
return err;
|
||||
|
||||
if (!req->ignored) {
|
||||
uint16_t addr = get_2(mbsn);
|
||||
uint16_t quantity = get_2(mbsn);
|
||||
|
||||
if (quantity < 1 || quantity > 125)
|
||||
return handle_exception(mbsn, req, MBSN_EXCEPTION_ILLEGAL_DATA_VALUE);
|
||||
|
||||
@ -461,14 +421,10 @@ static mbsn_error handle_read_registers(mbsn_t* mbsn, msg_state* req,
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
err = send_1(mbsn, regs_bytes, &res.crc);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
put_1(mbsn, regs_bytes);
|
||||
|
||||
for (int i = 0; i < quantity; i++) {
|
||||
err = send_2(mbsn, regs[i], &res.crc);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
put_2(mbsn, regs[i]);
|
||||
}
|
||||
|
||||
err = send_msg_footer(mbsn, &res);
|
||||
@ -506,13 +462,7 @@ static mbsn_error handle_read_input_registers(mbsn_t* mbsn, msg_state* req) {
|
||||
|
||||
|
||||
static mbsn_error handle_write_single_coil(mbsn_t* mbsn, msg_state* req) {
|
||||
uint16_t addr;
|
||||
mbsn_error err = recv_2(mbsn, &addr, &req->crc);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
uint16_t value;
|
||||
err = recv_2(mbsn, &value, &req->crc);
|
||||
mbsn_error err = recv(mbsn, 4);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
@ -521,6 +471,9 @@ static mbsn_error handle_write_single_coil(mbsn_t* mbsn, msg_state* req) {
|
||||
return err;
|
||||
|
||||
if (!req->ignored) {
|
||||
uint16_t addr = get_2(mbsn);
|
||||
uint16_t value = get_2(mbsn);
|
||||
|
||||
if (value != 0 && value != 0xFF00)
|
||||
return handle_exception(mbsn, req, MBSN_EXCEPTION_ILLEGAL_DATA_VALUE);
|
||||
|
||||
@ -540,9 +493,7 @@ static mbsn_error handle_write_single_coil(mbsn_t* mbsn, msg_state* req) {
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
err = send_2(mbsn, addr, &res.crc);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
put_2(mbsn, addr);
|
||||
|
||||
err = send_msg_footer(mbsn, &res);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
@ -559,13 +510,7 @@ static mbsn_error handle_write_single_coil(mbsn_t* mbsn, msg_state* req) {
|
||||
|
||||
|
||||
static mbsn_error handle_write_single_register(mbsn_t* mbsn, msg_state* req) {
|
||||
uint16_t addr;
|
||||
mbsn_error err = recv_2(mbsn, &addr, &req->crc);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
uint16_t value;
|
||||
err = recv_2(mbsn, &value, &req->crc);
|
||||
mbsn_error err = recv(mbsn, 4);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
@ -574,6 +519,9 @@ static mbsn_error handle_write_single_register(mbsn_t* mbsn, msg_state* req) {
|
||||
return err;
|
||||
|
||||
if (!req->ignored) {
|
||||
uint16_t addr = get_2(mbsn);
|
||||
uint16_t value = get_2(mbsn);
|
||||
|
||||
if (mbsn->callbacks.write_single_register) {
|
||||
err = mbsn->callbacks.write_single_register(addr, value);
|
||||
if (err != MBSN_ERROR_NONE) {
|
||||
@ -590,7 +538,7 @@ static mbsn_error handle_write_single_register(mbsn_t* mbsn, msg_state* req) {
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
err = send_2(mbsn, value, &res.crc);
|
||||
err = put_2(mbsn, value);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
@ -609,25 +557,21 @@ static mbsn_error handle_write_single_register(mbsn_t* mbsn, msg_state* req) {
|
||||
|
||||
|
||||
static mbsn_error handle_write_multiple_coils(mbsn_t* mbsn, msg_state* req) {
|
||||
uint16_t addr;
|
||||
mbsn_error err = recv_2(mbsn, &addr, &req->crc);
|
||||
mbsn_error err = recv(mbsn, 5);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
uint16_t quantity;
|
||||
err = recv_2(mbsn, &quantity, &req->crc);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
uint16_t addr = get_2(mbsn);
|
||||
uint16_t quantity = get_2(mbsn);
|
||||
uint8_t coils_bytes = get_1(mbsn);
|
||||
|
||||
uint8_t coils_bytes;
|
||||
err = recv_1(mbsn, &coils_bytes, &req->crc);
|
||||
err = recv(mbsn, coils_bytes);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
mbsn_bitfield coils;
|
||||
err = recv_n(mbsn, coils, coils_bytes, &req->crc);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
for (int i = 0; i < coils_bytes; i++)
|
||||
coils[i] = get_1(mbsn);
|
||||
|
||||
err = recv_msg_footer(mbsn, req);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
@ -659,13 +603,8 @@ static mbsn_error handle_write_multiple_coils(mbsn_t* mbsn, msg_state* req) {
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
err = send_2(mbsn, addr, &res.crc);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
err = send_2(mbsn, quantity, &res.crc);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
put_2(mbsn, addr);
|
||||
put_2(mbsn, quantity);
|
||||
|
||||
err = send_msg_footer(mbsn, &res);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
@ -682,26 +621,21 @@ static mbsn_error handle_write_multiple_coils(mbsn_t* mbsn, msg_state* req) {
|
||||
|
||||
|
||||
static mbsn_error handle_write_multiple_registers(mbsn_t* mbsn, msg_state* req) {
|
||||
uint16_t addr;
|
||||
mbsn_error err = recv_2(mbsn, &addr, &req->crc);
|
||||
mbsn_error err = recv(mbsn, 5);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
uint16_t quantity;
|
||||
err = recv_2(mbsn, &quantity, &req->crc);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
uint16_t addr = get_2(mbsn);
|
||||
uint16_t quantity = get_2(mbsn);
|
||||
uint8_t registers_bytes = get_1(mbsn);
|
||||
|
||||
uint8_t registers_bytes;
|
||||
err = recv_1(mbsn, ®isters_bytes, &req->crc);
|
||||
err = recv(mbsn, registers_bytes);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
uint16_t registers[0x007B];
|
||||
for (int i = 0; i < quantity; i++) {
|
||||
err = recv_2(mbsn, registers + i, &req->crc);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
registers[i] = get_2(mbsn);
|
||||
}
|
||||
|
||||
err = recv_msg_footer(mbsn, req);
|
||||
@ -734,13 +668,8 @@ static mbsn_error handle_write_multiple_registers(mbsn_t* mbsn, msg_state* req)
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
err = send_2(mbsn, addr, &res.crc);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
err = send_2(mbsn, quantity, &res.crc);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
put_2(mbsn, addr);
|
||||
put_2(mbsn, quantity);
|
||||
|
||||
err = send_msg_footer(mbsn, &res);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
@ -802,9 +731,14 @@ static mbsn_error handle_req_fc(mbsn_t* mbsn, msg_state* req) {
|
||||
mbsn_error mbsn_server_receive(mbsn_t* mbsn) {
|
||||
msg_state req = msg_state_create();
|
||||
|
||||
mbsn_error err = recv_msg_header(mbsn, &req);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
bool first_byte_received = false;
|
||||
mbsn_error err = recv_msg_header(mbsn, &req, &first_byte_received);
|
||||
if (err != MBSN_ERROR_NONE) {
|
||||
if (!first_byte_received && err == MBSN_ERROR_TIMEOUT)
|
||||
return MBSN_ERROR_NONE;
|
||||
else
|
||||
return err;
|
||||
}
|
||||
|
||||
/*
|
||||
// We should wait for the read timeout for the first message byte
|
||||
@ -879,7 +813,7 @@ mbsn_error mbsn_server_receive(mbsn_t* mbsn) {
|
||||
|
||||
|
||||
static mbsn_error recv_res_header(mbsn_t* mbsn, msg_state* req, msg_state* res_out) {
|
||||
mbsn_error err = recv_msg_header(mbsn, res_out);
|
||||
mbsn_error err = recv_msg_header(mbsn, res_out, NULL);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
@ -896,7 +830,7 @@ static mbsn_error recv_res_header(mbsn_t* mbsn, msg_state* req, msg_state* res_o
|
||||
}
|
||||
|
||||
|
||||
static mbsn_error read_discrete(mbsn_t* mbsn, uint8_t fc, uint16_t address, uint16_t quantity, mbsn_bitfield* values) {
|
||||
static mbsn_error read_discrete(mbsn_t* mbsn, uint8_t fc, uint16_t address, uint16_t quantity, mbsn_bitfield values) {
|
||||
if (quantity < 1 || quantity > 2000)
|
||||
return MBSN_ERROR_INVALID_ARGUMENT;
|
||||
|
||||
@ -905,13 +839,8 @@ static mbsn_error read_discrete(mbsn_t* mbsn, uint8_t fc, uint16_t address, uint
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
err = send_2(mbsn, address, &req.crc);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
err = send_2(mbsn, quantity, &req.crc);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
put_2(mbsn, address);
|
||||
put_2(mbsn, quantity);
|
||||
|
||||
err = send_msg_footer(mbsn, &req);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
@ -922,15 +851,20 @@ static mbsn_error read_discrete(mbsn_t* mbsn, uint8_t fc, uint16_t address, uint
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
uint8_t coils_bytes;
|
||||
err = recv_1(mbsn, &coils_bytes, &res.crc);
|
||||
err = recv(mbsn, 1);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
err = recv_n(mbsn, (uint8_t*) &values, coils_bytes, &res.crc);
|
||||
uint8_t coils_bytes = get_1(mbsn);
|
||||
|
||||
err = recv(mbsn, coils_bytes);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
for (int i = 0; i < coils_bytes; i++) {
|
||||
values[i] = get_1(mbsn);
|
||||
}
|
||||
|
||||
err = recv_msg_footer(mbsn, &res);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
@ -939,12 +873,12 @@ static mbsn_error read_discrete(mbsn_t* mbsn, uint8_t fc, uint16_t address, uint
|
||||
}
|
||||
|
||||
|
||||
mbsn_error mbsn_read_coils(mbsn_t* mbsn, uint16_t address, uint16_t quantity, mbsn_bitfield* coils_out) {
|
||||
mbsn_error mbsn_read_coils(mbsn_t* mbsn, uint16_t address, uint16_t quantity, mbsn_bitfield coils_out) {
|
||||
return read_discrete(mbsn, 1, address, quantity, coils_out);
|
||||
}
|
||||
|
||||
|
||||
mbsn_error mbsn_read_discrete_inputs(mbsn_t* mbsn, uint16_t address, uint16_t quantity, mbsn_bitfield* inputs_out) {
|
||||
mbsn_error mbsn_read_discrete_inputs(mbsn_t* mbsn, uint16_t address, uint16_t quantity, mbsn_bitfield inputs_out) {
|
||||
return read_discrete(mbsn, 1, address, quantity, inputs_out);
|
||||
}
|
||||
|
||||
@ -958,13 +892,8 @@ mbsn_error read_registers(mbsn_t* mbsn, uint8_t fc, uint16_t address, uint16_t q
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
err = send_2(mbsn, address, &req.crc);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
err = send_2(mbsn, quantity, &req.crc);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
put_2(mbsn, address);
|
||||
put_2(mbsn, quantity);
|
||||
|
||||
err = send_msg_footer(mbsn, &req);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
@ -975,15 +904,18 @@ mbsn_error read_registers(mbsn_t* mbsn, uint8_t fc, uint16_t address, uint16_t q
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
uint8_t registers_bytes;
|
||||
err = recv_1(mbsn, ®isters_bytes, &res.crc);
|
||||
err = recv(mbsn, 1);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
uint8_t registers_bytes = get_1(mbsn);
|
||||
|
||||
err = recv(mbsn, registers_bytes);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
for (int i = 0; i < registers_bytes / 2; i++) {
|
||||
err = recv_2(mbsn, registers + i, &res.crc);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
registers[i] = get_2(mbsn);
|
||||
}
|
||||
|
||||
err = recv_msg_footer(mbsn, &res);
|
||||
@ -1013,13 +945,8 @@ mbsn_error mbsn_write_single_coil(mbsn_t* mbsn, uint16_t address, bool value) {
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
err = send_2(mbsn, address, &req.crc);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
err = send_2(mbsn, value ? 0xFF00 : 0, &req.crc);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
put_2(mbsn, address);
|
||||
put_2(mbsn, value ? 0xFF00 : 0);
|
||||
|
||||
err = send_msg_footer(mbsn, &req);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
@ -1030,15 +957,12 @@ mbsn_error mbsn_write_single_coil(mbsn_t* mbsn, uint16_t address, bool value) {
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
uint16_t address_res;
|
||||
err = recv_2(mbsn, &address_res, &res.crc);
|
||||
err = recv(mbsn, 4);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
uint16_t value_res;
|
||||
err = recv_2(mbsn, &value_res, &res.crc);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
uint16_t address_res = get_2(mbsn);
|
||||
uint16_t value_res = get_2(mbsn);
|
||||
|
||||
err = recv_msg_footer(mbsn, &res);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
@ -1060,13 +984,8 @@ mbsn_error mbsn_write_single_register(mbsn_t* mbsn, uint16_t address, uint16_t v
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
err = send_2(mbsn, address, &req.crc);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
err = send_2(mbsn, value, &req.crc);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
put_2(mbsn, address);
|
||||
put_2(mbsn, value);
|
||||
|
||||
err = send_msg_footer(mbsn, &req);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
@ -1077,15 +996,12 @@ mbsn_error mbsn_write_single_register(mbsn_t* mbsn, uint16_t address, uint16_t v
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
uint16_t address_res;
|
||||
err = recv_2(mbsn, &address_res, &res.crc);
|
||||
err = recv(mbsn, 4);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
uint16_t value_res;
|
||||
err = recv_2(mbsn, &value_res, &res.crc);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
uint16_t address_res = get_2(mbsn);
|
||||
uint16_t value_res = get_2(mbsn);
|
||||
|
||||
err = recv_msg_footer(mbsn, &res);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
@ -1101,7 +1017,7 @@ mbsn_error mbsn_write_single_register(mbsn_t* mbsn, uint16_t address, uint16_t v
|
||||
}
|
||||
|
||||
|
||||
mbsn_error mbsn_write_multiple_coils(mbsn_t* mbsn, uint16_t address, uint16_t quantity, const mbsn_bitfield* coils) {
|
||||
mbsn_error mbsn_write_multiple_coils(mbsn_t* mbsn, uint16_t address, uint16_t quantity, const mbsn_bitfield coils) {
|
||||
if (quantity < 0 || quantity > 0x07B0)
|
||||
return MBSN_ERROR_INVALID_ARGUMENT;
|
||||
|
||||
@ -1112,21 +1028,13 @@ mbsn_error mbsn_write_multiple_coils(mbsn_t* mbsn, uint16_t address, uint16_t qu
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
err = send_2(mbsn, address, &req.crc);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
put_2(mbsn, address);
|
||||
put_2(mbsn, quantity);
|
||||
put_1(mbsn, coils_bytes);
|
||||
|
||||
err = send_2(mbsn, quantity, &req.crc);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
err = send_1(mbsn, coils_bytes, &req.crc);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
err = send_n(mbsn, (uint8_t*) coils, coils_bytes, &req.crc);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
for (int i = 0; i < coils_bytes; i++) {
|
||||
put_1(mbsn, coils[i]);
|
||||
}
|
||||
|
||||
err = send_msg_footer(mbsn, &req);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
@ -1137,15 +1045,12 @@ mbsn_error mbsn_write_multiple_coils(mbsn_t* mbsn, uint16_t address, uint16_t qu
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
uint16_t address_res;
|
||||
err = recv_2(mbsn, &address_res, &res.crc);
|
||||
err = recv(mbsn, 4);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
uint16_t quantity_res;
|
||||
err = recv_2(mbsn, &quantity_res, &res.crc);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
uint16_t address_res = get_2(mbsn);
|
||||
uint16_t quantity_res = get_2(mbsn);
|
||||
|
||||
err = recv_msg_footer(mbsn, &res);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
@ -1172,22 +1077,12 @@ mbsn_error mbsn_write_multiple_registers(mbsn_t* mbsn, uint16_t address, uint16_
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
err = send_2(mbsn, address, &req.crc);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
put_2(mbsn, address);
|
||||
put_2(mbsn, quantity);
|
||||
put_1(mbsn, registers_bytes);
|
||||
|
||||
err = send_2(mbsn, quantity, &req.crc);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
err = send_1(mbsn, registers_bytes, &req.crc);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
for(int i = 0; i < quantity; i++) {
|
||||
err = send_2(mbsn, registers[i], &req.crc);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
for (int i = 0; i < quantity; i++) {
|
||||
put_2(mbsn, registers[i]);
|
||||
}
|
||||
|
||||
err = send_msg_footer(mbsn, &req);
|
||||
@ -1199,15 +1094,12 @@ mbsn_error mbsn_write_multiple_registers(mbsn_t* mbsn, uint16_t address, uint16_
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
uint16_t address_res;
|
||||
err = recv_2(mbsn, &address_res, &res.crc);
|
||||
err = recv(mbsn, 4);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
uint16_t quantity_res;
|
||||
err = recv_2(mbsn, &quantity_res, &res.crc);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
return err;
|
||||
uint16_t address_res = get_2(mbsn);
|
||||
uint16_t quantity_res = get_2(mbsn);
|
||||
|
||||
err = recv_msg_footer(mbsn, &res);
|
||||
if (err != MBSN_ERROR_NONE)
|
||||
|
||||
17
modbusino.h
17
modbusino.h
@ -38,8 +38,8 @@ typedef enum mbsn_transport {
|
||||
|
||||
typedef struct mbsn_transport_conf {
|
||||
mbsn_transport transport;
|
||||
int (*read_byte)(uint8_t* b, uint32_t);
|
||||
int (*write_byte)(uint8_t b, uint32_t);
|
||||
int (*read_byte)(uint8_t* b, int32_t);
|
||||
int (*write_byte)(uint8_t b, int32_t);
|
||||
} mbsn_transport_conf;
|
||||
|
||||
|
||||
@ -69,14 +69,17 @@ typedef struct mbsn_callbacks {
|
||||
|
||||
typedef struct mbsn_t {
|
||||
// Private fields
|
||||
uint8_t msg_buf[260];
|
||||
uint16_t msg_buf_idx;
|
||||
|
||||
mbsn_callbacks callbacks;
|
||||
|
||||
int32_t byte_timeout_ms;
|
||||
int32_t read_timeout_ms;
|
||||
|
||||
mbsn_transport transport;
|
||||
mbsn_error (*transport_read_byte)(uint8_t*, uint32_t);
|
||||
mbsn_error (*transport_write_byte)(uint8_t, uint32_t);
|
||||
mbsn_error (*transport_read_byte)(uint8_t*, int32_t);
|
||||
mbsn_error (*transport_write_byte)(uint8_t, int32_t);
|
||||
|
||||
uint8_t address_rtu;
|
||||
uint8_t server_dest_address_rtu;
|
||||
@ -96,9 +99,9 @@ void mbsn_client_set_server_address_rtu(mbsn_t* mbsn, uint8_t address);
|
||||
|
||||
mbsn_error mbsn_server_receive(mbsn_t* mbsn);
|
||||
|
||||
mbsn_error mbsn_read_coils(mbsn_t* mbsn, uint16_t address, uint16_t quantity, mbsn_bitfield* coils_out);
|
||||
mbsn_error mbsn_read_coils(mbsn_t* mbsn, uint16_t address, uint16_t quantity, mbsn_bitfield coils_out);
|
||||
|
||||
mbsn_error mbsn_read_discrete_inputs(mbsn_t* mbsn, uint16_t address, uint16_t quantity, mbsn_bitfield* inputs_out);
|
||||
mbsn_error mbsn_read_discrete_inputs(mbsn_t* mbsn, uint16_t address, uint16_t quantity, mbsn_bitfield inputs_out);
|
||||
|
||||
mbsn_error mbsn_read_holding_registers(mbsn_t* mbsn, uint16_t address, uint16_t quantity, uint16_t* registers_out);
|
||||
|
||||
@ -108,7 +111,7 @@ mbsn_error mbsn_write_single_coil(mbsn_t* mbsn, uint16_t address, bool value);
|
||||
|
||||
mbsn_error mbsn_write_single_register(mbsn_t* mbsn, uint16_t address, uint16_t value);
|
||||
|
||||
mbsn_error mbsn_write_multiple_coils(mbsn_t* mbsn, uint16_t address, uint16_t quantity, const mbsn_bitfield* coils);
|
||||
mbsn_error mbsn_write_multiple_coils(mbsn_t* mbsn, uint16_t address, uint16_t quantity, const mbsn_bitfield coils);
|
||||
|
||||
mbsn_error mbsn_write_multiple_registers(mbsn_t* mbsn, uint16_t address, uint16_t quantity, const uint16_t* registers);
|
||||
|
||||
|
||||
@ -74,9 +74,7 @@ void test_server_receive_base(mbsn_transport transport) {
|
||||
|
||||
uint64_t diff = now_ms() - start;
|
||||
|
||||
if (diff < read_timeout_ms) {
|
||||
fail();
|
||||
}
|
||||
assert(diff >= read_timeout_ms);
|
||||
}
|
||||
|
||||
should("honor byte_timeout and return MBSN_ERROR_TIMEOUT");
|
||||
@ -90,7 +88,7 @@ void test_server_receive_base(mbsn_transport transport) {
|
||||
err = mbsn_server_create(&mbsn, TEST_SERVER_ADDR, transport_conf, (mbsn_callbacks){});
|
||||
check(err);
|
||||
|
||||
mbsn_set_read_timeout(&mbsn, -1);
|
||||
mbsn_set_read_timeout(&mbsn, 1000);
|
||||
mbsn_set_byte_timeout(&mbsn, byte_timeout_ms);
|
||||
|
||||
err = mbsn_server_receive(&mbsn);
|
||||
|
||||
@ -1,11 +1,11 @@
|
||||
#include "modbusino.h"
|
||||
#include <assert.h>
|
||||
#include <stdio.h>
|
||||
#include <time.h>
|
||||
#include <unistd.h>
|
||||
|
||||
unsigned int nesting = 0;
|
||||
|
||||
#define fail() (assert(false))
|
||||
|
||||
#define should(s) \
|
||||
{ \
|
||||
for (int i = 0; i < nesting; i++) { \
|
||||
@ -25,26 +25,31 @@ unsigned int nesting = 0;
|
||||
|
||||
const uint8_t TEST_SERVER_ADDR = 1;
|
||||
|
||||
static uint64_t now_ms() {
|
||||
|
||||
uint64_t now_ms() {
|
||||
struct timespec ts = {0, 0};
|
||||
clock_gettime(CLOCK_MONOTONIC_RAW, &ts);
|
||||
return (uint64_t) (ts.tv_sec) * 1000 + (uint64_t) (ts.tv_nsec) / 1000000;
|
||||
}
|
||||
|
||||
int read_byte_empty(uint8_t* b, uint32_t timeout) {
|
||||
|
||||
int read_byte_empty(uint8_t* b, int32_t timeout) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
int write_byte_empty(uint8_t b, uint32_t timeout) {
|
||||
|
||||
int write_byte_empty(uint8_t b, int32_t timeout) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
int read_byte_timeout_1s(uint8_t* b, uint32_t timeout) {
|
||||
|
||||
int read_byte_timeout_1s(uint8_t* b, int32_t timeout) {
|
||||
usleep(timeout * 1000);
|
||||
return 0;
|
||||
}
|
||||
|
||||
int read_byte_timeout_third(uint8_t* b, uint32_t timeout) {
|
||||
|
||||
int read_byte_timeout_third(uint8_t* b, int32_t timeout) {
|
||||
static int stage = 0;
|
||||
switch (stage) {
|
||||
case 0:
|
||||
@ -53,7 +58,8 @@ int read_byte_timeout_third(uint8_t* b, uint32_t timeout) {
|
||||
stage++;
|
||||
return 1;
|
||||
case 2:
|
||||
usleep(timeout * 1000 + 100);
|
||||
assert(timeout > 0);
|
||||
usleep(timeout * 1000 + 100 * 1000);
|
||||
stage = 0;
|
||||
return 0;
|
||||
default:
|
||||
|
||||
Loading…
Reference in New Issue
Block a user