drivers: can: native_posix_linux: initial implementation
This driver provides an interface to SocketCAN interfaces of the Linux system running a Zephyr application with the native_posix board. These interfaces may be virtual or actual CAN buses. Signed-off-by: Martin Jäger <martin@libre.solar>
This commit is contained in:
Martin Jäger
Carles Cufí
parent
fd4d18852d
commit
77e4c6515a
8 changed files with 702 additions and 1 deletions
434
drivers/can/can_native_posix_linux.c
Normal file
434
drivers/can/can_native_posix_linux.c
Normal file
|
|
@ -0,0 +1,434 @@
|
|||
/*
|
||||
* Copyright (c) 2022 Martin Jäger <martin@libre.solar>
|
||||
*
|
||||
* SPDX-License-Identifier: Apache-2.0
|
||||
*/
|
||||
|
||||
#define DT_DRV_COMPAT zephyr_native_posix_linux_can
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
|
||||
#include <zephyr/drivers/can.h>
|
||||
#include <zephyr/kernel.h>
|
||||
#include <zephyr/logging/log.h>
|
||||
#include <zephyr/net/net_pkt.h>
|
||||
#include <zephyr/net/socketcan.h>
|
||||
#include <zephyr/net/socketcan_utils.h>
|
||||
|
||||
#include "can_utils.h"
|
||||
#include "can_native_posix_linux_socketcan.h"
|
||||
|
||||
LOG_MODULE_REGISTER(can_npl, CONFIG_CAN_LOG_LEVEL);
|
||||
|
||||
struct can_filter_context {
|
||||
can_rx_callback_t rx_cb;
|
||||
void *cb_arg;
|
||||
struct can_filter filter;
|
||||
};
|
||||
|
||||
struct can_npl_data {
|
||||
struct can_filter_context filters[CONFIG_CAN_MAX_FILTER];
|
||||
struct k_mutex filter_mutex;
|
||||
struct k_sem tx_idle;
|
||||
struct k_sem tx_done;
|
||||
can_tx_callback_t tx_callback;
|
||||
void *tx_user_data;
|
||||
bool loopback;
|
||||
bool mode_fd;
|
||||
int dev_fd; /* Linux socket file descriptor */
|
||||
struct k_thread rx_thread;
|
||||
|
||||
K_KERNEL_STACK_MEMBER(rx_thread_stack, CONFIG_ARCH_POSIX_RECOMMENDED_STACK_SIZE);
|
||||
};
|
||||
|
||||
struct can_npl_config {
|
||||
const char *if_name;
|
||||
};
|
||||
|
||||
static void dispatch_frame(const struct device *dev, struct can_frame *frame)
|
||||
{
|
||||
struct can_npl_data *data = dev->data;
|
||||
can_rx_callback_t callback;
|
||||
struct can_frame tmp_frame;
|
||||
|
||||
k_mutex_lock(&data->filter_mutex, K_FOREVER);
|
||||
|
||||
for (int filter_id = 0; filter_id < ARRAY_SIZE(data->filters); filter_id++) {
|
||||
if (data->filters[filter_id].rx_cb == NULL) {
|
||||
continue;
|
||||
}
|
||||
|
||||
if (!can_utils_filter_match(frame,
|
||||
&data->filters[filter_id].filter)) {
|
||||
continue;
|
||||
}
|
||||
|
||||
/* Make a temporary copy in case the user modifies the message */
|
||||
tmp_frame = *frame;
|
||||
|
||||
callback = data->filters[filter_id].rx_cb;
|
||||
callback(dev, &tmp_frame, data->filters[filter_id].cb_arg);
|
||||
}
|
||||
|
||||
k_mutex_unlock(&data->filter_mutex);
|
||||
}
|
||||
|
||||
static void rx_thread(void *arg1, void *arg2, void *arg3)
|
||||
{
|
||||
const struct device *dev = arg1;
|
||||
struct can_npl_data *data = dev->data;
|
||||
struct socketcan_frame sframe;
|
||||
struct can_frame frame;
|
||||
bool msg_confirm;
|
||||
int count;
|
||||
|
||||
ARG_UNUSED(arg2);
|
||||
ARG_UNUSED(arg3);
|
||||
|
||||
LOG_DBG("Starting Linux SocketCAN RX thread");
|
||||
|
||||
while (true) {
|
||||
while (linux_socketcan_poll_data(data->dev_fd) == 0) {
|
||||
count = linux_socketcan_read_data(data->dev_fd, (void *)(&sframe),
|
||||
sizeof(sframe), &msg_confirm);
|
||||
if (msg_confirm) {
|
||||
if (data->tx_callback != NULL) {
|
||||
data->tx_callback(dev, 0, data->tx_user_data);
|
||||
} else {
|
||||
k_sem_give(&data->tx_done);
|
||||
}
|
||||
|
||||
k_sem_give(&data->tx_idle);
|
||||
|
||||
if (!data->loopback) {
|
||||
continue;
|
||||
}
|
||||
}
|
||||
if (count <= 0) {
|
||||
break;
|
||||
}
|
||||
|
||||
socketcan_to_can_frame(&sframe, &frame);
|
||||
|
||||
LOG_DBG("Received %d bytes. Id: 0x%x, ID type: %s %s",
|
||||
frame.dlc, frame.id,
|
||||
frame.id_type == CAN_STANDARD_IDENTIFIER ?
|
||||
"standard" : "extended",
|
||||
frame.rtr == CAN_DATAFRAME ? "" : ", RTR frame");
|
||||
|
||||
dispatch_frame(dev, &frame);
|
||||
}
|
||||
|
||||
/* short sleep required to avoid blocking the whole native_posix process */
|
||||
k_sleep(K_MSEC(1));
|
||||
}
|
||||
}
|
||||
|
||||
static int can_npl_send(const struct device *dev, const struct can_frame *frame,
|
||||
k_timeout_t timeout, can_tx_callback_t callback, void *user_data)
|
||||
{
|
||||
struct can_npl_data *data = dev->data;
|
||||
struct socketcan_frame sframe;
|
||||
uint8_t max_dlc = CAN_MAX_DLC;
|
||||
size_t mtu = CAN_MTU;
|
||||
int ret = -EIO;
|
||||
|
||||
LOG_DBG("Sending %d bytes on %s. Id: 0x%x, ID type: %s %s",
|
||||
frame->dlc, dev->name, frame->id,
|
||||
frame->id_type == CAN_STANDARD_IDENTIFIER ?
|
||||
"standard" : "extended",
|
||||
frame->rtr == CAN_DATAFRAME ? "" : ", RTR frame");
|
||||
|
||||
#ifdef CONFIG_CAN_FD_MODE
|
||||
if (data->mode_fd && frame->fd == 1) {
|
||||
max_dlc = CANFD_MAX_DLC;
|
||||
mtu = CANFD_MTU;
|
||||
}
|
||||
#endif /* CONFIG_CAN_FD_MODE */
|
||||
|
||||
if (frame->dlc > max_dlc) {
|
||||
LOG_ERR("DLC of %d exceeds maximum (%d)", frame->dlc, max_dlc);
|
||||
return -EINVAL;
|
||||
}
|
||||
|
||||
if (data->dev_fd <= 0) {
|
||||
LOG_ERR("No file descriptor: %d", data->dev_fd);
|
||||
return -EIO;
|
||||
}
|
||||
|
||||
socketcan_from_can_frame(frame, &sframe);
|
||||
|
||||
if (k_sem_take(&data->tx_idle, timeout) != 0) {
|
||||
return -EAGAIN;
|
||||
}
|
||||
|
||||
data->tx_callback = callback;
|
||||
data->tx_user_data = user_data;
|
||||
|
||||
ret = linux_socketcan_write_data(data->dev_fd, &sframe, mtu);
|
||||
if (ret < 0) {
|
||||
LOG_ERR("Cannot send CAN data len %d (%d)", sframe.len, -errno);
|
||||
}
|
||||
|
||||
if (callback == NULL) {
|
||||
k_sem_take(&data->tx_done, K_FOREVER);
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
static int can_npl_add_rx_filter(const struct device *dev, can_rx_callback_t cb,
|
||||
void *cb_arg, const struct can_filter *filter)
|
||||
{
|
||||
struct can_npl_data *data = dev->data;
|
||||
struct can_filter_context *filter_ctx;
|
||||
int filter_id = -ENOSPC;
|
||||
|
||||
LOG_DBG("Setting filter ID: 0x%x, mask: 0x%x", filter->id,
|
||||
filter->id_mask);
|
||||
LOG_DBG("Filter type: %s ID %s mask",
|
||||
filter->id_type == CAN_STANDARD_IDENTIFIER ?
|
||||
"standard" : "extended",
|
||||
((filter->id_type && (filter->id_mask == CAN_STD_ID_MASK)) ||
|
||||
(!filter->id_type && (filter->id_mask == CAN_EXT_ID_MASK))) ?
|
||||
"with" : "without");
|
||||
|
||||
k_mutex_lock(&data->filter_mutex, K_FOREVER);
|
||||
|
||||
for (int i = 0; i < ARRAY_SIZE(data->filters); i++) {
|
||||
if (data->filters[i].rx_cb == NULL) {
|
||||
filter_id = i;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if (filter_id < 0) {
|
||||
LOG_ERR("No free filter left");
|
||||
k_mutex_unlock(&data->filter_mutex);
|
||||
return filter_id;
|
||||
}
|
||||
|
||||
filter_ctx = &data->filters[filter_id];
|
||||
filter_ctx->rx_cb = cb;
|
||||
filter_ctx->cb_arg = cb_arg;
|
||||
filter_ctx->filter = *filter;
|
||||
|
||||
k_mutex_unlock(&data->filter_mutex);
|
||||
|
||||
LOG_DBG("Filter added. ID: %d", filter_id);
|
||||
|
||||
return filter_id;
|
||||
}
|
||||
|
||||
static void can_npl_remove_rx_filter(const struct device *dev, int filter_id)
|
||||
{
|
||||
struct can_npl_data *data = dev->data;
|
||||
|
||||
if (filter_id < 0 || filter_id >= ARRAY_SIZE(data->filters)) {
|
||||
return;
|
||||
}
|
||||
|
||||
k_mutex_lock(&data->filter_mutex, K_FOREVER);
|
||||
data->filters[filter_id].rx_cb = NULL;
|
||||
k_mutex_unlock(&data->filter_mutex);
|
||||
|
||||
LOG_DBG("Filter removed. ID: %d", filter_id);
|
||||
}
|
||||
|
||||
static int can_npl_get_capabilities(const struct device *dev, can_mode_t *cap)
|
||||
{
|
||||
ARG_UNUSED(dev);
|
||||
|
||||
*cap = CAN_MODE_NORMAL | CAN_MODE_LOOPBACK;
|
||||
|
||||
#if CONFIG_CAN_FD_MODE
|
||||
*cap |= CAN_MODE_FD;
|
||||
#endif /* CONFIG_CAN_FD_MODE */
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
static int can_npl_set_mode(const struct device *dev, can_mode_t mode)
|
||||
{
|
||||
struct can_npl_data *data = dev->data;
|
||||
|
||||
#ifdef CONFIG_CAN_FD_MODE
|
||||
if ((mode & ~(CAN_MODE_LOOPBACK | CAN_MODE_FD)) != 0) {
|
||||
LOG_ERR("unsupported mode: 0x%08x", mode);
|
||||
return -ENOTSUP;
|
||||
}
|
||||
#else
|
||||
if ((mode & ~(CAN_MODE_LOOPBACK)) != 0) {
|
||||
LOG_ERR("unsupported mode: 0x%08x", mode);
|
||||
return -ENOTSUP;
|
||||
}
|
||||
#endif /* CONFIG_CAN_FD_MODE */
|
||||
|
||||
/* loopback is handled internally in rx_thread */
|
||||
data->loopback = (mode & CAN_MODE_LOOPBACK) != 0;
|
||||
|
||||
data->mode_fd = (mode & CAN_MODE_FD) != 0;
|
||||
linux_socketcan_set_mode_fd(data->dev_fd, data->mode_fd);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
static int can_npl_set_timing(const struct device *dev, const struct can_timing *timing)
|
||||
{
|
||||
ARG_UNUSED(dev);
|
||||
ARG_UNUSED(timing);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
#ifdef CONFIG_CAN_FD_MODE
|
||||
static int can_npl_set_timing_data(const struct device *dev, const struct can_timing *timing)
|
||||
{
|
||||
ARG_UNUSED(dev);
|
||||
ARG_UNUSED(timing);
|
||||
|
||||
return 0;
|
||||
}
|
||||
#endif /* CONFIG_CAN_FD_MODE */
|
||||
|
||||
static int can_npl_get_state(const struct device *dev, enum can_state *state,
|
||||
struct can_bus_err_cnt *err_cnt)
|
||||
{
|
||||
ARG_UNUSED(dev);
|
||||
|
||||
if (state != NULL) {
|
||||
/* SocketCAN does not forward error frames by default */
|
||||
*state = CAN_STATE_ERROR_ACTIVE;
|
||||
}
|
||||
|
||||
if (err_cnt) {
|
||||
err_cnt->tx_err_cnt = 0;
|
||||
err_cnt->rx_err_cnt = 0;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
#ifndef CONFIG_CAN_AUTO_BUS_OFF_RECOVERY
|
||||
static int can_npl_recover(const struct device *dev, k_timeout_t timeout)
|
||||
{
|
||||
ARG_UNUSED(dev);
|
||||
ARG_UNUSED(timeout);
|
||||
|
||||
return 0;
|
||||
}
|
||||
#endif /* CONFIG_CAN_AUTO_BUS_OFF_RECOVERY */
|
||||
|
||||
static void can_npl_set_state_change_callback(const struct device *dev,
|
||||
can_state_change_callback_t cb,
|
||||
void *user_data)
|
||||
{
|
||||
ARG_UNUSED(dev);
|
||||
ARG_UNUSED(cb);
|
||||
ARG_UNUSED(user_data);
|
||||
}
|
||||
|
||||
static int can_npl_get_core_clock(const struct device *dev, uint32_t *rate)
|
||||
{
|
||||
/* Return 16MHz as an realistic value for the testcases */
|
||||
*rate = 16000000;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
static int can_npl_get_max_filters(const struct device *dev, enum can_ide id_type)
|
||||
{
|
||||
ARG_UNUSED(id_type);
|
||||
|
||||
return CONFIG_CAN_MAX_FILTER;
|
||||
}
|
||||
|
||||
static const struct can_driver_api can_npl_driver_api = {
|
||||
.get_capabilities = can_npl_get_capabilities,
|
||||
.set_mode = can_npl_set_mode,
|
||||
.set_timing = can_npl_set_timing,
|
||||
.send = can_npl_send,
|
||||
.add_rx_filter = can_npl_add_rx_filter,
|
||||
.remove_rx_filter = can_npl_remove_rx_filter,
|
||||
.get_state = can_npl_get_state,
|
||||
#ifndef CONFIG_CAN_AUTO_BUS_OFF_RECOVERY
|
||||
.recover = can_npl_recover,
|
||||
#endif
|
||||
.set_state_change_callback = can_npl_set_state_change_callback,
|
||||
.get_core_clock = can_npl_get_core_clock,
|
||||
.get_max_filters = can_npl_get_max_filters,
|
||||
.timing_min = {
|
||||
.sjw = 0x1,
|
||||
.prop_seg = 0x01,
|
||||
.phase_seg1 = 0x01,
|
||||
.phase_seg2 = 0x01,
|
||||
.prescaler = 0x01
|
||||
},
|
||||
.timing_max = {
|
||||
.sjw = 0x0F,
|
||||
.prop_seg = 0x0F,
|
||||
.phase_seg1 = 0x0F,
|
||||
.phase_seg2 = 0x0F,
|
||||
.prescaler = 0xFFFF
|
||||
},
|
||||
#ifdef CONFIG_CAN_FD_MODE
|
||||
.set_timing_data = can_npl_set_timing_data,
|
||||
.timing_data_min = {
|
||||
.sjw = 0x1,
|
||||
.prop_seg = 0x01,
|
||||
.phase_seg1 = 0x01,
|
||||
.phase_seg2 = 0x01,
|
||||
.prescaler = 0x01
|
||||
},
|
||||
.timing_data_max = {
|
||||
.sjw = 0x0F,
|
||||
.prop_seg = 0x0F,
|
||||
.phase_seg1 = 0x0F,
|
||||
.phase_seg2 = 0x0F,
|
||||
.prescaler = 0xFFFF
|
||||
},
|
||||
#endif /* CONFIG_CAN_FD_MODE */
|
||||
};
|
||||
|
||||
static int can_npl_init(const struct device *dev)
|
||||
{
|
||||
const struct can_npl_config *cfg = dev->config;
|
||||
struct can_npl_data *data = dev->data;
|
||||
|
||||
k_mutex_init(&data->filter_mutex);
|
||||
k_sem_init(&data->tx_idle, 1, 1);
|
||||
k_sem_init(&data->tx_done, 0, 1);
|
||||
|
||||
data->dev_fd = linux_socketcan_iface_open(cfg->if_name);
|
||||
if (data->dev_fd < 0) {
|
||||
LOG_ERR("Cannot open %s (%d)", cfg->if_name, data->dev_fd);
|
||||
return -ENODEV;
|
||||
}
|
||||
|
||||
k_thread_create(&data->rx_thread, data->rx_thread_stack,
|
||||
K_KERNEL_STACK_SIZEOF(data->rx_thread_stack),
|
||||
rx_thread, (void *)dev, NULL, NULL,
|
||||
CONFIG_CAN_NATIVE_POSIX_LINUX_RX_THREAD_PRIORITY,
|
||||
0, K_NO_WAIT);
|
||||
|
||||
LOG_DBG("Init of %s done", dev->name);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
#define CAN_NATIVE_POSIX_LINUX_INIT(inst) \
|
||||
\
|
||||
static const struct can_npl_config can_npl_cfg_##inst = { \
|
||||
.if_name = DT_INST_PROP(inst, host_interface), \
|
||||
}; \
|
||||
\
|
||||
static struct can_npl_data can_npl_data_##inst; \
|
||||
\
|
||||
DEVICE_DT_INST_DEFINE(inst, &can_npl_init, NULL, \
|
||||
&can_npl_data_##inst, &can_npl_cfg_##inst, \
|
||||
POST_KERNEL, CONFIG_CAN_INIT_PRIORITY, \
|
||||
&can_npl_driver_api);
|
||||
|
||||
DT_INST_FOREACH_STATUS_OKAY(CAN_NATIVE_POSIX_LINUX_INIT)
|
||||
Loading…
Add table
Add a link
Reference in a new issue