drivers: can: Implement can_attach_workq (54d065c6) · Commits · Wenxi XU / zephyr

drivers/can/Kconfig

+7 −0

Original line number	Diff line number	Diff line
		@@ -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

drivers/can/can_common.c

+110 −2

Original line number	Diff line number	Diff line
		@@ -11,6 +11,17 @@
		#include <logging/log.h>
		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)
		@@ -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);
		}

include/can.h

+50 −0

Original line number	Diff line number	Diff line
		@@ -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.
		*

Admin message