You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
409 lines
8.9 KiB
409 lines
8.9 KiB
/* |
|
* 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) |
|
{ |
|
k_sem_reset(&ctx->client_wait_sem); |
|
|
|
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; |
|
}
|
|
|