Peter M. Groen / libmodbus

Commit 3a6476e34cc2e3fd01f2b97fcfe762bdf0964407

Authored by Stéphane Raimbault
1 parent 05984bc0

Another round of DRY in modbus_reply()

Inline Side-by-side
Showing 1 changed file with 30 additions and 75 deletions
src/modbus.c
  View file @3a6476e
... ... @@ -721,112 +721,67 @@ int modbus_reply(modbus_t *ctx, const uint8_t *req,
721 721  
722 722 /* Data are flushed on illegal number of values errors. */
723 723 switch (function) {
724   - case MODBUS_FC_READ_COILS: {
  724 + case MODBUS_FC_READ_COILS:
  725 + case MODBUS_FC_READ_DISCRETE_INPUTS: {
  726 + unsigned int is_input = (function == MODBUS_FC_READ_DISCRETE_INPUTS);
  727 + int start_bits = is_input ? mb_mapping->start_input_bits : mb_mapping->start_bits;
  728 + int nb_bits = is_input ? mb_mapping->nb_input_bits : mb_mapping->nb_bits;
  729 + uint8_t *tab_bits = is_input ? mb_mapping->tab_input_bits : mb_mapping->tab_bits;
  730 + const char * const name = is_input ? "read_input_bits" : "read_bits";
725 731 int nb = (req[offset + 3] << 8) + req[offset + 4];
726 732 /* The mapping can be shifted to reduce memory consumption and it
727 733 doesn't always start at address zero. */
728   - int mapping_address = address - mb_mapping->start_bits;
  734 + int mapping_address = address - start_bits;
729 735  
730 736 if (nb < 1 || MODBUS_MAX_READ_BITS < nb) {
731 737 rsp_length = response_exception(
732 738 ctx, &sft, MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE, rsp, TRUE,
733   - "Illegal nb of values %d in read_bits (max %d)\n",
734   - nb, MODBUS_MAX_READ_BITS);
735   - } else if (mapping_address < 0 ||
736   - (mapping_address + nb) > mb_mapping->nb_bits) {
  739 + "Illegal nb of values %d in %s (max %d)\n",
  740 + nb, name, MODBUS_MAX_READ_BITS);
  741 + } else if (mapping_address < 0 || (mapping_address + nb) > nb_bits) {
737 742 rsp_length = response_exception(
738 743 ctx, &sft,
739 744 MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp, FALSE,
740   - "Illegal data address 0x%0X in read_bits\n",
741   - mapping_address < 0 ? address : address + nb);
  745 + "Illegal data address 0x%0X in %s\n",
  746 + mapping_address < 0 ? address : address + nb, name);
742 747 } else {
743 748 rsp_length = ctx->backend->build_response_basis(&sft, rsp);
744 749 rsp[rsp_length++] = (nb / 8) + ((nb % 8) ? 1 : 0);
745   - rsp_length = response_io_status(mb_mapping->tab_bits,
746   - mapping_address, nb,
  750 + rsp_length = response_io_status(tab_bits, mapping_address, nb,
747 751 rsp, rsp_length);
748 752 }
749 753 }
750 754 break;
751   - case MODBUS_FC_READ_DISCRETE_INPUTS: {
752   - /* Similar to coil status (but too many arguments to use a
753   - * function) */
754   - int nb = (req[offset + 3] << 8) + req[offset + 4];
755   - int mapping_address = address - mb_mapping->start_input_bits;
756   -
757   - if (nb < 1 || MODBUS_MAX_READ_BITS < nb) {
758   - rsp_length = response_exception(
759   - ctx, &sft, MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE, rsp, TRUE,
760   - "Illegal nb of values %d in read_input_bits (max %d)\n",
761   - nb, MODBUS_MAX_READ_BITS);
762   - } else if (mapping_address < 0 ||
763   - (mapping_address + nb) > mb_mapping->nb_input_bits) {
764   - rsp_length = response_exception(
765   - ctx, &sft,
766   - MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp, FALSE,
767   - "Illegal data address 0x%0X in read_input_bits\n",
768   - mapping_address < 0 ? address : address + nb);
769   - } else {
770   - rsp_length = ctx->backend->build_response_basis(&sft, rsp);
771   - rsp[rsp_length++] = (nb / 8) + ((nb % 8) ? 1 : 0);
772   - rsp_length = response_io_status(mb_mapping->tab_input_bits,
773   - mapping_address, nb,
774   - rsp, rsp_length);
775   - }
776   - }
777   - break;
778   - case MODBUS_FC_READ_HOLDING_REGISTERS: {
779   - int nb = (req[offset + 3] << 8) + req[offset + 4];
780   - int mapping_address = address - mb_mapping->start_registers;
781   -
782   - if (nb < 1 || MODBUS_MAX_READ_REGISTERS < nb) {
783   - rsp_length = response_exception(
784   - ctx, &sft, MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE, rsp, TRUE,
785   - "Illegal nb of values %d in read_registers (max %d)\n",
786   - nb, MODBUS_MAX_READ_REGISTERS);
787   - } else if (mapping_address < 0 ||
788   - (mapping_address + nb) > mb_mapping->nb_registers) {
789   - rsp_length = response_exception(
790   - ctx, &sft, MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp, FALSE,
791   - "Illegal data address 0x%0X in read_registers\n",
792   - mapping_address < 0 ? address : address + nb);
793   - } else {
794   - int i;
795   -
796   - rsp_length = ctx->backend->build_response_basis(&sft, rsp);
797   - rsp[rsp_length++] = nb << 1;
798   - for (i = mapping_address; i < mapping_address + nb; i++) {
799   - rsp[rsp_length++] = mb_mapping->tab_registers[i] >> 8;
800   - rsp[rsp_length++] = mb_mapping->tab_registers[i] & 0xFF;
801   - }
802   - }
803   - }
804   - break;
  755 + case MODBUS_FC_READ_HOLDING_REGISTERS:
805 756 case MODBUS_FC_READ_INPUT_REGISTERS: {
806   - /* Similar to holding registers (but too many arguments to use a
807   - * function) */
  757 + unsigned int is_input = (function == MODBUS_FC_READ_INPUT_REGISTERS);
  758 + int start_registers = is_input ? mb_mapping->start_input_registers : mb_mapping->start_registers;
  759 + int nb_registers = is_input ? mb_mapping->nb_input_registers : mb_mapping->nb_registers;
  760 + uint16_t *tab_registers = is_input ? mb_mapping->tab_input_registers : mb_mapping->tab_registers;
  761 + const char * const name = is_input ? "read_input_registers" : "read_registers";
808 762 int nb = (req[offset + 3] << 8) + req[offset + 4];
809   - int mapping_address = address - mb_mapping->start_input_registers;
  763 + /* The mapping can be shifted to reduce memory consumption and it
  764 + doesn't always start at address zero. */
  765 + int mapping_address = address - start_registers;
810 766  
811 767 if (nb < 1 || MODBUS_MAX_READ_REGISTERS < nb) {
812 768 rsp_length = response_exception(
813 769 ctx, &sft, MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE, rsp, TRUE,
814   - "Illegal number of values %d in read_input_registers (max %d)\n",
815   - nb, MODBUS_MAX_READ_REGISTERS);
816   - } else if (mapping_address < 0 ||
817   - (mapping_address + nb) > mb_mapping->nb_input_registers) {
  770 + "Illegal nb of values %d in %s (max %d)\n",
  771 + nb, name, MODBUS_MAX_READ_REGISTERS);
  772 + } else if (mapping_address < 0 || (mapping_address + nb) > nb_registers) {
818 773 rsp_length = response_exception(
819 774 ctx, &sft, MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp, FALSE,
820   - "Illegal data address 0x%0X in read_input_registers\n",
821   - mapping_address < 0 ? address : address + nb);
  775 + "Illegal data address 0x%0X in %s\n",
  776 + mapping_address < 0 ? address : address + nb, name);
822 777 } else {
823 778 int i;
824 779  
825 780 rsp_length = ctx->backend->build_response_basis(&sft, rsp);
826 781 rsp[rsp_length++] = nb << 1;
827 782 for (i = mapping_address; i < mapping_address + nb; i++) {
828   - rsp[rsp_length++] = mb_mapping->tab_input_registers[i] >> 8;
829   - rsp[rsp_length++] = mb_mapping->tab_input_registers[i] & 0xFF;
  783 + rsp[rsp_length++] = tab_registers[i] >> 8;
  784 + rsp[rsp_length++] = tab_registers[i] & 0xFF;
830 785 }
831 786 }
832 787 }
... ...