From e36c06aae87a5d176d86715312ab19678e2667c3 Mon Sep 17 00:00:00 2001 From: mj Date: Wed, 22 Dec 2021 17:18:43 +0100 Subject: [PATCH] Fixed non-deterministic buffer pos increments --- fw/Core/Src/modbus.c | 21 +++++++++++---------- 1 file changed, 11 insertions(+), 10 deletions(-) diff --git a/fw/Core/Src/modbus.c b/fw/Core/Src/modbus.c index d19f1ea..36c2869 100644 --- a/fw/Core/Src/modbus.c +++ b/fw/Core/Src/modbus.c @@ -162,6 +162,7 @@ static int8_t modbus_transaction_to_buffer(uint8_t *buffer, uint8_t *msg_len, mo crc16 = modbus_CRC16(buffer, buffer_pos); /* last two bytes is the checksum itself */ buffer[buffer_pos++] = crc16 & 0xff; buffer[buffer_pos++] = crc16 >> 8; + return MODBUS_OK; } static int8_t modbus_process_device_id_request(const uint8_t *buffer, int len, modbus_transaction_t *transaction) @@ -169,8 +170,6 @@ static int8_t modbus_process_device_id_request(const uint8_t *buffer, int len, m uint8_t MEI_type; uint8_t read_device_id_code; uint8_t object_id; - uint8_t conformity_masked; - uint8_t individual_object_access; uint8_t buffer_pos = 0; if (transaction->broadcast == 1) { @@ -199,9 +198,6 @@ static int8_t modbus_process_device_id_request(const uint8_t *buffer, int len, m transaction->exception = MODBUS_EXCEPTION_ILLEGAL_DEVICE_ID_CODE; return MODBUS_OK; } - if (read_device_id_code == MODBUS_INDIVIDUAL_ACCESS) { - individual_object_access = 1; - } transaction->read_device_id_code = read_device_id_code; /* next byte is object id */ object_id = buffer[buffer_pos++]; @@ -267,14 +263,17 @@ static int8_t modbus_process_read_write_request(const uint8_t *buffer, int len, /* buffer too short to contain everything we need */ return MODBUS_ERROR; } - transaction->register_address = (buffer[buffer_pos++] << 8) | buffer[buffer_pos++]; + transaction->register_address = (buffer[buffer_pos + 1] << 8) | buffer[buffer_pos + 2]; + buffer += 2; // TODO check length! if (flags & MODBUS_FLAG_WRITE) { if (flags & MODBUS_FLAG_SINGLE) { - transaction->holding_registers[0] = (buffer[buffer_pos++] << 8) | buffer[buffer_pos++]; + transaction->holding_registers[0] = (buffer[buffer_pos + 1] << 8) | buffer[buffer_pos + 2]; + buffer_pos += 2; } else { /* Write multiple registers */ - transaction->register_count = (buffer[buffer_pos++] << 8) | buffer[buffer_pos++]; + transaction->register_count = (buffer[buffer_pos + 1] << 8) | buffer[buffer_pos + 2]; + buffer_pos += 2; if (len < MODBUS_MINIMAL_WRITE_MULTIPLE_LEN) { return MODBUS_ERROR; } @@ -287,12 +286,14 @@ static int8_t modbus_process_read_write_request(const uint8_t *buffer, int len, return MODBUS_ERROR; } for (uint8_t i = 0; i < transaction->register_count; i++) { - transaction->holding_registers[i] = (buffer[buffer_pos++] << 8) | buffer[buffer_pos++]; + transaction->holding_registers[i] = (buffer[buffer_pos + 1] << 8) | buffer[buffer_pos + 2]; + buffer_pos += 2; } } } } else { - transaction->register_count = (buffer[buffer_pos++] << 8) | buffer[buffer_pos++]; + transaction->register_count = (buffer[buffer_pos + 1] << 8) | buffer[buffer_pos + 2]; + buffer_pos += 2; if ( transaction->register_count < 1 || transaction->register_count > MODBUS_MAX_REGISTERS