subsys/modbus/modbus_core.c

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

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

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

#define DT_DRV_COMPAT zephyr_modbus_serial

#define MB_RTU_DEFINE_GPIO_CFG(inst, prop)				\
	static struct gpio_dt_spec prop##_cfg_##inst = {		\
		.port = DEVICE_DT_GET(DT_INST_PHANDLE(inst, prop)),	\
		.pin = DT_INST_GPIO_PIN(inst, prop),			\
		.dt_flags = DT_INST_GPIO_FLAGS(inst,  prop),		\
	};

#define MB_RTU_DEFINE_GPIO_CFGS(inst)					\
	COND_CODE_1(DT_INST_NODE_HAS_PROP(inst, de_gpios),		\
		    (MB_RTU_DEFINE_GPIO_CFG(inst, de_gpios)), ())	\
	COND_CODE_1(DT_INST_NODE_HAS_PROP(inst, re_gpios),		\
		    (MB_RTU_DEFINE_GPIO_CFG(inst, re_gpios)), ())

DT_INST_FOREACH_STATUS_OKAY(MB_RTU_DEFINE_GPIO_CFGS)

#define MB_RTU_ASSIGN_GPIO_CFG(inst, prop)			\
	COND_CODE_1(DT_INST_NODE_HAS_PROP(inst, prop),		\
		    (&prop##_cfg_##inst), (NULL))

#define MODBUS_DT_GET_SERIAL_DEV(inst) {			\
		.dev = DEVICE_DT_GET(DT_INST_PARENT(inst)),	\
		.de = MB_RTU_ASSIGN_GPIO_CFG(inst, de_gpios),	\
		.re = MB_RTU_ASSIGN_GPIO_CFG(inst, re_gpios),	\
	},

#ifdef CONFIG_MODBUS_SERIAL
static struct modbus_serial_config modbus_serial_cfg[] = {
	DT_INST_FOREACH_STATUS_OKAY(MODBUS_DT_GET_SERIAL_DEV)
};
#endif

#define MODBUS_DT_GET_DEV(inst) {				\
		.iface_name = DEVICE_DT_NAME(DT_DRV_INST(inst)),\
		.cfg = &modbus_serial_cfg[inst],		\
	},

#define DEFINE_MODBUS_RAW_ADU(x, _) {				\
		.iface_name = "RAW_"#x,				\
		.rawcb.raw_tx_cb = NULL,				\
		.mode = MODBUS_MODE_RAW,			\
	}


static struct modbus_context mb_ctx_tbl[] = {
	DT_INST_FOREACH_STATUS_OKAY(MODBUS_DT_GET_DEV)
#ifdef CONFIG_MODBUS_RAW_ADU
	LISTIFY(CONFIG_MODBUS_NUMOF_RAW_ADU, DEFINE_MODBUS_RAW_ADU, (,), _)
#endif
};

static void modbus_rx_handler(struct k_work *item)
{
	struct modbus_context *ctx;

	ctx = CONTAINER_OF(item, struct modbus_context, server_work);

	switch (ctx->mode) {
	case MODBUS_MODE_RTU:
	case MODBUS_MODE_ASCII:
		if (IS_ENABLED(CONFIG_MODBUS_SERIAL)) {
			modbus_serial_rx_disable(ctx);
			ctx->rx_adu_err = modbus_serial_rx_adu(ctx);
		}
		break;
	case MODBUS_MODE_RAW:
		if (IS_ENABLED(CONFIG_MODBUS_RAW_ADU)) {
			ctx->rx_adu_err = modbus_raw_rx_adu(ctx);
		}
		break;
	default:
		LOG_ERR("Unknown MODBUS mode");
		return;
	}

	if (ctx->client == true) {
		k_sem_give(&ctx->client_wait_sem);
	} 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) {
				modbus_serial_rx_enable(ctx);
			}
			break;
		default:
			break;
		}
	}
}

void modbus_tx_adu(struct modbus_context *ctx)
{
	switch (ctx->mode) {
	case MODBUS_MODE_RTU:
	case MODBUS_MODE_ASCII:
		if (IS_ENABLED(CONFIG_MODBUS_SERIAL) &&
		    modbus_serial_tx_adu(ctx)) {
			LOG_ERR("Unsupported MODBUS serial mode");
		}
		break;
	case MODBUS_MODE_RAW:
		if (IS_ENABLED(CONFIG_MODBUS_RAW_ADU) &&
		    modbus_raw_tx_adu(ctx)) {
			LOG_ERR("Unsupported MODBUS raw mode");
		}
		break;
	default:
		LOG_ERR("Unknown MODBUS mode");
	}
}

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 -ETIMEDOUT;
	}

	return ctx->rx_adu_err;
}

struct modbus_context *modbus_get_context(const uint8_t iface)
{
	struct modbus_context *ctx;

	if (iface >= ARRAY_SIZE(mb_ctx_tbl)) {
		LOG_ERR("Interface %u not available", iface);
		return NULL;
	}

	ctx = &mb_ctx_tbl[iface];

	if (!atomic_test_bit(&ctx->state, MODBUS_STATE_CONFIGURED)) {
		LOG_ERR("Interface not configured");
		return NULL;
	}

	return ctx;
}

int modbus_iface_get_by_ctx(const struct modbus_context *ctx)
{
	for (int i = 0; i < ARRAY_SIZE(mb_ctx_tbl); i++) {
		if (&mb_ctx_tbl[i] == ctx) {
			return i;
		}
	}

	return -ENODEV;
}

int modbus_iface_get_by_name(const char *iface_name)
{
	for (int i = 0; i < ARRAY_SIZE(mb_ctx_tbl); i++) {
		if (strcmp(iface_name, mb_ctx_tbl[i].iface_name) == 0) {
			return i;
		}
	}

	return -ENODEV;
}

static struct modbus_context *modbus_init_iface(const uint8_t iface)
{
	struct modbus_context *ctx;

	if (iface >= ARRAY_SIZE(mb_ctx_tbl)) {
		LOG_ERR("Interface %u not available", iface);
		return NULL;
	}

	ctx = &mb_ctx_tbl[iface];

	if (atomic_test_and_set_bit(&ctx->state, MODBUS_STATE_CONFIGURED)) {
		LOG_ERR("Interface already used");
		return NULL;
	}

	k_mutex_init(&ctx->iface_lock);
	k_sem_init(&ctx->client_wait_sem, 0, 1);
	k_work_init(&ctx->server_work, modbus_rx_handler);

	return ctx;
}

static int modbus_user_fc_init(struct modbus_context *ctx, struct modbus_iface_param param)
{
	sys_slist_init(&ctx->user_defined_cbs);
	LOG_DBG("Initializing user-defined function code support.");

	return 0;
}

int modbus_init_server(const int iface, struct modbus_iface_param param)
{
	struct modbus_context *ctx = NULL;
	int rc = 0;

	if (!IS_ENABLED(CONFIG_MODBUS_SERVER)) {
		LOG_ERR("Modbus server support is not enabled");
		rc = -ENOTSUP;
		goto init_server_error;
	}

	if (param.server.user_cb == NULL) {
		LOG_ERR("User callbacks should be available");
		rc = -EINVAL;
		goto init_server_error;
	}

	ctx = modbus_init_iface(iface);
	if (ctx == NULL) {
		rc = -EINVAL;
		goto init_server_error;
	}

	ctx->client = false;

	if (modbus_user_fc_init(ctx, param) != 0) {
		LOG_ERR("Failed to init MODBUS user defined function codes");
		rc = -EINVAL;
		goto init_server_error;
	}

	switch (param.mode) {
	case MODBUS_MODE_RTU:
	case MODBUS_MODE_ASCII:
		if (IS_ENABLED(CONFIG_MODBUS_SERIAL) &&
		    modbus_serial_init(ctx, param) != 0) {
			LOG_ERR("Failed to init MODBUS over serial line");
			rc = -EINVAL;
			goto init_server_error;
		}
		break;
	case MODBUS_MODE_RAW:
		if (IS_ENABLED(CONFIG_MODBUS_RAW_ADU) &&
		    modbus_raw_init(ctx, param) != 0) {
			LOG_ERR("Failed to init MODBUS raw ADU support");
			rc = -EINVAL;
			goto init_server_error;
		}
		break;
	default:
		LOG_ERR("Unknown MODBUS mode");
		rc = -ENOTSUP;
		goto init_server_error;
	}

	ctx->unit_id = param.server.unit_id;
	ctx->mbs_user_cb = param.server.user_cb;
	if (IS_ENABLED(CONFIG_MODBUS_FC08_DIAGNOSTIC)) {
		modbus_reset_stats(ctx);
	}

	LOG_DBG("Modbus interface %s initialized", ctx->iface_name);

	return 0;

init_server_error:
	if (ctx != NULL) {
		atomic_clear_bit(&ctx->state, MODBUS_STATE_CONFIGURED);
	}

	return rc;
}

int modbus_register_user_fc(const int iface, struct modbus_custom_fc *custom_fc)
{
	struct modbus_context *ctx = modbus_get_context(iface);

	if (!custom_fc) {
		LOG_ERR("Provided function code handler was NULL");
		return -EINVAL;
	}

	if (custom_fc->fc & BIT(7)) {
		LOG_ERR("Function codes must have MSB of 0");
		return -EINVAL;
	}

	custom_fc->excep_code = MODBUS_EXC_NONE;

	LOG_DBG("Registered new custom function code %d", custom_fc->fc);
	sys_slist_append(&ctx->user_defined_cbs, &custom_fc->node);

	return 0;
}

int modbus_init_client(const int iface, struct modbus_iface_param param)
{
	struct modbus_context *ctx = NULL;
	int rc = 0;

	if (!IS_ENABLED(CONFIG_MODBUS_CLIENT)) {
		LOG_ERR("Modbus client support is not enabled");
		rc = -ENOTSUP;
		goto init_client_error;
	}

	ctx = modbus_init_iface(iface);
	if (ctx == NULL) {
		rc = -EINVAL;
		goto init_client_error;
	}

	ctx->client = true;

	switch (param.mode) {
	case MODBUS_MODE_RTU:
	case MODBUS_MODE_ASCII:
		if (IS_ENABLED(CONFIG_MODBUS_SERIAL) &&
		    modbus_serial_init(ctx, param) != 0) {
			LOG_ERR("Failed to init MODBUS over serial line");
			rc = -EINVAL;
			goto init_client_error;
		}
		break;
	case MODBUS_MODE_RAW:
		if (IS_ENABLED(CONFIG_MODBUS_RAW_ADU) &&
		    modbus_raw_init(ctx, param) != 0) {
			LOG_ERR("Failed to init MODBUS raw ADU support");
			rc = -EINVAL;
			goto init_client_error;
		}
		break;
	default:
		LOG_ERR("Unknown MODBUS mode");
		rc = -ENOTSUP;
		goto init_client_error;
	}

	ctx->unit_id = 0;
	ctx->mbs_user_cb = NULL;
	ctx->rxwait_to = param.rx_timeout;

	return 0;

init_client_error:
	if (ctx != NULL) {
		atomic_clear_bit(&ctx->state, MODBUS_STATE_CONFIGURED);
	}

	return rc;
}

int modbus_disable(const uint8_t iface)
{
	struct modbus_context *ctx;
	struct k_work_sync work_sync;

	ctx = modbus_get_context(iface);
	if (ctx == NULL) {
		LOG_ERR("Interface %u not initialized", iface);
		return -EINVAL;
	}

	switch (ctx->mode) {
	case MODBUS_MODE_RTU:
	case MODBUS_MODE_ASCII:
		if (IS_ENABLED(CONFIG_MODBUS_SERIAL)) {
			modbus_serial_disable(ctx);
		}
		break;
	case MODBUS_MODE_RAW:
		break;
	default:
		LOG_ERR("Unknown MODBUS mode");
	}

	k_work_cancel_sync(&ctx->server_work, &work_sync);
	ctx->rxwait_to = 0;
	ctx->unit_id = 0;
	ctx->mbs_user_cb = NULL;
	atomic_clear_bit(&ctx->state, MODBUS_STATE_CONFIGURED);

	LOG_INF("Modbus interface %u disabled", iface);

	return 0;
}