|
@@ -188,6 +188,7 @@ static int _modbus_tcp_check_integrity(modbus_t *ctx, uint8_t *msg, const int ms
|
|
|
static int _modbus_tcp_pre_check_confirmation(modbus_t *ctx, const uint8_t *req,
|
|
|
const uint8_t *rsp, int rsp_length)
|
|
|
{
|
|
|
+ unsigned int protocol_id;
|
|
|
/* Check transaction ID */
|
|
|
if (req[0] != rsp[0] || req[1] != rsp[1]) {
|
|
|
if (ctx->debug) {
|
|
@@ -199,10 +200,11 @@ static int _modbus_tcp_pre_check_confirmation(modbus_t *ctx, const uint8_t *req,
|
|
|
}
|
|
|
|
|
|
/* Check protocol ID */
|
|
|
- if (rsp[2] != 0x0 && rsp[3] != 0x0) {
|
|
|
+ protocol_id = (rsp[2] << 8) + rsp[3];
|
|
|
+ if (protocol_id != 0x0) {
|
|
|
if (ctx->debug) {
|
|
|
fprintf(stderr, "Invalid protocol ID received 0x%X (not 0x0)\n",
|
|
|
- (rsp[2] << 8) + rsp[3]);
|
|
|
+ protocol_id);
|
|
|
}
|
|
|
errno = EMBBADDATA;
|
|
|
return -1;
|