From d1133793497b666563a17eb6042ec2d5c6ffff0a Mon Sep 17 00:00:00 2001 From: Patrick Boettcher Date: Mon, 7 Aug 2017 16:09:06 +0200 Subject: [PATCH] Add modbus_reply_callback() and modbus_set_reply_callback() Based on @frodete's proposal (#319) this implementation adds a a callback-based reply-functionality. The old mapping-implementation is now based on this version. As a nice side-effect some DRY code optimizations could have been done. Also adds a MODBUS_SLAVE_ACCEPT_ALL "address", which can be used with modbus_set_slave(). This makes the modbus_receive()-function accept all requests, independently of the slave-address found in the decoded-request. Useful for multi-slave-implementation using libmodbus. --- doc/Makefile.am | 3 +- doc/modbus_reply.txt | 1 + doc/modbus_reply_callback.txt | 175 ++++++++++ doc/modbus_set_slave.txt | 4 + src/modbus-private.h | 3 + src/modbus-rtu.c | 10 +- src/modbus.c | 614 ++++++++++++++++++++++------------ src/modbus.h | 16 +- 8 files changed, 598 insertions(+), 228 deletions(-) create mode 100644 doc/modbus_reply_callback.txt diff --git a/doc/Makefile.am b/doc/Makefile.am index 5a52c0409..bc0efed90 100644 --- a/doc/Makefile.am +++ b/doc/Makefile.am @@ -12,7 +12,7 @@ TXT3 = \ modbus_get_float_dcba.txt \ modbus_get_header_length.txt \ modbus_get_response_timeout.txt \ - modbus_get_slave.txt \ + modbus_get_slave.txt \ modbus_get_socket.txt \ modbus_mapping_free.txt \ modbus_mapping_new.txt \ @@ -29,6 +29,7 @@ TXT3 = \ modbus_receive.txt \ modbus_reply_exception.txt \ modbus_reply.txt \ + modbus_reply_callback.txt \ modbus_report_slave_id.txt \ modbus_rtu_get_serial_mode.txt \ modbus_rtu_set_serial_mode.txt \ diff --git a/doc/modbus_reply.txt b/doc/modbus_reply.txt index 0b29d6f2c..9aa74a3af 100644 --- a/doc/modbus_reply.txt +++ b/doc/modbus_reply.txt @@ -42,6 +42,7 @@ or write). SEE ALSO -------- +linkmb:modbus_reply_callback[3] linkmb:modbus_reply_exception[3] linkmb:libmodbus[7] diff --git a/doc/modbus_reply_callback.txt b/doc/modbus_reply_callback.txt new file mode 100644 index 000000000..3aa316764 --- /dev/null +++ b/doc/modbus_reply_callback.txt @@ -0,0 +1,175 @@ +modbus_reply_callback(3) +======================== + +NAME +---- +modbus_reply_callback - reply to a modbus-request by invoking a user-callback. + +SYNOPSIS +-------- +*int modbus_reply_callback(modbus_t *'ctx', const uint8_t *'request', int 'req_length');* + +*int modbus_set_reply_callbacks(modbus_t *'ctx', modbus_reply_callbacks_t *'cb', void *'user_ctx');* + +[source,c] +---------- +typedef struct { + int (*accept_rtu_slave)(void *user_ctx, int slave); + int (*verify)(void *user_ctx, int slave, int function, uint16_t address, int nb); + int (*read)(void *user_ctx, int slave, int function, uint16_t address, int nb, uint8_t bytes[], int len); + int (*write)(void *user_ctx, int slave, int function, uint16_t address, int nb, const uint8_t bytes[]); +} modbus_reply_callbacks_t; +---------- + +DESCRIPTION +----------- +*modbus_set_reply_callbacks()* has to be used to set the callback-structure into the +libmodbus-context. An optional parameter _user_ctx_ can be suppied which will be passed to the +callback function, typically for context purpose. + +The *modbus_reply_callback()* function receives the request-byte-buffer and its byte-length as +filled in and returned by *modbus_receive()*. It decodes the modbus-request and calls +function-callbacks set by using *modbus_set_reply_callbacks()*. + +Compared to *modbus_reply()* mapping-based function it is designed for more complex situations where +actions are needed to be performed instead of reading or writing to a Modbus mapping. + +Based on the request *modbus_reply_callback()* shall invoke one or more callbacks and send a +response according the received request. + +The user has to provide three callback functions: *verify*, *read* and *write*. When implementing +a RTU-slave, the additional *accept_rtu_slave*-callback has to be provided. + +At different stages *modbus_reply_callback()* calls these callbacks by passing different arguments and +the user-context-pointer _user_ctx_ as the first one. + +The first callback invoked by modbus_reply_callback() is *accept_rtu_slave* if the user implements a +RTU-slave. In its implementation the user has to check whether the slave-id, _slave_, which was +decoded from the request, should be answered to or not and returning TRUE if so, otherwise FALSE. +Returning FALSE will make *modbus_reply_callback()* exit immedialty and return 0. + +All callbacks hereafter receive the following arguments: + + * _slave_ still indicating the RTU-slave-id if the modbus-instance is in RTU-mode otherwise it has + no meaning. + * _function_ containting the value of the modbus-function decoded from the request. + * _address_, the first register or coil-address. + * _nb_, the number of values to be handled. + * _bytes[]_, the byte-buffer containing the raw-data (write-request) or where the raw-data has to + be filled in (read-request). + * The *read*-callback additionally receives the _len_ argument indicating how many bytes can be + filled in. + +*modbus_reply_callback()* is then doing some basic sanitizing on standard-specific parameters before +calling the second callback *verify*. + +This is done to verify whether the access is valid for this instance in regards to the address-range +or the modbus-function of the request. The user has to return 0 if the range defined by _address_ +and _nb_ is inside the device's range and the modbus-function can be handled. + +Otherwise the user shall return EMBXILADD or EMBXILFUN respectively, *modbus_reply_callback()' then sends +an exception as a response. + +The *read* and *write* callbacks are called if *verify* returned 0. The role of these callbacks is +to extract from the buffer (write-requests) or fill into the buffer (read-requests) the requested +data. + +The buffer-pointer given to these two callbacks is pointing to the beginning of the payload-section of +the received or to-be-sent buffer. The data-format used is the one defined by the +modbus-specification and needs to be encoded, or decoded, properly inside the callback-function. +Coils are encoded as up to 8 coils per byte, registers are 16-bit wide and thus consume 2 bytes of the +buffer per value. See below. + +The return value of the read- or write-callbacks has to indicate the number of buffer-bytes +consumed. Negative numbers or zero are interpreted as no response should be send. To, for example, +trigger a timeout on the other side. + +The library does not do any differentiation regarding the actual modbus-function and the appropriate +invoked callback. All MODBUS_READ-functions are handled by the read-callback, all +MODBUS_WRITE-function-requests are passed to the write-callback. When encountering the +MODBUS_FC_WRITE_AND_READ_REGISTERS-function modbus_reply_callback is calling the write and the +read-callback. + +These functions are designed for implementing a Modbus TCP server and RTU slaves. + +BUFFER DATA FORMAT +------------------ + +This table shows the relation between the byte-count and the value-count. + + +------------------------------------+----------+-------------+-----------------+ + | Name | Callback | Bytes-size | Value-size (sz) | + +------------------------------------+----------+-------------+-----------------+ + | MODBUS_FC_READ_COILS | read | (sz + 8 )/8 | as decoded | + | MODBUS_FC_READ_DISCRETE_INPUTS | read | (sz + 8) /8 | as decoded | + | MODBUS_FC_READ_HOLDING_REGISTERS | read | * 2 | as decoded | + | MODBUS_FC_READ_INPUT_REGISTERS | read | * 2 | as decoded | + | MODBUS_FC_WRITE_SINGLE_COIL | write | 1 | 1 | + | MODBUS_FC_WRITE_SINGLE_REGISTER | write | 2 | 1 | + | MODBUS_FC_WRITE_MULTIPLE_COILS | write | (sz + 8) /8 | as decoded | + | MODBUS_FC_WRITE_MULTIPLE_REGISTERS | write | * 2 | as decoded | + +------------------------------------+----------+-------------+-----------------+ + +At transport-level modbus knows two different data formats: Registers and bits. The transport-buffer +is byte-oriented, hence the 'uint8_t *'-type. + +*read* is used for any read access. Based on the function-argument the user has to do the +de-serialization from the payload format to useful data. Coils/bits are stuffed into bytes, +registers are combined over 2 bytes in big-endian byte-order (MSB first). + +*write* is used for any write access. Based on the function-argument the user has to do the +serialization into the payload buffer. The write-callback has to return the number of bytes added to +the buffer. For example having added 9 bits two bytes have been filled and thus the return-value is +two. + +Registers are of 16 bit size and are encored with their most-significant-byte first (big-endian). +For example to put two registers into a byte buffer: + + uint16_t value1 = 0x1234, value2 = 0x5678; + + buf[0] = (value1 >> 8) & 0xff; + buf[1] = value1 & 0xff; + buf[2] = (value2 >> 8) & 0xff; + buf[3] = value2 & 0xff; + +Results in 0x12, 0x34, 0x56, 0x78 on the transport-layer. + +Bits are packed inside the byte-buffer. One byte can thus contain up to 8 bits Here is an example +which shows howto insert 4 bits + + uint8_t buf[1]; + buf[0] = (b3 << 3) | (b2 << 2) | (b1 << 1) | (b0); + + +RETURN VALUE +------------ +The function shall return the length of the response sent if successful. Otherwise it shall return +-1 and set errno. + +ERRORS +------ +*EMBMDATA*:: +Sending has failed + +See also the errors returned by the syscall used to send the response (eg. send +or write). + +EXAMPLE +------- + +The 'modbus_reply' function (based on modbus_mapping_t) is using the 'modbus_reply_callback' +implementation for all read and write ModBus-function and is there a good example. It can be found +in modbus.c . + +SEE ALSO +-------- +linkmb:modbus_reply_exception[3] +linkmb:modbus_reply[3] +linkmb:modbus_set_bits_from_bytes[3] +linkmb:libmodbus[7] +linkmb:modbus_set_slave[3] + +AUTHORS +------- +This libmodbus documentation was written by Patrick Boettcher, based on a first +version by Frode Tennebø diff --git a/doc/modbus_set_slave.txt b/doc/modbus_set_slave.txt index 7f9ecb099..3623f2a04 100644 --- a/doc/modbus_set_slave.txt +++ b/doc/modbus_set_slave.txt @@ -35,6 +35,10 @@ remote device or software drops the requests! The special value The broadcast address is `MODBUS_BROADCAST_ADDRESS`. This special value must be use when you want all Modbus devices of the network receive the request. +When implementing a modbus-slave (a server) setting the *MODBUS_SLAVE_ACCEPT_ALL* address to the +context will make that modbus_receive() will accept all requests instead of filtering out those not +matching the set slave-id. This is useful when implementing multi-slave-implementations. + RETURN VALUE ------------ diff --git a/src/modbus-private.h b/src/modbus-private.h index 2c601c495..23a91d0aa 100644 --- a/src/modbus-private.h +++ b/src/modbus-private.h @@ -101,6 +101,9 @@ struct _modbus { struct timeval indication_timeout; const modbus_backend_t *backend; void *backend_data; + + const modbus_reply_callbacks_t *reply_cb; + void *reply_user_ctx; }; void _modbus_init_common(modbus_t *ctx); diff --git a/src/modbus-rtu.c b/src/modbus-rtu.c index 190298e30..94d517bf1 100644 --- a/src/modbus-rtu.c +++ b/src/modbus-rtu.c @@ -92,7 +92,8 @@ static const uint8_t table_crc_lo[] = { static int _modbus_set_slave(modbus_t *ctx, int slave) { /* Broadcast address is 0 (MODBUS_BROADCAST_ADDRESS) */ - if (slave >= 0 && slave <= 247) { + if ((slave >= 0 && slave <= 247) || + slave == MODBUS_SLAVE_ACCEPT_ALL) { ctx->slave = slave; } else { errno = EINVAL; @@ -364,8 +365,11 @@ static int _modbus_rtu_check_integrity(modbus_t *ctx, uint8_t *msg, int slave = msg[0]; /* Filter on the Modbus unit identifier (slave) in RTU mode to avoid useless - * CRC computing. */ - if (slave != ctx->slave && slave != MODBUS_BROADCAST_ADDRESS) { + * CRC computing. Accept if it is the broadcast address or the slave in the context + * is set to MODBUS_SLAVE_ACCEPT_ALL - then assumes Multi-Slave-server */ + if (slave != ctx->slave && + slave != MODBUS_BROADCAST_ADDRESS && + ctx->slave != MODBUS_SLAVE_ACCEPT_ALL) { if (ctx->debug) { printf("Request for slave %d ignored (not %d)\n", slave, ctx->slave); } diff --git a/src/modbus.c b/src/modbus.c index 03d8da248..936c1e0a4 100644 --- a/src/modbus.c +++ b/src/modbus.c @@ -702,8 +702,55 @@ 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) +static const char *names[] = { + [MODBUS_FC_READ_COILS] = "read_bits", + [MODBUS_FC_READ_DISCRETE_INPUTS] = "read_input_bits", + [MODBUS_FC_READ_HOLDING_REGISTERS] = "read_registers", + [MODBUS_FC_READ_INPUT_REGISTERS] = "read_input_registers", + [MODBUS_FC_WRITE_SINGLE_COIL] = "write_bit", + [MODBUS_FC_WRITE_SINGLE_REGISTER] = "write_register", + [MODBUS_FC_READ_EXCEPTION_STATUS] = "read_exception_status", + [MODBUS_FC_WRITE_MULTIPLE_COILS] = "write_multiple_bits", + [MODBUS_FC_WRITE_MULTIPLE_REGISTERS] = "write_multiple_registers", + [MODBUS_FC_REPORT_SLAVE_ID] = "report_slave_id", + [MODBUS_FC_MASK_WRITE_REGISTER] = "mask_write_register", + [MODBUS_FC_WRITE_AND_READ_REGISTERS] = "write_and_read_registers", +}; + +int modbus_set_reply_callbacks(modbus_t *ctx, const modbus_reply_callbacks_t *cb, void *user_ctx) +{ + if (ctx == NULL) { + errno = EINVAL; + return -1; + } + + if (cb != NULL && + (cb->verify == NULL || + cb->read == NULL || + cb->write == NULL)) { + + if (ctx->debug) + printf("callback-structure is not correctly populated\n"); + + errno = EINVAL; + return -1; + } + + if (ctx->backend->backend_type == _MODBUS_BACKEND_TYPE_RTU && + cb->accept_rtu_slave == NULL) { + if (ctx->debug) + printf("callback-structure is not correctly populated - missing accept_rtu_slave\n"); + errno = EINVAL; + return -1; + } + + ctx->reply_cb = cb; + ctx->reply_user_ctx = user_ctx; + + return 0; +} + +int modbus_reply_callback(modbus_t *ctx, const uint8_t *req, int req_length) { int offset; int slave; @@ -713,7 +760,7 @@ int modbus_reply(modbus_t *ctx, const uint8_t *req, int rsp_length = 0; sft_t sft; - if (ctx == NULL) { + if (ctx == NULL || ctx->reply_cb == NULL) { errno = EINVAL; return -1; } @@ -723,180 +770,84 @@ int modbus_reply(modbus_t *ctx, const uint8_t *req, function = req[offset]; address = (req[offset + 1] << 8) + req[offset + 2]; + /* special RTU-cases error checking */ + if (ctx->backend->backend_type == _MODBUS_BACKEND_TYPE_RTU) { + /* we accept BROADCAST_ADDRESSes */ + if (slave != MODBUS_BROADCAST_ADDRESS) { + /* check whether this slave is handled by this instance and + * suppress any responses when the slave-id not accepted by the user */ + if (ctx->reply_cb->accept_rtu_slave(ctx->reply_user_ctx, slave) == FALSE) { + if (ctx->debug) + fprintf(stderr, "slave ID %d is not handled by this instance\n", slave); + return 0; + } + } + + // TODO broadcast responses should use the slave-id, probably + } + sft.slave = slave; sft.function = function; sft.t_id = ctx->backend->prepare_response_tid(req, &req_length); - /* Data are flushed on illegal number of values errors. */ + /* first do some verifications + * for read and write this is the first stage only + * "simple"-function-replies are constructed here */ + + int nb = 0; /* extracted number of values to written or read */ + int max_nb = 0; /* maximum number of values to written or read */ + int is_read = 0; /* is this a read-request */ + int verified = 0; /* return-code of intermediate verify-calls */ + switch (function) { case MODBUS_FC_READ_COILS: - case MODBUS_FC_READ_DISCRETE_INPUTS: { - unsigned int is_input = (function == MODBUS_FC_READ_DISCRETE_INPUTS); - int start_bits = is_input ? mb_mapping->start_input_bits : mb_mapping->start_bits; - int nb_bits = is_input ? mb_mapping->nb_input_bits : mb_mapping->nb_bits; - uint8_t *tab_bits = is_input ? mb_mapping->tab_input_bits : mb_mapping->tab_bits; - const char * const name = is_input ? "read_input_bits" : "read_bits"; - int nb = (req[offset + 3] << 8) + req[offset + 4]; - /* The mapping can be shifted to reduce memory consumption and it - doesn't always start at address zero. */ - int mapping_address = address - start_bits; - - if (nb < 1 || MODBUS_MAX_READ_BITS < nb) { - rsp_length = response_exception( - ctx, &sft, MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE, rsp, TRUE, - "Illegal nb of values %d in %s (max %d)\n", - nb, name, MODBUS_MAX_READ_BITS); - } else if (mapping_address < 0 || (mapping_address + nb) > nb_bits) { - rsp_length = response_exception( - ctx, &sft, - MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp, FALSE, - "Illegal data address 0x%0X in %s\n", - mapping_address < 0 ? address : address + nb, name); - } else { - rsp_length = ctx->backend->build_response_basis(&sft, rsp); - rsp[rsp_length++] = (nb / 8) + ((nb % 8) ? 1 : 0); - rsp_length = response_io_status(tab_bits, mapping_address, nb, - rsp, rsp_length); - } - } + case MODBUS_FC_READ_DISCRETE_INPUTS: + is_read = 1; + /* fall-through */ + case MODBUS_FC_WRITE_MULTIPLE_COILS: + nb = (req[offset + 3] << 8) + req[offset + 4]; + max_nb = is_read ? MODBUS_MAX_READ_BITS : MODBUS_MAX_WRITE_BITS; break; - case MODBUS_FC_READ_HOLDING_REGISTERS: - case MODBUS_FC_READ_INPUT_REGISTERS: { - unsigned int is_input = (function == MODBUS_FC_READ_INPUT_REGISTERS); - int start_registers = is_input ? mb_mapping->start_input_registers : mb_mapping->start_registers; - int nb_registers = is_input ? mb_mapping->nb_input_registers : mb_mapping->nb_registers; - uint16_t *tab_registers = is_input ? mb_mapping->tab_input_registers : mb_mapping->tab_registers; - const char * const name = is_input ? "read_input_registers" : "read_registers"; - int nb = (req[offset + 3] << 8) + req[offset + 4]; - /* The mapping can be shifted to reduce memory consumption and it - doesn't always start at address zero. */ - int mapping_address = address - start_registers; - - if (nb < 1 || MODBUS_MAX_READ_REGISTERS < nb) { - rsp_length = response_exception( - ctx, &sft, MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE, rsp, TRUE, - "Illegal nb of values %d in %s (max %d)\n", - nb, name, MODBUS_MAX_READ_REGISTERS); - } else if (mapping_address < 0 || (mapping_address + nb) > nb_registers) { - rsp_length = response_exception( - ctx, &sft, MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp, FALSE, - "Illegal data address 0x%0X in %s\n", - mapping_address < 0 ? address : address + nb, name); - } else { - int i; - - rsp_length = ctx->backend->build_response_basis(&sft, rsp); - rsp[rsp_length++] = nb << 1; - for (i = mapping_address; i < mapping_address + nb; i++) { - rsp[rsp_length++] = tab_registers[i] >> 8; - rsp[rsp_length++] = tab_registers[i] & 0xFF; - } - } - } + case MODBUS_FC_WRITE_SINGLE_COIL: + nb = 1; + max_nb = MODBUS_MAX_WRITE_BITS; break; - case MODBUS_FC_WRITE_SINGLE_COIL: { - int mapping_address = address - mb_mapping->start_bits; - - if (mapping_address < 0 || mapping_address >= mb_mapping->nb_bits) { - rsp_length = response_exception( - ctx, &sft, MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp, FALSE, - "Illegal data address 0x%0X in write_bit\n", - address); - } else { - int data = (req[offset + 3] << 8) + req[offset + 4]; - - if (data == 0xFF00 || data == 0x0) { - mb_mapping->tab_bits[mapping_address] = data ? ON : OFF; - memcpy(rsp, req, req_length); - rsp_length = req_length; - } else { - rsp_length = response_exception( - ctx, &sft, - MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE, rsp, FALSE, - "Illegal data value 0x%0X in write_bit request at address %0X\n", - data, address); - } - } - } + case MODBUS_FC_MASK_WRITE_REGISTER: + case MODBUS_FC_WRITE_SINGLE_REGISTER: + nb = 1; + max_nb = MODBUS_MAX_WRITE_REGISTERS; break; - case MODBUS_FC_WRITE_SINGLE_REGISTER: { - int mapping_address = address - mb_mapping->start_registers; + case MODBUS_FC_READ_HOLDING_REGISTERS: + case MODBUS_FC_READ_INPUT_REGISTERS: + is_read = 1; + /* fall-through */ + case MODBUS_FC_WRITE_MULTIPLE_REGISTERS: + nb = (req[offset + 3] << 8) + req[offset + 4]; + max_nb = is_read ? MODBUS_MAX_READ_REGISTERS : MODBUS_MAX_WRITE_REGISTERS; + break; + case MODBUS_FC_WRITE_AND_READ_REGISTERS: + nb = (req[offset + 3] << 8) + req[offset + 4]; + max_nb = MODBUS_MAX_WR_READ_REGISTERS; - if (mapping_address < 0 || mapping_address >= mb_mapping->nb_registers) { - rsp_length = response_exception( - ctx, &sft, - MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp, FALSE, - "Illegal data address 0x%0X in write_register\n", - address); - } else { - int data = (req[offset + 3] << 8) + req[offset + 4]; + { /* write-part is verified here */ + uint16_t address_write = (req[offset + 5] << 8) + req[offset + 6]; + int nb_write = (req[offset + 7] << 8) + req[offset + 8]; - mb_mapping->tab_registers[mapping_address] = data; - memcpy(rsp, req, req_length); - rsp_length = req_length; - } - } - break; - case MODBUS_FC_WRITE_MULTIPLE_COILS: { - int nb = (req[offset + 3] << 8) + req[offset + 4]; - int mapping_address = address - mb_mapping->start_bits; + /* first the address verification */ + verified = ctx->reply_cb->verify(ctx->reply_user_ctx, slave, MODBUS_FC_WRITE_MULTIPLE_REGISTERS, + address_write, nb_write); - if (nb < 1 || MODBUS_MAX_WRITE_BITS < nb) { - /* 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. */ - rsp_length = response_exception( - ctx, &sft, MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE, rsp, TRUE, - "Illegal number of values %d in write_bits (max %d)\n", - nb, MODBUS_MAX_WRITE_BITS); - } else if (mapping_address < 0 || - (mapping_address + nb) > mb_mapping->nb_bits) { - rsp_length = response_exception( - ctx, &sft, - MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp, FALSE, - "Illegal data address 0x%0X in write_bits\n", - mapping_address < 0 ? address : address + nb); - } else { - /* 6 = byte count */ - modbus_set_bits_from_bytes(mb_mapping->tab_bits, mapping_address, 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; - } - } - break; - case MODBUS_FC_WRITE_MULTIPLE_REGISTERS: { - int nb = (req[offset + 3] << 8) + req[offset + 4]; - int mapping_address = address - mb_mapping->start_registers; + int nb_write_bytes = req[offset + 9]; + if (nb_write < 1 || + MODBUS_MAX_WR_WRITE_REGISTERS < nb_write || + nb_write_bytes != nb_write * 2) { - if (nb < 1 || MODBUS_MAX_WRITE_REGISTERS < nb) { - rsp_length = response_exception( - ctx, &sft, MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE, rsp, TRUE, - "Illegal number of values %d in write_registers (max %d)\n", - nb, MODBUS_MAX_WRITE_REGISTERS); - } else if (mapping_address < 0 || - (mapping_address + nb) > mb_mapping->nb_registers) { - rsp_length = response_exception( - ctx, &sft, MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp, FALSE, - "Illegal data address 0x%0X in write_registers\n", - mapping_address < 0 ? address : address + nb); - } else { - int i, j; - for (i = mapping_address, j = 6; i < mapping_address + nb; i++, j += 2) { - /* 6 and 7 = first value */ - mb_mapping->tab_registers[i] = - (req[offset + j] << 8) + req[offset + j + 1]; + rsp_length = response_exception( + ctx, &sft, MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE, rsp, TRUE, + "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); } - - 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; case MODBUS_FC_REPORT_SLAVE_ID: { int str_len; @@ -913,8 +864,7 @@ int modbus_reply(modbus_t *ctx, const uint8_t *req, memcpy(rsp + rsp_length, "LMB" LIBMODBUS_VERSION_STRING, str_len); rsp_length += str_len; rsp[byte_count_pos] = rsp_length - byte_count_pos - 1; - } - break; + } break; case MODBUS_FC_READ_EXCEPTION_STATUS: if (ctx->debug) { fprintf(stderr, "FIXME Not implemented\n"); @@ -922,82 +872,300 @@ int modbus_reply(modbus_t *ctx, const uint8_t *req, errno = ENOPROTOOPT; return -1; break; - case MODBUS_FC_MASK_WRITE_REGISTER: { - int mapping_address = address - mb_mapping->start_registers; + default: + rsp_length = response_exception( + ctx, &sft, MODBUS_EXCEPTION_ILLEGAL_FUNCTION, rsp, TRUE, + "Unknown Modbus function code: 0x%0X\n", function); + break; + } - if (mapping_address < 0 || mapping_address >= mb_mapping->nb_registers) { - rsp_length = response_exception( - ctx, &sft, MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp, FALSE, - "Illegal data address 0x%0X in write_register\n", - address); - } else { - uint16_t data = mb_mapping->tab_registers[mapping_address]; - uint16_t and = (req[offset + 3] << 8) + req[offset + 4]; - uint16_t or = (req[offset + 5] << 8) + req[offset + 6]; + if (ctx->debug) + fprintf(stderr, "function %s (%x), %d, %d, max: %d, resp: %d\n", + names[function], function, address, nb, max_nb, rsp_length); + + /* we already have a response - we are done */ + if (rsp_length > 0) + goto send_response; + + /* verify this (second part) of the read/write access + * MODBUS_FC_WRITE_AND_READ_REGISTERS has two verifications to be done - one is aleady done */ + if (verified == 0) + verified = ctx->reply_cb->verify(ctx->reply_user_ctx, slave, function, address, nb); + + /* out of reply-buffer-range */ + if (nb < 1 || max_nb < nb) { + /* Maybe 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. */ + rsp_length = response_exception( + ctx, &sft, MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE, rsp, TRUE, + "Illegal nb of values %d in %s (max %d)\n", + nb, names[function], max_nb); + goto send_response; + } - data = (data & and) | (or & (~and)); - mb_mapping->tab_registers[mapping_address] = data; + if (verified == EMBXILADD) { /* verify found an invalid address */ + rsp_length = response_exception( + ctx, &sft, + MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp, FALSE, + "Illegal data address 0x%0X in %s\n", + address, names[function]); + goto send_response; + } else if (verified == EMBXILFUN) { /* verify found an invalid function */ + rsp_length = response_exception( + ctx, &sft, + MODBUS_EXCEPTION_ILLEGAL_FUNCTION, rsp, FALSE, + "Slave/client does not accept Modbus function code: 0x%0X (%s)\n", + function, names[function]); + goto send_response; + } else if (verified != 0) { /* another error has occured */ + errno = EINVAL; + return -1; + } + + /* user verification was successful */ + + int rc; + rsp_length = ctx->backend->build_response_basis(&sft, rsp); + + switch (function) { + case MODBUS_FC_READ_COILS: + case MODBUS_FC_READ_DISCRETE_INPUTS: + rsp[rsp_length++] = (nb / 8) + ((nb % 8) ? 1 : 0); + rc = ctx->reply_cb->read(ctx->reply_user_ctx, slave, function, + address, nb, &rsp[rsp_length], sizeof(rsp) - rsp_length); + if (rc <= 0) { + rsp_length = 0; + goto send_response; + } + + rsp_length += rc; + break; + case MODBUS_FC_READ_HOLDING_REGISTERS: + case MODBUS_FC_READ_INPUT_REGISTERS: + rsp[rsp_length++] = nb * 2; /* number of register x 2 is the number of bytes */ + rc = ctx->reply_cb->read(ctx->reply_user_ctx, slave, function, + address, nb, &rsp[rsp_length], sizeof(rsp) - rsp_length); + if (rc <= 0) { + rsp_length = 0; + goto send_response; + } + + rsp_length += rc; + break; + + case MODBUS_FC_WRITE_MULTIPLE_COILS: + case MODBUS_FC_WRITE_MULTIPLE_REGISTERS: + /* 6 = byte count (and 7 for registers */ + rc = ctx->reply_cb->write(ctx->reply_user_ctx, slave, function, address, nb, &req[offset + 6]); + + if (rc <= 0) { + rsp_length = 0; + goto send_response; + } + + /* 4 to copy the reg/bit address (2) and the quantity of bits/regs */ + memcpy(rsp + rsp_length, req + rsp_length, 4); + rsp_length += 4; + break; + + case MODBUS_FC_WRITE_SINGLE_COIL: { + int data = (req[offset + 3] << 8) + req[offset + 4]; /* transform 0xff00/0x0000 to 0x01 in a byte */ + + if (data == 0xFF00 || data == 0x0) { + uint8_t b = data ? ON : OFF; + rc = ctx->reply_cb->write(ctx->reply_user_ctx, slave, function, address, 1, &b); + if (rc <= 0) { + rsp_length = 0; + goto send_response; + } memcpy(rsp, req, req_length); rsp_length = req_length; + } else { + rsp_length = response_exception( + ctx, &sft, + MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE, rsp, FALSE, + "Illegal data value 0x%0X in write_bit request at address %0X\n", + data, address); } - } + } break; + + case MODBUS_FC_MASK_WRITE_REGISTER: + case MODBUS_FC_WRITE_SINGLE_REGISTER: + rc = ctx->reply_cb->write(ctx->reply_user_ctx, slave, function, address, 1, &req[offset + 3]); + if (rc <= 0) { + rsp_length = 0; + goto send_response; + } + memcpy(rsp, req, req_length); + rsp_length = req_length; break; + case MODBUS_FC_WRITE_AND_READ_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]; - int nb_write_bytes = req[offset + 9]; - int mapping_address = address - mb_mapping->start_registers; - int mapping_address_write = address_write - mb_mapping->start_registers; - 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) { - rsp_length = response_exception( - ctx, &sft, MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE, rsp, TRUE, - "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 if (mapping_address < 0 || - (mapping_address + nb) > mb_mapping->nb_registers || - mapping_address < 0 || - (mapping_address_write + nb_write) > mb_mapping->nb_registers) { - rsp_length = response_exception( - ctx, &sft, MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp, FALSE, - "Illegal data read address 0x%0X or write address 0x%0X write_and_read_registers\n", - mapping_address < 0 ? address : address + nb, - mapping_address_write < 0 ? address_write : address_write + nb_write); - } 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 = mapping_address_write, j = 10; - i < mapping_address_write + nb_write; i++, j += 2) { - mb_mapping->tab_registers[i] = - (req[offset + j] << 8) + req[offset + j + 1]; - } + rsp_length = ctx->backend->build_response_basis(&sft, rsp); + rsp[rsp_length++] = nb << 1; - /* and read the data for the response */ - for (i = mapping_address; i < mapping_address + nb; i++) { - rsp[rsp_length++] = mb_mapping->tab_registers[i] >> 8; - rsp[rsp_length++] = mb_mapping->tab_registers[i] & 0xFF; - } + /* Write first. 10 and 11 are the offset of the first values to write */ + rc = ctx->reply_cb->write(ctx->reply_user_ctx, slave, function, address_write, nb_write, &req[offset + 10]); + if (rc <= 0) { + rsp_length = 0; + goto send_response; } - } + + /* and read the data for the response */ + rc = ctx->reply_cb->read(ctx->reply_user_ctx, slave, function, + address, nb, &rsp[rsp_length], sizeof(rsp) - rsp_length); + if (rc <= 0) { + rsp_length = 0; + goto send_response; + } else + rsp_length += rc; + } break; + default: break; + } + +send_response: + if ((ctx->backend->backend_type == _MODBUS_BACKEND_TYPE_RTU && + slave == MODBUS_BROADCAST_ADDRESS) || + rsp_length == 0) /* this indicates that the user does not want us to send response, + probably to trigger a timeout on the other side */ + return 0; + else + return send_msg(ctx, rsp, rsp_length); +} + +static int mb_mapping_accept_rtu_slave(void *user_ctx, int slave) +{ + return TRUE; +} + +static int mb_mapping_verify(void *user_ctx, int slave, int function, uint16_t address, int nb) +{ + modbus_mapping_t *mb_mapping = user_ctx; + + unsigned int is_input = 0; + + switch (function) { + case MODBUS_FC_READ_DISCRETE_INPUTS: + is_input = 1; /* fall-through */ + case MODBUS_FC_READ_COILS: + case MODBUS_FC_WRITE_SINGLE_COIL: + case MODBUS_FC_WRITE_MULTIPLE_COILS: { + int start_bits = is_input ? mb_mapping->start_input_bits : mb_mapping->start_bits; + int nb_bits = is_input ? mb_mapping->nb_input_bits : mb_mapping->nb_bits; + int mapping_address = address - start_bits; + if (mapping_address < 0 || (mapping_address + nb) > nb_bits) + return EMBXILADD; + } break; + + case MODBUS_FC_READ_INPUT_REGISTERS: + is_input = 1; /* fall-through */ + case MODBUS_FC_READ_HOLDING_REGISTERS: + case MODBUS_FC_WRITE_SINGLE_REGISTER: + case MODBUS_FC_WRITE_MULTIPLE_REGISTERS: + case MODBUS_FC_MASK_WRITE_REGISTER: + case MODBUS_FC_WRITE_AND_READ_REGISTERS: { + int nb_registers = is_input ? mb_mapping->nb_input_registers : mb_mapping->nb_registers; + int start_registers = is_input ? mb_mapping->start_input_registers : mb_mapping->start_registers; + int mapping_address = address - start_registers; + if (mapping_address < 0 || (mapping_address + nb) > nb_registers) + return EMBXILADD; + } break; + } + + return 0; +} + +static int mb_mapping_read(void *user_ctx, int slave, int function, uint16_t address, int nb, uint8_t *rsp, int max_len) +{ + modbus_mapping_t *mb_mapping = user_ctx; + + unsigned int is_input = 0; + int length = 0, i; + + switch (function) { + case MODBUS_FC_READ_INPUT_REGISTERS: + is_input = 1; /* fall-through */ + case MODBUS_FC_READ_HOLDING_REGISTERS: + case MODBUS_FC_WRITE_AND_READ_REGISTERS: { + int start_registers = is_input ? mb_mapping->start_input_registers : mb_mapping->start_registers; + uint16_t *tab_registers = is_input ? mb_mapping->tab_input_registers : mb_mapping->tab_registers; + int mapping_address = address - start_registers; + + for (i = mapping_address; i < mapping_address + nb; i++) { + rsp[length++] = tab_registers[i] >> 8; + rsp[length++] = tab_registers[i] & 0xFF; + } + } break; + + case MODBUS_FC_READ_DISCRETE_INPUTS: + is_input = 1; /* fall-through */ + case MODBUS_FC_READ_COILS: { + uint8_t *tab_bits = is_input ? mb_mapping->tab_input_bits : mb_mapping->tab_bits; + int start_bits = is_input ? mb_mapping->start_input_bits : mb_mapping->start_bits; + int mapping_address = address - start_bits; + length = response_io_status(tab_bits, mapping_address, nb, rsp, 0); + } break; default: - rsp_length = response_exception( - ctx, &sft, MODBUS_EXCEPTION_ILLEGAL_FUNCTION, rsp, TRUE, - "Unknown Modbus function code: 0x%0X\n", function); break; } - /* Suppress any responses when the request was a broadcast */ - return (ctx->backend->backend_type == _MODBUS_BACKEND_TYPE_RTU && - slave == MODBUS_BROADCAST_ADDRESS) ? 0 : send_msg(ctx, rsp, rsp_length); + return length; +} + +static int mb_mapping_write(void *user_ctx, int slave, int function, uint16_t address, int nb, const uint8_t *req) +{ + modbus_mapping_t *mb_mapping = user_ctx; + + switch (function) { + case MODBUS_FC_WRITE_SINGLE_COIL: + case MODBUS_FC_WRITE_MULTIPLE_COILS: { + int mapping_address = address - mb_mapping->start_bits; + modbus_set_bits_from_bytes(mb_mapping->tab_bits, mapping_address, nb, req); + } break; + + case MODBUS_FC_WRITE_SINGLE_REGISTER: + case MODBUS_FC_WRITE_MULTIPLE_REGISTERS: + case MODBUS_FC_WRITE_AND_READ_REGISTERS: { + uint16_t mapping_address = address - mb_mapping->start_registers; + int i, j; + for (i = mapping_address, j = 0; i < mapping_address + nb; i++, j += 2) { + mb_mapping->tab_registers[i] = (req[j] << 8) + req[j + 1]; + } + } break; + + case MODBUS_FC_MASK_WRITE_REGISTER: { + uint16_t mapping_address = address - mb_mapping->start_registers; + uint16_t data = mb_mapping->tab_registers[mapping_address]; + uint16_t and = (req[0] << 8) + req[1]; + uint16_t or = (req[2] << 8) + req[3]; + data = (data & and) | (or &(~and)); + mb_mapping->tab_registers[mapping_address] = data; + } break; + + default: + return -EINVAL; + } + return 0; +} + +static const modbus_reply_callbacks_t mb_mapping_callbacks = { + mb_mapping_accept_rtu_slave, + mb_mapping_verify, + mb_mapping_read, + mb_mapping_write, +}; + +int modbus_reply(modbus_t *ctx, const uint8_t *req, + int req_length, modbus_mapping_t *mb_mapping) +{ + modbus_set_reply_callbacks(ctx, &mb_mapping_callbacks, mb_mapping); + return modbus_reply_callback(ctx, req, req_length); } int modbus_reply_exception(modbus_t *ctx, const uint8_t *req, @@ -1562,7 +1730,7 @@ int modbus_report_slave_id(modbus_t *ctx, int max_dest, uint8_t *dest) void _modbus_init_common(modbus_t *ctx) { /* Slave and socket are initialized to -1 */ - ctx->slave = -1; + ctx->slave = MODBUS_SLAVE_INIT; ctx->s = -1; ctx->debug = FALSE; diff --git a/src/modbus.h b/src/modbus.h index fda3f02b7..a682983fa 100644 --- a/src/modbus.h +++ b/src/modbus.h @@ -71,7 +71,9 @@ MODBUS_BEGIN_DECLS #define MODBUS_FC_MASK_WRITE_REGISTER 0x16 #define MODBUS_FC_WRITE_AND_READ_REGISTERS 0x17 -#define MODBUS_BROADCAST_ADDRESS 0 +#define MODBUS_SLAVE_ACCEPT_ALL -2 +#define MODBUS_SLAVE_INIT -1 +#define MODBUS_BROADCAST_ADDRESS 0 /* Modbus_Application_Protocol_V1_1b.pdf (chapter 6 section 1 page 12) * Quantity of Coils to read (2 bytes): 1 to 2000 (0x7D0) @@ -176,6 +178,13 @@ typedef enum MODBUS_ERROR_RECOVERY_PROTOCOL = (1<<2) } modbus_error_recovery_mode; +typedef struct { + int (*accept_rtu_slave)(void *user_ctx, int slave); + int (*verify)(void *user_ctx, int slave, int function, uint16_t address, int nb); + int (*read)(void *user_ctx, int slave, int function, uint16_t address, int nb, uint8_t bytes[], int len); + int (*write)(void *user_ctx, int slave, int function, uint16_t address, int nb, const uint8_t bytes[]); +} modbus_reply_callbacks_t; + MODBUS_API int modbus_set_slave(modbus_t* ctx, int slave); MODBUS_API int modbus_get_slave(modbus_t* ctx); MODBUS_API int modbus_set_error_recovery(modbus_t *ctx, modbus_error_recovery_mode error_recovery); @@ -237,6 +246,11 @@ 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, unsigned int exception_code); +MODBUS_API int modbus_set_reply_callbacks(modbus_t *ctx, + const modbus_reply_callbacks_t *cb, + void *user_ctx); +MODBUS_API int modbus_reply_callback(modbus_t *ctx, const uint8_t *req, + int req_length); /** * UTILS FUNCTIONS