Peter M. Groen / libmodbus

Commit 73ce26e098dd41a1dcdbd2021c13ea3df7af1c4e

Authored by Stéphane Raimbault
1 parent 4cf22c31

Enhance detection of ILLEGAL_DATA_ADDRESS error.

Inline Side-by-side
Showing 1 changed file with 33 additions and 24 deletions
modbus/modbus.c
  View file @73ce26e
@@ -136,7 +136,7 @@ static int read_reg_response(modbus_param_t *mb_param, @@ -136,7 +136,7 @@ static int read_reg_response(modbus_param_t *mb_param,
136 /* Treats errors and flush or close connection if necessary */ 136 /* Treats errors and flush or close connection if necessary */
137 static void error_treat(int code, const char *string, modbus_param_t *mb_param) 137 static void error_treat(int code, const char *string, modbus_param_t *mb_param)
138 { 138 {
139 - printf("\n\nERROR %s (%.2X)\n\n", string, code); 139 + printf("\nERROR %s (%d)\n", string, code);
140 140
141 if (mb_param->error_handling == FLUSH_OR_RECONNECT_ON_ERROR) { 141 if (mb_param->error_handling == FLUSH_OR_RECONNECT_ON_ERROR) {
142 switch (code) { 142 switch (code) {
@@ -405,7 +405,7 @@ static uint8_t compute_query_size_header(uint8_t function) @@ -405,7 +405,7 @@ static uint8_t compute_query_size_header(uint8_t function)
405 else 405 else
406 byte = 0; 406 byte = 0;
407 407
408 - printf("compute_query_size_header FC %d, B%d\n", function, byte); 408 +// printf("compute_query_size_header FC %d, B%d\n", function, byte);
409 409
410 return byte; 410 return byte;
411 } 411 }
@@ -423,7 +423,7 @@ static uint8_t compute_query_size_data(modbus_param_t *mb_param, uint8_t *msg) @@ -423,7 +423,7 @@ static uint8_t compute_query_size_data(modbus_param_t *mb_param, uint8_t *msg)
423 byte = 0; 423 byte = 0;
424 424
425 byte += mb_param->checksum_size; 425 byte += mb_param->checksum_size;
426 - printf("compute_query_size_data FC %d, B %d\n", function, byte); 426 +// printf("compute_query_size_data FC %d, B %d\n", function, byte);
427 427
428 return byte; 428 return byte;
429 } 429 }
@@ -556,7 +556,8 @@ int receive_msg(modbus_param_t *mb_param, @@ -556,7 +556,8 @@ int receive_msg(modbus_param_t *mb_param,
556 break; 556 break;
557 } 557 }
558 } 558 }
559 - printf("\nsize_to_read: %d\n", size_to_read); 559 + if (mb_param->debug)
  560 + printf("\nsize_to_read: %d\n", size_to_read);
560 561
561 /* Moves the pointer to receive other datas */ 562 /* Moves the pointer to receive other datas */
562 p_msg = &(p_msg[read_ret]); 563 p_msg = &(p_msg[read_ret]);
@@ -733,46 +734,50 @@ void manage_query(modbus_param_t *mb_param, uint8_t *query, @@ -733,46 +734,50 @@ void manage_query(modbus_param_t *mb_param, uint8_t *query,
733 int response_size = 0; 734 int response_size = 0;
734 735
735 switch (function) { 736 switch (function) {
736 - case FC_READ_COIL_STATUS:  
737 - if (address >= mb_mapping->nb_coil_status) {  
738 - printf("Illegal data address %0X in read_coil_status\n", address); 737 + case FC_READ_COIL_STATUS: {
  738 + int count = (query[offset+4] << 8) + query[offset+5];
  739 +
  740 + if ((address + count) > mb_mapping->nb_coil_status) {
  741 + printf("Illegal data address %0X in read_coil_status\n",
  742 + address + count);
739 response_size = response_exception(mb_param, slave, function, 743 response_size = response_exception(mb_param, slave, function,
740 ILLEGAL_DATA_ADDRESS, response); 744 ILLEGAL_DATA_ADDRESS, response);
741 } else { 745 } else {
742 - int count = (query[offset+4] << 8) + query[offset+5];  
743 -  
744 - // FIXME Check address + count  
745 -  
746 offset = build_response_basis(mb_param, slave, function, response); 746 offset = build_response_basis(mb_param, slave, function, response);
747 response[offset++] = (count / 8) + ((count % 8) ? 1 : 0); 747 response[offset++] = (count / 8) + ((count % 8) ? 1 : 0);
748 response_size = response_io_status(address, count, 748 response_size = response_io_status(address, count,
749 mb_mapping->tab_coil_status, 749 mb_mapping->tab_coil_status,
750 response, offset); 750 response, offset);
751 } 751 }
  752 + }
752 break; 753 break;
753 - case FC_READ_INPUT_STATUS: 754 + case FC_READ_INPUT_STATUS: {
754 /* Similar to coil status */ 755 /* Similar to coil status */
755 - if (address >= mb_mapping->nb_input_status) {  
756 - printf("Illegal data address %0X in read_input_status\n", address); 756 + int count = (query[offset+4] << 8) + query[offset+5];
  757 +
  758 + if ((address + count) > mb_mapping->nb_input_status) {
  759 + printf("Illegal data address %0X in read_input_status\n",
  760 + address + count);
757 response_size = response_exception(mb_param, slave, function, 761 response_size = response_exception(mb_param, slave, function,
758 ILLEGAL_DATA_ADDRESS, response); 762 ILLEGAL_DATA_ADDRESS, response);
759 } else { 763 } else {
760 - int count = (query[offset+4] << 8) + query[offset+5];  
761 -  
762 offset = build_response_basis(mb_param, slave, function, response); 764 offset = build_response_basis(mb_param, slave, function, response);
763 response[offset++] = (count / 8) + ((count % 8) ? 1 : 0); 765 response[offset++] = (count / 8) + ((count % 8) ? 1 : 0);
764 response_size = response_io_status(address, count, 766 response_size = response_io_status(address, count,
765 mb_mapping->tab_input_status, 767 mb_mapping->tab_input_status,
766 response, offset); 768 response, offset);
767 } 769 }
  770 + }
768 break; 771 break;
769 - case FC_READ_HOLDING_REGISTERS:  
770 - if (address >= mb_mapping->nb_holding_registers) {  
771 - printf("Illegal data address %0X in read_holding_registers\n", address); 772 + case FC_READ_HOLDING_REGISTERS: {
  773 + int count = (query[offset+4] << 8) + query[offset+5];
  774 +
  775 + if ((address + count) > mb_mapping->nb_holding_registers) {
  776 + printf("Illegal data address %0X in read_holding_registers\n",
  777 + address + count);
772 response_size = response_exception(mb_param, slave, function, 778 response_size = response_exception(mb_param, slave, function,
773 ILLEGAL_DATA_ADDRESS, response); 779 ILLEGAL_DATA_ADDRESS, response);
774 } else { 780 } else {
775 - int count = (query[offset+4] << 8) + query[offset+5];  
776 int i; 781 int i;
777 782
778 offset = build_response_basis(mb_param, slave, function, response); 783 offset = build_response_basis(mb_param, slave, function, response);
@@ -783,15 +788,18 @@ void manage_query(modbus_param_t *mb_param, uint8_t *query, @@ -783,15 +788,18 @@ void manage_query(modbus_param_t *mb_param, uint8_t *query,
783 } 788 }
784 response_size = offset; 789 response_size = offset;
785 } 790 }
  791 + }
786 break; 792 break;
787 - case FC_READ_INPUT_REGISTERS: 793 + case FC_READ_INPUT_REGISTERS: {
788 /* Similar to holding registers */ 794 /* Similar to holding registers */
789 - if (address >= mb_mapping->nb_input_registers) {  
790 - printf("Illegal data address %0X in read_input_registers\n", address); 795 + int count = (query[offset+4] << 8) + query[offset+5];
  796 +
  797 + if ((address + count) > mb_mapping->nb_input_registers) {
  798 + printf("Illegal data address %0X in read_input_registers\n",
  799 + address + count);
791 response_size = response_exception(mb_param, slave, function, 800 response_size = response_exception(mb_param, slave, function,
792 ILLEGAL_DATA_ADDRESS, response); 801 ILLEGAL_DATA_ADDRESS, response);
793 } else { 802 } else {
794 - int count = (query[offset+4] << 8) + query[offset+5];  
795 int i; 803 int i;
796 804
797 offset = build_response_basis(mb_param, slave, function, response); 805 offset = build_response_basis(mb_param, slave, function, response);
@@ -802,6 +810,7 @@ void manage_query(modbus_param_t *mb_param, uint8_t *query, @@ -802,6 +810,7 @@ void manage_query(modbus_param_t *mb_param, uint8_t *query,
802 } 810 }
803 response_size = offset; 811 response_size = offset;
804 } 812 }
  813 + }
805 break; 814 break;
806 case FC_FORCE_SINGLE_COIL: 815 case FC_FORCE_SINGLE_COIL:
807 if (address >= mb_mapping->nb_coil_status) { 816 if (address >= mb_mapping->nb_coil_status) {