diff --git a/src/modbus-private.h b/src/modbus-private.h index 6cd34248..ea83187f 100644 --- a/src/modbus-private.h +++ b/src/modbus-private.h @@ -75,7 +75,7 @@ typedef struct _modbus_backend { int (*build_request_basis)( modbus_t *ctx, int function, int addr, int nb, uint8_t *req); int (*build_response_basis)(sft_t *sft, uint8_t *rsp); - int (*prepare_response_tid)(const uint8_t *req, int *req_length); + int (*get_response_tid)(const uint8_t *req); int (*send_msg_pre)(uint8_t *req, int req_length); ssize_t (*send)(modbus_t *ctx, const uint8_t *req, int req_length); int (*receive)(modbus_t *ctx, uint8_t *req); diff --git a/src/modbus-rtu.c b/src/modbus-rtu.c index b7749230..8b5ee08a 100644 --- a/src/modbus-rtu.c +++ b/src/modbus-rtu.c @@ -129,9 +129,8 @@ static uint16_t crc16(uint8_t *buffer, uint16_t buffer_length) return (crc_hi << 8 | crc_lo); } -static int _modbus_rtu_prepare_response_tid(const uint8_t *req, int *req_length) +static int _modbus_rtu_get_response_tid(const uint8_t *req) { - (*req_length) -= _MODBUS_RTU_CHECKSUM_LENGTH; /* No TID */ return 0; } @@ -1187,7 +1186,7 @@ const modbus_backend_t _modbus_rtu_backend = { _modbus_set_slave, _modbus_rtu_build_request_basis, _modbus_rtu_build_response_basis, - _modbus_rtu_prepare_response_tid, + _modbus_rtu_get_response_tid, _modbus_rtu_send_msg_pre, _modbus_rtu_send, _modbus_rtu_receive, diff --git a/src/modbus-tcp.c b/src/modbus-tcp.c index 733a4a96..60ac6b46 100644 --- a/src/modbus-tcp.c +++ b/src/modbus-tcp.c @@ -153,7 +153,7 @@ static int _modbus_tcp_build_response_basis(sft_t *sft, uint8_t *rsp) return _MODBUS_TCP_PRESET_RSP_LENGTH; } -static int _modbus_tcp_prepare_response_tid(const uint8_t *req, int *req_length) +static int _modbus_tcp_get_response_tid(const uint8_t *req) { return (req[0] << 8) + req[1]; } @@ -838,7 +838,7 @@ const modbus_backend_t _modbus_tcp_backend = { _modbus_set_slave, _modbus_tcp_build_request_basis, _modbus_tcp_build_response_basis, - _modbus_tcp_prepare_response_tid, + _modbus_tcp_get_response_tid, _modbus_tcp_send_msg_pre, _modbus_tcp_send, _modbus_tcp_receive, @@ -861,7 +861,7 @@ const modbus_backend_t _modbus_tcp_pi_backend = { _modbus_set_slave, _modbus_tcp_build_request_basis, _modbus_tcp_build_response_basis, - _modbus_tcp_prepare_response_tid, + _modbus_tcp_get_response_tid, _modbus_tcp_send_msg_pre, _modbus_tcp_send, _modbus_tcp_receive, diff --git a/src/modbus.c b/src/modbus.c index fe192a73..e3737bb2 100644 --- a/src/modbus.c +++ b/src/modbus.c @@ -125,7 +125,7 @@ int modbus_flush(modbus_t *ctx) return rc; } -/* Computes the length of the expected response */ +/* Computes the length of the expected response including checksum */ static unsigned int compute_response_length_from_request(modbus_t *ctx, uint8_t *req) { int length; @@ -394,8 +394,7 @@ int _modbus_receive_msg(modbus_t *ctx, uint8_t *msg, msg_type_t msg_type) 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 */ + /* Wait for a message, we don't know when the message will be received */ if (ctx->indication_timeout.tv_sec == 0 && ctx->indication_timeout.tv_usec == 0) { /* By default, the indication timeout isn't set */ p_tv = NULL; @@ -805,7 +804,7 @@ int modbus_reply(modbus_t *ctx, sft.slave = slave; sft.function = function; - sft.t_id = ctx->backend->prepare_response_tid(req, &req_length); + sft.t_id = ctx->backend->get_response_tid(req); /* Data are flushed on illegal number of values errors. */ switch (function) { @@ -901,7 +900,7 @@ int modbus_reply(modbus_t *ctx, MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp, FALSE, - "Illegal data address 0x%0X in write_bit\n", + "Illegal data address 0x%0X in write bit\n", address); break; } @@ -910,20 +909,26 @@ int modbus_reply(modbus_t *ctx, rsp_length = compute_response_length_from_request(ctx, (uint8_t *) req); if (rsp_length != req_length) { /* Bad use of modbus_reply */ - rsp_length = - response_exception(ctx, - &sft, - MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE, - rsp, - FALSE, - "Invalid request length used in modbus_reply (%d)\n", - req_length); + rsp_length = response_exception( + ctx, + &sft, + MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE, + rsp, + FALSE, + "Invalid request length in modbus_reply to write bit (%d)\n", + req_length); break; } + /* Don't copy the CRC, if any, it will be computed later (even if identical to the + * request) */ + rsp_length -= ctx->backend->checksum_length; + int data = (req[offset + 3] << 8) + req[offset + 4]; if (data == 0xFF00 || data == 0x0) { + /* Apply the change to mapping */ mb_mapping->tab_bits[mapping_address] = data ? ON : OFF; + /* Prepare response */ memcpy(rsp, req, rsp_length); } else { rsp_length = response_exception( @@ -955,19 +960,21 @@ int modbus_reply(modbus_t *ctx, rsp_length = compute_response_length_from_request(ctx, (uint8_t *) req); if (rsp_length != req_length) { /* Bad use of modbus_reply */ - rsp_length = - response_exception(ctx, - &sft, - MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE, - rsp, - FALSE, - "Invalid request length used in modbus_reply (%d)\n", - req_length); + rsp_length = response_exception( + ctx, + &sft, + MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE, + rsp, + FALSE, + "Invalid request length in modbus_reply to write register (%d)\n", + req_length); break; } int data = (req[offset + 3] << 8) + req[offset + 4]; mb_mapping->tab_registers[mapping_address] = data; + + rsp_length -= ctx->backend->checksum_length; memcpy(rsp, req, rsp_length); } break; case MODBUS_FC_WRITE_MULTIPLE_COILS: { @@ -1088,8 +1095,23 @@ int modbus_reply(modbus_t *ctx, data = (data & and) | (or &(~and) ); mb_mapping->tab_registers[mapping_address] = data; - memcpy(rsp, req, req_length); - rsp_length = req_length; + + rsp_length = compute_response_length_from_request(ctx, (uint8_t *) req); + if (rsp_length != req_length) { + /* Bad use of modbus_reply */ + rsp_length = response_exception(ctx, + &sft, + MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE, + rsp, + FALSE, + "Invalid request length in modbus_reply " + "to mask write register (%d)\n", + req_length); + break; + } + + rsp_length -= ctx->backend->checksum_length; + memcpy(rsp, req, rsp_length); } } break; case MODBUS_FC_WRITE_AND_READ_REGISTERS: { @@ -1177,7 +1199,6 @@ int modbus_reply_exception(modbus_t *ctx, const uint8_t *req, unsigned int excep int function; uint8_t rsp[MAX_MESSAGE_LENGTH]; int rsp_length; - int dummy_length = 99; sft_t sft; if (ctx == NULL) { @@ -1191,7 +1212,7 @@ int modbus_reply_exception(modbus_t *ctx, const uint8_t *req, unsigned int excep sft.slave = slave; sft.function = function + 0x80; - sft.t_id = ctx->backend->prepare_response_tid(req, &dummy_length); + sft.t_id = ctx->backend->get_response_tid(req); rsp_length = ctx->backend->build_response_basis(&sft, rsp); /* Positive exception code */