diff --git a/drivers/can/Kconfig b/drivers/can/Kconfig index 73155db7851..81a6dd8d433 100644 --- a/drivers/can/Kconfig +++ b/drivers/can/Kconfig @@ -29,6 +29,13 @@ config CAN_INIT_PRIORITY Note that the priority needs to be lower than the net stack so that it can start before the networking sub-system. +config CAN_WORKQ_FRAMES_BUF_CNT + int "Work queue buffer frame count" + default 4 + range 1 65534 + help + Number of frames in the buffer of a zcan_work. + config CAN_1 bool "Enable CAN 1" default y diff --git a/drivers/can/can_common.c b/drivers/can/can_common.c index 72dd1dd6ccd..e13858eab73 100644 --- a/drivers/can/can_common.c +++ b/drivers/can/can_common.c @@ -11,11 +11,22 @@ #include LOG_MODULE_REGISTER(can_driver); +#define WORK_BUF_COUNT_IS_POWER_OF_2 !(CONFIG_CAN_WORKQ_FRAMES_BUF_CNT & \ + (CONFIG_CAN_WORKQ_FRAMES_BUF_CNT - 1)) + +#define WORK_BUF_MOD_MASK (CONFIG_CAN_WORKQ_FRAMES_BUF_CNT - 1) + +#if WORK_BUF_COUNT_IS_POWER_OF_2 +#define WORK_BUF_MOD_SIZE(x) ((x) & WORK_BUF_MOD_MASK) +#else +#define WORK_BUF_MOD_SIZE(x) ((x) % CONFIG_CAN_WORKQ_FRAMES_BUF_CNT) +#endif + #define WORK_BUF_FULL 0xFFFF static void can_msgq_put(struct zcan_frame *frame, void *arg) { - struct k_msgq *msgq= (struct k_msgq*)arg; + struct k_msgq *msgq = (struct k_msgq *)arg; int ret; __ASSERT_NO_MSG(msgq); @@ -23,7 +34,7 @@ static void can_msgq_put(struct zcan_frame *frame, void *arg) ret = k_msgq_put(msgq, frame, K_NO_WAIT); if (ret) { LOG_ERR("Msgq %p overflowed. Frame ID: 0x%x", arg, - frame->id_type == CAN_STANDARD_IDENTIFIER ? + frame->id_type == CAN_STANDARD_IDENTIFIER ? frame->std_id : frame->ext_id); } } @@ -35,3 +46,100 @@ int z_impl_can_attach_msgq(struct device *dev, struct k_msgq *msg_q, return api->attach_isr(dev, can_msgq_put, msg_q, filter); } + +static inline void can_work_buffer_init(struct can_frame_buffer *buffer) +{ + buffer->head = 0; + buffer->tail = 0; +} + +static inline int can_work_buffer_put(struct zcan_frame *frame, + struct can_frame_buffer *buffer) +{ + u16_t next_head = WORK_BUF_MOD_SIZE(buffer->head + 1); + + if (buffer->head == WORK_BUF_FULL) { + return -1; + } + + buffer->buf[buffer->head] = *frame; + + /* Buffer is almost full */ + if (next_head == buffer->tail) { + buffer->head = WORK_BUF_FULL; + } else { + buffer->head = next_head; + } + + return 0; +} + +static inline +struct zcan_frame *can_work_buffer_get_next(struct can_frame_buffer *buffer) +{ + /* Buffer empty */ + if (buffer->head == buffer->tail) { + return NULL; + } else { + return &buffer->buf[buffer->tail]; + } +} + +static inline void can_work_buffer_free_next(struct can_frame_buffer *buffer) +{ + u16_t next_tail = WORK_BUF_MOD_SIZE(buffer->tail + 1); + + if (buffer->head == buffer->tail) { + return; + } + + if (buffer->head == WORK_BUF_FULL) { + buffer->head = buffer->tail; + } + + buffer->tail = next_tail; +} + +static void can_work_handler(struct k_work *work) +{ + struct zcan_work *can_work = CONTAINER_OF(work, struct zcan_work, + work_item); + struct zcan_frame *frame; + + while ((frame = can_work_buffer_get_next(&can_work->buf))) { + can_work->cb(frame, can_work->cb_arg); + can_work_buffer_free_next(&can_work->buf); + } +} + +static void can_work_isr_put(struct zcan_frame *frame, void *arg) +{ + struct zcan_work *work = (struct zcan_work *)arg; + int ret; + + ret = can_work_buffer_put(frame, &work->buf); + if (ret) { + LOG_ERR("Workq buffer overflow. Msg ID: 0x%x", + frame->id_type == CAN_STANDARD_IDENTIFIER ? + frame->std_id : frame->ext_id); + return; + } + + k_work_submit_to_queue(work->work_queue, &work->work_item); +} + +int can_attach_workq(struct device *dev, struct k_work_q *work_q, + struct zcan_work *work, + can_rx_callback_t callback, void *callback_arg, + const struct zcan_filter *filter) +{ + const struct can_driver_api *api = dev->driver_api; + + k_work_init(&work->work_item, can_work_handler); + work->work_queue = work_q; + work->cb = callback; + work->cb_arg = callback_arg; + can_work_buffer_init(&work->buf); + + return api->attach_isr(dev, can_work_isr_put, work, filter); +} diff --git a/include/can.h b/include/can.h index fb27fa95147..b4d6050b2ac 100644 --- a/include/can.h +++ b/include/can.h @@ -241,6 +241,28 @@ 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 +#define CONFIG_CAN_WORKQ_FRAMES_BUF_CNT 4 +#endif +struct can_frame_buffer { + struct zcan_frame buf[CONFIG_CAN_WORKQ_FRAMES_BUF_CNT]; + u16_t head; + u16_t tail; +}; + +/** + * @brief CAN work structure + * + * Used to attach a work queue to a filter. + */ +struct zcan_work { + struct k_work work_item; + struct k_work_q *work_queue; + struct can_frame_buffer buf; + can_rx_callback_t cb; + void *cb_arg; +}; + struct can_driver_api { can_configure_t configure; can_send_t send; @@ -325,6 +347,34 @@ static inline int can_write(struct device *dev, const u8_t *data, u8_t length, return can_send(dev, &msg, timeout, NULL, NULL); } +/** + * @brief Attach a CAN work queue to a single or group of identifiers. + * + * This routine attaches a work queue to identifiers specified by a filter. + * Whenever the filter matches, the message is pushed to the buffer + * of the zcan_work structure and the work element is put to the workqueue. + * If a message passes more than one filter the priority of the match + * is hardware dependent. + * A CAN work queue can be attached to more than one filter. + * The work queue must be initialized before and the caller must have + * appropriate permissions on it. + * + * @param dev Pointer to the device structure for the driver instance. + * @param work_q Pointer to the already initialized work queue. + * @param work Pointer to a zcan_work. The work will be initialized. + * @param callback This function is called by workq whenever a message arrives. + * @param callback_arg Is passed to the callback when called. + * @param filter Pointer to a zcan_filter structure defining the id + * filtering. + * + * @retval filter id on success. + * @retval CAN_NO_FREE_FILTER if there is no filter left. + */ +int can_attach_workq(struct device *dev, struct k_work_q *work_q, + struct zcan_work *work, + can_rx_callback_t callback, void *callback_arg, + const struct zcan_filter *filter); + /** * @brief Attach a message queue to a single or group of identifiers. *