diff --git a/Cargo.toml b/Cargo.toml index 52743e56..8b85a14f 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -16,6 +16,9 @@ cortex-m = "0.7.1" nb = "0.1.1" stm32g4 = "0.13.0" paste = "1.0" +bitflags = "1.2" +vcell = "0.1" +static_assertions = "1.1" [dependencies.cast] version = "0.2.7" @@ -40,6 +43,10 @@ version = "1.0.2" default-features = false version = "1.1" +[dependencies.defmt] +version = "0.2.3" +optional = true + [dev-dependencies] cortex-m-rt = "0.6.10" cortex-m-rtfm = "0.5.1" @@ -55,6 +62,7 @@ rtt-target = { version = "0.3.0", features = ["cortex-m"] } panic-rtt-target = { version = "0.1.1", features = ["cortex-m"] } mpu6050 = "0.1.4" +#TODO: Separate feature sets [features] default = ["rt"] rt = ["stm32g4/rt"] @@ -70,6 +78,7 @@ stm32g4a1 = ["stm32g4/stm32g4a1"] log-itm = ["cortex-m-log/itm"] log-rtt = [] log-semihost = ["cortex-m-log/semihosting"] +unstable-defmt = ["defmt"] [profile.dev] codegen-units = 1 diff --git a/examples/can-echo.rs b/examples/can-echo.rs new file mode 100644 index 00000000..2c4fed4d --- /dev/null +++ b/examples/can-echo.rs @@ -0,0 +1,150 @@ +#![no_main] +#![no_std] + +use crate::hal::{ + fdcan::{ + config::NominalBitTiming, + filter::{StandardFilter, StandardFilterSlot}, + frame::{FrameFormat, TxFrameHeader}, + id::StandardId, + FdCan, + }, + gpio::{GpioExt as _, Speed}, + nb::block, + rcc::{Config, RccExt, SysClockSrc}, + stm32::Peripherals, + time::U32Ext, +}; +use stm32g4xx_hal as hal; + +use core::num::{NonZeroU16, NonZeroU8}; + +use cortex_m_rt::entry; + +use log::info; + +#[macro_use] +mod utils; + +#[entry] +fn main() -> ! { + utils::logger::init(); + + info!("Start"); + + // APB1 (HSE): 24MHz, Bit rate: 125kBit/s, Sample Point 87.5% + // Value was calculated with http://www.bittiming.can-wiki.info/ + // TODO: use the can_bit_timings crate + let btr = NominalBitTiming { + prescaler: NonZeroU16::new(12).unwrap(), + seg1: NonZeroU8::new(13).unwrap(), + seg2: NonZeroU8::new(2).unwrap(), + sync_jump_width: NonZeroU8::new(1).unwrap(), + }; + + info!("Init Clocks"); + + let dp = Peripherals::take().unwrap(); + let _cp = cortex_m::Peripherals::take().expect("cannot take core peripherals"); + let rcc = dp.RCC.constrain(); + let mut rcc = rcc.freeze(Config::new(SysClockSrc::HSE(24.mhz()))); + + info!("Split GPIO"); + + let gpiob = dp.GPIOB.split(&mut rcc); + + let can1 = { + info!("Init CAN 1"); + let rx = gpiob.pb8.into_alternate().set_speed(Speed::VeryHigh); + let tx = gpiob.pb9.into_alternate().set_speed(Speed::VeryHigh); + + info!("-- Create CAN 1 instance"); + let can = FdCan::new(dp.FDCAN1, tx, rx, &rcc); + + info!("-- Set CAN 1 in Config Mode"); + let mut can = can.into_config_mode(); + can.set_protocol_exception_handling(false); + + info!("-- Configure nominal timing"); + can.set_nominal_bit_timing(btr); + + info!("-- Configure Filters"); + can.set_standard_filter( + StandardFilterSlot::_0, + StandardFilter::accept_all_into_fifo0(), + ); + + info!("-- Current Config: {:#?}", can.get_config()); + + info!("-- Set CAN1 in to normal mode"); + // can.into_external_loopback() + can.into_normal() + }; + + // let can2 = { + // info!("Init CAN 2"); + // let rx = gpiob.pb5.into_alternate().set_speed(Speed::VeryHigh); + // let tx = gpiob.pb13.into_alternate().set_speed(Speed::VeryHigh); + + // info!("-- Create CAN 2 instance"); + // let can = FdCan::new(dp.FDCAN2, tx, rx, &rcc); + + // info!("-- Set CAN in Config Mode"); + // let mut can = can.into_config_mode(); + + // info!("-- Configure nominal timing"); + // can.set_nominal_bit_timing(btr); + + // info!("-- Configure Filters"); + // can.set_standard_filter( + // StandardFilterSlot::_0, + // StandardFilter::accept_all_into_fifo0(), + // ); + + // info!("-- Set CAN2 in to normal mode"); + // // can.into_external_loopback() + // can.into_normal() + // }; + + let mut can = can1; + + info!("Create Message Data"); + let mut buffer = [0xAAAAAAAA, 0xFFFFFFFF, 0x0, 0x0, 0x0, 0x0]; + info!("Create Message Header"); + let header = TxFrameHeader { + len: 2 * 4, + id: StandardId::new(0x1).unwrap().into(), + frame_format: FrameFormat::Standard, + bit_rate_switching: false, + marker: None, + }; + info!("Initial Header: {:#X?}", &header); + + info!("Transmit initial message"); + block!(can.transmit(header, &mut |b| { + let len = b.len(); + b[..len].clone_from_slice(&buffer[..len]); + },)) + .unwrap(); + + loop { + if let Ok(rxheader) = block!(can.receive0(&mut |h, b| { + info!("Received Header: {:#X?}", &h); + info!("received data: {:X?}", &b); + + for (i, d) in b.iter().enumerate() { + buffer[i] = *d; + } + h + })) { + block!( + can.transmit(rxheader.unwrap().to_tx_header(None), &mut |b| { + let len = b.len(); + b[..len].clone_from_slice(&buffer[..len]); + info!("Transmit: {:X?}", b); + }) + ) + .unwrap(); + } + } +} diff --git a/src/can.rs b/src/can.rs new file mode 100644 index 00000000..112ddaff --- /dev/null +++ b/src/can.rs @@ -0,0 +1,226 @@ +//! # Controller Area Network (CAN) Interface +//! + +use crate::fdcan; +use crate::fdcan::message_ram; +// use crate::stm32::{self, FDCAN1, FDCAN2, FDCAN3}; +use crate::rcc::Rcc; +use crate::stm32; + +mod sealed { + // Prevent creation outside of this module + pub trait Sealed {} + /// A TX pin configured for CAN communication + pub trait Tx {} + /// An RX pin configured for CAN communication + pub trait Rx {} +} + +/// Implements sealed::{Tx,Rx} for pins associated with a CAN peripheral +macro_rules! pins { + ($PER:ident => + (tx: [ $($( #[ $pmetatx:meta ] )* $tx:ident<$txaf:ident>),+ $(,)? ], + rx: [ $($( #[ $pmetarx:meta ] )* $rx:ident<$rxaf:ident>),+ $(,)? ])) => { + $( + $( #[ $pmetatx ] )* + impl crate::can::sealed::Tx<$PER> for $tx> {} + )+ + $( + $( #[ $pmetarx ] )* + impl crate::can::sealed::Rx<$PER> for $rx> {} + )+ + }; +} + +mod fdcan1 { + use super::FdCan; + use crate::fdcan; + use crate::fdcan::message_ram; + use crate::gpio::{ + gpioa::{PA11, PA12}, + gpiob::{PB8, PB9}, + gpiod::{PD0, PD1}, + AF9, + }; + use crate::rcc::Rcc; + use crate::stm32; + use crate::stm32::FDCAN1; + + // All STM32G4 models with CAN support these pins + pins! { + FDCAN1 => ( + tx: [ + PA12, + PB9, + PD1, + ], + rx: [ + PA11, + PB8, + PD0, + ] + ) + } + + unsafe impl fdcan::Instance for FdCan { + const REGISTERS: *mut stm32::fdcan::RegisterBlock = FDCAN1::ptr() as *mut _; + } + + unsafe impl message_ram::MsgRamExt for FdCan { + const MSG_RAM: *mut message_ram::RegisterBlock = (0x4000_a400 as *mut _); + } + + /// Implements sealed::Sealed and Enable for a CAN peripheral (e.g. CAN1) + impl crate::can::sealed::Sealed for crate::stm32::FDCAN1 {} + impl crate::can::Enable for crate::stm32::FDCAN1 { + #[inline(always)] + fn enable(rcc: &Rcc) { + // TODO: make this configurable + // Select P clock as FDCAN clock source + rcc.rb.ccipr.modify(|_, w| { + // This is sound, as `0b10` is a valid value for this field. + unsafe { + w.fdcansel().bits(0b10); + } + + w + }); + + // Enable peripheral + rcc.rb.apb1enr1.modify(|_, w| w.fdcanen().set_bit()); + } + } +} + +#[cfg(any( + feature = "stm32g471", + feature = "stm32g473", + feature = "stm32g474", + feature = "stm32g483", + feature = "stm32g484", + feature = "stm32g491", + feature = "stm32g4A1", +))] +mod fdcan2 { + use super::FdCan; + use crate::fdcan; + use crate::fdcan::message_ram; + use crate::gpio::{ + gpiob::{PB12, PB13, PB5, PB6}, + AF9, + }; + use crate::rcc::Rcc; + use crate::stm32::{self, FDCAN2}; + + pins! { + FDCAN2 => ( + tx: [ + PB6, + PB13, + ], + rx: [ + PB5, + PB12, + ]) + } + + unsafe impl fdcan::Instance for FdCan { + const REGISTERS: *mut stm32::fdcan::RegisterBlock = FDCAN2::ptr() as *mut _; + } + + unsafe impl message_ram::MsgRamExt for FdCan { + // const MSG_RAM: *mut message_ram::RegisterBlock = (0x4000_a754 as *mut _); + const MSG_RAM: *mut message_ram::RegisterBlock = (0x4000_a750 as *mut _); + } + + impl crate::can::sealed::Sealed for crate::stm32::FDCAN2 {} + impl crate::can::Enable for crate::stm32::FDCAN2 { + #[inline(always)] + fn enable(rcc: &Rcc) { + // Enable peripheral + rcc.rb.apb1enr1.modify(|_, w| w.fdcanen().set_bit()); + + // TODO: make this configurable + // Select P clock as FDCAN clock source + rcc.rb.ccipr.modify(|_, w| { + // This is sound, as `0b10` is a valid value for this field. + unsafe { + w.fdcansel().bits(0b10); + } + + w + }); + } + } +} + +#[cfg(any( + feature = "stm32g473", + feature = "stm32g474", + feature = "stm32g483", + feature = "stm32g484", +))] +mod fdcan3 { + use super::FdCan; + use crate::fdcan; + use crate::fdcan::message_ram; + use crate::gpio::{ + gpioa::{PA15, PA8}, + gpiob::{PB3, PB4}, + AF11, + }; + use crate::rcc::Rcc; + use crate::stm32::{self, FDCAN3}; + + pins! { + FDCAN3 => ( + tx: [ + PA15, + PB4, + ], + rx: [ + PA8, + PB3, + ]) + } + + unsafe impl fdcan::Instance for FdCan { + const REGISTERS: *mut stm32::fdcan::RegisterBlock = FDCAN3::ptr() as *mut _; + } + + unsafe impl message_ram::MsgRamExt for FdCan { + const MSG_RAM: *mut message_ram::RegisterBlock = (0x4000_aaa0 as *mut _); + } +} + +/// Enable/disable peripheral +pub trait Enable: sealed::Sealed { + /// Enables this peripheral by setting the associated enable bit in an RCC enable register + fn enable(rcc: &Rcc); +} + +/// Interface to the CAN peripheral. +pub struct FdCan { + _peripheral: Instance, +} + +impl FdCan +where + Instance: Enable, +{ + /// Creates a CAN interface. + pub fn new(can: Instance, tx: TX, rx: RX, rcc: &Rcc) -> FdCan + where + TX: sealed::Tx, + RX: sealed::Rx, + { + Instance::enable(rcc); + //TODO: Set Speed to VeryHigh? + FdCan { _peripheral: can } + } + + pub fn new_unchecked(can: Instance, rcc: &Rcc) -> FdCan { + Instance::enable(rcc); + FdCan { _peripheral: can } + } +} diff --git a/src/fdcan.rs b/src/fdcan.rs new file mode 100644 index 00000000..018c271b --- /dev/null +++ b/src/fdcan.rs @@ -0,0 +1,1770 @@ +#![deny(missing_docs)] + +//! FdCAN Operations + +/// Configuration of an FdCAN instance +pub mod config; +#[cfg(feature = "embedded-can-03")] +mod embedded_can; +/// Filtering of CAN Messages +pub mod filter; +/// Header and info of transmitted and receiving frames +pub mod frame; +/// Standard and Extended Id +pub mod id; +/// Interrupt Line Information +pub mod interrupt; +mod message_ram; + +use id::{Id, IdReg}; + +use crate::rcc::Rcc; +use crate::stm32::fdcan::RegisterBlock; +use config::{ + ClockDivider, DataBitTiming, FdCanConfig, FrameTransmissionConfig, GlobalFilter, + NominalBitTiming, TimestampSource, +}; +use filter::{ + ActivateFilter as _, ExtendedFilter, ExtendedFilterSlot, StandardFilter, StandardFilterSlot, + EXTENDED_FILTER_MAX, STANDARD_FILTER_MAX, +}; +use frame::MergeTxFrameHeader; +use frame::{RxFrameInfo, TxFrameHeader}; +use interrupt::{Interrupt, InterruptLine, Interrupts}; + +use message_ram::MsgRamExt; +use message_ram::RxFifoElement; + +use core::cmp::Ord; +use core::convert::Infallible; +use core::convert::TryFrom; +use core::marker::PhantomData; +use core::ptr::NonNull; + +mod sealed { + /// A TX pin configured for CAN communication + pub trait Tx {} + /// An RX pin configured for CAN communication + pub trait Rx {} +} + +/// An FdCAN peripheral instance. +/// +/// This trait is meant to be implemented for a HAL-specific type that represent ownership of +/// the CAN peripheral (and any pins required by it, although that is entirely up to the HAL). +/// +/// # Safety +/// +/// It is only safe to implement this trait, when: +/// +/// * The implementing type has ownership of the peripheral, preventing any other accesses to the +/// register block. +/// * `REGISTERS` is a pointer to that peripheral's register block and can be safely accessed for as +/// long as ownership or a borrow of the implementing type is present. +pub unsafe trait Instance: MsgRamExt + crate::rcc::Instance { + /// Pointer to the instance's register block. + const REGISTERS: *mut RegisterBlock; +} + +/// Indicates if an Receive Overflow has occurred +#[derive(Clone, Copy, Debug)] +#[cfg_attr(feature = "unstable-defmt", derive(defmt::Format))] +pub enum ReceiveErrorOverflow { + /// No overflow has occurred + Normal(u8), + /// An overflow has occurred + Overflow(u8), +} + +///Error Counters +#[derive(Clone, Copy, Debug)] +#[cfg_attr(feature = "unstable-defmt", derive(defmt::Format))] +#[allow(dead_code)] +pub struct ErrorCounters { + /// General CAN error counter + can_errors: u8, + /// Receive CAN error counter + receive_err: ReceiveErrorOverflow, + /// Transmit CAN error counter + transmit_err: u8, +} + +/// Loopback Mode +#[derive(Clone, Copy, Debug)] +#[cfg_attr(feature = "unstable-defmt", derive(defmt::Format))] +enum LoopbackMode { + None, + Internal, + External, +} + +/// Bus Activity +#[derive(Clone, Copy, Debug)] +#[cfg_attr(feature = "unstable-defmt", derive(defmt::Format))] +pub enum Activity { + /// Node is Synchronizing + Synchronizing = 0b00, + /// Node is Idle + Idle = 0b01, + /// Node is receiver only + Receiver = 0b10, + /// Node is transmitter only + Transmitter = 0b11, +} +impl TryFrom for Activity { + type Error = (); + fn try_from(value: u8) -> Result { + match value { + 0b000 => Ok(Self::Synchronizing), + 0b001 => Ok(Self::Idle), + 0b010 => Ok(Self::Receiver), + 0b011 => Ok(Self::Transmitter), + _ => Err(()), + } + } +} + +/// Indicates the type of the last error which occurred on the CAN bus +#[derive(Clone, Copy, Debug)] +#[cfg_attr(feature = "unstable-defmt", derive(defmt::Format))] +pub enum LastErrorCode { + /// There has been no error since last read + NoError = 0b000, + /// More than 5 equal bits in sequence are not allowed + StuffError = 0b001, + /// a fixed format part of ta received frame had the wrong format + FormError = 0b010, + /// message tramsitted by this node was not acknowledged by another + AckError = 0b011, + /// During transmit, the node wanted to send a 1 but monitored a 0 + Bit1Error = 0b100, + /// During transmit, the node wanted to send a 0 but monitored a 1 + Bit0Error = 0b101, + /// CRC checksum of a received message was incorrect + CRCError = 0b110, + /// No CAN bus event detected since last read + NoChange = 0b111, +} +impl TryFrom for LastErrorCode { + type Error = (); + fn try_from(value: u8) -> Result { + match value { + 0b000 => Ok(Self::NoError), + 0b001 => Ok(Self::StuffError), + 0b010 => Ok(Self::FormError), + 0b011 => Ok(Self::AckError), + 0b100 => Ok(Self::Bit1Error), + 0b101 => Ok(Self::Bit0Error), + 0b110 => Ok(Self::CRCError), + 0b111 => Ok(Self::NoChange), + _ => Err(()), + } + } +} + +/// Some status indications regarding the FDCAN protocl +#[derive(Clone, Copy, Debug)] +#[cfg_attr(feature = "unstable-defmt", derive(defmt::Format))] +pub struct ProtocolStatus { + /// Type of current activity + activity: Activity, + /// Transmitter delay companstation + transmitter_delay_comp: u8, + /// But Off Status + bus_off_status: bool, + /// Shows if Error counters are aat their limit of 96 + error_warning: bool, + /// Shows if the node send and active error flag (false) or stays silent (true). + error_passive_state: bool, + /// Indicates te last type of error which occurred on the CAN bus. + last_error: LastErrorCode, +} + +/// Allows for Transmit Operations +pub trait Transmit {} +/// Allows for Receive Operations +pub trait Receive {} + +/// Allows for the FdCan Instance to be released or to enter ConfigMode +pub struct PoweredDownMode; +/// Allows for the configuration for the Instance +pub struct ConfigMode; +/// This mode can be used for a “Hot Selftest”, meaning the FDCAN can be tested without +/// affecting a running CAN system connected to the FDCAN_TX and FDCAN_RX pins. In this +/// mode, FDCAN_RX pin is disconnected from the FDCAN and FDCAN_TX pin is held +/// recessive. +pub struct InternalLoopbackMode; +impl Transmit for InternalLoopbackMode {} +impl Receive for InternalLoopbackMode {} +/// This mode is provided for hardware self-test. To be independent from external stimulation, +/// the FDCAN ignores acknowledge errors (recessive bit sampled in the acknowledge slot of a +/// data / remote frame) in Loop Back mode. In this mode the FDCAN performs an internal +/// feedback from its transmit output to its receive input. The actual value of the FDCAN_RX +/// input pin is disregarded by the FDCAN. The transmitted messages can be monitored at the +/// FDCAN_TX transmit pin. +pub struct ExternalLoopbackMode; +impl Transmit for ExternalLoopbackMode {} +impl Receive for ExternalLoopbackMode {} +/// The normal use of the FdCan instance after configurations +pub struct NormalOperationMode; +impl Transmit for NormalOperationMode {} +impl Receive for NormalOperationMode {} +/// In Restricted operation mode the node is able to receive data and remote frames and to give +/// acknowledge to valid frames, but it does not send data frames, remote frames, active error +/// frames, or overload frames. In case of an error condition or overload condition, it does not +/// send dominant bits, instead it waits for the occurrence of bus idle condition to resynchronize +/// itself to the CAN communication. The error counters for transmit and receive are frozen while +/// error logging (can_errors) is active. TODO: automatically enter in this mode? +pub struct RestrictedOperationMode; +impl Receive for RestrictedOperationMode {} +/// In Bus monitoring mode (for more details refer to ISO11898-1, 10.12 Bus monitoring), +/// the FDCAN is able to receive valid data frames and valid remote frames, but cannot start a +/// transmission. In this mode, it sends only recessive bits on the CAN bus. If the FDCAN is +/// required to send a dominant bit (ACK bit, overload flag, active error flag), the bit is +/// rerouted internally so that the FDCAN can monitor it, even if the CAN bus remains in recessive +/// state. In Bus monitoring mode the TXBRP register is held in reset state. The Bus monitoring +/// mode can be used to analyze the traffic on a CAN bus without affecting it by the transmission +/// of dominant bits. +pub struct BusMonitoringMode; +impl Receive for BusMonitoringMode {} +/// Test mode must be used for production tests or self test only. The software control for +/// FDCAN_TX pin interferes with all CAN protocol functions. It is not recommended to use test +/// modes for application. +pub struct TestMode; + +/// Interface to a FdCAN peripheral. +pub struct FdCan { + control: FdCanControl, +} + +impl FdCan +where + I: Instance, +{ + fn create_can(config: FdCanConfig, instance: I) -> FdCan { + FdCan { + control: FdCanControl { + config, + instance, + _mode: core::marker::PhantomData, + }, + } + } + + fn into_can_mode(self) -> FdCan { + FdCan { + control: FdCanControl { + config: self.control.config, + instance: self.control.instance, + _mode: core::marker::PhantomData, + }, + } + } + + /// Returns a reference to the peripheral instance. + /// + /// This allows accessing HAL-specific data stored in the instance type. + #[inline] + pub fn instance(&mut self) -> &mut I { + &mut self.control.instance + } + + #[inline] + fn registers(&self) -> &RegisterBlock { + unsafe { &*I::REGISTERS } + } + + #[inline] + fn msg_ram_mut(&mut self) -> &mut message_ram::RegisterBlock { + self.instance().msg_ram_mut() + } + + #[inline] + fn reset_msg_ram(&mut self) { + self.msg_ram_mut().reset(); + } + + #[inline] + fn enter_init_mode(&mut self) { + let can = self.registers(); + + can.cccr.modify(|_, w| w.init().set_bit()); + while can.cccr.read().init().bit_is_clear() {} + can.cccr.modify(|_, w| w.cce().set_bit()); + } + + /// Returns the current FDCAN config settings + #[inline] + pub fn get_config(&self) -> FdCanConfig { + self.control.config + } + + /// Enables or disables loopback mode: Internally connects the TX and RX + /// signals together. + #[inline] + fn set_loopback_mode(&mut self, mode: LoopbackMode) { + let (test, mon, lbck) = match mode { + LoopbackMode::None => (false, false, false), + LoopbackMode::Internal => (true, true, true), + LoopbackMode::External => (true, false, true), + }; + + self.set_test_mode(test); + self.set_bus_monitoring_mode(mon); + + let can = self.registers(); + can.test.modify(|_, w| w.lbck().bit(lbck)); + } + + /// Enables or disables silent mode: Disconnects the TX signal from the pin. + #[inline] + fn set_bus_monitoring_mode(&mut self, enabled: bool) { + let can = self.registers(); + can.cccr.modify(|_, w| w.mon().bit(enabled)); + } + + #[inline] + fn set_restricted_operations(&mut self, enabled: bool) { + let can = self.registers(); + can.cccr.modify(|_, w| w.asm().bit(enabled)); + } + + #[inline] + fn set_normal_operations(&mut self, _enabled: bool) { + self.set_loopback_mode(LoopbackMode::None); + } + + #[inline] + fn set_test_mode(&mut self, enabled: bool) { + let can = self.registers(); + can.cccr.modify(|_, w| w.test().bit(enabled)); + } + + #[inline] + fn set_power_down_mode(&mut self, enabled: bool) { + let can = self.registers(); + can.cccr.modify(|_, w| w.csr().bit(enabled)); + while can.cccr.read().csa().bit() != enabled {} + } + + /// Enable/Disable the specific Interrupt Line + #[inline] + pub fn enable_interrupt_line(&mut self, line: InterruptLine, enabled: bool) { + let can = self.registers(); + match line { + InterruptLine::_0 => can.ile.modify(|_, w| w.eint0().bit(enabled)), + InterruptLine::_1 => can.ile.modify(|_, w| w.eint1().bit(enabled)), + } + } + + /// Starts listening for a CAN interrupt. + #[inline] + pub fn enable_interrupt(&mut self, interrupt: Interrupt) { + self.enable_interrupts(Interrupts::from_bits_truncate(interrupt as u32)) + } + + /// Starts listening for a set of CAN interrupts. + #[inline] + pub fn enable_interrupts(&mut self, interrupts: Interrupts) { + self.registers() + .ie + .modify(|r, w| unsafe { w.bits(r.bits() | interrupts.bits()) }) + } + + /// Stops listening for a CAN interrupt. + pub fn disable_interrupt(&mut self, interrupt: Interrupt) { + self.disable_interrupts(Interrupts::from_bits_truncate(interrupt as u32)) + } + + /// Stops listening for a set of CAN interrupts. + #[inline] + pub fn disable_interrupts(&mut self, interrupts: Interrupts) { + self.registers() + .ie + .modify(|r, w| unsafe { w.bits(r.bits() & !interrupts.bits()) }) + } + + /// Retrieve the CAN error counters + #[inline] + pub fn error_counters(&self) -> ErrorCounters { + self.control.error_counters() + } + + /// Set an Standard Address CAN filter into slot 'id' + #[inline] + pub fn set_standard_filter(&mut self, slot: StandardFilterSlot, filter: StandardFilter) { + self.msg_ram_mut().filters.flssa[slot as usize].activate(filter); + } + + /// Set an array of Standard Address CAN filters and overwrite the current set + pub fn set_standard_filters( + &mut self, + filters: &[StandardFilter; STANDARD_FILTER_MAX as usize], + ) { + for (i, f) in filters.iter().enumerate() { + self.msg_ram_mut().filters.flssa[i].activate(*f); + } + } + + /// Set an Extended Address CAN filter into slot 'id' + #[inline] + pub fn set_extended_filter(&mut self, slot: ExtendedFilterSlot, filter: ExtendedFilter) { + self.msg_ram_mut().filters.flesa[slot as usize].activate(filter); + } + + /// Set an array of Extended Address CAN filters and overwrite the current set + pub fn set_extended_filters( + &mut self, + filters: &[ExtendedFilter; EXTENDED_FILTER_MAX as usize], + ) { + for (i, f) in filters.iter().enumerate() { + self.msg_ram_mut().filters.flesa[i].activate(*f); + } + } + + /// Retrieve the current protocol status + pub fn get_protocol_status(&self) -> ProtocolStatus { + let psr = self.registers().psr.read(); + ProtocolStatus { + activity: Activity::try_from(0 /*psr.act().bits()*/).unwrap(), //TODO: stm32g4 does not allow reading from this register + transmitter_delay_comp: psr.tdcv().bits(), + bus_off_status: psr.bo().bit_is_set(), + error_warning: psr.ew().bit_is_set(), + error_passive_state: psr.ep().bit_is_set(), + last_error: LastErrorCode::try_from(psr.lec().bits()).unwrap(), + } + } + + /// Clear specified interrupt + #[inline] + pub fn clear_interrupt(&mut self, interrupt: Interrupt) { + self.control.clear_interrupt(interrupt) + } + + /// Clear specified interrupts + #[inline] + pub fn clear_interrupts(&mut self, interrupts: Interrupts) { + self.control.clear_interrupts(interrupts) + } + + /// Splits this `FdCan` instance into transmitting and receiving halves, by reference. + #[inline] + #[allow(clippy::type_complexity)] + fn split_by_ref_generic( + &mut self, + ) -> ( + &mut FdCanControl, + &mut Tx, + &mut Rx, + &mut Rx, + ) { + // Safety: We take `&mut self` and the return value lifetimes are tied to `self`'s lifetime. + let tx = unsafe { Tx::conjure_by_ref() }; + let rx0 = unsafe { Rx::conjure_by_ref() }; + let rx1 = unsafe { Rx::conjure_by_ref() }; + (&mut self.control, tx, rx0, rx1) + } + + /// Consumes this `FdCan` instance and splits it into transmitting and receiving halves. + #[inline] + #[allow(clippy::type_complexity)] + fn split_generic( + self, + ) -> ( + FdCanControl, + Tx, + Rx, + Rx, + ) { + // Safety: We must be carefull to not let them use each others registers + unsafe { (self.control, Tx::conjure(), Rx::conjure(), Rx::conjure()) } + } + + /// Combines an FdCanControl, Tx and the two Rx instances back into an FdCan instance + #[inline] + #[allow(clippy::type_complexity)] + pub fn combine( + t: ( + FdCanControl, + Tx, + Rx, + Rx, + ), + ) -> Self { + Self::create_can(t.0.config, t.0.instance) + } +} + +/// Select an FDCAN Clock Source +pub enum FdCanClockSource { + /// Select HSE as the FDCAN clock source + HSE = 0b00, + /// Select PLL "Q" clock as the FDCAN clock source + PLLQ = 0b01, + /// Select "P" clock as the FDCAN clock source + PCLK = 0b10, + //Reserved = 0b10, +} + +impl FdCan +where + I: Instance, +{ + /// Creates a CAN interface. + /// + /// Sets the FDCAN clock to the P clock if no FDCAN clock has been configured + /// If one has been configured, it will leave it as is. + #[inline] + pub fn new(can_instance: I, _tx: TX, _rx: RX, rcc: &Rcc) -> Self + where + TX: sealed::Tx, + RX: sealed::Rx, + { + I::enable(&rcc.rb); + + if rcc.rb.ccipr.read().fdcansel() == 0 { + // Select P clock as FDCAN clock source + rcc.rb.ccipr.modify(|_, w| { + // This is sound, as `FdCanClockSource` only contains valid values for this field. + unsafe { + w.fdcansel().bits(FdCanClockSource::PCLK as u8); + } + + w + }); + } + //TODO: Set Speed to VeryHigh? + + let can = Self::create_can(FdCanConfig::default(), can_instance); + let reg = can.registers(); + assert!(reg.endn.read() == 0x87654321_u32); + can + } + + /// Creates a CAN interface. + /// + /// Sets the FDCAN clock to the selected clock source + /// Note that this is shared across all instances. + /// Do not call this if there is allready an active FDCAN instance. + #[inline] + pub fn new_with_clock_source( + can_instance: I, + _tx: TX, + _rx: RX, + rcc: &Rcc, + clock_source: FdCanClockSource, + ) -> Self + where + TX: sealed::Tx, + RX: sealed::Rx, + { + rcc.rb.ccipr.modify(|_, w| { + // This is sound, as `FdCanClockSource` only contains valid values for this field. + unsafe { + w.fdcansel().bits(clock_source as u8); + } + + w + }); + + Self::new(can_instance, _tx, _rx, rcc) + } + + /// Moves out of PoweredDownMode and into ConfigMode + #[inline] + pub fn into_config_mode(mut self) -> FdCan { + self.set_power_down_mode(false); + self.enter_init_mode(); + + self.reset_msg_ram(); + + let can = self.registers(); + + // Framework specific settings are set here. // + + // set TxBuffer to Queue Mode; + // TODO: don't require this. + // can.txbc.write(|w| w.tfqm().set_bit()); + //FIXME: stm32g4 has the wrong layout here! + //We should be able to use the above, + //But right now, we just set the 24th bit. + can.txbc.write(|w| unsafe { w.bits(1_u32 << 24) }); + + // set standard filters list size to 28 + // set extended filters list size to 8 + // REQUIRED: we use the memory map as if these settings are set + // instead of re-calculating them. + can.rxgfc.modify(|_, w| unsafe { + w.lse() + .bits(EXTENDED_FILTER_MAX) + .lss() + .bits(STANDARD_FILTER_MAX) + }); + for fid in 0..STANDARD_FILTER_MAX { + self.set_standard_filter((fid as u8).into(), StandardFilter::disable()); + } + for fid in 0..EXTENDED_FILTER_MAX { + self.set_extended_filter(fid.into(), ExtendedFilter::disable()); + } + + self.into_can_mode() + } + + /// Disables the CAN interface and returns back the raw peripheral it was created from. + #[inline] + pub fn free(mut self) -> I { + self.disable_interrupts(Interrupts::all()); + + //TODO check this! + self.enter_init_mode(); + self.set_power_down_mode(true); + self.control.instance + } +} + +impl FdCan +where + I: Instance, +{ + #[inline] + fn leave_init_mode(&mut self) { + self.apply_config(self.control.config); + + let can = self.registers(); + can.cccr.modify(|_, w| w.cce().clear_bit()); + can.cccr.modify(|_, w| w.init().clear_bit()); + while can.cccr.read().init().bit_is_set() {} + } + + /// Moves out of ConfigMode and into InternalLoopbackMode + #[inline] + pub fn into_internal_loopback(mut self) -> FdCan { + self.set_loopback_mode(LoopbackMode::Internal); + self.leave_init_mode(); + + self.into_can_mode() + } + + /// Moves out of ConfigMode and into ExternalLoopbackMode + #[inline] + pub fn into_external_loopback(mut self) -> FdCan { + self.set_loopback_mode(LoopbackMode::External); + self.leave_init_mode(); + + self.into_can_mode() + } + + /// Moves out of ConfigMode and into RestrictedOperationMode + #[inline] + pub fn into_restricted(mut self) -> FdCan { + self.set_restricted_operations(true); + self.leave_init_mode(); + + self.into_can_mode() + } + + /// Moves out of ConfigMode and into NormalOperationMode + #[inline] + pub fn into_normal(mut self) -> FdCan { + self.set_normal_operations(true); + self.leave_init_mode(); + + self.into_can_mode() + } + + /// Moves out of ConfigMode and into BusMonitoringMode + #[inline] + pub fn into_bus_monitoring(mut self) -> FdCan { + self.set_bus_monitoring_mode(true); + self.leave_init_mode(); + + self.into_can_mode() + } + + /// Moves out of ConfigMode and into Testmode + #[inline] + pub fn into_test_mode(mut self) -> FdCan { + self.set_test_mode(true); + self.leave_init_mode(); + + self.into_can_mode() + } + + /// Moves out of ConfigMode and into PoweredDownmode + #[inline] + pub fn into_powered_down(mut self) -> FdCan { + self.set_power_down_mode(true); + self.leave_init_mode(); + + self.into_can_mode() + } + + /// Applies the settings of a new FdCanConfig + /// See `[FdCanConfig]` for more information + #[inline] + pub fn apply_config(&mut self, config: FdCanConfig) { + self.set_data_bit_timing(config.dbtr); + self.set_nominal_bit_timing(config.nbtr); + self.set_automatic_retransmit(config.automatic_retransmit); + self.set_transmit_pause(config.transmit_pause); + self.set_frame_transmit(config.frame_transmit); + self.set_interrupt_line_config(config.interrupt_line_config); + self.set_non_iso_mode(config.non_iso_mode); + self.set_edge_filtering(config.edge_filtering); + self.set_protocol_exception_handling(config.protocol_exception_handling); + self.set_global_filter(config.global_filter); + } + + /// Configures the bit timings. + /// + /// You can use to calculate the `btr` parameter. Enter + /// parameters as follows: + /// + /// - *Clock Rate*: The input clock speed to the CAN peripheral (*not* the CPU clock speed). + /// This is the clock rate of the peripheral bus the CAN peripheral is attached to (eg. APB1). + /// - *Sample Point*: Should normally be left at the default value of 87.5%. + /// - *SJW*: Should normally be left at the default value of 1. + /// + /// Then copy the `CAN_BUS_TIME` register value from the table and pass it as the `btr` + /// parameter to this method. + #[inline] + pub fn set_nominal_bit_timing(&mut self, btr: NominalBitTiming) { + self.control.config.nbtr = btr; + + let can = self.registers(); + can.nbtp.write(|w| unsafe { + w.nbrp() + .bits(btr.nbrp() - 1) + .ntseg1() + .bits(btr.ntseg1() - 1) + .ntseg2() + .bits(btr.ntseg2() - 1) + .nsjw() + .bits(btr.nsjw() - 1) + }); + } + + /// Configures the data bit timings for the FdCan Variable Bitrates. + /// This is not used when frame_transmit is set to anything other than AllowFdCanAndBRS. + #[inline] + pub fn set_data_bit_timing(&mut self, btr: DataBitTiming) { + self.control.config.dbtr = btr; + + let can = self.registers(); + can.dbtp.write(|w| unsafe { + w.dbrp() + .bits(btr.dbrp() - 1) + .dtseg1() + .bits(btr.dtseg1() - 1) + .dtseg2() + .bits(btr.dtseg2() - 1) + .dsjw() + .bits(btr.dsjw() - 1) + }); + } + + /// Enables or disables automatic retransmission of messages + /// + /// If this is enabled, the CAN peripheral will automatically try to retransmit each frame + /// util it can be sent. Otherwise, it will try only once to send each frame. + /// + /// Automatic retransmission is enabled by default. + #[inline] + pub fn set_automatic_retransmit(&mut self, enabled: bool) { + let can = self.registers(); + can.cccr.modify(|_, w| w.dar().bit(!enabled)); + self.control.config.automatic_retransmit = enabled; + } + + /// Configures the transmit pause feature + /// See `[FdCanConfig]` for more information + #[inline] + pub fn set_transmit_pause(&mut self, enabled: bool) { + let can = self.registers(); + can.cccr.modify(|_, w| w.dar().bit(!enabled)); + self.control.config.transmit_pause = enabled; + } + + /// Configures non-iso mode + /// See `[FdCanConfig]` for more information + #[inline] + pub fn set_non_iso_mode(&mut self, enabled: bool) { + let can = self.registers(); + can.cccr.modify(|_, w| w.niso().bit(enabled)); + self.control.config.non_iso_mode = enabled; + } + + /// Configures edge filtering + /// See `[FdCanConfig]` for more information + #[inline] + pub fn set_edge_filtering(&mut self, enabled: bool) { + let can = self.registers(); + can.cccr.modify(|_, w| w.efbi().bit(enabled)); + self.control.config.edge_filtering = enabled; + } + + /// Configures frame transmission mode + /// See `[FdCanConfig]` for more information + #[inline] + pub fn set_frame_transmit(&mut self, fts: FrameTransmissionConfig) { + let (fdoe, brse) = match fts { + FrameTransmissionConfig::ClassicCanOnly => (false, false), + FrameTransmissionConfig::AllowFdCan => (true, false), + FrameTransmissionConfig::AllowFdCanAndBRS => (true, true), + }; + + let can = self.registers(); + can.cccr.modify(|_, w| w.fdoe().bit(fdoe).brse().bit(brse)); + + self.control.config.frame_transmit = fts; + } + + /// Configures the interrupt lines + /// See `[FdCanConfig]` for more information + #[inline] + pub fn set_interrupt_line_config(&mut self, l0int: Interrupts) { + let can = self.registers(); + + can.ils.modify(|_, w| unsafe { w.bits(l0int.bits()) }); + + self.control.config.interrupt_line_config = l0int; + } + + /// Sets the protocol exception handling on/off + #[inline] + pub fn set_protocol_exception_handling(&mut self, enabled: bool) { + let can = self.registers(); + + can.cccr.modify(|_, w| w.pxhd().bit(!enabled)); + + self.control.config.protocol_exception_handling = enabled; + } + + /// Sets the General FdCAN clock divider for this instance + //TODO: ?clock divider is a shared register? + #[inline] + pub fn set_clock_divider(&mut self, div: ClockDivider) { + let can = self.registers(); + + can.ckdiv.write(|w| unsafe { w.pdiv().bits(div as u8) }); + + self.control.config.clock_divider = div; + } + + /// Configures and resets the timestamp counter + #[inline] + pub fn set_timestamp_counter_source(&mut self, select: TimestampSource) { + let (tcp, tss) = match select { + TimestampSource::None => (0, 0b00), + TimestampSource::Prescaler(p) => (p as u8, 0b01), + TimestampSource::FromTIM3 => (0, 0b10), + }; + self.registers() + .tscc + .write(|w| unsafe { w.tcp().bits(tcp).tss().bits(tss) }); + + self.control.config.timestamp_source = select; + } + + /// Configures the global filter settings + #[inline] + pub fn set_global_filter(&mut self, filter: GlobalFilter) { + self.registers().rxgfc.modify(|_, w| { + unsafe { + w.anfs() + .bits(filter.handle_standard_frames as u8) + .anfe() + .bits(filter.handle_extended_frames as u8) + } + .rrfs() + .bit(filter.reject_remote_standard_frames) + .rrfe() + .bit(filter.reject_remote_extended_frames) + }); + } + + /// Returns the current FdCan timestamp counter + #[inline] + pub fn timestamp(&self) -> u16 { + self.control.timestamp() + } +} + +impl FdCan +where + I: Instance, +{ + /// Returns out of InternalLoopbackMode and back into ConfigMode + #[inline] + pub fn into_config_mode(mut self) -> FdCan { + self.set_loopback_mode(LoopbackMode::None); + self.enter_init_mode(); + + self.into_can_mode() + } +} + +impl FdCan +where + I: Instance, +{ + /// Returns out of ExternalLoopbackMode and back into ConfigMode + #[inline] + pub fn into_config_mode(mut self) -> FdCan { + self.set_loopback_mode(LoopbackMode::None); + self.enter_init_mode(); + + self.into_can_mode() + } +} + +impl FdCan +where + I: Instance, +{ + /// Returns out of NormalOperationMode and back into ConfigMode + #[inline] + pub fn into_config_mode(mut self) -> FdCan { + self.set_normal_operations(false); + self.enter_init_mode(); + + self.into_can_mode() + } +} + +impl FdCan +where + I: Instance, +{ + /// Returns out of RestrictedOperationMode and back into ConfigMode + #[inline] + pub fn into_config_mode(mut self) -> FdCan { + self.set_restricted_operations(false); + self.enter_init_mode(); + + self.into_can_mode() + } +} + +impl FdCan +where + I: Instance, +{ + /// Returns out of BusMonitoringMode and back into ConfigMode + #[inline] + pub fn into_config_mode(mut self) -> FdCan { + self.set_bus_monitoring_mode(false); + self.enter_init_mode(); + + self.into_can_mode() + } +} + +/// states of the test.tx register +pub enum TestTransmitPinState { + /// CAN core has control (default) + CoreHasControl = 0b00, + /// Sample point can be monitored + ShowSamplePoint = 0b01, + /// Set to Dominant (0) Level + SetDominant = 0b10, + /// Set to Recessive (1) Level + SetRecessive = 0b11, +} + +impl FdCan +where + I: Instance, +{ + /// Returns out of TestMode and back into ConfigMode + #[inline] + pub fn into_config_mode(mut self) -> FdCan { + self.set_test_mode(false); + self.enter_init_mode(); + + self.into_can_mode() + } + + /// Gets the state of the receive pin to either Dominant (false), or Recessive (true) + pub fn get_receive_pin(&mut self) -> bool { + let can = self.registers(); + + can.test.read().rx().bit_is_set() + } + + /// Sets the state of the transmit pin according to TestTransmitPinState + pub fn set_transmit_pin(&mut self, state: TestTransmitPinState) { + let can = self.registers(); + + //SAFE: state has all possible values, and this can only occur in TestMode + can.test.modify(|_, w| unsafe { w.tx().bits(state as u8) }); + } +} + +impl FdCan +where + I: Instance, + M: Transmit + Receive, +{ + /// Splits this `FdCan` instance into transmitting and receiving halves, by reference. + #[inline] + #[allow(clippy::type_complexity)] + pub fn split_by_ref( + &mut self, + ) -> ( + &mut FdCanControl, + &mut Tx, + &mut Rx, + &mut Rx, + ) { + self.split_by_ref_generic() + } + + /// Consumes this `FdCan` instance and splits it into transmitting and receiving halves. + #[allow(clippy::type_complexity)] + pub fn split( + self, + ) -> ( + FdCanControl, + Tx, + Rx, + Rx, + ) { + self.split_generic() + } +} + +impl FdCan +where + I: Instance, + M: Transmit, +{ + /// Puts a CAN frame in a free transmit mailbox for transmission on the bus. + /// + /// Frames are transmitted to the bus based on their priority (identifier). + /// Transmit order is preserved for frames with identical identifiers. + /// If all transmit mailboxes are full, this overwrites the mailbox with + /// the lowest priority. + #[inline] + pub fn transmit( + &mut self, + frame: TxFrameHeader, + write: &mut WTX, + ) -> nb::Result, Infallible> + where + WTX: FnMut(&mut [u32]), + { + // Safety: We have a `&mut self` and have unique access to the peripheral. + unsafe { Tx::::conjure().transmit(frame, write) } + } + + /// Puts a CAN frame in a free transmit mailbox for transmission on the bus. + /// + /// Frames are transmitted to the bus based on their priority (identifier). + /// Transmit order is preserved for frames with identical identifiers. + /// If all transmit mailboxes are full, `pending` is called with the mailbox, + /// header and data of the to-be-replaced frame. + pub fn transmit_preserve( + &mut self, + frame: TxFrameHeader, + write: &mut WTX, + pending: &mut PTX, + ) -> nb::Result, Infallible> + where + PTX: FnMut(Mailbox, TxFrameHeader, &[u32]) -> P, + WTX: FnMut(&mut [u32]), + { + // Safety: We have a `&mut self` and have unique access to the peripheral. + unsafe { Tx::::conjure().transmit_preserve(frame, write, pending) } + } + + /// Returns `true` if no frame is pending for transmission. + #[inline] + pub fn is_transmitter_idle(&self) -> bool { + // Safety: Read-only operation. + unsafe { Tx::::conjure().is_idle() } + } + + /// Attempts to abort the sending of a frame that is pending in a mailbox. + /// + /// If there is no frame in the provided mailbox, or its transmission succeeds before it can be + /// aborted, this function has no effect and returns `false`. + /// + /// If there is a frame in the provided mailbox, and it is canceled successfully, this function + /// returns `true`. + #[inline] + pub fn abort(&mut self, mailbox: Mailbox) -> bool { + // Safety: We have a `&mut self` and have unique access to the peripheral. + unsafe { Tx::::conjure().abort(mailbox) } + } +} + +impl FdCan +where + I: Instance, + M: Receive, +{ + /// Returns a received frame from FIFO_0 if available. + #[inline] + pub fn receive0( + &mut self, + receive: &mut RECV, + ) -> nb::Result, Infallible> + where + RECV: FnMut(RxFrameInfo, &[u32]) -> R, + { + // Safety: We have a `&mut self` and have unique access to the peripheral. + unsafe { Rx::::conjure().receive(receive) } + } + + /// Returns a received frame from FIFO_1 if available. + #[inline] + pub fn receive1( + &mut self, + receive: &mut RECV, + ) -> nb::Result, Infallible> + where + RECV: FnMut(RxFrameInfo, &[u32]) -> R, + { + // Safety: We have a `&mut self` and have unique access to the peripheral. + unsafe { Rx::::conjure().receive(receive) } + } +} + +/// FdCanControl Struct +/// Used to house some information during an FdCan split. +/// and can be used for some generic information retrieval during operation. +pub struct FdCanControl +where + I: Instance, +{ + config: FdCanConfig, + instance: I, + _mode: PhantomData, +} +impl FdCanControl +where + I: Instance, +{ + #[inline] + fn registers(&self) -> &RegisterBlock { + unsafe { &*I::REGISTERS } + } + + /// Returns the current error counters + #[inline] + pub fn error_counters(&self) -> ErrorCounters { + let can = self.registers(); + let cel: u8 = can.ecr.read().cel().bits(); + let rp: bool = can.ecr.read().rp().bits(); + let rec: u8 = can.ecr.read().rec().bits(); + let tec: u8 = can.ecr.read().tec().bits(); + + ErrorCounters { + can_errors: cel, + transmit_err: tec, + receive_err: match rp { + false => ReceiveErrorOverflow::Normal(rec), + true => ReceiveErrorOverflow::Overflow(rec), + }, + } + } + + /// Returns the current FdCan Timestamp counter + #[inline] + pub fn timestamp(&self) -> u16 { + self.registers().tscv.read().tsc().bits() + } + + /// Clear specified interrupt + #[inline] + pub fn clear_interrupt(&mut self, interrupt: Interrupt) { + let can = self.registers(); + can.ir.write(|w| unsafe { w.bits(interrupt as u32) }); + } + + /// Clear specified interrupts + #[inline] + pub fn clear_interrupts(&mut self, interrupts: Interrupts) { + let can = self.registers(); + can.ir.write(|w| unsafe { w.bits(interrupts.bits()) }); + } +} + +/// Interface to the CAN transmitter part. +pub struct Tx { + _can: PhantomData, + _mode: PhantomData, +} + +impl Tx +where + I: Instance, +{ + #[inline] + unsafe fn conjure() -> Self { + Self { + _can: PhantomData, + _mode: PhantomData, + } + } + + /// Creates a `&mut Self` out of thin air. + /// + /// This is only safe if it is the only way to access a `Tx`. + #[inline] + unsafe fn conjure_by_ref<'a>() -> &'a mut Self { + // Cause out of bounds access when `Self` is not zero-sized. + #[allow(clippy::unnecessary_operation)] + [()][core::mem::size_of::()]; + + // Any aligned pointer is valid for ZSTs. + &mut *NonNull::dangling().as_ptr() + } + + #[inline] + fn registers(&self) -> &RegisterBlock { + unsafe { &*I::REGISTERS } + } + + #[inline] + fn tx_msg_ram(&self) -> &message_ram::Transmit { + unsafe { &(*I::MSG_RAM).transmit } + } + + #[inline] + fn tx_msg_ram_mut(&mut self) -> &mut message_ram::Transmit { + unsafe { &mut (*I::MSG_RAM).transmit } + } + + /// Puts a CAN frame in a transmit mailbox for transmission on the bus. + /// + /// Frames are transmitted to the bus based on their priority (identifier). Transmit order is + /// preserved for frames with identical identifiers. + /// + /// If all transmit mailboxes are full, a higher priority frame can replace a lower-priority + /// frame, which is returned via the closure 'pending'. If 'pending' is called; it's return value + /// is returned via Option

, if it is not, None is returned. + /// If there are only higher priority frames in the queue, this returns Err::WouldBlock + pub fn transmit( + &mut self, + frame: TxFrameHeader, + write: &mut WTX, + ) -> nb::Result, Infallible> + where + WTX: FnMut(&mut [u32]), + { + self.transmit_preserve(frame, write, &mut |_, _, _| ()) + } + + /// As Transmit, but if there is a pending frame, `pending` will be called so that the frame can + /// be preserved. + pub fn transmit_preserve( + &mut self, + frame: TxFrameHeader, + write: &mut WTX, + pending: &mut PTX, + ) -> nb::Result, Infallible> + where + PTX: FnMut(Mailbox, TxFrameHeader, &[u32]) -> P, + WTX: FnMut(&mut [u32]), + { + let can = self.registers(); + let queue_is_full = self.tx_queue_is_full(); + + let id = frame.into(); + + // If the queue is full, + // Discard the first slot with a lower priority message + let (idx, pending_frame) = if queue_is_full { + if self.is_available(Mailbox::_0, id) { + ( + Mailbox::_0, + self.abort_pending_mailbox(Mailbox::_0, pending), + ) + } else if self.is_available(Mailbox::_1, id) { + ( + Mailbox::_1, + self.abort_pending_mailbox(Mailbox::_1, pending), + ) + } else if self.is_available(Mailbox::_2, id) { + ( + Mailbox::_2, + self.abort_pending_mailbox(Mailbox::_2, pending), + ) + } else { + // For now we bail when there is no lower priority slot available + // Can this lead to priority inversion? + return Err(nb::Error::WouldBlock); + } + } else { + // Read the Write Pointer + let idx = can.txfqs.read().tfqpi().bits(); + + (Mailbox::new(idx), None) + }; + + self.write_mailbox(idx, frame, write); + + Ok(pending_frame) + } + + /// 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 { + self.registers().txfqs.read().tfqf().bit() + } + + /// Returns `Ok` when the mailbox is free or if it contains pending frame with a + /// lower priority (higher ID) than the identifier `id`. + #[inline] + fn is_available(&self, idx: Mailbox, id: IdReg) -> bool { + if self.has_pending_frame(idx) { + //read back header section + let header: TxFrameHeader = (&self.tx_msg_ram().tbsa[idx as usize].header).into(); + let old_id: IdReg = header.into(); + + id > old_id + } else { + true + } + } + + #[inline] + fn write_mailbox(&mut self, idx: Mailbox, tx_header: TxFrameHeader, transmit: TX) -> R + where + TX: FnOnce(&mut [u32]) -> R, + { + let tx_ram = self.tx_msg_ram_mut(); + + // Clear mail slot; mainly for debugging purposes. + tx_ram.tbsa[idx as usize].reset(); + + // Calculate length of data in words + let data_len = ((tx_header.len as usize) + 3) / 4; + + //set header section + tx_ram.tbsa[idx as usize].header.merge(tx_header); + + //set data + let result = transmit(&mut tx_ram.tbsa[idx as usize].data[0..data_len]); + + // Set as ready to transmit + self.registers() + .txbar + .modify(|r, w| unsafe { w.ar().bits(r.ar().bits() | 1 << (idx as u32)) }); + + result + } + + #[inline] + fn abort_pending_mailbox(&mut self, idx: Mailbox, pending: PTX) -> Option + where + PTX: FnOnce(Mailbox, TxFrameHeader, &[u32]) -> R, + { + if self.abort(idx) { + let tx_ram = self.tx_msg_ram(); + + //read back header section + let header = (&tx_ram.tbsa[idx as usize].header).into(); + Some(pending(idx, header, &tx_ram.tbsa[idx as usize].data)) + } else { + // Abort request failed because the frame was already sent (or being sent) on + // the bus. All mailboxes are now free. This can happen for small prescaler + // values (e.g. 1MBit/s bit timing with a source clock of 8MHz) or when an ISR + // has preempted the execution. + None + } + } + + /// Attempts to abort the sending of a frame that is pending in a mailbox. + /// + /// If there is no frame in the provided mailbox, or its transmission succeeds before it can be + /// aborted, this function has no effect and returns `false`. + /// + /// If there is a frame in the provided mailbox, and it is canceled successfully, this function + /// returns `true`. + #[inline] + fn abort(&mut self, idx: Mailbox) -> bool { + let can = self.registers(); + + // Check if there is a request pending to abort + if self.has_pending_frame(idx) { + let idx: u8 = idx.into(); + + // Abort Request + can.txbcr.write(|w| unsafe { w.cr().bits(idx) }); + + // Wait for the abort request to be finished. + loop { + if can.txbcf.read().cf().bits() & idx != 0 { + // Return false when a transmission has occured + break can.txbto.read().to().bits() & idx == 0; + } + } + } else { + false + } + } + + #[inline] + fn has_pending_frame(&self, idx: Mailbox) -> bool { + let can = self.registers(); + let idx: u8 = idx.into(); + + can.txbrp.read().trp().bits() & idx != 0 + } + + /// Returns `true` if no frame is pending for transmission. + #[inline] + pub fn is_idle(&self) -> bool { + let can = self.registers(); + can.txbrp.read().trp().bits() == 0x0 + } + + /// Clears the transmission complete flag. + #[inline] + pub fn clear_transmission_completed_flag(&mut self) { + let can = self.registers(); + can.ir.write(|w| w.tc().set_bit()); + } + + /// Clears the transmission cancelled flag. + #[inline] + pub fn clear_transmission_cancelled_flag(&mut self) { + let can = self.registers(); + can.ir.write(|w| w.tcf().set_bit()); + } +} + +#[doc(hidden)] +pub trait FifoNr: crate::sealed::Sealed { + const NR: usize; +} +#[doc(hidden)] +pub struct Fifo0; +impl crate::sealed::Sealed for Fifo0 {} +impl FifoNr for Fifo0 { + const NR: usize = 0; +} +#[doc(hidden)] +pub struct Fifo1; +impl crate::sealed::Sealed for Fifo1 {} +impl FifoNr for Fifo1 { + const NR: usize = 1; +} + +/// Notes whether an overrun has occurred. +/// Since both arms contain T, this can be 'unwrap'ed without causing a panic. +#[derive(Clone, Copy, Debug)] +#[cfg_attr(feature = "unstable-defmt", derive(defmt::Format))] +pub enum ReceiveOverrun { + /// No overrun has occured + NoOverrun(T), + /// an overrun has occurered + Overrun(T), +} +impl ReceiveOverrun { + /// unwraps itself and returns T + /// in contradiction to Option::unwrap, this does not panic + /// since both elements contain T. + #[inline] + pub fn unwrap(self) -> T { + match self { + ReceiveOverrun::NoOverrun(t) | ReceiveOverrun::Overrun(t) => t, + } + } +} + +/// Interface to the CAN receiver part. +pub struct Rx +where + FIFONR: FifoNr, +{ + _can: PhantomData, + _mode: PhantomData, + _nr: PhantomData, +} + +impl Rx +where + FIFONR: FifoNr, + I: Instance, +{ + #[inline] + unsafe fn conjure() -> Self { + Self { + _can: PhantomData, + _mode: PhantomData, + _nr: PhantomData, + } + } + + /// Creates a `&mut Self` out of thin air. + /// + /// This is only safe if it is the only way to access an `Rx`. + #[inline] + unsafe fn conjure_by_ref<'a>() -> &'a mut Self { + // Cause out of bounds access when `Self` is not zero-sized. + #[allow(clippy::unnecessary_operation)] + [()][core::mem::size_of::()]; + + // Any aligned pointer is valid for ZSTs. + &mut *NonNull::dangling().as_ptr() + } + + /// Returns a received frame if available. + /// + /// Returns `Err` when a frame was lost due to buffer overrun. + pub fn receive( + &mut self, + receive: &mut RECV, + ) -> nb::Result, Infallible> + where + RECV: FnMut(RxFrameInfo, &[u32]) -> R, + { + if !self.rx_fifo_is_empty() { + let mbox = self.get_rx_mailbox(); + let idx: usize = mbox.into(); + let mailbox: &RxFifoElement = &self.rx_msg_ram().fxsa[idx]; + + let header: RxFrameInfo = (&mailbox.header).into(); + let word_len = (header.len + 3) / 4; + let result = Ok(receive(header, &mailbox.data[0..word_len as usize])); + self.release_mailbox(mbox); + + if self.has_overrun() { + result.map(ReceiveOverrun::Overrun) + } else { + result.map(ReceiveOverrun::NoOverrun) + } + } else { + Err(nb::Error::WouldBlock) + } + } + + #[inline] + fn registers(&self) -> &RegisterBlock { + unsafe { &*I::REGISTERS } + } + + #[inline] + fn rx_msg_ram(&self) -> &message_ram::Receive { + unsafe { &(&(*I::MSG_RAM).receive)[FIFONR::NR] } + } + + #[inline] + fn has_overrun(&self) -> bool { + let can = self.registers(); + match FIFONR::NR { + 0 => can.rxf0s.read().rf0l().bit(), + 1 => can.rxf1s.read().rf1l().bit(), + _ => unreachable!(), + } + } + + /// Returns if the fifo contains any new messages. + #[inline] + pub fn rx_fifo_is_empty(&self) -> bool { + let can = self.registers(); + match FIFONR::NR { + 0 => can.rxf0s.read().f0fl().bits() == 0, + 1 => can.rxf1s.read().f1fl().bits() == 0, + _ => unreachable!(), + } + } + + #[inline] + fn release_mailbox(&mut self, idx: Mailbox) { + unsafe { + (*I::MSG_RAM).receive[FIFONR::NR].fxsa[idx as u8 as usize].reset(); + } + + let can = self.registers(); + match FIFONR::NR { + 0 => can.rxf0a.write(|w| unsafe { w.f0ai().bits(idx.into()) }), + 1 => can.rxf1a.write(|w| unsafe { w.f1ai().bits(idx.into()) }), + _ => unreachable!(), + } + } + + #[inline] + fn get_rx_mailbox(&self) -> Mailbox { + let can = self.registers(); + let idx = match FIFONR::NR { + 0 => can.rxf0s.read().f0gi().bits(), + 1 => can.rxf1s.read().f1gi().bits(), + _ => unreachable!(), + }; + Mailbox::new(idx) + } +} + +/// The three mailboxes. +/// These are used for the transmit queue +/// and the two Receive FIFOs +#[derive(Debug, Copy, Clone, Ord, PartialOrd, Eq, PartialEq)] +#[cfg_attr(feature = "unstable-defmt", derive(defmt::Format))] +pub enum Mailbox { + /// Transmit mailbox 0 + _0 = 0, + /// Transmit mailbox 1 + _1 = 1, + /// Transmit mailbox 2 + _2 = 2, +} +impl Mailbox { + #[inline] + fn new(idx: u8) -> Self { + match idx & 0b11 { + 0 => Mailbox::_0, + 1 => Mailbox::_1, + 2 => Mailbox::_2, + _ => unreachable!(), + } + } +} +impl From for u8 { + #[inline] + fn from(m: Mailbox) -> Self { + m as u8 + } +} +impl From for usize { + #[inline] + fn from(m: Mailbox) -> Self { + m as u8 as usize + } +} + +mod impls { + use super::sealed; + + /// Implements sealed::{Tx,Rx} for pins associated with a CAN peripheral + macro_rules! pins { + ($PER:ident => + (tx: [ $($( #[ $pmetatx:meta ] )* $tx:ident<$txaf:ident>),+ $(,)? ], + rx: [ $($( #[ $pmetarx:meta ] )* $rx:ident<$rxaf:ident>),+ $(,)? ])) => { + $( + $( #[ $pmetatx ] )* + impl super::sealed::Tx<$PER> for $tx> {} + )+ + $( + $( #[ $pmetarx ] )* + impl super::sealed::Rx<$PER> for $rx> {} + )+ + }; + } + + mod fdcan1 { + use crate::fdcan; + use crate::fdcan::message_ram; + use crate::gpio::{ + gpioa::{PA11, PA12}, + gpiob::{PB8, PB9}, + gpiod::{PD0, PD1}, + AF9, + }; + use crate::stm32; + use crate::stm32::FDCAN1; + + // All STM32G4 models with CAN support these pins + pins! { + FDCAN1 => ( + tx: [ + PA12, + PB9, + PD1, + ], + rx: [ + PA11, + PB8, + PD0, + ] + ) + } + + unsafe impl message_ram::MsgRamExt for FDCAN1 { + const MSG_RAM: *mut message_ram::RegisterBlock = (0x4000_a400 as *mut _); + } + unsafe impl fdcan::Instance for FDCAN1 { + const REGISTERS: *mut stm32::fdcan::RegisterBlock = FDCAN1::ptr() as *mut _; + } + } + + #[cfg(any( + feature = "stm32g471", + feature = "stm32g473", + feature = "stm32g474", + feature = "stm32g483", + feature = "stm32g484", + feature = "stm32g491", + feature = "stm32g4A1", + ))] + mod fdcan2 { + use crate::fdcan; + use crate::fdcan::message_ram; + use crate::gpio::{ + gpiob::{PB12, PB13, PB5, PB6}, + AF9, + }; + use crate::stm32::{self, FDCAN2}; + + pins! { + FDCAN2 => ( + tx: [ + PB6, + PB13, + ], + rx: [ + PB5, + PB12, + ]) + } + + unsafe impl fdcan::Instance for FDCAN2 { + const REGISTERS: *mut stm32::fdcan::RegisterBlock = FDCAN2::ptr() as *mut _; + } + + unsafe impl message_ram::MsgRamExt for FDCAN2 { + const MSG_RAM: *mut message_ram::RegisterBlock = (0x4000_a750 as *mut _); + } + } + + #[cfg(any( + feature = "stm32g473", + feature = "stm32g474", + feature = "stm32g483", + feature = "stm32g484", + ))] + mod fdcan3 { + use crate::fdcan; + use crate::fdcan::message_ram; + use crate::gpio::{ + gpioa::{PA15, PA8}, + gpiob::{PB3, PB4}, + AF11, + }; + use crate::stm32::{self, FDCAN3}; + + pins! { + FDCAN3 => ( + tx: [ + PA15, + PB4, + ], + rx: [ + PA8, + PB3, + ]) + } + + unsafe impl fdcan::Instance for FDCAN3 { + const REGISTERS: *mut stm32::fdcan::RegisterBlock = FDCAN3::ptr() as *mut _; + } + + unsafe impl message_ram::MsgRamExt for FDCAN3 { + const MSG_RAM: *mut message_ram::RegisterBlock = (0x4000_aaa0 as *mut _); + } + } +} diff --git a/src/fdcan/config.rs b/src/fdcan/config.rs new file mode 100644 index 00000000..6831e401 --- /dev/null +++ b/src/fdcan/config.rs @@ -0,0 +1,454 @@ +pub use super::interrupt::{Interrupt, InterruptLine, Interrupts}; +use core::num::{NonZeroU16, NonZeroU8}; + +/// Configures the bit timings. +/// +/// You can use to calculate the `btr` parameter. Enter +/// parameters as follows: +/// +/// - *Clock Rate*: The input clock speed to the CAN peripheral (*not* the CPU clock speed). +/// This is the clock rate of the peripheral bus the CAN peripheral is attached to (eg. APB1). +/// - *Sample Point*: Should normally be left at the default value of 87.5%. +/// - *SJW*: Should normally be left at the default value of 1. +/// +/// Then copy the `CAN_BUS_TIME` register value from the table and pass it as the `btr` +/// parameter to this method. +#[derive(Clone, Copy, Debug)] +#[cfg_attr(feature = "unstable-defmt", derive(defmt::Format))] +pub struct NominalBitTiming { + /// Value by which the oscillator frequency is divided for generating the bit time quanta. The bit + /// time is built up from a multiple of this quanta. Valid values are 1 to 512. + pub prescaler: NonZeroU16, + /// Valid values are 1 to 128. + pub seg1: NonZeroU8, + /// Valid values are 1 to 255. + pub seg2: NonZeroU8, + /// Valid values are 1 to 128. + pub sync_jump_width: NonZeroU8, +} +impl NominalBitTiming { + #[inline] + pub(crate) fn nbrp(&self) -> u16 { + u16::from(self.prescaler) & 0x1FF + } + #[inline] + pub(crate) fn ntseg1(&self) -> u8 { + u8::from(self.seg1) + } + #[inline] + pub(crate) fn ntseg2(&self) -> u8 { + u8::from(self.seg2) & 0x7F + } + #[inline] + pub(crate) fn nsjw(&self) -> u8 { + u8::from(self.sync_jump_width) & 0x7F + } +} + +impl Default for NominalBitTiming { + #[inline] + fn default() -> Self { + Self { + prescaler: NonZeroU16::new(1).unwrap(), + seg1: NonZeroU8::new(0xA).unwrap(), + seg2: NonZeroU8::new(0x3).unwrap(), + sync_jump_width: NonZeroU8::new(0x3).unwrap(), + } + } +} + +/// Configures the data bit timings for the FdCan Variable Bitrates. +/// This is not used when frame_transmit is set to anything other than AllowFdCanAndBRS. +#[derive(Clone, Copy, Debug)] +#[cfg_attr(feature = "unstable-defmt", derive(defmt::Format))] +pub struct DataBitTiming { + /// Tranceiver Delay Compensation + pub transceiver_delay_compensation: bool, + /// The value by which the oscillator frequency is divided to generate the bit time quanta. The bit + /// time is built up from a multiple of this quanta. Valid values for the Baud Rate Prescaler are 1 + /// to 31. + pub prescaler: NonZeroU8, + /// Valid values are 1 to 31. + pub seg1: NonZeroU8, + /// Valid values are 1 to 15. + pub seg2: NonZeroU8, + /// Must always be smaller than DTSEG2, valid values are 1 to 15. + pub sync_jump_width: NonZeroU8, +} +impl DataBitTiming { + // #[inline] + // fn tdc(&self) -> u8 { + // let tsd = self.transceiver_delay_compensation as u8; + // //TODO: stm32g4 does not export the TDC field + // todo!() + // } + #[inline] + pub(crate) fn dbrp(&self) -> u8 { + u8::from(self.prescaler) & 0x1F + } + #[inline] + pub(crate) fn dtseg1(&self) -> u8 { + u8::from(self.seg1) & 0x1F + } + #[inline] + pub(crate) fn dtseg2(&self) -> u8 { + u8::from(self.seg2) & 0x0F + } + #[inline] + pub(crate) fn dsjw(&self) -> u8 { + u8::from(self.sync_jump_width) & 0x0F + } +} + +impl Default for DataBitTiming { + #[inline] + fn default() -> Self { + Self { + transceiver_delay_compensation: false, + prescaler: NonZeroU8::new(1).unwrap(), + seg1: NonZeroU8::new(0xA).unwrap(), + seg2: NonZeroU8::new(0x3).unwrap(), + sync_jump_width: NonZeroU8::new(0x3).unwrap(), + } + } +} + +/// Configures which modes to use +/// Individual headers can contain a desire to be send via FdCan +/// or use Bit rate switching. But if this general setting does not allow +/// that, only classic CAN is used instead. +#[derive(Clone, Copy, Debug)] +#[cfg_attr(feature = "unstable-defmt", derive(defmt::Format))] +pub enum FrameTransmissionConfig { + /// Only allow Classic CAN message Frames + ClassicCanOnly, + /// Allow (non-brs) FdCAN Message Frames + AllowFdCan, + /// Allow FdCAN Message Frames and allow Bit Rate Switching + AllowFdCanAndBRS, +} + +/// +#[derive(Clone, Copy, Debug)] +#[cfg_attr(feature = "unstable-defmt", derive(defmt::Format))] +pub enum ClockDivider { + /// Divide by 1 + _1 = 0b0000, + /// Divide by 2 + _2 = 0b0001, + /// Divide by 4 + _4 = 0b0010, + /// Divide by 6 + _6 = 0b0011, + /// Divide by 8 + _8 = 0b0100, + /// Divide by 10 + _10 = 0b0101, + /// Divide by 12 + _12 = 0b0110, + /// Divide by 14 + _14 = 0b0111, + /// Divide by 16 + _16 = 0b1000, + /// Divide by 18 + _18 = 0b1001, + /// Divide by 20 + _20 = 0b1010, + /// Divide by 22 + _22 = 0b1011, + /// Divide by 24 + _24 = 0b1100, + /// Divide by 26 + _26 = 0b1101, + /// Divide by 28 + _28 = 0b1110, + /// Divide by 30 + _30 = 0b1111, +} + +/// Prescaler of the Timestamp counter +#[derive(Clone, Copy, Debug)] +#[cfg_attr(feature = "unstable-defmt", derive(defmt::Format))] +pub enum TimestampPrescaler { + /// 1 + _1 = 1, + /// 2 + _2 = 2, + /// 3 + _3 = 3, + /// 4 + _4 = 4, + /// 5 + _5 = 5, + /// 6 + _6 = 6, + /// 7 + _7 = 7, + /// 8 + _8 = 8, + /// 9 + _9 = 9, + /// 10 + _10 = 10, + /// 11 + _11 = 11, + /// 12 + _12 = 12, + /// 13 + _13 = 13, + /// 14 + _14 = 14, + /// 15 + _15 = 15, + /// 16 + _16 = 16, +} + +/// Selects the source of the Timestamp counter +#[derive(Clone, Copy, Debug)] +#[cfg_attr(feature = "unstable-defmt", derive(defmt::Format))] +pub enum TimestampSource { + /// The Timestamp counter is disabled + None, + /// Using the FdCan input clock as the Timstamp counter's source, + /// and using a specific prescaler + Prescaler(TimestampPrescaler), + /// Using TIM3 as a source + FromTIM3, +} + +/// How to handle frames in the global filter +#[derive(Clone, Copy, Debug)] +#[cfg_attr(feature = "unstable-defmt", derive(defmt::Format))] +pub enum NonMatchingFilter { + /// Frames will go to Fifo0 when they do no match any specific filter + IntoRxFifo0 = 0b00, + /// Frames will go to Fifo1 when they do no match any specific filter + IntoRxFifo1 = 0b01, + /// Frames will be rejected when they do not match any specific filter + Reject = 0b11, +} + +/// How to handle frames which do not match a specific filter +#[derive(Clone, Copy, Debug)] +#[cfg_attr(feature = "unstable-defmt", derive(defmt::Format))] +pub struct GlobalFilter { + /// How to handle non-matching standard frames + pub handle_standard_frames: NonMatchingFilter, + + /// How to handle non-matching extended frames + pub handle_extended_frames: NonMatchingFilter, + + /// How to handle remote standard frames + pub reject_remote_standard_frames: bool, + + /// How to handle remote extended frames + pub reject_remote_extended_frames: bool, +} +impl GlobalFilter { + /// Reject all non-matching and remote frames + pub const fn reject_all() -> Self { + Self { + handle_standard_frames: NonMatchingFilter::Reject, + handle_extended_frames: NonMatchingFilter::Reject, + reject_remote_standard_frames: true, + reject_remote_extended_frames: true, + } + } + + /// How to handle non-matching standard frames + pub const fn set_handle_standard_frames(mut self, filter: NonMatchingFilter) -> Self { + self.handle_standard_frames = filter; + self + } + /// How to handle non-matching exteded frames + pub const fn set_handle_extended_frames(mut self, filter: NonMatchingFilter) -> Self { + self.handle_extended_frames = filter; + self + } + /// How to handle remote standard frames + pub const fn set_reject_remote_standard_frames(mut self, filter: bool) -> Self { + self.reject_remote_standard_frames = filter; + self + } + /// How to handle remote extended frames + pub const fn set_reject_remote_extended_frames(mut self, filter: bool) -> Self { + self.reject_remote_extended_frames = filter; + self + } +} +impl Default for GlobalFilter { + #[inline] + fn default() -> Self { + Self { + handle_standard_frames: NonMatchingFilter::IntoRxFifo0, + handle_extended_frames: NonMatchingFilter::IntoRxFifo0, + reject_remote_standard_frames: false, + reject_remote_extended_frames: false, + } + } +} + +/// FdCan Config Struct +#[derive(Clone, Copy, Debug)] +#[cfg_attr(feature = "unstable-defmt", derive(defmt::Format))] +pub struct FdCanConfig { + /// Nominal Bit Timings + pub nbtr: NominalBitTiming, + /// (Variable) Data Bit Timings + pub dbtr: DataBitTiming, + /// Enables or disables automatic retransmission of messages + /// + /// If this is enabled, the CAN peripheral will automatically try to retransmit each frame + /// util it can be sent. Otherwise, it will try only once to send each frame. + /// + /// Automatic retransmission is enabled by default. + pub automatic_retransmit: bool, + /// Enabled or disables the pausing between transmissions + /// + /// This feature looses up burst transmissions coming from a single node and it protects against + /// "babbling idiot" scenarios where the application program erroneously requests too many + /// transmissions. + pub transmit_pause: bool, + /// Enabled or disables the pausing between transmissions + /// + /// This feature looses up burst transmissions coming from a single node and it protects against + /// "babbling idiot" scenarios where the application program erroneously requests too many + /// transmissions. + pub frame_transmit: FrameTransmissionConfig, + /// Non Isoe Mode + /// If this is set, the FDCAN uses the CAN FD frame format as specified by the Bosch CAN + /// FD Specification V1.0. + pub non_iso_mode: bool, + /// Edge Filtering: Two consecutive dominant tq required to detect an edge for hard synchronization + pub edge_filtering: bool, + /// Enables protocol exception handling + pub protocol_exception_handling: bool, + /// Sets the general clock divider for this FdCAN instance + pub clock_divider: ClockDivider, + /// This sets the interrupts for each interrupt line of the FdCan (FDCAN_INT0/1) + /// Each interrupt set to 0 is set to line_0, each set to 1 is set to line_1. + /// NOTE: This does not enable or disable the interrupt, but merely configure + /// them to which interrupt the WOULD trigger if they are enabled. + pub interrupt_line_config: Interrupts, + /// Sets the timestamp source + pub timestamp_source: TimestampSource, + /// Configures the Global Filter + pub global_filter: GlobalFilter, +} + +impl FdCanConfig { + /// Configures the bit timings. + #[inline] + pub const fn set_nominal_bit_timing(mut self, btr: NominalBitTiming) -> Self { + self.nbtr = btr; + self + } + + /// Configures the bit timings. + #[inline] + pub const fn set_data_bit_timing(mut self, btr: DataBitTiming) -> Self { + self.dbtr = btr; + self + } + + /// Enables or disables automatic retransmission of messages + /// + /// If this is enabled, the CAN peripheral will automatically try to retransmit each frame + /// util it can be sent. Otherwise, it will try only once to send each frame. + /// + /// Automatic retransmission is enabled by default. + #[inline] + pub const fn set_automatic_retransmit(mut self, enabled: bool) -> Self { + self.automatic_retransmit = enabled; + self + } + + /// Enabled or disables the pausing between transmissions + /// + /// This feature looses up burst transmissions coming from a single node and it protects against + /// "babbling idiot" scenarios where the application program erroneously requests too many + /// transmissions. + #[inline] + pub const fn set_transmit_pause(mut self, enabled: bool) -> Self { + self.transmit_pause = enabled; + self + } + + /// If this is set, the FDCAN uses the CAN FD frame format as specified by the Bosch CAN + /// FD Specification V1.0. + #[inline] + pub const fn set_non_iso_mode(mut self, enabled: bool) -> Self { + self.non_iso_mode = enabled; + self + } + + /// Two consecutive dominant tq required to detect an edge for hard synchronization + #[inline] + pub const fn set_edge_filtering(mut self, enabled: bool) -> Self { + self.edge_filtering = enabled; + self + } + + /// Sets the allowed transmission types for messages. + #[inline] + pub const fn set_frame_transmit(mut self, fts: FrameTransmissionConfig) -> Self { + self.frame_transmit = fts; + self + } + + /// Enables protocol exception handling + #[inline] + pub const fn set_protocol_exception_handling(mut self, peh: bool) -> Self { + self.protocol_exception_handling = peh; + self + } + + /// Configures which interrupt go to which interrupt lines + #[inline] + pub const fn set_interrupt_line_config(mut self, l0int: Interrupts) -> Self { + self.interrupt_line_config = l0int; + self + } + + /// Sets the general clock divider for this FdCAN instance + #[inline] + pub const fn set_clock_divider(mut self, div: ClockDivider) -> Self { + self.clock_divider = div; + self + } + + /// Sets the timestamp source + #[inline] + pub const fn set_timestamp_source(mut self, tss: TimestampSource) -> Self { + self.timestamp_source = tss; + self + } + + /// Sets the global filter settings + #[inline] + pub const fn set_global_filter(mut self, filter: GlobalFilter) -> Self { + self.global_filter = filter; + self + } +} + +impl Default for FdCanConfig { + #[inline] + fn default() -> Self { + Self { + nbtr: NominalBitTiming::default(), + dbtr: DataBitTiming::default(), + automatic_retransmit: true, + transmit_pause: false, + frame_transmit: FrameTransmissionConfig::ClassicCanOnly, + non_iso_mode: false, + edge_filtering: false, + interrupt_line_config: Interrupts::none(), + protocol_exception_handling: true, + clock_divider: ClockDivider::_1, + timestamp_source: TimestampSource::None, + global_filter: GlobalFilter::default(), + } + } +} diff --git a/src/fdcan/embedded_can.rs b/src/fdcan/embedded_can.rs new file mode 100644 index 00000000..667b85e1 --- /dev/null +++ b/src/fdcan/embedded_can.rs @@ -0,0 +1,106 @@ +//! `embedded_can` trait impls. + +use crate::{Can, Data, ExtendedId, Frame, Id, Instance, StandardId}; +use embedded_can_03 as embedded_can; + +impl embedded_can::Can for Can +where + I: Instance, +{ + type Frame = Frame; + + type Error = (); + + fn try_transmit( + &mut self, + frame: &Self::Frame, + ) -> nb::Result, Self::Error> { + match self.transmit(frame) { + Ok(status) => Ok(status.dequeued_frame().cloned()), + Err(nb::Error::WouldBlock) => Err(nb::Error::WouldBlock), + Err(nb::Error::Other(e)) => match e {}, + } + } + + fn try_receive(&mut self) -> nb::Result { + self.receive() + } +} + +impl embedded_can::Frame for Frame { + fn new(id: impl Into, data: &[u8]) -> Result { + let id = match id.into() { + embedded_can::Id::Standard(id) => unsafe { + Id::Standard(StandardId::new_unchecked(id.as_raw())) + }, + embedded_can::Id::Extended(id) => unsafe { + Id::Extended(ExtendedId::new_unchecked(id.as_raw())) + }, + }; + + let data = Data::new(data).ok_or(())?; + Ok(Frame::new_data(id, data)) + } + + fn new_remote(id: impl Into, dlc: usize) -> Result { + let id = match id.into() { + embedded_can::Id::Standard(id) => unsafe { + Id::Standard(StandardId::new_unchecked(id.as_raw())) + }, + embedded_can::Id::Extended(id) => unsafe { + Id::Extended(ExtendedId::new_unchecked(id.as_raw())) + }, + }; + + if dlc <= 8 { + Ok(Frame::new_remote(id, dlc as u8)) + } else { + Err(()) + } + } + + #[inline] + fn is_extended(&self) -> bool { + self.is_extended() + } + + #[inline] + fn is_standard(&self) -> bool { + self.is_standard() + } + + #[inline] + fn is_remote_frame(&self) -> bool { + self.is_remote_frame() + } + + #[inline] + fn is_data_frame(&self) -> bool { + self.is_data_frame() + } + + #[inline] + fn id(&self) -> embedded_can::Id { + match self.id() { + Id::Standard(id) => unsafe { + embedded_can::Id::Standard(embedded_can::StandardId::new_unchecked(id.as_raw())) + }, + Id::Extended(id) => unsafe { + embedded_can::Id::Extended(embedded_can::ExtendedId::new_unchecked(id.as_raw())) + }, + } + } + + #[inline] + fn dlc(&self) -> usize { + self.dlc() as usize + } + + fn data(&self) -> &[u8] { + if let Some(data) = self.data() { + data + } else { + &[] + } + } +} diff --git a/src/fdcan/filter.rs b/src/fdcan/filter.rs new file mode 100644 index 00000000..ed63ac3b --- /dev/null +++ b/src/fdcan/filter.rs @@ -0,0 +1,401 @@ +use super::id::{ExtendedId, StandardId}; + +pub use message_ram::{EXTENDED_FILTER_MAX, STANDARD_FILTER_MAX}; + +/// A Standard Filter +pub type StandardFilter = Filter; +/// An Extended Filter +pub type ExtendedFilter = Filter; + +impl Default for StandardFilter { + fn default() -> Self { + StandardFilter::disable() + } +} +impl Default for ExtendedFilter { + fn default() -> Self { + ExtendedFilter::disable() + } +} + +impl StandardFilter { + /// Accept all messages in FIFO 0 + pub fn accept_all_into_fifo0() -> StandardFilter { + StandardFilter { + filter: FilterType::BitMask { + filter: 0x0, + mask: 0x0, + }, + action: Action::StoreInFifo0, + } + } + + /// Accept all messages in FIFO 1 + pub fn accept_all_into_fifo1() -> StandardFilter { + StandardFilter { + filter: FilterType::BitMask { + filter: 0x0, + mask: 0x0, + }, + action: Action::StoreInFifo0, + } + } + + /// Reject all messages + pub fn reject_all() -> StandardFilter { + StandardFilter { + filter: FilterType::BitMask { + filter: 0x0, + mask: 0x0, + }, + action: Action::Reject, + } + } + + /// Disable the filter + pub fn disable() -> StandardFilter { + StandardFilter { + filter: FilterType::Disabled, + action: Action::Disable, + } + } +} + +impl ExtendedFilter { + /// Accept all messages in FIFO 0 + pub fn accept_all_into_fifo0() -> ExtendedFilter { + ExtendedFilter { + filter: FilterType::BitMask { + filter: 0x0, + mask: 0x0, + }, + action: Action::StoreInFifo0, + } + } + + /// Accept all messages in FIFO 1 + pub fn accept_all_into_fifo1() -> ExtendedFilter { + ExtendedFilter { + filter: FilterType::BitMask { + filter: 0x0, + mask: 0x0, + }, + action: Action::StoreInFifo0, + } + } + + /// Reject all messages + pub fn reject_all() -> ExtendedFilter { + ExtendedFilter { + filter: FilterType::BitMask { + filter: 0x0, + mask: 0x0, + }, + action: Action::Reject, + } + } + + /// Disable the filter + pub fn disable() -> ExtendedFilter { + ExtendedFilter { + filter: FilterType::Disabled, + action: Action::Disable, + } + } +} + +/// Filter Type +//#[cfg_attr(feature = "unstable-defmt", derive(defmt::Format))] +#[derive(Clone, Copy, Debug)] +pub enum FilterType +where + ID: Copy + Clone + core::fmt::Debug, + UNIT: Copy + Clone + core::fmt::Debug, +{ + /// Match with a range between two messages + Range { + /// First Id of the range + from: ID, + /// Last Id of the range + to: ID, + }, + /// Match with a bitmask + BitMask { + /// Filter of the bitmask + filter: UNIT, + /// Mask of the bitmask + mask: UNIT, + }, + /// Match with a single ID + DedicatedSingle(ID), + /// Match with one of two ID's + DedicatedDual(ID, ID), + /// Filter is disabled + Disabled, +} +impl From> for super::message_ram::enums::FilterType +where + ID: Copy + Clone + core::fmt::Debug, + UNIT: Copy + Clone + core::fmt::Debug, +{ + fn from(f: FilterType) -> Self { + match f { + FilterType::Range { to: _, from: _ } => Self::RangeFilter, + FilterType::BitMask { filter: _, mask: _ } => Self::ClassicFilter, + FilterType::DedicatedSingle(_) => Self::DualIdFilter, + FilterType::DedicatedDual(_, _) => Self::DualIdFilter, + FilterType::Disabled => Self::FilterDisabled, + } + } +} + +/// Filter Action +#[derive(Clone, Copy, Debug)] +#[cfg_attr(feature = "unstable-defmt", derive(defmt::Format))] +pub enum Action { + /// No Action + Disable = 0b000, + /// Store an matching message in FIFO 0 + StoreInFifo0 = 0b001, + /// Store an matching message in FIFO 1 + StoreInFifo1 = 0b010, + /// Reject an matching message + Reject = 0b011, + /// Flag a matching message (But not store?!?) + FlagHighPrio = 0b100, + /// Flag a matching message as a High Priority message and store it in FIFO 0 + FlagHighPrioAndStoreInFifo0 = 0b101, + /// Flag a matching message as a High Priority message and store it in FIFO 1 + FlagHighPrioAndStoreInFifo1 = 0b110, +} +impl From for super::message_ram::enums::FilterElementConfig { + fn from(a: Action) -> Self { + match a { + Action::Disable => Self::DisableFilterElement, + Action::StoreInFifo0 => Self::StoreInFifo0, + Action::StoreInFifo1 => Self::StoreInFifo1, + Action::Reject => Self::Reject, + Action::FlagHighPrio => Self::SetPriority, + Action::FlagHighPrioAndStoreInFifo0 => Self::SetPriorityAndStoreInFifo0, + Action::FlagHighPrioAndStoreInFifo1 => Self::SetPriorityAndStoreInFifo1, + } + } +} + +/// Filter +#[derive(Clone, Copy, Debug)] +//#[cfg_attr(feature = "unstable-defmt", derive(defmt::Format))] +pub struct Filter +where + ID: Copy + Clone + core::fmt::Debug, + UNIT: Copy + Clone + core::fmt::Debug, +{ + /// How to match an incoming message + pub filter: FilterType, + /// What to do with a matching message + pub action: Action, +} + +/// Standard Filter Slot +#[derive(Debug, Copy, Clone, Eq, PartialEq)] +#[cfg_attr(feature = "unstable-defmt", derive(defmt::Format))] +pub enum StandardFilterSlot { + /// 0 + _0 = 0, + /// 1 + _1 = 1, + /// 2 + _2 = 2, + /// 3 + _3 = 3, + /// 4 + _4 = 4, + /// 5 + _5 = 5, + /// 6 + _6 = 6, + /// 7 + _7 = 7, + /// 8 + _8 = 8, + /// 9 + _9 = 9, + /// 10 + _10 = 10, + /// 11 + _11 = 11, + /// 12 + _12 = 12, + /// 13 + _13 = 13, + /// 14 + _14 = 14, + /// 15 + _15 = 15, + /// 16 + _16 = 16, + /// 17 + _17 = 17, + /// 18 + _18 = 18, + /// 19 + _19 = 19, + /// 20 + _20 = 20, + /// 21 + _21 = 21, + /// 22 + _22 = 22, + /// 23 + _23 = 23, + /// 24 + _24 = 24, + /// 25 + _25 = 25, + /// 26 + _26 = 26, + /// 27 + _27 = 27, +} +impl From for StandardFilterSlot { + fn from(u: u8) -> Self { + match u { + 0 => StandardFilterSlot::_0, + 1 => StandardFilterSlot::_1, + 2 => StandardFilterSlot::_2, + 3 => StandardFilterSlot::_3, + 4 => StandardFilterSlot::_4, + 5 => StandardFilterSlot::_5, + 6 => StandardFilterSlot::_6, + 7 => StandardFilterSlot::_7, + 8 => StandardFilterSlot::_8, + 9 => StandardFilterSlot::_9, + 10 => StandardFilterSlot::_10, + 11 => StandardFilterSlot::_11, + 12 => StandardFilterSlot::_12, + 13 => StandardFilterSlot::_13, + 14 => StandardFilterSlot::_14, + 15 => StandardFilterSlot::_15, + 16 => StandardFilterSlot::_16, + 17 => StandardFilterSlot::_17, + 18 => StandardFilterSlot::_18, + 19 => StandardFilterSlot::_19, + 20 => StandardFilterSlot::_20, + 21 => StandardFilterSlot::_21, + 22 => StandardFilterSlot::_22, + 23 => StandardFilterSlot::_23, + 24 => StandardFilterSlot::_24, + 25 => StandardFilterSlot::_25, + 26 => StandardFilterSlot::_26, + 27 => StandardFilterSlot::_27, + _ => panic!("Standard Filter Slot Too High!"), + } + } +} + +/// Extended Filter Slot +#[derive(Debug, Copy, Clone, Eq, PartialEq)] +#[cfg_attr(feature = "unstable-defmt", derive(defmt::Format))] +pub enum ExtendedFilterSlot { + /// 0 + _0 = 0, + /// 1 + _1 = 1, + /// 2 + _2 = 2, + /// 3 + _3 = 3, + /// 4 + _4 = 4, + /// 5 + _5 = 5, + /// 6 + _6 = 6, + /// 7 + _7 = 7, +} +impl From for ExtendedFilterSlot { + fn from(u: u8) -> Self { + match u { + 0 => ExtendedFilterSlot::_0, + 1 => ExtendedFilterSlot::_1, + 2 => ExtendedFilterSlot::_2, + 3 => ExtendedFilterSlot::_3, + 4 => ExtendedFilterSlot::_4, + 5 => ExtendedFilterSlot::_5, + 6 => ExtendedFilterSlot::_6, + 7 => ExtendedFilterSlot::_7, + _ => panic!("Extended Filter Slot Too High!"), // Should be unreachable + } + } +} + +/// Enum over both Standard and Extended Filter ID's +#[derive(Debug, Copy, Clone, Eq, PartialEq)] +#[cfg_attr(feature = "unstable-defmt", derive(defmt::Format))] +pub enum FilterId { + /// Standard Filter Slots + Standard(StandardFilterSlot), + /// Extended Filter Slots + Extended(ExtendedFilterSlot), +} + +pub(crate) trait ActivateFilter +where + ID: Copy + Clone + core::fmt::Debug, + UNIT: Copy + Clone + core::fmt::Debug, +{ + fn activate(&mut self, f: Filter); + // fn read(&self) -> Filter; +} + +use super::message_ram; + +impl ActivateFilter for message_ram::StandardFilter { + fn activate(&mut self, f: Filter) { + let sft = f.filter.into(); + + let (sfid1, sfid2) = match f.filter { + FilterType::Range { to, from } => (to.as_raw(), from.as_raw()), + FilterType::DedicatedSingle(id) => (id.as_raw(), id.as_raw()), + FilterType::DedicatedDual(id1, id2) => (id1.as_raw(), id2.as_raw()), + FilterType::BitMask { filter, mask } => (filter, mask), + FilterType::Disabled => (0x0, 0x0), + }; + let sfec = f.action.into(); + self.write(|w| { + unsafe { w.sfid1().bits(sfid1).sfid2().bits(sfid2) } + .sft() + .set_filter_type(sft) + .sfec() + .set_filter_element_config(sfec) + }); + } + // fn read(&self) -> Filter { + // todo!() + // } +} +impl ActivateFilter for message_ram::ExtendedFilter { + fn activate(&mut self, f: Filter) { + let eft = f.filter.into(); + + let (efid1, efid2) = match f.filter { + FilterType::Range { to, from } => (to.as_raw(), from.as_raw()), + FilterType::DedicatedSingle(id) => (id.as_raw(), id.as_raw()), + FilterType::DedicatedDual(id1, id2) => (id1.as_raw(), id2.as_raw()), + FilterType::BitMask { filter, mask } => (filter, mask), + FilterType::Disabled => (0x0, 0x0), + }; + let efec = f.action.into(); + self.write(|w| { + unsafe { w.efid1().bits(efid1).efid2().bits(efid2) } + .eft() + .set_filter_type(eft) + .efec() + .set_filter_element_config(efec) + }); + } + // fn read(&self) -> Filter { + // todo!() + // } +} diff --git a/src/fdcan/frame.rs b/src/fdcan/frame.rs new file mode 100644 index 00000000..01fe13e7 --- /dev/null +++ b/src/fdcan/frame.rs @@ -0,0 +1,203 @@ +use core::cmp::Ordering; + +use super::Id; +use super::IdReg; + +use super::filter::FilterId; + +use super::message_ram::enums::FrameFormat as PacFrameFormat; +use super::message_ram::{RxFifoElementHeader, TxBufferElementHeader}; + +use super::message_ram::enums::RemoteTransmissionRequest; +use super::message_ram::enums::{DataLength, FilterFrameMatch}; + +/// Type of Frame +#[derive(Clone, Copy, Debug, PartialEq)] +#[cfg_attr(feature = "unstable-defmt", derive(defmt::Format))] +pub enum FrameFormat { + /// Frame used by Classic CAN + Standard = 0, + /// New frame format used by FdCan + Fdcan = 1, +} +impl From for PacFrameFormat { + fn from(ff: FrameFormat) -> Self { + match ff { + FrameFormat::Standard => PacFrameFormat::Standard, + FrameFormat::Fdcan => PacFrameFormat::Fdcan, + } + } +} +impl From for FrameFormat { + fn from(ff: PacFrameFormat) -> Self { + match ff { + PacFrameFormat::Standard => FrameFormat::Standard, + PacFrameFormat::Fdcan => FrameFormat::Fdcan, + } + } +} + +/// Priority of a CAN frame. +/// +/// The priority of a frame is determined by the bits that are part of the *arbitration field*. +/// These consist of the frame identifier bits (including the *IDE* bit, which is 0 for extended +/// frames and 1 for standard frames), as well as the *RTR* bit, which determines whether a frame +/// is a data or remote frame. Lower values of the *arbitration field* have higher priority. +/// +/// This struct wraps the *arbitration field* and implements `PartialOrd` and `Ord` accordingly, +/// ordering higher priorities greater than lower ones. +#[derive(Debug, Copy, Clone)] +#[cfg_attr(feature = "unstable-defmt", derive(defmt::Format))] +pub struct FramePriority(pub(crate) IdReg); + +/// Ordering is based on the Identifier and frame type (data vs. remote) and can be used to sort +/// frames by priority. +impl Ord for FramePriority { + fn cmp(&self, other: &Self) -> Ordering { + self.0.cmp(&other.0) + } +} + +impl PartialOrd for FramePriority { + fn partial_cmp(&self, other: &Self) -> Option { + Some(self.cmp(other)) + } +} + +impl PartialEq for FramePriority { + fn eq(&self, other: &Self) -> bool { + self.cmp(other) == Ordering::Equal + } +} + +impl Eq for FramePriority {} + +/// Header of a transmit request +#[derive(Debug, Copy, Clone)] +#[cfg_attr(feature = "unstable-defmt", derive(defmt::Format))] +pub struct TxFrameHeader { + /// Length of the data in bytes + pub len: u8, + /// Type of message + pub frame_format: FrameFormat, + /// Id + pub id: Id, + /// Should we use bit rate switching + /// + /// Not that this is a request and if the global frame_transmit is set to ClassicCanOnly + /// this is ignored. + pub bit_rate_switching: bool, + //pub error_state: Option<()>, //TODO + /// + pub marker: Option, +} +impl From for IdReg { + fn from(header: TxFrameHeader) -> IdReg { + let id: IdReg = header.id.into(); + id.with_rtr(header.len == 0) + } +} + +pub(crate) trait MergeTxFrameHeader { + fn merge(&self, header: TxFrameHeader); +} +impl MergeTxFrameHeader for TxBufferElementHeader { + fn merge(&self, header: TxFrameHeader) { + let id: IdReg = header.id.into(); + self.write(|w| { + unsafe { w.id().bits(id.as_raw_id()) } + .rtr() + .bit(header.len == 0) + .xtd() + .set_id_type(header.id.into()) + .set_len(DataLength::new(header.len, header.frame_format.into())) + .set_event(header.marker.into()) + .fdf() + .set_format(header.frame_format.into()) + .brs() + .bit(header.bit_rate_switching) + //esi.set_error_indicator(//TODO//) + }); + } +} + +impl From<&TxBufferElementHeader> for TxFrameHeader { + fn from(reg: &TxBufferElementHeader) -> Self { + let reader = reg.read(); + let id = reader.id().bits(); + let rtr = reader.rtr().rtr(); + let xtd = reader.xtd().id_type(); + let len = reader.to_data_length(); + let ff: PacFrameFormat = len.into(); + TxFrameHeader { + len: len.len(), + frame_format: ff.into(), + id: IdReg::from_register(id, rtr, xtd).into(), + bit_rate_switching: reader.brs().is_with_brs(), + //error_state: Option<()>, //TODO + marker: reader.to_event().into(), + } + } +} + +/// Header of a Received Frame +#[derive(Debug, Copy, Clone)] +#[cfg_attr(feature = "unstable-defmt", derive(defmt::Format))] +pub struct RxFrameInfo { + /// Length in bytes + pub len: u8, + /// Frame Format + pub frame_format: FrameFormat, + /// Id + pub id: Id, + /// Is this an Remote Transmit Request + pub rtr: bool, + /// Did this message match any filters + pub filter_match: Option, + /// was this received with bit rate switching + pub bit_rate_switching: bool, + //pub error_state: (), //TODO + /// Time stamp counter + pub time_stamp: u16, +} +impl RxFrameInfo { + /// Transforms an RxFrameInfo into an TxFrameHeader + pub fn to_tx_header(self, marker: Option) -> TxFrameHeader { + TxFrameHeader { + len: self.len, + frame_format: self.frame_format, + id: self.id, + bit_rate_switching: self.bit_rate_switching, + marker, + } + } +} +impl From<&RxFifoElementHeader> for RxFrameInfo { + fn from(reg: &RxFifoElementHeader) -> Self { + let reader = reg.read(); + let len = reader.to_data_length(); + let ff: PacFrameFormat = len.into(); + let id = reader.id().bits(); + let rtr = reader.rtr().rtr(); + let xtd = reader.xtd().id_type(); + let id = IdReg::from_register(id, rtr, xtd).to_id(); + let filter = reader.to_filter_match(); + let filter = match filter { + FilterFrameMatch::DidNotMatch => None, + FilterFrameMatch::DidMatch(filter) => Some(match id { + Id::Standard(_) => FilterId::Standard(filter.into()), + Id::Extended(_) => FilterId::Extended(filter.into()), + }), + }; + RxFrameInfo { + len: len.len(), + frame_format: ff.into(), + id, + rtr: rtr == RemoteTransmissionRequest::TransmitRemoteFrame, + filter_match: filter, + bit_rate_switching: reader.brs().is_with_brs(), + time_stamp: reader.txts().bits(), + //error_state //TODO + } + } +} diff --git a/src/fdcan/frame/tests.rs b/src/fdcan/frame/tests.rs new file mode 100644 index 00000000..ebcdd961 --- /dev/null +++ b/src/fdcan/frame/tests.rs @@ -0,0 +1,84 @@ +use crate::{ExtendedId, Frame, StandardId}; + +#[test] +fn data_greater_remote() { + let id = StandardId::new(0).unwrap(); + + let data_frame = Frame::new_data(id, []); + let remote_frame = Frame::new_remote(id, 0); + assert!(data_frame.is_data_frame()); + assert!(remote_frame.is_remote_frame()); + + assert!(data_frame.priority() > remote_frame.priority()); +} + +#[test] +fn lower_ids_win_arbitration() { + let zero = Frame::new_data(StandardId::new(0).unwrap(), []); + let one = Frame::new_data(StandardId::new(1).unwrap(), []); + assert!(zero.is_standard()); + assert!(!zero.is_extended()); + assert!(one.is_standard()); + assert!(!one.is_extended()); + assert!(zero.priority() > one.priority()); + + // Standard IDs have priority over Extended IDs if the Base ID matches. + let ext_one = Frame::new_data( + ExtendedId::new(0b00000000001_000000000000000000).unwrap(), + [], + ); + assert!(!ext_one.is_standard()); + assert!(ext_one.is_extended()); + assert!(one.priority() > ext_one.priority()); + assert!(zero.priority() > ext_one.priority()); + + // Ext. ID with Base ID 0 has priority over Standard ID 1. + let ext_zero = Frame::new_data( + ExtendedId::new(0b00000000000_100000000000000000).unwrap(), + [], + ); + assert!(!ext_zero.is_standard()); + assert!(ext_zero.is_extended()); + assert!(one.priority() < ext_zero.priority()); + // ...but not over Standard ID 0. + assert!(zero.priority() > ext_zero.priority()); +} + +#[test] +fn highest_standard_higher_prio_than_highest_ext() { + let std = Frame::new_data(StandardId::MAX, []); + let ext = Frame::new_data(ExtendedId::MAX, []); + + assert!(std.is_standard()); + assert!(!std.is_extended()); + assert!(!ext.is_standard()); + assert!(ext.is_extended()); + assert!(std.priority() > ext.priority()); +} + +#[test] +fn data_neq_remote() { + let id = StandardId::new(0).unwrap(); + + let data_frame = Frame::new_data(id, []); + let remote_frame = Frame::new_remote(id, 0); + + assert_ne!(data_frame, remote_frame); +} + +#[test] +fn remote_eq_remote_ignores_data() { + let mut remote1 = Frame::new_remote(StandardId::MAX, 7); + let mut remote2 = Frame::new_remote(StandardId::MAX, 7); + + remote1.data.bytes = [0xAA; 8]; + remote2.data.bytes = [0x55; 8]; + + assert_eq!(remote1, remote2); +} + +#[test] +fn max_len() { + Frame::new_data(StandardId::MAX, [0; 8]); + Frame::new_remote(StandardId::MAX, 8); +} diff --git a/src/fdcan/id.rs b/src/fdcan/id.rs new file mode 100644 index 00000000..26cbb354 --- /dev/null +++ b/src/fdcan/id.rs @@ -0,0 +1,307 @@ +//! CAN Identifiers. + +use core::cmp::{Ord, Ordering}; + +use super::message_ram::enums::{IdType, RemoteTransmissionRequest}; + +/// Standard 11-bit CAN Identifier (`0..=0x7FF`). +#[derive(Debug, Copy, Clone, Eq, PartialEq)] +#[cfg_attr(feature = "unstable-defmt", derive(defmt::Format))] +pub struct StandardId(u16); + +impl StandardId { + /// CAN ID `0`, the highest priority. + pub const ZERO: Self = Self(0); + + /// CAN ID `0x7FF`, the lowest priority. + pub const MAX: Self = Self(0x7FF); + + /// Tries to create a `StandardId` from a raw 16-bit integer. + /// + /// This will return `None` if `raw` is out of range of an 11-bit integer (`> 0x7FF`). + #[inline] + pub const fn new(raw: u16) -> Option { + if raw <= 0x7FF { + Some(Self(raw)) + } else { + None + } + } + + /// Creates a new `StandardId` without checking if it is inside the valid range. + /// + /// # Safety + /// + /// The caller must ensure that `raw` is in the valid range, otherwise the behavior is + /// undefined. + #[inline] + pub const unsafe fn new_unchecked(raw: u16) -> Self { + Self(raw) + } + + /// Returns this CAN Identifier as a raw 16-bit integer. + #[inline] + pub(crate) fn as_raw(&self) -> u16 { + self.0 + } +} +impl From for IdType { + fn from(_id: StandardId) -> Self { + IdType::StandardId + } +} + +/// Extended 29-bit CAN Identifier (`0..=1FFF_FFFF`). +#[derive(Debug, Copy, Clone, Eq, PartialEq)] +#[cfg_attr(feature = "unstable-defmt", derive(defmt::Format))] +pub struct ExtendedId(u32); + +impl ExtendedId { + /// CAN ID `0`, the highest priority. + pub const ZERO: Self = Self(0); + + /// CAN ID `0x1FFFFFFF`, the lowest priority. + pub const MAX: Self = Self(0x1FFF_FFFF); + + /// Tries to create a `ExtendedId` from a raw 32-bit integer. + /// + /// This will return `None` if `raw` is out of range of an 29-bit integer (`> 0x1FFF_FFFF`). + #[inline] + pub const fn new(raw: u32) -> Option { + if raw <= 0x1FFF_FFFF { + Some(Self(raw)) + } else { + None + } + } + + /// Creates a new `ExtendedId` without checking if it is inside the valid range. + /// + /// # Safety + /// + /// The caller must ensure that `raw` is in the valid range, otherwise the behavior is + /// undefined. + #[inline] + pub const unsafe fn new_unchecked(raw: u32) -> Self { + Self(raw) + } + + /// Returns this CAN Identifier as a raw 32-bit integer. + #[inline] + pub(crate) fn as_raw(&self) -> u32 { + self.0 + } + + /// Returns the Base ID part of this extended identifier. + pub fn standard_id(&self) -> StandardId { + // ID-28 to ID-18 + StandardId((self.0 >> 18) as u16) + } +} + +impl From for IdType { + fn from(_id: ExtendedId) -> Self { + IdType::ExtendedId + } +} + +/// A CAN Identifier (standard or extended). +#[derive(Debug, Copy, Clone, Eq, PartialEq)] +#[cfg_attr(feature = "unstable-defmt", derive(defmt::Format))] +pub enum Id { + /// Standard 11-bit Identifier (`0..=0x7FF`). + Standard(StandardId), + + /// Extended 29-bit Identifier (`0..=0x1FFF_FFFF`). + Extended(ExtendedId), +} + +impl From for Id { + #[inline] + fn from(id: StandardId) -> Self { + Id::Standard(id) + } +} + +impl From for Id { + #[inline] + fn from(id: ExtendedId) -> Self { + Id::Extended(id) + } +} + +impl From for IdType { + #[inline] + fn from(id: Id) -> Self { + match id { + Id::Standard(id) => id.into(), + Id::Extended(id) => id.into(), + } + } +} + +/// Identifier of a CAN message. +/// +/// FdCan be either a standard identifier (11bit, Range: 0..0x3FF) or a +/// extendended identifier (29bit , Range: 0..0x1FFFFFFF). +/// +/// The `Ord` trait can be used to determine the frame’s priority this ID +/// belongs to. +/// Lower identifier values have a higher priority. Additionally standard frames +/// have a higher priority than extended frames and data frames have a higher +/// priority than remote frames. +#[derive(Clone, Copy, Debug, PartialEq, Eq)] +#[cfg_attr(feature = "unstable-defmt", derive(defmt::Format))] +pub(crate) struct IdReg(u32); + +impl IdReg { + const STANDARD_SHIFT: u32 = 18; + #[allow(dead_code)] + const STANDARD_MASK: u32 = 0x1FFC0000; + + const EXTENDED_SHIFT: u32 = 0; + const EXTENDED_MASK: u32 = 0x1FFFFFFF; + + const XTD_SHIFT: u32 = 30; + const XTD_MASK: u32 = 1 << Self::XTD_SHIFT; + + const RTR_SHIFT: u32 = 29; + const RTR_MASK: u32 = 1 << Self::RTR_SHIFT; + + /// Creates a new standard identifier (11bit, Range: 0..0x7FF) + /// + /// Panics for IDs outside the allowed range. + fn new_standard(id: StandardId) -> Self { + Self(u32::from(id.as_raw()) << Self::STANDARD_SHIFT) + } + + /// Creates a new extendended identifier (29bit , Range: 0..0x1FFFFFFF). + /// + /// Panics for IDs outside the allowed range. + fn new_extended(id: ExtendedId) -> Self { + Self(id.as_raw() << Self::EXTENDED_SHIFT | (1 << Self::XTD_SHIFT)) + } + + pub(crate) fn as_raw_id(&self) -> u32 { + self.0 & Self::EXTENDED_MASK + } + + pub(crate) fn from_register(id: u32, rtr: RemoteTransmissionRequest, xtd: IdType) -> Self { + let rtr: u32 = match rtr { + RemoteTransmissionRequest::TransmitDataFrame => 0, + RemoteTransmissionRequest::TransmitRemoteFrame => 1 << Self::RTR_SHIFT, + }; + let xtd: u32 = match xtd { + IdType::StandardId => 0, + IdType::ExtendedId => 1 << Self::XTD_SHIFT, + }; + Self(id | rtr | xtd) + } + + /// Sets the remote transmission (RTR) flag. This marks the identifier as + /// being part of a remote frame. + #[must_use = "returns a new IdReg without modifying `self`"] + pub(crate) fn with_rtr(self, rtr: bool) -> Self { + if rtr { + Self(self.0 | (1 << Self::RTR_SHIFT)) + } else { + Self(self.0 & !Self::RTR_MASK) + } + } + + /// Returns the identifier. + pub fn to_id(self) -> Id { + if self.is_extended() { + Id::Extended(unsafe { ExtendedId::new_unchecked(self.0 >> Self::EXTENDED_SHIFT) }) + } else { + Id::Standard(unsafe { + StandardId::new_unchecked((self.0 >> Self::STANDARD_SHIFT) as u16) + }) + } + } + + /// Returns `true` if the identifier is an extended identifier. + pub fn is_extended(self) -> bool { + (self.0 & Self::XTD_MASK) != 0 + } + + /// Returns `true` if the identifier is a standard identifier. + pub fn is_standard(self) -> bool { + !self.is_extended() + } + + /// Returns `true` if the identifer is part of a remote frame (RTR bit set). + pub(crate) fn rtr(self) -> bool { + self.0 & Self::RTR_MASK != 0 + } +} +impl From for IdReg { + fn from(id: Id) -> Self { + match id { + Id::Standard(s) => IdReg::new_standard(s), + Id::Extended(e) => IdReg::new_extended(e), + } + } +} +impl From for Id { + fn from(idr: IdReg) -> Self { + idr.to_id() + } +} +impl From for IdType { + #[inline] + fn from(id: IdReg) -> Self { + if id.is_standard() { + IdType::StandardId + } else { + IdType::ExtendedId + } + } +} +impl From for RemoteTransmissionRequest { + #[inline] + fn from(id: IdReg) -> Self { + if id.rtr() { + RemoteTransmissionRequest::TransmitRemoteFrame + } else { + RemoteTransmissionRequest::TransmitDataFrame + } + } +} + +/// `IdReg` is ordered by priority. +impl Ord for IdReg { + fn cmp(&self, other: &Self) -> Ordering { + // When the IDs match, data frames have priority over remote frames. + let rtr = self.rtr().cmp(&other.rtr()).reverse(); + + let id_a = self.to_id(); + let id_b = other.to_id(); + match (id_a, id_b) { + (Id::Standard(a), Id::Standard(b)) => { + // Lower IDs have priority over higher IDs. + a.as_raw().cmp(&b.as_raw()).reverse().then(rtr) + } + (Id::Extended(a), Id::Extended(b)) => a.as_raw().cmp(&b.as_raw()).reverse().then(rtr), + (Id::Standard(a), Id::Extended(b)) => { + // Standard frames have priority over extended frames if their Base IDs match. + a.as_raw() + .cmp(&b.standard_id().as_raw()) + .reverse() + .then(Ordering::Greater) + } + (Id::Extended(a), Id::Standard(b)) => a + .standard_id() + .as_raw() + .cmp(&b.as_raw()) + .reverse() + .then(Ordering::Less), + } + } +} + +impl PartialOrd for IdReg { + fn partial_cmp(&self, other: &Self) -> Option { + Some(self.cmp(other)) + } +} diff --git a/src/fdcan/interrupt.rs b/src/fdcan/interrupt.rs new file mode 100644 index 00000000..1cd1e237 --- /dev/null +++ b/src/fdcan/interrupt.rs @@ -0,0 +1,202 @@ +//! Interrupt types. + +use core::ops; + +#[allow(unused_imports)] // for intra-doc links only +use crate::fdcan::{FdCan, Rx}; + +/// FdCAN interrupt sources. +/// +/// These can be individually enabled and disabled in the FdCAN peripheral. Note that each FdCAN +/// peripheral only exposes 2 interrupts to the microcontroller: +/// +/// FDCANx_INTR0, +/// FDCANx_INTR1, +/// +/// The interrupts available on each line can be configured using the [`config::FdCanConfig`] struct. +#[derive(Debug, Copy, Clone, Eq, PartialEq)] +#[cfg_attr(feature = "unstable-defmt", derive(defmt::Format))] +#[non_exhaustive] +pub enum Interrupt { + /// Rx FIFO 0 has a new message + RxFifo0NewMsg = 1 << 0, + /// Rx FIFO 0 is full + RxFifo0Full = 1 << 1, + /// Rx FIFO 0 has lost a message + RxFifo0MsgLost = 1 << 2, + /// Rx FIFO 1 has a new message + RxFifo1NewMsg = 1 << 3, + /// Rx FIFO 1 is full + RxFifo1Full = 1 << 4, + /// Rx FIFO 1 has lost a message + RxFifo1MsgLost = 1 << 5, + /// A High Priority Message has been flagged by a filter + RxHighPrio = 1 << 6, + /// Transmit has been completed + TxComplete = 1 << 7, + /// Tx message has been cancelled + TxCancel = 1 << 8, + /// Tx Fifo is empty + TxEmpty = 1 << 9, + /// An new Event has been received in the Tx Event Fifo + TxEventNew = 1 << 10, + /// The TxEvent Fifo is full + TxEventFull = 1 << 11, + /// An Tx Event has been lost + TxEventLost = 1 << 12, + /// Timestamp wrap around has occurred + TsWrapAround = 1 << 13, + /// Message RAM access failure + /// + /// The flag is set when the Rx handler: + /// has not completed acceptance filtering or storage of an accepted message until the + /// arbitration field of the following message has been received. In this case acceptance + /// filtering or message storage is aborted and the Rx handler starts processing of the + /// following message. was unable to write a message to the message RAM. In this case + /// message storage is aborted. + /// In both cases the FIFO put index is not updated. The partly stored message is overwritten + /// when the next message is stored to this location. + /// The flag is also set when the Tx Handler was not able to read a message from the Message + /// RAM in time. In this case message transmission is aborted. In case of a Tx Handler access + /// failure the FDCAN is switched into Restricted operation Mode (see Restricted operation + /// mode). + MsgRamAccessFailure = 1 << 14, + /// Timeout Occurred + TimeoutOccurred = 1 << 15, + /// Overflow of CAN error logging counter occurred + ErrLogOverflow = 1 << 16, + /// Errr Passive + ErrPassive = 1 << 17, + /// Warning Status + WarningStatus = 1 << 18, + /// Bus_Off status + BusOff = 1 << 19, + /// Watchdog interrupt + WatchdogInt = 1 << 20, + /// Protocol error in arbitration phase (nominal bit time is used) + ProtErrArbritation = 1 << 21, + /// Protocol error in data phase (data bit time is used) + ProtErrData = 1 << 22, + /// Access to reserved address + ReservedAccess = 1 << 23, +} + +bitflags::bitflags! { + /// A set of FdCAN interrupts. + #[cfg_attr(feature = "unstable-defmt", derive(defmt::Format))] + pub struct Interrupts: u32 { + /// Rx FIFO 0 has a new message + const RX_FIFO_0_NEW_MESSAGE = 1 << 0; + /// Rx FIFO 0 is full + const RX_FIFO_0_FULL = 1 << 1; + /// Rx FIFO 0 has lost a message + const RX_FIFO_0_MSG_LOST = 1 << 2; + /// Rx FIFO 1 has a new message + const RX_FIFO_1_NEW_MESSAGE = 1 << 3; + /// Rx FIFO 1 is full + const RX_FIFO_1_FULL = 1 << 4; + /// Rx FIFO 1 has lost a message + const RX_FIFO_1_MSG_LOST = 1 << 5; + /// A High Priority Message has been flagged by a filter + const RX_HIGH_PRIO_MSG = 1<<6; + /// Transmit has been completed + const TX_COMPLETE = 1<<7; + /// Tx message has been cancelled + const TX_CANCEL = 1<<8; + /// Tx Fifo is empty + const TX_EMPTY = 1<<9; + /// An new Event has been received in the Tx Event Fifo + const TX_EVENT_NEW = 1<<10; + /// The TxEvent Fifo is full + const TX_EVENT_FULL = 1<<11; + /// An Tx Event has been lost + const TX_EVENT_LOST = 1<<12; + /// Timestamp wrap around has occurred + const TS_WRAP_AROUND = 1<<13; + /// Message RAM access failure + /// + /// The flag is set when the Rx handler: + /// has not completed acceptance filtering or storage of an accepted message until the + /// arbitration field of the following message has been received. In this case acceptance + /// filtering or message storage is aborted and the Rx handler starts processing of the + /// following message. was unable to write a message to the message RAM. In this case + /// message storage is aborted. + /// In both cases the FIFO put index is not updated. The partly stored message is overwritten + /// when the next message is stored to this location. + /// The flag is also set when the Tx Handler was not able to read a message from the Message + /// RAM in time. In this case message transmission is aborted. In case of a Tx Handler access + /// failure the FDCAN is switched into Restricted operation Mode (see Restricted operation + /// mode). + const MSG_RAM_ACCESS_FAILURE = 1<<14; + /// Timeout Occurred + const TIMEOUT_OCCURRED = 1<<15; + /// Overflow of CAN error logging counter occurred + const ERR_LOG_OVERFLOW = 1<<16; + /// Err Passive + const ERR_PASSIVE = 1<<17; + /// Warning Status + const WARNING_STATUS = 1<<18; + /// Bus_Off status + const BUS_OFF = 1<<19; + /// Watchdog interrupt + const WATCHDOG_INT = 1<<20; + /// Protocol error in arbitration phase (nominal bit time is used) + const PROT_ERR_ARBRITATION = 1<<21; + /// Protocol error in data phase (data bit time is used) + const PROT_ERR_DATA = 1<<22; + /// Access to reserved address + const RESERVED_ACCESS = 1<<23; + } +} + +impl Interrupts { + /// No Interrupt masks selected + pub fn none() -> Self { + Self::from_bits_truncate(0) + } +} + +impl From for Interrupts { + #[inline] + fn from(i: Interrupt) -> Self { + Self::from_bits_truncate(i as u32) + } +} + +/// Adds an interrupt to the interrupt set. +impl ops::BitOrAssign for Interrupts { + #[inline] + fn bitor_assign(&mut self, rhs: Interrupt) { + *self |= Self::from(rhs); + } +} + +/// There are two interrupt lines for the FdCan +/// The events linked to these can be configured +/// see `[config::FdCanConfig]` +#[derive(Debug, Copy, Clone, Eq, PartialEq)] +#[cfg_attr(feature = "unstable-defmt", derive(defmt::Format))] +pub enum InterruptLine { + /// Interrupt Line 0 + _0 = 0, + /// Interrupt Line 1 + _1 = 1, +} + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn interrupt_flags() { + assert_eq!( + Interrupts::from(Interrupt::TxComplete), + Interrupts::TX_COMPLETE + ); + assert_eq!(Interrupts::from(Interrupt::TxEmpty), Interrupts::TX_EMPTY); + + let mut ints = Interrupts::RX_FIFO0_FULL; + ints |= Interrupt::RxFifo1Full; + assert_eq!(ints, Interrupts::RX_FIFO0_FULL | Interrupts::RX_FIFO1_FULL); + } +} diff --git a/src/fdcan/message_ram.rs b/src/fdcan/message_ram.rs new file mode 100644 index 00000000..9fbdcc34 --- /dev/null +++ b/src/fdcan/message_ram.rs @@ -0,0 +1,153 @@ +pub(crate) mod common; +pub(crate) mod enums; +pub(crate) mod generic; + +/// Number of Receive Fifos configured by this module +pub const RX_FIFOS_MAX: u8 = 2; +/// Number of Receive Messages per RxFifo configured by this module +pub const RX_FIFO_MAX: u8 = 3; +/// Number of Transmit Messages configured by this module +pub const TX_FIFO_MAX: u8 = 3; +/// Number of Transmit Events configured by this module +pub const TX_EVENT_MAX: u8 = 3; +/// Number of Standard Filters configured by this module +pub const STANDARD_FILTER_MAX: u8 = 28; +/// Number of Extended Filters configured by this module +pub const EXTENDED_FILTER_MAX: u8 = 8; + +/// MessageRam Overlay +#[repr(C)] +pub struct RegisterBlock { + pub(crate) filters: Filters, + pub(crate) receive: [Receive; RX_FIFOS_MAX as usize], + pub(crate) transmit: Transmit, +} +impl RegisterBlock { + pub fn reset(&mut self) { + self.filters.reset(); + self.receive[0].reset(); + self.receive[1].reset(); + self.transmit.reset(); + } +} + +#[repr(C)] +pub(crate) struct Filters { + pub(crate) flssa: [StandardFilter; STANDARD_FILTER_MAX as usize], + pub(crate) flesa: [ExtendedFilter; EXTENDED_FILTER_MAX as usize], +} +impl Filters { + pub fn reset(&mut self) { + for sf in &mut self.flssa { + sf.reset(); + } + for ef in &mut self.flesa { + ef.reset(); + } + } +} + +#[repr(C)] +pub(crate) struct Receive { + pub(crate) fxsa: [RxFifoElement; RX_FIFO_MAX as usize], +} +impl Receive { + pub fn reset(&mut self) { + for fe in &mut self.fxsa { + fe.reset(); + } + } +} + +#[repr(C)] +pub(crate) struct Transmit { + pub(crate) efsa: [TxEventElement; TX_EVENT_MAX as usize], + pub(crate) tbsa: [TxBufferElement; TX_FIFO_MAX as usize], +} +impl Transmit { + pub fn reset(&mut self) { + for ee in &mut self.efsa { + ee.reset(); + } + for be in &mut self.tbsa { + be.reset(); + } + } +} + +pub(crate) mod standard_filter; +pub(crate) type StandardFilterType = u32; +pub(crate) type StandardFilter = generic::Reg; +pub(crate) struct _StandardFilter; +impl generic::Readable for StandardFilter {} +impl generic::Writable for StandardFilter {} + +pub(crate) mod extended_filter; +pub(crate) type ExtendedFilterType = [u32; 2]; +pub(crate) type ExtendedFilter = generic::Reg; +pub(crate) struct _ExtendedFilter; +impl generic::Readable for ExtendedFilter {} +impl generic::Writable for ExtendedFilter {} + +pub(crate) mod txevent_element; +pub(crate) type TxEventElementType = [u32; 2]; +pub(crate) type TxEventElement = generic::Reg; +pub(crate) struct _TxEventElement; +impl generic::Readable for TxEventElement {} +impl generic::Writable for TxEventElement {} + +pub(crate) mod rxfifo_element; +#[repr(C)] +pub(crate) struct RxFifoElement { + pub(crate) header: RxFifoElementHeader, + pub(crate) data: [u32; 16], // TODO: Does this need to be volatile? +} +impl RxFifoElement { + pub(crate) fn reset(&mut self) { + self.header.reset(); + self.data = [0; 16]; + } +} +pub(crate) type RxFifoElementHeaderType = [u32; 2]; +pub(crate) type RxFifoElementHeader = generic::Reg; +pub(crate) struct _RxFifoElement; +impl generic::Readable for RxFifoElementHeader {} +impl generic::Writable for RxFifoElementHeader {} + +pub(crate) mod txbuffer_element; +#[repr(C)] +pub(crate) struct TxBufferElement { + pub(crate) header: TxBufferElementHeader, + pub(crate) data: [u32; 16], // TODO: Does this need to be volatile? +} +impl TxBufferElement { + pub(crate) fn reset(&mut self) { + self.header.reset(); + self.data = [0; 16]; + } +} +pub(crate) type TxBufferElementHeader = generic::Reg; +pub(crate) type TxBufferElementHeaderType = [u32; 2]; +pub(crate) struct _TxBufferElement; +impl generic::Readable for TxBufferElementHeader {} +impl generic::Writable for TxBufferElementHeader {} + +/// Accessor for the FdCan Message Ram Area +pub unsafe trait MsgRamExt { + const MSG_RAM: *mut RegisterBlock; + fn msg_ram(&self) -> &RegisterBlock { + unsafe { &*Self::MSG_RAM } + } + fn msg_ram_mut(&mut self) -> &mut RegisterBlock { + unsafe { &mut *Self::MSG_RAM } + } +} + +// Ensure the RegisterBlock is the same size as on pg 1957 of RM0440. +static_assertions::assert_eq_size!(Filters, [u32; 28 + 16]); +static_assertions::assert_eq_size!(Receive, [u32; 54]); +static_assertions::assert_eq_size!(Transmit, [u32; 6 + 54]); +static_assertions::assert_eq_size!( + RegisterBlock, + [u32; 28 /*Standard Filters*/ +16 /*Extended Filters*/ +54 /*RxFifo0*/ +54 /*RxFifo1*/ +6 /*TxEvent*/ +54 /*TxFifo */] +); diff --git a/src/fdcan/message_ram/common.rs b/src/fdcan/message_ram/common.rs new file mode 100644 index 00000000..027211c5 --- /dev/null +++ b/src/fdcan/message_ram/common.rs @@ -0,0 +1,129 @@ +use super::enums::{ + BitRateSwitching, ErrorStateIndicator, FilterElementConfig, FilterType, FrameFormat, IdType, + RemoteTransmissionRequest, +}; +use super::generic; + +#[doc = "Reader of field `ID`"] +pub type ID_R = generic::R; + +#[doc = "Reader of field `RTR`"] +pub type RTR_R = generic::R; +impl RTR_R { + pub fn rtr(&self) -> RemoteTransmissionRequest { + match self.bits { + false => RemoteTransmissionRequest::TransmitDataFrame, + true => RemoteTransmissionRequest::TransmitRemoteFrame, + } + } + pub fn is_transmit_remote_frame(&self) -> bool { + *self == RemoteTransmissionRequest::TransmitRemoteFrame + } + pub fn is_transmit_data_frame(&self) -> bool { + *self == RemoteTransmissionRequest::TransmitDataFrame + } +} + +#[doc = "Reader of field `XTD`"] +pub type XTD_R = generic::R; +impl XTD_R { + pub fn id_type(&self) -> IdType { + match self.bits() { + false => IdType::StandardId, + true => IdType::ExtendedId, + } + } + pub fn is_standard_id(&self) -> bool { + *self == IdType::StandardId + } + pub fn is_exteded_id(&self) -> bool { + *self == IdType::ExtendedId + } +} + +#[doc = "Reader of field `ESI`"] +pub type ESI_R = generic::R; +impl ESI_R { + pub fn error_state(&self) -> ErrorStateIndicator { + match self.bits() { + false => ErrorStateIndicator::ErrorActive, + true => ErrorStateIndicator::ErrorPassive, + } + } + pub fn is_error_active(&self) -> bool { + *self == ErrorStateIndicator::ErrorActive + } + pub fn is_error_passive(&self) -> bool { + *self == ErrorStateIndicator::ErrorPassive + } +} + +#[doc = "Reader of field `DLC`"] +pub type DLC_R = generic::R; + +#[doc = "Reader of field `BRS`"] +pub type BRS_R = generic::R; +impl BRS_R { + pub fn bit_rate_switching(&self) -> BitRateSwitching { + match self.bits() { + true => BitRateSwitching::WithBRS, + false => BitRateSwitching::WithoutBRS, + } + } + pub fn is_with_brs(&self) -> bool { + *self == BitRateSwitching::WithBRS + } + pub fn is_without_brs(&self) -> bool { + *self == BitRateSwitching::WithoutBRS + } +} + +#[doc = "Reader of field `FDF`"] +pub type FDF_R = generic::R; +impl FDF_R { + pub fn frame_format(&self) -> FrameFormat { + match self.bits() { + false => FrameFormat::Standard, + true => FrameFormat::Fdcan, + } + } + pub fn is_standard_format(&self) -> bool { + *self == FrameFormat::Standard + } + pub fn is_fdcan_format(&self) -> bool { + *self == FrameFormat::Fdcan + } +} + +#[doc = "Reader of field `(X|S)FT`"] +pub type ESFT_R = generic::R; +impl ESFT_R { + #[doc = r"Gets the Filtertype"] + #[inline(always)] + pub fn to_filter_type(&self) -> FilterType { + match self.bits() { + 0b00 => FilterType::RangeFilter, + 0b01 => FilterType::DualIdFilter, + 0b10 => FilterType::ClassicFilter, + 0b11 => FilterType::FilterDisabled, + _ => unreachable!(), + } + } +} + +#[doc = "Reader of field `(E|S)FEC`"] +pub type ESFEC_R = generic::R; +impl ESFEC_R { + pub fn to_filter_element_config(&self) -> FilterElementConfig { + match self.bits() { + 0b000 => FilterElementConfig::DisableFilterElement, + 0b001 => FilterElementConfig::StoreInFifo0, + 0b010 => FilterElementConfig::StoreInFifo1, + 0b011 => FilterElementConfig::Reject, + 0b100 => FilterElementConfig::SetPriority, + 0b101 => FilterElementConfig::SetPriorityAndStoreInFifo0, + 0b110 => FilterElementConfig::SetPriorityAndStoreInFifo1, + _ => unimplemented!(), + } + } +} diff --git a/src/fdcan/message_ram/enums.rs b/src/fdcan/message_ram/enums.rs new file mode 100644 index 00000000..33f3f5d2 --- /dev/null +++ b/src/fdcan/message_ram/enums.rs @@ -0,0 +1,240 @@ +/// Datalength is the message length generalised over +/// the Standard (Classic) and FDCAN message types +#[derive(Clone, Copy, Debug, PartialEq)] +#[cfg_attr(feature = "unstable-defmt", derive(defmt::Format))] +pub enum DataLength { + Standard(u8), + Fdcan(u8), +} +impl DataLength { + /// Creates a DataLength type + /// + /// Uses the byte length and Type of frame as input + pub fn new(len: u8, ff: FrameFormat) -> DataLength { + match ff { + FrameFormat::Standard => match len { + 0..=8 => DataLength::Standard(len), + _ => panic!("DataLength > 8"), + }, + FrameFormat::Fdcan => match len { + 0..=64 => DataLength::Fdcan(len), + _ => panic!("DataLength > 64"), + }, + } + } + /// Specialised function to create standard frames + pub fn new_standard(len: u8) -> DataLength { + Self::new(len, FrameFormat::Standard) + } + /// Specialised function to create FDCAN frames + pub fn new_fdcan(len: u8) -> DataLength { + Self::new(len, FrameFormat::Fdcan) + } + + /// returns the length in bytes + pub fn len(&self) -> u8 { + match self { + DataLength::Standard(l) | DataLength::Fdcan(l) => *l, + } + } + + pub(crate) fn dlc(&self) -> u8 { + match self { + DataLength::Standard(l) => *l, + DataLength::Fdcan(l) => match l { + 0..=8 => *l, + 9..=12 => 12, + 13..=16 => 16, + 17..=20 => 20, + 21..=24 => 24, + 25..=32 => 32, + 33..=48 => 48, + 49..=64 => 64, + _ => panic!("DataLength > 64"), + }, + } + } +} +impl From for FrameFormat { + fn from(dl: DataLength) -> FrameFormat { + match dl { + DataLength::Standard(_) => FrameFormat::Standard, + DataLength::Fdcan(_) => FrameFormat::Fdcan, + } + } +} + +/// Wheter or not to generate an Tx Event +#[derive(Clone, Copy, Debug, PartialEq)] +#[cfg_attr(feature = "unstable-defmt", derive(defmt::Format))] +pub enum Event { + /// Do not generate an Tx Event + NoEvent, + /// Generate an Tx Event with a specified ID + Event(u8), +} + +impl From for EventControl { + fn from(e: Event) -> Self { + match e { + Event::NoEvent => EventControl::DoNotStore, + Event::Event(_) => EventControl::Store, + } + } +} + +impl From> for Event { + fn from(mm: Option) -> Self { + match mm { + None => Event::NoEvent, + Some(mm) => Event::Event(mm), + } + } +} + +impl From for Option { + fn from(e: Event) -> Option { + match e { + Event::NoEvent => None, + Event::Event(mm) => Some(mm), + } + } +} + +/// TODO +#[derive(Clone, Copy, Debug, PartialEq)] +#[cfg_attr(feature = "unstable-defmt", derive(defmt::Format))] +pub enum ErrorStateIndicator { + /// TODO + ErrorActive = 0, + /// TODO + ErrorPassive = 1, +} +impl From for bool { + #[inline(always)] + fn from(e: ErrorStateIndicator) -> Self { + e as u8 != 0 + } +} + +/// Type of frame, standard (classic) or FdCAN +#[derive(Clone, Copy, Debug, PartialEq)] +#[cfg_attr(feature = "unstable-defmt", derive(defmt::Format))] +pub enum FrameFormat { + Standard = 0, + Fdcan = 1, +} +impl From for bool { + #[inline(always)] + fn from(e: FrameFormat) -> Self { + e as u8 != 0 + } +} + +/// Type of Id, Standard or Extended +#[derive(Clone, Copy, Debug, PartialEq)] +#[cfg_attr(feature = "unstable-defmt", derive(defmt::Format))] +pub enum IdType { + /// Standard ID + StandardId = 0, + /// Extended ID + ExtendedId = 1, +} +impl From for bool { + #[inline(always)] + fn from(e: IdType) -> Self { + e as u8 != 0 + } +} + +/// Whether the frame contains data or requests data +#[derive(Clone, Copy, Debug, PartialEq)] +#[cfg_attr(feature = "unstable-defmt", derive(defmt::Format))] +pub enum RemoteTransmissionRequest { + /// Frame contains data + TransmitDataFrame = 0, + /// frame does not contain data + TransmitRemoteFrame = 1, +} +impl From for bool { + #[inline(always)] + fn from(e: RemoteTransmissionRequest) -> Self { + e as u8 != 0 + } +} + +/// Whether BitRateSwitching should be or was enabled +#[derive(Clone, Copy, Debug, PartialEq)] +#[cfg_attr(feature = "unstable-defmt", derive(defmt::Format))] +pub enum BitRateSwitching { + /// disable bit rate switching + WithoutBRS = 0, + /// enable bit rate switching + WithBRS = 1, +} +impl From for bool { + #[inline(always)] + fn from(e: BitRateSwitching) -> Self { + e as u8 != 0 + } +} + +/// Whether to store transmit Events +#[derive(Clone, Copy, Debug, PartialEq)] +#[cfg_attr(feature = "unstable-defmt", derive(defmt::Format))] +pub enum EventControl { + /// do not store an tx event + DoNotStore, + /// store transmit events + Store, +} +impl From for bool { + #[inline(always)] + fn from(e: EventControl) -> Self { + e as u8 != 0 + } +} + +/// If an received message matched any filters +#[derive(Clone, Copy, Debug, PartialEq)] +#[cfg_attr(feature = "unstable-defmt", derive(defmt::Format))] +pub enum FilterFrameMatch { + /// This did match filter + DidMatch(u8), + /// This received frame did not match any specific filters + DidNotMatch, +} + +/// Type of filter to be used +#[derive(Clone, Copy, Debug, PartialEq)] +#[cfg_attr(feature = "unstable-defmt", derive(defmt::Format))] +pub enum FilterType { + /// Filter uses the range between two id's + RangeFilter = 0b00, + /// The filter matches on two specific id's (or one ID checked twice) + DualIdFilter = 0b01, + /// Filter is using a bitmask + ClassicFilter = 0b10, + /// Filter is disabled + FilterDisabled = 0b11, +} + +#[derive(Clone, Copy, Debug, PartialEq)] +#[cfg_attr(feature = "unstable-defmt", derive(defmt::Format))] +pub enum FilterElementConfig { + /// Filter is disabled + DisableFilterElement = 0b000, + /// Store a matching message in FIFO 0 + StoreInFifo0 = 0b001, + /// Store a matching message in FIFO 1 + StoreInFifo1 = 0b010, + /// Reject a matching message + Reject = 0b011, + /// Flag that a priority message has been received, *But do note store!*?? + SetPriority = 0b100, + /// Flag and store message in FIFO 0 + SetPriorityAndStoreInFifo0 = 0b101, + /// Flag and store message in FIFO 1 + SetPriorityAndStoreInFifo1 = 0b110, + //_Unused = 0b111, +} diff --git a/src/fdcan/message_ram/extended_filter.rs b/src/fdcan/message_ram/extended_filter.rs new file mode 100644 index 00000000..5f66468c --- /dev/null +++ b/src/fdcan/message_ram/extended_filter.rs @@ -0,0 +1,130 @@ +use super::common::{ESFEC_R, ESFT_R}; +use super::enums::{FilterElementConfig, FilterType}; +use super::generic; + +#[doc = "Reader of register ExtendedFilter"] +pub(crate) type R = generic::R; +#[doc = "Writer for register ExtendedFilter"] +pub(crate) type W = generic::W; +#[doc = "Register ExtendedFilter `reset()`'s"] +impl generic::ResetValue for super::ExtendedFilter { + type Type = super::ExtendedFilterType; + #[inline(always)] + fn reset_value() -> Self::Type { + // Sets filter element to Disabled + [0x0, 0x0] + } +} + +#[doc = "Reader of field `EFID2`"] +pub(crate) type EFID2_R = generic::R; +#[doc = "Write proxy for field `EFID2`"] +pub(crate) struct EFID2_W<'a> { + w: &'a mut W, +} +impl<'a> EFID2_W<'a> { + #[doc = r"Writes raw bits to the field"] + #[inline(always)] + pub unsafe fn bits(self, value: u32) -> &'a mut W { + self.w.bits[1] = (self.w.bits[1] & !(0x1FFFFFFF)) | ((value as u32) & 0x1FFFFFFF); + self.w + } +} + +#[doc = "Reader of field `EFID1`"] +pub(crate) type EFID1_R = generic::R; +#[doc = "Write proxy for field `EFID1`"] +pub(crate) struct EFID1_W<'a> { + w: &'a mut W, +} +impl<'a> EFID1_W<'a> { + #[doc = r"Writes raw bits to the field"] + #[inline(always)] + pub unsafe fn bits(self, value: u32) -> &'a mut W { + self.w.bits[0] = (self.w.bits[0] & !(0x1FFFFFFF)) | ((value as u32) & 0x1FFFFFFF); + self.w + } +} + +#[doc = "Write proxy for field `EFEC`"] +pub(crate) struct EFEC_W<'a> { + w: &'a mut W, +} +impl<'a> EFEC_W<'a> { + #[doc = r"Writes raw bits to the field"] + #[inline(always)] + pub unsafe fn bits(self, value: u8) -> &'a mut W { + self.w.bits[0] = (self.w.bits[0] & !(0x07 << 29)) | (((value as u32) & 0x07) << 29); + self.w + } + #[doc = r"Sets the field according to FilterElementConfig"] + #[inline(always)] + pub fn set_filter_element_config(self, fec: FilterElementConfig) -> &'a mut W { + //SAFETY: FilterElementConfig only be valid options + unsafe { self.bits(fec as u8) } + } +} + +#[doc = "Write proxy for field `EFT`"] +pub(crate) struct EFT_W<'a> { + w: &'a mut W, +} +impl<'a> EFT_W<'a> { + #[doc = r"Sets the field according the FilterType"] + #[inline(always)] + pub fn set_filter_type(self, filter: FilterType) -> &'a mut W { + //SAFETY: FilterType only be valid options + unsafe { self.bits(filter as u8) } + } + #[doc = r"Writes raw bits to the field"] + #[inline(always)] + pub unsafe fn bits(self, value: u8) -> &'a mut W { + self.w.bits[1] = (self.w.bits[1] & !(0x03 << 30)) | (((value as u32) & 0x03) << 30); + self.w + } +} + +impl R { + #[doc = "Byte 0 - Bits 0:28 - EFID1"] + #[inline(always)] + pub fn sfid1(&self) -> EFID1_R { + EFID1_R::new(((self.bits[0]) & 0x1FFFFFFF) as u32) + } + #[doc = "Byte 0 - Bits 29:31 - EFEC"] + #[inline(always)] + pub fn efec(&self) -> ESFEC_R { + ESFEC_R::new(((self.bits[0] >> 29) & 0x07) as u8) + } + #[doc = "Byte 1 - Bits 0:28 - EFID2"] + #[inline(always)] + pub fn sfid2(&self) -> EFID2_R { + EFID2_R::new(((self.bits[1]) & 0x1FFFFFFF) as u32) + } + #[doc = "Byte 1 - Bits 30:31 - EFT"] + #[inline(always)] + pub fn eft(&self) -> ESFT_R { + ESFT_R::new(((self.bits[1] >> 30) & 0x03) as u8) + } +} +impl W { + #[doc = "Byte 0 - Bits 0:28 - EFID1"] + #[inline(always)] + pub fn efid1(&mut self) -> EFID1_W { + EFID1_W { w: self } + } + #[doc = "Byte 0 - Bits 29:31 - EFEC"] + #[inline(always)] + pub fn efec(&mut self) -> EFEC_W { + EFEC_W { w: self } + } + #[doc = "Byte 1 - Bits 0:28 - EFID2"] + #[inline(always)] + pub fn efid2(&mut self) -> EFID2_W { + EFID2_W { w: self } + } + #[doc = "Byte 1 - Bits 30:31 - EFT"] + #[inline(always)] + pub fn eft(&mut self) -> EFT_W { + EFT_W { w: self } + } +} diff --git a/src/fdcan/message_ram/generic.rs b/src/fdcan/message_ram/generic.rs new file mode 100644 index 00000000..7f232ca5 --- /dev/null +++ b/src/fdcan/message_ram/generic.rs @@ -0,0 +1,256 @@ +use core::marker; + +///This trait shows that register has `read` method +/// +///Registers marked with `Writable` can be also `modify`'ed +pub trait Readable {} + +///This trait shows that register has `write`, `write_with_zero` and `reset` method +/// +///Registers marked with `Readable` can be also `modify`'ed +pub trait Writable {} + +///Reset value of the register +/// +///This value is initial value for `write` method. +///It can be also directly writed to register by `reset` method. +pub trait ResetValue { + ///Register size + type Type; + ///Reset value of the register + fn reset_value() -> Self::Type; +} + +///This structure provides volatile access to register +pub struct Reg { + register: vcell::VolatileCell, + _marker: marker::PhantomData, +} + +unsafe impl Send for Reg {} + +impl Reg +where + Self: Readable, + U: Copy, +{ + ///Reads the contents of `Readable` register + /// + ///You can read the contents of a register in such way: + ///```ignore + ///let bits = periph.reg.read().bits(); + ///``` + ///or get the content of a particular field of a register. + ///```ignore + ///let reader = periph.reg.read(); + ///let bits = reader.field1().bits(); + ///let flag = reader.field2().bit_is_set(); + ///``` + #[inline(always)] + pub fn read(&self) -> R { + R { + bits: self.register.get(), + _reg: marker::PhantomData, + } + } +} + +impl Reg +where + Self: ResetValue + Writable, + U: Copy, +{ + ///Writes the reset value to `Writable` register + /// + ///Resets the register to its initial state + #[inline(always)] + pub fn reset(&self) { + self.register.set(Self::reset_value()) + } +} + +impl Reg +where + Self: ResetValue + Writable, + U: Copy, +{ + ///Writes bits to `Writable` register + /// + ///You can write raw bits into a register: + ///```ignore + ///periph.reg.write(|w| unsafe { w.bits(rawbits) }); + ///``` + ///or write only the fields you need: + ///```ignore + ///periph.reg.write(|w| w + /// .field1().bits(newfield1bits) + /// .field2().set_bit() + /// .field3().variant(VARIANT) + ///); + ///``` + ///Other fields will have reset value. + #[inline(always)] + pub fn write(&self, f: F) + where + F: FnOnce(&mut W) -> &mut W, + { + self.register.set( + f(&mut W { + bits: Self::reset_value(), + _reg: marker::PhantomData, + }) + .bits, + ); + } +} + +impl Reg +where + Self: Writable, + U: Copy + Default, +{ + ///Writes Zero to `Writable` register + /// + ///Similar to `write`, but unused bits will contain 0. + #[inline(always)] + pub fn write_with_zero(&self, f: F) + where + F: FnOnce(&mut W) -> &mut W, + { + self.register.set( + f(&mut W { + bits: U::default(), + _reg: marker::PhantomData, + }) + .bits, + ); + } +} + +impl Reg +where + Self: Readable + Writable, + U: Copy, +{ + ///Modifies the contents of the register + /// + ///E.g. to do a read-modify-write sequence to change parts of a register: + ///```ignore + ///periph.reg.modify(|r, w| unsafe { w.bits( + /// r.bits() | 3 + ///) }); + ///``` + ///or + ///```ignore + ///periph.reg.modify(|_, w| w + /// .field1().bits(newfield1bits) + /// .field2().set_bit() + /// .field3().variant(VARIANT) + ///); + ///``` + ///Other fields will have value they had before call `modify`. + #[inline(always)] + pub fn modify(&self, f: F) + where + for<'w> F: FnOnce(&R, &'w mut W) -> &'w mut W, + { + let bits = self.register.get(); + self.register.set( + f( + &R { + bits, + _reg: marker::PhantomData, + }, + &mut W { + bits, + _reg: marker::PhantomData, + }, + ) + .bits, + ); + } +} + +///Register/field reader +/// +///Result of the [`read`](Reg::read) method of a register. +///Also it can be used in the [`modify`](Reg::read) method +pub struct R { + pub(crate) bits: U, + _reg: marker::PhantomData, +} + +impl R +where + U: Copy, +{ + ///Create new instance of reader + #[inline(always)] + pub(crate) fn new(bits: U) -> Self { + Self { + bits, + _reg: marker::PhantomData, + } + } + ///Read raw bits from register/field + #[inline(always)] + pub fn bits(&self) -> U { + self.bits + } +} + +impl PartialEq for R +where + U: PartialEq, + FI: Copy + Into, +{ + #[inline(always)] + fn eq(&self, other: &FI) -> bool { + self.bits.eq(&(*other).into()) + } +} + +impl R { + ///Value of the field as raw bits + #[inline(always)] + pub fn bit(&self) -> bool { + self.bits + } + ///Returns `true` if the bit is clear (0) + #[inline(always)] + pub fn bit_is_clear(&self) -> bool { + !self.bit() + } + ///Returns `true` if the bit is set (1) + #[inline(always)] + pub fn bit_is_set(&self) -> bool { + self.bit() + } +} + +///Register writer +/// +///Used as an argument to the closures in the [`write`](Reg::write) and [`modify`](Reg::modify) methods of the register +pub struct W { + ///Writable bits + pub(crate) bits: U, + _reg: marker::PhantomData, +} + +impl W { + ///Writes raw bits to the register + #[inline(always)] + pub unsafe fn bits(&mut self, bits: U) -> &mut Self { + self.bits = bits; + self + } +} + +// ///Used if enumerated values cover not the whole range +// #[derive(Clone,Copy,PartialEq)] +// pub enum Variant { +// ///Expected variant +// Val(T), +// ///Raw bits +// Res(U), +// } diff --git a/src/fdcan/message_ram/rxfifo_element.rs b/src/fdcan/message_ram/rxfifo_element.rs new file mode 100644 index 00000000..4158afd0 --- /dev/null +++ b/src/fdcan/message_ram/rxfifo_element.rs @@ -0,0 +1,96 @@ +use super::common::{BRS_R, DLC_R, ESI_R, FDF_R, ID_R, RTR_R, XTD_R}; +use super::enums::{DataLength, FilterFrameMatch}; +use super::generic; + +#[doc = "Reader of register RxFifoElement"] +pub(crate) type R = generic::R; +// #[doc = "Writer for register ExtendedFilter"] +// pub(crate) type W = generic::W; +#[doc = "Register ExtendedFilter `reset()`'s"] +impl generic::ResetValue for super::RxFifoElementHeader { + type Type = super::RxFifoElementHeaderType; + #[inline(always)] + fn reset_value() -> Self::Type { + [0x0, 0x0] + } +} + +#[doc = "Reader of field `RXTS`"] +pub(crate) type RXTS_R = generic::R; + +#[doc = "Reader of field `FIDX`"] +pub(crate) type FIDX_R = generic::R; + +pub(crate) struct _ANMF; +#[doc = "Reader of field `ANMF`"] +pub(crate) type ANMF_R = generic::R; +impl ANMF_R { + pub fn is_matching_frame(&self) -> bool { + self.bit_is_clear() + } +} + +impl R { + #[doc = "Byte 0 - Bits 0:28 - ID"] + #[inline(always)] + pub fn id(&self) -> ID_R { + ID_R::new(((self.bits[0]) & 0x1FFFFFFF) as u32) + } + #[doc = "Byte 0 - Bit 29 - RTR"] + #[inline(always)] + pub fn rtr(&self) -> RTR_R { + RTR_R::new(((self.bits[0] >> 29) & 0x01) != 0) + } + #[doc = "Byte 0 - Bit 30 - XTD"] + #[inline(always)] + pub fn xtd(&self) -> XTD_R { + XTD_R::new(((self.bits[0] >> 30) & 0x01) != 0) + } + #[doc = "Byte 0 - Bit 30 - ESI"] + #[inline(always)] + pub fn esi(&self) -> ESI_R { + ESI_R::new(((self.bits[0] >> 31) & 0x01) != 0) + } + #[doc = "Byte 1 - Bits 0:15 - RXTS"] + #[inline(always)] + pub fn txts(&self) -> RXTS_R { + RXTS_R::new(((self.bits[1]) & 0xFFFF) as u16) + } + #[doc = "Byte 1 - Bits 16:19 - DLC"] + #[inline(always)] + pub fn dlc(&self) -> DLC_R { + DLC_R::new(((self.bits[1] >> 16) & 0x0F) as u8) + } + #[doc = "Byte 1 - Bits 20 - BRS"] + #[inline(always)] + pub fn brs(&self) -> BRS_R { + BRS_R::new(((self.bits[1] >> 20) & 0x01) != 0) + } + #[doc = "Byte 1 - Bits 20 - FDF"] + #[inline(always)] + pub fn fdf(&self) -> FDF_R { + FDF_R::new(((self.bits[1] >> 21) & 0x01) != 0) + } + #[doc = "Byte 1 - Bits 24:30 - FIDX"] + #[inline(always)] + pub fn fidx(&self) -> FIDX_R { + FIDX_R::new(((self.bits[1] >> 24) & 0xFF) as u8) + } + #[doc = "Byte 1 - Bits 31 - ANMF"] + #[inline(always)] + pub fn anmf(&self) -> ANMF_R { + ANMF_R::new(((self.bits[1] >> 31) & 0x01) != 0) + } + pub fn to_data_length(&self) -> DataLength { + let dlc = self.dlc().bits(); + let ff = self.fdf().frame_format(); + DataLength::new(dlc, ff) + } + pub fn to_filter_match(&self) -> FilterFrameMatch { + if self.anmf().is_matching_frame() { + FilterFrameMatch::DidMatch(self.fidx().bits()) + } else { + FilterFrameMatch::DidNotMatch + } + } +} diff --git a/src/fdcan/message_ram/standard_filter.rs b/src/fdcan/message_ram/standard_filter.rs new file mode 100644 index 00000000..2e35a9ef --- /dev/null +++ b/src/fdcan/message_ram/standard_filter.rs @@ -0,0 +1,130 @@ +use super::common::{ESFEC_R, ESFT_R}; +use super::enums::{FilterElementConfig, FilterType}; +use super::generic; + +#[doc = "Reader of register StandardFilter"] +pub(crate) type R = generic::R; +#[doc = "Writer for register StandardFilter"] +pub(crate) type W = generic::W; +#[doc = "Register StandardFilter `reset()`'s with value 0xC0000"] +impl generic::ResetValue for super::StandardFilter { + type Type = super::StandardFilterType; + #[inline(always)] + fn reset_value() -> Self::Type { + // Sets filter element to Disabled + 0xC000 + } +} + +#[doc = "Reader of field `SFID2`"] +pub(crate) type SFID2_R = generic::R; +#[doc = "Write proxy for field `SFID2`"] +pub(crate) struct SFID2_W<'a> { + w: &'a mut W, +} +impl<'a> SFID2_W<'a> { + #[doc = r"Writes raw bits to the field"] + #[inline(always)] + pub unsafe fn bits(self, value: u16) -> &'a mut W { + self.w.bits = (self.w.bits & !(0x03ff)) | ((value as u32) & 0x03ff); + self.w + } +} + +#[doc = "Reader of field `SFID1`"] +pub(crate) type SFID1_R = generic::R; +#[doc = "Write proxy for field `SFID1`"] +pub(crate) struct SFID1_W<'a> { + w: &'a mut W, +} +impl<'a> SFID1_W<'a> { + #[doc = r"Writes raw bits to the field"] + #[inline(always)] + pub unsafe fn bits(self, value: u16) -> &'a mut W { + self.w.bits = (self.w.bits & !(0x03ff << 16)) | (((value as u32) & 0x03ff) << 16); + self.w + } +} + +#[doc = "Write proxy for field `SFEC`"] +pub(crate) struct SFEC_W<'a> { + w: &'a mut W, +} +impl<'a> SFEC_W<'a> { + #[doc = r"Writes raw bits to the field"] + #[inline(always)] + pub unsafe fn bits(self, value: u8) -> &'a mut W { + self.w.bits = (self.w.bits & !(0x07 << 27)) | (((value as u32) & 0x07) << 27); + self.w + } + #[doc = r"Sets the field according to FilterElementConfig"] + #[inline(always)] + pub fn set_filter_element_config(self, fec: FilterElementConfig) -> &'a mut W { + //SAFETY: FilterElementConfig only be valid options + unsafe { self.bits(fec as u8) } + } +} + +#[doc = "Write proxy for field `SFT`"] +pub(crate) struct SFT_W<'a> { + w: &'a mut W, +} +impl<'a> SFT_W<'a> { + #[doc = r"Sets the field according the FilterType"] + #[inline(always)] + pub fn set_filter_type(self, filter: FilterType) -> &'a mut W { + //SAFETY: FilterType only be valid options + unsafe { self.bits(filter as u8) } + } + #[doc = r"Writes raw bits to the field"] + #[inline(always)] + pub unsafe fn bits(self, value: u8) -> &'a mut W { + self.w.bits = (self.w.bits & !(0x03 << 30)) | (((value as u32) & 0x03) << 30); + self.w + } +} + +impl R { + #[doc = "Bits 0:10 - SFID2"] + #[inline(always)] + pub fn sfid2(&self) -> SFID2_R { + SFID2_R::new((self.bits & 0x03ff) as u16) + } + #[doc = "Bits 16:26 - SFID1"] + #[inline(always)] + pub fn sfid1(&self) -> SFID1_R { + SFID1_R::new(((self.bits >> 16) & 0x03ff) as u16) + } + #[doc = "Bits 27:29 - SFEC"] + #[inline(always)] + pub fn sfec(&self) -> ESFEC_R { + ESFEC_R::new(((self.bits >> 27) & 0x07) as u8) + } + #[doc = "Bits 30:31 - SFT"] + #[inline(always)] + pub fn sft(&self) -> ESFT_R { + ESFT_R::new(((self.bits >> 30) & 0x03) as u8) + } +} +impl W { + #[doc = "Bits 0:10 - SFID2"] + #[inline(always)] + pub fn sfid2(&mut self) -> SFID2_W { + SFID2_W { w: self } + } + #[doc = "Bits 16:26 - SFID1"] + #[inline(always)] + pub fn sfid1(&mut self) -> SFID1_W { + SFID1_W { w: self } + } + #[doc = "Bits 27:29 - SFEC"] + #[inline(always)] + pub fn sfec(&mut self) -> SFEC_W { + SFEC_W { w: self } + } + #[doc = "Bits 30:31 - SFT"] + #[inline(always)] + pub fn sft(&mut self) -> SFT_W { + SFT_W { w: self } + } +} diff --git a/src/fdcan/message_ram/txbuffer_element.rs b/src/fdcan/message_ram/txbuffer_element.rs new file mode 100644 index 00000000..f6f1ee6e --- /dev/null +++ b/src/fdcan/message_ram/txbuffer_element.rs @@ -0,0 +1,407 @@ +use super::common::{BRS_R, DLC_R, ESI_R, FDF_R, ID_R, RTR_R, XTD_R}; +use super::enums::{ + BitRateSwitching, DataLength, ErrorStateIndicator, Event, EventControl, FrameFormat, IdType, + RemoteTransmissionRequest, +}; +use super::generic; + +#[doc = "Reader of register TxBufferElement"] +pub(crate) type R = generic::R; +#[doc = "Writer for register TxBufferElement"] +pub(crate) type W = generic::W; +impl generic::ResetValue for super::TxBufferElementHeader { + type Type = super::TxBufferElementHeaderType; + + #[allow(dead_code)] + #[inline(always)] + fn reset_value() -> Self::Type { + [0; 2] + } +} + +#[doc = "Write proxy for field `ESI`"] +pub(crate) struct ESI_W<'a> { + w: &'a mut W, +} +impl<'a> ESI_W<'a> { + #[doc = r"Writes `variant` to the field"] + #[inline(always)] + #[allow(dead_code)] + pub fn set_error_indicator(self, esi: ErrorStateIndicator) -> &'a mut W { + self.bit(esi as u8 != 0) + } + + #[doc = r"Sets the field bit"] + #[inline(always)] + #[allow(dead_code)] + pub fn set_bit(self) -> &'a mut W { + self.bit(true) + } + #[doc = r"Clears the field bit"] + #[inline(always)] + #[allow(dead_code)] + pub fn clear_bit(self) -> &'a mut W { + self.bit(false) + } + #[doc = r"Writes raw bits to the field"] + #[inline(always)] + #[allow(dead_code)] + pub fn bit(self, value: bool) -> &'a mut W { + self.w.bits[0] = (self.w.bits[0] & !(0x01 << 31)) | (((value as u32) & 0x01) << 31); + self.w + } +} + +#[doc = "Write proxy for field `XTD`"] +pub(crate) struct XTD_W<'a> { + w: &'a mut W, +} +impl<'a> XTD_W<'a> { + #[doc = r"Writes `variant` to the field"] + #[inline(always)] + #[allow(dead_code)] + pub fn set_id_type(self, idt: IdType) -> &'a mut W { + self.bit(idt as u8 != 0) + } + + #[doc = r"Sets the field bit"] + #[inline(always)] + #[allow(dead_code)] + pub fn set_bit(self) -> &'a mut W { + self.bit(true) + } + #[doc = r"Clears the field bit"] + #[inline(always)] + #[allow(dead_code)] + pub fn clear_bit(self) -> &'a mut W { + self.bit(false) + } + #[doc = r"Writes raw bits to the field"] + #[inline(always)] + #[allow(dead_code)] + pub fn bit(self, value: bool) -> &'a mut W { + self.w.bits[0] = (self.w.bits[0] & !(0x01 << 30)) | (((value as u32) & 0x01) << 30); + self.w + } +} + +#[doc = "Write proxy for field `RTR`"] +pub(crate) struct RTR_W<'a> { + w: &'a mut W, +} +impl<'a> RTR_W<'a> { + #[doc = r"Writes `variant` to the field"] + #[inline(always)] + #[allow(dead_code)] + pub fn set_rtr(self, rtr: RemoteTransmissionRequest) -> &'a mut W { + self.bit(rtr as u8 != 0) + } + + #[doc = r"Sets the field bit"] + #[inline(always)] + #[allow(dead_code)] + pub fn set_bit(self) -> &'a mut W { + self.bit(true) + } + #[doc = r"Clears the field bit"] + #[inline(always)] + #[allow(dead_code)] + pub fn clear_bit(self) -> &'a mut W { + self.bit(false) + } + #[doc = r"Writes raw bits to the field"] + #[inline(always)] + #[allow(dead_code)] + pub fn bit(self, value: bool) -> &'a mut W { + self.w.bits[0] = (self.w.bits[0] & !(0x01 << 29)) | (((value as u32) & 0x01) << 29); + self.w + } +} + +#[doc = "Write proxy for field `ID`"] +pub(crate) struct ID_W<'a> { + w: &'a mut W, +} +impl<'a> ID_W<'a> { + #[doc = r"Writes raw bits to the field"] + #[inline(always)] + #[allow(dead_code)] + pub unsafe fn bits(self, value: u32) -> &'a mut W { + self.w.bits[0] = (self.w.bits[0] & !(0x0FFFFFFF)) | ((value as u32) & 0x0FFFFFFF); + self.w + } +} + +#[doc = "Write proxy for field `DLC`"] +pub(crate) struct DLC_W<'a> { + w: &'a mut W, +} +impl<'a> DLC_W<'a> { + #[doc = r"Writes raw bits to the field"] + #[inline(always)] + #[allow(dead_code)] + pub unsafe fn bits(self, value: u8) -> &'a mut W { + self.w.bits[1] = (self.w.bits[1] & !(0x0F << 16)) | (((value as u32) & 0x0F) << 16); + self.w + } +} + +#[doc = "Write proxy for field `BRS`"] +pub(crate) struct BRS_W<'a> { + w: &'a mut W, +} +impl<'a> BRS_W<'a> { + #[doc = r"Writes `variant` to the field"] + #[inline(always)] + #[allow(dead_code)] + pub fn set_brs(self, brs: BitRateSwitching) -> &'a mut W { + self.bit(brs as u8 != 0) + } + + #[doc = r"Sets the field bit"] + #[inline(always)] + #[allow(dead_code)] + pub fn set_bit(self) -> &'a mut W { + self.bit(true) + } + #[doc = r"Clears the field bit"] + #[inline(always)] + #[allow(dead_code)] + pub fn clear_bit(self) -> &'a mut W { + self.bit(false) + } + #[doc = r"Writes raw bits to the field"] + #[inline(always)] + #[allow(dead_code)] + pub fn bit(self, value: bool) -> &'a mut W { + self.w.bits[1] = (self.w.bits[1] & !(0x01 << 20)) | (((value as u32) & 0x01) << 20); + self.w + } +} + +#[doc = "Write proxy for field `FDF`"] +pub(crate) struct FDF_W<'a> { + w: &'a mut W, +} +impl<'a> FDF_W<'a> { + #[doc = r"Writes `variant` to the field"] + #[inline(always)] + #[allow(dead_code)] + pub fn set_format(self, fdf: FrameFormat) -> &'a mut W { + self.bit(fdf as u8 != 0) + } + + #[doc = r"Sets the field bit"] + #[inline(always)] + #[allow(dead_code)] + pub fn set_bit(self) -> &'a mut W { + self.bit(true) + } + #[doc = r"Clears the field bit"] + #[inline(always)] + #[allow(dead_code)] + pub fn clear_bit(self) -> &'a mut W { + self.bit(false) + } + #[doc = r"Writes raw bits to the field"] + #[inline(always)] + #[allow(dead_code)] + pub fn bit(self, value: bool) -> &'a mut W { + self.w.bits[1] = (self.w.bits[1] & !(0x01 << 21)) | (((value as u32) & 0x01) << 21); + self.w + } +} + +#[doc = "Reader of field `EFC`"] +pub(crate) type EFC_R = generic::R; +impl EFC_R { + pub fn to_event_control(&self) -> EventControl { + match self.bit() { + false => EventControl::DoNotStore, + true => EventControl::Store, + } + } +} +#[doc = "Write proxy for field `EFC`"] +pub(crate) struct EFC_W<'a> { + w: &'a mut W, +} +impl<'a> EFC_W<'a> { + #[doc = r"Writes `variant` to the field"] + #[inline(always)] + #[allow(dead_code)] + pub fn set_event_control(self, efc: EventControl) -> &'a mut W { + self.bit(match efc { + EventControl::DoNotStore => false, + EventControl::Store => true, + }) + } + + #[doc = r"Sets the field bit"] + #[inline(always)] + #[allow(dead_code)] + pub fn set_bit(self) -> &'a mut W { + self.bit(true) + } + #[doc = r"Clears the field bit"] + #[inline(always)] + #[allow(dead_code)] + pub fn clear_bit(self) -> &'a mut W { + self.bit(false) + } + #[doc = r"Writes raw bits to the field"] + #[inline(always)] + #[allow(dead_code)] + pub fn bit(self, value: bool) -> &'a mut W { + self.w.bits[1] = (self.w.bits[1] & !(0x01 << 23)) | (((value as u32) & 0x01) << 23); + self.w + } +} + +struct Marker(u8); +impl From for Marker { + fn from(e: Event) -> Marker { + match e { + Event::NoEvent => Marker(0), + Event::Event(mm) => Marker(mm), + } + } +} + +#[doc = "Reader of field `MM`"] +pub(crate) type MM_R = generic::R; +#[doc = "Write proxy for field `MM`"] +pub(crate) struct MM_W<'a> { + w: &'a mut W, +} +impl<'a> MM_W<'a> { + #[doc = r"Writes raw bits to the field"] + #[inline(always)] + pub unsafe fn bits(self, value: u8) -> &'a mut W { + self.w.bits[1] = (self.w.bits[1] & !(0x7F << 24)) | (((value as u32) & 0x7F) << 24); + self.w + } + + fn set_message_marker(self, mm: Marker) -> &'a mut W { + unsafe { self.bits(mm.0) } + } +} + +impl R { + #[doc = "Byte 0 - Bits 0:28 - ID"] + #[inline(always)] + pub fn id(&self) -> ID_R { + ID_R::new(((self.bits[0]) & 0x1FFFFFFF) as u32) + } + #[doc = "Byte 0 - Bit 29 - RTR"] + #[inline(always)] + pub fn rtr(&self) -> RTR_R { + RTR_R::new(((self.bits[0] >> 29) & 0x01) != 0) + } + #[doc = "Byte 0 - Bit 30 - XTD"] + #[inline(always)] + pub fn xtd(&self) -> XTD_R { + XTD_R::new(((self.bits[0] >> 30) & 0x01) != 0) + } + #[doc = "Byte 0 - Bit 30 - ESI"] + #[inline(always)] + pub fn esi(&self) -> ESI_R { + ESI_R::new(((self.bits[0] >> 31) & 0x01) != 0) + } + #[doc = "Byte 1 - Bits 16:19 - DLC"] + #[inline(always)] + pub fn dlc(&self) -> DLC_R { + DLC_R::new(((self.bits[1] >> 16) & 0x0F) as u8) + } + #[doc = "Byte 1 - Bits 20 - BRS"] + #[inline(always)] + pub fn brs(&self) -> BRS_R { + BRS_R::new(((self.bits[1] >> 20) & 0x01) != 0) + } + #[doc = "Byte 1 - Bits 20 - FDF"] + #[inline(always)] + pub fn fdf(&self) -> FDF_R { + FDF_R::new(((self.bits[1] >> 21) & 0x01) != 0) + } + #[doc = "Byte 1 - Bits 23 - EFC"] + #[inline(always)] + pub fn efc(&self) -> EFC_R { + EFC_R::new(((self.bits[1] >> 23) & 0x01) != 0) + } + #[doc = "Byte 1 - Bits 24:31 - MM"] + #[inline(always)] + pub fn mm(&self) -> MM_R { + MM_R::new(((self.bits[1] >> 24) & 0xFF) as u8) + } + pub fn to_data_length(&self) -> DataLength { + let dlc = self.dlc().bits(); + let ff = self.fdf().frame_format(); + DataLength::new(dlc, ff) + } + pub fn to_event(&self) -> Event { + let mm = self.mm().bits(); + let efc = self.efc().to_event_control(); + match efc { + EventControl::DoNotStore => Event::NoEvent, + EventControl::Store => Event::Event(mm), + } + } +} +impl W { + #[doc = "Byte 0 - Bits 0:28 - ID"] + #[inline(always)] + pub fn id(&mut self) -> ID_W { + ID_W { w: self } + } + #[doc = "Byte 0 - Bit 29 - RTR"] + #[inline(always)] + pub fn rtr(&mut self) -> RTR_W { + RTR_W { w: self } + } + #[doc = "Byte 0 - Bit 30 - XTD"] + #[inline(always)] + pub fn xtd(&mut self) -> XTD_W { + XTD_W { w: self } + } + #[doc = "Byte 0 - Bit 31 - ESI"] + #[inline(always)] + pub fn esi(&mut self) -> ESI_W { + ESI_W { w: self } + } + #[doc = "Byte 1 - Bit 16:19 - DLC"] + #[inline(always)] + pub fn dlc(&mut self) -> DLC_W { + DLC_W { w: self } + } + #[doc = "Byte 1 - Bit 20 - BRS"] + #[inline(always)] + pub fn brs(&mut self) -> BRS_W { + BRS_W { w: self } + } + #[doc = "Byte 1 - Bit 21 - FDF"] + #[inline(always)] + pub fn fdf(&mut self) -> FDF_W { + FDF_W { w: self } + } + #[doc = "Byte 1 - Bit 23 - EFC"] + #[inline(always)] + pub fn efc(&mut self) -> EFC_W { + EFC_W { w: self } + } + #[doc = "Byte 1 - Bit 24:31 - MM"] + #[inline(always)] + pub fn mm(&mut self) -> MM_W { + MM_W { w: self } + } + #[doc = "Convenience function for setting the data length and frame format"] + #[inline(always)] + pub fn set_len(&mut self, dl: impl Into) -> &mut Self { + let dl: DataLength = dl.into(); + self.fdf().set_format(dl.into()); + unsafe { self.dlc().bits(dl.dlc()) } + } + pub fn set_event(&mut self, event: Event) -> &mut Self { + self.mm().set_message_marker(event.into()); + self.efc().set_event_control(event.into()) + } +} diff --git a/src/fdcan/message_ram/txevent_element.rs b/src/fdcan/message_ram/txevent_element.rs new file mode 100644 index 00000000..d202b858 --- /dev/null +++ b/src/fdcan/message_ram/txevent_element.rs @@ -0,0 +1,135 @@ +use super::common::{BRS_R, DLC_R, ESI_R, RTR_R, XTD_R}; +// use super::enums::{ +// BitRateSwitching, ErrorStateIndicator, FrameFormat, IdType, RemoteTransmissionRequest, +// }; +use super::generic; + +#[doc = "Reader of register TxEventElement"] +pub(crate) type R = generic::R; +// #[doc = "Writer for register TxEventElement"] +// pub(crate) type W = generic::W; +#[doc = "Register TxEventElement `reset()`'s"] +impl generic::ResetValue for super::TxEventElement { + type Type = super::TxEventElementType; + #[inline(always)] + fn reset_value() -> Self::Type { + [0, 0] + } +} + +#[doc = "Reader of field `ID`"] +pub(crate) type ID_R = generic::R; + +#[doc = "Reader of field `TXTS`"] +pub(crate) type TXTS_R = generic::R; + +#[derive(Clone, Copy, Debug, PartialEq)] +pub(crate) enum DataLengthFormat { + StandardLength = 0, + FDCANLength = 1, +} +impl From for bool { + #[inline(always)] + fn from(dlf: DataLengthFormat) -> Self { + dlf as u8 != 0 + } +} + +#[doc = "Reader of field `EDL`"] +pub(crate) type EDL_R = generic::R; +impl EDL_R { + pub fn data_length_format(&self) -> DataLengthFormat { + match self.bits() { + false => DataLengthFormat::StandardLength, + true => DataLengthFormat::FDCANLength, + } + } + pub fn is_standard_length(&self) -> bool { + *self == DataLengthFormat::StandardLength + } + pub fn is_fdcan_length(&self) -> bool { + *self == DataLengthFormat::FDCANLength + } +} + +#[derive(Clone, Copy, Debug, PartialEq)] +pub(crate) enum EventType { + //_Reserved = 0b00, + TxEvent = 0b01, + TxDespiteAbort = 0b10, + //_Reserved = 0b10, +} + +#[doc = "Reader of field `EFC`"] +pub(crate) type EFC_R = generic::R; +impl EFC_R { + pub fn event_type(&self) -> EventType { + match self.bits() { + 0b01 => EventType::TxEvent, + 0b10 => EventType::TxDespiteAbort, + _ => unimplemented!(), + } + } + pub fn is_tx_event(&self) -> bool { + self.event_type() == EventType::TxEvent + } + pub fn is_despite_abort(&self) -> bool { + self.event_type() == EventType::TxDespiteAbort + } +} + +#[doc = "Reader of field `MM`"] +pub(crate) type MM_R = generic::R; + +impl R { + #[doc = "Byte 0 - Bits 0:28 - ID"] + #[inline(always)] + pub fn id(&self) -> ID_R { + ID_R::new(((self.bits[0]) & 0x1FFFFFFF) as u32) + } + #[doc = "Byte 0 - Bit 29 - RTR"] + #[inline(always)] + pub fn rtr(&self) -> RTR_R { + RTR_R::new(((self.bits[0] >> 29) & 0x01) != 0) + } + #[doc = "Byte 0 - Bit 30 - XTD"] + #[inline(always)] + pub fn xtd(&self) -> XTD_R { + XTD_R::new(((self.bits[0] >> 30) & 0x01) != 0) + } + #[doc = "Byte 0 - Bit 30 - ESI"] + #[inline(always)] + pub fn esi(&self) -> ESI_R { + ESI_R::new(((self.bits[0] >> 31) & 0x01) != 0) + } + #[doc = "Byte 1 - Bits 0:15 - TXTS"] + #[inline(always)] + pub fn txts(&self) -> TXTS_R { + TXTS_R::new(((self.bits[1]) & 0xFFFF) as u16) + } + #[doc = "Byte 1 - Bits 16:19 - DLC"] + #[inline(always)] + pub fn dlc(&self) -> DLC_R { + DLC_R::new(((self.bits[1] >> 16) & 0x0F) as u8) + } + #[doc = "Byte 1 - Bits 20 - BRS"] + #[inline(always)] + pub fn brs(&self) -> BRS_R { + BRS_R::new(((self.bits[1] >> 20) & 0x01) != 0) + } + #[doc = "Byte 1 - Bits 21 - EDL"] + #[inline(always)] + pub fn edl(&self) -> EDL_R { + EDL_R::new(((self.bits[1] >> 21) & 0x01) != 0) + } + #[doc = "Byte 1 - Bits 22:23 - EFC"] + #[inline(always)] + pub fn efc(&self) -> EFC_R { + EFC_R::new(((self.bits[1] >> 22) & 0x03) as u8) + } + #[doc = "Byte 1 - Bits 24:31 - MM"] + #[inline(always)] + pub fn mm(&self) -> MM_R { + MM_R::new(((self.bits[1] >> 24) & 0xFF) as u8) + } +} diff --git a/src/lib.rs b/src/lib.rs index 2c55c8c2..c3fc81aa 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -62,6 +62,8 @@ pub use crate::stm32::interrupt; pub mod adc; pub mod bb; +// pub mod can; +pub mod fdcan; // pub mod crc; // pub mod dac; pub mod delay; diff --git a/src/rcc/enable.rs b/src/rcc/enable.rs index 741f43c5..f5a9ed10 100644 --- a/src/rcc/enable.rs +++ b/src/rcc/enable.rs @@ -43,6 +43,7 @@ macro_rules! bus { impl RccBus for crate::stm32::$PER { type Bus = $busX; } + impl crate::rcc::Instance for crate::stm32::$PER {} bus_enable!($PER => ($busX, $bit)); bus_reset!($PER => ($busX, $bit)); )+ @@ -129,7 +130,7 @@ bus! { I2C1 => (APB1_1, 21), I2C2 => (APB1_1, 22), USB_FS_DEVICE => (APB1_1, 23), - FDCAN => (APB1_1, 25), + FDCAN1 => (APB1_1, 25), PWR => (APB1_1, 28), I2C3 => (APB1_1, 30), LPTIMER1 => (APB1_1, 31), @@ -137,6 +138,19 @@ bus! { UCPD1 => (APB1_2, 8), } +#[cfg(any( + feature = "stm32g471", + feature = "stm32g473", + feature = "stm32g474", + feature = "stm32g483", + feature = "stm32g484", + feature = "stm32g491", + feature = "stm32g4A1" +))] +bus! { + FDCAN2 => (APB1_1, 25), +} + #[cfg(any( feature = "stm32g471", feature = "stm32g473", @@ -180,6 +194,7 @@ bus! { feature = "stm32g484" ))] bus! { + FDCAN3 => (APB1_1, 25), TIM20 => (APB2, 20), } diff --git a/src/rcc/mod.rs b/src/rcc/mod.rs index d23a9789..8cf008e0 100644 --- a/src/rcc/mod.rs +++ b/src/rcc/mod.rs @@ -8,11 +8,13 @@ mod enable; pub use clockout::*; pub use config::*; +pub trait Instance: crate::Sealed + Enable + Reset + GetBusFreq {} + /// HSI speed pub const HSI_FREQ: u32 = 16_000_000; /// Clock frequencies -#[derive(Clone, Copy)] +#[derive(Clone, Copy, Debug)] pub struct Clocks { /// System frequency pub sys_clk: Hertz, @@ -29,7 +31,7 @@ pub struct Clocks { } /// PLL Clock frequencies -#[derive(Clone, Copy)] +#[derive(Clone, Copy, Debug)] pub struct PLLClocks { /// R frequency pub r: Hertz,