From 9a2733e7ecc2c622704753fae7fbf14521de10ea Mon Sep 17 00:00:00 2001 From: Hannu Vuolasaho Date: Tue, 10 Aug 2010 08:39:54 +0200 Subject: [PATCH] New read and write registers function --- src/modbus-private.h | 1 + src/modbus.c | 107 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++- src/modbus.h | 3 +++ tests/bandwidth-client.c | 37 +++++++++++++++++++++++++++++++++++++ tests/random-test-client.c | 39 +++++++++++++++++++++++++++++++++++++++ tests/unit-test-client.c | 55 ++++++++++++++++++++++++++++++++++++++++++++++++++++--- 6 files changed, 238 insertions(+), 4 deletions(-) diff --git a/src/modbus-private.h b/src/modbus-private.h index 2bde32d..1089f7f 100644 --- a/src/modbus-private.h +++ b/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; diff --git a/src/modbus.c b/src/modbus.c index 1d7b137..b269203 100644 --- a/src/modbus.c +++ b/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; diff --git a/src/modbus.h b/src/modbus.h index 852b231..8f60859 100644 --- a/src/modbus.h +++ b/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, diff --git a/tests/bandwidth-client.c b/tests/bandwidth-client.c index f562018..44d8fae 100644 --- a/tests/bandwidth-client.c +++ b/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 + 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 */ -- libgit2 0.21.4