subsys/modbus/modbus_client.c - third_party/github/zephyrproject-rtos/zephyr - Git at Google

 /*
  * Copyright (c) 2020 PHYTEC Messtechnik GmbH
  *
  * SPDX-License-Identifier: Apache-2.0
  */

 /*
  * This file is based on mbm_core.c from uC/Modbus Stack.
  *
  *                                uC/Modbus
  *                         The Embedded Modbus Stack
  *
  *      Copyright 2003-2020 Silicon Laboratories Inc. www.silabs.com
  *
  *                   SPDX-License-Identifier: APACHE-2.0
  *
  * This software is subject to an open source license and is distributed by
  *  Silicon Laboratories Inc. pursuant to the terms of the Apache License,
  *      Version 2.0 available at www.apache.org/licenses/LICENSE-2.0.
  */

 #include <string.h>
 #include <zephyr/sys/byteorder.h>
 #include <modbus_internal.h>

 #include <zephyr/logging/log.h>
 LOG_MODULE_REGISTER(modbus_c, CONFIG_MODBUS_LOG_LEVEL);

 static int mbc_validate_response_fc(struct modbus_context *ctx,
 				    const uint8_t unit_id,
 				    uint8_t fc)
 {
 	uint8_t resp_fc = ctx->rx_adu.fc;
 	uint8_t excep_code = ctx->rx_adu.data[0];
 	const uint8_t excep_bit = BIT(7);
 	const uint8_t excep_mask = BIT_MASK(7);

 	if (unit_id != ctx->rx_adu.unit_id) {
 		return -EIO;
 	}

 	if (fc != (resp_fc & excep_mask)) {
 		return -EIO;
 	}

 	if (resp_fc & excep_bit) {
 		if (excep_code > MODBUS_EXC_NONE) {
 			return excep_code;
 		}

 		return -EIO;
 	}

 	return 0;
 }

 static int mbc_validate_fc03fp_response(struct modbus_context *ctx, float *ptbl)
 {
 	size_t resp_byte_cnt;
 	size_t req_byte_cnt;
 	uint16_t req_qty;
 	uint8_t *resp_data;

 	resp_byte_cnt = ctx->rx_adu.data[0];
 	resp_data = &ctx->rx_adu.data[1];
 	req_qty = sys_get_be16(&ctx->tx_adu.data[2]);
 	req_byte_cnt = req_qty * sizeof(uint16_t);

 	if (req_byte_cnt != resp_byte_cnt) {
 		LOG_ERR("Mismatch in the number of registers");
 		return -EINVAL;
 	}

 	for (uint16_t i = 0; i < req_qty / 2; i++) {
 		uint32_t reg_val = sys_get_be32(resp_data);

 		memcpy(&ptbl[i], &reg_val, sizeof(float));
 		resp_data += sizeof(uint32_t);
 	}

 	return 0;
 }

 static int mbc_validate_rd_response(struct modbus_context *ctx,
 				    const uint8_t unit_id,
 				    uint8_t fc,
 				    uint8_t *data)
 {
 	int err;
 	size_t resp_byte_cnt;
 	size_t req_byte_cnt;
 	uint16_t req_qty;
 	uint16_t req_addr;
 	uint8_t *resp_data;
 	uint16_t *data_p16 = (uint16_t *)data;

 	if (data == NULL) {
 		return -EINVAL;
 	}

 	resp_byte_cnt = ctx->rx_adu.data[0];
 	resp_data = &ctx->rx_adu.data[1];
 	req_qty = sys_get_be16(&ctx->tx_adu.data[2]);
 	req_addr = sys_get_be16(&ctx->tx_adu.data[0]);

 	if ((resp_byte_cnt + 1) > sizeof(ctx->rx_adu.data)) {
 		LOG_ERR("Byte count exceeds buffer length");
 		return -EINVAL;
 	}

 	switch (fc) {
 	case MODBUS_FC01_COIL_RD:
 	case MODBUS_FC02_DI_RD:
 		req_byte_cnt = ((req_qty - 1) / 8) + 1;
 		if (req_byte_cnt != resp_byte_cnt) {
 			LOG_ERR("Mismatch in the number of coils or inputs");
 			err = -EINVAL;
 		} else {
 			for (uint8_t i = 0; i < resp_byte_cnt; i++) {
 				data[i] = resp_data[i];
 			}

 			err = 0;
 		}
 		break;

 	case MODBUS_FC03_HOLDING_REG_RD:
 		if (IS_ENABLED(CONFIG_MODBUS_FP_EXTENSIONS) &&
 		    (req_addr >= MODBUS_FP_EXTENSIONS_ADDR)) {
 			err = mbc_validate_fc03fp_response(ctx, (float *)data);
 			break;
 		}
 		__fallthrough;
 	case MODBUS_FC04_IN_REG_RD:
 		req_byte_cnt = req_qty * sizeof(uint16_t);
 		if (req_byte_cnt != resp_byte_cnt) {
 			LOG_ERR("Mismatch in the number of registers");
 			err = -EINVAL;
 		} else {
 			for (uint16_t i = 0; i < req_qty; i++) {
 				data_p16[i] = sys_get_be16(resp_data);
 				resp_data += sizeof(uint16_t);
 			}

 			err = 0;
 		}
 		break;

 	default:
 		LOG_ERR("Validation not implemented for FC 0x%02x", fc);
 		err = -ENOTSUP;
 	}

 	return err;
 }

 static int mbc_validate_fc08_response(struct modbus_context *ctx,
 				      const uint8_t unit_id,
 				      uint16_t *data)
 {
 	int err;
 	uint16_t resp_sfunc;
 	uint16_t resp_data;
 	uint16_t req_sfunc;
 	uint16_t req_data;

 	if (data == NULL) {
 		return -EINVAL;
 	}

 	req_sfunc = sys_get_be16(&ctx->tx_adu.data[0]);
 	req_data = sys_get_be16(&ctx->tx_adu.data[2]);
 	resp_sfunc = sys_get_be16(&ctx->rx_adu.data[0]);
 	resp_data = sys_get_be16(&ctx->rx_adu.data[2]);

 	if (req_sfunc != resp_sfunc) {
 		LOG_ERR("Mismatch in the sub-function code");
 		return -EINVAL;
 	}

 	switch (resp_sfunc) {
 	case MODBUS_FC08_SUBF_QUERY:
 	case MODBUS_FC08_SUBF_CLR_CTR:
 		if (req_data != resp_data) {
 			LOG_ERR("Request and response data are different");
 			err = -EINVAL;
 		} else {
 			*data = resp_data;
 			err = 0;
 		}
 		break;

 	case MODBUS_FC08_SUBF_BUS_MSG_CTR:
 	case MODBUS_FC08_SUBF_BUS_CRC_CTR:
 	case MODBUS_FC08_SUBF_BUS_EXCEPT_CTR:
 	case MODBUS_FC08_SUBF_SERVER_MSG_CTR:
 	case MODBUS_FC08_SUBF_SERVER_NO_RESP_CTR:
 		*data = resp_data;
 		err = 0;
 		break;

 	default:
 		err = -EINVAL;
 	}

 	return err;
 }

 static int mbc_validate_wr_response(struct modbus_context *ctx,
 				    const uint8_t unit_id,
 				    uint8_t fc)
 {
 	int err;
 	uint16_t req_addr;
 	uint16_t req_value;
 	uint16_t resp_addr;
 	uint16_t resp_value;

 	req_addr = sys_get_be16(&ctx->tx_adu.data[0]);
 	req_value = sys_get_be16(&ctx->tx_adu.data[2]);
 	resp_addr = sys_get_be16(&ctx->rx_adu.data[0]);
 	resp_value = sys_get_be16(&ctx->rx_adu.data[2]);

 	switch (fc) {
 	case MODBUS_FC05_COIL_WR:
 	case MODBUS_FC06_HOLDING_REG_WR:
 	case MODBUS_FC15_COILS_WR:
 	case MODBUS_FC16_HOLDING_REGS_WR:
 		if (req_addr != resp_addr || req_value != resp_value) {
 			err = ENXIO;
 		} else {
 			err = 0;
 		}
 		break;

 	default:
 		LOG_ERR("Validation not implemented for FC 0x%02x", fc);
 		err = -ENOTSUP;
 	}

 	return err;
 }

 static int mbc_send_cmd(struct modbus_context *ctx, const uint8_t unit_id,
 			uint8_t fc, void *data)
 {
 	int err;

 	ctx->tx_adu.unit_id = unit_id;
 	ctx->tx_adu.fc = fc;

 	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 unit ID or function code");
 		return err;
 	} else if (err > 0) {
 		LOG_INF("Modbus FC %u, error code %u", fc, err);
 		return err;
 	}

 	switch (fc) {
 	case MODBUS_FC01_COIL_RD:
 	case MODBUS_FC02_DI_RD:
 	case MODBUS_FC03_HOLDING_REG_RD:
 	case MODBUS_FC04_IN_REG_RD:
 		err = mbc_validate_rd_response(ctx, unit_id, fc, data);
 		break;

 	case MODBUS_FC08_DIAGNOSTICS:
 		err = mbc_validate_fc08_response(ctx, unit_id, data);
 		break;

 	case MODBUS_FC05_COIL_WR:
 	case MODBUS_FC06_HOLDING_REG_WR:
 	case MODBUS_FC15_COILS_WR:
 	case MODBUS_FC16_HOLDING_REGS_WR:
 		err = mbc_validate_wr_response(ctx, unit_id, fc);
 		break;

 	default:
 		LOG_ERR("FC 0x%02x not implemented", fc);
 		err = -ENOTSUP;
 	}

 	return err;
 }

 int modbus_read_coils(const int iface,
 		      const uint8_t unit_id,
 		      const uint16_t start_addr,
 		      uint8_t *const coil_tbl,
 		      const uint16_t num_coils)
 {
 	struct modbus_context *ctx = modbus_get_context(iface);
 	int err;

 	if (ctx == NULL) {
 		return -ENODEV;
 	}

 	k_mutex_lock(&ctx->iface_lock, K_FOREVER);

 	ctx->tx_adu.length = 4;
 	sys_put_be16(start_addr, &ctx->tx_adu.data[0]);
 	sys_put_be16(num_coils, &ctx->tx_adu.data[2]);

 	err = mbc_send_cmd(ctx, unit_id, MODBUS_FC01_COIL_RD, coil_tbl);
 	k_mutex_unlock(&ctx->iface_lock);

 	return err;
 }

 int modbus_read_dinputs(const int iface,
 			const uint8_t unit_id,
 			const uint16_t start_addr,
 			uint8_t *const di_tbl,
 			const uint16_t num_di)
 {
 	struct modbus_context *ctx = modbus_get_context(iface);
 	int err;

 	if (ctx == NULL) {
 		return -ENODEV;
 	}

 	k_mutex_lock(&ctx->iface_lock, K_FOREVER);

 	ctx->tx_adu.length = 4;
 	sys_put_be16(start_addr, &ctx->tx_adu.data[0]);
 	sys_put_be16(num_di, &ctx->tx_adu.data[2]);

 	err = mbc_send_cmd(ctx, unit_id, MODBUS_FC02_DI_RD, di_tbl);
 	k_mutex_unlock(&ctx->iface_lock);

 	return err;
 }

 int modbus_read_holding_regs(const int iface,
 			     const uint8_t unit_id,
 			     const uint16_t start_addr,
 			     uint16_t *const reg_buf,
 			     const uint16_t num_regs)
 {
 	struct modbus_context *ctx = modbus_get_context(iface);
 	int err;

 	if (ctx == NULL) {
 		return -ENODEV;
 	}

 	k_mutex_lock(&ctx->iface_lock, K_FOREVER);

 	ctx->tx_adu.length = 4;
 	sys_put_be16(start_addr, &ctx->tx_adu.data[0]);
 	sys_put_be16(num_regs, &ctx->tx_adu.data[2]);

 	err = mbc_send_cmd(ctx, unit_id, MODBUS_FC03_HOLDING_REG_RD, reg_buf);
 	k_mutex_unlock(&ctx->iface_lock);

 	return err;
 }


 #ifdef CONFIG_MODBUS_FP_EXTENSIONS
 int modbus_read_holding_regs_fp(const int iface,
 			       const uint8_t unit_id,
 			       const uint16_t start_addr,
 			       float *const reg_buf,
 			       const uint16_t num_regs)
 {
 	struct modbus_context *ctx = modbus_get_context(iface);
 	int err;

 	if (ctx == NULL) {
 		return -ENODEV;
 	}

 	k_mutex_lock(&ctx->iface_lock, K_FOREVER);

 	ctx->tx_adu.length = 4;
 	sys_put_be16(start_addr, &ctx->tx_adu.data[0]);
 	/* A 32-bit float is mapped to two 16-bit registers */
 	sys_put_be16(num_regs * 2, &ctx->tx_adu.data[2]);

 	err = mbc_send_cmd(ctx, unit_id, MODBUS_FC03_HOLDING_REG_RD, reg_buf);
 	k_mutex_unlock(&ctx->iface_lock);

 	return err;
 }
 #endif

 int modbus_read_input_regs(const int iface,
 			   const uint8_t unit_id,
 			   const uint16_t start_addr,
 			   uint16_t *const reg_buf,
 			   const uint16_t num_regs)
 {
 	struct modbus_context *ctx = modbus_get_context(iface);
 	int err;

 	if (ctx == NULL) {
 		return -ENODEV;
 	}

 	k_mutex_lock(&ctx->iface_lock, K_FOREVER);

 	ctx->tx_adu.length = 4;
 	sys_put_be16(start_addr, &ctx->tx_adu.data[0]);
 	sys_put_be16(num_regs, &ctx->tx_adu.data[2]);

 	err = mbc_send_cmd(ctx, unit_id, MODBUS_FC04_IN_REG_RD, reg_buf);
 	k_mutex_unlock(&ctx->iface_lock);

 	return err;
 }

 int modbus_write_coil(const int iface,
 		      const uint8_t unit_id,
 		      const uint16_t coil_addr,
 		      const bool coil_state)
 {
 	struct modbus_context *ctx = modbus_get_context(iface);
 	int err;
 	uint16_t coil_val;

 	if (ctx == NULL) {
 		return -ENODEV;
 	}

 	k_mutex_lock(&ctx->iface_lock, K_FOREVER);

 	if (coil_state == false) {
 		coil_val = MODBUS_COIL_OFF_CODE;
 	} else {
 		coil_val = MODBUS_COIL_ON_CODE;
 	}

 	ctx->tx_adu.length = 4;
 	sys_put_be16(coil_addr, &ctx->tx_adu.data[0]);
 	sys_put_be16(coil_val, &ctx->tx_adu.data[2]);

 	err = mbc_send_cmd(ctx, unit_id, MODBUS_FC05_COIL_WR, NULL);
 	k_mutex_unlock(&ctx->iface_lock);

 	return err;
 }

 int modbus_write_holding_reg(const int iface,
 			     const uint8_t unit_id,
 			     const uint16_t start_addr,
 			     const uint16_t reg_val)
 {
 	struct modbus_context *ctx = modbus_get_context(iface);
 	int err;

 	if (ctx == NULL) {
 		return -ENODEV;
 	}

 	k_mutex_lock(&ctx->iface_lock, K_FOREVER);

 	ctx->tx_adu.length = 4;
 	sys_put_be16(start_addr, &ctx->tx_adu.data[0]);
 	sys_put_be16(reg_val, &ctx->tx_adu.data[2]);

 	err = mbc_send_cmd(ctx, unit_id, MODBUS_FC06_HOLDING_REG_WR, NULL);
 	k_mutex_unlock(&ctx->iface_lock);

 	return err;
 }

 int modbus_request_diagnostic(const int iface,
 			      const uint8_t unit_id,
 			      const uint16_t sfunc,
 			      const uint16_t data,
 			      uint16_t *const data_out)
 {
 	struct modbus_context *ctx = modbus_get_context(iface);
 	int err;

 	if (ctx == NULL) {
 		return -ENODEV;
 	}

 	k_mutex_lock(&ctx->iface_lock, K_FOREVER);

 	ctx->tx_adu.length = 4;
 	sys_put_be16(sfunc, &ctx->tx_adu.data[0]);
 	sys_put_be16(data, &ctx->tx_adu.data[2]);

 	err = mbc_send_cmd(ctx, unit_id, MODBUS_FC08_DIAGNOSTICS, data_out);
 	k_mutex_unlock(&ctx->iface_lock);

 	return err;
 }

 int modbus_write_coils(const int iface,
 		       const uint8_t unit_id,
 		       const uint16_t start_addr,
 		       uint8_t *const coil_tbl,
 		       const uint16_t num_coils)
 {
 	struct modbus_context *ctx = modbus_get_context(iface);
 	size_t length = 0;
 	uint8_t *data_ptr;
 	size_t num_bytes;
 	int err;

 	if (ctx == NULL) {
 		return -ENODEV;
 	}

 	k_mutex_lock(&ctx->iface_lock, K_FOREVER);

 	sys_put_be16(start_addr, &ctx->tx_adu.data[0]);
 	length += sizeof(start_addr);
 	sys_put_be16(num_coils, &ctx->tx_adu.data[2]);
 	length += sizeof(num_coils);

 	num_bytes = (uint8_t)(((num_coils - 1) / 8) + 1);
 	ctx->tx_adu.data[4] = num_bytes;
 	length += num_bytes + 1;

 	if (length > sizeof(ctx->tx_adu.data)) {
 		LOG_ERR("Length of data buffer is not sufficient");
 		k_mutex_unlock(&ctx->iface_lock);
 		return -ENOBUFS;
 	}

 	ctx->tx_adu.length = length;
 	data_ptr = &ctx->tx_adu.data[5];

 	memcpy(data_ptr, coil_tbl, num_bytes);

 	err = mbc_send_cmd(ctx, unit_id, MODBUS_FC15_COILS_WR, NULL);
 	k_mutex_unlock(&ctx->iface_lock);

 	return err;
 }

 int modbus_write_holding_regs(const int iface,
 			      const uint8_t unit_id,
 			      const uint16_t start_addr,
 			      uint16_t *const reg_buf,
 			      const uint16_t num_regs)
 {
 	struct modbus_context *ctx = modbus_get_context(iface);
 	size_t length = 0;
 	uint8_t *data_ptr;
 	size_t num_bytes;
 	int err;

 	if (ctx == NULL) {
 		return -ENODEV;
 	}

 	k_mutex_lock(&ctx->iface_lock, K_FOREVER);

 	sys_put_be16(start_addr, &ctx->tx_adu.data[0]);
 	length += sizeof(start_addr);
 	sys_put_be16(num_regs, &ctx->tx_adu.data[2]);
 	length += sizeof(num_regs);

 	num_bytes = num_regs * sizeof(uint16_t);
 	ctx->tx_adu.data[4] = num_bytes;
 	length += num_bytes + 1;

 	if (length > sizeof(ctx->tx_adu.data)) {
 		LOG_ERR("Length of data buffer is not sufficient");
 		k_mutex_unlock(&ctx->iface_lock);
 		return -ENOBUFS;
 	}

 	ctx->tx_adu.length = length;
 	data_ptr = &ctx->tx_adu.data[5];

 	for (uint16_t i = 0; i < num_regs; i++) {
 		sys_put_be16(reg_buf[i], data_ptr);
 		data_ptr += sizeof(uint16_t);
 	}

 	err = mbc_send_cmd(ctx, unit_id, MODBUS_FC16_HOLDING_REGS_WR, NULL);
 	k_mutex_unlock(&ctx->iface_lock);

 	return err;
 }

 #ifdef CONFIG_MODBUS_FP_EXTENSIONS
 int modbus_write_holding_regs_fp(const int iface,
 				 const uint8_t unit_id,
 				 const uint16_t start_addr,
 				 float *const reg_buf,
 				 const uint16_t num_regs)
 {
 	struct modbus_context *ctx = modbus_get_context(iface);
 	size_t length = 0;
 	uint8_t *data_ptr;
 	size_t num_bytes;
 	int err;

 	if (ctx == NULL) {
 		return -ENODEV;
 	}

 	k_mutex_lock(&ctx->iface_lock, K_FOREVER);

 	sys_put_be16(start_addr, &ctx->tx_adu.data[0]);
 	length += sizeof(start_addr);
 	/* A 32-bit float is mapped to two 16-bit registers */
 	sys_put_be16(num_regs * 2, &ctx->tx_adu.data[2]);
 	length += sizeof(num_regs);

 	num_bytes = num_regs * sizeof(float);
 	ctx->tx_adu.data[4] = num_bytes;
 	length += num_bytes + 1;

 	if (length > sizeof(ctx->tx_adu.data)) {
 		LOG_ERR("Length of data buffer is not sufficient");
 		k_mutex_unlock(&ctx->iface_lock);
 		return -ENOBUFS;
 	}

 	ctx->tx_adu.length = length;
 	data_ptr = &ctx->tx_adu.data[5];

 	for (uint16_t i = 0; i < num_regs; i++) {
 		uint32_t reg_val;

 		memcpy(&reg_val, &reg_buf[i], sizeof(reg_val));
 		sys_put_be32(reg_val, data_ptr);
 		data_ptr += sizeof(uint32_t);
 	}

 	err = mbc_send_cmd(ctx, unit_id, MODBUS_FC16_HOLDING_REGS_WR, NULL);
 	k_mutex_unlock(&ctx->iface_lock);

 	return err;
 }
 #endif
	/*
	* Copyright (c) 2020 PHYTEC Messtechnik GmbH
	*
	* SPDX-License-Identifier: Apache-2.0
	*/

	/*
	* This file is based on mbm_core.c from uC/Modbus Stack.
	*
	* uC/Modbus
	* The Embedded Modbus Stack
	*
	* Copyright 2003-2020 Silicon Laboratories Inc. www.silabs.com
	*
	* SPDX-License-Identifier: APACHE-2.0
	*
	* This software is subject to an open source license and is distributed by
	* Silicon Laboratories Inc. pursuant to the terms of the Apache License,
	* Version 2.0 available at www.apache.org/licenses/LICENSE-2.0.
	*/

	#include <string.h>
	#include <zephyr/sys/byteorder.h>
	#include <modbus_internal.h>

	#include <zephyr/logging/log.h>
	LOG_MODULE_REGISTER(modbus_c, CONFIG_MODBUS_LOG_LEVEL);

	static int mbc_validate_response_fc(struct modbus_context *ctx,
	const uint8_t unit_id,
	uint8_t fc)
	{
	uint8_t resp_fc = ctx->rx_adu.fc;
	uint8_t excep_code = ctx->rx_adu.data[0];
	const uint8_t excep_bit = BIT(7);
	const uint8_t excep_mask = BIT_MASK(7);

	if (unit_id != ctx->rx_adu.unit_id) {
	return -EIO;
	}

	if (fc != (resp_fc & excep_mask)) {
	return -EIO;
	}

	if (resp_fc & excep_bit) {
	if (excep_code > MODBUS_EXC_NONE) {
	return excep_code;
	}

	return -EIO;
	}

	return 0;
	}

	static int mbc_validate_fc03fp_response(struct modbus_context ctx, float ptbl)
	{
	size_t resp_byte_cnt;
	size_t req_byte_cnt;
	uint16_t req_qty;
	uint8_t *resp_data;

	resp_byte_cnt = ctx->rx_adu.data[0];
	resp_data = &ctx->rx_adu.data[1];
	req_qty = sys_get_be16(&ctx->tx_adu.data[2]);
	req_byte_cnt = req_qty * sizeof(uint16_t);

	if (req_byte_cnt != resp_byte_cnt) {
	LOG_ERR("Mismatch in the number of registers");
	return -EINVAL;
	}

	for (uint16_t i = 0; i < req_qty / 2; i++) {
	uint32_t reg_val = sys_get_be32(resp_data);

	memcpy(&ptbl[i], &reg_val, sizeof(float));
	resp_data += sizeof(uint32_t);
	}

	return 0;
	}

	static int mbc_validate_rd_response(struct modbus_context *ctx,
	const uint8_t unit_id,
	uint8_t fc,
	uint8_t *data)
	{
	int err;
	size_t resp_byte_cnt;
	size_t req_byte_cnt;
	uint16_t req_qty;
	uint16_t req_addr;
	uint8_t *resp_data;
	uint16_t data_p16 = (uint16_t )data;

	if (data == NULL) {
	return -EINVAL;
	}

	resp_byte_cnt = ctx->rx_adu.data[0];
	resp_data = &ctx->rx_adu.data[1];
	req_qty = sys_get_be16(&ctx->tx_adu.data[2]);
	req_addr = sys_get_be16(&ctx->tx_adu.data[0]);

	if ((resp_byte_cnt + 1) > sizeof(ctx->rx_adu.data)) {
	LOG_ERR("Byte count exceeds buffer length");
	return -EINVAL;
	}

	switch (fc) {
	case MODBUS_FC01_COIL_RD:
	case MODBUS_FC02_DI_RD:
	req_byte_cnt = ((req_qty - 1) / 8) + 1;
	if (req_byte_cnt != resp_byte_cnt) {
	LOG_ERR("Mismatch in the number of coils or inputs");
	err = -EINVAL;
	} else {
	for (uint8_t i = 0; i < resp_byte_cnt; i++) {
	data[i] = resp_data[i];
	}

	err = 0;
	}
	break;

	case MODBUS_FC03_HOLDING_REG_RD:
	if (IS_ENABLED(CONFIG_MODBUS_FP_EXTENSIONS) &&
	(req_addr >= MODBUS_FP_EXTENSIONS_ADDR)) {
	err = mbc_validate_fc03fp_response(ctx, (float *)data);
	break;
	}
	__fallthrough;
	case MODBUS_FC04_IN_REG_RD:
	req_byte_cnt = req_qty * sizeof(uint16_t);
	if (req_byte_cnt != resp_byte_cnt) {
	LOG_ERR("Mismatch in the number of registers");
	err = -EINVAL;
	} else {
	for (uint16_t i = 0; i < req_qty; i++) {
	data_p16[i] = sys_get_be16(resp_data);
	resp_data += sizeof(uint16_t);
	}

	err = 0;
	}
	break;

	default:
	LOG_ERR("Validation not implemented for FC 0x%02x", fc);
	err = -ENOTSUP;
	}

	return err;
	}

	static int mbc_validate_fc08_response(struct modbus_context *ctx,
	const uint8_t unit_id,
	uint16_t *data)
	{
	int err;
	uint16_t resp_sfunc;
	uint16_t resp_data;
	uint16_t req_sfunc;
	uint16_t req_data;

	if (data == NULL) {
	return -EINVAL;
	}

	req_sfunc = sys_get_be16(&ctx->tx_adu.data[0]);
	req_data = sys_get_be16(&ctx->tx_adu.data[2]);
	resp_sfunc = sys_get_be16(&ctx->rx_adu.data[0]);
	resp_data = sys_get_be16(&ctx->rx_adu.data[2]);

	if (req_sfunc != resp_sfunc) {
	LOG_ERR("Mismatch in the sub-function code");
	return -EINVAL;
	}

	switch (resp_sfunc) {
	case MODBUS_FC08_SUBF_QUERY:
	case MODBUS_FC08_SUBF_CLR_CTR:
	if (req_data != resp_data) {
	LOG_ERR("Request and response data are different");
	err = -EINVAL;
	} else {
	*data = resp_data;
	err = 0;
	}
	break;

	case MODBUS_FC08_SUBF_BUS_MSG_CTR:
	case MODBUS_FC08_SUBF_BUS_CRC_CTR:
	case MODBUS_FC08_SUBF_BUS_EXCEPT_CTR:
	case MODBUS_FC08_SUBF_SERVER_MSG_CTR:
	case MODBUS_FC08_SUBF_SERVER_NO_RESP_CTR:
	*data = resp_data;
	err = 0;
	break;

	default:
	err = -EINVAL;
	}

	return err;
	}

	static int mbc_validate_wr_response(struct modbus_context *ctx,
	const uint8_t unit_id,
	uint8_t fc)
	{
	int err;
	uint16_t req_addr;
	uint16_t req_value;
	uint16_t resp_addr;
	uint16_t resp_value;

	req_addr = sys_get_be16(&ctx->tx_adu.data[0]);
	req_value = sys_get_be16(&ctx->tx_adu.data[2]);
	resp_addr = sys_get_be16(&ctx->rx_adu.data[0]);
	resp_value = sys_get_be16(&ctx->rx_adu.data[2]);

	switch (fc) {
	case MODBUS_FC05_COIL_WR:
	case MODBUS_FC06_HOLDING_REG_WR:
	case MODBUS_FC15_COILS_WR:
	case MODBUS_FC16_HOLDING_REGS_WR:
	if (req_addr != resp_addr \|\| req_value != resp_value) {
	err = ENXIO;
	} else {
	err = 0;
	}
	break;

	default:
	LOG_ERR("Validation not implemented for FC 0x%02x", fc);
	err = -ENOTSUP;
	}

	return err;
	}

	static int mbc_send_cmd(struct modbus_context *ctx, const uint8_t unit_id,
	uint8_t fc, void *data)
	{
	int err;

	ctx->tx_adu.unit_id = unit_id;
	ctx->tx_adu.fc = fc;

	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 unit ID or function code");
	return err;
	} else if (err > 0) {
	LOG_INF("Modbus FC %u, error code %u", fc, err);
	return err;
	}

	switch (fc) {
	case MODBUS_FC01_COIL_RD:
	case MODBUS_FC02_DI_RD:
	case MODBUS_FC03_HOLDING_REG_RD:
	case MODBUS_FC04_IN_REG_RD:
	err = mbc_validate_rd_response(ctx, unit_id, fc, data);
	break;

	case MODBUS_FC08_DIAGNOSTICS:
	err = mbc_validate_fc08_response(ctx, unit_id, data);
	break;

	case MODBUS_FC05_COIL_WR:
	case MODBUS_FC06_HOLDING_REG_WR:
	case MODBUS_FC15_COILS_WR:
	case MODBUS_FC16_HOLDING_REGS_WR:
	err = mbc_validate_wr_response(ctx, unit_id, fc);
	break;

	default:
	LOG_ERR("FC 0x%02x not implemented", fc);
	err = -ENOTSUP;
	}

	return err;
	}

	int modbus_read_coils(const int iface,
	const uint8_t unit_id,
	const uint16_t start_addr,
	uint8_t *const coil_tbl,
	const uint16_t num_coils)
	{
	struct modbus_context *ctx = modbus_get_context(iface);
	int err;

	if (ctx == NULL) {
	return -ENODEV;
	}

	k_mutex_lock(&ctx->iface_lock, K_FOREVER);

	ctx->tx_adu.length = 4;
	sys_put_be16(start_addr, &ctx->tx_adu.data[0]);
	sys_put_be16(num_coils, &ctx->tx_adu.data[2]);

	err = mbc_send_cmd(ctx, unit_id, MODBUS_FC01_COIL_RD, coil_tbl);
	k_mutex_unlock(&ctx->iface_lock);

	return err;
	}

	int modbus_read_dinputs(const int iface,
	const uint8_t unit_id,
	const uint16_t start_addr,
	uint8_t *const di_tbl,
	const uint16_t num_di)
	{
	struct modbus_context *ctx = modbus_get_context(iface);
	int err;

	if (ctx == NULL) {
	return -ENODEV;
	}

	k_mutex_lock(&ctx->iface_lock, K_FOREVER);

	ctx->tx_adu.length = 4;
	sys_put_be16(start_addr, &ctx->tx_adu.data[0]);
	sys_put_be16(num_di, &ctx->tx_adu.data[2]);

	err = mbc_send_cmd(ctx, unit_id, MODBUS_FC02_DI_RD, di_tbl);
	k_mutex_unlock(&ctx->iface_lock);

	return err;
	}

	int modbus_read_holding_regs(const int iface,
	const uint8_t unit_id,
	const uint16_t start_addr,
	uint16_t *const reg_buf,
	const uint16_t num_regs)
	{
	struct modbus_context *ctx = modbus_get_context(iface);
	int err;

	if (ctx == NULL) {
	return -ENODEV;
	}

	k_mutex_lock(&ctx->iface_lock, K_FOREVER);

	ctx->tx_adu.length = 4;
	sys_put_be16(start_addr, &ctx->tx_adu.data[0]);
	sys_put_be16(num_regs, &ctx->tx_adu.data[2]);

	err = mbc_send_cmd(ctx, unit_id, MODBUS_FC03_HOLDING_REG_RD, reg_buf);
	k_mutex_unlock(&ctx->iface_lock);

	return err;
	}


	#ifdef CONFIG_MODBUS_FP_EXTENSIONS
	int modbus_read_holding_regs_fp(const int iface,
	const uint8_t unit_id,
	const uint16_t start_addr,
	float *const reg_buf,
	const uint16_t num_regs)
	{
	struct modbus_context *ctx = modbus_get_context(iface);
	int err;

	if (ctx == NULL) {
	return -ENODEV;
	}

	k_mutex_lock(&ctx->iface_lock, K_FOREVER);

	ctx->tx_adu.length = 4;
	sys_put_be16(start_addr, &ctx->tx_adu.data[0]);
	/* A 32-bit float is mapped to two 16-bit registers */
	sys_put_be16(num_regs * 2, &ctx->tx_adu.data[2]);

	err = mbc_send_cmd(ctx, unit_id, MODBUS_FC03_HOLDING_REG_RD, reg_buf);
	k_mutex_unlock(&ctx->iface_lock);

	return err;
	}
	#endif

	int modbus_read_input_regs(const int iface,
	const uint8_t unit_id,
	const uint16_t start_addr,
	uint16_t *const reg_buf,
	const uint16_t num_regs)
	{
	struct modbus_context *ctx = modbus_get_context(iface);
	int err;

	if (ctx == NULL) {
	return -ENODEV;
	}

	k_mutex_lock(&ctx->iface_lock, K_FOREVER);

	ctx->tx_adu.length = 4;
	sys_put_be16(start_addr, &ctx->tx_adu.data[0]);
	sys_put_be16(num_regs, &ctx->tx_adu.data[2]);

	err = mbc_send_cmd(ctx, unit_id, MODBUS_FC04_IN_REG_RD, reg_buf);
	k_mutex_unlock(&ctx->iface_lock);

	return err;
	}

	int modbus_write_coil(const int iface,
	const uint8_t unit_id,
	const uint16_t coil_addr,
	const bool coil_state)
	{
	struct modbus_context *ctx = modbus_get_context(iface);
	int err;
	uint16_t coil_val;

	if (ctx == NULL) {
	return -ENODEV;
	}

	k_mutex_lock(&ctx->iface_lock, K_FOREVER);

	if (coil_state == false) {
	coil_val = MODBUS_COIL_OFF_CODE;
	} else {
	coil_val = MODBUS_COIL_ON_CODE;
	}

	ctx->tx_adu.length = 4;
	sys_put_be16(coil_addr, &ctx->tx_adu.data[0]);
	sys_put_be16(coil_val, &ctx->tx_adu.data[2]);

	err = mbc_send_cmd(ctx, unit_id, MODBUS_FC05_COIL_WR, NULL);
	k_mutex_unlock(&ctx->iface_lock);

	return err;
	}

	int modbus_write_holding_reg(const int iface,
	const uint8_t unit_id,
	const uint16_t start_addr,
	const uint16_t reg_val)
	{
	struct modbus_context *ctx = modbus_get_context(iface);
	int err;

	if (ctx == NULL) {
	return -ENODEV;
	}

	k_mutex_lock(&ctx->iface_lock, K_FOREVER);

	ctx->tx_adu.length = 4;
	sys_put_be16(start_addr, &ctx->tx_adu.data[0]);
	sys_put_be16(reg_val, &ctx->tx_adu.data[2]);

	err = mbc_send_cmd(ctx, unit_id, MODBUS_FC06_HOLDING_REG_WR, NULL);
	k_mutex_unlock(&ctx->iface_lock);

	return err;
	}

	int modbus_request_diagnostic(const int iface,
	const uint8_t unit_id,
	const uint16_t sfunc,
	const uint16_t data,
	uint16_t *const data_out)
	{
	struct modbus_context *ctx = modbus_get_context(iface);
	int err;

	if (ctx == NULL) {
	return -ENODEV;
	}

	k_mutex_lock(&ctx->iface_lock, K_FOREVER);

	ctx->tx_adu.length = 4;
	sys_put_be16(sfunc, &ctx->tx_adu.data[0]);
	sys_put_be16(data, &ctx->tx_adu.data[2]);

	err = mbc_send_cmd(ctx, unit_id, MODBUS_FC08_DIAGNOSTICS, data_out);
	k_mutex_unlock(&ctx->iface_lock);

	return err;
	}

	int modbus_write_coils(const int iface,
	const uint8_t unit_id,
	const uint16_t start_addr,
	uint8_t *const coil_tbl,
	const uint16_t num_coils)
	{
	struct modbus_context *ctx = modbus_get_context(iface);
	size_t length = 0;
	uint8_t *data_ptr;
	size_t num_bytes;
	int err;

	if (ctx == NULL) {
	return -ENODEV;
	}

	k_mutex_lock(&ctx->iface_lock, K_FOREVER);

	sys_put_be16(start_addr, &ctx->tx_adu.data[0]);
	length += sizeof(start_addr);
	sys_put_be16(num_coils, &ctx->tx_adu.data[2]);
	length += sizeof(num_coils);

	num_bytes = (uint8_t)(((num_coils - 1) / 8) + 1);
	ctx->tx_adu.data[4] = num_bytes;
	length += num_bytes + 1;

	if (length > sizeof(ctx->tx_adu.data)) {
	LOG_ERR("Length of data buffer is not sufficient");
	k_mutex_unlock(&ctx->iface_lock);
	return -ENOBUFS;
	}

	ctx->tx_adu.length = length;
	data_ptr = &ctx->tx_adu.data[5];

	memcpy(data_ptr, coil_tbl, num_bytes);

	err = mbc_send_cmd(ctx, unit_id, MODBUS_FC15_COILS_WR, NULL);
	k_mutex_unlock(&ctx->iface_lock);

	return err;
	}

	int modbus_write_holding_regs(const int iface,
	const uint8_t unit_id,
	const uint16_t start_addr,
	uint16_t *const reg_buf,
	const uint16_t num_regs)
	{
	struct modbus_context *ctx = modbus_get_context(iface);
	size_t length = 0;
	uint8_t *data_ptr;
	size_t num_bytes;
	int err;

	if (ctx == NULL) {
	return -ENODEV;
	}

	k_mutex_lock(&ctx->iface_lock, K_FOREVER);

	sys_put_be16(start_addr, &ctx->tx_adu.data[0]);
	length += sizeof(start_addr);
	sys_put_be16(num_regs, &ctx->tx_adu.data[2]);
	length += sizeof(num_regs);

	num_bytes = num_regs * sizeof(uint16_t);
	ctx->tx_adu.data[4] = num_bytes;
	length += num_bytes + 1;

	if (length > sizeof(ctx->tx_adu.data)) {
	LOG_ERR("Length of data buffer is not sufficient");
	k_mutex_unlock(&ctx->iface_lock);
	return -ENOBUFS;
	}

	ctx->tx_adu.length = length;
	data_ptr = &ctx->tx_adu.data[5];

	for (uint16_t i = 0; i < num_regs; i++) {
	sys_put_be16(reg_buf[i], data_ptr);
	data_ptr += sizeof(uint16_t);
	}

	err = mbc_send_cmd(ctx, unit_id, MODBUS_FC16_HOLDING_REGS_WR, NULL);
	k_mutex_unlock(&ctx->iface_lock);

	return err;
	}

	#ifdef CONFIG_MODBUS_FP_EXTENSIONS
	int modbus_write_holding_regs_fp(const int iface,
	const uint8_t unit_id,
	const uint16_t start_addr,
	float *const reg_buf,
	const uint16_t num_regs)
	{
	struct modbus_context *ctx = modbus_get_context(iface);
	size_t length = 0;
	uint8_t *data_ptr;
	size_t num_bytes;
	int err;

	if (ctx == NULL) {
	return -ENODEV;
	}

	k_mutex_lock(&ctx->iface_lock, K_FOREVER);

	sys_put_be16(start_addr, &ctx->tx_adu.data[0]);
	length += sizeof(start_addr);
	/* A 32-bit float is mapped to two 16-bit registers */
	sys_put_be16(num_regs * 2, &ctx->tx_adu.data[2]);
	length += sizeof(num_regs);

	num_bytes = num_regs * sizeof(float);
	ctx->tx_adu.data[4] = num_bytes;
	length += num_bytes + 1;

	if (length > sizeof(ctx->tx_adu.data)) {
	LOG_ERR("Length of data buffer is not sufficient");
	k_mutex_unlock(&ctx->iface_lock);
	return -ENOBUFS;
	}

	ctx->tx_adu.length = length;
	data_ptr = &ctx->tx_adu.data[5];

	for (uint16_t i = 0; i < num_regs; i++) {
	uint32_t reg_val;

	memcpy(&reg_val, &reg_buf[i], sizeof(reg_val));
	sys_put_be32(reg_val, data_ptr);
	data_ptr += sizeof(uint32_t);
	}

	err = mbc_send_cmd(ctx, unit_id, MODBUS_FC16_HOLDING_REGS_WR, NULL);
	k_mutex_unlock(&ctx->iface_lock);

	return err;
	}
	#endif