From 73a88a747c473a7427ef70f0351ff6f5370c3495 Mon Sep 17 00:00:00 2001 From: Stéphane Raimbault Date: Fri, 20 May 2016 12:01:13 +0200 Subject: [PATCH] DRY in modbus_reply by improving response_exception() --- src/modbus.c | 216 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++------------------------------------------------------------------------------------------------------------------------------------------ 1 file changed, 78 insertions(+), 138 deletions(-) diff --git a/src/modbus.c b/src/modbus.c index 48e2c59..e2e524a 100644 --- a/src/modbus.c +++ b/src/modbus.c @@ -10,6 +10,7 @@ #include #include #include +#include #include #include #include @@ -658,14 +659,30 @@ static int response_io_status(uint8_t *tab_io_status, /* Build the exception response */ static int response_exception(modbus_t *ctx, sft_t *sft, - int exception_code, uint8_t *rsp) + int exception_code, uint8_t *rsp, + unsigned int to_flush, + const char* template, ...) { int rsp_length; + /* Print debug message */ + if (ctx->debug) { + va_list ap; + + va_start(ap, template); + vfprintf(stderr, template, ap); + va_end(ap); + } + + /* Flush if required */ + if (to_flush) { + _sleep_response_timeout(ctx); + modbus_flush(ctx); + } + + /* Build exception response */ sft->function = sft->function + 0x80; rsp_length = ctx->backend->build_response_basis(sft, rsp); - - /* Positive exception code */ rsp[rsp_length++] = exception_code; return rsp_length; @@ -711,25 +728,17 @@ int modbus_reply(modbus_t *ctx, const uint8_t *req, int mapping_address = address - mb_mapping->start_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); + ctx, &sft, MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE, rsp, TRUE, + "Illegal nb of values %d in read_bits (max %d)\n", + nb, MODBUS_MAX_READ_BITS); } else if (mapping_address < 0 || (mapping_address + nb) > mb_mapping->nb_bits) { - if (ctx->debug) { - fprintf(stderr, "Illegal data address 0x%0X in read_bits\n", - mapping_address < 0 ? address : address + nb); - } rsp_length = response_exception( ctx, &sft, - MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp); + MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp, FALSE, + "Illegal data address 0x%0X in read_bits\n", + mapping_address < 0 ? address : address + nb); } else { rsp_length = ctx->backend->build_response_basis(&sft, rsp); rsp[rsp_length++] = (nb / 8) + ((nb % 8) ? 1 : 0); @@ -746,25 +755,17 @@ int modbus_reply(modbus_t *ctx, const uint8_t *req, int mapping_address = address - mb_mapping->start_input_bits; 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); - } - _sleep_response_timeout(ctx); - modbus_flush(ctx); rsp_length = response_exception( - ctx, &sft, - MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE, rsp); + ctx, &sft, MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE, rsp, TRUE, + "Illegal nb of values %d in read_input_bits (max %d)\n", + nb, MODBUS_MAX_READ_BITS); } else if (mapping_address < 0 || (mapping_address + nb) > mb_mapping->nb_input_bits) { - if (ctx->debug) { - fprintf(stderr, "Illegal data address 0x%0X in read_input_bits\n", - mapping_address < 0 ? address : address + nb); - } rsp_length = response_exception( ctx, &sft, - MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp); + MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp, FALSE, + "Illegal data address 0x%0X in read_input_bits\n", + mapping_address < 0 ? address : address + nb); } else { rsp_length = ctx->backend->build_response_basis(&sft, rsp); rsp[rsp_length++] = (nb / 8) + ((nb % 8) ? 1 : 0); @@ -779,25 +780,16 @@ int modbus_reply(modbus_t *ctx, const uint8_t *req, int mapping_address = address - mb_mapping->start_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); + ctx, &sft, MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE, rsp, TRUE, + "Illegal nb of values %d in read_holding_registers (max %d)\n", + nb, MODBUS_MAX_READ_REGISTERS); } else if (mapping_address < 0 || (mapping_address + nb) > mb_mapping->nb_registers) { - if (ctx->debug) { - fprintf(stderr, "Illegal data address 0x%0X in read_registers\n", - mapping_address < 0 ? address : address + nb); - } rsp_length = response_exception( - ctx, &sft, - MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp); + ctx, &sft, MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp, FALSE, + "Illegal data address 0x%0X in read_registers\n", + mapping_address < 0 ? address : address + nb); } else { int i; @@ -817,25 +809,16 @@ int modbus_reply(modbus_t *ctx, const uint8_t *req, int mapping_address = address - mb_mapping->start_input_registers; 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); - } - _sleep_response_timeout(ctx); - modbus_flush(ctx); rsp_length = response_exception( - ctx, &sft, - MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE, rsp); + ctx, &sft, MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE, rsp, TRUE, + "Illegal number of values %d in read_input_registers (max %d)\n", + nb, MODBUS_MAX_READ_REGISTERS); } else if (mapping_address < 0 || (mapping_address + nb) > mb_mapping->nb_input_registers) { - if (ctx->debug) { - fprintf(stderr, "Illegal data address 0x%0X in read_input_registers\n", - mapping_address < 0 ? address : address + nb); - } rsp_length = response_exception( - ctx, &sft, - MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp); + ctx, &sft, MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp, FALSE, + "Illegal data address 0x%0X in read_input_registers\n", + mapping_address < 0 ? address : address + nb); } else { int i; @@ -852,14 +835,10 @@ int modbus_reply(modbus_t *ctx, const uint8_t *req, int mapping_address = address - mb_mapping->start_bits; if (mapping_address < 0 || mapping_address >= mb_mapping->nb_bits) { - 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); + 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]; @@ -868,14 +847,11 @@ int modbus_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, FALSE, + "Illegal data value 0x%0X in write_bit request at address %0X\n", + data, address); } } } @@ -884,13 +860,11 @@ int modbus_reply(modbus_t *ctx, const uint8_t *req, int mapping_address = address - mb_mapping->start_registers; if (mapping_address < 0 || mapping_address >= mb_mapping->nb_registers) { - 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, FALSE, + "Illegal data address 0x%0X in write_register\n", + address); } else { int data = (req[offset + 3] << 8) + req[offset + 4]; @@ -905,28 +879,20 @@ int modbus_reply(modbus_t *ctx, const uint8_t *req, int mapping_address = address - mb_mapping->start_bits; 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); + 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) { - if (ctx->debug) { - fprintf(stderr, "Illegal data address 0x%0X in write_bits\n", - mapping_address < 0 ? address : address + nb); - } rsp_length = response_exception( ctx, &sft, - MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp); + 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, @@ -944,28 +910,16 @@ int modbus_reply(modbus_t *ctx, const uint8_t *req, int mapping_address = address - mb_mapping->start_registers; 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); + 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) { - if (ctx->debug) { - fprintf(stderr, "Illegal data address 0x%0X in write_registers\n", - mapping_address < 0 ? address : address + nb); - } rsp_length = response_exception( - ctx, &sft, - MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp); + 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) { @@ -1009,13 +963,10 @@ int modbus_reply(modbus_t *ctx, const uint8_t *req, int mapping_address = address - mb_mapping->start_registers; if (mapping_address < 0 || mapping_address >= mb_mapping->nb_registers) { - 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); + 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]; @@ -1039,29 +990,19 @@ int modbus_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); + 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) { - if (ctx->debug) { - fprintf(stderr, - "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); - } - rsp_length = response_exception(ctx, &sft, - MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp); + 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); @@ -1085,9 +1026,8 @@ int modbus_reply(modbus_t *ctx, const uint8_t *req, break; default: - rsp_length = response_exception(ctx, &sft, - MODBUS_EXCEPTION_ILLEGAL_FUNCTION, - rsp); + rsp_length = response_exception( + ctx, &sft, MODBUS_EXCEPTION_ILLEGAL_FUNCTION, rsp, FALSE, ""); break; } -- libgit2 0.21.4