From 8a998c10cf5f61671dfe21e72f14e0a12e8131bb Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?R=C3=BCdiger=20Ranft?= Date: Mon, 25 Apr 2016 12:42:19 +0200 Subject: [PATCH 01/14] Preparations to create a virtualized mapping. --- src/Makefile.am | 6 ++++-- src/virtual-reply.c | 11 +++++++++++ src/virtual-reply.h | 28 ++++++++++++++++++++++++++++ 3 files changed, 43 insertions(+), 2 deletions(-) create mode 100644 src/virtual-reply.c create mode 100644 src/virtual-reply.h diff --git a/src/Makefile.am b/src/Makefile.am index 551fe4328..80982d55f 100644 --- a/src/Makefile.am +++ b/src/Makefile.am @@ -20,7 +20,8 @@ libmodbus_la_SOURCES = \ modbus-tcp.c \ modbus-tcp.h \ modbus-tcp-private.h \ - modbus-version.h + modbus-version.h \ + virtual-reply.c libmodbus_la_LDFLAGS = -no-undefined \ -version-info $(LIBMODBUS_LT_VERSION_INFO) @@ -35,7 +36,8 @@ endif # Header files to install libmodbusincludedir = $(includedir)/modbus -libmodbusinclude_HEADERS = modbus.h modbus-version.h modbus-rtu.h modbus-tcp.h +libmodbusinclude_HEADERS = modbus.h modbus-version.h modbus-rtu.h modbus-tcp.h \ + virtual-reply.h DISTCLEANFILES = modbus-version.h EXTRA_DIST += modbus-version.h.in diff --git a/src/virtual-reply.c b/src/virtual-reply.c new file mode 100644 index 000000000..34285f3bb --- /dev/null +++ b/src/virtual-reply.c @@ -0,0 +1,11 @@ +#include "virtual-reply.h" + +#include +#include + +void modbus_virtualize_mapping(modbus_vmapping_t* dest, + modbus_mapping_t* source) +{ + memset(dest, 0, sizeof(*dest)); + dest->app = source; +} diff --git a/src/virtual-reply.h b/src/virtual-reply.h new file mode 100644 index 000000000..d8b8033b8 --- /dev/null +++ b/src/virtual-reply.h @@ -0,0 +1,28 @@ +/* + * Copyright © 2001-2013 Rüdiger Ranft + * + * SPDX-License-Identifier: LGPL-2.1+ + */ +#ifndef MODBUS_VIRTUAL_RESPONSE_H +#define MODBUS_VIRTUAL_RESPONSE_H + +#include "modbus.h" + +#ifndef _MSC_VER +#include +#else +#include "stdint.h" +#endif + +typedef struct { + void* app; + uint8_t* (*tab_bits)(void* app, int addr, int nb); + uint8_t* (*tab_input_bits)(void* app, int addr, int nb); + uint16_t* (*tab_input_registers)(void* app, int addr, int nb); + uint16_t* (*tab_registers)(void* app, int addr, int nb); +} modbus_vmapping_t; + +void modbus_virtualize_mapping(modbus_vmapping_t* dest, + modbus_mapping_t* source); + +#endif From 043bdc23130b7156cb6d9ca6ddd15408ce76814e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?R=C3=BCdiger=20Ranft?= Date: Tue, 26 Apr 2016 07:56:21 +0200 Subject: [PATCH 02/14] Virtualized READ_DISCRETE_INPUTS --- src/modbus.c | 58 +++++++++++++++++++++++++++++---------------- src/modbus.h | 5 ++++ src/virtual-reply.c | 18 +++++++++++++- src/virtual-reply.h | 2 -- 4 files changed, 59 insertions(+), 24 deletions(-) diff --git a/src/modbus.c b/src/modbus.c index 1a5a778b6..819f2e3f2 100644 --- a/src/modbus.c +++ b/src/modbus.c @@ -630,7 +630,7 @@ static int check_confirmation(modbus_t *ctx, uint8_t *req, return rc; } -static int response_io_status(int address, int nb, +static int response_io_status(int nb, uint8_t *tab_io_status, uint8_t *rsp, int offset) { @@ -639,7 +639,7 @@ static int response_io_status(int address, int nb, int one_byte = 0; int i; - for (i = address; i < address+nb; i++) { + for (i = 0; i < nb; i++) { one_byte |= tab_io_status[i] << shift; if (shift == 7) { /* Byte is full */ @@ -677,8 +677,9 @@ static int response_exception(modbus_t *ctx, sft_t *sft, If an error occurs, this function construct the response accordingly. */ -int modbus_reply(modbus_t *ctx, const uint8_t *req, - int req_length, modbus_mapping_t *mb_mapping) +int modbus_virt_reply(modbus_t *ctx, const uint8_t *req, + int req_length, modbus_vmapping_t* vm, + modbus_mapping_t *mb_mapping) { int offset; int slave; @@ -688,7 +689,7 @@ int modbus_reply(modbus_t *ctx, const uint8_t *req, int rsp_length = 0; sft_t sft; - if (ctx == NULL) { + if ((ctx == NULL)||(vm==NULL)||(mb_mapping==NULL)||(req==NULL)) { errno = EINVAL; return -1; } @@ -730,8 +731,8 @@ int modbus_reply(modbus_t *ctx, const uint8_t *req, } else { rsp_length = ctx->backend->build_response_basis(&sft, rsp); rsp[rsp_length++] = (nb / 8) + ((nb % 8) ? 1 : 0); - rsp_length = response_io_status(addr, nb, - mb_mapping->tab_bits, + rsp_length = response_io_status(nb, + mb_mapping->tab_bits+addr, rsp, rsp_length); } } @@ -740,7 +741,6 @@ int modbus_reply(modbus_t *ctx, const uint8_t *req, /* Similar to coil status (but too many arguments to use a * function) */ int nb = (req[offset + 3] << 8) + req[offset + 4]; - int addr = address - mb_mapping->offset_input_bits; if (nb < 1 || MODBUS_MAX_READ_BITS < nb) { if (ctx->debug) { @@ -753,20 +753,28 @@ int modbus_reply(modbus_t *ctx, const uint8_t *req, rsp_length = response_exception( ctx, &sft, MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE, rsp); - } else if (address < mb_mapping->offset_input_bits || (addr + nb) > mb_mapping->nb_input_bits) { - if (ctx->debug) { - fprintf(stderr, "Illegal data address 0x%0X in read_input_bits\n", - address < mb_mapping->offset_input_bits ? address : address + nb); - } - rsp_length = response_exception( - ctx, &sft, - MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp); } else { - rsp_length = ctx->backend->build_response_basis(&sft, rsp); - rsp[rsp_length++] = (nb / 8) + ((nb % 8) ? 1 : 0); - rsp_length = response_io_status(addr, nb, - mb_mapping->tab_input_bits, - rsp, rsp_length); + if(!vm->tab_input_bits) + { + errno = EINVAL; + return -1; + } + uint8_t* data = vm->tab_input_bits(vm->app, address, nb); + if(data) { + rsp_length = ctx->backend->build_response_basis(&sft, rsp); + rsp[rsp_length++] = (nb / 8) + ((nb % 8) ? 1 : 0); + rsp_length = response_io_status(nb, + data, + rsp, rsp_length); + } else { + if(ctx->debug) { + fprintf(stderr, "Virtual mapping failed for %d Bits" + " starting at address 0x%X\n", address, nb); + } + rsp_length = response_exception( + ctx, &sft, + MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp); + } } } break; @@ -1085,6 +1093,14 @@ int modbus_reply(modbus_t *ctx, const uint8_t *req, return (slave == MODBUS_BROADCAST_ADDRESS) ? 0 : send_msg(ctx, rsp, rsp_length); } +int modbus_reply(modbus_t *ctx, const uint8_t *req, + int req_length, modbus_mapping_t *mb_mapping) +{ + modbus_vmapping_t vm; + modbus_virtualize_mapping(&vm, mb_mapping); + return modbus_virt_reply(ctx, req, req_length, &vm, mb_mapping); +} + int modbus_reply_exception(modbus_t *ctx, const uint8_t *req, unsigned int exception_code) { diff --git a/src/modbus.h b/src/modbus.h index 47a69d172..e1da163ad 100644 --- a/src/modbus.h +++ b/src/modbus.h @@ -227,6 +227,11 @@ MODBUS_API int modbus_receive(modbus_t *ctx, uint8_t *req); MODBUS_API int modbus_receive_confirmation(modbus_t *ctx, uint8_t *rsp); +#include "virtual-reply.h" + +int modbus_virt_reply(modbus_t *ctx, const uint8_t *req, + int req_length, modbus_vmapping_t *vm, + modbus_mapping_t *mb_mapping); MODBUS_API int modbus_reply(modbus_t *ctx, const uint8_t *req, int req_length, modbus_mapping_t *mb_mapping); MODBUS_API int modbus_reply_exception(modbus_t *ctx, const uint8_t *req, diff --git a/src/virtual-reply.c b/src/virtual-reply.c index 34285f3bb..4380b63d4 100644 --- a/src/virtual-reply.c +++ b/src/virtual-reply.c @@ -1,11 +1,27 @@ -#include "virtual-reply.h" +#include "modbus.h" #include #include +static uint8_t* get_discrete_inputs(void* app, int address, int nb) +{ + if(!app) + return NULL; + + modbus_mapping_t* mb_mapping = app; + int addr = address - mb_mapping->offset_input_bits; + if ( (address < mb_mapping->offset_input_bits) + || ((addr + nb) > mb_mapping->nb_input_bits)) { + return NULL; + } + return mb_mapping->tab_input_bits + addr; +} + + void modbus_virtualize_mapping(modbus_vmapping_t* dest, modbus_mapping_t* source) { memset(dest, 0, sizeof(*dest)); dest->app = source; + dest->tab_input_bits = get_discrete_inputs; } diff --git a/src/virtual-reply.h b/src/virtual-reply.h index d8b8033b8..a0e188ff9 100644 --- a/src/virtual-reply.h +++ b/src/virtual-reply.h @@ -6,8 +6,6 @@ #ifndef MODBUS_VIRTUAL_RESPONSE_H #define MODBUS_VIRTUAL_RESPONSE_H -#include "modbus.h" - #ifndef _MSC_VER #include #else From c86cf0625bc0bedf908751f18d447dd66d4e2efd Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?R=C3=BCdiger=20Ranft?= Date: Tue, 26 Apr 2016 08:37:56 +0200 Subject: [PATCH 03/14] Virtualized READ_COILS and combined it with READ_DISCRETE_INPUTS --- src/modbus.c | 49 +++++++++++---------------------------------- src/virtual-reply.c | 15 ++++++++++++++ 2 files changed, 27 insertions(+), 37 deletions(-) diff --git a/src/modbus.c b/src/modbus.c index 819f2e3f2..595e249a3 100644 --- a/src/modbus.c +++ b/src/modbus.c @@ -705,39 +705,11 @@ int modbus_virt_reply(modbus_t *ctx, const uint8_t *req, /* Data are flushed on illegal number of values errors. */ switch (function) { - case MODBUS_FC_READ_COILS: { - int nb = (req[offset + 3] << 8) + req[offset + 4]; - int addr = address - mb_mapping->offset_bits; - if (nb < 1 || MODBUS_MAX_READ_BITS < nb) { - if (ctx->debug) { - fprintf(stderr, - "Illegal nb of values %d in read_bits (max %d)\n", - nb, MODBUS_MAX_READ_BITS); - } - _sleep_response_timeout(ctx); - modbus_flush(ctx); - rsp_length = response_exception( - ctx, &sft, - MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE, rsp); - } else if (address < mb_mapping->offset_bits || (addr + nb) > mb_mapping->nb_bits) { - if (ctx->debug) { - fprintf(stderr, "Illegal data address 0x%0X in read_bits\n", - address < mb_mapping->offset_bits ? address : address + nb); - } - rsp_length = response_exception( - ctx, &sft, - MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp); - } else { - rsp_length = ctx->backend->build_response_basis(&sft, rsp); - rsp[rsp_length++] = (nb / 8) + ((nb % 8) ? 1 : 0); - rsp_length = response_io_status(nb, - mb_mapping->tab_bits+addr, - rsp, rsp_length); - } - } - break; + case MODBUS_FC_READ_COILS: case MODBUS_FC_READ_DISCRETE_INPUTS: { + const unsigned coil = (function == MODBUS_FC_READ_COILS); + char const * const fn_name = coil ? "read_bits" : "read_input_bits"; /* Similar to coil status (but too many arguments to use a * function) */ int nb = (req[offset + 3] << 8) + req[offset + 4]; @@ -745,8 +717,8 @@ int modbus_virt_reply(modbus_t *ctx, const uint8_t *req, if (nb < 1 || MODBUS_MAX_READ_BITS < nb) { if (ctx->debug) { fprintf(stderr, - "Illegal nb of values %d in read_input_bits (max %d)\n", - nb, MODBUS_MAX_READ_BITS); + "Illegal nb of values %d in %s (max %d)\n", + nb, fn_name, MODBUS_MAX_READ_BITS); } _sleep_response_timeout(ctx); modbus_flush(ctx); @@ -754,12 +726,14 @@ int modbus_virt_reply(modbus_t *ctx, const uint8_t *req, ctx, &sft, MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE, rsp); } else { - if(!vm->tab_input_bits) + uint8_t* (*get_fn)(void*, int, int) + = coil? vm->tab_bits : vm->tab_input_bits; + if(!get_fn) { errno = EINVAL; return -1; } - uint8_t* data = vm->tab_input_bits(vm->app, address, nb); + uint8_t* data = get_fn(vm->app, address, nb); if(data) { rsp_length = ctx->backend->build_response_basis(&sft, rsp); rsp[rsp_length++] = (nb / 8) + ((nb % 8) ? 1 : 0); @@ -768,8 +742,9 @@ int modbus_virt_reply(modbus_t *ctx, const uint8_t *req, rsp, rsp_length); } else { if(ctx->debug) { - fprintf(stderr, "Virtual mapping failed for %d Bits" - " starting at address 0x%X\n", address, nb); + fprintf(stderr, "Virtual mapping failed for %d Bits," + " starting at address 0x%X" + " for function %s\n", address, nb, fn_name); } rsp_length = response_exception( ctx, &sft, diff --git a/src/virtual-reply.c b/src/virtual-reply.c index 4380b63d4..e73113884 100644 --- a/src/virtual-reply.c +++ b/src/virtual-reply.c @@ -17,6 +17,20 @@ static uint8_t* get_discrete_inputs(void* app, int address, int nb) return mb_mapping->tab_input_bits + addr; } +static uint8_t* get_coils(void* app, int address, int nb) +{ + if(!app) + return NULL; + + modbus_mapping_t* mb_mapping = app; + int addr = address - mb_mapping->offset_bits; + if ( (address < mb_mapping->offset_bits) + || ((addr + nb) > mb_mapping->nb_bits)) { + return NULL; + } + return mb_mapping->tab_bits + addr; +} + void modbus_virtualize_mapping(modbus_vmapping_t* dest, modbus_mapping_t* source) @@ -24,4 +38,5 @@ void modbus_virtualize_mapping(modbus_vmapping_t* dest, memset(dest, 0, sizeof(*dest)); dest->app = source; dest->tab_input_bits = get_discrete_inputs; + dest->tab_bits = get_coils; } From 90d28bc964bf7cefb6abadbf21b49c5525baf44e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?R=C3=BCdiger=20Ranft?= Date: Tue, 26 Apr 2016 08:53:17 +0200 Subject: [PATCH 04/14] Unified the address calculation functions. --- src/virtual-reply.c | 31 +++++++++++++++++++------------ 1 file changed, 19 insertions(+), 12 deletions(-) diff --git a/src/virtual-reply.c b/src/virtual-reply.c index e73113884..a5b8cf4fe 100644 --- a/src/virtual-reply.c +++ b/src/virtual-reply.c @@ -3,18 +3,28 @@ #include #include +static uint8_t* get_bit_bucket(uint8_t *base, int address, int nb, int offset, + int count) +{ + if(!base) + return NULL; + + const int addr = address - offset; + if ((address < offset) || ((addr + nb) > count)) + return NULL; + + return base + addr; +} + static uint8_t* get_discrete_inputs(void* app, int address, int nb) { if(!app) return NULL; modbus_mapping_t* mb_mapping = app; - int addr = address - mb_mapping->offset_input_bits; - if ( (address < mb_mapping->offset_input_bits) - || ((addr + nb) > mb_mapping->nb_input_bits)) { - return NULL; - } - return mb_mapping->tab_input_bits + addr; + return get_bit_bucket(mb_mapping->tab_input_bits, address, nb, + mb_mapping->offset_input_bits, + mb_mapping->nb_input_bits); } static uint8_t* get_coils(void* app, int address, int nb) @@ -23,12 +33,9 @@ static uint8_t* get_coils(void* app, int address, int nb) return NULL; modbus_mapping_t* mb_mapping = app; - int addr = address - mb_mapping->offset_bits; - if ( (address < mb_mapping->offset_bits) - || ((addr + nb) > mb_mapping->nb_bits)) { - return NULL; - } - return mb_mapping->tab_bits + addr; + return get_bit_bucket(mb_mapping->tab_bits, address, nb, + mb_mapping->offset_bits, + mb_mapping->nb_bits); } From 1db0c8da0d3929c51679b1c5ea79859c5b7d8139 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?R=C3=BCdiger=20Ranft?= Date: Tue, 26 Apr 2016 11:11:58 +0200 Subject: [PATCH 05/14] Virtualized WRITE_SINGLE_COIL --- src/modbus.c | 19 +++++++++++-------- 1 file changed, 11 insertions(+), 8 deletions(-) diff --git a/src/modbus.c b/src/modbus.c index 595e249a3..ad9f9244a 100644 --- a/src/modbus.c +++ b/src/modbus.c @@ -825,23 +825,26 @@ int modbus_virt_reply(modbus_t *ctx, const uint8_t *req, } } break; - case MODBUS_FC_WRITE_SINGLE_COIL: { - int addr = address - mb_mapping->offset_bits; - - if (address < mb_mapping->offset_bits || addr >= mb_mapping->nb_bits) { + case MODBUS_FC_WRITE_SINGLE_COIL: + if(!vm->tab_bits) { + errno = EINVAL; + return -1; + } + { + uint8_t* dest = vm->tab_bits(vm->app, address, 1); + if(!dest) { if (ctx->debug) { fprintf(stderr, - "Illegal data address 0x%0X in write_bit\n", - address); + "Illegal data address 0x%0X in write_bit\n", address); } rsp_length = response_exception( ctx, &sft, MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp); } else { - int data = (req[offset + 3] << 8) + req[offset + 4]; + const int data = (req[offset + 3] << 8) + req[offset + 4]; if (data == 0xFF00 || data == 0x0) { - mb_mapping->tab_bits[addr] = (data) ? ON : OFF; + *dest = (data) ? ON : OFF; memcpy(rsp, req, req_length); rsp_length = req_length; } else { From b5d0fb5ff06f1d4cbdb7ff940a29236b552a6579 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?R=C3=BCdiger=20Ranft?= Date: Tue, 26 Apr 2016 14:05:31 +0200 Subject: [PATCH 06/14] Switched WRITE_MULTIPLE_COILS to virtual mapping. --- src/modbus.c | 37 ++++++++++++++++++++++--------------- 1 file changed, 22 insertions(+), 15 deletions(-) diff --git a/src/modbus.c b/src/modbus.c index ad9f9244a..a14f935c1 100644 --- a/src/modbus.c +++ b/src/modbus.c @@ -881,8 +881,12 @@ int modbus_virt_reply(modbus_t *ctx, const uint8_t *req, } break; case MODBUS_FC_WRITE_MULTIPLE_COILS: { + if(!vm->tab_bits) { + errno = EINVAL; + return -1; + } + int nb = (req[offset + 3] << 8) + req[offset + 4]; - int addr = address - mb_mapping->offset_bits; if (nb < 1 || MODBUS_MAX_WRITE_BITS < nb) { if (ctx->debug) { @@ -898,22 +902,25 @@ int modbus_virt_reply(modbus_t *ctx, const uint8_t *req, rsp_length = response_exception( ctx, &sft, MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE, rsp); - } else if (address < mb_mapping->offset_bits || (addr + nb) > mb_mapping->nb_bits) { - if (ctx->debug) { - fprintf(stderr, "Illegal data address 0x%0X in write_bits\n", - address < mb_mapping->offset_bits ? address : address + nb); - } - rsp_length = response_exception( - ctx, &sft, - MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp); } else { - /* 6 = byte count */ - modbus_set_bits_from_bytes(mb_mapping->tab_bits, addr, nb, &req[offset + 6]); + uint8_t* dest = vm->tab_bits(vm->app, address, nb); + if(!dest) + { + if (ctx->debug) { + fprintf(stderr, "Illegal data address 0x%0X in write_bits\n", address); + } + rsp_length = response_exception( + ctx, &sft, + MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp); + } else { + /* 6 = byte count */ + modbus_set_bits_from_bytes(dest, 0, nb, &req[offset + 6]); - rsp_length = ctx->backend->build_response_basis(&sft, rsp); - /* 4 to copy the bit address (2) and the quantity of bits */ - memcpy(rsp + rsp_length, req + rsp_length, 4); - rsp_length += 4; + rsp_length = ctx->backend->build_response_basis(&sft, rsp); + /* 4 to copy the bit address (2) and the quantity of bits */ + memcpy(rsp + rsp_length, req + rsp_length, 4); + rsp_length += 4; + } } } break; From fef0b5d86dbb6cfb1cb7ac780184dba6d5bdf939 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?R=C3=BCdiger=20Ranft?= Date: Tue, 26 Apr 2016 15:16:37 +0200 Subject: [PATCH 07/14] Support for 16bit register virtualization started. --- src/modbus.c | 39 +++++++++++++++++++++++---------------- src/virtual-reply.c | 36 ++++++++++++++++++++++++++++++++++++ 2 files changed, 59 insertions(+), 16 deletions(-) diff --git a/src/modbus.c b/src/modbus.c index a14f935c1..c448c8299 100644 --- a/src/modbus.c +++ b/src/modbus.c @@ -753,9 +753,13 @@ int modbus_virt_reply(modbus_t *ctx, const uint8_t *req, } } break; - case MODBUS_FC_READ_HOLDING_REGISTERS: { + case MODBUS_FC_READ_HOLDING_REGISTERS: + if(!vm->tab_registers) { + errno = EINVAL; + return -1; + } + { int nb = (req[offset + 3] << 8) + req[offset + 4]; - int addr = address - mb_mapping->offset_registers; if (nb < 1 || MODBUS_MAX_READ_REGISTERS < nb) { if (ctx->debug) { @@ -768,22 +772,25 @@ int modbus_virt_reply(modbus_t *ctx, const uint8_t *req, rsp_length = response_exception( ctx, &sft, MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE, rsp); - } else if (address < mb_mapping->offset_registers || (addr + nb) > mb_mapping->nb_registers) { - if (ctx->debug) { - fprintf(stderr, "Illegal data address 0x%0X in read_registers\n", - address < mb_mapping->offset_registers ? address : address + nb); - } - rsp_length = response_exception( - ctx, &sft, - MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp); } else { - int i; + uint16_t* source = vm->tab_registers(vm->app, address, nb); + if(!source) { + if (ctx->debug) { + fprintf(stderr, "Illegal data address 0x%0X in read_registers\n", + address < mb_mapping->offset_registers ? address : address + nb); + } + rsp_length = response_exception( + ctx, &sft, + MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp); + } else { + int i; - rsp_length = ctx->backend->build_response_basis(&sft, rsp); - rsp[rsp_length++] = nb << 1; - for (i = addr; i < addr + nb; i++) { - rsp[rsp_length++] = mb_mapping->tab_registers[i] >> 8; - rsp[rsp_length++] = mb_mapping->tab_registers[i] & 0xFF; + rsp_length = ctx->backend->build_response_basis(&sft, rsp); + rsp[rsp_length++] = nb << 1; + for (i = 0; i < nb; i++) { + rsp[rsp_length++] = source[i] >> 8; + rsp[rsp_length++] = source[i] & 0xFF; + } } } } diff --git a/src/virtual-reply.c b/src/virtual-reply.c index a5b8cf4fe..bc5e15191 100644 --- a/src/virtual-reply.c +++ b/src/virtual-reply.c @@ -38,6 +38,40 @@ static uint8_t* get_coils(void* app, int address, int nb) mb_mapping->nb_bits); } +static uint16_t* get_specific_register(uint16_t *base, int address, int nb, + int offset, int count) +{ + if(!base) + return NULL; + + const int addr = address - offset; + if ((address < offset) || ((addr + nb) > count)) + return NULL; + + return base + addr; +} + +static uint16_t* get_register(void* app, int address, int nb) +{ + if(!app) + return NULL; + + modbus_mapping_t* mb_mapping = app; + return get_specific_register(mb_mapping->tab_registers, address, nb, + mb_mapping->offset_registers, + mb_mapping->nb_registers); +} + +static uint16_t* get_input_register(void* app, int address, int nb) +{ + if(!app) + return NULL; + + modbus_mapping_t* mb_mapping = app; + return get_specific_register(mb_mapping->tab_input_registers, address, nb, + mb_mapping->offset_input_registers, + mb_mapping->nb_input_registers); +} void modbus_virtualize_mapping(modbus_vmapping_t* dest, modbus_mapping_t* source) @@ -46,4 +80,6 @@ void modbus_virtualize_mapping(modbus_vmapping_t* dest, dest->app = source; dest->tab_input_bits = get_discrete_inputs; dest->tab_bits = get_coils; + dest->tab_registers = get_register; + dest->tab_input_registers = get_input_register; } From c36aac615f7f199dca5fa2835c716dfd8a6f259e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?R=C3=BCdiger=20Ranft?= Date: Tue, 26 Apr 2016 15:51:12 +0200 Subject: [PATCH 08/14] Moved the usage of `tab_registers` into the virtual mapping. --- src/modbus.c | 142 +++++++++++++++++++++++++++++---------------------- 1 file changed, 80 insertions(+), 62 deletions(-) diff --git a/src/modbus.c b/src/modbus.c index c448c8299..360824e3d 100644 --- a/src/modbus.c +++ b/src/modbus.c @@ -755,9 +755,9 @@ int modbus_virt_reply(modbus_t *ctx, const uint8_t *req, break; case MODBUS_FC_READ_HOLDING_REGISTERS: if(!vm->tab_registers) { - errno = EINVAL; - return -1; - } + errno = EINVAL; + return -1; + } { int nb = (req[offset + 3] << 8) + req[offset + 4]; @@ -867,10 +867,14 @@ int modbus_virt_reply(modbus_t *ctx, const uint8_t *req, } } break; - case MODBUS_FC_WRITE_SINGLE_REGISTER: { - int addr = address - mb_mapping->offset_registers; - - if (address < mb_mapping->offset_registers || addr >= mb_mapping->nb_registers) { + case MODBUS_FC_WRITE_SINGLE_REGISTER: + if(!vm->tab_registers) { + errno = EINVAL; + return -1; + } + { + uint16_t* dest = vm->tab_registers(vm->app, address, 1); + if (!dest) { if (ctx->debug) { fprintf(stderr, "Illegal data address 0x%0X in write_register\n", address); @@ -881,7 +885,7 @@ int modbus_virt_reply(modbus_t *ctx, const uint8_t *req, } else { int data = (req[offset + 3] << 8) + req[offset + 4]; - mb_mapping->tab_registers[addr] = data; + *dest = data; memcpy(rsp, req, req_length); rsp_length = req_length; } @@ -931,9 +935,13 @@ int modbus_virt_reply(modbus_t *ctx, const uint8_t *req, } } break; - case MODBUS_FC_WRITE_MULTIPLE_REGISTERS: { + case MODBUS_FC_WRITE_MULTIPLE_REGISTERS: + if(!vm->tab_registers) { + errno = EINVAL; + return -1; + } + { int nb = (req[offset + 3] << 8) + req[offset + 4]; - int addr = address - mb_mapping->offset_registers; if (nb < 1 || MODBUS_MAX_WRITE_REGISTERS < nb) { if (ctx->debug) { @@ -949,26 +957,28 @@ int modbus_virt_reply(modbus_t *ctx, const uint8_t *req, rsp_length = response_exception( ctx, &sft, MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE, rsp); - } else if (address < mb_mapping->offset_registers || (addr + nb) > mb_mapping->nb_registers) { - if (ctx->debug) { - fprintf(stderr, "Illegal data address 0x%0X in write_registers\n", - address < mb_mapping->offset_registers ? address : address + nb); - } - rsp_length = response_exception( - ctx, &sft, - MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp); } else { - int i, j; - for (i = addr, j = 6; i < addr + nb; i++, j += 2) { - /* 6 and 7 = first value */ - mb_mapping->tab_registers[i] = - (req[offset + j] << 8) + req[offset + j + 1]; - } + uint16_t* dest = vm->tab_registers(vm->app, address, nb); + if(!dest) { + if (ctx->debug) { + fprintf(stderr, "Illegal data address 0x%0X in write_registers\n", + address); + } + rsp_length = response_exception( + ctx, &sft, + MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp); + } else { + int i, j; + for (i = 0, j = 6; i < nb; i++, j += 2) { + /* 6 and 7 = first value */ + dest[i] = (req[offset + j] << 8) + req[offset + j + 1]; + } - rsp_length = ctx->backend->build_response_basis(&sft, rsp); - /* 4 to copy the address (2) and the no. of registers */ - memcpy(rsp + rsp_length, req + rsp_length, 4); - rsp_length += 4; + rsp_length = ctx->backend->build_response_basis(&sft, rsp); + /* 4 to copy the address (2) and the no. of registers */ + memcpy(rsp + rsp_length, req + rsp_length, 4); + rsp_length += 4; + } } } break; @@ -996,10 +1006,14 @@ int modbus_virt_reply(modbus_t *ctx, const uint8_t *req, errno = ENOPROTOOPT; return -1; break; - case MODBUS_FC_MASK_WRITE_REGISTER: { - int addr = address - mb_mapping->offset_registers; - - if (address < mb_mapping->offset_registers || addr >= mb_mapping->nb_registers) { + case MODBUS_FC_MASK_WRITE_REGISTER: + if(!vm->tab_registers) { + errno = EINVAL; + return -1; + } + { + uint16_t* dest = vm->tab_registers(vm->app, address, 1); + if (!dest) { if (ctx->debug) { fprintf(stderr, "Illegal data address 0x%0X in write_register\n", address); @@ -1008,24 +1022,27 @@ int modbus_virt_reply(modbus_t *ctx, const uint8_t *req, ctx, &sft, MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp); } else { - uint16_t data = mb_mapping->tab_registers[addr]; + uint16_t data = *dest; uint16_t and = (req[offset + 3] << 8) + req[offset + 4]; uint16_t or = (req[offset + 5] << 8) + req[offset + 6]; data = (data & and) | (or & (~and)); - mb_mapping->tab_registers[addr] = data; + *dest = data; memcpy(rsp, req, req_length); rsp_length = req_length; } } break; - case MODBUS_FC_WRITE_AND_READ_REGISTERS: { + case MODBUS_FC_WRITE_AND_READ_REGISTERS: + if(!vm->tab_registers) { + errno = EINVAL; + return -1; + } + { 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]; int nb_write_bytes = req[offset + 9]; - int addr = address - mb_mapping->offset_registers; - int addr_write = address_write - mb_mapping->offset_registers; if (nb_write < 1 || MODBUS_MAX_WR_WRITE_REGISTERS < nb_write || nb < 1 || MODBUS_MAX_WR_READ_REGISTERS < nb || @@ -1041,34 +1058,35 @@ int modbus_virt_reply(modbus_t *ctx, const uint8_t *req, rsp_length = response_exception( ctx, &sft, MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE, rsp); - } else if (address < mb_mapping->offset_registers || - (addr + nb) > mb_mapping->nb_registers || - address_write < mb_mapping->offset_registers || - (addr_write + nb_write) > mb_mapping->nb_registers) { - if (ctx->debug) { - fprintf(stderr, - "Illegal data read address 0x%0X or write address 0x%0X write_and_read_registers\n", - address < mb_mapping->offset_registers ? address : address + nb, - address_write < mb_mapping->offset_registers ? address_write : address_write + nb_write); - } - rsp_length = response_exception(ctx, &sft, - MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp); } else { - int i, j; - rsp_length = ctx->backend->build_response_basis(&sft, rsp); - rsp[rsp_length++] = nb << 1; + uint16_t* read = vm->tab_registers(vm->app, address, nb); + uint16_t* write = vm->tab_registers(vm->app, address_write, nb_write); + if(!(read && write)) { + if (ctx->debug) { + fprintf(stderr, + "Illegal data read address 0x%0X or write address 0x%0X" + " write_and_read_registers\n", + address, address_write); + } + rsp_length = response_exception(ctx, &sft, + MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp); + } else { + int i, j; + rsp_length = ctx->backend->build_response_basis(&sft, rsp); + rsp[rsp_length++] = nb << 1; - /* Write first. - 10 and 11 are the offset of the first values to write */ - for (i = addr_write, j = 10; i < addr_write + nb_write; i++, j += 2) { - mb_mapping->tab_registers[i] = - (req[offset + j] << 8) + req[offset + j + 1]; - } + /* Write first. + 10 and 11 are the offset of the first values to write */ + for (i = 0, j = 10; i < nb_write; i++, j += 2) { + write[i] = (req[offset + j] << 8) + req[offset + j + 1]; + } - /* and read the data for the response */ - for (i = addr; i < addr + nb; i++) { - rsp[rsp_length++] = mb_mapping->tab_registers[i] >> 8; - rsp[rsp_length++] = mb_mapping->tab_registers[i] & 0xFF; + /* and read the data for the response */ + for (i = 0; i < nb; i++) { + const uint16_t to_send = read[i]; + rsp[rsp_length++] = to_send >> 8; + rsp[rsp_length++] = to_send & 0xFF; + } } } } From 387b19a26ee77024b8302d6581be19fb4857a46f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?R=C3=BCdiger=20Ranft?= Date: Wed, 27 Apr 2016 09:59:48 +0200 Subject: [PATCH 09/14] Virtualized the last function call. --- src/modbus.c | 39 +++++++++++++++++++++++---------------- 1 file changed, 23 insertions(+), 16 deletions(-) diff --git a/src/modbus.c b/src/modbus.c index 360824e3d..7d74ee93f 100644 --- a/src/modbus.c +++ b/src/modbus.c @@ -795,11 +795,15 @@ int modbus_virt_reply(modbus_t *ctx, const uint8_t *req, } } break; - case MODBUS_FC_READ_INPUT_REGISTERS: { + case MODBUS_FC_READ_INPUT_REGISTERS: + if(!vm->tab_input_registers) { + errno = EINVAL; + return -1; + } + { /* Similar to holding registers (but too many arguments to use a * function) */ int nb = (req[offset + 3] << 8) + req[offset + 4]; - int addr = address - mb_mapping->offset_input_registers; if (nb < 1 || MODBUS_MAX_READ_REGISTERS < nb) { if (ctx->debug) { @@ -812,22 +816,25 @@ int modbus_virt_reply(modbus_t *ctx, const uint8_t *req, rsp_length = response_exception( ctx, &sft, MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE, rsp); - } else if (address < mb_mapping->offset_input_registers || (addr + nb) > mb_mapping->nb_input_registers) { - if (ctx->debug) { - fprintf(stderr, "Illegal data address 0x%0X in read_input_registers\n", - address < mb_mapping->offset_input_registers ? address : address + nb); - } - rsp_length = response_exception( - ctx, &sft, - MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp); } else { - int i; + uint16_t* source = vm->tab_input_registers(vm->app, address, nb); + if(!source) { + if (ctx->debug) { + fprintf(stderr, "Illegal data address 0x%0X in read_input_registers\n", + address < mb_mapping->offset_input_registers ? address : address + nb); + } + rsp_length = response_exception( + ctx, &sft, + MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp); + } else { + int i; - rsp_length = ctx->backend->build_response_basis(&sft, rsp); - rsp[rsp_length++] = nb << 1; - for (i = addr; i < addr + nb; i++) { - rsp[rsp_length++] = mb_mapping->tab_input_registers[i] >> 8; - rsp[rsp_length++] = mb_mapping->tab_input_registers[i] & 0xFF; + rsp_length = ctx->backend->build_response_basis(&sft, rsp); + rsp[rsp_length++] = nb << 1; + for (i = 0; i < nb; i++) { + rsp[rsp_length++] = source[i] >> 8; + rsp[rsp_length++] = source[i] & 0xFF; + } } } } From 41159e2e5f9562f5e9b65589fe7ff90e3f4f38fe Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?R=C3=BCdiger=20Ranft?= Date: Wed, 27 Apr 2016 10:02:06 +0200 Subject: [PATCH 10/14] Dropped the memory-mapping parameter from modbus_virt_reply --- src/modbus.c | 11 +++++------ src/modbus.h | 3 +-- 2 files changed, 6 insertions(+), 8 deletions(-) diff --git a/src/modbus.c b/src/modbus.c index 7d74ee93f..3184173a8 100644 --- a/src/modbus.c +++ b/src/modbus.c @@ -678,8 +678,7 @@ static int response_exception(modbus_t *ctx, sft_t *sft, accordingly. */ int modbus_virt_reply(modbus_t *ctx, const uint8_t *req, - int req_length, modbus_vmapping_t* vm, - modbus_mapping_t *mb_mapping) + int req_length, modbus_vmapping_t* vm) { int offset; int slave; @@ -689,7 +688,7 @@ int modbus_virt_reply(modbus_t *ctx, const uint8_t *req, int rsp_length = 0; sft_t sft; - if ((ctx == NULL)||(vm==NULL)||(mb_mapping==NULL)||(req==NULL)) { + if ((ctx == NULL)||(vm==NULL)||(req==NULL)) { errno = EINVAL; return -1; } @@ -777,7 +776,7 @@ int modbus_virt_reply(modbus_t *ctx, const uint8_t *req, if(!source) { if (ctx->debug) { fprintf(stderr, "Illegal data address 0x%0X in read_registers\n", - address < mb_mapping->offset_registers ? address : address + nb); + address); } rsp_length = response_exception( ctx, &sft, @@ -821,7 +820,7 @@ int modbus_virt_reply(modbus_t *ctx, const uint8_t *req, if(!source) { if (ctx->debug) { fprintf(stderr, "Illegal data address 0x%0X in read_input_registers\n", - address < mb_mapping->offset_input_registers ? address : address + nb); + address); } rsp_length = response_exception( ctx, &sft, @@ -1115,7 +1114,7 @@ int modbus_reply(modbus_t *ctx, const uint8_t *req, { modbus_vmapping_t vm; modbus_virtualize_mapping(&vm, mb_mapping); - return modbus_virt_reply(ctx, req, req_length, &vm, mb_mapping); + return modbus_virt_reply(ctx, req, req_length, &vm); } int modbus_reply_exception(modbus_t *ctx, const uint8_t *req, diff --git a/src/modbus.h b/src/modbus.h index e1da163ad..13d022f68 100644 --- a/src/modbus.h +++ b/src/modbus.h @@ -230,8 +230,7 @@ MODBUS_API int modbus_receive_confirmation(modbus_t *ctx, uint8_t *rsp); #include "virtual-reply.h" int modbus_virt_reply(modbus_t *ctx, const uint8_t *req, - int req_length, modbus_vmapping_t *vm, - modbus_mapping_t *mb_mapping); + int req_length, modbus_vmapping_t *vm); MODBUS_API int modbus_reply(modbus_t *ctx, const uint8_t *req, int req_length, modbus_mapping_t *mb_mapping); MODBUS_API int modbus_reply_exception(modbus_t *ctx, const uint8_t *req, From f832b327200b33899478c05717f5259179d806f3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?R=C3=BCdiger=20Ranft?= Date: Wed, 27 Apr 2016 10:48:15 +0200 Subject: [PATCH 11/14] Unified READ_HOLDING_REGISTERS and READ_INPUT_REGISTERS. --- src/modbus.c | 63 ++++++++++------------------------------------------ 1 file changed, 12 insertions(+), 51 deletions(-) diff --git a/src/modbus.c b/src/modbus.c index 3184173a8..9aa58f318 100644 --- a/src/modbus.c +++ b/src/modbus.c @@ -709,8 +709,6 @@ int modbus_virt_reply(modbus_t *ctx, const uint8_t *req, case MODBUS_FC_READ_DISCRETE_INPUTS: { const unsigned coil = (function == MODBUS_FC_READ_COILS); char const * const fn_name = coil ? "read_bits" : "read_input_bits"; - /* Similar to coil status (but too many arguments to use a - * function) */ int nb = (req[offset + 3] << 8) + req[offset + 4]; if (nb < 1 || MODBUS_MAX_READ_BITS < nb) { @@ -753,62 +751,25 @@ int modbus_virt_reply(modbus_t *ctx, const uint8_t *req, } break; case MODBUS_FC_READ_HOLDING_REGISTERS: - if(!vm->tab_registers) { - errno = EINVAL; - return -1; - } - { - int nb = (req[offset + 3] << 8) + req[offset + 4]; + case MODBUS_FC_READ_INPUT_REGISTERS: { + const unsigned is_input = MODBUS_FC_READ_INPUT_REGISTERS == function; + char const*const name + = is_input ? "read_input_registers": "read_holding_registers"; + uint16_t* (*get_fn)(void*, int, int) + = is_input ? vm->tab_input_registers:vm->tab_registers; - if (nb < 1 || MODBUS_MAX_READ_REGISTERS < nb) { - if (ctx->debug) { - fprintf(stderr, - "Illegal nb of values %d in read_holding_registers (max %d)\n", - nb, MODBUS_MAX_READ_REGISTERS); - } - _sleep_response_timeout(ctx); - modbus_flush(ctx); - rsp_length = response_exception( - ctx, &sft, - MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE, rsp); - } else { - uint16_t* source = vm->tab_registers(vm->app, address, nb); - if(!source) { - if (ctx->debug) { - fprintf(stderr, "Illegal data address 0x%0X in read_registers\n", - address); - } - rsp_length = response_exception( - ctx, &sft, - MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp); - } else { - int i; + const int nb = (req[offset + 3] << 8) + req[offset + 4]; - rsp_length = ctx->backend->build_response_basis(&sft, rsp); - rsp[rsp_length++] = nb << 1; - for (i = 0; i < nb; i++) { - rsp[rsp_length++] = source[i] >> 8; - rsp[rsp_length++] = source[i] & 0xFF; - } - } - } - } - break; - case MODBUS_FC_READ_INPUT_REGISTERS: - if(!vm->tab_input_registers) { + if(!get_fn) { errno = EINVAL; return -1; } - { - /* Similar to holding registers (but too many arguments to use a - * function) */ - int nb = (req[offset + 3] << 8) + req[offset + 4]; if (nb < 1 || MODBUS_MAX_READ_REGISTERS < nb) { if (ctx->debug) { fprintf(stderr, - "Illegal number of values %d in read_input_registers (max %d)\n", - nb, MODBUS_MAX_READ_REGISTERS); + "Illegal nb of values %d in %s (max %d)\n", + nb, name, MODBUS_MAX_READ_REGISTERS); } _sleep_response_timeout(ctx); modbus_flush(ctx); @@ -816,10 +777,10 @@ int modbus_virt_reply(modbus_t *ctx, const uint8_t *req, ctx, &sft, MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE, rsp); } else { - uint16_t* source = vm->tab_input_registers(vm->app, address, nb); + uint16_t* source = get_fn(vm->app, address, nb); if(!source) { if (ctx->debug) { - fprintf(stderr, "Illegal data address 0x%0X in read_input_registers\n", + fprintf(stderr, "Illegal data address 0x%0X in read_registers\n", address); } rsp_length = response_exception( From baafd9ba1cab10097412da88686194bee46a792f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?R=C3=BCdiger=20Ranft?= Date: Wed, 27 Apr 2016 11:02:09 +0200 Subject: [PATCH 12/14] Refactored the errorhandling. --- src/modbus.c | 52 ++++++++++++++++++++++------------------------------ 1 file changed, 22 insertions(+), 30 deletions(-) diff --git a/src/modbus.c b/src/modbus.c index 9aa58f318..3e857c4a6 100644 --- a/src/modbus.c +++ b/src/modbus.c @@ -671,6 +671,15 @@ static int response_exception(modbus_t *ctx, sft_t *sft, return rsp_length; } +static unsigned assure_or_set_errno(unsigned condition, int code) +{ + if(!condition){ + errno = code; + return 0; + } + return 1; +} + /* Send a response to the received request. Analyses the request and constructs a response. @@ -688,10 +697,8 @@ int modbus_virt_reply(modbus_t *ctx, const uint8_t *req, int rsp_length = 0; sft_t sft; - if ((ctx == NULL)||(vm==NULL)||(req==NULL)) { - errno = EINVAL; + if (!assure_or_set_errno(ctx && vm && req, EINVAL)) return -1; - } offset = ctx->backend->header_length; slave = req[offset - 1]; @@ -725,11 +732,10 @@ int modbus_virt_reply(modbus_t *ctx, const uint8_t *req, } else { uint8_t* (*get_fn)(void*, int, int) = coil? vm->tab_bits : vm->tab_input_bits; - if(!get_fn) - { - errno = EINVAL; + + if(!assure_or_set_errno(get_fn, EINVAL)) return -1; - } + uint8_t* data = get_fn(vm->app, address, nb); if(data) { rsp_length = ctx->backend->build_response_basis(&sft, rsp); @@ -760,10 +766,8 @@ int modbus_virt_reply(modbus_t *ctx, const uint8_t *req, const int nb = (req[offset + 3] << 8) + req[offset + 4]; - if(!get_fn) { - errno = EINVAL; + if(!assure_or_set_errno(get_fn, EINVAL)) return -1; - } if (nb < 1 || MODBUS_MAX_READ_REGISTERS < nb) { if (ctx->debug) { @@ -800,10 +804,8 @@ int modbus_virt_reply(modbus_t *ctx, const uint8_t *req, } break; case MODBUS_FC_WRITE_SINGLE_COIL: - if(!vm->tab_bits) { - errno = EINVAL; + if(!assure_or_set_errno(vm->tab_bits, EINVAL)) return -1; - } { uint8_t* dest = vm->tab_bits(vm->app, address, 1); if(!dest) { @@ -835,10 +837,8 @@ int modbus_virt_reply(modbus_t *ctx, const uint8_t *req, } break; case MODBUS_FC_WRITE_SINGLE_REGISTER: - if(!vm->tab_registers) { - errno = EINVAL; + if(!assure_or_set_errno(vm->tab_registers, EINVAL)) return -1; - } { uint16_t* dest = vm->tab_registers(vm->app, address, 1); if (!dest) { @@ -859,13 +859,11 @@ int modbus_virt_reply(modbus_t *ctx, const uint8_t *req, } break; case MODBUS_FC_WRITE_MULTIPLE_COILS: { - if(!vm->tab_bits) { - errno = EINVAL; - return -1; - } - int nb = (req[offset + 3] << 8) + req[offset + 4]; + if(!assure_or_set_errno(vm->tab_bits, EINVAL)) + return -1; + if (nb < 1 || MODBUS_MAX_WRITE_BITS < nb) { if (ctx->debug) { fprintf(stderr, @@ -903,10 +901,8 @@ int modbus_virt_reply(modbus_t *ctx, const uint8_t *req, } break; case MODBUS_FC_WRITE_MULTIPLE_REGISTERS: - if(!vm->tab_registers) { - errno = EINVAL; + if(!assure_or_set_errno(vm->tab_registers, EINVAL)) return -1; - } { int nb = (req[offset + 3] << 8) + req[offset + 4]; @@ -974,10 +970,8 @@ int modbus_virt_reply(modbus_t *ctx, const uint8_t *req, return -1; break; case MODBUS_FC_MASK_WRITE_REGISTER: - if(!vm->tab_registers) { - errno = EINVAL; + if(!assure_or_set_errno(vm->tab_registers, EINVAL)) return -1; - } { uint16_t* dest = vm->tab_registers(vm->app, address, 1); if (!dest) { @@ -1001,10 +995,8 @@ int modbus_virt_reply(modbus_t *ctx, const uint8_t *req, } break; case MODBUS_FC_WRITE_AND_READ_REGISTERS: - if(!vm->tab_registers) { - errno = EINVAL; + if(!assure_or_set_errno(vm->tab_registers, EINVAL)) return -1; - } { int nb = (req[offset + 3] << 8) + req[offset + 4]; uint16_t address_write = (req[offset + 5] << 8) + req[offset + 6]; From 617132d79fb37cae9017d99b6f679ebe07ba4cca Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?R=C3=BCdiger=20Ranft?= Date: Wed, 27 Apr 2016 11:43:44 +0200 Subject: [PATCH 13/14] Refactored the error handling code. The parts of the error handling, where the same pattern was written along all the code, got unified into own functions in order to keep the code clean. --- src/modbus.c | 188 ++++++++++++++++++++++----------------------------- 1 file changed, 79 insertions(+), 109 deletions(-) diff --git a/src/modbus.c b/src/modbus.c index 3e857c4a6..65d2a05ce 100644 --- a/src/modbus.c +++ b/src/modbus.c @@ -16,6 +16,7 @@ #ifndef _MSC_VER #include #endif +#include #include @@ -657,9 +658,32 @@ static int response_io_status(int nb, } /* Build the exception response */ +#ifdef __GNUC__ static int response_exception(modbus_t *ctx, sft_t *sft, - int exception_code, uint8_t *rsp) + int exception_code, uint8_t *rsp, + unsigned perform_flush, const char* format, ...) + __attribute__ ((format (printf, 6, 7))); +#endif + +static int response_exception(modbus_t *ctx, sft_t *sft, + int exception_code, uint8_t *rsp, + unsigned perform_flush, const char* format, ...) { + if (ctx->debug) { + va_list ap; + va_start(ap, format); + vfprintf(stderr, format, ap); + va_end(ap); + } + + if(perform_flush) { + /* May be the indication has been truncated on reading because of + * invalid address (eg. nb is 0 but the request contains values to + * write) so it's necessary to flush. */ + _sleep_response_timeout(ctx); + modbus_flush(ctx); + } + int rsp_length; sft->function = sft->function + 0x80; @@ -719,21 +743,15 @@ int modbus_virt_reply(modbus_t *ctx, const uint8_t *req, int nb = (req[offset + 3] << 8) + req[offset + 4]; if (nb < 1 || MODBUS_MAX_READ_BITS < nb) { - if (ctx->debug) { - fprintf(stderr, - "Illegal nb of values %d in %s (max %d)\n", - nb, fn_name, MODBUS_MAX_READ_BITS); - } - _sleep_response_timeout(ctx); - modbus_flush(ctx); rsp_length = response_exception( ctx, &sft, - MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE, rsp); + MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE, rsp, 1, + "Illegal nb of values %d in %s (max %d)\n", nb, fn_name, MODBUS_MAX_READ_BITS); } else { uint8_t* (*get_fn)(void*, int, int) = coil? vm->tab_bits : vm->tab_input_bits; - if(!assure_or_set_errno(get_fn, EINVAL)) + if(!assure_or_set_errno(!!get_fn, EINVAL)) return -1; uint8_t* data = get_fn(vm->app, address, nb); @@ -744,14 +762,11 @@ int modbus_virt_reply(modbus_t *ctx, const uint8_t *req, data, rsp, rsp_length); } else { - if(ctx->debug) { - fprintf(stderr, "Virtual mapping failed for %d Bits," - " starting at address 0x%X" - " for function %s\n", address, nb, fn_name); - } - rsp_length = response_exception( - ctx, &sft, - MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp); + rsp_length = response_exception(ctx, &sft, + MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp, 0, + "Virtual mapping failed for %d Bits," + " starting at address 0x%X" + " for function %s\n", address, nb, fn_name); } } } @@ -766,30 +781,23 @@ int modbus_virt_reply(modbus_t *ctx, const uint8_t *req, const int nb = (req[offset + 3] << 8) + req[offset + 4]; - if(!assure_or_set_errno(get_fn, EINVAL)) + if(!assure_or_set_errno(!!get_fn, EINVAL)) return -1; if (nb < 1 || MODBUS_MAX_READ_REGISTERS < nb) { - if (ctx->debug) { - fprintf(stderr, - "Illegal nb of values %d in %s (max %d)\n", - nb, name, MODBUS_MAX_READ_REGISTERS); - } - _sleep_response_timeout(ctx); - modbus_flush(ctx); rsp_length = response_exception( ctx, &sft, - MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE, rsp); + MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE, rsp, 1, + "Illegal nb of values %d in %s (max %d)\n", + nb, name, MODBUS_MAX_READ_REGISTERS); } else { uint16_t* source = get_fn(vm->app, address, nb); if(!source) { - if (ctx->debug) { - fprintf(stderr, "Illegal data address 0x%0X in read_registers\n", - address); - } rsp_length = response_exception( ctx, &sft, - MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp); + MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp, 0, + "Illegal data address 0x%0X in read_registers\n", + address); } else { int i; @@ -804,18 +812,15 @@ int modbus_virt_reply(modbus_t *ctx, const uint8_t *req, } break; case MODBUS_FC_WRITE_SINGLE_COIL: - if(!assure_or_set_errno(vm->tab_bits, EINVAL)) + if(!assure_or_set_errno(!!vm->tab_bits, EINVAL)) return -1; { uint8_t* dest = vm->tab_bits(vm->app, address, 1); if(!dest) { - if (ctx->debug) { - fprintf(stderr, - "Illegal data address 0x%0X in write_bit\n", address); - } rsp_length = response_exception( ctx, &sft, - MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp); + MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp, 0, + "Illegal data address 0x%0X in write_bit\n", address); } else { const int data = (req[offset + 3] << 8) + req[offset + 4]; @@ -824,31 +829,26 @@ int modbus_virt_reply(modbus_t *ctx, const uint8_t *req, memcpy(rsp, req, req_length); rsp_length = req_length; } else { - if (ctx->debug) { - fprintf(stderr, - "Illegal data value 0x%0X in write_bit request at address %0X\n", - data, address); - } rsp_length = response_exception( ctx, &sft, - MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE, rsp); + MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE, rsp, 0, + "Illegal data value 0x%0X in write_bit request at address %0X\n", + data, address); } } } break; case MODBUS_FC_WRITE_SINGLE_REGISTER: - if(!assure_or_set_errno(vm->tab_registers, EINVAL)) + if(!assure_or_set_errno(!!vm->tab_registers, EINVAL)) return -1; { uint16_t* dest = vm->tab_registers(vm->app, address, 1); if (!dest) { - if (ctx->debug) { - fprintf(stderr, "Illegal data address 0x%0X in write_register\n", - address); - } rsp_length = response_exception( ctx, &sft, - MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp); + MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp, 0, + "Illegal data address 0x%0X in write_register\n", + address); } else { int data = (req[offset + 3] << 8) + req[offset + 4]; @@ -861,33 +861,22 @@ int modbus_virt_reply(modbus_t *ctx, const uint8_t *req, case MODBUS_FC_WRITE_MULTIPLE_COILS: { int nb = (req[offset + 3] << 8) + req[offset + 4]; - if(!assure_or_set_errno(vm->tab_bits, EINVAL)) + if(!assure_or_set_errno(!!vm->tab_bits, EINVAL)) return -1; if (nb < 1 || MODBUS_MAX_WRITE_BITS < nb) { - if (ctx->debug) { - fprintf(stderr, - "Illegal number of values %d in write_bits (max %d)\n", - nb, MODBUS_MAX_WRITE_BITS); - } - /* May be the indication has been truncated on reading because of - * invalid address (eg. nb is 0 but the request contains values to - * write) so it's necessary to flush. */ - _sleep_response_timeout(ctx); - modbus_flush(ctx); rsp_length = response_exception( ctx, &sft, - MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE, rsp); + MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE, rsp, 1, + "Illegal number of values %d in write_bits (max %d)\n", + nb, MODBUS_MAX_WRITE_BITS); } else { uint8_t* dest = vm->tab_bits(vm->app, address, nb); - if(!dest) - { - if (ctx->debug) { - fprintf(stderr, "Illegal data address 0x%0X in write_bits\n", address); - } + if(!dest) { rsp_length = response_exception( ctx, &sft, - MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp); + MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp, 0, + "Illegal data address 0x%0X in write_bits\n", address); } else { /* 6 = byte count */ modbus_set_bits_from_bytes(dest, 0, nb, &req[offset + 6]); @@ -901,35 +890,25 @@ int modbus_virt_reply(modbus_t *ctx, const uint8_t *req, } break; case MODBUS_FC_WRITE_MULTIPLE_REGISTERS: - if(!assure_or_set_errno(vm->tab_registers, EINVAL)) + if(!assure_or_set_errno(!!vm->tab_registers, EINVAL)) return -1; { int nb = (req[offset + 3] << 8) + req[offset + 4]; if (nb < 1 || MODBUS_MAX_WRITE_REGISTERS < nb) { - if (ctx->debug) { - fprintf(stderr, - "Illegal number of values %d in write_registers (max %d)\n", - nb, MODBUS_MAX_WRITE_REGISTERS); - } - /* May be the indication has been truncated on reading because of - * invalid address (eg. nb is 0 but the request contains values to - * write) so it's necessary to flush. */ - _sleep_response_timeout(ctx); - modbus_flush(ctx); rsp_length = response_exception( ctx, &sft, - MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE, rsp); + MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE, rsp, 1, + "Illegal number of values %d in write_registers (max %d)\n", + nb, MODBUS_MAX_WRITE_REGISTERS); } else { uint16_t* dest = vm->tab_registers(vm->app, address, nb); if(!dest) { - if (ctx->debug) { - fprintf(stderr, "Illegal data address 0x%0X in write_registers\n", - address); - } rsp_length = response_exception( ctx, &sft, - MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp); + MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp, 0, + "Illegal data address 0x%0X in write_registers\n", + address); } else { int i, j; for (i = 0, j = 6; i < nb; i++, j += 2) { @@ -970,18 +949,16 @@ int modbus_virt_reply(modbus_t *ctx, const uint8_t *req, return -1; break; case MODBUS_FC_MASK_WRITE_REGISTER: - if(!assure_or_set_errno(vm->tab_registers, EINVAL)) + if(!assure_or_set_errno(!!vm->tab_registers, EINVAL)) return -1; { uint16_t* dest = vm->tab_registers(vm->app, address, 1); if (!dest) { - if (ctx->debug) { - fprintf(stderr, "Illegal data address 0x%0X in write_register\n", - address); - } rsp_length = response_exception( ctx, &sft, - MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp); + MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp, 0, + "Illegal data address 0x%0X in write_register\n", + address); } else { uint16_t data = *dest; uint16_t and = (req[offset + 3] << 8) + req[offset + 4]; @@ -995,7 +972,7 @@ int modbus_virt_reply(modbus_t *ctx, const uint8_t *req, } break; case MODBUS_FC_WRITE_AND_READ_REGISTERS: - if(!assure_or_set_errno(vm->tab_registers, EINVAL)) + if(!assure_or_set_errno(!!vm->tab_registers, EINVAL)) return -1; { int nb = (req[offset + 3] << 8) + req[offset + 4]; @@ -1006,29 +983,21 @@ int modbus_virt_reply(modbus_t *ctx, const uint8_t *req, if (nb_write < 1 || MODBUS_MAX_WR_WRITE_REGISTERS < nb_write || nb < 1 || MODBUS_MAX_WR_READ_REGISTERS < nb || nb_write_bytes != nb_write * 2) { - if (ctx->debug) { - fprintf(stderr, - "Illegal nb of values (W%d, R%d) in write_and_read_registers (max W%d, R%d)\n", - nb_write, nb, - MODBUS_MAX_WR_WRITE_REGISTERS, MODBUS_MAX_WR_READ_REGISTERS); - } - _sleep_response_timeout(ctx); - modbus_flush(ctx); rsp_length = response_exception( ctx, &sft, - MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE, rsp); + MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE, rsp, 1, + "Illegal nb of values (W%d, R%d) in write_and_read_registers (max W%d, R%d)\n", + nb_write, nb, + MODBUS_MAX_WR_WRITE_REGISTERS, MODBUS_MAX_WR_READ_REGISTERS); } else { uint16_t* read = vm->tab_registers(vm->app, address, nb); uint16_t* write = vm->tab_registers(vm->app, address_write, nb_write); if(!(read && write)) { - if (ctx->debug) { - fprintf(stderr, - "Illegal data read address 0x%0X or write address 0x%0X" - " write_and_read_registers\n", - address, address_write); - } rsp_length = response_exception(ctx, &sft, - MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp); + MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp, 0, + "Illegal data read address 0x%0X or write address 0x%0X" + " write_and_read_registers\n", + address, address_write); } else { int i, j; rsp_length = ctx->backend->build_response_basis(&sft, rsp); @@ -1054,7 +1023,8 @@ int modbus_virt_reply(modbus_t *ctx, const uint8_t *req, default: rsp_length = response_exception(ctx, &sft, MODBUS_EXCEPTION_ILLEGAL_FUNCTION, - rsp); + rsp, 0, "Illegal function code: %d\n", + function); break; } From 50d2b924cb8a7ecba0561b0e77c44c9417b09ca4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?R=C3=BCdiger=20Ranft?= Date: Mon, 2 May 2016 11:37:13 +0200 Subject: [PATCH 14/14] Added a notification after the transaction. The application now does get a notification after the read or write is done. Also the reason for the buffer retreival is now visible to the application. --- src/modbus.c | 48 ++++++++++++++++++++++++++++++++++----------- src/virtual-reply.c | 16 +++++++++++---- src/virtual-reply.h | 29 +++++++++++++++++++++++---- 3 files changed, 74 insertions(+), 19 deletions(-) diff --git a/src/modbus.c b/src/modbus.c index 65d2a05ce..49c5eb180 100644 --- a/src/modbus.c +++ b/src/modbus.c @@ -748,19 +748,23 @@ int modbus_virt_reply(modbus_t *ctx, const uint8_t *req, MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE, rsp, 1, "Illegal nb of values %d in %s (max %d)\n", nb, fn_name, MODBUS_MAX_READ_BITS); } else { - uint8_t* (*get_fn)(void*, int, int) + uint8_t* (*get_fn)(void*, int, int, modbus_vmap_reason) = coil? vm->tab_bits : vm->tab_input_bits; + void (*done_fn)(void*, uint8_t*, int, int, modbus_vmap_reason) + = coil? vm->tab_bits_done : vm->tab_input_bits_done; if(!assure_or_set_errno(!!get_fn, EINVAL)) return -1; - uint8_t* data = get_fn(vm->app, address, nb); + uint8_t* data = get_fn(vm->app, address, nb, MODBUS_VMAP_READ); if(data) { rsp_length = ctx->backend->build_response_basis(&sft, rsp); rsp[rsp_length++] = (nb / 8) + ((nb % 8) ? 1 : 0); rsp_length = response_io_status(nb, data, rsp, rsp_length); + if(done_fn) + done_fn(vm->app, data, address, nb, MODBUS_VMAP_READ); } else { rsp_length = response_exception(ctx, &sft, MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp, 0, @@ -776,8 +780,10 @@ int modbus_virt_reply(modbus_t *ctx, const uint8_t *req, const unsigned is_input = MODBUS_FC_READ_INPUT_REGISTERS == function; char const*const name = is_input ? "read_input_registers": "read_holding_registers"; - uint16_t* (*get_fn)(void*, int, int) + uint16_t* (*get_fn)(void*, int, int, modbus_vmap_reason) = is_input ? vm->tab_input_registers:vm->tab_registers; + void (*done_fn)(void*, uint16_t*, int, int, modbus_vmap_reason) + = is_input ? vm->tab_input_registers_done: vm->tab_registers_done; const int nb = (req[offset + 3] << 8) + req[offset + 4]; @@ -791,7 +797,7 @@ int modbus_virt_reply(modbus_t *ctx, const uint8_t *req, "Illegal nb of values %d in %s (max %d)\n", nb, name, MODBUS_MAX_READ_REGISTERS); } else { - uint16_t* source = get_fn(vm->app, address, nb); + uint16_t* source = get_fn(vm->app, address, nb, MODBUS_VMAP_READ); if(!source) { rsp_length = response_exception( ctx, &sft, @@ -807,6 +813,8 @@ int modbus_virt_reply(modbus_t *ctx, const uint8_t *req, rsp[rsp_length++] = source[i] >> 8; rsp[rsp_length++] = source[i] & 0xFF; } + if(done_fn) + done_fn(vm->app, source, address, nb, MODBUS_VMAP_READ); } } } @@ -815,7 +823,7 @@ int modbus_virt_reply(modbus_t *ctx, const uint8_t *req, if(!assure_or_set_errno(!!vm->tab_bits, EINVAL)) return -1; { - uint8_t* dest = vm->tab_bits(vm->app, address, 1); + uint8_t* dest = vm->tab_bits(vm->app, address, 1, MODBUS_VMAP_WRITE); if(!dest) { rsp_length = response_exception( ctx, &sft, @@ -835,6 +843,8 @@ int modbus_virt_reply(modbus_t *ctx, const uint8_t *req, "Illegal data value 0x%0X in write_bit request at address %0X\n", data, address); } + if(vm->tab_bits_done) + vm->tab_bits_done(vm->app, dest, address, 1, MODBUS_VMAP_WRITE); } } break; @@ -842,7 +852,7 @@ int modbus_virt_reply(modbus_t *ctx, const uint8_t *req, if(!assure_or_set_errno(!!vm->tab_registers, EINVAL)) return -1; { - uint16_t* dest = vm->tab_registers(vm->app, address, 1); + uint16_t* dest = vm->tab_registers(vm->app, address, 1, MODBUS_VMAP_WRITE); if (!dest) { rsp_length = response_exception( ctx, &sft, @@ -855,6 +865,8 @@ int modbus_virt_reply(modbus_t *ctx, const uint8_t *req, *dest = data; memcpy(rsp, req, req_length); rsp_length = req_length; + if(vm->tab_registers_done) + vm->tab_registers_done(vm->app, dest, address, 1, MODBUS_VMAP_WRITE); } } break; @@ -871,7 +883,7 @@ int modbus_virt_reply(modbus_t *ctx, const uint8_t *req, "Illegal number of values %d in write_bits (max %d)\n", nb, MODBUS_MAX_WRITE_BITS); } else { - uint8_t* dest = vm->tab_bits(vm->app, address, nb); + uint8_t* dest = vm->tab_bits(vm->app, address, nb, MODBUS_VMAP_WRITE); if(!dest) { rsp_length = response_exception( ctx, &sft, @@ -885,6 +897,8 @@ int modbus_virt_reply(modbus_t *ctx, const uint8_t *req, /* 4 to copy the bit address (2) and the quantity of bits */ memcpy(rsp + rsp_length, req + rsp_length, 4); rsp_length += 4; + if(vm->tab_bits_done) + vm->tab_bits_done(vm->app, dest, address, nb, MODBUS_VMAP_WRITE); } } } @@ -902,7 +916,7 @@ int modbus_virt_reply(modbus_t *ctx, const uint8_t *req, "Illegal number of values %d in write_registers (max %d)\n", nb, MODBUS_MAX_WRITE_REGISTERS); } else { - uint16_t* dest = vm->tab_registers(vm->app, address, nb); + uint16_t* dest = vm->tab_registers(vm->app, address, nb, MODBUS_VMAP_WRITE); if(!dest) { rsp_length = response_exception( ctx, &sft, @@ -920,6 +934,8 @@ int modbus_virt_reply(modbus_t *ctx, const uint8_t *req, /* 4 to copy the address (2) and the no. of registers */ memcpy(rsp + rsp_length, req + rsp_length, 4); rsp_length += 4; + if(vm->tab_registers_done) + vm->tab_registers_done(vm->app, dest, address, nb, MODBUS_VMAP_WRITE); } } } @@ -952,7 +968,7 @@ int modbus_virt_reply(modbus_t *ctx, const uint8_t *req, if(!assure_or_set_errno(!!vm->tab_registers, EINVAL)) return -1; { - uint16_t* dest = vm->tab_registers(vm->app, address, 1); + uint16_t* dest = vm->tab_registers(vm->app, address, 1, MODBUS_VMAP_WRITE); if (!dest) { rsp_length = response_exception( ctx, &sft, @@ -968,6 +984,8 @@ int modbus_virt_reply(modbus_t *ctx, const uint8_t *req, *dest = data; memcpy(rsp, req, req_length); rsp_length = req_length; + if(vm->tab_registers_done) + vm->tab_registers_done(vm->app, dest, address, 1, MODBUS_VMAP_WRITE); } } break; @@ -990,8 +1008,10 @@ int modbus_virt_reply(modbus_t *ctx, const uint8_t *req, nb_write, nb, MODBUS_MAX_WR_WRITE_REGISTERS, MODBUS_MAX_WR_READ_REGISTERS); } else { - uint16_t* read = vm->tab_registers(vm->app, address, nb); - uint16_t* write = vm->tab_registers(vm->app, address_write, nb_write); + uint16_t* read = vm->tab_registers(vm->app, address, nb, + MODBUS_VMAP_READ); + uint16_t* write = vm->tab_registers(vm->app, address_write, + nb_write, MODBUS_VMAP_WRITE); if(!(read && write)) { rsp_length = response_exception(ctx, &sft, MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp, 0, @@ -1015,6 +1035,12 @@ int modbus_virt_reply(modbus_t *ctx, const uint8_t *req, rsp[rsp_length++] = to_send >> 8; rsp[rsp_length++] = to_send & 0xFF; } + if(vm->tab_registers_done) { + vm->tab_registers_done(vm->app, read, address, nb, + MODBUS_VMAP_READ); + vm->tab_registers_done(vm->app, write, address, nb, + MODBUS_VMAP_WRITE); + } } } } diff --git a/src/virtual-reply.c b/src/virtual-reply.c index bc5e15191..94eb6260d 100644 --- a/src/virtual-reply.c +++ b/src/virtual-reply.c @@ -16,8 +16,10 @@ static uint8_t* get_bit_bucket(uint8_t *base, int address, int nb, int offset, return base + addr; } -static uint8_t* get_discrete_inputs(void* app, int address, int nb) +static uint8_t* get_discrete_inputs(void* app, int address, int nb, + modbus_vmap_reason reason) { + (void) reason; if(!app) return NULL; @@ -27,8 +29,10 @@ static uint8_t* get_discrete_inputs(void* app, int address, int nb) mb_mapping->nb_input_bits); } -static uint8_t* get_coils(void* app, int address, int nb) +static uint8_t* get_coils(void* app, int address, int nb, + modbus_vmap_reason reason) { + (void) reason; if(!app) return NULL; @@ -51,8 +55,10 @@ static uint16_t* get_specific_register(uint16_t *base, int address, int nb, return base + addr; } -static uint16_t* get_register(void* app, int address, int nb) +static uint16_t* get_register(void* app, int address, int nb, + modbus_vmap_reason reason) { + (void) reason; if(!app) return NULL; @@ -62,8 +68,10 @@ static uint16_t* get_register(void* app, int address, int nb) mb_mapping->nb_registers); } -static uint16_t* get_input_register(void* app, int address, int nb) +static uint16_t* get_input_register(void* app, int address, int nb, + modbus_vmap_reason reason) { + (void) reason; if(!app) return NULL; diff --git a/src/virtual-reply.h b/src/virtual-reply.h index a0e188ff9..098911cfa 100644 --- a/src/virtual-reply.h +++ b/src/virtual-reply.h @@ -12,12 +12,33 @@ #include "stdint.h" #endif +typedef enum { + MODBUS_VMAP_READ, + MODBUS_VMAP_WRITE +} modbus_vmap_reason; + typedef struct { void* app; - uint8_t* (*tab_bits)(void* app, int addr, int nb); - uint8_t* (*tab_input_bits)(void* app, int addr, int nb); - uint16_t* (*tab_input_registers)(void* app, int addr, int nb); - uint16_t* (*tab_registers)(void* app, int addr, int nb); + uint8_t* (*tab_bits)(void* app, int addr, int nb, + modbus_vmap_reason what); + void (*tab_bits_done)(void* app, uint8_t* store, int addr, int nb, + modbus_vmap_reason what); + + uint8_t* (*tab_input_bits)(void* app, int addr, int nb, + modbus_vmap_reason what); + void (*tab_input_bits_done)(void* app, uint8_t* store, int addr, int nb, + modbus_vmap_reason what); + + uint16_t* (*tab_input_registers)(void* app, int addr, int nb, + modbus_vmap_reason what); + void (*tab_input_registers_done)(void* app, uint16_t*store, int addr, + int nb, modbus_vmap_reason what); + + + uint16_t* (*tab_registers)(void* app, int addr, int nb, + modbus_vmap_reason what); + void (*tab_registers_done)(void* app, uint16_t* store, int addr, int nb, + modbus_vmap_reason what); } modbus_vmapping_t; void modbus_virtualize_mapping(modbus_vmapping_t* dest,