drivers: can: Extend CAN API for bus-state management.

This commit extends the CAN API with the following functions:
- can_get_state
- can_recover
- can_register_state_change_isr

This functions can be used to get the error-counters and the state
of the CAN controller. The recover function can be used to recover
from bus-off state when automatic recovery is disabled.

Signed-off-by: Alexander Wachter <alexander.wachter@student.tugraz.at>
This commit is contained in:
Alexander Wachter 2018-06-19 18:26:31 +02:00 committed by Jukka Rissanen
commit c0da8a7901
10 changed files with 598 additions and 56 deletions

View file

@ -50,6 +50,15 @@ config CAN_RX_TIMESTAMP
The value is incremented every bit time and starts when the controller
is initialized.
config CAN_AUTO_BUS_OFF_RECOVERY
bool "Enable automatic recovery from bus-off"
default y
help
This option enables the automatic bus-off recovery according to
ISO 11898-1 (recovery after 128 occurrences of 11 consecutive
recessive bits). When this option is enabled, the recovery API is not
available.
config CAN_0
bool "Enable CAN 0"
help

View file

@ -9,6 +9,7 @@
config CAN_MCP2515
bool "MCP2515 CAN Driver"
depends on SPI
select CAN_AUTO_BUS_OFF_RECOVERY
help
Enable MCP2515 CAN Driver

View file

@ -8,7 +8,8 @@
#include <drivers/can.h>
static inline int z_vrfy_can_configure(struct device *dev, enum can_mode mode,
u32_t bitrate) {
u32_t bitrate)
{
Z_OOPS(Z_SYSCALL_DRIVER_CAN(dev, configure));
@ -21,8 +22,8 @@ static inline int z_vrfy_can_send(struct device *dev,
const struct zcan_frame *msg,
s32_t timeout,
can_tx_callback_t callback_isr,
void *callback_arg) {
void *callback_arg)
{
Z_OOPS(Z_SYSCALL_DRIVER_CAN(dev, send));
Z_OOPS(Z_SYSCALL_MEMORY_READ((const struct zcan_frame *)msg,
@ -43,7 +44,8 @@ static inline int z_vrfy_can_send(struct device *dev,
static inline int z_vrfy_can_attach_msgq(struct device *dev,
struct k_msgq *msgq,
const struct zcan_filter *filter) {
const struct zcan_filter *filter)
{
Z_OOPS(Z_SYSCALL_OBJ(dev, K_OBJ_DRIVER_CAN));
Z_OOPS(Z_SYSCALL_MEMORY_READ((struct zcan_filter *)filter,
@ -57,9 +59,37 @@ static inline int z_vrfy_can_attach_msgq(struct device *dev,
#include <syscalls/can_attach_msgq_mrsh.c>
static inline void z_vrfy_can_detach(struct device *dev, int filter_id)
{
Z_OOPS(Z_SYSCALL_DRIVER_CAN(dev, detach));
z_impl_can_detach((struct device *)dev, (int)filter_id);
}
#include <syscalls/can_detach_mrsh.c>
static inline
enum can_state z_vrfy_can_get_state(struct device *dev,
struct can_bus_err_cnt *err_cnt)
{
Z_OOPS(Z_SYSCALL_OBJ(dev, K_OBJ_DRIVER_CAN));
if (err_cnt) {
Z_OOPS(Z_SYSCALL_MEMORY_WRITE(err_cnt, sizeof(err_cnt)));
}
return z_impl_can_get_state(dev, err_cnt);
}
#include <syscalls/can_get_state_mrsh.c>
#ifndef CONFIG_CAN_AUTO_BUS_OFF_RECOVERY
static inline int z_vrfy_can_recover(struct device *dev, s32_t timeout)
{
Z_OOPS(Z_SYSCALL_OBJ(dev, K_OBJ_DRIVER_CAN));
return z_impl_can_recover(dev, s32_t timeout);
}
#include <syscalls/can_recover_mrsh.c>
#endif /* CONFIG_CAN_AUTO_BUS_OFF_RECOVERY */

View file

@ -36,6 +36,8 @@ static inline int check_filter_match(const struct zcan_frame *frame,
return ((id & mask) == (frame_id & mask));
}
int can_loopback_send(struct device *dev, const struct zcan_frame *frame,
s32_t timeout, can_tx_callback_t callback,
void *callback_arg)
@ -149,11 +151,46 @@ int can_loopback_configure(struct device *dev, enum can_mode mode,
return 0;
}
static enum can_state can_loopback_get_state(struct device *dev,
struct can_bus_err_cnt *err_cnt)
{
ARG_UNUSED(dev);
if (err_cnt) {
err_cnt->tx_err_cnt = 0;
err_cnt->rx_err_cnt = 0;
}
return CAN_ERROR_ACTIVE;
}
#ifndef CONFIG_CAN_AUTO_BUS_OFF_RECOVERY
int can_loopback_recover(struct device *dev, s32_t timeout)
{
ARG_UNUSED(dev);
ARG_UNUSED(timeout);
return 0;
}
#endif /* CONFIG_CAN_AUTO_BUS_OFF_RECOVERY */
static void can_loopback_register_state_change_isr(struct device *dev,
can_state_change_isr_t isr)
{
ARG_UNUSED(dev);
ARG_UNUSED(isr);
}
static const struct can_driver_api can_api_funcs = {
.configure = can_loopback_configure,
.send = can_loopback_send,
.attach_isr = can_loopback_attach_isr,
.detach = can_loopback_detach
.detach = can_loopback_detach,
.get_state = can_loopback_get_state,
#ifndef CONFIG_CAN_AUTO_BUS_OFF_RECOVERY
.recover = can_loopback_recover,
#endif
.register_state_change_isr = can_loopback_register_state_change_isr
};

View file

@ -181,6 +181,8 @@ static int mcp2515_configure(struct device *dev, enum can_mode mode,
u32_t bitrate)
{
const struct mcp2515_config *dev_cfg = DEV_CFG(dev);
struct mcp2515_data *dev_data = DEV_DATA(dev);
int ret;
/* CNF3, CNF2, CNF1, CANINTE */
u8_t config_buf[4];
@ -213,12 +215,9 @@ static int mcp2515_configure(struct device *dev, enum can_mode mode,
const u8_t cnf3 = sof | wakfil | und | phseg2;
/* CANINTE
* MERRE<7>:WAKIE<6>:ERRIE<5>:TX2IE<4>:TX1IE<3>:TX0IE<2>:RX1IE<1>:
* RX0IE<0>
* all TX and RX buffer interrupts enabled
*/
const u8_t caninte = BIT(4) | BIT(3) | BIT(2) | BIT(1) | BIT(0);
const u8_t caninte = MCP2515_INTE_RX0IE | MCP2515_INTE_RX1IE |
MCP2515_INTE_TX0IE | MCP2515_INTE_TX1IE |
MCP2515_INTE_TX2IE | MCP2515_INTE_ERRIE;
/* Receive everything, filtering done in driver, RXB0 roll over into
* RXB1 */
@ -251,17 +250,41 @@ static int mcp2515_configure(struct device *dev, enum can_mode mode,
config_buf[2] = cnf1;
config_buf[3] = caninte;
k_mutex_lock(&dev_data->mutex, K_FOREVER);
/* will enter configuration mode automatically */
mcp2515_cmd_soft_reset(dev);
ret = mcp2515_cmd_soft_reset(dev);
if (ret < 0) {
LOG_ERR("Failed to reset the device [%d]", ret);
goto done;
}
mcp2515_cmd_write_reg(dev, MCP2515_ADDR_CNF3, config_buf,
sizeof(config_buf));
ret = mcp2515_cmd_write_reg(dev, MCP2515_ADDR_CNF3, config_buf,
sizeof(config_buf));
if (ret < 0) {
LOG_ERR("Failed to write the configuration [%d]", ret);
}
mcp2515_cmd_bit_modify(dev, MCP2515_ADDR_RXB0CTRL, rx0_ctrl, rx0_ctrl);
mcp2515_cmd_bit_modify(dev, MCP2515_ADDR_RXB1CTRL, rx1_ctrl, rx1_ctrl);
ret = mcp2515_cmd_bit_modify(dev, MCP2515_ADDR_RXB0CTRL, rx0_ctrl,
rx0_ctrl);
if (ret < 0) {
LOG_ERR("Failed to write RXB0CTRL [%d]", ret);
}
return mcp2515_set_mode(dev,
mcp2515_convert_canmode_to_mcp2515mode(mode));
ret = mcp2515_cmd_bit_modify(dev, MCP2515_ADDR_RXB1CTRL, rx1_ctrl,
rx1_ctrl);
if (ret < 0) {
LOG_ERR("Failed to write RXB1CTRL [%d]", ret);
}
done:
ret = mcp2515_set_mode(dev,
mcp2515_convert_canmode_to_mcp2515mode(mode));
if (ret < 0) {
LOG_ERR("Failed to set the mode [%d]", ret);
}
k_mutex_unlock(&dev_data->mutex);
return ret;
}
static int mcp2515_send(struct device *dev, const struct zcan_frame *msg,
@ -276,7 +299,7 @@ static int mcp2515_send(struct device *dev, const struct zcan_frame *msg,
return CAN_TIMEOUT;
}
k_mutex_lock(&dev_data->tx_mutex, K_FOREVER);
k_mutex_lock(&dev_data->mutex, K_FOREVER);
/* find a free tx slot */
for (; tx_idx < MCP2515_TX_CNT; tx_idx++) {
@ -286,7 +309,7 @@ static int mcp2515_send(struct device *dev, const struct zcan_frame *msg,
}
}
k_mutex_unlock(&dev_data->tx_mutex);
k_mutex_unlock(&dev_data->mutex);
if (tx_idx == MCP2515_TX_CNT) {
LOG_WRN("no free tx slot available");
@ -323,7 +346,7 @@ static int mcp2515_attach_isr(struct device *dev, can_rx_callback_t rx_cb,
__ASSERT(rx_cb != NULL, "response_ptr can not be null");
k_mutex_lock(&dev_data->filter_mutex, K_FOREVER);
k_mutex_lock(&dev_data->mutex, K_FOREVER);
/* find free filter */
while ((BIT(filter_idx) & dev_data->filter_usage)
@ -343,7 +366,7 @@ static int mcp2515_attach_isr(struct device *dev, can_rx_callback_t rx_cb,
filter_idx = CAN_NO_FREE_FILTER;
}
k_mutex_unlock(&dev_data->filter_mutex);
k_mutex_unlock(&dev_data->mutex);
return filter_idx;
}
@ -352,9 +375,17 @@ static void mcp2515_detach(struct device *dev, int filter_nr)
{
struct mcp2515_data *dev_data = DEV_DATA(dev);
k_mutex_lock(&dev_data->filter_mutex, K_FOREVER);
k_mutex_lock(&dev_data->mutex, K_FOREVER);
dev_data->filter_usage &= ~BIT(filter_nr);
k_mutex_unlock(&dev_data->filter_mutex);
k_mutex_unlock(&dev_data->mutex);
}
static void mcp2515_register_state_change_isr(struct device *dev,
can_state_change_isr_t isr)
{
struct mcp2515_data *dev_data = DEV_DATA(dev);
dev_data->state_change_isr = isr;
}
static u8_t mcp2515_filter_match(struct zcan_frame *msg,
@ -388,7 +419,7 @@ static void mcp2515_rx_filter(struct device *dev, struct zcan_frame *msg)
can_rx_callback_t callback;
struct zcan_frame tmp_msg;
k_mutex_lock(&dev_data->filter_mutex, K_FOREVER);
k_mutex_lock(&dev_data->mutex, K_FOREVER);
for (; filter_idx < CONFIG_CAN_MCP2515_MAX_FILTER; filter_idx++) {
if (!(BIT(filter_idx) & dev_data->filter_usage)) {
@ -407,7 +438,7 @@ static void mcp2515_rx_filter(struct device *dev, struct zcan_frame *msg)
callback(&tmp_msg, dev_data->cb_arg[filter_idx]);
}
k_mutex_unlock(&dev_data->filter_mutex);
k_mutex_unlock(&dev_data->mutex);
}
static void mcp2515_rx(struct device *dev, u8_t rx_idx)
@ -435,12 +466,71 @@ static void mcp2515_tx_done(struct device *dev, u8_t tx_idx)
dev_data->tx_cb[tx_idx].cb(0, dev_data->tx_cb[tx_idx].cb_arg);
}
k_mutex_lock(&dev_data->tx_mutex, K_FOREVER);
k_mutex_lock(&dev_data->mutex, K_FOREVER);
dev_data->tx_busy_map &= ~BIT(tx_idx);
k_mutex_unlock(&dev_data->tx_mutex);
k_mutex_unlock(&dev_data->mutex);
k_sem_give(&dev_data->tx_sem);
}
static enum can_state mcp2515_get_state(struct device *dev,
struct can_bus_err_cnt *err_cnt)
{
u8_t eflg;
u8_t err_cnt_buf[2];
int ret;
ret = mcp2515_cmd_read_reg(dev, MCP2515_ADDR_EFLG, &eflg, sizeof(eflg));
if (ret < 0) {
LOG_ERR("Failed to read error register [%d]", ret);
return CAN_BUS_UNKNOWN;
}
if (err_cnt) {
ret = mcp2515_cmd_read_reg(dev, MCP2515_ADDR_TEC, err_cnt_buf,
sizeof(err_cnt_buf));
if (ret < 0) {
LOG_ERR("Failed to read error counters [%d]", ret);
return CAN_BUS_UNKNOWN;
}
err_cnt->tx_err_cnt = err_cnt_buf[0];
err_cnt->rx_err_cnt = err_cnt_buf[1];
}
if (eflg & MCP2515_EFLG_TXBO) {
return CAN_BUS_OFF;
}
if ((eflg & MCP2515_EFLG_RXEP) || (eflg & MCP2515_EFLG_TXEP)) {
return CAN_ERROR_PASSIVE;
}
return CAN_ERROR_ACTIVE;
}
static void mcp2515_handle_errors(struct device *dev)
{
struct mcp2515_data *dev_data = DEV_DATA(dev);
can_state_change_isr_t state_change_isr = dev_data->state_change_isr;
enum can_state state;
struct can_bus_err_cnt err_cnt;
state = mcp2515_get_state(dev, state_change_isr ? &err_cnt : NULL);
if (state_change_isr && dev_data->old_state != state) {
dev_data->old_state = state;
state_change_isr(state, err_cnt);
}
}
#ifndef CONFIG_CAN_AUTO_BUS_OFF_RECOVERY
static void mcp2515_recover(struct device *dev, s32_t timeout)
{
ARG_UNUSED(dev);
ARG_UNUSED(timeout);
}
#endif
static void mcp2515_handle_interrupts(struct device *dev)
{
u8_t canintf;
@ -468,6 +558,10 @@ static void mcp2515_handle_interrupts(struct device *dev)
mcp2515_tx_done(dev, 2);
}
if (canintf & MCP2515_CANINTF_ERRIF) {
mcp2515_handle_errors(dev);
}
/* clear the flags we handled */
mcp2515_cmd_bit_modify(dev, MCP2515_ADDR_CANINTF, canintf,
~canintf);
@ -501,7 +595,12 @@ static const struct can_driver_api can_api_funcs = {
.configure = mcp2515_configure,
.send = mcp2515_send,
.attach_isr = mcp2515_attach_isr,
.detach = mcp2515_detach
.detach = mcp2515_detach,
.get_state = mcp2515_get_state,
#ifndef CONFIG_CAN_AUTO_BUS_OFF_RECOVERY
.recover = mcp2515_recover,
#endif
.register_state_change_isr = mcp2515_register_state_change_isr
};
@ -512,12 +611,11 @@ static int mcp2515_init(struct device *dev)
int ret;
k_sem_init(&dev_data->int_sem, 0, UINT_MAX);
k_mutex_init(&dev_data->tx_mutex);
k_mutex_init(&dev_data->mutex);
k_sem_init(&dev_data->tx_sem, 3, 3);
k_sem_init(&dev_data->tx_cb[0].sem, 0, 1);
k_sem_init(&dev_data->tx_cb[1].sem, 0, 1);
k_sem_init(&dev_data->tx_cb[2].sem, 0, 1);
k_mutex_init(&dev_data->filter_mutex);
/* SPI config */
dev_data->spi_cfg.operation = SPI_WORD_SET(8);
@ -585,6 +683,7 @@ static int mcp2515_init(struct device *dev)
(void)memset(dev_data->rx_cb, 0, sizeof(dev_data->rx_cb));
(void)memset(dev_data->filter, 0, sizeof(dev_data->filter));
dev_data->old_state = CAN_ERROR_ACTIVE;
ret = mcp2515_configure(dev, CAN_NORMAL_MODE, dev_cfg->bus_speed);

View file

@ -40,16 +40,19 @@ struct mcp2515_data {
/* tx data */
struct k_sem tx_sem;
struct k_mutex tx_mutex;
struct mcp2515_tx_cb tx_cb[MCP2515_TX_CNT];
u8_t tx_busy_map;
/* filter data */
struct k_mutex filter_mutex;
u32_t filter_usage;
can_rx_callback_t rx_cb[CONFIG_CAN_MCP2515_MAX_FILTER];
void *cb_arg[CONFIG_CAN_MCP2515_MAX_FILTER];
struct zcan_filter filter[CONFIG_CAN_MCP2515_MAX_FILTER];
can_state_change_isr_t state_change_isr;
/* general data */
struct k_mutex mutex;
enum can_state old_state;
};
struct mcp2515_config {
@ -85,11 +88,14 @@ struct mcp2515_config {
/* MCP2515 Registers */
#define MCP2515_ADDR_CANSTAT 0x0E
#define MCP2515_ADDR_CANCTRL 0x0F
#define MCP2515_ADDR_TEC 0x1C
#define MCP2515_ADDR_REC 0x1D
#define MCP2515_ADDR_CNF3 0x28
#define MCP2515_ADDR_CNF2 0x29
#define MCP2515_ADDR_CNF1 0x2A
#define MCP2515_ADDR_CANINTE 0x2B
#define MCP2515_ADDR_CANINTF 0x2C
#define MCP2515_ADDR_EFLG 0x2D
#define MCP2515_ADDR_TXB0CTRL 0x30
#define MCP2515_ADDR_TXB1CTRL 0x40
#define MCP2515_ADDR_TXB2CTRL 0x50
@ -123,6 +129,23 @@ struct mcp2515_config {
#define MCP2515_CANINTF_WAKIF BIT(6)
#define MCP2515_CANINTF_MERRF BIT(7)
#define MCP2515_INTE_RX0IE BIT(0)
#define MCP2515_INTE_RX1IE BIT(1)
#define MCP2515_INTE_TX0IE BIT(2)
#define MCP2515_INTE_TX1IE BIT(3)
#define MCP2515_INTE_TX2IE BIT(4)
#define MCP2515_INTE_ERRIE BIT(5)
#define MCP2515_INTE_WAKIE BIT(6)
#define MCP2515_INTE_MERRE BIT(7)
#define MCP2515_EFLG_EWARN BIT(0)
#define MCP2515_EFLG_RXWAR BIT(1)
#define MCP2515_EFLG_TXWAR BIT(2)
#define MCP2515_EFLG_RXEP BIT(3)
#define MCP2515_EFLG_TXEP BIT(4)
#define MCP2515_EFLG_TXBO BIT(5)
#define MCP2515_EFLG_RX0OVR BIT(6)
#define MCP2515_EFLG_RX1OVR BIT(7)
#define MCP2515_TXCTRL_TXREQ BIT(3)

View file

@ -86,6 +86,8 @@ struct mcux_flexcan_data {
ATOMIC_DEFINE(tx_allocs, MCUX_FLEXCAN_MAX_TX);
struct k_sem tx_allocs_sem;
struct mcux_flexcan_tx_callback tx_cbs[MCUX_FLEXCAN_MAX_TX];
enum can_state state;
can_state_change_isr_t state_change_isr;
};
#if (!defined(FSL_FEATURE_FLEXCAN_HAS_ERRATA_9595) || \
@ -384,6 +386,68 @@ static int mcux_flexcan_attach_isr(struct device *dev, can_rx_callback_t isr,
return alloc;
}
static void mcux_flexcan_register_state_change_isr(struct device *dev,
can_state_change_isr_t isr)
{
struct mcux_flexcan_data *data = dev->driver_data;
data->state_change_isr = isr;
}
static enum can_state mcux_flexcan_get_state(struct device *dev,
struct can_bus_err_cnt *err_cnt)
{
const struct mcux_flexcan_config *config = dev->config->config_info;
u32_t status_flags;
if (err_cnt) {
FLEXCAN_GetBusErrCount(config->base, &err_cnt->tx_err_cnt,
&err_cnt->rx_err_cnt);
}
status_flags = (FLEXCAN_GetStatusFlags(config->base) &
CAN_ESR1_FLTCONF_MASK) << CAN_ESR1_FLTCONF_SHIFT;
if (status_flags & 0x02) {
return CAN_BUS_OFF;
}
if (status_flags & 0x01) {
return CAN_ERROR_PASSIVE;
}
return CAN_ERROR_ACTIVE;
}
#ifndef CONFIG_CAN_AUTO_BUS_OFF_RECOVERY
int mcux_flexcan_recover(struct device *dev, s32_t timeout)
{
const struct mcux_flexcan_config *config = dev->config->config_info;
int ret = 0;
u64_t start_time;
if (mcux_flexcan_get_state(dev, NULL) != CAN_BUS_OFF) {
return 0;
}
start_time = k_uptime_get();
config->base->CTRL1 &= ~CAN_CTRL1_BOFFREC_MASK;
if (timeout != K_NO_WAIT) {
while (mcux_flexcan_get_state(dev, NULL) == CAN_BUS_OFF) {
if (timeout != K_FOREVER &&
k_uptime_get() - start_time >= timeout) {
ret = CAN_TIMEOUT;
}
}
}
config->base->CTRL1 |= CAN_CTRL1_BOFFREC_MASK;
return ret;
}
#endif /* CONFIG_CAN_AUTO_BUS_OFF_RECOVERY */
static void mcux_flexcan_detach(struct device *dev, int filter_id)
{
const struct mcux_flexcan_config *config = dev->config->config_info;
@ -421,6 +485,8 @@ static inline void mcux_flexcan_transfer_error_status(struct device *dev,
int status = CAN_TX_OK;
void *arg;
int alloc;
enum can_state state;
struct can_bus_err_cnt err_cnt;
if (error & CAN_ESR1_FLTCONF(2)) {
LOG_DBG("Tx bus off (error 0x%08x)", error);
@ -442,6 +508,14 @@ static inline void mcux_flexcan_transfer_error_status(struct device *dev,
LOG_DBG("Unhandled error (error 0x%08x)", error);
}
state = mcux_flexcan_get_state(dev, &err_cnt);
if (data->state != state) {
data->state = state;
if (data->state_change_isr) {
data->state_change_isr(state, err_cnt);
}
}
if (status == CAN_TX_OK) {
/*
* Error/status is not TX related. No further action
@ -598,6 +672,11 @@ static int mcux_flexcan_init(struct device *dev)
config->irq_config_func(dev);
#ifndef CONFIG_CAN_AUTO_BUS_OFF_RECOVERY
config->base->CTRL1 |= CAN_CTRL1_BOFFREC_MASK;
#endif /* CONFIG_CAN_AUTO_BUS_OFF_RECOVERY */
data->state = mcux_flexcan_get_state(dev, NULL);
return 0;
}
@ -606,6 +685,11 @@ static const struct can_driver_api mcux_flexcan_driver_api = {
.send = mcux_flexcan_send,
.attach_isr = mcux_flexcan_attach_isr,
.detach = mcux_flexcan_detach,
.get_state = mcux_flexcan_get_state,
#ifndef CONFIG_CAN_AUTO_BUS_OFF_RECOVERY
.recover = mcux_flexcan_recover,
#endif
.register_state_change_isr = mcux_flexcan_register_state_change_isr
};
#ifdef CONFIG_CAN_0

View file

@ -18,7 +18,8 @@
#include <logging/log.h>
LOG_MODULE_DECLARE(can_driver, CONFIG_CAN_LOG_LEVEL);
#define INIT_TIMEOUT (10 * sys_clock_hw_cycles_per_sec() / MSEC_PER_SEC)
#define CAN_INIT_TIMEOUT (10 * sys_clock_hw_cycles_per_sec() / MSEC_PER_SEC)
#define CAN_BOFF_RECOVER_TIMEOUT (10 * sys_clock_hw_cycles_per_sec() / MSEC_PER_SEC)
/*
* Translation tables
@ -92,6 +93,32 @@ void can_stm32_rx_isr_handler(CAN_TypeDef *can, struct can_stm32_data *data)
}
}
static inline void can_stm32_bus_state_change_isr(CAN_TypeDef *can,
struct can_stm32_data *data)
{
struct can_bus_err_cnt err_cnt;
enum can_state state;
if (!(can->ESR & CAN_ESR_EPVF) && !(can->ESR & CAN_ESR_BOFF)) {
return;
}
err_cnt.tx_err_cnt = ((can->ESR & CAN_ESR_TEC) >> CAN_ESR_TEC_Pos);
err_cnt.rx_err_cnt = ((can->ESR & CAN_ESR_REC) >> CAN_ESR_REC_Pos);
if (can->ESR & CAN_ESR_BOFF) {
state = CAN_BUS_OFF;
} else if (can->ESR & CAN_ESR_EPVF) {
state = CAN_ERROR_PASSIVE;
} else {
state = CAN_ERROR_ACTIVE;
}
if (data->state_change_isr) {
data->state_change_isr(state, err_cnt);
}
}
static inline
void can_stm32_tx_isr_handler(CAN_TypeDef *can, struct can_stm32_data *data)
{
@ -156,7 +183,10 @@ static void can_stm32_isr(void *arg)
can_stm32_tx_isr_handler(can, data);
can_stm32_rx_isr_handler(can, data);
if (can->MSR & CAN_MSR_ERRI) {
can_stm32_bus_state_change_isr(can, data);
can->MSR |= CAN_MSR_ERRI;
}
}
#else
@ -191,6 +221,27 @@ static void can_stm32_tx_isr(void *arg)
can_stm32_tx_isr_handler(can, data);
}
static void can_stm32_state_change_isr(void *arg)
{
struct device *dev;
struct can_stm32_data *data;
const struct can_stm32_config *cfg;
CAN_TypeDef *can;
dev = (struct device *)arg;
data = DEV_DATA(dev);
cfg = DEV_CFG(dev);
can = cfg->can;
/*Signal bus-off to waiting tx*/
if (can->MSR & CAN_MSR_ERRI) {
can_stm32_tx_isr_handler(can, data);
can_stm32_bus_state_change_isr(can, data);
can->MSR |= CAN_MSR_ERRI;
}
}
#endif
static int can_enter_init_mode(CAN_TypeDef *can)
@ -201,7 +252,8 @@ static int can_enter_init_mode(CAN_TypeDef *can)
start_time = k_cycle_get_32();
while ((can->MSR & CAN_MSR_INAK) == 0U) {
if (k_cycle_get_32() - start_time > INIT_TIMEOUT) {
if (k_cycle_get_32() - start_time > CAN_INIT_TIMEOUT) {
can->MCR &= ~CAN_MCR_INRQ;
return CAN_TIMEOUT;
}
}
@ -217,7 +269,7 @@ static int can_leave_init_mode(CAN_TypeDef *can)
start_time = k_cycle_get_32();
while ((can->MSR & CAN_MSR_INAK) != 0U) {
if (k_cycle_get_32() - start_time > INIT_TIMEOUT) {
if (k_cycle_get_32() - start_time > CAN_INIT_TIMEOUT) {
return CAN_TIMEOUT;
}
}
@ -233,7 +285,7 @@ static int can_leave_sleep_mode(CAN_TypeDef *can)
start_time = k_cycle_get_32();
while ((can->MSR & CAN_MSR_SLAK) != 0) {
if (k_cycle_get_32() - start_time > INIT_TIMEOUT) {
if (k_cycle_get_32() - start_time > CAN_INIT_TIMEOUT) {
return CAN_TIMEOUT;
}
}
@ -247,6 +299,7 @@ int can_stm32_runtime_configure(struct device *dev, enum can_mode mode,
CAN_HandleTypeDef hcan;
const struct can_stm32_config *cfg = DEV_CFG(dev);
CAN_TypeDef *can = cfg->can;
struct can_stm32_data *data = DEV_DATA(dev);
struct device *clock;
u32_t clock_rate;
u32_t prescaler;
@ -301,10 +354,11 @@ int can_stm32_runtime_configure(struct device *dev, enum can_mode mode,
(mode == CAN_SILENT_MODE) ? CAN_BTR_SILM :
CAN_BTR_LBKM | CAN_BTR_SILM;
k_mutex_lock(&data->inst_mutex, K_FOREVER);
ret = can_enter_init_mode(can);
if (ret) {
LOG_ERR("Failed to enter init mode");
return ret;
goto done;
}
can->BTR = reg_mode | sjw | ts1 | ts2 | (prescaler - 1U);
@ -312,11 +366,14 @@ int can_stm32_runtime_configure(struct device *dev, enum can_mode mode,
ret = can_leave_init_mode(can);
if (ret) {
LOG_ERR("Failed to leave init mode");
return ret;
goto done;
}
LOG_DBG("Runtime configure of %s done", dev->config->name);
return 0;
ret = 0;
done:
k_mutex_unlock(&data->inst_mutex);
return ret;
}
static int can_stm32_init(struct device *dev)
@ -327,8 +384,7 @@ static int can_stm32_init(struct device *dev)
struct device *clock;
int ret;
k_mutex_init(&data->tx_mutex);
k_mutex_init(&data->set_filter_mutex);
k_mutex_init(&data->inst_mutex);
k_sem_init(&data->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);
@ -336,6 +392,7 @@ static int can_stm32_init(struct device *dev)
data->mb0.tx_callback = NULL;
data->mb1.tx_callback = NULL;
data->mb2.tx_callback = NULL;
data->state_change_isr = NULL;
data->filter_usage = (1ULL << CAN_MAX_NUMBER_OF_FILTERS) - 1ULL;
(void)memset(data->rx_cb, 0, sizeof(data->rx_cb));
@ -369,6 +426,9 @@ static int can_stm32_init(struct device *dev)
#ifdef CONFIG_CAN_RX_TIMESTAMP
can->MCR |= CAN_MCR_TTCM;
#endif
#ifdef CONFIG_CAN_AUTO_BUS_OFF_RECOVERY
can->MCR |= CAN_MCR_ABOM;
#endif
ret = can_stm32_runtime_configure(dev, CAN_NORMAL_MODE, 0);
if (ret) {
@ -384,6 +444,88 @@ static int can_stm32_init(struct device *dev)
return 0;
}
static void can_stm32_register_state_change_isr(struct device *dev,
can_state_change_isr_t isr)
{
struct can_stm32_data *data = DEV_DATA(dev);
const struct can_stm32_config *cfg = DEV_CFG(dev);
CAN_TypeDef *can = cfg->can;
data->state_change_isr = isr;
if (isr == NULL) {
can->IER &= ~CAN_IER_EPVIE;
} else {
can->IER |= CAN_IER_EPVIE;
}
}
static enum can_state can_stm32_get_state(struct device *dev,
struct can_bus_err_cnt *err_cnt)
{
const struct can_stm32_config *cfg = DEV_CFG(dev);
CAN_TypeDef *can = cfg->can;
if (err_cnt) {
err_cnt->tx_err_cnt =
((can->ESR & CAN_ESR_TEC) >> CAN_ESR_TEC_Pos);
err_cnt->rx_err_cnt =
((can->ESR & CAN_ESR_REC) >> CAN_ESR_REC_Pos);
}
if (can->ESR & CAN_ESR_BOFF) {
return CAN_BUS_OFF;
}
if (can->ESR & CAN_ESR_EPVF) {
return CAN_ERROR_PASSIVE;
}
return CAN_ERROR_ACTIVE;
}
#ifndef CONFIG_CAN_AUTO_BUS_OFF_RECOVERY
int can_stm32_recover(struct device *dev, s32_t timeout)
{
const struct can_stm32_config *cfg = DEV_CFG(dev);
struct can_stm32_data *data = DEV_DATA(dev);
CAN_TypeDef *can = cfg->can;
int ret = CAN_TIMEOUT;
u32_t start_time;
if (!(can->ESR & CAN_ESR_BOFF)) {
return 0;
}
if (k_mutex_lock(&data->inst_mutex, K_FOREVER)) {
return CAN_TIMEOUT;
}
ret = can_enter_init_mode(can);
if (ret) {
goto done;
}
can_leave_init_mode(can);
start_time = k_cycle_get_32();
while (can->ESR & CAN_ESR_BOFF) {
if (k_cycle_get_32() - start_time >= CAN_BOFF_RECOVER_TIMEOUT) {
goto done;
}
}
ret = 0;
done:
k_mutex_unlock(&data->inst_mutex);
return ret;
}
#endif /* CONFIG_CAN_AUTO_BUS_OFF_RECOVERY */
int can_stm32_send(struct device *dev, const struct zcan_frame *msg,
s32_t timeout, can_tx_callback_t callback, void *callback_arg)
{
@ -392,7 +534,6 @@ int can_stm32_send(struct device *dev, const struct zcan_frame *msg,
CAN_TypeDef *can = cfg->can;
u32_t transmit_status_register = can->TSR;
CAN_TxMailBox_TypeDef *mailbox = NULL;
struct k_mutex *tx_mutex = &data->tx_mutex;
struct can_mailbox *mb = NULL;
LOG_DBG("Sending %d bytes on %s. "
@ -413,16 +554,16 @@ int can_stm32_send(struct device *dev, const struct zcan_frame *msg,
return CAN_TX_BUS_OFF;
}
k_mutex_lock(tx_mutex, K_FOREVER);
k_mutex_lock(&data->inst_mutex, K_FOREVER);
while (!(transmit_status_register & CAN_TSR_TME)) {
k_mutex_unlock(tx_mutex);
k_mutex_unlock(&data->inst_mutex);
LOG_DBG("Transmit buffer full. Wait with timeout (%dms)",
timeout);
if (k_sem_take(&data->tx_int_sem, timeout)) {
return CAN_TIMEOUT;
}
k_mutex_lock(tx_mutex, K_FOREVER);
k_mutex_lock(&data->inst_mutex, K_FOREVER);
transmit_status_register = can->TSR;
}
@ -465,7 +606,7 @@ int can_stm32_send(struct device *dev, const struct zcan_frame *msg,
mailbox->TDHR = msg->data_32[1];
mailbox->TIR |= CAN_TI0R_TXRQ;
k_mutex_unlock(tx_mutex);
k_mutex_unlock(&data->inst_mutex);
if (callback == NULL) {
k_sem_take(&mb->tx_int_sem, K_FOREVER);
@ -817,9 +958,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;
k_mutex_lock(&data->set_filter_mutex, K_FOREVER);
k_mutex_lock(&data->inst_mutex, K_FOREVER);
filter_nr = can_stm32_attach(dev, isr, cb_arg, filter);
k_mutex_unlock(&data->set_filter_mutex);
k_mutex_unlock(&data->inst_mutex);
return filter_nr;
}
@ -838,7 +979,7 @@ void can_stm32_detach(struct device *dev, int filter_nr)
__ASSERT_NO_MSG(filter_nr >= 0 && filter_nr < CAN_MAX_NUMBER_OF_FILTERS);
k_mutex_lock(&data->set_filter_mutex, K_FOREVER);
k_mutex_lock(&data->inst_mutex, K_FOREVER);
bank_nr = filter_nr / 4;
bank_bit = (1U << bank_nr);
@ -870,14 +1011,19 @@ void can_stm32_detach(struct device *dev, int filter_nr)
data->rx_cb[filter_index] = NULL;
data->cb_arg[filter_index] = NULL;
k_mutex_unlock(&data->set_filter_mutex);
k_mutex_unlock(&data->inst_mutex);
}
static const struct can_driver_api can_api_funcs = {
.configure = can_stm32_runtime_configure,
.send = can_stm32_send,
.attach_isr = can_stm32_attach_isr,
.detach = can_stm32_detach
.detach = can_stm32_detach,
.get_state = can_stm32_get_state,
#ifndef CONFIG_CAN_AUTO_BUS_OFF_RECOVERY
.recover = can_stm32_recover,
#endif
.register_state_change_isr = can_stm32_register_state_change_isr
};
#ifdef CONFIG_CAN_1
@ -921,7 +1067,7 @@ static void config_can_1_irq(CAN_TypeDef *can)
irq_enable(DT_CAN_1_IRQ_TX);
IRQ_CONNECT(DT_CAN_1_IRQ_SCE, DT_CAN_1_IRQ_PRIORITY,
can_stm32_tx_isr, DEVICE_GET(can_stm32_1), 0);
can_stm32_state_change_isr, DEVICE_GET(can_stm32_1), 0);
irq_enable(DT_CAN_1_IRQ_SCE);
#endif
can->IER |= CAN_IER_TMEIE | CAN_IER_ERRIE | CAN_IER_FMPIE0 |

View file

@ -56,8 +56,7 @@ enum can_filter_type {
};
struct can_stm32_data {
struct k_mutex tx_mutex;
struct k_mutex set_filter_mutex;
struct k_mutex inst_mutex;
struct k_sem tx_int_sem;
struct can_mailbox mb0;
struct can_mailbox mb1;
@ -65,6 +64,7 @@ struct can_stm32_data {
u64_t filter_usage;
can_rx_callback_t rx_cb[CONFIG_CAN_MAX_FILTER];
void *cb_arg[CONFIG_CAN_MAX_FILTER];
can_state_change_isr_t state_change_isr;
};
struct can_stm32_config {