Peter M. Groen / libmodbus

Commit 618f06bafb7a213eeaa88a7fca68285434b22f4c

Authored by Stéphane Raimbault
1 parent 084e7e6a

Check the number of values in the response is corresponding to the query.

Inline Side-by-side
Showing 1 changed file with 33 additions and 13 deletions
modbus/modbus.c
  View file @618f06b
... ... @@ -616,32 +616,51 @@ static int modbus_receive(modbus_param_t *mb_param,
616 616 response, &response_length);
617 617 if (ret == 0) {
618 618 /* GOOD RESPONSE */
  619 + int query_nb_value;
  620 + int response_nb_value;
619 621  
620   - /* The number of values is returned for the following
621   - * cases */
  622 + /* The number of values is returned if it's corresponding
  623 + * to the query */
622 624 switch (response[offset + 1]) {
623 625 case FC_READ_COIL_STATUS:
624 626 case FC_READ_INPUT_STATUS:
625   - /* Read functions 1 value = 1 byte */
626   - ret = response[offset + 2];
  627 + /* Read functions, 8 values in a byte (nb
  628 + * of values in the query and byte count in
  629 + * the response. */
  630 + query_nb_value = (query[offset+4] << 8) + query[offset+5];
  631 + query_nb_value = (query_nb_value / 8) + ((query_nb_value % 8) ? 1 : 0);
  632 + response_nb_value = response[offset + 2];
627 633 break;
628 634 case FC_READ_HOLDING_REGISTERS:
629 635 case FC_READ_INPUT_REGISTERS:
630 636 /* Read functions 1 value = 2 bytes */
631   - ret = response[offset + 2] / 2;
  637 + query_nb_value = (query[offset+4] << 8) + query[offset+5];
  638 + response_nb_value = (response[offset + 2] / 2);
632 639 break;
633 640 case FC_FORCE_MULTIPLE_COILS:
634 641 case FC_PRESET_MULTIPLE_REGISTERS:
635 642 /* N Write functions */
636   - ret = response[offset + 4] << 8 |
637   - response[offset + 5];
  643 + query_nb_value = (query[offset+4] << 8) + query[offset+5];
  644 + response_nb_value = (response[offset + 4] << 8) | response[offset + 5];
638 645 break;
639 646 case FC_REPORT_SLAVE_ID:
640 647 /* Report slave ID (bytes received) */
  648 + query_nb_value = response_nb_value = response_length;
641 649 break;
642 650 default:
643 651 /* 1 Write functions & others */
644   - ret = 1;
  652 + query_nb_value = response_nb_value = 1;
  653 + }
  654 +
  655 + if (query_nb_value == response_nb_value) {
  656 + ret = response_nb_value;
  657 + } else {
  658 + char *s_error = malloc(64 * sizeof(char));
  659 + sprintf(s_error, "Quantity (%d) not corresponding to the query (%d)",
  660 + response_nb_value, query_nb_value);
  661 + ret = ILLEGAL_DATA_VALUE;
  662 + error_treat(mb_param, ILLEGAL_DATA_VALUE, s_error);
  663 + free(s_error);
645 664 }
646 665 } else if (ret == COMM_TIME_OUT) {
647 666  
... ... @@ -669,9 +688,8 @@ static int modbus_receive(modbus_param_t *mb_param,
669 688 if (exception_code < NB_TAB_ERROR_MSG) {
670 689 error_treat(mb_param, -exception_code,
671 690 TAB_ERROR_MSG[response[offset + 2]]);
  691 + /* RETURN THE EXCEPTION CODE */
672 692 /* Modbus error code is negative */
673   -
674   - /* RETURN THE GOOD EXCEPTION CODE */
675 693 return -exception_code;
676 694 } else {
677 695 /* The chances are low to hit this
... ... @@ -781,7 +799,8 @@ void modbus_manage_query(modbus_param_t *mb_param, const uint8_t *query,
781 799 }
782 800 break;
783 801 case FC_READ_INPUT_STATUS: {
784   - /* Similar to coil status */
  802 + /* Similar to coil status (but too much arguments to use a
  803 + * function) */
785 804 int nb = (query[offset+4] << 8) + query[offset+5];
786 805  
787 806 if ((address + nb) > mb_mapping->nb_input_status) {
... ... @@ -819,7 +838,8 @@ void modbus_manage_query(modbus_param_t *mb_param, const uint8_t *query,
819 838 }
820 839 break;
821 840 case FC_READ_INPUT_REGISTERS: {
822   - /* Similar to holding registers */
  841 + /* Similar to holding registers (but too much arguments to use a
  842 + * function) */
823 843 int nb = (query[offset+4] << 8) + query[offset+5];
824 844  
825 845 if ((address + nb) > mb_mapping->nb_input_registers) {
... ... @@ -1059,7 +1079,7 @@ static int read_registers(modbus_param_t *mb_param, int slave, int function,
1059 1079 /* If ret is negative, the loop is jumped ! */
1060 1080 for (i = 0; i < ret; i++) {
1061 1081 /* shift reg hi_byte to temp OR with lo_byte */
1062   - data_dest[i] = response[offset + 3 + (i << 1)] << 8 |
  1082 + data_dest[i] = (response[offset + 3 + (i << 1)] << 8) |
1063 1083 response[offset + 4 + (i << 1)];
1064 1084 }
1065 1085 }
... ...