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 <johann.fischer@nordicsemi.no>

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);
+	err = modbus_tx_wait_rx_adu(ctx);
-
+	if (err != 0) {
-	if (k_sem_take(&ctx->client_wait_sem, K_USEC(ctx->rxwait_to)) != 0) {
+		return err;
-		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 = mbc_validate_response_fc(ctx, unit_id, fc);
 	if (err < 0) {
-		LOG_ERR("Failed to validate address or function code");
+		LOG_ERR("Failed to validate unit ID or function code");
-		goto exit_error;
+		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;
 }
 

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;
+	} else if (IS_ENABLED(CONFIG_MODBUS_SERVER)) {
-	}
-
-	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;

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);

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;
+		send_reply = false;
-	} else {
-		return send_reply;
 	}
-}
-
-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;
 }