diff --git a/boards/posix/native_posix/native_posix.dts b/boards/posix/native_posix/native_posix.dts index 7165926d317a4..3bd5555088985 100644 --- a/boards/posix/native_posix/native_posix.dts +++ b/boards/posix/native_posix/native_posix.dts @@ -194,4 +194,16 @@ sample-point = <875>; bus-speed = <125000>; }; + + can0: can { + status = "disabled"; + compatible = "zephyr,native-posix-linux-can"; + /* adjust zcan0 to desired host interface or create an alternative + * name, e.g.: sudo ip link property add dev vcan0 altname zcan0 + */ + host-interface = "zcan0"; + sjw = <1>; + sample-point = <875>; + bus-speed = <125000>; + }; }; diff --git a/drivers/can/CMakeLists.txt b/drivers/can/CMakeLists.txt index 3d41b6585de44..a28ca83df6980 100644 --- a/drivers/can/CMakeLists.txt +++ b/drivers/can/CMakeLists.txt @@ -14,6 +14,21 @@ zephyr_library_sources_ifdef(CONFIG_CAN_STM32FD can_stm32fd.c) zephyr_library_sources_ifdef(CONFIG_CAN_STM32H7 can_stm32h7.c) zephyr_library_sources_ifdef(CONFIG_CAN_RCAR can_rcar.c) +if(CONFIG_CAN_NATIVE_POSIX_LINUX) + if(${CMAKE_HOST_SYSTEM_NAME} STREQUAL Linux) + zephyr_library_include_directories(${ZEPHYR_BASE}/subsys/net/l2) + zephyr_library_compile_definitions(NO_POSIX_CHEATS) + zephyr_library_compile_definitions(_BSD_SOURCE) + zephyr_library_compile_definitions(_DEFAULT_SOURCE) + zephyr_library_sources( + can_native_posix_linux.c + can_native_posix_linux_socketcan.c + ) + else() + message(FATAL_ERROR "CONFIG_CAN_NATIVE_POSIX_LINUX only available on Linux") + endif() +endif() + zephyr_library_sources_ifdef(CONFIG_CAN_SJA1000 can_sja1000.c) zephyr_library_sources_ifdef(CONFIG_CAN_ESP32_TWAI can_esp32_twai.c) diff --git a/drivers/can/Kconfig b/drivers/can/Kconfig index af103623e7d4b..1916498ab054c 100644 --- a/drivers/can/Kconfig +++ b/drivers/can/Kconfig @@ -98,7 +98,7 @@ source "drivers/can/Kconfig.mcp2515" source "drivers/can/Kconfig.mcan" source "drivers/can/Kconfig.rcar" source "drivers/can/Kconfig.loopback" - +source "drivers/can/Kconfig.native_posix_linux" source "drivers/can/Kconfig.sja1000" source "drivers/can/Kconfig.esp32" diff --git a/drivers/can/Kconfig.native_posix_linux b/drivers/can/Kconfig.native_posix_linux new file mode 100644 index 0000000000000..c6719e1de49e5 --- /dev/null +++ b/drivers/can/Kconfig.native_posix_linux @@ -0,0 +1,30 @@ +# Native Linux SocketCAN configuration options + +# Copyright (c) 2022 Martin Jäger +# SPDX-License-Identifier: Apache-2.0 + +config CAN_NATIVE_POSIX_LINUX + bool "Native Linux SocketCAN Driver" + default y + depends on DT_HAS_ZEPHYR_NATIVE_POSIX_LINUX_CAN_ENABLED + help + Enable native Linux SocketCAN Driver + +if CAN_NATIVE_POSIX_LINUX + +config CAN_NATIVE_POSIX_LINUX_RX_THREAD_PRIORITY + int "Priority for internal RX thread" + default 2 + help + Priority level of the internal thread which is run for + handling of incoming packets. + +config CAN_MAX_FILTER + int "Maximum number of concurrent active filters" + default 5 + range 1 32 + help + Defines the array size of the callback/msgq pointers. + Must be at least the size of concurrent reads. + +endif # CAN_NATIVE_POSIX_LINUX diff --git a/drivers/can/can_native_posix_linux.c b/drivers/can/can_native_posix_linux.c new file mode 100644 index 0000000000000..ac2d06a65aaf6 --- /dev/null +++ b/drivers/can/can_native_posix_linux.c @@ -0,0 +1,434 @@ +/* + * Copyright (c) 2022 Martin Jäger + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#define DT_DRV_COMPAT zephyr_native_posix_linux_can + +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#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) diff --git a/drivers/can/can_native_posix_linux_socketcan.c b/drivers/can/can_native_posix_linux_socketcan.c new file mode 100644 index 0000000000000..4747031195f81 --- /dev/null +++ b/drivers/can/can_native_posix_linux_socketcan.c @@ -0,0 +1,164 @@ +/* + * Copyright (c) 2019 Intel Corporation + * Copyright (c) 2022 Martin Jäger + * + * SPDX-License-Identifier: Apache-2.0 + */ + +/** + * @file + * + * Routines setting up the host system. Those are placed in separate file + * because there is naming conflicts between host and zephyr network stacks. + */ + +#include +#include +#include +#include +#include +#include + +/* Linux host include files. */ +#ifdef __linux +#include +#include +#include +#include +#include +#include +#include +#include +#else +#error "This driver can only be built on Linux systems" +#endif + +#include "can_native_posix_linux_socketcan.h" + +int linux_socketcan_iface_open(const char *if_name) +{ + struct sockaddr_can addr; + struct ifreq ifr; + int fd, opt, ret = -EINVAL; + + fd = socket(PF_CAN, SOCK_RAW, CAN_RAW); + if (fd < 0) { + return -errno; + } + + (void)memset(&ifr, 0, sizeof(ifr)); + (void)memset(&addr, 0, sizeof(addr)); + + strncpy(ifr.ifr_name, if_name, IFNAMSIZ); + + ret = ioctl(fd, SIOCGIFINDEX, (void *)&ifr); + if (ret < 0) { + close(fd); + return -errno; + } + + addr.can_ifindex = ifr.ifr_ifindex; + addr.can_family = PF_CAN; + + ret = bind(fd, (struct sockaddr *)&addr, sizeof(addr)); + if (ret < 0) { + close(fd); + return -errno; + } + + /* this option must always be enabled in order to receive TX confirmations */ + opt = 1; + ret = setsockopt(fd, SOL_CAN_RAW, CAN_RAW_RECV_OWN_MSGS, &opt, sizeof(opt)); + if (ret < 0) { + close(fd); + return -errno; + } + + return fd; +} + +int linux_socketcan_iface_close(int fd) +{ + return close(fd); +} + +int linux_socketcan_poll_data(int fd) +{ + struct timeval timeout; + fd_set rset; + int ret; + + FD_ZERO(&rset); + + FD_SET(fd, &rset); + + timeout.tv_sec = 0; + timeout.tv_usec = 0; + + ret = select(fd + 1, &rset, NULL, NULL, &timeout); + if (ret < 0 && errno != EINTR) { + return -errno; + } else if (ret > 0) { + if (FD_ISSET(fd, &rset)) { + return 0; + } + } + + return -EAGAIN; +} + +ssize_t linux_socketcan_read_data(int fd, void *buf, size_t buf_len, bool *msg_confirm) +{ + struct canfd_frame *frame = (struct canfd_frame *)buf; + struct msghdr msg = {0}; + + struct iovec iov = { + .iov_base = buf, + .iov_len = buf_len, + }; + msg.msg_iov = &iov; + msg.msg_iovlen = 1; + + int ret = recvmsg(fd, &msg, MSG_WAITALL); + + if (msg_confirm != NULL) { + *msg_confirm = (msg.msg_flags & MSG_CONFIRM) != 0; + } + + /* Make sure to set the flags for all frames received via the Linux API. + * + * Zephyr relies on defined flags field of the SocketCAN data for both FD and classical CAN + * frames. In Linux the flags field is undefined for legacy frames. + */ + if (ret == CANFD_MTU) { + frame->flags |= CANFD_FDF; + } else if (ret == CAN_MTU) { + frame->flags = 0; + } + + return ret; +} + +ssize_t linux_socketcan_write_data(int fd, void *buf, size_t buf_len) +{ + return write(fd, buf, buf_len); +} + +int linux_socketcan_setsockopt(int fd, int level, int optname, + const void *optval, socklen_t optlen) +{ + return setsockopt(fd, level, optname, optval, optlen); +} + +int linux_socketcan_getsockopt(int fd, int level, int optname, + void *optval, socklen_t *optlen) +{ + return getsockopt(fd, level, optname, optval, optlen); +} + +int linux_socketcan_set_mode_fd(int fd, bool mode_fd) +{ + int opt = mode_fd ? 1 : 0; + + return setsockopt(fd, SOL_CAN_RAW, CAN_RAW_FD_FRAMES, &opt, sizeof(opt)); +} diff --git a/drivers/can/can_native_posix_linux_socketcan.h b/drivers/can/can_native_posix_linux_socketcan.h new file mode 100644 index 0000000000000..76db602af9895 --- /dev/null +++ b/drivers/can/can_native_posix_linux_socketcan.h @@ -0,0 +1,32 @@ +/* + * Copyright (c) 2019 Intel Corporation + * + * SPDX-License-Identifier: Apache-2.0 + */ + +/** @file + * @brief Private functions for native posix canbus driver. + */ + +#ifndef ZEPHYR_DRIVERS_CAN_NATIVE_POSIX_LINUX_SOCKETCAN_H_ +#define ZEPHYR_DRIVERS_CAN_NATIVE_POSIX_LINUX_SOCKETCAN_H_ + +int linux_socketcan_iface_open(const char *if_name); + +int linux_socketcan_iface_close(int fd); + +int linux_socketcan_poll_data(int fd); + +ssize_t linux_socketcan_read_data(int fd, void *buf, size_t buf_len, bool *msg_confirm); + +ssize_t linux_socketcan_write_data(int fd, void *buf, size_t buf_len); + +int linux_socketcan_setsockopt(int fd, int level, int optname, const void *optval, + socklen_t optlen); + +int linux_socketcan_getsockopt(int fd, int level, int optname, void *optval, + socklen_t *optlen); + +int linux_socketcan_set_mode_fd(int fd, bool mode_fd); + +#endif /* ZEPHYR_DRIVERS_CAN_NATIVE_POSIX_LINUX_SOCKETCAN_H_ */ diff --git a/dts/bindings/can/zephyr,native-posix-linux-can.yaml b/dts/bindings/can/zephyr,native-posix-linux-can.yaml new file mode 100644 index 0000000000000..966a45fd5e51c --- /dev/null +++ b/dts/bindings/can/zephyr,native-posix-linux-can.yaml @@ -0,0 +1,14 @@ +# Copyright (c) 2022 Martin Jäger +# SPDX-License-Identifier: Apache-2.0 + +description: Zephyr CAN driver using Linux SocketCAN + +compatible: "zephyr,native-posix-linux-can" + +include: can-controller.yaml + +properties: + host-interface: + type: string + required: true + description: Linux host interface name (e.g. zcan0, vcan0, can0, ...)