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 136 /* Treats errors and flush or close connection if necessary */
137 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 141 if (mb_param->error_handling == FLUSH_OR_RECONNECT_ON_ERROR) {
142 142 switch (code) {
... ... @@ -405,7 +405,7 @@ static uint8_t compute_query_size_header(uint8_t function)
405 405 else
406 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 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 423 byte = 0;
424 424  
425 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 428 return byte;
429 429 }
... ... @@ -556,7 +556,8 @@ int receive_msg(modbus_param_t *mb_param,
556 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 562 /* Moves the pointer to receive other datas */
562 563 p_msg = &(p_msg[read_ret]);
... ... @@ -733,46 +734,50 @@ void manage_query(modbus_param_t *mb_param, uint8_t *query,
733 734 int response_size = 0;
734 735  
735 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 743 response_size = response_exception(mb_param, slave, function,
740 744 ILLEGAL_DATA_ADDRESS, response);
741 745 } else {
742   - int count = (query[offset+4] << 8) + query[offset+5];
743   -
744   - // FIXME Check address + count
745   -
746 746 offset = build_response_basis(mb_param, slave, function, response);
747 747 response[offset++] = (count / 8) + ((count % 8) ? 1 : 0);
748 748 response_size = response_io_status(address, count,
749 749 mb_mapping->tab_coil_status,
750 750 response, offset);
751 751 }
  752 + }
752 753 break;
753   - case FC_READ_INPUT_STATUS:
  754 + case FC_READ_INPUT_STATUS: {
754 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 761 response_size = response_exception(mb_param, slave, function,
758 762 ILLEGAL_DATA_ADDRESS, response);
759 763 } else {
760   - int count = (query[offset+4] << 8) + query[offset+5];
761   -
762 764 offset = build_response_basis(mb_param, slave, function, response);
763 765 response[offset++] = (count / 8) + ((count % 8) ? 1 : 0);
764 766 response_size = response_io_status(address, count,
765 767 mb_mapping->tab_input_status,
766 768 response, offset);
767 769 }
  770 + }
768 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 778 response_size = response_exception(mb_param, slave, function,
773 779 ILLEGAL_DATA_ADDRESS, response);
774 780 } else {
775   - int count = (query[offset+4] << 8) + query[offset+5];
776 781 int i;
777 782  
778 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 788 }
784 789 response_size = offset;
785 790 }
  791 + }
786 792 break;
787   - case FC_READ_INPUT_REGISTERS:
  793 + case FC_READ_INPUT_REGISTERS: {
788 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 800 response_size = response_exception(mb_param, slave, function,
792 801 ILLEGAL_DATA_ADDRESS, response);
793 802 } else {
794   - int count = (query[offset+4] << 8) + query[offset+5];
795 803 int i;
796 804  
797 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 810 }
803 811 response_size = offset;
804 812 }
  813 + }
805 814 break;
806 815 case FC_FORCE_SINGLE_COIL:
807 816 if (address >= mb_mapping->nb_coil_status) {
... ...