New read and write registers function

src/modbus-private.h

@@ -61,6 +61,7 @@ MODBUS_BEGIN_DECLS
 #define FC_WRITE_MULTIPLE_COILS      0x0F
 #define FC_WRITE_MULTIPLE_REGISTERS  0x10
 #define FC_REPORT_SLAVE_ID           0x11
+#define FC_READ_AND_WRITE_REGISTERS  0x17
 
 typedef enum { RTU=0, TCP } type_com_t;
 

src/modbus.c

@@ -264,6 +264,7 @@ static unsigned int compute_response_length(modbus_t *ctx, uint8_t *req)
         length = 2 + (nb / 8) + ((nb % 8) ? 1 : 0);
     }
         break;
+    case FC_READ_AND_WRITE_REGISTERS:
     case FC_READ_HOLDING_REGISTERS:
     case FC_READ_INPUT_REGISTERS:
         /* Header + 2 * nb values */
@@ -512,6 +513,8 @@ static uint8_t compute_header_length(int function, msg_type_t msg_type)
             length = 0;
         else
             length = 1;
+    } else if (function == FC_READ_AND_WRITE_REGISTERS) {
+        length = 9;
     } else {
         length = 0;
     }
@@ -529,6 +532,8 @@ static int compute_data_length(modbus_t *ctx, uint8_t *msg)
         length = msg[TAB_HEADER_LENGTH[ctx->type_com] + 5];
     } else if (function == FC_REPORT_SLAVE_ID) {
         length = msg[TAB_HEADER_LENGTH[ctx->type_com] + 1];
+    } else if (function == FC_READ_AND_WRITE_REGISTERS) {
+        length = msg[TAB_HEADER_LENGTH[ctx->type_com] + 9];
     } else
         length = 0;
 
@@ -789,6 +794,7 @@ static int receive_msg_req(modbus_t *ctx, uint8_t *req, uint8_t *rsp)
             req_nb_value = (req_nb_value / 8) + ((req_nb_value % 8) ? 1 : 0);
             rsp_nb_value = rsp[offset + 1];
             break;
+        case FC_READ_AND_WRITE_REGISTERS:
         case FC_READ_HOLDING_REGISTERS:
         case FC_READ_INPUT_REGISTERS:
             /* Read functions 1 value = 2 bytes */
@@ -1130,6 +1136,40 @@ int modbus_reply(modbus_t *ctx, const uint8_t *req,
         errno = ENOPROTOOPT;
         return -1;
         break;
+
+    case FC_READ_AND_WRITE_REGISTERS: {
+        int nb = (req[offset + 3] << 8) + req[offset + 4];
+        uint16_t address_write = (req[offset + 5] << 8) + req[offset + 6];
+        int nb_write = (req[offset + 7] << 8) + req[offset + 8];
+
+        if ((address + nb) > mb_mapping->nb_registers &&
+            (address_write + nb_write) > mb_mapping->nb_registers) {
+            if (ctx->debug) {
+                fprintf(stderr,
+                        "Illegal data read address %0X or write address %0X in read_and_write_registers\n",
+                        address + nb, address_write + nb_write);
+            }
+            rsp_length = response_exception(ctx, &sft,
+                                            MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp);
+        } else {
+            int i, j;
+            rsp_length = build_response_basis(ctx, &sft, rsp);
+            rsp[rsp_length++] = nb << 1;
+
+            for (i = address; i < address + nb; i++) {
+                rsp[rsp_length++] = mb_mapping->tab_registers[i] >> 8;
+                rsp[rsp_length++] = mb_mapping->tab_registers[i] & 0xFF;
+            }
+
+            /* 10 and 11 = first value */
+            for (i = address_write, j = 10; i < address_write + nb_write; i++, j += 2) {
+                mb_mapping->tab_registers[i] =
+                    (req[offset + j] << 8) + req[offset + j + 1];
+            }
+        }
+    }
+        break;
+
     default:
         rsp_length = response_exception(ctx, &sft,
                                         MODBUS_EXCEPTION_ILLEGAL_FUNCTION,
@@ -1445,8 +1485,73 @@ int modbus_write_registers(modbus_t *ctx, int addr, int nb, const uint16_t *data
     return rc;
 }
 
+/* Read multiple registers from remote device to dest array and write multiple
+   registers to remote device from data array. */
+int modbus_read_and_write_holding_registers(modbus_t *ctx,
+                                            int read_addr, int read_nb, uint16_t *dest,
+                                            int write_addr, int write_nb, const uint16_t *data)
+{
+    int rc;
+    int req_length;
+    int i;
+    int byte_count;
+    uint8_t req[MAX_MESSAGE_LENGTH];
+    uint8_t rsp[MAX_MESSAGE_LENGTH];
+
+    if (read_nb > MODBUS_MAX_READ_REGISTERS) {
+        if (ctx->debug) {
+            fprintf(stderr,
+                    "ERROR Too many holding registers requested (%d > %d)\n",
+                    read_nb, MODBUS_MAX_READ_REGISTERS);
+        }
+        errno = EMBMDATA;
+        return -1;
+    }
+
+    if (write_nb > MODBUS_MAX_RW_WRITE_REGISTERS) {
+        if (ctx->debug) {
+            fprintf(stderr,
+                    "ERROR Too many holding registers wrote (%d > %d)\n",
+                    write_nb, MODBUS_MAX_RW_WRITE_REGISTERS);
+        }
+        errno = EMBMDATA;
+        return -1;
+    }
+    req_length = build_request_basis(ctx, FC_READ_AND_WRITE_REGISTERS,
+                                     read_addr, read_nb, req);
+
+    req[req_length++] = write_addr >> 8;
+    req[req_length++] = write_addr & 0x00ff;
+    req[req_length++] = write_nb >> 8;
+    req[req_length++] = write_nb & 0x00ff;
+    byte_count = write_nb * 2;
+    req[req_length++] = byte_count;
+
+    for (i = 0; i < write_nb; i++) {
+        req[req_length++] = data[i] >> 8;
+        req[req_length++] = data[i] & 0x00FF;
+    }
+
+    rc = send_msg(ctx, req, req_length);
+    if (rc > 0) {
+        int offset;
+
+        rc = receive_msg_req(ctx, req, rsp);
+        offset = TAB_HEADER_LENGTH[ctx->type_com];
+
+        /* If rc is negative, the loop is jumped ! */
+        for (i = 0; i < rc; i++) {
+            /* shift reg hi_byte to temp OR with lo_byte */
+            dest[i] = (rsp[offset + 2 + (i << 1)] << 8) |
+                rsp[offset + 3 + (i << 1)];
+        }
+    }
+
+    return rc;
+}
+
 /* Send a request to get the slave ID of the device (only available in serial
- * communication). */
+   communication). */
 int modbus_report_slave_id(modbus_t *ctx, uint8_t *data_dest)
 {
     int rc;

src/modbus.h

@@ -182,6 +182,9 @@ int modbus_write_bit(modbus_t *ctx, int coil_addr, int state);
 int modbus_write_register(modbus_t *ctx, int reg_addr, int value);
 int modbus_write_bits(modbus_t *ctx, int addr, int nb, const uint8_t *data);
 int modbus_write_registers(modbus_t *ctx, int addr, int nb, const uint16_t *data);
+int modbus_read_and_write_holding_registers(modbus_t *ctx, int read_addr,
+                                     int read_nb, uint16_t *dest, int write_addr,
+                                     int write_nb, const uint16_t *data);
 int modbus_report_slave_id(modbus_t *ctx, uint8_t *dest);
 
 modbus_mapping_t* modbus_mapping_new(int nb_coil_status, int nb_input_status,

tests/bandwidth-client.c

@@ -143,6 +143,43 @@ int main(void)
     printf("* %'d KiB/s\n", rate);
     printf("\n");
 
+    printf("READ AND WRITE REGISTERS\n\n");
+
+    nb_points = MODBUS_MAX_RW_WRITE_REGISTERS;
+    start = gettime_ms();
+    for (i=0; i<NB_LOOPS; i++) {
+        rc = modbus_read_and_write_holding_registers(ctx, 0, nb_points, tab_reg,0, nb_points, tab_reg);
+        if (rc == -1) {
+            fprintf(stderr, "%s\n", modbus_strerror(errno));
+            return -1;
+        }
+    }
+    end = gettime_ms();
+    elapsed = end - start;
+
+    rate = (NB_LOOPS * nb_points) * G_MSEC_PER_SEC / (end - start);
+    printf("Transfert rate in points/seconds:\n");
+    printf("* %'d registers/s\n", rate);
+    printf("\n");
+
+    bytes = NB_LOOPS * nb_points * sizeof(uint16_t);
+    rate = bytes / 1024 * G_MSEC_PER_SEC / (end - start);
+    printf("Values:\n");
+    printf("* %d x %d values\n", NB_LOOPS, nb_points);
+    printf("* %.3f ms for %d bytes\n", elapsed, bytes);
+    printf("* %'d KiB/s\n", rate);
+    printf("\n");
+
+    /* TCP:Query and reponse header and values */
+    bytes = 12 + 9 + (nb_points * sizeof(uint16_t));
+    printf("Values and TCP Modbus overhead:\n");
+    printf("* %d x %d bytes\n", NB_LOOPS, bytes);
+    bytes = NB_LOOPS * bytes;
+    rate = bytes / 1024 * G_MSEC_PER_SEC / (end - start);
+    printf("* %.3f ms for %d bytes\n", elapsed, bytes);
+    printf("* %'d KiB/s\n", rate);
+    printf("\n");
+
     /* Free the memory */
     free(tab_bit);
     free(tab_reg);

tests/random-test-client.c

@@ -56,6 +56,7 @@ int main(void)
     uint8_t *tab_rq_bits;
     uint8_t *tab_rp_bits;
     uint16_t *tab_rq_registers;
+    uint16_t *tab_rw_rq_registers;
     uint16_t *tab_rp_registers;
 
     /*
@@ -88,6 +89,9 @@ int main(void)
     tab_rp_registers = (uint16_t *) malloc(nb * sizeof(uint16_t));
     memset(tab_rp_registers, 0, nb * sizeof(uint16_t));
 
+    tab_rw_rq_registers = (uint16_t *) malloc(nb * sizeof(uint16_t));
+    memset(tab_rw_rq_registers, 0, nb * sizeof(uint16_t));
+
     nb_loop = nb_fail = 0;
     while (nb_loop++ < LOOP) {
         for (addr = ADDRESS_START; addr <= ADDRESS_END; addr++) {
@@ -96,6 +100,7 @@ int main(void)
             /* Random numbers (short) */
             for (i=0; i<nb; i++) {
                 tab_rq_registers[i] = (uint16_t) (65535.0*rand() / (RAND_MAX + 1.0));
+                tab_rw_rq_registers[i] = ~tab_rq_registers[i];
                 tab_rq_bits[i] = tab_rq_registers[i] % 2;
             }
             nb = ADDRESS_END - addr;
@@ -188,7 +193,41 @@ int main(void)
                     }
                 }
             }
+            /* R/W MULTIPLE REGISTERS*/
+            rc = modbus_read_and_write_holding_registers(ctx, addr, nb, tab_rp_registers,
+                                                         addr, nb, tab_rw_rq_registers);
+            if (rc != nb) {
+                printf("ERROR modbus_read_and_write_registers (%d)\n", rc);
+                printf("Address = %d, nb = %d\n", addr, nb);
+                nb_fail++;
+            } else {
+                for (i=0; i<nb; i++) {
+                    if (tab_rq_registers[i] != tab_rp_registers[i]) {
+                        printf("ERROR modbus_read_and_write_registers READ\n");
+                        printf("Address = %d, value %d (0x%X) != %d (0x%X)\n",
+                               addr, tab_rq_registers[i], tab_rq_registers[i],
+                               tab_rp_registers[i], tab_rp_registers[i]);
+                        nb_fail++;
+                    }
+                }
 
+                rc = modbus_read_registers(ctx, addr, nb, tab_rp_registers);
+                if (rc != nb) {
+                    printf("ERROR modbus_read_registers (%d)\n", rc);
+                    printf("Address = %d, nb = %d\n", addr, nb);
+                    nb_fail++;
+                } else {
+                    for (i=0; i<nb; i++) {
+                        if (tab_rw_rq_registers[i] != tab_rp_registers[i]) {
+                            printf("ERROR modbus_read_and_write_registers WRITE\n");
+                            printf("Address = %d, value %d (0x%X) != %d (0x%X)\n",
+                                   addr, tab_rw_rq_registers[i], tab_rw_rq_registers[i],
+                                   tab_rp_registers[i], tab_rp_registers[i]);
+                            nb_fail++;
+                        }
+                    }
+                }
+            }
         }
 
         printf("Test: ");

tests/unit-test-client.c

@@ -204,7 +204,7 @@ int main(void)
     rc = modbus_write_registers(ctx, UT_REGISTERS_ADDRESS,
                                 UT_REGISTERS_NB_POINTS,
                                 UT_REGISTERS_TAB);
-    printf("1/3 modbus_write_registers: ");
+    printf("1/5 modbus_write_registers: ");
     if (rc == UT_REGISTERS_NB_POINTS) {
         printf("OK\n");
     } else {
@@ -215,7 +215,7 @@ int main(void)
     rc = modbus_read_registers(ctx, UT_REGISTERS_ADDRESS,
                                UT_REGISTERS_NB_POINTS,
                                tab_rp_registers);
-    printf("2/3 modbus_read_registers: ");
+    printf("2/5 modbus_read_registers: ");
     if (rc != UT_REGISTERS_NB_POINTS) {
         printf("FAILED (nb points %d)\n", rc);
         goto close;
@@ -233,13 +233,62 @@ int main(void)
 
     rc = modbus_read_registers(ctx, UT_REGISTERS_ADDRESS,
                                0, tab_rp_registers);
-    printf("3/3 modbus_read_registers (0): ");
+    printf("3/5 modbus_read_registers (0): ");
     if (rc != 0) {
         printf("FAILED (nb points %d)\n", rc);
         goto close;
     }
     printf("OK\n");
 
+    nb_points = (UT_REGISTERS_NB_POINTS >
+                 UT_INPUT_REGISTERS_NB_POINTS) ?
+        UT_REGISTERS_NB_POINTS : UT_INPUT_REGISTERS_NB_POINTS;
+    memset(tab_rp_registers, 0, nb_points * sizeof(uint16_t));
+
+    /* Write registers to zero from tab_rp_registers and read registers to
+       tab_rp_registers.  They should be same as UT_REGISTERS_TAB. */
+    rc = modbus_read_and_write_holding_registers(ctx,
+                                                 UT_REGISTERS_ADDRESS,
+                                                 UT_REGISTERS_NB_POINTS,
+                                                 tab_rp_registers,
+                                                 UT_REGISTERS_ADDRESS,
+                                                 UT_REGISTERS_NB_POINTS,
+                                                 tab_rp_registers);
+    printf("4/5 modbus_read_and_write_registers, read part: ");
+    if (rc != UT_REGISTERS_NB_POINTS) {
+        printf("FAILED (nb points %d)\n", rc);
+        goto close;
+    }
+
+    for (i=0; i < UT_REGISTERS_NB_POINTS; i++) {
+        if (tab_rp_registers[i] != UT_REGISTERS_TAB[i]) {
+            printf("FAILED (%0X != %0X)\n",
+                   tab_rp_registers[i],
+                   UT_REGISTERS_TAB[i]);
+            goto close;
+        }
+    }
+    printf("OK\n");
+
+    rc = modbus_read_registers(ctx, UT_REGISTERS_ADDRESS,
+                               UT_REGISTERS_NB_POINTS,
+                               tab_rp_registers);
+    printf("5/5 modbus_read_and_write_registers, write part: ");
+    if (rc != UT_REGISTERS_NB_POINTS) {
+        printf("FAILED (nb points %d)\n", rc);
+        goto close;
+    }
+
+    for (i=0; i < UT_REGISTERS_NB_POINTS; i++) {
+        if (tab_rp_registers[i] != 0) {
+            printf("FAILED (%0X != %0X)\n",
+                   tab_rp_registers[i],
+                   UT_REGISTERS_TAB[i]);
+            goto close;
+        }
+    }
+    printf("OK\n");
+
     /* End of many registers */