michaelh/zephyr
1
0
Fork 0
Code Releases Activity

drivers: can: stm32_can rework

Browse source 
This commit makes the stm32 CAN driver more readable and fix
a bug where the usage and rx_response array are not shifted correctly.

Signed-off-by: Alexander Wachter <alexander.wachter@student.tugraz.at>
This commit is contained in:
Alexander Wachter 2019-02-06 22:34:59 +01:00 • committed by Anas Nashif
commit 873875411e
2 changed files with 133 additions and 130 deletions
Download patch file Download diff file Expand all files Collapse all files

253
drivers/can/stm32_can.c
View file

@ -19,6 +19,14 @@
#include <logging/log.h> #include <logging/log.h>
LOG_MODULE_REGISTER(stm32_can); LOG_MODULE_REGISTER(stm32_can);
/*
* Translation tables
* filter_in_bank[enum can_filter_type] = number of filters in bank for this type
* reg_demand[enum can_filter_type] = how many registers are used for this type
*/
static const u8_t filter_in_bank[] = {2, 4, 1, 2};
static const u8_t reg_demand[] = {2, 1, 4, 2};
static void can_stm32_signal_tx_complete(struct can_mailbox *mb) static void can_stm32_signal_tx_complete(struct can_mailbox *mb)
{ {
if (mb->tx_callback) { if (mb->tx_callback) {
@ -28,7 +36,7 @@ static void can_stm32_signal_tx_complete(struct can_mailbox *mb)
} }
} }
static inline void can_stm32_get_msg_fifo(CAN_FIFOMailBox_TypeDef *mbox, static void can_stm32_get_msg_fifo(CAN_FIFOMailBox_TypeDef *mbox,
struct can_msg *msg) struct can_msg *msg)
{ {
if (mbox->RIR & CAN_RI0R_IDE) { if (mbox->RIR & CAN_RI0R_IDE) {
@ -401,6 +409,18 @@ int can_stm32_send(struct device *dev, struct can_msg *msg, s32_t timeout,
return 0; return 0;
} }
static inline int can_stm32_check_free(void **arr, int start, int end)
{
int i;
for (i = start; i <= end; i++) {
if (arr[i] != NULL) {
return 0;
}
}
return 1;
}
static int can_stm32_shift_arr(void **arr, int start, int count) static int can_stm32_shift_arr(void **arr, int start, int count)
{ {
void **start_ptr = arr + start; void **start_ptr = arr + start;
@ -413,11 +433,17 @@ static int can_stm32_shift_arr(void **arr, int start, int count)
if (count > 0) { if (count > 0) {
void *move_dest; void *move_dest;
if ((start + count) >= CONFIG_CAN_MAX_FILTER || /* Check if nothing used will be overwritten */
arr[CONFIG_CAN_MAX_FILTER - 1 - count] != NULL) { if (!can_stm32_check_free(arr, CONFIG_CAN_MAX_FILTER - count,
CONFIG_CAN_MAX_FILTER - 1)) {
return CAN_NO_FREE_FILTER; return CAN_NO_FREE_FILTER;
} }
/* 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 *); cnt = (CONFIG_CAN_MAX_FILTER - start - count) * sizeof(void *);
move_dest = start_ptr + count; move_dest = start_ptr + count;
memmove(move_dest, start_ptr, cnt); memmove(move_dest, start_ptr, cnt);
@ -440,33 +466,40 @@ static int can_stm32_shift_arr(void **arr, int start, int count)
static inline void can_stm32_shift_bits(u64_t *bits, int start, int count) static inline void can_stm32_shift_bits(u64_t *bits, int start, int count)
{ {
u64_t mask_right = (UINT64_MAX >> start); u64_t mask_right;
u64_t mask_left;
if (count > 0) { if (count > 0) {
*bits = (*bits & ~mask_right) | ((*bits & mask_right) << count); mask_right = ~(UINT64_MAX << start);
*bits = (*bits & mask_right) | ((*bits & ~mask_right) << count);
} else if (count < 0) { } else if (count < 0) {
u64_t mask_left;
count = -count; count = -count;
mask_left = ~(UINT64_MAX >> (start - count)); mask_left = UINT64_MAX << start;
*bits = (*bits & mask_left) | ((*bits & mask_right) >> count); 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)
{
u32_t mode_masked = (mode_reg >> bank_nr) & 0x01;
u32_t scale_masked = (scale_reg >> bank_nr) & 0x01;
enum can_filter_type type = (scale_masked << 1) | mode_masked;
return type;
}
static int can_calc_filter_index(int filter_nr, u32_t mode_reg, u32_t scale_reg) static int can_calc_filter_index(int filter_nr, u32_t mode_reg, u32_t scale_reg)
{ {
int filter_bank = filter_nr / 4; int filter_bank = filter_nr / 4;
int cnt = 0; int cnt = 0;
u32_t mode_masked; u32_t mode_masked, scale_masked;
u32_t scale_masked; enum can_filter_type filter_type;
/*count filters in the banks before */ /*count filters in the banks before */
for (int i = 0; i < filter_bank; i++) { for (int i = 0; i < filter_bank; i++) {
mode_masked = mode_reg & (1U << i); filter_type = can_stm32_get_filter_type(i, mode_reg, scale_reg);
scale_masked = scale_reg & (1U << i); cnt += filter_in_bank[filter_type];
cnt += !scale_masked && mode_masked ? 4 :
scale_masked && !mode_masked ? 1 :
2;
} }
/* plus the filters in the same bank */ /* plus the filters in the same bank */
@ -477,20 +510,6 @@ static int can_calc_filter_index(int filter_nr, u32_t mode_reg, u32_t scale_reg)
return cnt; return cnt;
} }
enum can_filter_type can_stm32_get_filter_type(u32_t bank_bit, u32_t mode_reg,
u32_t scale_reg)
{
u32_t mode_masked = mode_reg & bank_bit;
u32_t scale_masked = scale_reg & bank_bit;
enum can_filter_type type =
!scale_masked && mode_masked ? CAN_FILTER_STANDARD :
!scale_masked && !mode_masked ? CAN_FILTER_STANDARD_MASKED :
scale_masked && mode_masked ? CAN_FILTER_EXTENDED :
CAN_FILTER_EXTENDED_MASKED;
return type;
}
static void can_stm32_set_filter_bank(int filter_nr, static void can_stm32_set_filter_bank(int filter_nr,
CAN_FilterRegister_TypeDef *filter_reg, CAN_FilterRegister_TypeDef *filter_reg,
enum can_filter_type filter_type, enum can_filter_type filter_type,
@ -520,10 +539,10 @@ static void can_stm32_set_filter_bank(int filter_nr,
switch (filter_nr & 0x02) { switch (filter_nr & 0x02) {
case 0: case 0:
filter_reg->FR1 = id | (mask << 16); filter_reg->FR1 = id | (mask << 16);
break; break;
case 2: case 2:
filter_reg->FR2 = id | (mask << 16); filter_reg->FR2 = id | (mask << 16);
break; break;
} }
break; break;
@ -545,50 +564,47 @@ static void can_stm32_set_filter_bank(int filter_nr,
} }
} }
static inline
int can_stm32_calc_shift_width(enum can_filter_type new_filter_type,
enum can_filter_type old_filter_type)
{
switch (new_filter_type) {
case CAN_FILTER_STANDARD:
return old_filter_type == CAN_FILTER_EXTENDED_MASKED ? 3 : 1;
case CAN_FILTER_STANDARD_MASKED:
return old_filter_type == CAN_FILTER_STANDARD ? -2 :
old_filter_type == CAN_FILTER_EXTENDED_MASKED ? 1 :
0;
case CAN_FILTER_EXTENDED:
return old_filter_type == CAN_FILTER_STANDARD ? -2 :
old_filter_type == CAN_FILTER_EXTENDED_MASKED ? 1 :
0;
case CAN_FILTER_EXTENDED_MASKED:
return old_filter_type == CAN_FILTER_STANDARD ? -3 : -1;
}
return 0;
}
static inline void can_stm32_set_mode_scale(enum can_filter_type filter_type, static inline void can_stm32_set_mode_scale(enum can_filter_type filter_type,
u32_t *mode_reg, u32_t *scale_reg, u32_t *mode_reg, u32_t *scale_reg,
u32_t bank_bit) int filter_nr)
{ {
switch (filter_type) { u32_t mode_reg_bit = (filter_type & 0x01) << filter_nr;
case CAN_FILTER_STANDARD: u32_t scale_reg_bit = (filter_type >> 1) << filter_nr;
*mode_reg |= bank_bit;
*scale_reg &= ~bank_bit; *mode_reg &= ~(1 << filter_nr);
break; *mode_reg |= mode_reg_bit;
case CAN_FILTER_STANDARD_MASKED:
*mode_reg &= ~bank_bit; *scale_reg &= ~(1 << filter_nr);
*scale_reg &= ~bank_bit; *scale_reg |= scale_reg_bit;
break; }
case CAN_FILTER_EXTENDED:
*mode_reg |= bank_bit; static inline u32_t can_generate_std_mask(const struct can_filter *filter)
*scale_reg |= bank_bit; {
break; return (filter->std_id_mask << CAN_FIRX_STD_ID_POS) |
case CAN_FILTER_EXTENDED_MASKED: (filter->rtr_mask << CAN_FIRX_STD_RTR_POS) |
*mode_reg &= ~bank_bit; (1U << CAN_FIRX_STD_IDE_POS);
*scale_reg |= bank_bit; }
break;
} static inline u32_t can_generate_ext_mask(const struct can_filter *filter)
{
return (filter->ext_id_mask << CAN_FIRX_EXT_EXT_ID_POS) |
(filter->rtr_mask << CAN_FIRX_EXT_RTR_POS) |
(1U << CAN_FIRX_EXT_IDE_POS);
}
static inline u32_t can_generate_std_id(const struct can_filter *filter)
{
return (filter->std_id << CAN_FIRX_STD_ID_POS) |
(filter->rtr << CAN_FIRX_STD_RTR_POS);
}
static inline u32_t can_generate_ext_id(const struct can_filter *filter)
{
return (filter->ext_id << CAN_FIRX_EXT_EXT_ID_POS) |
(filter->rtr << CAN_FIRX_EXT_RTR_POS) |
(1U << CAN_FIRX_EXT_IDE_POS);
} }
static inline int can_stm32_set_filter(const struct can_filter *filter, static inline int can_stm32_set_filter(const struct can_filter *filter,
@ -599,7 +615,7 @@ static inline int can_stm32_set_filter(const struct can_filter *filter,
u32_t mask = 0U; u32_t mask = 0U;
u32_t id = 0U; u32_t id = 0U;
int filter_nr = 0; int filter_nr = 0;
int filter_index_tmp = CAN_NO_FREE_FILTER; int filter_index_new = CAN_NO_FREE_FILTER;
int bank_nr; int bank_nr;
u32_t bank_bit; u32_t bank_bit;
int register_demand; int register_demand;
@ -607,37 +623,25 @@ static inline int can_stm32_set_filter(const struct can_filter *filter,
enum can_filter_type bank_mode; enum can_filter_type bank_mode;
if (filter->id_type == CAN_STANDARD_IDENTIFIER) { if (filter->id_type == CAN_STANDARD_IDENTIFIER) {
id = (filter->std_id << CAN_FIRX_STD_ID_POS) id = can_generate_std_id(filter);
| (filter->rtr << CAN_FIRX_STD_RTR_POS); filter_type = CAN_FILTER_STANDARD;
if (filter->std_id_mask == CAN_STD_ID_MASK && filter->rtr_mask) { if (filter->std_id_mask != CAN_STD_ID_MASK) {
filter_type = CAN_FILTER_STANDARD; mask = can_generate_std_mask(filter);
register_demand = 1;
} else {
filter_type = CAN_FILTER_STANDARD_MASKED; filter_type = CAN_FILTER_STANDARD_MASKED;
register_demand = 2;
mask = (filter->std_id_mask << CAN_FIRX_STD_ID_POS)
| (filter->rtr_mask << CAN_FIRX_STD_RTR_POS)
| (1U << CAN_FIRX_STD_IDE_POS);
} }
} else { } else {
id = (filter->ext_id << CAN_FIRX_EXT_EXT_ID_POS) id = can_generate_ext_id(filter);
| (filter->rtr << CAN_FIRX_EXT_RTR_POS) filter_type = CAN_FILTER_EXTENDED;
| (1U << CAN_FIRX_EXT_IDE_POS);
if (filter->ext_id_mask == CAN_EXT_ID_MASK && filter->rtr_mask) { if (filter->ext_id_mask != CAN_EXT_ID_MASK) {
filter_type = CAN_FILTER_EXTENDED; mask = can_generate_ext_mask(filter);
register_demand = 2;
} else {
filter_type = CAN_FILTER_EXTENDED_MASKED; filter_type = CAN_FILTER_EXTENDED_MASKED;
register_demand = 4;
mask = (filter->ext_id_mask << CAN_FIRX_EXT_EXT_ID_POS)
| (filter->rtr_mask << CAN_FIRX_EXT_RTR_POS)
| (1U << CAN_FIRX_EXT_IDE_POS);
} }
} }
register_demand = reg_demand[filter_type];
LOG_DBG("Setting filter ID: 0x%x, mask: 0x%x", filter->ext_id, LOG_DBG("Setting filter ID: 0x%x, mask: 0x%x", filter->ext_id,
filter->ext_id_mask); filter->ext_id_mask);
LOG_DBG("Filter type: %s ID %s mask (%d)", LOG_DBG("Filter type: %s ID %s mask (%d)",
@ -651,25 +655,23 @@ static inline int can_stm32_set_filter(const struct can_filter *filter,
do { do {
u64_t usage_shifted = (device_data->filter_usage >> filter_nr); u64_t usage_shifted = (device_data->filter_usage >> filter_nr);
u8_t usage_demand_mask = (1U << register_demand) - 1; u64_t usage_demand_mask = (1U << register_demand) - 1;
bool bank_is_empty; bool bank_is_empty;
bank_nr = filter_nr / 4; bank_nr = filter_nr / 4;
bank_bit = (1U << bank_nr); bank_bit = (1U << bank_nr);
bank_mode = can_stm32_get_filter_type(bank_bit, can->FM1R, bank_mode = can_stm32_get_filter_type(bank_nr, can->FM1R,
can->FS1R); can->FS1R);
bank_is_empty = CAN_BANK_IS_EMPTY(device_data->filter_usage, bank_is_empty = CAN_BANK_IS_EMPTY(device_data->filter_usage,
bank_nr); bank_nr);
if ((usage_shifted & usage_demand_mask) == usage_demand_mask) { if (!bank_is_empty && bank_mode != filter_type) {
if (bank_mode == filter_type || bank_is_empty) { filter_nr = (bank_nr + 1) * 4;
device_data->filter_usage &= } else if (usage_shifted & usage_demand_mask) {
~((u64_t)usage_demand_mask << filter_nr); device_data->filter_usage &=
break; ~(usage_demand_mask << filter_nr);
} else { break;
/* Filter Bank has unsuitable configuration */
filter_nr = (bank_nr + 1) * 4;
}
} else { } else {
filter_nr += register_demand; filter_nr += register_demand;
} }
@ -686,37 +688,39 @@ static inline int can_stm32_set_filter(const struct can_filter *filter,
/* TODO fifo balancing */ /* TODO fifo balancing */
if (filter_type != bank_mode) { if (filter_type != bank_mode) {
int shift_width; int shift_width, start_index;
int res; int res;
u32_t mode_reg = can->FM1R; u32_t mode_reg = can->FM1R;
u32_t scale_reg = can->FS1R; u32_t scale_reg = can->FS1R;
can_stm32_set_mode_scale(filter_type, &mode_reg, &scale_reg, can_stm32_set_mode_scale(filter_type, &mode_reg, &scale_reg,
bank_bit); filter_nr);
shift_width = can_stm32_calc_shift_width(filter_type, shift_width = filter_in_bank[filter_type] - filter_in_bank[bank_mode];
bank_mode);
filter_index_tmp = can_calc_filter_index(filter_nr, mode_reg, filter_index_new = can_calc_filter_index(filter_nr, mode_reg,
scale_reg); scale_reg);
start_index = filter_index_new + filter_in_bank[bank_mode];
res = can_stm32_shift_arr(device_data->rx_response, res = can_stm32_shift_arr(device_data->rx_response,
filter_index_tmp + 1, shift_width); start_index,
shift_width);
if (filter_index_tmp >= CAN_MAX_NUMBER_OF_FILTERS || res) { if (filter_index_new >= CONFIG_CAN_MAX_FILTER || res) {
LOG_INF("No space for a new filter!"); LOG_INF("No space for a new filter!");
filter_nr = CAN_NO_FREE_FILTER; filter_nr = CAN_NO_FREE_FILTER;
goto done; goto done;
} }
can_stm32_shift_bits(&device_data->response_type, can_stm32_shift_bits(&device_data->response_type,
filter_index_tmp + 1, shift_width); start_index,
shift_width);
can->FM1R = mode_reg; can->FM1R = mode_reg;
can->FS1R = scale_reg; can->FS1R = scale_reg;
} else { } else {
filter_index_tmp = can_calc_filter_index(filter_nr, can->FM1R, filter_index_new = can_calc_filter_index(filter_nr, can->FM1R,
can->FS1R); can->FS1R);
if (filter_index_tmp >= CAN_MAX_NUMBER_OF_FILTERS) { if (filter_index_new >= CAN_MAX_NUMBER_OF_FILTERS) {
filter_nr = CAN_NO_FREE_FILTER; filter_nr = CAN_NO_FREE_FILTER;
goto done; goto done;
} }
@ -728,8 +732,8 @@ done:
can->FA1R |= bank_bit; can->FA1R |= bank_bit;
can->FMR &= ~(CAN_FMR_FINIT); can->FMR &= ~(CAN_FMR_FINIT);
LOG_DBG("Filter set! Filter number: %d (index %d)", LOG_DBG("Filter set! Filter number: %d (index %d)",
filter_nr, filter_index_tmp); filter_nr, filter_index_new);
*filter_index = filter_index_tmp; *filter_index = filter_index_new;
return filter_nr; return filter_nr;
} }
@ -804,17 +808,13 @@ void can_stm32_detach(struct device *dev, int filter_nr)
scale_reg = can->FS1R; scale_reg = can->FS1R;
filter_index = can_calc_filter_index(filter_nr, mode_reg, scale_reg); filter_index = can_calc_filter_index(filter_nr, mode_reg, scale_reg);
type = can_stm32_get_filter_type(bank_bit, mode_reg, scale_reg); type = can_stm32_get_filter_type(bank_nr, mode_reg, scale_reg);
LOG_DBG("Detatch filter number %d (index %d), type %d", filter_nr, LOG_DBG("Detatch filter number %d (index %d), type %d", filter_nr,
filter_index, filter_index,
type); type);
reset_mask = (type == CAN_FILTER_STANDARD) ? 0x01 : reset_mask = ((1 << (reg_demand[type])) - 1) << filter_nr;
(type == CAN_FILTER_EXTENDED_MASKED) ? 0x0F :
0x03;
reset_mask = reset_mask << filter_nr;
data->filter_usage |= reset_mask; data->filter_usage |= reset_mask;
can->FMR |= CAN_FMR_FINIT; can->FMR |= CAN_FMR_FINIT;
can->FA1R &= ~bank_bit; can->FA1R &= ~bank_bit;
@ -830,6 +830,7 @@ void can_stm32_detach(struct device *dev, int filter_nr)
can->FMR &= ~(CAN_FMR_FINIT); can->FMR &= ~(CAN_FMR_FINIT);
data->rx_response[filter_index] = NULL; data->rx_response[filter_index] = NULL;
data->response_type &= ~(1ULL << filter_index);
k_mutex_unlock(&data->set_filter_mutex); k_mutex_unlock(&data->set_filter_mutex);
} }

10
drivers/can/stm32_can.h
View file

@ -45,11 +45,13 @@ struct can_mailbox {
u32_t error_flags; u32_t error_flags;
}; };
/* number = FSCx | FMBx */
enum can_filter_type { enum can_filter_type {
CAN_FILTER_STANDARD, CAN_FILTER_STANDARD_MASKED = 0,
CAN_FILTER_STANDARD_MASKED, CAN_FILTER_STANDARD = 1,
CAN_FILTER_EXTENDED, CAN_FILTER_EXTENDED_MASKED = 2,
CAN_FILTER_EXTENDED_MASKED CAN_FILTER_EXTENDED = 3
}; };
struct can_stm32_data { struct can_stm32_data {