bartoszek/zephyr
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
4e976e0d58
zephyr/subsys/modbus/modbus_core.c
Henrik Lindblom 01757cfd32 modbus: add support for defining custom functions 
Enables support for custom function codes. Modbus specification allows
vendor specific function codes in the range 65-72 & 100-110 [1] and this
feature allows users to implement custom logic for those codes.
Additionally, since the Zephyr Modbus stack doesn't implement all defined
Modbus fcs this feature allows users to add support for codes outside the
basic register reading / writing functionality offered by Zephyr.

Custom function codes can be added on a per-interface basis and the handler
structures are allocated by the caller.

[1]: https://modbus.org/docs/Modbus_Application_Protocol_V1_1b.pdf

Signed-off-by: Henrik Lindblom <henrik.lindblom@vaisala.com>
2023-09-29 13:05:42 +02:00

409 lines
8.9 KiB
C
Raw Blame History

/*
* 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->mode = MODBUS_MODE_RTU;
ctx->mbs_user_cb = NULL;
atomic_clear_bit(&ctx->state, MODBUS_STATE_CONFIGURED);
LOG_INF("Modbus interface %u disabled", iface);
return 0;
}
Reference in New Issue View Git Blame Copy Permalink