diff --git a/drivers/can/CMakeLists.txt b/drivers/can/CMakeLists.txt index 7f598a0e272..4b4b337e997 100644 --- a/drivers/can/CMakeLists.txt +++ b/drivers/can/CMakeLists.txt @@ -1,5 +1,6 @@ # SPDX-License-Identifier: Apache-2.0 -zephyr_sources_ifdef(CONFIG_CAN_STM32 stm32_can.c) +zephyr_sources_ifdef(CONFIG_CAN can_common.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/can_common.c b/drivers/can/can_common.c new file mode 100644 index 00000000000..72dd1dd6ccd --- /dev/null +++ b/drivers/can/can_common.c @@ -0,0 +1,37 @@ +/* + * Copyright (c) 2019 Alexander Wachter + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#include +#include + +#define LOG_LEVEL CONFIG_CAN_LOG_LEVEL +#include +LOG_MODULE_REGISTER(can_driver); + +#define WORK_BUF_FULL 0xFFFF + +static void can_msgq_put(struct zcan_frame *frame, void *arg) +{ + struct k_msgq *msgq= (struct k_msgq*)arg; + int ret; + + __ASSERT_NO_MSG(msgq); + + 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->std_id : frame->ext_id); + } +} + +int z_impl_can_attach_msgq(struct device *dev, struct k_msgq *msg_q, + const struct zcan_filter *filter) +{ + const struct can_driver_api *api = dev->driver_api; + + return api->attach_isr(dev, can_msgq_put, msg_q, filter); +} diff --git a/drivers/can/can_handlers.c b/drivers/can/can_handlers.c index 476d2488c51..7e53971c6a4 100644 --- a/drivers/can/can_handlers.c +++ b/drivers/can/can_handlers.c @@ -36,7 +36,7 @@ Z_SYSCALL_HANDLER(can_send, dev, msg, timeout, callback_isr, callback_arg) { Z_SYSCALL_HANDLER(can_attach_msgq, dev, msgq, filter) { - Z_OOPS(Z_SYSCALL_DRIVER_CAN(dev, attach_msgq)); + Z_OOPS(Z_SYSCALL_OBJ(dev, K_OBJ_DRIVER_CAN)); Z_OOPS(Z_SYSCALL_MEMORY_READ((struct zcan_filter *)filter, sizeof(struct zcan_filter))); diff --git a/drivers/can/stm32_can.c b/drivers/can/stm32_can.c index 40be49291b3..c1a91f86f88 100644 --- a/drivers/can/stm32_can.c +++ b/drivers/can/stm32_can.c @@ -15,9 +15,8 @@ #include #include "stm32_can.h" -#define LOG_LEVEL CONFIG_CAN_LOG_LEVEL #include -LOG_MODULE_REGISTER(stm32_can); +LOG_MODULE_DECLARE(can_driver, CONFIG_CAN_LOG_LEVEL); /* * Translation tables @@ -59,6 +58,7 @@ void can_stm32_rx_isr_handler(CAN_TypeDef *can, struct can_stm32_data *data) CAN_FIFOMailBox_TypeDef *mbox; int filter_match_index; struct zcan_frame msg; + can_rx_callback_t callback; while (can->RF0R & CAN_RF0R_FMP0) { mbox = &can->sFIFOMailBox[0]; @@ -72,17 +72,10 @@ void can_stm32_rx_isr_handler(CAN_TypeDef *can, struct can_stm32_data *data) LOG_DBG("Message on filter index %d", filter_match_index); can_stm32_get_msg_fifo(mbox, &msg); - if (data->rx_response[filter_match_index]) { - if (data->response_type & (1ULL << filter_match_index)) { - struct k_msgq *msg_q = - data->rx_response[filter_match_index]; + callback = data->rx_cb[filter_match_index]; - k_msgq_put(msg_q, &msg, K_NO_WAIT); - } else { - can_rx_callback_t callback = - data->rx_response[filter_match_index]; - callback(&msg, data->cb_arg[filter_match_index]); - } + if (callback) { + callback(&msg, data->cb_arg[filter_match_index]); } /* Release message */ @@ -469,22 +462,6 @@ static int can_stm32_shift_arr(void **arr, int start, int count) return 0; } -static inline void can_stm32_shift_bits(u64_t *bits, int start, int count) -{ - u64_t mask_right; - u64_t mask_left; - - if (count > 0) { - mask_right = ~(UINT64_MAX << start); - *bits = (*bits & mask_right) | ((*bits & ~mask_right) << count); - } else if (count < 0) { - count = -count; - mask_left = UINT64_MAX << start; - mask_right = ~(UINT64_MAX << (start - count)); - *bits = (*bits & mask_right) | ((*bits & mask_left) >> count); - } -} - enum can_filter_type can_stm32_get_filter_type(int bank_nr, u32_t mode_reg, u32_t scale_reg) { @@ -709,7 +686,7 @@ static inline int can_stm32_set_filter(const struct zcan_filter *filter, start_index = filter_index_new + filter_in_bank[bank_mode]; if (shift_width && start_index <= CAN_MAX_NUMBER_OF_FILTERS) { - res = can_stm32_shift_arr(device_data->rx_response, + res = can_stm32_shift_arr((void **)device_data->rx_cb, start_index, shift_width); @@ -722,10 +699,6 @@ static inline int can_stm32_set_filter(const struct zcan_filter *filter, filter_nr = CAN_NO_FREE_FILTER; goto done; } - - can_stm32_shift_bits(&device_data->response_type, - start_index, - shift_width); } can->FM1R = mode_reg; @@ -750,39 +723,22 @@ done: return filter_nr; } - -static inline int can_stm32_attach(struct device *dev, void *response_ptr, +static inline int can_stm32_attach(struct device *dev, can_rx_callback_t cb, void *cb_arg, - const struct zcan_filter *filter, - int *filter_index) + const struct zcan_filter *filter) { const struct can_stm32_config *cfg = DEV_CFG(dev); struct can_stm32_data *data = DEV_DATA(dev); CAN_TypeDef *can = cfg->can; - int filter_index_tmp = 0; + int filter_index = 0; int filter_nr; - filter_nr = can_stm32_set_filter(filter, data, can, &filter_index_tmp); + filter_nr = can_stm32_set_filter(filter, data, can, &filter_index); if (filter_nr != CAN_NO_FREE_FILTER) { - data->rx_response[filter_index_tmp] = response_ptr; - data->cb_arg[filter_index_tmp] = cb_arg; + data->rx_cb[filter_index] = cb; + data->cb_arg[filter_index] = cb_arg; } - *filter_index = filter_index_tmp; - return filter_nr; -} - -int can_stm32_attach_msgq(struct device *dev, struct k_msgq *msgq, - const struct zcan_filter *filter) -{ - int filter_nr; - int filter_index; - struct can_stm32_data *data = DEV_DATA(dev); - - k_mutex_lock(&data->set_filter_mutex, K_FOREVER); - filter_nr = can_stm32_attach(dev, msgq, NULL, filter, &filter_index); - data->response_type |= (1ULL << filter_index); - k_mutex_unlock(&data->set_filter_mutex); return filter_nr; } @@ -792,11 +748,9 @@ int can_stm32_attach_isr(struct device *dev, can_rx_callback_t isr, { struct can_stm32_data *data = DEV_DATA(dev); int filter_nr; - int filter_index; k_mutex_lock(&data->set_filter_mutex, K_FOREVER); - filter_nr = can_stm32_attach(dev, isr, cb_arg, filter, &filter_index); - data->response_type &= ~(1ULL << filter_index); + filter_nr = can_stm32_attach(dev, isr, cb_arg, filter); k_mutex_unlock(&data->set_filter_mutex); return filter_nr; } @@ -845,9 +799,8 @@ void can_stm32_detach(struct device *dev, int filter_nr) } can->FMR &= ~(CAN_FMR_FINIT); - data->rx_response[filter_index] = NULL; + data->rx_cb[filter_index] = NULL; data->cb_arg[filter_index] = NULL; - data->response_type &= ~(1ULL << filter_index); k_mutex_unlock(&data->set_filter_mutex); } @@ -855,7 +808,6 @@ void can_stm32_detach(struct device *dev, int filter_nr) static const struct can_driver_api can_api_funcs = { .configure = can_stm32_runtime_configure, .send = can_stm32_send, - .attach_msgq = can_stm32_attach_msgq, .attach_isr = can_stm32_attach_isr, .detach = can_stm32_detach }; diff --git a/drivers/can/stm32_can.h b/drivers/can/stm32_can.h index 3a4d6bbc875..48921895868 100644 --- a/drivers/can/stm32_can.h +++ b/drivers/can/stm32_can.h @@ -63,8 +63,7 @@ struct can_stm32_data { struct can_mailbox mb1; struct can_mailbox mb2; u64_t filter_usage; - u64_t response_type; - void *rx_response[CONFIG_CAN_MAX_FILTER]; + can_rx_callback_t rx_cb[CONFIG_CAN_MAX_FILTER]; void *cb_arg[CONFIG_CAN_MAX_FILTER]; }; diff --git a/include/can.h b/include/can.h index 1bea3e68595..fb27fa95147 100644 --- a/include/can.h +++ b/include/can.h @@ -245,7 +245,6 @@ struct can_driver_api { can_configure_t configure; can_send_t send; can_attach_isr_t attach_isr; - can_attach_msgq_t attach_msgq; can_detach_t detach; }; @@ -348,16 +347,6 @@ static inline int can_write(struct device *dev, const u8_t *data, u8_t length, __syscall int can_attach_msgq(struct device *dev, struct k_msgq *msg_q, const struct zcan_filter *filter); -static inline int z_impl_can_attach_msgq(struct device *dev, - struct k_msgq *msg_q, - const struct zcan_filter *filter) -{ - const struct can_driver_api *api = - (const struct can_driver_api *)dev->driver_api; - - return api->attach_msgq(dev, msg_q, filter); -} - /** * @brief Attach an isr callback function to a single or group of identifiers. *