diff --git a/embassy-stm32/src/can/enums.rs b/embassy-stm32/src/can/enums.rs index 135010011..36139a45c 100644 --- a/embassy-stm32/src/can/enums.rs +++ b/embassy-stm32/src/can/enums.rs @@ -1,5 +1,4 @@ //! Enums shared between CAN controller types. -use core::convert::TryFrom; /// Bus error #[derive(Debug)] @@ -29,19 +28,3 @@ pub enum BusError { /// At least one of error counter has reached the Error_Warning limit of 96. BusWarning, } -impl TryFrom<u8> for BusError { - type Error = (); - fn try_from(value: u8) -> Result<Self, Self::Error> { - match value { - //0b000 => None, - 0b001 => Ok(Self::Stuff), - 0b010 => Ok(Self::Form), - 0b011 => Ok(Self::Acknowledge), - 0b100 => Ok(Self::BitRecessive), - 0b101 => Ok(Self::BitDominant), - 0b110 => Ok(Self::Crc), - //0b111 => Ok(Self::NoError), - _ => Err(()), - } - } -} diff --git a/embassy-stm32/src/can/fd/peripheral.rs b/embassy-stm32/src/can/fd/peripheral.rs index 3422d7148..487b8f413 100644 --- a/embassy-stm32/src/can/fd/peripheral.rs +++ b/embassy-stm32/src/can/fd/peripheral.rs @@ -3,6 +3,9 @@ use core::convert::Infallible; use core::slice; +use cfg_if::cfg_if; + +use crate::can::enums::*; use crate::can::fd::config::*; use crate::can::fd::message_ram::enums::*; use crate::can::fd::message_ram::{RegisterBlock, RxFifoElement, TxBufferElement}; @@ -98,6 +101,42 @@ impl Registers { self.regs.txbar().modify(|w| w.set_ar(bufidx, true)); } + fn reg_to_error(value: u8) -> Option<BusError> { + match value { + //0b000 => None, + 0b001 => Some(BusError::Stuff), + 0b010 => Some(BusError::Form), + 0b011 => Some(BusError::Acknowledge), + 0b100 => Some(BusError::BitRecessive), + 0b101 => Some(BusError::BitDominant), + 0b110 => Some(BusError::Crc), + //0b111 => Some(BusError::NoError), + _ => None, + } + } + + pub fn curr_error(&self) -> Option<BusError> { + let err = { self.regs.psr().read() }; + if err.bo() { + return Some(BusError::BusOff); + } else if err.ep() { + return Some(BusError::BusPassive); + } else if err.ew() { + return Some(BusError::BusWarning); + } else { + cfg_if! { + if #[cfg(stm32h7)] { + let lec = err.lec(); + } else { + let lec = err.lec().to_bits(); + } + } + if let Some(err) = Self::reg_to_error(lec) { + return Some(err); + } + } + None + } /// Returns if the tx queue is able to accept new messages without having to cancel an existing one #[inline] pub fn tx_queue_is_full(&self) -> bool { diff --git a/embassy-stm32/src/can/fdcan.rs b/embassy-stm32/src/can/fdcan.rs index b94e42707..987748e06 100644 --- a/embassy-stm32/src/can/fdcan.rs +++ b/embassy-stm32/src/can/fdcan.rs @@ -4,7 +4,6 @@ use core::marker::PhantomData; use core::task::Poll; pub mod fd; -use cfg_if::cfg_if; use embassy_hal_internal::{into_ref, PeripheralRef}; use fd::config::*; use fd::filter::*; @@ -184,29 +183,6 @@ fn calc_timestamp<T: Instance>(_ns_per_timer_tick: u64, ts_val: u16) -> Timestam ts_val } -fn curr_error<T: Instance>() -> Option<BusError> { - let err = { T::regs().psr().read() }; - if err.bo() { - return Some(BusError::BusOff); - } else if err.ep() { - return Some(BusError::BusPassive); - } else if err.ew() { - return Some(BusError::BusWarning); - } else { - cfg_if! { - if #[cfg(stm32h7)] { - let lec = err.lec(); - } else { - let lec = err.lec().to_bits(); - } - } - if let Ok(err) = BusError::try_from(lec) { - return Some(err); - } - } - None -} - impl<'d, T: Instance> Fdcan<'d, T, ConfigMode> { /// Creates a new Fdcan instance, keeping the peripheral in sleep mode. /// You must call [Fdcan::enable_non_blocking] to use the peripheral. @@ -426,7 +402,7 @@ where } else if let Some((msg, ts)) = T::registers().read_classic(1) { let ts = calc_timestamp::<T>(self.ns_per_timer_tick, ts); return Poll::Ready(Ok((msg, ts))); - } else if let Some(err) = curr_error::<T>() { + } else if let Some(err) = T::registers().curr_error() { // TODO: this is probably wrong return Poll::Ready(Err(err)); } @@ -466,7 +442,7 @@ where } else if let Some((msg, ts)) = T::registers().read_fd(1) { let ts = calc_timestamp::<T>(self.ns_per_timer_tick, ts); return Poll::Ready(Ok((msg, ts))); - } else if let Some(err) = curr_error::<T>() { + } else if let Some(err) = T::registers().curr_error() { // TODO: this is probably wrong return Poll::Ready(Err(err)); } @@ -560,7 +536,7 @@ impl<'c, 'd, T: Instance, M: Receive> FdcanRx<'d, T, M> { } else if let Some((msg, ts)) = T::registers().read_classic(1) { let ts = calc_timestamp::<T>(self.ns_per_timer_tick, ts); return Poll::Ready(Ok((msg, ts))); - } else if let Some(err) = curr_error::<T>() { + } else if let Some(err) = T::registers().curr_error() { // TODO: this is probably wrong return Poll::Ready(Err(err)); } @@ -581,7 +557,7 @@ impl<'c, 'd, T: Instance, M: Receive> FdcanRx<'d, T, M> { } else if let Some((msg, ts)) = T::registers().read_fd(1) { let ts = calc_timestamp::<T>(self.ns_per_timer_tick, ts); return Poll::Ready(Ok((msg, ts))); - } else if let Some(err) = curr_error::<T>() { + } else if let Some(err) = T::registers().curr_error() { // TODO: this is probably wrong return Poll::Ready(Err(err)); }