michaelh/zephyr
1
0
Fork 0
Code Releases Activity

drivers: can: stm32: rework filter handling

Browse source 
The previous driver implementation aimed to maximize the amount of
filters, which resulted in a fairly complex implementation.

Background: The bxCAN assigns a number to the filters in the order they
appear in the filter banks. This number is used to match messages in
the FIFO with their filter. If the scale or mode of a filter bank is
changed, all following filter numbers get an offset and the assigned
callbacks have to be shifted accordingly.

The required additional space for the shifting operations resulted in
non-deterministic behaviour and the maximum number of filters could not
be determined at compile-time, which made several tests like
tests/drivers/can/api fail.

This implementation uses a more simple but reliable approach for
filtering and and reserves fixed space for extended and standard ID
filters in the available banks (configurable via Kconfig).
The list mode is not used, which may reduce the total number of usable
filters depending on the application.

The maximum amount of filters is 14 if all filters use ext. IDs,
28 if all use std IDs and something in between if mixed IDs are used.

Also see issue #47986 for more detailed background information.

Signed-off-by: Martin Jäger <martin@libre.solar>
This commit is contained in:
Martin Jäger 2022-07-23 08:01:10 +02:00 • committed by Fabio Baltieri
commit c56b1dbca1
3 changed files with 177 additions and 331 deletions
Download patch file Download diff file Expand all files Collapse all files

37
drivers/can/Kconfig.stm32
View file

@ -12,11 +12,34 @@ config CAN_STM32
Enable STM32 CAN Driver. Enable STM32 CAN Driver.
Tested on STM32F0, STM32F4, STM32L4 and STM32F7 series. Tested on STM32F0, STM32F4, STM32L4 and STM32F7 series.
config CAN_MAX_FILTER config CAN_MAX_STD_ID_FILTER
int "Maximum number of concurrent active filters" int "Maximum number of std ID filters"
depends on CAN_STM32 default 14
default 5 range 0 28
range 1 56
help help
Defines the array size of the callback pointers. Defines the maximum number of filters with standard ID (11-bit)
Must be at least the size of concurrent reads. that can be added by the application.
One standard ID filter with mask occupies 1/2 of the 14 available
filter banks.
The following equation determines the maximum total number of
filters:
CAN_MAX_STD_ID_FILTER + CAN_MAX_EXT_ID_FILTER * 2 <= 28
config CAN_MAX_EXT_ID_FILTER
int "Maximum number of ext ID filters"
default 7
range 0 14
help
Defines the maximum number of filters with extended ID (29-bit)
that can be added by the application.
One extended ID filter with mask occupies 1 of the 14 available
filter banks.
The following equation determines the maximum total number of
filters:
CAN_MAX_STD_ID_FILTER + CAN_MAX_EXT_ID_FILTER * 2 <= 28

450
drivers/can/can_stm32.c
View file

@ -1,5 +1,6 @@
/* /*
* Copyright (c) 2018 Alexander Wachter * Copyright (c) 2018 Alexander Wachter
* Copyright (c) 2022 Martin Jäger <martin@libre.solar>
* *
* SPDX-License-Identifier: Apache-2.0 * SPDX-License-Identifier: Apache-2.0
*/ */
@ -42,13 +43,10 @@ LOG_MODULE_REGISTER(can_stm32, CONFIG_CAN_LOG_LEVEL);
#error You must either set a sampling-point or timings (phase-seg* and prop-seg) #error You must either set a sampling-point or timings (phase-seg* and prop-seg)
#endif #endif
/* #if (CONFIG_CAN_MAX_STD_ID_FILTER + CONFIG_CAN_MAX_EXT_ID_FILTER * 2) > \
* Translation tables (CAN_STM32_NUM_FILTER_BANKS * 2)
* max_filters_per_bank[enum can_stm32_filter_type] = number of filters in bank for this type #error Number of configured filters exceeds available filter bank slots.
* filter_reg_demand[enum can_stm32_filter_type] = how many registers are used for this type #endif
*/
static const uint8_t max_filters_per_bank[] = {2, 4, 1, 2};
static const uint8_t filter_reg_demand[] = {2, 1, 4, 2};
/* /*
* Mutex to prevent simultaneous access to filter registers shared between CAN1 * Mutex to prevent simultaneous access to filter registers shared between CAN1
@ -90,26 +88,30 @@ static inline void can_stm32_rx_isr_handler(const struct device *dev)
const struct can_stm32_config *cfg = dev->config; const struct can_stm32_config *cfg = dev->config;
CAN_TypeDef *can = cfg->can; CAN_TypeDef *can = cfg->can;
CAN_FIFOMailBox_TypeDef *mbox; CAN_FIFOMailBox_TypeDef *mbox;
int filter_match_index; int filter_id, index;
struct zcan_frame frame; struct zcan_frame frame;
can_rx_callback_t callback; can_rx_callback_t callback = NULL;
void *cb_arg;
while (can->RF0R & CAN_RF0R_FMP0) { while (can->RF0R & CAN_RF0R_FMP0) {
mbox = &can->sFIFOMailBox[0]; mbox = &can->sFIFOMailBox[0];
filter_match_index = ((mbox->RDTR & CAN_RDT0R_FMI) filter_id = ((mbox->RDTR & CAN_RDT0R_FMI) >> CAN_RDT0R_FMI_Pos);
>> CAN_RDT0R_FMI_Pos);
if (filter_match_index >= CONFIG_CAN_MAX_FILTER) { LOG_DBG("Message on filter_id %d", filter_id);
break;
}
LOG_DBG("Message on filter index %d", filter_match_index);
can_stm32_rx_fifo_pop(mbox, &frame); can_stm32_rx_fifo_pop(mbox, &frame);
callback = data->rx_cb[filter_match_index]; if (filter_id < CONFIG_CAN_MAX_EXT_ID_FILTER) {
callback = data->rx_cb_ext[filter_id];
cb_arg = data->cb_arg_ext[filter_id];
} else if (filter_id < CAN_STM32_MAX_FILTER_ID) {
index = filter_id - CONFIG_CAN_MAX_EXT_ID_FILTER;
callback = data->rx_cb_std[index];
cb_arg = data->cb_arg_std[index];
}
if (callback) { if (callback) {
callback(dev, &frame, data->cb_arg[filter_match_index]); callback(dev, &frame, cb_arg);
} }
/* Release message */ /* Release message */
@ -482,6 +484,17 @@ static int can_stm32_get_max_bitrate(const struct device *dev, uint32_t *max_bit
return 0; return 0;
} }
static int can_stm32_get_max_filters(const struct device *dev, enum can_ide id_type)
{
ARG_UNUSED(dev);
if (id_type == CAN_STANDARD_IDENTIFIER) {
return CONFIG_CAN_MAX_STD_ID_FILTER;
} else {
return CONFIG_CAN_MAX_EXT_ID_FILTER;
}
}
static int can_stm32_init(const struct device *dev) static int can_stm32_init(const struct device *dev)
{ {
const struct can_stm32_config *cfg = dev->config; const struct can_stm32_config *cfg = dev->config;
@ -489,6 +502,7 @@ static int can_stm32_init(const struct device *dev)
CAN_TypeDef *can = cfg->can; CAN_TypeDef *can = cfg->can;
struct can_timing timing; struct can_timing timing;
const struct device *clock; const struct device *clock;
uint32_t bank_offset;
int ret; int ret;
k_mutex_init(&filter_mutex); k_mutex_init(&filter_mutex);
@ -497,15 +511,6 @@ static int can_stm32_init(const struct device *dev)
k_sem_init(&data->mb0.tx_int_sem, 0, 1); k_sem_init(&data->mb0.tx_int_sem, 0, 1);
k_sem_init(&data->mb1.tx_int_sem, 0, 1); k_sem_init(&data->mb1.tx_int_sem, 0, 1);
k_sem_init(&data->mb2.tx_int_sem, 0, 1); k_sem_init(&data->mb2.tx_int_sem, 0, 1);
data->mb0.tx_callback = NULL;
data->mb1.tx_callback = NULL;
data->mb2.tx_callback = NULL;
data->state_change_cb = NULL;
data->state_change_cb_data = NULL;
data->filter_usage = (1ULL << CAN_STM32_MAX_NUM_FILTERS) - 1ULL;
(void)memset(data->rx_cb, 0, sizeof(data->rx_cb));
(void)memset(data->cb_arg, 0, sizeof(data->cb_arg));
if (cfg->phy != NULL) { if (cfg->phy != NULL) {
if (!device_is_ready(cfg->phy)) { if (!device_is_ready(cfg->phy)) {
@ -541,6 +546,12 @@ static int can_stm32_init(const struct device *dev)
return ret; return ret;
} }
/* configure scale of filter banks < CONFIG_CAN_MAX_EXT_ID_FILTER for ext ids */
bank_offset = (cfg->can == cfg->master_can) ? 0 : CAN_STM32_NUM_FILTER_BANKS;
cfg->master_can->FMR |= CAN_FMR_FINIT;
cfg->master_can->FS1R |= ((1U << CONFIG_CAN_MAX_EXT_ID_FILTER) - 1) << bank_offset;
cfg->master_can->FMR &= ~CAN_FMR_FINIT;
can->MCR &= ~CAN_MCR_TTCM & ~CAN_MCR_ABOM & ~CAN_MCR_AWUM & can->MCR &= ~CAN_MCR_TTCM & ~CAN_MCR_ABOM & ~CAN_MCR_AWUM &
~CAN_MCR_NART & ~CAN_MCR_RFLM & ~CAN_MCR_TXFP; ~CAN_MCR_NART & ~CAN_MCR_RFLM & ~CAN_MCR_TXFP;
#ifdef CONFIG_CAN_RX_TIMESTAMP #ifdef CONFIG_CAN_RX_TIMESTAMP
@ -584,7 +595,7 @@ static int can_stm32_init(const struct device *dev)
cfg->config_irq(can); cfg->config_irq(can);
can->IER |= CAN_IER_TMEIE; can->IER |= CAN_IER_TMEIE;
LOG_INF("Init of %s done", dev->name);
return 0; return 0;
} }
@ -693,15 +704,15 @@ static int can_stm32_send(const struct device *dev, const struct zcan_frame *fra
} }
if (transmit_status_register & CAN_TSR_TME0) { if (transmit_status_register & CAN_TSR_TME0) {
LOG_DBG("Using mailbox 0"); LOG_DBG("Using TX mailbox 0");
mailbox = &can->sTxMailBox[0]; mailbox = &can->sTxMailBox[0];
mb = &(data->mb0); mb = &(data->mb0);
} else if (transmit_status_register & CAN_TSR_TME1) { } else if (transmit_status_register & CAN_TSR_TME1) {
LOG_DBG("Using mailbox 1"); LOG_DBG("Using TX mailbox 1");
mailbox = &can->sTxMailBox[1]; mailbox = &can->sTxMailBox[1];
mb = &data->mb1; mb = &data->mb1;
} else if (transmit_status_register & CAN_TSR_TME2) { } else if (transmit_status_register & CAN_TSR_TME2) {
LOG_DBG("Using mailbox 2"); LOG_DBG("Using TX mailbox 2");
mailbox = &can->sTxMailBox[2]; mailbox = &can->sTxMailBox[2];
mb = &data->mb2; mb = &data->mb2;
} }
@ -741,150 +752,26 @@ static int can_stm32_send(const struct device *dev, const struct zcan_frame *fra
return 0; return 0;
} }
static int can_stm32_shift_arr(void **arr, int start, int count) static void can_stm32_set_filter_bank(int filter_id, CAN_FilterRegister_TypeDef *filter_reg,
enum can_ide type, uint32_t id, uint32_t mask)
{ {
void **start_ptr = arr + start; switch (type) {
size_t cnt; case CAN_STANDARD_IDENTIFIER:
if ((filter_id - CONFIG_CAN_MAX_EXT_ID_FILTER) % 2 == 0) {
if (start > CONFIG_CAN_MAX_FILTER) { /* even std filter id: first 1/2 bank */
return -ENOSPC;
}
if (count > 0) {
void *move_dest;
/* Check if nothing used will be overwritten */
for (int i = CONFIG_CAN_MAX_FILTER - count; i <= CONFIG_CAN_MAX_FILTER - 1; i++) {
if (arr[i] != NULL) {
return -ENOSPC;
}
}
/* No need to shift. Destination is already outside the arr */
if ((start + count) >= CONFIG_CAN_MAX_FILTER) {
return 0;
}
cnt = (CONFIG_CAN_MAX_FILTER - start - count) * sizeof(void *);
move_dest = start_ptr + count;
memmove(move_dest, start_ptr, cnt);
(void)memset(start_ptr, 0, count * sizeof(void *));
} else if (count < 0) {
count = -count;
if (start - count < 0) {
return -ENOSPC;
}
cnt = (CONFIG_CAN_MAX_FILTER - start) * sizeof(void *);
memmove(start_ptr - count, start_ptr, cnt);
(void)memset(arr + CONFIG_CAN_MAX_FILTER - count, 0,
count * sizeof(void *));
}
return 0;
}
static enum can_stm32_filter_type can_stm32_get_filter_type(int bank_num, uint32_t mode_reg,
uint32_t scale_reg)
{
uint32_t mode_masked = (mode_reg >> bank_num) & 0x01;
uint32_t scale_masked = (scale_reg >> bank_num) & 0x01;
enum can_stm32_filter_type type = (scale_masked << 1) | mode_masked;
return type;
}
static int can_stm32_calc_filter_index(int filter_id, int bank_offset, uint32_t mode_reg,
uint32_t scale_reg)
{
int filter_bank = bank_offset + filter_id / 4;
int cnt = 0;
uint32_t mode_masked, scale_masked;
enum can_stm32_filter_type filter_type;
/* count filters in the banks before this bank */
for (int i = bank_offset; i < filter_bank; i++) {
filter_type = can_stm32_get_filter_type(i, mode_reg, scale_reg);
cnt += max_filters_per_bank[filter_type];
}
/* plus the filters in the same bank */
mode_masked = mode_reg & (1U << filter_bank);
scale_masked = scale_reg & (1U << filter_bank);
cnt += (!scale_masked && mode_masked) ? filter_id & 0x03 :
(filter_id & 0x03) >> 1;
return cnt;
}
static void can_stm32_set_filter_bank(int filter_id,
CAN_FilterRegister_TypeDef *filter_reg,
enum can_stm32_filter_type filter_type,
uint32_t id, uint32_t mask)
{
switch (filter_type) {
case CAN_STM32_FILTER_STANDARD:
switch (filter_id & 0x03) {
case 0:
filter_reg->FR1 = (filter_reg->FR1 & 0xFFFF0000) | id;
break;
case 1:
filter_reg->FR1 = (filter_reg->FR1 & 0x0000FFFF)
| (id << 16);
break;
case 2:
filter_reg->FR2 = (filter_reg->FR2 & 0xFFFF0000) | id;
break;
case 3:
filter_reg->FR2 = (filter_reg->FR2 & 0x0000FFFF)
| (id << 16);
break;
}
break;
case CAN_STM32_FILTER_STANDARD_MASKED:
switch (filter_id & 0x02) {
case 0:
filter_reg->FR1 = id | (mask << 16); filter_reg->FR1 = id | (mask << 16);
break; } else {
case 2: /* uneven std filter id: first 1/2 bank */
filter_reg->FR2 = id | (mask << 16); filter_reg->FR2 = id | (mask << 16);
break;
} }
break; break;
case CAN_STM32_FILTER_EXTENDED: case CAN_EXTENDED_IDENTIFIER:
switch (filter_id & 0x02) {
case 0:
filter_reg->FR1 = id;
break;
case 2:
filter_reg->FR2 = id;
break;
}
break;
case CAN_STM32_FILTER_EXTENDED_MASKED:
filter_reg->FR1 = id; filter_reg->FR1 = id;
filter_reg->FR2 = mask; filter_reg->FR2 = mask;
break; break;
} }
} }
static inline void can_stm32_set_filter_type(enum can_stm32_filter_type filter_type,
uint32_t *mode_reg, uint32_t *scale_reg,
int bank_num)
{
uint32_t mode_reg_bit = (filter_type & 0x01) << bank_num;
uint32_t scale_reg_bit = (filter_type >> 1) << bank_num;
*mode_reg &= ~(1 << bank_num);
*mode_reg |= mode_reg_bit;
*scale_reg &= ~(1 << bank_num);
*scale_reg |= scale_reg_bit;
}
static inline uint32_t can_stm32_filter_to_std_mask(const struct zcan_filter *filter) static inline uint32_t can_stm32_filter_to_std_mask(const struct zcan_filter *filter)
{ {
return (filter->id_mask << CAN_STM32_FIRX_STD_ID_POS) | return (filter->id_mask << CAN_STM32_FIRX_STD_ID_POS) |
@ -913,22 +800,16 @@ static inline uint32_t can_stm32_filter_to_ext_id(const struct zcan_filter *filt
(1U << CAN_STM32_FIRX_EXT_IDE_POS); (1U << CAN_STM32_FIRX_EXT_IDE_POS);
} }
static inline int can_stm32_set_filter(const struct device *dev, const struct zcan_filter *filter, static inline int can_stm32_set_filter(const struct device *dev, const struct zcan_filter *filter)
int *filter_index)
{ {
const struct can_stm32_config *cfg = dev->config; const struct can_stm32_config *cfg = dev->config;
struct can_stm32_data *device_data = dev->data; struct can_stm32_data *data = dev->data;
CAN_TypeDef *can = cfg->master_can; CAN_TypeDef *can = cfg->master_can;
uint32_t mask = 0U; uint32_t mask = 0U;
uint32_t id = 0U; uint32_t id = 0U;
int filter_id = 0; int filter_id = -ENOSPC;
int filter_index_new = -ENOSPC;
int bank_num;
int bank_offset = 0; int bank_offset = 0;
uint32_t bank_bit; int bank_num;
int register_demand;
enum can_stm32_filter_type filter_type;
enum can_stm32_filter_type bank_type;
if (cfg->can != cfg->master_can) { if (cfg->can != cfg->master_can) {
/* CAN slave instance: start with offset */ /* CAN slave instance: start with offset */
@ -936,134 +817,77 @@ static inline int can_stm32_set_filter(const struct device *dev, const struct zc
} }
if (filter->id_type == CAN_STANDARD_IDENTIFIER) { if (filter->id_type == CAN_STANDARD_IDENTIFIER) {
id = can_stm32_filter_to_std_id(filter); for (int i = 0; i < CONFIG_CAN_MAX_STD_ID_FILTER; i++) {
filter_type = CAN_STM32_FILTER_STANDARD; if (data->rx_cb_std[i] == NULL) {
id = can_stm32_filter_to_std_id(filter);
if (filter->id_mask != CAN_STD_ID_MASK) { mask = can_stm32_filter_to_std_mask(filter);
mask = can_stm32_filter_to_std_mask(filter); filter_id = CONFIG_CAN_MAX_EXT_ID_FILTER + i;
filter_type = CAN_STM32_FILTER_STANDARD_MASKED; bank_num = bank_offset + CONFIG_CAN_MAX_EXT_ID_FILTER + i / 2;
} break;
} else {
id = can_stm32_filter_to_ext_id(filter);
filter_type = CAN_STM32_FILTER_EXTENDED;
if (filter->id_mask != CAN_EXT_ID_MASK) {
mask = can_stm32_filter_to_ext_mask(filter);
filter_type = CAN_STM32_FILTER_EXTENDED_MASKED;
}
}
register_demand = filter_reg_demand[filter_type];
LOG_DBG("Setting filter ID: 0x%x, mask: 0x%x", filter->id,
filter->id_mask);
LOG_DBG("Filter type: %s ID %s mask (%d)",
(filter_type == CAN_STM32_FILTER_STANDARD ||
filter_type == CAN_STM32_FILTER_STANDARD_MASKED) ?
"standard" : "extended",
(filter_type == CAN_STM32_FILTER_STANDARD_MASKED ||
filter_type == CAN_STM32_FILTER_EXTENDED_MASKED) ?
"with" : "without",
filter_type);
do {
uint64_t usage_shifted = (device_data->filter_usage >> filter_id);
uint64_t usage_demand_mask = (1ULL << register_demand) - 1;
bool bank_is_empty;
bank_num = bank_offset + filter_id / 4;
bank_bit = (1U << bank_num);
bank_type = can_stm32_get_filter_type(bank_num, can->FM1R,
can->FS1R);
bank_is_empty = CAN_STM32_BANK_IS_EMPTY(device_data->filter_usage,
bank_num, bank_offset);
if (!bank_is_empty && bank_type != filter_type) {
filter_id = (filter_id / 4 + 1) * 4;
} else if (usage_shifted & usage_demand_mask) {
device_data->filter_usage &=
~(usage_demand_mask << filter_id);
break;
} else {
filter_id += register_demand;
}
if (!usage_shifted) {
LOG_INF("No free filter bank found");
return -ENOSPC;
}
} while (filter_id < CAN_STM32_MAX_NUM_FILTERS);
/* set the filter init mode */
can->FMR |= CAN_FMR_FINIT;
can->FA1R &= ~bank_bit;
/* TODO fifo balancing */
if (filter_type != bank_type) {
int shift_width, start_index;
int res;
uint32_t mode_reg = can->FM1R;
uint32_t scale_reg = can->FS1R;
can_stm32_set_filter_type(filter_type, &mode_reg, &scale_reg, bank_num);
shift_width = max_filters_per_bank[filter_type] - max_filters_per_bank[bank_type];
filter_index_new = can_stm32_calc_filter_index(filter_id, bank_offset,
mode_reg, scale_reg);
start_index = filter_index_new + max_filters_per_bank[bank_type];
if (shift_width && start_index <= CAN_STM32_MAX_NUM_FILTERS) {
res = can_stm32_shift_arr((void **)device_data->rx_cb,
start_index, shift_width);
res |= can_stm32_shift_arr(device_data->cb_arg,
start_index, shift_width);
if (filter_index_new >= CONFIG_CAN_MAX_FILTER || res) {
LOG_INF("No space for a new filter!");
filter_id = -ENOSPC;
goto done;
} }
} }
can->FM1R = mode_reg;
can->FS1R = scale_reg;
} else { } else {
filter_index_new = can_stm32_calc_filter_index(filter_id, bank_offset, for (int i = 0; i < CONFIG_CAN_MAX_EXT_ID_FILTER; i++) {
can->FM1R, can->FS1R); if (data->rx_cb_ext[i] == NULL) {
if (filter_index_new >= CAN_STM32_MAX_NUM_FILTERS) { id = can_stm32_filter_to_ext_id(filter);
filter_id = -ENOSPC; mask = can_stm32_filter_to_ext_mask(filter);
goto done; filter_id = i;
bank_num = bank_offset + i;
break;
}
} }
} }
can_stm32_set_filter_bank(filter_id, &can->sFilterRegister[bank_num], if (filter_id != -ENOSPC) {
filter_type, id, mask); LOG_DBG("Adding filter_id %d, CAN ID: 0x%x, mask: 0x%x",
done: filter_id, filter->id, filter->id_mask);
can->FA1R |= bank_bit;
can->FMR &= ~(CAN_FMR_FINIT); /* set the filter init mode */
LOG_DBG("Filter set: id %d, index %d, bank %d", filter_id, filter_index_new, bank_num); can->FMR |= CAN_FMR_FINIT;
*filter_index = filter_index_new;
can_stm32_set_filter_bank(filter_id, &can->sFilterRegister[bank_num],
filter->id_type, id, mask);
can->FA1R |= 1U << bank_num;
can->FMR &= ~(CAN_FMR_FINIT);
} else {
LOG_WRN("No free filter left");
}
return filter_id; return filter_id;
} }
/*
* This driver uses masked mode for all filters (CAN_FM1R left at reset value
* 0x00) in order to simplify mapping between filter match index from the FIFOs
* and array index for the callbacks. All ext ID filters are stored in the
* banks below CONFIG_CAN_MAX_EXT_ID_FILTER, followed by the std ID filters,
* which consume only 1/2 bank per filter.
*
* The more complicated list mode must be implemented if someone requires more
* than 28 std ID or 14 ext ID filters.
*
* Currently, all filter banks are assigned to FIFO 0 and FIFO 1 is not used.
*/
static int can_stm32_add_rx_filter(const struct device *dev, can_rx_callback_t cb, static int can_stm32_add_rx_filter(const struct device *dev, can_rx_callback_t cb,
void *cb_arg, const struct zcan_filter *filter) void *cb_arg, const struct zcan_filter *filter)
{ {
struct can_stm32_data *data = dev->data; struct can_stm32_data *data = dev->data;
int filter_index = 0;
int filter_id; int filter_id;
k_mutex_lock(&filter_mutex, K_FOREVER); k_mutex_lock(&filter_mutex, K_FOREVER);
k_mutex_lock(&data->inst_mutex, K_FOREVER); k_mutex_lock(&data->inst_mutex, K_FOREVER);
filter_id = can_stm32_set_filter(dev, filter, &filter_index); filter_id = can_stm32_set_filter(dev, filter);
if (filter_id != -ENOSPC) { if (filter_id >= 0) {
data->rx_cb[filter_index] = cb; if (filter->id_type == CAN_STANDARD_IDENTIFIER) {
data->cb_arg[filter_index] = cb_arg; data->rx_cb_std[filter_id - CONFIG_CAN_MAX_EXT_ID_FILTER] = cb;
data->cb_arg_std[filter_id - CONFIG_CAN_MAX_EXT_ID_FILTER] = cb_arg;
} else {
data->rx_cb_ext[filter_id] = cb;
data->cb_arg_ext[filter_id] = cb_arg;
}
} }
k_mutex_unlock(&data->inst_mutex); k_mutex_unlock(&data->inst_mutex);
@ -1077,16 +901,12 @@ static void can_stm32_remove_rx_filter(const struct device *dev, int filter_id)
const struct can_stm32_config *cfg = dev->config; const struct can_stm32_config *cfg = dev->config;
struct can_stm32_data *data = dev->data; struct can_stm32_data *data = dev->data;
CAN_TypeDef *can = cfg->master_can; CAN_TypeDef *can = cfg->master_can;
enum can_ide filter_type;
int bank_offset = 0; int bank_offset = 0;
int bank_num; int bank_num;
int filter_index; bool bank_unused;
uint32_t bank_bit;
uint32_t mode_reg;
uint32_t scale_reg;
enum can_stm32_filter_type type;
uint32_t reset_mask;
__ASSERT_NO_MSG(filter_id >= 0 && filter_id < CAN_STM32_MAX_NUM_FILTERS); __ASSERT_NO_MSG(filter_id >= 0 && filter_id < CAN_STM32_MAX_FILTER_ID);
k_mutex_lock(&filter_mutex, K_FOREVER); k_mutex_lock(&filter_mutex, K_FOREVER);
k_mutex_lock(&data->inst_mutex, K_FOREVER); k_mutex_lock(&data->inst_mutex, K_FOREVER);
@ -1095,35 +915,46 @@ static void can_stm32_remove_rx_filter(const struct device *dev, int filter_id)
bank_offset = CAN_STM32_NUM_FILTER_BANKS; bank_offset = CAN_STM32_NUM_FILTER_BANKS;
} }
bank_num = bank_offset + filter_id / 4; if (filter_id < CONFIG_CAN_MAX_EXT_ID_FILTER) {
bank_bit = (1U << bank_num); filter_type = CAN_EXTENDED_IDENTIFIER;
mode_reg = can->FM1R; bank_num = bank_offset + filter_id;
scale_reg = can->FS1R;
filter_index = can_stm32_calc_filter_index(filter_id, bank_offset, mode_reg, scale_reg); data->rx_cb_ext[filter_id] = NULL;
type = can_stm32_get_filter_type(bank_num, mode_reg, scale_reg); data->cb_arg_ext[filter_id] = NULL;
LOG_DBG("Detach filter number %d (index %d), type %d", filter_id, bank_unused = true;
filter_index, } else {
type); int filter_index = filter_id - CONFIG_CAN_MAX_EXT_ID_FILTER;
filter_type = CAN_STANDARD_IDENTIFIER;
bank_num = bank_offset + CONFIG_CAN_MAX_EXT_ID_FILTER +
(filter_id - CONFIG_CAN_MAX_EXT_ID_FILTER) / 2;
data->rx_cb_std[filter_index] = NULL;
data->cb_arg_std[filter_index] = NULL;
if (filter_index % 2 == 1) {
bank_unused = data->rx_cb_std[filter_index - 1] == NULL;
} else if (filter_index + 1 < CONFIG_CAN_MAX_STD_ID_FILTER) {
bank_unused = data->rx_cb_std[filter_index + 1] == NULL;
} else {
bank_unused = true;
}
}
LOG_DBG("Removing filter_id %d, type %d", filter_id, (uint32_t)filter_type);
reset_mask = ((1 << (filter_reg_demand[type])) - 1) << filter_id;
data->filter_usage |= reset_mask;
can->FMR |= CAN_FMR_FINIT; can->FMR |= CAN_FMR_FINIT;
can->FA1R &= ~bank_bit;
can_stm32_set_filter_bank(filter_id, &can->sFilterRegister[bank_num], can_stm32_set_filter_bank(filter_id, &can->sFilterRegister[bank_num],
type, 0, 0xFFFFFFFF); filter_type, 0, 0xFFFFFFFF);
if (!CAN_STM32_BANK_IS_EMPTY(data->filter_usage, bank_num, bank_offset)) { if (bank_unused) {
can->FA1R |= bank_bit; can->FA1R &= ~(1U << bank_num);
} else { LOG_DBG("Filter bank %d is unused -> deactivate", bank_num);
LOG_DBG("Bank number %d is empty -> deactivate", bank_num);
} }
can->FMR &= ~(CAN_FMR_FINIT); can->FMR &= ~(CAN_FMR_FINIT);
data->rx_cb[filter_index] = NULL;
data->cb_arg[filter_index] = NULL;
k_mutex_unlock(&data->inst_mutex); k_mutex_unlock(&data->inst_mutex);
k_mutex_unlock(&filter_mutex); k_mutex_unlock(&filter_mutex);
@ -1143,6 +974,7 @@ static const struct can_driver_api can_api_funcs = {
.set_state_change_callback = can_stm32_set_state_change_callback, .set_state_change_callback = can_stm32_set_state_change_callback,
.get_core_clock = can_stm32_get_core_clock, .get_core_clock = can_stm32_get_core_clock,
.get_max_bitrate = can_stm32_get_max_bitrate, .get_max_bitrate = can_stm32_get_max_bitrate,
.get_max_filters = can_stm32_get_max_filters,
.timing_min = { .timing_min = {
.sjw = 0x1, .sjw = 0x1,
.prop_seg = 0x00, .prop_seg = 0x00,

21
drivers/can/can_stm32.h
View file

@ -10,7 +10,8 @@
#include <zephyr/drivers/can.h> #include <zephyr/drivers/can.h>
#define CAN_STM32_NUM_FILTER_BANKS (14) #define CAN_STM32_NUM_FILTER_BANKS (14)
#define CAN_STM32_MAX_NUM_FILTERS (CAN_STM32_NUM_FILTER_BANKS * 4) #define CAN_STM32_MAX_FILTER_ID \
(CONFIG_CAN_MAX_EXT_ID_FILTER + CONFIG_CAN_MAX_STD_ID_FILTER * 2)
#define CAN_STM32_FIRX_STD_IDE_POS (3U) #define CAN_STM32_FIRX_STD_IDE_POS (3U)
#define CAN_STM32_FIRX_STD_RTR_POS (4U) #define CAN_STM32_FIRX_STD_RTR_POS (4U)
@ -21,9 +22,6 @@
#define CAN_STM32_FIRX_EXT_STD_ID_POS (21U) #define CAN_STM32_FIRX_EXT_STD_ID_POS (21U)
#define CAN_STM32_FIRX_EXT_EXT_ID_POS (3U) #define CAN_STM32_FIRX_EXT_EXT_ID_POS (3U)
#define CAN_STM32_BANK_IS_EMPTY(usage, bank_nr, bank_offset) \
(((usage >> ((bank_nr - bank_offset) * 4)) & 0x0F) == 0x0F)
struct can_stm32_mailbox { struct can_stm32_mailbox {
can_tx_callback_t tx_callback; can_tx_callback_t tx_callback;
void *callback_arg; void *callback_arg;
@ -31,23 +29,16 @@ struct can_stm32_mailbox {
int error; int error;
}; };
/* number = FSCx | FMBx */
enum can_stm32_filter_type {
CAN_STM32_FILTER_STANDARD_MASKED = 0,
CAN_STM32_FILTER_STANDARD = 1,
CAN_STM32_FILTER_EXTENDED_MASKED = 2,
CAN_STM32_FILTER_EXTENDED = 3
};
struct can_stm32_data { struct can_stm32_data {
struct k_mutex inst_mutex; struct k_mutex inst_mutex;
struct k_sem tx_int_sem; struct k_sem tx_int_sem;
struct can_stm32_mailbox mb0; struct can_stm32_mailbox mb0;
struct can_stm32_mailbox mb1; struct can_stm32_mailbox mb1;
struct can_stm32_mailbox mb2; struct can_stm32_mailbox mb2;
uint64_t filter_usage; can_rx_callback_t rx_cb_std[CONFIG_CAN_MAX_STD_ID_FILTER];
can_rx_callback_t rx_cb[CONFIG_CAN_MAX_FILTER]; can_rx_callback_t rx_cb_ext[CONFIG_CAN_MAX_EXT_ID_FILTER];
void *cb_arg[CONFIG_CAN_MAX_FILTER]; void *cb_arg_std[CONFIG_CAN_MAX_STD_ID_FILTER];
void *cb_arg_ext[CONFIG_CAN_MAX_EXT_ID_FILTER];
can_state_change_callback_t state_change_cb; can_state_change_callback_t state_change_cb;
void *state_change_cb_data; void *state_change_cb_data;
enum can_state state; enum can_state state;