diff --git a/drivers/can/CMakeLists.txt b/drivers/can/CMakeLists.txt index 4b4b337e997..44f7d0f2726 100644 --- a/drivers/can/CMakeLists.txt +++ b/drivers/can/CMakeLists.txt @@ -1,6 +1,7 @@ # SPDX-License-Identifier: Apache-2.0 zephyr_sources_ifdef(CONFIG_CAN can_common.c) +zephyr_sources_ifdef(CONFIG_CAN_LOOPBACK can_loopback.c) zephyr_sources_ifdef(CONFIG_CAN_MCP2515 mcp2515.c) zephyr_sources_ifdef(CONFIG_CAN_STM32 stm32_can.c) zephyr_sources_ifdef(CONFIG_USERSPACE can_handlers.c) diff --git a/drivers/can/Kconfig b/drivers/can/Kconfig index 81a6dd8d433..606ab41e1f4 100644 --- a/drivers/can/Kconfig +++ b/drivers/can/Kconfig @@ -44,5 +44,6 @@ config CAN_1 source "drivers/can/Kconfig.stm32" source "drivers/can/Kconfig.mcp2515" +source "drivers/can/Kconfig.loopback" endif # CAN diff --git a/drivers/can/Kconfig.loopback b/drivers/can/Kconfig.loopback new file mode 100644 index 00000000000..9b04de1c6d4 --- /dev/null +++ b/drivers/can/Kconfig.loopback @@ -0,0 +1,44 @@ +# Kconfig.loopback - CAN loopback device configuration options + +# +# Copyright (c) 2019 Alexander Wachter +# +# SPDX-License-Identifier: Apache-2.0 +# + +config CAN_LOOPBACK + bool "Loopback CAN driver" + help + This is a dummy driver that can only loopback messages. + +if CAN_LOOPBACK + +config CAN_LOOPBACK_DEV_NAME + string "CAN loopback device name" + default "CAN_LOOPBACK" + help + "Device name for the loopback device" + +config CAN_MAX_FILTER + int "Maximum number of concurrent active filters" + default 5 + range 1 1024 + help + Defines the array size of the filters. + Must be at least the size of concurrent reads. + +config CAN_LOOPBACK_FRAME_BUF_SIZE + int "Frame buffer count" + range 1 65534 + default 4 + help + Defines the number of frames that can be buffered for each filter. + +config CAN_RECEIVER_WORKQ_PRIO + int "Priority of the receiver workq" + default 2 + help + Specify the priority for the receiver workq. This workq calls the + receiver callbacks. + +endif # CAN_LOOPBACK diff --git a/drivers/can/can_loopback.c b/drivers/can/can_loopback.c new file mode 100644 index 00000000000..747b18a98ab --- /dev/null +++ b/drivers/can/can_loopback.c @@ -0,0 +1,218 @@ +/* + * Copyright (c) 2018 Alexander Wachter + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#include +#include +#include +#include +#include "can_loopback.h" + +#include +LOG_MODULE_DECLARE(can_driver, CONFIG_CAN_LOG_LEVEL); + +static void dispatch_frame(const struct zcan_frame *frame, + struct can_loopback_filter *filter) +{ + struct zcan_frame frame_tmp = *frame; + + filter->rx_cb(&frame_tmp, filter->cb_arg); +} + +static inline int check_filter_match(const struct zcan_frame *frame, + const struct zcan_filter *filter) +{ + u32_t id, mask, frame_id; + + frame_id = frame->id_type == CAN_STANDARD_IDENTIFIER ? + frame->std_id : frame->ext_id; + id = filter->id_type == CAN_STANDARD_IDENTIFIER ? + filter->std_id : filter->ext_id; + mask = filter->id_type == CAN_STANDARD_IDENTIFIER ? + filter->std_id_mask : filter->ext_id_mask; + + return ((id & mask) == (frame_id & mask)); +} + +int can_loopback_send(struct device *dev, const struct zcan_frame *frame, + s32_t timeout, can_tx_callback_t callback, + void *callback_arg) +{ + struct can_loopback_data *data = DEV_DATA(dev); + struct can_loopback_filter *filter; + + LOG_DBG("Sending %d bytes on %s. " + "Id: 0x%x, " + "ID type: %s, " + "Remote Frame: %s" + , frame->dlc, dev->config->name + , frame->id_type == CAN_STANDARD_IDENTIFIER ? + frame->std_id : frame->ext_id + , frame->id_type == CAN_STANDARD_IDENTIFIER ? + "standard" : "extended" + , frame->rtr == CAN_DATAFRAME ? "no" : "yes"); + + if (!data->loopback) { + + return 0; + } + + k_mutex_lock(&data->mtx, timeout); + for (int i = 0; i < CONFIG_CAN_MAX_FILTER; i++) { + filter = &data->filters[i]; + if (filter->rx_cb) { + if (check_filter_match(frame, &filter->filter)) { + dispatch_frame(frame, filter); + } + } + } + + k_mutex_unlock(&data->mtx); + + if (callback) { + callback(CAN_TX_OK, callback_arg); + } + + return 0; +} + + +static inline int get_free_filter(struct can_loopback_filter *filters) +{ + for (int i = 0; i < CONFIG_CAN_MAX_FILTER; i++) { + if (filters[i].rx_cb == NULL) { + return i; + } + } + + return CAN_NO_FREE_FILTER; +} + +int can_loopback_attach_isr(struct device *dev, can_rx_callback_t isr, + void *cb_arg, + const struct zcan_filter *filter) +{ + struct can_loopback_data *data = DEV_DATA(dev); + struct can_loopback_filter *loopback_filter; + int filter_id; + + LOG_DBG("Setting filter ID: 0x%x, mask: 0x%x", filter->ext_id, + filter->ext_id_mask); + LOG_DBG("Filter type: %s ID %s mask", + filter->id_type == CAN_STANDARD_IDENTIFIER ? + "standard" : "extended", + ((filter->id_type && (filter->std_id_mask == CAN_STD_ID_MASK)) || + (!filter->id_type && (filter->ext_id_mask == CAN_EXT_ID_MASK))) ? + "with" : "without"); + + k_mutex_lock(&data->mtx, K_FOREVER); + filter_id = get_free_filter(data->filters); + + if (filter_id < 0) { + LOG_ERR("No free filter left"); + k_mutex_unlock(&data->mtx); + return filter_id; + } + + loopback_filter = &data->filters[filter_id]; + + loopback_filter->rx_cb = isr; + loopback_filter->cb_arg = cb_arg; + loopback_filter->filter = *filter; + k_mutex_unlock(&data->mtx); + + LOG_DBG("Filter attached. ID: %d", filter_id); + + return filter_id; +} + +void can_loopback_detach(struct device *dev, int filter_id) +{ + struct can_loopback_data *data = DEV_DATA(dev); + + LOG_DBG("Detach filter ID: %d", filter_id); + k_mutex_lock(&data->mtx, K_FOREVER); + data->filters[filter_id].rx_cb = NULL; + k_mutex_unlock(&data->mtx); +} + +int can_loopback_configure(struct device *dev, enum can_mode mode, + u32_t bitrate) +{ + struct can_loopback_data *data = DEV_DATA(dev); + + ARG_UNUSED(bitrate); + + data->loopback = mode == CAN_LOOPBACK_MODE ? 1 : 0; + return 0; +} + +static const struct can_driver_api can_api_funcs = { + .configure = can_loopback_configure, + .send = can_loopback_send, + .attach_isr = can_loopback_attach_isr, + .detach = can_loopback_detach +}; + + +static int can_loopback_init(struct device *dev) +{ + struct can_loopback_data *data = DEV_DATA(dev); + + k_mutex_init(&data->mtx); + + for (int i = 0; i < CONFIG_CAN_MAX_FILTER; i++) { + data->filters[i].rx_cb = NULL; + } + + LOG_INF("Init of %s done", dev->config->name); + return 0; +} + +#ifdef CONFIG_CAN_1 + +static struct can_loopback_data can_loopback_dev_data_1; + +DEVICE_AND_API_INIT(can_loopback_1, CONFIG_CAN_LOOPBACK_DEV_NAME, + &can_loopback_init, + &can_loopback_dev_data_1, NULL, + POST_KERNEL, CONFIG_KERNEL_INIT_PRIORITY_DEVICE, + &can_api_funcs); + + +#if defined(CONFIG_NET_SOCKETS_CAN) + +#include "socket_can_generic.h" + +static int socket_can_init_1(struct device *dev) +{ + struct device *can_dev = DEVICE_GET(can_loopback_1); + struct socket_can_context *socket_context = dev->driver_data; + + LOG_DBG("Init socket CAN device %p (%s) for dev %p (%s)", + dev, dev->config->name, can_dev, can_dev->config->name); + + socket_context->can_dev = can_dev; + socket_context->msgq = &socket_can_msgq; + + socket_context->rx_tid = + k_thread_create(&socket_context->rx_thread_data, + rx_thread_stack, + K_THREAD_STACK_SIZEOF(rx_thread_stack), + rx_thread, socket_context, NULL, NULL, + RX_THREAD_PRIORITY, 0, K_NO_WAIT); + + return 0; +} + +NET_DEVICE_INIT(socket_can_loopback_1, SOCKET_CAN_NAME_1, socket_can_init_1, + &socket_can_context_1, NULL, + CONFIG_KERNEL_INIT_PRIORITY_DEVICE, + &socket_can_api, + CANBUS_L2, NET_L2_GET_CTX_TYPE(CANBUS_L2), CAN_MTU); + +#endif /* CONFIG_NET_SOCKETS_CAN */ + +#endif /*CONFIG_CAN_1*/ diff --git a/drivers/can/can_loopback.h b/drivers/can/can_loopback.h new file mode 100644 index 00000000000..4c76d942bc5 --- /dev/null +++ b/drivers/can/can_loopback.h @@ -0,0 +1,32 @@ +/* + * Copyright (c) 2019 Alexander Wachter + * + * SPDX-License-Identifier: Apache-2.0 + * + */ + +#ifndef ZEPHYR_DRIVERS_CAN_LOOPBACK_CAN_H_ +#define ZEPHYR_DRIVERS_CAN_LOOPBACK_CAN_H_ + +#include + +#define DEV_DATA(dev) ((struct can_loopback_data *const)(dev)->driver_data) +#define DEV_CFG(dev) \ + ((const struct can_loopback_config *const)(dev)->config->config_info) + +struct can_loopback_filter { + can_rx_callback_t rx_cb; + void *cb_arg; + struct zcan_filter filter; +}; + +struct can_loopback_data { + struct can_loopback_filter filters[CONFIG_CAN_MAX_FILTER]; + struct k_mutex mtx; + bool loopback; +}; + +struct can_loopback_config { +}; + +#endif /*ZEPHYR_DRIVERS_CAN_LOOPBACK_CAN_H_*/ diff --git a/include/can.h b/include/can.h index b4d6050b2ac..99bcfc914b6 100644 --- a/include/can.h +++ b/include/can.h @@ -241,7 +241,7 @@ typedef int (*can_attach_isr_t)(struct device *dev, can_rx_callback_t isr, typedef void (*can_detach_t)(struct device *dev, int filter_id); -#ifdef CONFIG_CAN_WORKQ_FRAMES_BUF_CNT +#ifndef CONFIG_CAN_WORKQ_FRAMES_BUF_CNT #define CONFIG_CAN_WORKQ_FRAMES_BUF_CNT 4 #endif struct can_frame_buffer {