drivers: can: stm32: rework filter handling
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:
parent
3524958083
commit
c56b1dbca1
3 changed files with 177 additions and 331 deletions
|
@ -1,5 +1,6 @@
|
|||
/*
|
||||
* Copyright (c) 2018 Alexander Wachter
|
||||
* Copyright (c) 2022 Martin Jäger <martin@libre.solar>
|
||||
*
|
||||
* 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)
|
||||
#endif
|
||||
|
||||
/*
|
||||
* Translation tables
|
||||
* max_filters_per_bank[enum can_stm32_filter_type] = number of filters in bank for this type
|
||||
* filter_reg_demand[enum can_stm32_filter_type] = how many registers are used for this type
|
||||
*/
|
||||
static const uint8_t max_filters_per_bank[] = {2, 4, 1, 2};
|
||||
static const uint8_t filter_reg_demand[] = {2, 1, 4, 2};
|
||||
#if (CONFIG_CAN_MAX_STD_ID_FILTER + CONFIG_CAN_MAX_EXT_ID_FILTER * 2) > \
|
||||
(CAN_STM32_NUM_FILTER_BANKS * 2)
|
||||
#error Number of configured filters exceeds available filter bank slots.
|
||||
#endif
|
||||
|
||||
/*
|
||||
* 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;
|
||||
CAN_TypeDef *can = cfg->can;
|
||||
CAN_FIFOMailBox_TypeDef *mbox;
|
||||
int filter_match_index;
|
||||
int filter_id, index;
|
||||
struct zcan_frame frame;
|
||||
can_rx_callback_t callback;
|
||||
can_rx_callback_t callback = NULL;
|
||||
void *cb_arg;
|
||||
|
||||
while (can->RF0R & CAN_RF0R_FMP0) {
|
||||
mbox = &can->sFIFOMailBox[0];
|
||||
filter_match_index = ((mbox->RDTR & CAN_RDT0R_FMI)
|
||||
>> CAN_RDT0R_FMI_Pos);
|
||||
filter_id = ((mbox->RDTR & CAN_RDT0R_FMI) >> CAN_RDT0R_FMI_Pos);
|
||||
|
||||
if (filter_match_index >= CONFIG_CAN_MAX_FILTER) {
|
||||
break;
|
||||
}
|
||||
LOG_DBG("Message on filter_id %d", filter_id);
|
||||
|
||||
LOG_DBG("Message on filter index %d", filter_match_index);
|
||||
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) {
|
||||
callback(dev, &frame, data->cb_arg[filter_match_index]);
|
||||
callback(dev, &frame, cb_arg);
|
||||
}
|
||||
|
||||
/* Release message */
|
||||
|
@ -482,6 +484,17 @@ static int can_stm32_get_max_bitrate(const struct device *dev, uint32_t *max_bit
|
|||
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)
|
||||
{
|
||||
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;
|
||||
struct can_timing timing;
|
||||
const struct device *clock;
|
||||
uint32_t bank_offset;
|
||||
int ret;
|
||||
|
||||
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->mb1.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 (!device_is_ready(cfg->phy)) {
|
||||
|
@ -541,6 +546,12 @@ static int can_stm32_init(const struct device *dev)
|
|||
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_NART & ~CAN_MCR_RFLM & ~CAN_MCR_TXFP;
|
||||
#ifdef CONFIG_CAN_RX_TIMESTAMP
|
||||
|
@ -584,7 +595,7 @@ static int can_stm32_init(const struct device *dev)
|
|||
|
||||
cfg->config_irq(can);
|
||||
can->IER |= CAN_IER_TMEIE;
|
||||
LOG_INF("Init of %s done", dev->name);
|
||||
|
||||
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) {
|
||||
LOG_DBG("Using mailbox 0");
|
||||
LOG_DBG("Using TX mailbox 0");
|
||||
mailbox = &can->sTxMailBox[0];
|
||||
mb = &(data->mb0);
|
||||
} else if (transmit_status_register & CAN_TSR_TME1) {
|
||||
LOG_DBG("Using mailbox 1");
|
||||
LOG_DBG("Using TX mailbox 1");
|
||||
mailbox = &can->sTxMailBox[1];
|
||||
mb = &data->mb1;
|
||||
} else if (transmit_status_register & CAN_TSR_TME2) {
|
||||
LOG_DBG("Using mailbox 2");
|
||||
LOG_DBG("Using TX mailbox 2");
|
||||
mailbox = &can->sTxMailBox[2];
|
||||
mb = &data->mb2;
|
||||
}
|
||||
|
@ -741,150 +752,26 @@ static int can_stm32_send(const struct device *dev, const struct zcan_frame *fra
|
|||
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;
|
||||
size_t cnt;
|
||||
|
||||
if (start > CONFIG_CAN_MAX_FILTER) {
|
||||
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:
|
||||
switch (type) {
|
||||
case CAN_STANDARD_IDENTIFIER:
|
||||
if ((filter_id - CONFIG_CAN_MAX_EXT_ID_FILTER) % 2 == 0) {
|
||||
/* even std filter id: first 1/2 bank */
|
||||
filter_reg->FR1 = id | (mask << 16);
|
||||
break;
|
||||
case 2:
|
||||
} else {
|
||||
/* uneven std filter id: first 1/2 bank */
|
||||
filter_reg->FR2 = id | (mask << 16);
|
||||
break;
|
||||
}
|
||||
|
||||
break;
|
||||
case CAN_STM32_FILTER_EXTENDED:
|
||||
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:
|
||||
case CAN_EXTENDED_IDENTIFIER:
|
||||
filter_reg->FR1 = id;
|
||||
filter_reg->FR2 = mask;
|
||||
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)
|
||||
{
|
||||
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);
|
||||
}
|
||||
|
||||
static inline int can_stm32_set_filter(const struct device *dev, const struct zcan_filter *filter,
|
||||
int *filter_index)
|
||||
static inline int can_stm32_set_filter(const struct device *dev, const struct zcan_filter *filter)
|
||||
{
|
||||
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;
|
||||
uint32_t mask = 0U;
|
||||
uint32_t id = 0U;
|
||||
int filter_id = 0;
|
||||
int filter_index_new = -ENOSPC;
|
||||
int bank_num;
|
||||
int filter_id = -ENOSPC;
|
||||
int bank_offset = 0;
|
||||
uint32_t bank_bit;
|
||||
int register_demand;
|
||||
enum can_stm32_filter_type filter_type;
|
||||
enum can_stm32_filter_type bank_type;
|
||||
int bank_num;
|
||||
|
||||
if (cfg->can != cfg->master_can) {
|
||||
/* 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) {
|
||||
id = can_stm32_filter_to_std_id(filter);
|
||||
filter_type = CAN_STM32_FILTER_STANDARD;
|
||||
|
||||
if (filter->id_mask != CAN_STD_ID_MASK) {
|
||||
mask = can_stm32_filter_to_std_mask(filter);
|
||||
filter_type = CAN_STM32_FILTER_STANDARD_MASKED;
|
||||
}
|
||||
} 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;
|
||||
for (int i = 0; i < CONFIG_CAN_MAX_STD_ID_FILTER; i++) {
|
||||
if (data->rx_cb_std[i] == NULL) {
|
||||
id = can_stm32_filter_to_std_id(filter);
|
||||
mask = can_stm32_filter_to_std_mask(filter);
|
||||
filter_id = CONFIG_CAN_MAX_EXT_ID_FILTER + i;
|
||||
bank_num = bank_offset + CONFIG_CAN_MAX_EXT_ID_FILTER + i / 2;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
can->FM1R = mode_reg;
|
||||
can->FS1R = scale_reg;
|
||||
} else {
|
||||
filter_index_new = can_stm32_calc_filter_index(filter_id, bank_offset,
|
||||
can->FM1R, can->FS1R);
|
||||
if (filter_index_new >= CAN_STM32_MAX_NUM_FILTERS) {
|
||||
filter_id = -ENOSPC;
|
||||
goto done;
|
||||
for (int i = 0; i < CONFIG_CAN_MAX_EXT_ID_FILTER; i++) {
|
||||
if (data->rx_cb_ext[i] == NULL) {
|
||||
id = can_stm32_filter_to_ext_id(filter);
|
||||
mask = can_stm32_filter_to_ext_mask(filter);
|
||||
filter_id = i;
|
||||
bank_num = bank_offset + i;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
can_stm32_set_filter_bank(filter_id, &can->sFilterRegister[bank_num],
|
||||
filter_type, id, mask);
|
||||
done:
|
||||
can->FA1R |= bank_bit;
|
||||
can->FMR &= ~(CAN_FMR_FINIT);
|
||||
LOG_DBG("Filter set: id %d, index %d, bank %d", filter_id, filter_index_new, bank_num);
|
||||
*filter_index = filter_index_new;
|
||||
if (filter_id != -ENOSPC) {
|
||||
LOG_DBG("Adding filter_id %d, CAN ID: 0x%x, mask: 0x%x",
|
||||
filter_id, filter->id, filter->id_mask);
|
||||
|
||||
/* set the filter init mode */
|
||||
can->FMR |= CAN_FMR_FINIT;
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* 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,
|
||||
void *cb_arg, const struct zcan_filter *filter)
|
||||
{
|
||||
struct can_stm32_data *data = dev->data;
|
||||
int filter_index = 0;
|
||||
int filter_id;
|
||||
|
||||
k_mutex_lock(&filter_mutex, K_FOREVER);
|
||||
k_mutex_lock(&data->inst_mutex, K_FOREVER);
|
||||
|
||||
filter_id = can_stm32_set_filter(dev, filter, &filter_index);
|
||||
if (filter_id != -ENOSPC) {
|
||||
data->rx_cb[filter_index] = cb;
|
||||
data->cb_arg[filter_index] = cb_arg;
|
||||
filter_id = can_stm32_set_filter(dev, filter);
|
||||
if (filter_id >= 0) {
|
||||
if (filter->id_type == CAN_STANDARD_IDENTIFIER) {
|
||||
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);
|
||||
|
@ -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;
|
||||
struct can_stm32_data *data = dev->data;
|
||||
CAN_TypeDef *can = cfg->master_can;
|
||||
enum can_ide filter_type;
|
||||
int bank_offset = 0;
|
||||
int bank_num;
|
||||
int filter_index;
|
||||
uint32_t bank_bit;
|
||||
uint32_t mode_reg;
|
||||
uint32_t scale_reg;
|
||||
enum can_stm32_filter_type type;
|
||||
uint32_t reset_mask;
|
||||
bool bank_unused;
|
||||
|
||||
__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(&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_num = bank_offset + filter_id / 4;
|
||||
bank_bit = (1U << bank_num);
|
||||
mode_reg = can->FM1R;
|
||||
scale_reg = can->FS1R;
|
||||
if (filter_id < CONFIG_CAN_MAX_EXT_ID_FILTER) {
|
||||
filter_type = CAN_EXTENDED_IDENTIFIER;
|
||||
bank_num = bank_offset + filter_id;
|
||||
|
||||
filter_index = can_stm32_calc_filter_index(filter_id, bank_offset, mode_reg, scale_reg);
|
||||
type = can_stm32_get_filter_type(bank_num, mode_reg, scale_reg);
|
||||
data->rx_cb_ext[filter_id] = NULL;
|
||||
data->cb_arg_ext[filter_id] = NULL;
|
||||
|
||||
LOG_DBG("Detach filter number %d (index %d), type %d", filter_id,
|
||||
filter_index,
|
||||
type);
|
||||
bank_unused = true;
|
||||
} else {
|
||||
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->FA1R &= ~bank_bit;
|
||||
|
||||
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)) {
|
||||
can->FA1R |= bank_bit;
|
||||
} else {
|
||||
LOG_DBG("Bank number %d is empty -> deactivate", bank_num);
|
||||
if (bank_unused) {
|
||||
can->FA1R &= ~(1U << bank_num);
|
||||
LOG_DBG("Filter bank %d is unused -> deactivate", bank_num);
|
||||
}
|
||||
|
||||
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(&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,
|
||||
.get_core_clock = can_stm32_get_core_clock,
|
||||
.get_max_bitrate = can_stm32_get_max_bitrate,
|
||||
.get_max_filters = can_stm32_get_max_filters,
|
||||
.timing_min = {
|
||||
.sjw = 0x1,
|
||||
.prop_seg = 0x00,
|
||||
|
|
Loading…
Add table
Add a link
Reference in a new issue