From ec0e19920657e0b074d424410381d12b788494c9 Mon Sep 17 00:00:00 2001 From: Alexander Wachter Date: Wed, 14 Aug 2019 16:04:58 +0200 Subject: [PATCH] drivers: can: mcux: Persuade the driver to send in chronological order The mcux CAN controller uses frame ID and position to calculate the priority, but the driver expects chronological ordering. This PR mimics this behavior by only using the last free message box. Signed-off-by: Alexander Wachter --- drivers/can/can_mcux_flexcan.c | 59 ++++++++++++++++++++++++++++++---- 1 file changed, 52 insertions(+), 7 deletions(-) diff --git a/drivers/can/can_mcux_flexcan.c b/drivers/can/can_mcux_flexcan.c index b31b597324..1ed9911f77 100644 --- a/drivers/can/can_mcux_flexcan.c +++ b/drivers/can/can_mcux_flexcan.c @@ -25,6 +25,8 @@ LOG_MODULE_REGISTER(can_mcux_flexcan); (FSL_FEATURE_FLEXCAN_HAS_MESSAGE_BUFFER_MAX_NUMBERn(0) \ - MCUX_FLEXCAN_MAX_RX) +#define MCUX_N_TX_ALLOC_ELEM (1 + (MCUX_FLEXCAN_MAX_TX - 1) / ATOMIC_BITS) + /* * Convert from RX message buffer index to allocated filter ID and * vice versa. @@ -195,6 +197,45 @@ static void mcux_flexcan_copy_zfilter_to_mbconfig(const struct zcan_filter *src, } } +/* mcux_get_tx_alloc is a linear on array, and binary on atomic_val_t search + * for the highest bit set in data->tx_allocs. 0 is returned in case of an empty + * tx_alloc, the next free bit otherwise. + * The reason to always use a higher buffer number than the current in use is + * that a FIFO manner is kept. The Controller would otherwise send the frame + * that is in the lowest buffer number first. + */ +static int mcux_get_tx_alloc(struct mcux_flexcan_data *data) +{ + atomic_val_t *allocs = data->tx_allocs; + atomic_val_t pivot = ATOMIC_BITS / 2; + atomic_val_t alloc, mask; + int i; + + for (i = MCUX_N_TX_ALLOC_ELEM - 1; i >= 0; i--) { + alloc = allocs[i]; + if (alloc) { + for (atomic_val_t bits = ATOMIC_BITS / 2U; + bits; bits >>= 1) { + mask = GENMASK(pivot + bits - 1, pivot); + if (alloc & mask) { + pivot += bits / 2U; + } else { + pivot -= bits / 2U; + } + } + + if (!(alloc & mask)) { + pivot--; + } + + break; + } + } + + alloc = alloc ? (pivot + 1 + i * ATOMIC_BITS) : 0; + return alloc >= MCUX_FLEXCAN_MAX_TX ? -1 : alloc; +} + static int mcux_flexcan_send(struct device *dev, const struct zcan_frame *msg, s32_t timeout, can_tx_callback_t callback_isr, void *callback_arg) @@ -205,14 +246,19 @@ static int mcux_flexcan_send(struct device *dev, const struct zcan_frame *msg, status_t status; int alloc; - if (k_sem_take(&data->tx_allocs_sem, timeout) != 0) { - return CAN_TIMEOUT; - } + while (true) { + alloc = mcux_get_tx_alloc(data); + if (alloc >= 0) { + if (atomic_test_and_set_bit(data->tx_allocs, alloc)) { + continue; + } - for (alloc = 0; alloc < MCUX_FLEXCAN_MAX_TX; alloc++) { - if (!atomic_test_and_set_bit(data->tx_allocs, alloc)) { break; } + + if (k_sem_take(&data->tx_allocs_sem, timeout) != 0) { + return CAN_TIMEOUT; + } } mcux_flexcan_copy_zframe_to_frame(msg, &data->tx_cbs[alloc].frame); @@ -488,8 +534,7 @@ static int mcux_flexcan_init(struct device *dev) int i; k_mutex_init(&data->rx_mutex); - k_sem_init(&data->tx_allocs_sem, MCUX_FLEXCAN_MAX_TX, - MCUX_FLEXCAN_MAX_TX); + k_sem_init(&data->tx_allocs_sem, 0, 1); for (i = 0; i < ARRAY_SIZE(data->tx_cbs); i++) { k_sem_init(&data->tx_cbs[i].done, 0, 1);