Peter M. Groen / libmodbus

Commit 73a88a747c473a7427ef70f0351ff6f5370c3495

Authored by Stéphane Raimbault
1 parent 902c2873

DRY in modbus_reply by improving response_exception() 

Thanks to Rüdiger Ranft for the idea.
Inline Side-by-side
Showing 1 changed file with 78 additions and 138 deletions
src/modbus.c
  View file @73a88a7
... ... @@ -10,6 +10,7 @@
10 10 #include <stdio.h>
11 11 #include <string.h>
12 12 #include <stdlib.h>
  13 +#include <stdarg.h>
13 14 #include <errno.h>
14 15 #include <limits.h>
15 16 #include <time.h>
... ... @@ -658,14 +659,30 @@ static int response_io_status(uint8_t *tab_io_status,
658 659  
659 660 /* Build the exception response */
660 661 static int response_exception(modbus_t *ctx, sft_t *sft,
661   - int exception_code, uint8_t *rsp)
  662 + int exception_code, uint8_t *rsp,
  663 + unsigned int to_flush,
  664 + const char* template, ...)
662 665 {
663 666 int rsp_length;
664 667  
  668 + /* Print debug message */
  669 + if (ctx->debug) {
  670 + va_list ap;
  671 +
  672 + va_start(ap, template);
  673 + vfprintf(stderr, template, ap);
  674 + va_end(ap);
  675 + }
  676 +
  677 + /* Flush if required */
  678 + if (to_flush) {
  679 + _sleep_response_timeout(ctx);
  680 + modbus_flush(ctx);
  681 + }
  682 +
  683 + /* Build exception response */
665 684 sft->function = sft->function + 0x80;
666 685 rsp_length = ctx->backend->build_response_basis(sft, rsp);
667   -
668   - /* Positive exception code */
669 686 rsp[rsp_length++] = exception_code;
670 687  
671 688 return rsp_length;
... ... @@ -711,25 +728,17 @@ int modbus_reply(modbus_t *ctx, const uint8_t *req,
711 728 int mapping_address = address - mb_mapping->start_bits;
712 729  
713 730 if (nb < 1 || MODBUS_MAX_READ_BITS < nb) {
714   - if (ctx->debug) {
715   - fprintf(stderr,
716   - "Illegal nb of values %d in read_bits (max %d)\n",
717   - nb, MODBUS_MAX_READ_BITS);
718   - }
719   - _sleep_response_timeout(ctx);
720   - modbus_flush(ctx);
721 731 rsp_length = response_exception(
722   - ctx, &sft,
723   - MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE, rsp);
  732 + 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);
724 735 } else if (mapping_address < 0 ||
725 736 (mapping_address + nb) > mb_mapping->nb_bits) {
726   - if (ctx->debug) {
727   - fprintf(stderr, "Illegal data address 0x%0X in read_bits\n",
728   - mapping_address < 0 ? address : address + nb);
729   - }
730 737 rsp_length = response_exception(
731 738 ctx, &sft,
732   - MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp);
  739 + MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp, FALSE,
  740 + "Illegal data address 0x%0X in read_bits\n",
  741 + mapping_address < 0 ? address : address + nb);
733 742 } else {
734 743 rsp_length = ctx->backend->build_response_basis(&sft, rsp);
735 744 rsp[rsp_length++] = (nb / 8) + ((nb % 8) ? 1 : 0);
... ... @@ -746,25 +755,17 @@ int modbus_reply(modbus_t *ctx, const uint8_t *req,
746 755 int mapping_address = address - mb_mapping->start_input_bits;
747 756  
748 757 if (nb < 1 || MODBUS_MAX_READ_BITS < nb) {
749   - if (ctx->debug) {
750   - fprintf(stderr,
751   - "Illegal nb of values %d in read_input_bits (max %d)\n",
752   - nb, MODBUS_MAX_READ_BITS);
753   - }
754   - _sleep_response_timeout(ctx);
755   - modbus_flush(ctx);
756 758 rsp_length = response_exception(
757   - ctx, &sft,
758   - MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE, rsp);
  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);
759 762 } else if (mapping_address < 0 ||
760 763 (mapping_address + nb) > mb_mapping->nb_input_bits) {
761   - if (ctx->debug) {
762   - fprintf(stderr, "Illegal data address 0x%0X in read_input_bits\n",
763   - mapping_address < 0 ? address : address + nb);
764   - }
765 764 rsp_length = response_exception(
766 765 ctx, &sft,
767   - MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp);
  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);
768 769 } else {
769 770 rsp_length = ctx->backend->build_response_basis(&sft, rsp);
770 771 rsp[rsp_length++] = (nb / 8) + ((nb % 8) ? 1 : 0);
... ... @@ -779,25 +780,16 @@ int modbus_reply(modbus_t *ctx, const uint8_t *req,
779 780 int mapping_address = address - mb_mapping->start_registers;
780 781  
781 782 if (nb < 1 || MODBUS_MAX_READ_REGISTERS < nb) {
782   - if (ctx->debug) {
783   - fprintf(stderr,
784   - "Illegal nb of values %d in read_holding_registers (max %d)\n",
785   - nb, MODBUS_MAX_READ_REGISTERS);
786   - }
787   - _sleep_response_timeout(ctx);
788   - modbus_flush(ctx);
789 783 rsp_length = response_exception(
790   - ctx, &sft,
791   - MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE, rsp);
  784 + ctx, &sft, MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE, rsp, TRUE,
  785 + "Illegal nb of values %d in read_holding_registers (max %d)\n",
  786 + nb, MODBUS_MAX_READ_REGISTERS);
792 787 } else if (mapping_address < 0 ||
793 788 (mapping_address + nb) > mb_mapping->nb_registers) {
794   - if (ctx->debug) {
795   - fprintf(stderr, "Illegal data address 0x%0X in read_registers\n",
796   - mapping_address < 0 ? address : address + nb);
797   - }
798 789 rsp_length = response_exception(
799   - ctx, &sft,
800   - MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp);
  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);
801 793 } else {
802 794 int i;
803 795  
... ... @@ -817,25 +809,16 @@ int modbus_reply(modbus_t *ctx, const uint8_t *req,
817 809 int mapping_address = address - mb_mapping->start_input_registers;
818 810  
819 811 if (nb < 1 || MODBUS_MAX_READ_REGISTERS < nb) {
820   - if (ctx->debug) {
821   - fprintf(stderr,
822   - "Illegal number of values %d in read_input_registers (max %d)\n",
823   - nb, MODBUS_MAX_READ_REGISTERS);
824   - }
825   - _sleep_response_timeout(ctx);
826   - modbus_flush(ctx);
827 812 rsp_length = response_exception(
828   - ctx, &sft,
829   - MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE, rsp);
  813 + 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);
830 816 } else if (mapping_address < 0 ||
831 817 (mapping_address + nb) > mb_mapping->nb_input_registers) {
832   - if (ctx->debug) {
833   - fprintf(stderr, "Illegal data address 0x%0X in read_input_registers\n",
834   - mapping_address < 0 ? address : address + nb);
835   - }
836 818 rsp_length = response_exception(
837   - ctx, &sft,
838   - MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp);
  819 + 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);
839 822 } else {
840 823 int i;
841 824  
... ... @@ -852,14 +835,10 @@ int modbus_reply(modbus_t *ctx, const uint8_t *req,
852 835 int mapping_address = address - mb_mapping->start_bits;
853 836  
854 837 if (mapping_address < 0 || mapping_address >= mb_mapping->nb_bits) {
855   - if (ctx->debug) {
856   - fprintf(stderr,
857   - "Illegal data address 0x%0X in write_bit\n",
858   - address);
859   - }
860 838 rsp_length = response_exception(
861   - ctx, &sft,
862   - MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp);
  839 + ctx, &sft, MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp, FALSE,
  840 + "Illegal data address 0x%0X in write_bit\n",
  841 + address);
863 842 } else {
864 843 int data = (req[offset + 3] << 8) + req[offset + 4];
865 844  
... ... @@ -868,14 +847,11 @@ int modbus_reply(modbus_t *ctx, const uint8_t *req,
868 847 memcpy(rsp, req, req_length);
869 848 rsp_length = req_length;
870 849 } else {
871   - if (ctx->debug) {
872   - fprintf(stderr,
873   - "Illegal data value 0x%0X in write_bit request at address %0X\n",
874   - data, address);
875   - }
876 850 rsp_length = response_exception(
877 851 ctx, &sft,
878   - MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE, rsp);
  852 + MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE, rsp, FALSE,
  853 + "Illegal data value 0x%0X in write_bit request at address %0X\n",
  854 + data, address);
879 855 }
880 856 }
881 857 }
... ... @@ -884,13 +860,11 @@ int modbus_reply(modbus_t *ctx, const uint8_t *req,
884 860 int mapping_address = address - mb_mapping->start_registers;
885 861  
886 862 if (mapping_address < 0 || mapping_address >= mb_mapping->nb_registers) {
887   - if (ctx->debug) {
888   - fprintf(stderr, "Illegal data address 0x%0X in write_register\n",
889   - address);
890   - }
891 863 rsp_length = response_exception(
892 864 ctx, &sft,
893   - MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp);
  865 + MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp, FALSE,
  866 + "Illegal data address 0x%0X in write_register\n",
  867 + address);
894 868 } else {
895 869 int data = (req[offset + 3] << 8) + req[offset + 4];
896 870  
... ... @@ -905,28 +879,20 @@ int modbus_reply(modbus_t *ctx, const uint8_t *req,
905 879 int mapping_address = address - mb_mapping->start_bits;
906 880  
907 881 if (nb < 1 || MODBUS_MAX_WRITE_BITS < nb) {
908   - if (ctx->debug) {
909   - fprintf(stderr,
910   - "Illegal number of values %d in write_bits (max %d)\n",
911   - nb, MODBUS_MAX_WRITE_BITS);
912   - }
913 882 /* May be the indication has been truncated on reading because of
914 883 * invalid address (eg. nb is 0 but the request contains values to
915 884 * write) so it's necessary to flush. */
916   - _sleep_response_timeout(ctx);
917   - modbus_flush(ctx);
918 885 rsp_length = response_exception(
919   - ctx, &sft,
920   - MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE, rsp);
  886 + ctx, &sft, MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE, rsp, TRUE,
  887 + "Illegal number of values %d in write_bits (max %d)\n",
  888 + nb, MODBUS_MAX_WRITE_BITS);
921 889 } else if (mapping_address < 0 ||
922 890 (mapping_address + nb) > mb_mapping->nb_bits) {
923   - if (ctx->debug) {
924   - fprintf(stderr, "Illegal data address 0x%0X in write_bits\n",
925   - mapping_address < 0 ? address : address + nb);
926   - }
927 891 rsp_length = response_exception(
928 892 ctx, &sft,
929   - MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp);
  893 + MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp, FALSE,
  894 + "Illegal data address 0x%0X in write_bits\n",
  895 + mapping_address < 0 ? address : address + nb);
930 896 } else {
931 897 /* 6 = byte count */
932 898 modbus_set_bits_from_bytes(mb_mapping->tab_bits, mapping_address, nb,
... ... @@ -944,28 +910,16 @@ int modbus_reply(modbus_t *ctx, const uint8_t *req,
944 910 int mapping_address = address - mb_mapping->start_registers;
945 911  
946 912 if (nb < 1 || MODBUS_MAX_WRITE_REGISTERS < nb) {
947   - if (ctx->debug) {
948   - fprintf(stderr,
949   - "Illegal number of values %d in write_registers (max %d)\n",
950   - nb, MODBUS_MAX_WRITE_REGISTERS);
951   - }
952   - /* May be the indication has been truncated on reading because of
953   - * invalid address (eg. nb is 0 but the request contains values to
954   - * write) so it's necessary to flush. */
955   - _sleep_response_timeout(ctx);
956   - modbus_flush(ctx);
957 913 rsp_length = response_exception(
958   - ctx, &sft,
959   - MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE, rsp);
  914 + ctx, &sft, MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE, rsp, TRUE,
  915 + "Illegal number of values %d in write_registers (max %d)\n",
  916 + nb, MODBUS_MAX_WRITE_REGISTERS);
960 917 } else if (mapping_address < 0 ||
961 918 (mapping_address + nb) > mb_mapping->nb_registers) {
962   - if (ctx->debug) {
963   - fprintf(stderr, "Illegal data address 0x%0X in write_registers\n",
964   - mapping_address < 0 ? address : address + nb);
965   - }
966 919 rsp_length = response_exception(
967   - ctx, &sft,
968   - MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp);
  920 + ctx, &sft, MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp, FALSE,
  921 + "Illegal data address 0x%0X in write_registers\n",
  922 + mapping_address < 0 ? address : address + nb);
969 923 } else {
970 924 int i, j;
971 925 for (i = mapping_address, j = 6; i < mapping_address + nb; i++, j += 2) {
... ... @@ -1009,13 +963,10 @@ int modbus_reply(modbus_t *ctx, const uint8_t *req,
1009 963 int mapping_address = address - mb_mapping->start_registers;
1010 964  
1011 965 if (mapping_address < 0 || mapping_address >= mb_mapping->nb_registers) {
1012   - if (ctx->debug) {
1013   - fprintf(stderr, "Illegal data address 0x%0X in write_register\n",
1014   - address);
1015   - }
1016 966 rsp_length = response_exception(
1017   - ctx, &sft,
1018   - MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp);
  967 + ctx, &sft, MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp, FALSE,
  968 + "Illegal data address 0x%0X in write_register\n",
  969 + address);
1019 970 } else {
1020 971 uint16_t data = mb_mapping->tab_registers[mapping_address];
1021 972 uint16_t and = (req[offset + 3] << 8) + req[offset + 4];
... ... @@ -1039,29 +990,19 @@ int modbus_reply(modbus_t *ctx, const uint8_t *req,
1039 990 if (nb_write < 1 || MODBUS_MAX_WR_WRITE_REGISTERS < nb_write ||
1040 991 nb < 1 || MODBUS_MAX_WR_READ_REGISTERS < nb ||
1041 992 nb_write_bytes != nb_write * 2) {
1042   - if (ctx->debug) {
1043   - fprintf(stderr,
1044   - "Illegal nb of values (W%d, R%d) in write_and_read_registers (max W%d, R%d)\n",
1045   - nb_write, nb,
1046   - MODBUS_MAX_WR_WRITE_REGISTERS, MODBUS_MAX_WR_READ_REGISTERS);
1047   - }
1048   - _sleep_response_timeout(ctx);
1049   - modbus_flush(ctx);
1050 993 rsp_length = response_exception(
1051   - ctx, &sft,
1052   - MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE, rsp);
  994 + ctx, &sft, MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE, rsp, TRUE,
  995 + "Illegal nb of values (W%d, R%d) in write_and_read_registers (max W%d, R%d)\n",
  996 + nb_write, nb, MODBUS_MAX_WR_WRITE_REGISTERS, MODBUS_MAX_WR_READ_REGISTERS);
1053 997 } else if (mapping_address < 0 ||
1054 998 (mapping_address + nb) > mb_mapping->nb_registers ||
1055 999 mapping_address < 0 ||
1056 1000 (mapping_address_write + nb_write) > mb_mapping->nb_registers) {
1057   - if (ctx->debug) {
1058   - fprintf(stderr,
1059   - "Illegal data read address 0x%0X or write address 0x%0X write_and_read_registers\n",
1060   - mapping_address < 0 ? address : address + nb,
1061   - mapping_address_write < 0 ? address_write : address_write + nb_write);
1062   - }
1063   - rsp_length = response_exception(ctx, &sft,
1064   - MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp);
  1001 + rsp_length = response_exception(
  1002 + ctx, &sft, MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp, FALSE,
  1003 + "Illegal data read address 0x%0X or write address 0x%0X write_and_read_registers\n",
  1004 + mapping_address < 0 ? address : address + nb,
  1005 + mapping_address_write < 0 ? address_write : address_write + nb_write);
1065 1006 } else {
1066 1007 int i, j;
1067 1008 rsp_length = ctx->backend->build_response_basis(&sft, rsp);
... ... @@ -1085,9 +1026,8 @@ int modbus_reply(modbus_t *ctx, const uint8_t *req,
1085 1026 break;
1086 1027  
1087 1028 default:
1088   - rsp_length = response_exception(ctx, &sft,
1089   - MODBUS_EXCEPTION_ILLEGAL_FUNCTION,
1090   - rsp);
  1029 + rsp_length = response_exception(
  1030 + ctx, &sft, MODBUS_EXCEPTION_ILLEGAL_FUNCTION, rsp, FALSE, "");
1091 1031 break;
1092 1032 }
1093 1033  
... ...