From 73ce26e098dd41a1dcdbd2021c13ea3df7af1c4e Mon Sep 17 00:00:00 2001 From: Stéphane Raimbault Date: Wed, 9 Apr 2008 09:07:05 +0200 Subject: [PATCH] Enhance detection of ILLEGAL_DATA_ADDRESS error. --- modbus/modbus.c | 57 +++++++++++++++++++++++++++++++++------------------------ 1 file changed, 33 insertions(+), 24 deletions(-) diff --git a/modbus/modbus.c b/modbus/modbus.c index da13d40..09b2c67 100644 --- a/modbus/modbus.c +++ b/modbus/modbus.c @@ -136,7 +136,7 @@ static int read_reg_response(modbus_param_t *mb_param, /* Treats errors and flush or close connection if necessary */ static void error_treat(int code, const char *string, modbus_param_t *mb_param) { - printf("\n\nERROR %s (%.2X)\n\n", string, code); + printf("\nERROR %s (%d)\n", string, code); if (mb_param->error_handling == FLUSH_OR_RECONNECT_ON_ERROR) { switch (code) { @@ -405,7 +405,7 @@ static uint8_t compute_query_size_header(uint8_t function) else byte = 0; - printf("compute_query_size_header FC %d, B%d\n", function, byte); +// printf("compute_query_size_header FC %d, B%d\n", function, byte); return byte; } @@ -423,7 +423,7 @@ static uint8_t compute_query_size_data(modbus_param_t *mb_param, uint8_t *msg) byte = 0; byte += mb_param->checksum_size; - printf("compute_query_size_data FC %d, B %d\n", function, byte); +// printf("compute_query_size_data FC %d, B %d\n", function, byte); return byte; } @@ -556,7 +556,8 @@ int receive_msg(modbus_param_t *mb_param, break; } } - printf("\nsize_to_read: %d\n", size_to_read); + if (mb_param->debug) + printf("\nsize_to_read: %d\n", size_to_read); /* Moves the pointer to receive other datas */ p_msg = &(p_msg[read_ret]); @@ -733,46 +734,50 @@ void manage_query(modbus_param_t *mb_param, uint8_t *query, int response_size = 0; switch (function) { - case FC_READ_COIL_STATUS: - if (address >= mb_mapping->nb_coil_status) { - printf("Illegal data address %0X in read_coil_status\n", address); + case FC_READ_COIL_STATUS: { + int count = (query[offset+4] << 8) + query[offset+5]; + + if ((address + count) > mb_mapping->nb_coil_status) { + printf("Illegal data address %0X in read_coil_status\n", + address + count); response_size = response_exception(mb_param, slave, function, ILLEGAL_DATA_ADDRESS, response); } else { - int count = (query[offset+4] << 8) + query[offset+5]; - - // FIXME Check address + count - offset = build_response_basis(mb_param, slave, function, response); response[offset++] = (count / 8) + ((count % 8) ? 1 : 0); response_size = response_io_status(address, count, mb_mapping->tab_coil_status, response, offset); } + } break; - case FC_READ_INPUT_STATUS: + case FC_READ_INPUT_STATUS: { /* Similar to coil status */ - if (address >= mb_mapping->nb_input_status) { - printf("Illegal data address %0X in read_input_status\n", address); + int count = (query[offset+4] << 8) + query[offset+5]; + + if ((address + count) > mb_mapping->nb_input_status) { + printf("Illegal data address %0X in read_input_status\n", + address + count); response_size = response_exception(mb_param, slave, function, ILLEGAL_DATA_ADDRESS, response); } else { - int count = (query[offset+4] << 8) + query[offset+5]; - offset = build_response_basis(mb_param, slave, function, response); response[offset++] = (count / 8) + ((count % 8) ? 1 : 0); response_size = response_io_status(address, count, mb_mapping->tab_input_status, response, offset); } + } break; - case FC_READ_HOLDING_REGISTERS: - if (address >= mb_mapping->nb_holding_registers) { - printf("Illegal data address %0X in read_holding_registers\n", address); + case FC_READ_HOLDING_REGISTERS: { + int count = (query[offset+4] << 8) + query[offset+5]; + + if ((address + count) > mb_mapping->nb_holding_registers) { + printf("Illegal data address %0X in read_holding_registers\n", + address + count); response_size = response_exception(mb_param, slave, function, ILLEGAL_DATA_ADDRESS, response); } else { - int count = (query[offset+4] << 8) + query[offset+5]; int i; offset = build_response_basis(mb_param, slave, function, response); @@ -783,15 +788,18 @@ void manage_query(modbus_param_t *mb_param, uint8_t *query, } response_size = offset; } + } break; - case FC_READ_INPUT_REGISTERS: + case FC_READ_INPUT_REGISTERS: { /* Similar to holding registers */ - if (address >= mb_mapping->nb_input_registers) { - printf("Illegal data address %0X in read_input_registers\n", address); + int count = (query[offset+4] << 8) + query[offset+5]; + + if ((address + count) > mb_mapping->nb_input_registers) { + printf("Illegal data address %0X in read_input_registers\n", + address + count); response_size = response_exception(mb_param, slave, function, ILLEGAL_DATA_ADDRESS, response); } else { - int count = (query[offset+4] << 8) + query[offset+5]; int i; offset = build_response_basis(mb_param, slave, function, response); @@ -802,6 +810,7 @@ void manage_query(modbus_param_t *mb_param, uint8_t *query, } response_size = offset; } + } break; case FC_FORCE_SINGLE_COIL: if (address >= mb_mapping->nb_coil_status) { -- libgit2 0.21.4