diff --git a/src/modbus.c b/src/modbus.c index ef71185..125e88a 100644 --- a/src/modbus.c +++ b/src/modbus.c @@ -483,6 +483,7 @@ static int check_confirmation(modbus_t *ctx, uint8_t *req, int rc; int rsp_length_computed; const int offset = ctx->backend->header_length; + const int function = rsp[offset]; if (ctx->backend->pre_check_confirmation) { rc = ctx->backend->pre_check_confirmation(ctx, req, rsp, rsp_length); @@ -496,12 +497,33 @@ static int check_confirmation(modbus_t *ctx, uint8_t *req, rsp_length_computed = compute_response_length_from_request(ctx, req); + /* Exception code */ + if (function >= 0x80) { + if (rsp_length == (offset + 2 + ctx->backend->checksum_length) && + req[offset] == (rsp[offset] - 0x80)) { + /* Valid exception code received */ + + int exception_code = rsp[offset + 1]; + if (exception_code < MODBUS_EXCEPTION_MAX) { + errno = MODBUS_ENOBASE + exception_code; + } else { + errno = EMBBADEXC; + } + _error_print(ctx, NULL); + return -1; + } else { + errno = EMBBADEXC; + _error_print(ctx, NULL); + return -1; + } + } + /* Check length */ - if (rsp_length == rsp_length_computed || - rsp_length_computed == MSG_LENGTH_UNDEFINED) { + if ((rsp_length == rsp_length_computed || + rsp_length_computed == MSG_LENGTH_UNDEFINED) && + function < 0x80) { int req_nb_value; int rsp_nb_value; - const int function = rsp[offset]; /* Check function code */ if (function != req[offset]) { @@ -566,18 +588,6 @@ static int check_confirmation(modbus_t *ctx, uint8_t *req, errno = EMBBADDATA; rc = -1; } - } else if (rsp_length == (offset + 2 + ctx->backend->checksum_length) && - req[offset] == (rsp[offset] - 0x80)) { - /* EXCEPTION CODE RECEIVED */ - - int exception_code = rsp[offset + 1]; - if (exception_code < MODBUS_EXCEPTION_MAX) { - errno = MODBUS_ENOBASE + exception_code; - } else { - errno = EMBBADEXC; - } - _error_print(ctx, NULL); - rc = -1; } else { if (ctx->debug) { fprintf(stderr,