From 0bf4916efd676062b950a8552a8d54fd531392be Mon Sep 17 00:00:00 2001 From: Johann Fischer Date: Tue, 9 Mar 2021 12:00:32 +0100 Subject: [PATCH] modbus: move some RX/TX ADU related code to the core Let the core call the modbus_tx_adu() to make the process more comprehensible. Move tx-wait-for-rx handling outside of client code. Signed-off-by: Johann Fischer --- subsys/modbus/modbus_client.c | 21 +++++------------ subsys/modbus/modbus_core.c | 24 +++++++++++++++----- subsys/modbus/modbus_internal.h | 1 + subsys/modbus/modbus_server.c | 40 ++++++++++++++------------------- 4 files changed, 43 insertions(+), 43 deletions(-) diff --git a/subsys/modbus/modbus_client.c b/subsys/modbus/modbus_client.c index b761577ea67..787f31a67c9 100644 --- a/subsys/modbus/modbus_client.c +++ b/subsys/modbus/modbus_client.c @@ -244,26 +244,18 @@ static int mbc_send_cmd(struct modbus_context *ctx, const uint8_t unit_id, ctx->tx_adu.unit_id = unit_id; ctx->tx_adu.fc = fc; - modbus_tx_adu(ctx); - - if (k_sem_take(&ctx->client_wait_sem, K_USEC(ctx->rxwait_to)) != 0) { - LOG_WRN("Client wait-for-RX timeout"); - err = -EIO; - goto exit_error; - } - - if (ctx->rx_adu_err != 0) { - err = ctx->rx_adu_err; - goto exit_error; + err = modbus_tx_wait_rx_adu(ctx); + if (err != 0) { + return err; } err = mbc_validate_response_fc(ctx, unit_id, fc); if (err < 0) { - LOG_ERR("Failed to validate address or function code"); - goto exit_error; + LOG_ERR("Failed to validate unit ID or function code"); + return err; } else if (err > 0) { LOG_INF("Modbus FC %u, error code %u", fc, err); - goto exit_error; + return err; } switch (fc) { @@ -290,7 +282,6 @@ static int mbc_send_cmd(struct modbus_context *ctx, const uint8_t unit_id, err = -ENOTSUP; } -exit_error: return err; } diff --git a/subsys/modbus/modbus_core.c b/subsys/modbus/modbus_core.c index 09ca8c44705..c427ed28e7e 100644 --- a/subsys/modbus/modbus_core.c +++ b/subsys/modbus/modbus_core.c @@ -80,18 +80,20 @@ static void modbus_rx_handler(struct k_work *item) if (ctx->client == true) { k_sem_give(&ctx->client_wait_sem); - return; - } - - if (IS_ENABLED(CONFIG_MODBUS_SERVER)) { + } else if (IS_ENABLED(CONFIG_MODBUS_SERVER)) { bool respond = modbus_server_handler(ctx); + if (respond) { + modbus_tx_adu(ctx); + } else { + LOG_DBG("Server has dropped frame"); + } + switch (ctx->mode) { case MODBUS_MODE_RTU: case MODBUS_MODE_ASCII: if (IS_ENABLED(CONFIG_MODBUS_SERIAL) && respond == false) { - LOG_DBG("Server has dropped frame"); modbus_serial_rx_enable(ctx); } break; @@ -116,6 +118,18 @@ void modbus_tx_adu(struct modbus_context *ctx) } } +int modbus_tx_wait_rx_adu(struct modbus_context *ctx) +{ + modbus_tx_adu(ctx); + + if (k_sem_take(&ctx->client_wait_sem, K_USEC(ctx->rxwait_to)) != 0) { + LOG_WRN("Client wait-for-RX timeout"); + return -EIO; + } + + return ctx->rx_adu_err; +} + struct modbus_context *modbus_get_context(const uint8_t iface) { struct modbus_context *ctx; diff --git a/subsys/modbus/modbus_internal.h b/subsys/modbus/modbus_internal.h index a5ba7adce05..acfb0218d16 100644 --- a/subsys/modbus/modbus_internal.h +++ b/subsys/modbus/modbus_internal.h @@ -159,6 +159,7 @@ struct modbus_context { struct modbus_context *modbus_get_context(const uint8_t iface); void modbus_tx_adu(struct modbus_context *ctx); +int modbus_tx_wait_rx_adu(struct modbus_context *ctx); bool modbus_server_handler(struct modbus_context *ctx); void modbus_reset_stats(struct modbus_context *ctx); diff --git a/subsys/modbus/modbus_server.c b/subsys/modbus/modbus_server.c index f2b6f3fa934..bd0addac356 100644 --- a/subsys/modbus/modbus_server.c +++ b/subsys/modbus/modbus_server.c @@ -926,13 +926,26 @@ static bool mbs_fc16_hregs_write(struct modbus_context *ctx) return true; } -static bool mbs_fc_handler(struct modbus_context *ctx) +bool modbus_server_handler(struct modbus_context *ctx) { bool send_reply = false; uint8_t addr = ctx->rx_adu.unit_id; uint8_t fc = ctx->rx_adu.fc; + LOG_DBG("Server RX handler %p", ctx); + update_msg_ctr(ctx); + + if (ctx->rx_adu_err != 0) { + update_noresp_ctr(ctx); + if (ctx->rx_adu_err == -EIO) { + update_crcerr_ctr(ctx); + } + + return false; + } + if (addr != 0 && addr != ctx->unit_id) { + update_noresp_ctr(ctx); return false; } @@ -988,31 +1001,12 @@ static bool mbs_fc_handler(struct modbus_context *ctx) if (addr == 0) { /* Broadcast address, do not reply */ - return false; - } else { - return send_reply; + send_reply = false; } -} - -bool modbus_server_handler(struct modbus_context *ctx) -{ - LOG_DBG("Server RX handler %p", ctx); - - update_msg_ctr(ctx); - - if (ctx->rx_adu_err == -EIO) { - update_noresp_ctr(ctx); - update_crcerr_ctr(ctx); - } else if (ctx->rx_adu_err != 0) { - update_noresp_ctr(ctx); - } else { - if (mbs_fc_handler(ctx) == true) { - modbus_tx_adu(ctx); - return true; - } + if (send_reply == false) { update_noresp_ctr(ctx); } - return false; + return send_reply; }