diff --git a/Cargo.toml b/Cargo.toml index 5534dc8..86e1701 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -10,7 +10,8 @@ members = [ "examples/embassy", "board-tests", "bootloader", - "flashloader", "vorago-shared-peripherals", + "flashloader", + "vorago-shared-periphs", ] exclude = [ "flashloader/slot-a-blinky", diff --git a/va108xx-hal/Cargo.toml b/va108xx-hal/Cargo.toml index 82449b9..797e71e 100644 --- a/va108xx-hal/Cargo.toml +++ b/va108xx-hal/Cargo.toml @@ -15,6 +15,7 @@ cortex-m = { version = "0.7", features = ["critical-section-single-core"]} cortex-m-rt = "0.7" nb = "1" paste = "1" +vorago-shared-periphs = { path = "../vorago-shared-periphs", features = ["vor1x"] } embedded-hal = "1" embedded-hal-async = "1" embedded-hal-nb = "1" diff --git a/va108xx-hal/src/clock.rs b/va108xx-hal/src/clock.rs index 3383145..1e34bdb 100644 --- a/va108xx-hal/src/clock.rs +++ b/va108xx-hal/src/clock.rs @@ -65,14 +65,16 @@ pub fn set_clk_div_register(syscfg: &mut va108xx::Sysconfig, clk_sel: FilterClkS } #[inline] -pub fn enable_peripheral_clock(syscfg: &mut va108xx::Sysconfig, clock: PeripheralClocks) { +pub fn enable_peripheral_clock(clock: PeripheralClocks) { + let syscfg = unsafe { va108xx::Sysconfig::steal() }; syscfg .peripheral_clk_enable() .modify(|r, w| unsafe { w.bits(r.bits() | (1 << clock as u8)) }); } #[inline] -pub fn disable_peripheral_clock(syscfg: &mut va108xx::Sysconfig, clock: PeripheralClocks) { +pub fn disable_peripheral_clock(clock: PeripheralClocks) { + let syscfg = unsafe { va108xx::Sysconfig::steal() }; syscfg .peripheral_clk_enable() .modify(|r, w| unsafe { w.bits(r.bits() & !(1 << clock as u8)) }); diff --git a/va108xx-hal/src/gpio.rs b/va108xx-hal/src/gpio.rs new file mode 100644 index 0000000..fc1fb33 --- /dev/null +++ b/va108xx-hal/src/gpio.rs @@ -0,0 +1,2 @@ +//! GPIO support module. +pub use vorago_shared_periphs::gpio::*; diff --git a/va108xx-hal/src/gpio/asynch.rs b/va108xx-hal/src/gpio_tmp/asynch.rs similarity index 100% rename from va108xx-hal/src/gpio/asynch.rs rename to va108xx-hal/src/gpio_tmp/asynch.rs diff --git a/va108xx-hal/src/gpio/dynpin.rs b/va108xx-hal/src/gpio_tmp/dynpin.rs similarity index 100% rename from va108xx-hal/src/gpio/dynpin.rs rename to va108xx-hal/src/gpio_tmp/dynpin.rs diff --git a/va108xx-hal/src/gpio/mod.rs b/va108xx-hal/src/gpio_tmp/mod.rs similarity index 100% rename from va108xx-hal/src/gpio/mod.rs rename to va108xx-hal/src/gpio_tmp/mod.rs diff --git a/va108xx-hal/src/gpio/pin.rs b/va108xx-hal/src/gpio_tmp/pin.rs similarity index 100% rename from va108xx-hal/src/gpio/pin.rs rename to va108xx-hal/src/gpio_tmp/pin.rs diff --git a/va108xx-hal/src/i2c.rs b/va108xx-hal/src/i2c.rs index 1dd53e8..652f59f 100644 --- a/va108xx-hal/src/i2c.rs +++ b/va108xx-hal/src/i2c.rs @@ -3,9 +3,7 @@ //! ## Examples //! //! - [REB1 I2C temperature sensor example](https://egit.irs.uni-stuttgart.de/rust/va108xx-rs/src/branch/main/vorago-reb1/examples/adt75-temp-sensor.rs) -use crate::{ - clock::enable_peripheral_clock, pac, time::Hertz, typelevel::Sealed, PeripheralSelect, -}; +use crate::{clock::enable_peripheral_clock, pac, sealed::Sealed, time::Hertz, PeripheralSelect}; use core::{marker::PhantomData, ops::Deref}; use embedded_hal::i2c::{self, Operation, SevenBitAddress, TenBitAddress}; @@ -309,14 +307,13 @@ impl I2cBase { impl I2cBase { pub fn new( - syscfg: &mut pac::Sysconfig, - sysclk: impl Into, + sysclk: Hertz, i2c: I2c, speed_mode: I2cSpeed, ms_cfg: Option<&MasterConfig>, sl_cfg: Option<&SlaveConfig>, ) -> Result { - enable_peripheral_clock(syscfg, I2c::PERIPH_SEL); + enable_peripheral_clock(I2c::PERIPH_SEL); let mut i2c_base = I2cBase { i2c, @@ -467,14 +464,13 @@ pub struct I2cMaster { impl I2cMaster { pub fn new( - syscfg: &mut pac::Sysconfig, - sysclk: impl Into, + sysclk: Hertz, i2c: I2c, cfg: MasterConfig, speed_mode: I2cSpeed, ) -> Result { Ok(I2cMaster { - i2c_base: I2cBase::new(syscfg, sysclk, i2c, speed_mode, Some(&cfg), None)?, + i2c_base: I2cBase::new(sysclk, i2c, speed_mode, Some(&cfg), None)?, addr: PhantomData, } .enable_master()) @@ -728,14 +724,13 @@ pub struct I2cSlave { impl I2cSlave { fn new_generic( - sys_cfg: &mut pac::Sysconfig, - sys_clk: impl Into, + sys_clk: Hertz, i2c: I2c, cfg: SlaveConfig, speed_mode: I2cSpeed, ) -> Result { Ok(I2cSlave { - i2c_base: I2cBase::new(sys_cfg, sys_clk, i2c, speed_mode, None, Some(&cfg))?, + i2c_base: I2cBase::new(sys_clk, i2c, speed_mode, None, Some(&cfg))?, addr: PhantomData, } .enable_slave()) @@ -875,8 +870,7 @@ impl I2cSlave { impl I2cSlave { /// Create a new I2C slave for seven bit addresses pub fn new( - sys_cfg: &mut pac::Sysconfig, - sys_clk: impl Into, + sys_clk: Hertz, i2c: I2c, cfg: SlaveConfig, speed_mode: I2cSpeed, @@ -884,18 +878,17 @@ impl I2cSlave { if let I2cAddress::TenBit(_) = cfg.addr { return Err(InitError::WrongAddrMode); } - Ok(Self::new_generic(sys_cfg, sys_clk, i2c, cfg, speed_mode)?) + Ok(Self::new_generic(sys_clk, i2c, cfg, speed_mode)?) } } impl I2cSlave { pub fn new_ten_bit_addr( - sys_cfg: &mut pac::Sysconfig, - sys_clk: impl Into, + sys_clk: Hertz, i2c: I2c, cfg: SlaveConfig, speed_mode: I2cSpeed, ) -> Result { - Self::new_generic(sys_cfg, sys_clk, i2c, cfg, speed_mode) + Self::new_generic(sys_clk, i2c, cfg, speed_mode) } } diff --git a/va108xx-hal/src/lib.rs b/va108xx-hal/src/lib.rs index 1b28470..ccf0861 100644 --- a/va108xx-hal/src/lib.rs +++ b/va108xx-hal/src/lib.rs @@ -8,23 +8,16 @@ pub use va108xx as pac; pub mod clock; pub mod gpio; pub mod i2c; +pub mod pins; pub mod prelude; pub mod pwm; pub mod spi; pub mod sysconfig; pub mod time; pub mod timer; -pub mod typelevel; pub mod uart; -#[derive(Debug, Eq, Copy, Clone, PartialEq)] -#[cfg_attr(feature = "defmt", derive(defmt::Format))] -pub enum FunSel { - Sel0 = 0b00, - Sel1 = 0b01, - Sel2 = 0b10, - Sel3 = 0b11, -} +pub use vorago_shared_periphs::FunSel; #[derive(Debug, Copy, Clone, PartialEq, Eq)] #[cfg_attr(feature = "defmt", derive(defmt::Format))] @@ -118,3 +111,8 @@ pub unsafe fn enable_nvic_interrupt(irq: pac::Interrupt) { pub fn disable_nvic_interrupt(irq: pac::Interrupt) { cortex_m::peripheral::NVIC::mask(irq); } + +#[allow(dead_code)] +pub(crate) mod sealed { + pub trait Sealed {} +} diff --git a/va108xx-hal/src/pins.rs b/va108xx-hal/src/pins.rs new file mode 100644 index 0000000..b651c57 --- /dev/null +++ b/va108xx-hal/src/pins.rs @@ -0,0 +1,226 @@ +pub use vorago_shared_periphs::gpio::{Pin, PinId, Port}; + +use crate::{sysconfig::reset_peripheral_for_cycles, PeripheralSelect}; + +macro_rules! pin_id { + ($Id:ident, $Port:path, $num:literal) => { + // Need paste macro to use ident in doc attribute + paste::paste! { + #[doc = "Pin ID representing pin " $Id] + #[derive(Debug)] + pub enum $Id {} + + impl $crate::sealed::Sealed for $Id {} + impl PinId for $Id { + const PORT: Port = $Port; + const OFFSET: usize = $num; + } + } + }; +} + +impl crate::sealed::Sealed for Pin {} + +pin_id!(Pa0, Port::A, 0); +pin_id!(Pa1, Port::A, 1); +pin_id!(Pa2, Port::A, 2); +pin_id!(Pa3, Port::A, 3); +pin_id!(Pa4, Port::A, 4); +pin_id!(Pa5, Port::A, 5); +pin_id!(Pa6, Port::A, 6); +pin_id!(Pa7, Port::A, 7); +pin_id!(Pa8, Port::A, 8); +pin_id!(Pa9, Port::A, 9); +pin_id!(Pa10, Port::A, 10); +pin_id!(Pa11, Port::A, 11); +pin_id!(Pa12, Port::A, 12); +pin_id!(Pa13, Port::A, 13); +pin_id!(Pa14, Port::A, 14); +pin_id!(Pa15, Port::A, 15); +pin_id!(Pa16, Port::A, 16); +pin_id!(Pa17, Port::A, 17); +pin_id!(Pa18, Port::A, 18); +pin_id!(Pa19, Port::A, 19); +pin_id!(Pa20, Port::A, 20); +pin_id!(Pa21, Port::A, 21); +pin_id!(Pa22, Port::A, 22); +pin_id!(Pa23, Port::A, 23); +pin_id!(Pa24, Port::A, 24); +pin_id!(Pa25, Port::A, 25); +pin_id!(Pa26, Port::A, 26); +pin_id!(Pa27, Port::A, 27); +pin_id!(Pa28, Port::A, 28); +pin_id!(Pa29, Port::A, 29); +pin_id!(Pa30, Port::A, 30); +pin_id!(Pa31, Port::A, 31); + +pin_id!(Pb0, Port::B, 0); +pin_id!(Pb1, Port::B, 1); +pin_id!(Pb2, Port::B, 2); +pin_id!(Pb3, Port::B, 3); +pin_id!(Pb4, Port::B, 4); +pin_id!(Pb5, Port::B, 5); +pin_id!(Pb6, Port::B, 6); +pin_id!(Pb7, Port::B, 7); +pin_id!(Pb8, Port::B, 8); +pin_id!(Pb9, Port::B, 9); +pin_id!(Pb10, Port::B, 10); +pin_id!(Pb11, Port::B, 11); +pin_id!(Pb12, Port::B, 12); +pin_id!(Pb13, Port::B, 13); +pin_id!(Pb14, Port::B, 14); +pin_id!(Pb15, Port::B, 15); +pin_id!(Pb16, Port::B, 16); +pin_id!(Pb17, Port::B, 17); +pin_id!(Pb18, Port::B, 18); +pin_id!(Pb19, Port::B, 19); +pin_id!(Pb20, Port::B, 20); +pin_id!(Pb21, Port::B, 21); +pin_id!(Pb22, Port::B, 22); +pin_id!(Pb23, Port::B, 23); + +pub struct PinsA { + pub pa0: Pin, + pub pa1: Pin, + pub pa2: Pin, + pub pa3: Pin, + pub pa4: Pin, + pub pa5: Pin, + pub pa6: Pin, + pub pa7: Pin, + pub pa8: Pin, + pub pa9: Pin, + pub pa10: Pin, + pub pa11: Pin, + pub pa12: Pin, + pub pa13: Pin, + pub pa14: Pin, + pub pa15: Pin, + pub pa16: Pin, + pub pa17: Pin, + pub pa18: Pin, + pub pa19: Pin, + pub pa20: Pin, + pub pa21: Pin, + pub pa22: Pin, + pub pa23: Pin, + pub pa24: Pin, + pub pa25: Pin, + pub pa26: Pin, + pub pa27: Pin, + pub pa28: Pin, + pub pa29: Pin, + pub pa30: Pin, + pub pa31: Pin, +} + +impl PinsA { + pub fn new(_port_a: va108xx::Porta) -> Self { + let syscfg = unsafe { va108xx::Sysconfig::steal() }; + reset_peripheral_for_cycles(PeripheralSelect::PortA, 2); + syscfg.peripheral_clk_enable().modify(|_, w| { + w.porta().set_bit(); + w.gpio().set_bit(); + w.ioconfig().set_bit() + }); + Self { + pa0: Pin::new(), + pa1: Pin::new(), + pa2: Pin::new(), + pa3: Pin::new(), + pa4: Pin::new(), + pa5: Pin::new(), + pa6: Pin::new(), + pa7: Pin::new(), + pa8: Pin::new(), + pa9: Pin::new(), + pa10: Pin::new(), + pa11: Pin::new(), + pa12: Pin::new(), + pa13: Pin::new(), + pa14: Pin::new(), + pa15: Pin::new(), + pa16: Pin::new(), + pa17: Pin::new(), + pa18: Pin::new(), + pa19: Pin::new(), + pa20: Pin::new(), + pa21: Pin::new(), + pa22: Pin::new(), + pa23: Pin::new(), + pa24: Pin::new(), + pa25: Pin::new(), + pa26: Pin::new(), + pa27: Pin::new(), + pa28: Pin::new(), + pa29: Pin::new(), + pa30: Pin::new(), + pa31: Pin::new(), + } + } +} + +pub struct PinsB { + pub pb0: Pin, + pub pb1: Pin, + pub pb2: Pin, + pub pb3: Pin, + pub pb4: Pin, + pub pb5: Pin, + pub pb6: Pin, + pub pb7: Pin, + pub pb8: Pin, + pub pb9: Pin, + pub pb10: Pin, + pub pb11: Pin, + pub pb12: Pin, + pub pb13: Pin, + pub pb14: Pin, + pub pb15: Pin, + pub pb16: Pin, + pub pb17: Pin, + pub pb18: Pin, + pub pb19: Pin, + pub pb20: Pin, + pub pb21: Pin, + pub pb22: Pin, + pub pb23: Pin, +} + +impl PinsB { + pub fn new(_port_b: va108xx::Portb) -> Self { + let syscfg = unsafe { va108xx::Sysconfig::steal() }; + reset_peripheral_for_cycles(PeripheralSelect::PortB, 2); + syscfg.peripheral_clk_enable().modify(|_, w| { + w.portb().set_bit(); + w.gpio().set_bit(); + w.ioconfig().set_bit() + }); + Self { + pb0: Pin::new(), + pb1: Pin::new(), + pb2: Pin::new(), + pb3: Pin::new(), + pb4: Pin::new(), + pb5: Pin::new(), + pb6: Pin::new(), + pb7: Pin::new(), + pb8: Pin::new(), + pb9: Pin::new(), + pb10: Pin::new(), + pb11: Pin::new(), + pb12: Pin::new(), + pb13: Pin::new(), + pb14: Pin::new(), + pb15: Pin::new(), + pb16: Pin::new(), + pb17: Pin::new(), + pb18: Pin::new(), + pb19: Pin::new(), + pb20: Pin::new(), + pb21: Pin::new(), + pb22: Pin::new(), + pb23: Pin::new(), + } + } +} diff --git a/va108xx-hal/src/pwm.rs b/va108xx-hal/src/pwm.rs index b3f1de0..409d1b7 100644 --- a/va108xx-hal/src/pwm.rs +++ b/va108xx-hal/src/pwm.rs @@ -8,24 +8,16 @@ use core::convert::Infallible; use core::marker::PhantomData; +use crate::clock::enable_peripheral_clock; use crate::pac; use crate::time::Hertz; -use crate::timer::{TimDynRegister, TimPin, TimRegInterface, ValidTim, ValidTimAndPin}; -use crate::{clock::enable_peripheral_clock, gpio::DynPinId}; +use crate::timer::{Tim, TimPeripheralMarker, TimPin, TimRegInterface}; const DUTY_MAX: u16 = u16::MAX; #[derive(Debug)] #[cfg_attr(feature = "defmt", derive(defmt::Format))] -pub(crate) struct PwmCommon { - sys_clk: Hertz, - /// For PWMB, this is the upper limit - current_duty: u16, - /// For PWMA, this value will not be used - current_lower_limit: u16, - current_period: Hertz, - current_rst_val: u32, -} +pub(crate) struct PwmCommon {} enum StatusSelPwm { PwmA = 3, @@ -39,7 +31,8 @@ pub struct PwmB {} // Strongly typed PWM pin //================================================================================================== -pub struct PwmPin { +/* +pub struct PwmPin { pin_and_tim: (Pin, Tim), inner: ReducedPwmPin, mode: PhantomData, @@ -194,32 +187,55 @@ where pin } } +*/ //================================================================================================== -// Reduced PWM pin +// PWM pin //================================================================================================== /// Reduced version where type information is deleted -pub struct ReducedPwmPin { - dyn_reg: TimDynRegister, - common: PwmCommon, +pub struct PwmPin { + tim: Tim, + sys_clk: Hertz, + /// For PWMB, this is the upper limit + current_duty: u16, + /// For PWMA, this value will not be used + current_lower_limit: u16, + current_period: Hertz, + current_rst_val: u32, mode: PhantomData, } -impl ReducedPwmPin { - pub(crate) fn new(tim_id: u8, pin_id: DynPinId, common: PwmCommon) -> Self { - Self { - dyn_reg: TimDynRegister { tim_id, pin_id }, - common, +impl PwmPin { + /// Create a new strongly typed PWM pin + pub fn new( + sys_cfg: &mut pac::Sysconfig, + sys_clk: impl Into + Copy, + pin_and_tim: (Pin, Tim), + initial_period: impl Into + Copy, + ) -> Self { + let mut pin = PwmPin { + tim: Tim::TIM, + current_duty: 0, + current_lower_limit: 0, + current_period: initial_period.into(), + current_rst_val: 0, + sys_clk: sys_clk.into(), mode: PhantomData, - } + }; + enable_peripheral_clock(crate::clock::PeripheralClocks::Gpio); + enable_peripheral_clock(crate::clock::PeripheralClocks::Ioconfig); + sys_cfg + .tim_clk_enable() + .modify(|r, w| unsafe { w.bits(r.bits() | pin.pin_and_tim.1.mask_32()) }); + pin.enable_pwm_a(); + pin.set_period(initial_period); + pin } -} -impl ReducedPwmPin { #[inline] fn enable_pwm_a(&mut self) { - self.dyn_reg + self.tim .reg_block() .ctrl() .modify(|_, w| unsafe { w.status_sel().bits(StatusSelPwm::PwmA as u8) }); @@ -227,7 +243,7 @@ impl ReducedPwmPin { #[inline] fn enable_pwm_b(&mut self) { - self.dyn_reg + self.tim .reg_block() .ctrl() .modify(|_, w| unsafe { w.status_sel().bits(StatusSelPwm::PwmB as u8) }); @@ -235,26 +251,26 @@ impl ReducedPwmPin { #[inline] pub fn get_period(&self) -> Hertz { - self.common.current_period + self.current_period } #[inline] pub fn set_period(&mut self, period: impl Into) { - self.common.current_period = period.into(); + self.current_period = period.into(); // Avoid division by 0 - if self.common.current_period.raw() == 0 { + if self.current_period.raw() == 0 { return; } - self.common.current_rst_val = self.common.sys_clk.raw() / self.common.current_period.raw(); - self.dyn_reg + self.current_rst_val = self.common.sys_clk.raw() / self.common.current_period.raw(); + self.tim .reg_block() .rst_value() - .write(|w| unsafe { w.bits(self.common.current_rst_val) }); + .write(|w| unsafe { w.bits(self.current_rst_val) }); } #[inline] pub fn disable(&mut self) { - self.dyn_reg + self.tim .reg_block() .ctrl() .modify(|_, w| w.enable().clear_bit()); @@ -262,7 +278,7 @@ impl ReducedPwmPin { #[inline] pub fn enable(&mut self) { - self.dyn_reg + self.tim .reg_block() .ctrl() .modify(|_, w| w.enable().set_bit()); @@ -270,51 +286,41 @@ impl ReducedPwmPin { #[inline] pub fn period(&self) -> Hertz { - self.common.current_period + self.current_period } #[inline(always)] pub fn duty(&self) -> u16 { - self.common.current_duty + self.current_duty } } -impl From> for ReducedPwmPin -where - (Pin, Tim): ValidTimAndPin, -{ - fn from(value: PwmPin) -> Self { - value.downgrade() - } -} - -impl From> for ReducedPwmPin -where - (Pin, Tim): ValidTimAndPin, -{ - fn from(value: PwmPin) -> Self { - value.downgrade() - } -} - -impl From> for ReducedPwmPin { - fn from(other: ReducedPwmPin) -> Self { +impl From> for PwmPin { + fn from(other: PwmPin) -> Self { let mut pwmb = Self { - dyn_reg: other.dyn_reg, - common: other.common, mode: PhantomData, + tim: other.tim, + sys_clk: other.sys_clk, + current_duty: other.current_duty, + current_lower_limit: other.current_lower_limit, + current_period: other.current_period, + current_rst_val: other.current_rst_val, }; pwmb.enable_pwm_b(); pwmb } } -impl From> for ReducedPwmPin { - fn from(other: ReducedPwmPin) -> Self { +impl From> for PwmPin { + fn from(other: PwmPin) -> Self { let mut pwmb = Self { - dyn_reg: other.dyn_reg, - common: other.common, mode: PhantomData, + tim: other.tim, + sys_clk: other.sys_clk, + current_duty: other.current_duty, + current_lower_limit: other.current_lower_limit, + current_period: other.current_period, + current_rst_val: other.current_rst_val, }; pwmb.enable_pwm_a(); pwmb @@ -325,48 +331,15 @@ impl From> for ReducedPwmPin { // PWMB implementations //================================================================================================== -impl PwmPin -where - (Pin, Tim): ValidTimAndPin, -{ - pub fn pwmb_lower_limit(&self) -> u16 { - self.inner.pwmb_lower_limit() - } - - pub fn pwmb_upper_limit(&self) -> u16 { - self.inner.pwmb_upper_limit() - } - - /// Set the lower limit for PWMB - /// - /// The PWM signal will be 1 as long as the current RST counter is larger than - /// the lower limit. For example, with a lower limit of 0.5 and and an upper limit - /// of 0.7, Only a fixed period between 0.5 * period and 0.7 * period will be in a high - /// state - pub fn set_pwmb_lower_limit(&mut self, duty: u16) { - self.inner.set_pwmb_lower_limit(duty); - } - - /// Set the higher limit for PWMB - /// - /// The PWM signal will be 1 as long as the current RST counter is smaller than - /// the higher limit. For example, with a lower limit of 0.5 and and an upper limit - /// of 0.7, Only a fixed period between 0.5 * period and 0.7 * period will be in a high - /// state - pub fn set_pwmb_upper_limit(&mut self, duty: u16) { - self.inner.set_pwmb_upper_limit(duty); - } -} - -impl ReducedPwmPin { +impl PwmPin { #[inline(always)] pub fn pwmb_lower_limit(&self) -> u16 { - self.common.current_lower_limit + self.current_lower_limit } #[inline(always)] pub fn pwmb_upper_limit(&self) -> u16 { - self.common.current_duty + self.current_duty } /// Set the lower limit for PWMB @@ -377,11 +350,10 @@ impl ReducedPwmPin { /// state #[inline(always)] pub fn set_pwmb_lower_limit(&mut self, duty: u16) { - self.common.current_lower_limit = duty; - let pwmb_val: u64 = (self.common.current_rst_val as u64 - * self.common.current_lower_limit as u64) - / DUTY_MAX as u64; - self.dyn_reg + self.current_lower_limit = duty; + let pwmb_val: u64 = + (self.current_rst_val as u64 * self.current_lower_limit as u64) / DUTY_MAX as u64; + self.tim .reg_block() .pwmb_value() .write(|w| unsafe { w.bits(pwmb_val as u32) }); @@ -394,10 +366,10 @@ impl ReducedPwmPin { /// of 0.7, Only a fixed period between 0.5 * period and 0.7 * period will be in a high /// state pub fn set_pwmb_upper_limit(&mut self, duty: u16) { - self.common.current_duty = duty; - let pwma_val: u64 = (self.common.current_rst_val as u64 * self.common.current_duty as u64) - / DUTY_MAX as u64; - self.dyn_reg + self.current_duty = duty; + let pwma_val: u64 = + (self.current_rst_val as u64 * self.current_duty as u64) / DUTY_MAX as u64; + self.tim .reg_block() .pwma_value() .write(|w| unsafe { w.bits(pwma_val as u32) }); @@ -408,15 +380,11 @@ impl ReducedPwmPin { // Embedded HAL implementation: PWMA only //================================================================================================== -impl embedded_hal::pwm::ErrorType for PwmPin { +impl embedded_hal::pwm::ErrorType for PwmPin { type Error = Infallible; } -impl embedded_hal::pwm::ErrorType for ReducedPwmPin { - type Error = Infallible; -} - -impl embedded_hal::pwm::SetDutyCycle for ReducedPwmPin { +impl embedded_hal::pwm::SetDutyCycle for PwmPin { #[inline] fn max_duty_cycle(&self) -> u16 { DUTY_MAX @@ -424,11 +392,11 @@ impl embedded_hal::pwm::SetDutyCycle for ReducedPwmPin { #[inline] fn set_duty_cycle(&mut self, duty: u16) -> Result<(), Self::Error> { - self.common.current_duty = duty; - let pwma_val: u64 = (self.common.current_rst_val as u64 - * (DUTY_MAX as u64 - self.common.current_duty as u64)) + self.current_duty = duty; + let pwma_val: u64 = (self.current_rst_val as u64 + * (DUTY_MAX as u64 - self.current_duty as u64)) / DUTY_MAX as u64; - self.dyn_reg + self.tim .reg_block() .pwma_value() .write(|w| unsafe { w.bits(pwma_val as u32) }); @@ -436,18 +404,6 @@ impl embedded_hal::pwm::SetDutyCycle for ReducedPwmPin { } } -impl embedded_hal::pwm::SetDutyCycle for PwmPin { - #[inline] - fn max_duty_cycle(&self) -> u16 { - DUTY_MAX - } - - #[inline] - fn set_duty_cycle(&mut self, duty: u16) -> Result<(), Self::Error> { - self.inner.set_duty_cycle(duty) - } -} - /// Get the corresponding u16 duty cycle from a percent value ranging between 0.0 and 1.0. /// /// Please note that this might load a lot of floating point code because this processor does not diff --git a/va108xx-hal/src/spi.rs b/va108xx-hal/src/spi/mod.rs similarity index 56% rename from va108xx-hal/src/spi.rs rename to va108xx-hal/src/spi/mod.rs index afc8b5c..b705446 100644 --- a/va108xx-hal/src/spi.rs +++ b/va108xx-hal/src/spi/mod.rs @@ -9,21 +9,12 @@ //! - [Blocking SPI example](https://egit.irs.uni-stuttgart.de/rust/va108xx-rs/src/branch/main/examples/simple/examples/spi.rs) //! - [REB1 ADC example](https://egit.irs.uni-stuttgart.de/rust/va108xx-rs/src/branch/main/vorago-reb1/examples/max11519-adc.rs) //! - [REB1 EEPROM library](https://egit.irs.uni-stuttgart.de/rust/va108xx-rs/src/branch/main/vorago-reb1/src/m95m01.rs) -use crate::{ - clock::enable_peripheral_clock, - gpio::pin::{ - AltFunc1, AltFunc2, AltFunc3, Pin, PA10, PA11, PA12, PA13, PA14, PA15, PA16, PA17, PA18, - PA19, PA20, PA21, PA22, PA23, PA24, PA25, PA26, PA27, PA28, PA29, PA30, PA31, PB0, PB1, - PB10, PB11, PB12, PB13, PB14, PB15, PB16, PB17, PB18, PB19, PB2, PB22, PB23, PB3, PB4, PB5, - PB6, PB7, PB8, PB9, - }, - pac, - time::Hertz, - typelevel::Sealed, - PeripheralSelect, -}; +use crate::{clock::enable_peripheral_clock, pac, time::Hertz, PeripheralSelect}; use core::{convert::Infallible, fmt::Debug, marker::PhantomData, ops::Deref}; use embedded_hal::spi::{Mode, MODE_0}; +use pins::{HwCsProvider, PinMiso, PinMosi, PinSck}; + +pub mod pins; //================================================================================================== // Defintions @@ -48,16 +39,26 @@ pub enum HwChipSelectId { Id5 = 5, Id6 = 6, Id7 = 7, - Invalid = 0xff, } #[derive(Debug)] #[cfg_attr(feature = "defmt", derive(defmt::Format))] -pub enum SpiPort { - Porta = 0, - Portb = 1, - Portc = 2, - Invalid = 3, +pub enum SpiId { + A, + B, + C, +} + +impl SpiId { + pub unsafe fn reg_block(&self) -> &'static SpiRegBlock { + unsafe { + match self { + SpiId::A => va108xx::Spia::steal().reg_block(), + SpiId::B => va108xx::Spib::steal().reg_block(), + SpiId::C => va108xx::Spic::steal().reg_block(), + } + } + } } #[derive(Debug, PartialEq, Eq, Copy, Clone)] @@ -73,15 +74,20 @@ pub type SpiRegBlock = pac::spia::RegisterBlock; /// Common trait implemented by all PAC peripheral access structures. The register block /// format is the same for all SPI blocks. -pub trait Instance: Deref { - const IDX: u8; +pub trait SpiPeripheralMarker: Deref { + const ID: SpiId; const PERIPH_SEL: PeripheralSelect; fn ptr() -> *const SpiRegBlock; + + #[inline(always)] + fn reg_block(&self) -> &'static mut SpiRegBlock { + unsafe { &mut *(Self::ptr() as *mut _) } + } } -impl Instance for pac::Spia { - const IDX: u8 = 0; +impl SpiPeripheralMarker for pac::Spia { + const ID: SpiId = SpiId::A; const PERIPH_SEL: PeripheralSelect = PeripheralSelect::Spi0; #[inline(always)] @@ -90,8 +96,8 @@ impl Instance for pac::Spia { } } -impl Instance for pac::Spib { - const IDX: u8 = 1; +impl SpiPeripheralMarker for pac::Spib { + const ID: SpiId = SpiId::B; const PERIPH_SEL: PeripheralSelect = PeripheralSelect::Spi1; #[inline(always)] @@ -100,8 +106,8 @@ impl Instance for pac::Spib { } } -impl Instance for pac::Spic { - const IDX: u8 = 2; +impl SpiPeripheralMarker for pac::Spic { + const ID: SpiId = SpiId::C; const PERIPH_SEL: PeripheralSelect = PeripheralSelect::Spi2; #[inline(always)] @@ -110,170 +116,6 @@ impl Instance for pac::Spic { } } -//================================================================================================== -// Pin type definitions -//================================================================================================== - -pub trait PinSck: Sealed {} -pub trait PinMosi: Sealed {} -pub trait PinMiso: Sealed {} - -pub trait HwCsProvider: Sealed { - const CS_ID: HwChipSelectId; - const SPI_PORT: SpiPort; -} - -pub trait OptionalHwCs: HwCsProvider + Sealed {} - -macro_rules! hw_cs_pins { - ($SPIx:path, $portId: path: - $( - ($PXx:ident, $AFx:ident, $HwCsIdent:path, $typedef:ident), - )+ - ) => { - $( - impl HwCsProvider for Pin<$PXx, $AFx> { - const CS_ID: HwChipSelectId = $HwCsIdent; - const SPI_PORT: SpiPort = $portId; - } - impl OptionalHwCs<$SPIx> for Pin<$PXx, $AFx> {} - pub type $typedef = Pin<$PXx, $AFx>; - )+ - }; -} - -impl HwCsProvider for NoneT { - const CS_ID: HwChipSelectId = HwChipSelectId::Invalid; - const SPI_PORT: SpiPort = SpiPort::Invalid; -} - -impl OptionalHwCs for NoneT {} -impl OptionalHwCs for NoneT {} - -// SPIA - -impl PinSck for Pin {} -impl PinMosi for Pin {} -impl PinMiso for Pin {} - -pub type SpiAPortASck = Pin; -pub type SpiAPortAMosi = Pin; -pub type SpiAPortAMiso = Pin; - -impl PinSck for Pin {} -impl PinMosi for Pin {} -impl PinMiso for Pin {} - -pub type SpiAPortBSck = Pin; -pub type SpiAPortBMosi = Pin; -pub type SpiAPortBMiso = Pin; - -hw_cs_pins!( - pac::Spia, SpiPort::Porta: - (PA28, AltFunc1, HwChipSelectId::Id0, HwCs0SpiAPortA), - (PA27, AltFunc1, HwChipSelectId::Id1, HwCs1SpiAPortA), - (PA26, AltFunc1, HwChipSelectId::Id2, HwCs2SpiAPortA), - (PA25, AltFunc1, HwChipSelectId::Id3, HwCs3SpiAPortA), - (PA24, AltFunc1, HwChipSelectId::Id4, HwCs4SpiAPortA), - (PA23, AltFunc1, HwChipSelectId::Id5, HwCs5SpiAPortA), - (PA22, AltFunc1, HwChipSelectId::Id6, HwCs6SpiAPortA), - (PA21, AltFunc1, HwChipSelectId::Id7, HwCs7SpiAPortA), - (PB6, AltFunc2, HwChipSelectId::Id0, HwCs0SpiAPortB), - (PB5, AltFunc2, HwChipSelectId::Id6, HwCs6SpiAPortB), - (PB4, AltFunc2, HwChipSelectId::Id5, HwCs5SpiAPortB), - (PB3, AltFunc2, HwChipSelectId::Id4, HwCs4SpiAPortB), - (PB2, AltFunc2, HwChipSelectId::Id3, HwCs3SpiAPortB), - (PB1, AltFunc2, HwChipSelectId::Id2, HwCs2SpiAPortB), - (PB0, AltFunc2, HwChipSelectId::Id1, HwCs1SpiAPortB), -); - -// SPIB - -impl PinSck for Pin {} -impl PinMosi for Pin {} -impl PinMiso for Pin {} - -pub type SpiBPortASck = Pin; -pub type SpiBPortAMosi = Pin; -pub type SpiBPortAMiso = Pin; - -impl PinSck for Pin {} -impl PinMosi for Pin {} -impl PinMiso for Pin {} - -impl PinSck for Pin {} -impl PinMosi for Pin {} -impl PinMiso for Pin {} - -hw_cs_pins!( - pac::Spib, SpiPort::Portb: - (PB16, AltFunc1, HwChipSelectId::Id0, HwCs0SpiBPortB0), - (PB15, AltFunc1, HwChipSelectId::Id1, HwCs1SpiBPortB0), - (PB14, AltFunc1, HwChipSelectId::Id2, HwCs2SpiBPortB0), - (PB13, AltFunc1, HwChipSelectId::Id3, HwCs3SpiBPortB), - (PB12, AltFunc1, HwChipSelectId::Id4, HwCs4SpiBPortB), - (PB11, AltFunc1, HwChipSelectId::Id5, HwCs5SpiBPortB), - (PB12, AltFunc2, HwChipSelectId::Id0, HwCs0SpiBPortB2), - (PB11, AltFunc2, HwChipSelectId::Id1, HwCs1SpiBPortB2), - (PB10, AltFunc1, HwChipSelectId::Id6, HwCs6SpiBPortB), - (PB10, AltFunc2, HwChipSelectId::Id2, HwCs2SpiBPortB2), - (PB2, AltFunc1, HwChipSelectId::Id0, HwCs0SpiBPortB1), - (PB1, AltFunc1, HwChipSelectId::Id1, HwCs1SpiBPortB1), - (PB0, AltFunc1, HwChipSelectId::Id2, HwCs2SpiBPortB1), - (PA17, AltFunc2, HwChipSelectId::Id0, HwCs0SpiBPortA), - (PA16, AltFunc2, HwChipSelectId::Id1, HwCs1SpiBPortA), - (PA15, AltFunc2, HwChipSelectId::Id2, HwCs2SpiBPortA), - (PA14, AltFunc2, HwChipSelectId::Id3, HwCs3SpiBPortA), - (PA13, AltFunc2, HwChipSelectId::Id4, HwCs4SpiBPortA), - (PA12, AltFunc2, HwChipSelectId::Id5, HwCs5SpiBPortA0), - (PA11, AltFunc2, HwChipSelectId::Id6, HwCs6SpiBPortA0), - (PA10, AltFunc2, HwChipSelectId::Id7, HwCs7SpiBPortA0), - (PA23, AltFunc2, HwChipSelectId::Id5, HwCs5SpiBPortA1), - (PA22, AltFunc2, HwChipSelectId::Id6, HwCs6SpiBPortA1), - (PA21, AltFunc2, HwChipSelectId::Id7, HwCs7SpiBPortA1), -); - -// SPIC - -// Dummy pin defintion for the ROM SCK. -pub struct RomSck; -// Dummy pin defintion for the ROM MOSI. -pub struct RomMosi; -// Dummy pin defintion for the ROM MISO. -pub struct RomMiso; -// Dummy pin defintion for the ROM chip select. -pub struct RomCs; - -impl Sealed for RomSck {} -impl PinSck for RomSck {} -impl Sealed for RomMosi {} -impl PinMosi for RomMosi {} -impl Sealed for RomMiso {} -impl PinMiso for RomMiso {} -impl Sealed for RomCs {} - -hw_cs_pins!( - pac::Spic, SpiPort::Portc: - (PB9, AltFunc3, HwChipSelectId::Id1, HwCs1SpiCPortB0), - (PB8, AltFunc3, HwChipSelectId::Id2, HwCs2SpiCPortB0), - (PB7, AltFunc3, HwChipSelectId::Id3, HwCs3SpiCPortB), - (PB23, AltFunc3, HwChipSelectId::Id2, HwCs2SpiCPortB1), - (PB22, AltFunc3, HwChipSelectId::Id1, HwCs1SpiCPortB1), - (PA20, AltFunc1, HwChipSelectId::Id1, HwCs1SpiCPortA0), - (PA19, AltFunc1, HwChipSelectId::Id2, HwCs2SpiCPortA0), - (PB18, AltFunc1, HwChipSelectId::Id3, HwCs3SpiCPortA0), - (PA23, AltFunc3, HwChipSelectId::Id1, HwCs1SpiCPortA1), - (PA22, AltFunc3, HwChipSelectId::Id2, HwCs2SpiCPortA1), - (PA21, AltFunc3, HwChipSelectId::Id3, HwCs3SpiCPortA1), - (PA20, AltFunc3, HwChipSelectId::Id4, HwCs4SpiCPortA), -); - -impl HwCsProvider for RomCs { - const CS_ID: HwChipSelectId = HwChipSelectId::Id0; - const SPI_PORT: SpiPort = SpiPort::Portc; -} -impl OptionalHwCs for RomCs {} - //================================================================================================== // Config //================================================================================================== @@ -286,6 +128,7 @@ pub trait TransferConfigProvider { fn hw_cs_id(&self) -> u8; } +/* /// This struct contains all configuration parameter which are transfer specific /// and might change for transfers to different SPI slaves #[derive(Copy, Clone, Debug)] @@ -294,6 +137,7 @@ pub struct TransferConfigWithHwcs { pub hw_cs: Option, pub cfg: TransferConfig, } +*/ /// Type erased variant of the transfer configuration. This is required to avoid generics in /// the SPI constructor. @@ -310,9 +154,10 @@ pub struct TransferConfig { /// Only used when blockmode is used. The SCK will be stalled until an explicit stop bit /// is set on a written word. pub bmstall: bool, - pub hw_cs: HwChipSelectId, + pub hw_cs: Option, } +/* impl TransferConfigWithHwcs { pub fn new_no_hw_cs( clk_cfg: Option, @@ -384,9 +229,10 @@ impl TransferConfigProvider for TransferConfigWithHwcs HwCs::CS_ID as u8 } } +*/ /// Configuration options for the whole SPI bus. See Programmer Guide p.92 for more details -#[derive(Debug)] +#[derive(Debug, Copy, Clone)] #[cfg_attr(feature = "defmt", derive(defmt::Format))] pub struct SpiConfig { clk: SpiClkConfig, @@ -497,20 +343,20 @@ pub trait SpiLowLevel { /// there is actually data in the FIFO. /// /// Uses the [nb] API to allow usage in blocking and non-blocking contexts. - fn write_fifo(&self, data: u32) -> nb::Result<(), Infallible>; + fn write_fifo(&mut self, data: u32) -> nb::Result<(), Infallible>; /// Low level function to write a word to the SPI FIFO without checking whether /// there FIFO is full. /// /// This does not necesarily mean there is a space in the FIFO available. /// Use [Self::write_fifo] function to write a word into the FIFO reliably. - fn write_fifo_unchecked(&self, data: u32); + fn write_fifo_unchecked(&mut self, data: u32); /// Low level function to read a word from the SPI FIFO. Must be preceeded by a /// [Self::write_fifo] call. /// /// Uses the [nb] API to allow usage in blocking and non-blocking contexts. - fn read_fifo(&self) -> nb::Result; + fn read_fifo(&mut self) -> nb::Result; /// Low level function to read a word from from the SPI FIFO. /// @@ -518,11 +364,12 @@ pub trait SpiLowLevel { /// Use the [Self::read_fifo] function to read a word from the FIFO reliably using the [nb] /// API. /// You might also need to mask the value to ignore the BMSTART/BMSTOP bit. - fn read_fifo_unchecked(&self) -> u32; + fn read_fifo_unchecked(&mut self) -> u32; } -pub struct SpiBase { - spi: SpiInstance, +pub struct Spi { + id: SpiId, + reg_block: *mut SpiRegBlock, cfg: SpiConfig, sys_clk: Hertz, /// Fill word for read-only SPI transactions. @@ -532,10 +379,12 @@ pub struct SpiBase { word: PhantomData, } -pub struct Spi { +/* +pub struct Spi<, Pins, Word = u8> { inner: SpiBase, pins: Pins, } +*/ #[inline(always)] pub fn mode_to_cpo_cph_bit(mode: embedded_hal::spi::Mode) -> (bool, bool) { @@ -655,24 +504,88 @@ pub fn clk_div_for_target_clock( Some(rounded_div as u16) } -// Re-export this so it can be used for the constructor -pub use crate::typelevel::NoneT; - -impl SpiBase +impl Spi where >::Error: core::fmt::Debug, { - #[inline] - pub fn spi(&self) -> &SpiInstance { - &self.spi + /// Create a new SPI struct + /// + /// You can delete the pin type information by calling the + /// [`downgrade`](Self::downgrade) function + /// + /// ## Arguments + /// * `syscfg` - Can be passed optionally to enable the peripheral clock + /// * `sys_clk` - System clock + /// * `spi` - SPI bus to use + /// * `pins` - Pins to be used for SPI transactions. These pins are consumed + /// to ensure the pins can not be used for other purposes anymore + /// * `spi_cfg` - Configuration specific to the SPI bus + pub fn new( + sys_clk: impl Into, + spi: SpiI, + _pins: (Sck, Miso, Mosi), + spi_cfg: SpiConfig, + ) -> Self { + enable_peripheral_clock(SpiI::PERIPH_SEL); + let (cpo_bit, cph_bit) = mode_to_cpo_cph_bit(spi_cfg.init_mode); + spi.ctrl0().write(|w| { + unsafe { + w.size().bits(Word::word_reg()); + w.scrdv().bits(spi_cfg.clk.scrdv); + // Clear clock phase and polarity. Will be set to correct value for each + // transfer + w.spo().bit(cpo_bit); + w.sph().bit(cph_bit) + } + }); + + spi.ctrl1().write(|w| { + w.lbm().bit(spi_cfg.loopback_mode); + w.sod().bit(spi_cfg.slave_output_disable); + w.ms().bit(spi_cfg.ms); + w.mdlycap().bit(spi_cfg.master_delayer_capture); + w.blockmode().bit(spi_cfg.blockmode); + w.bmstall().bit(spi_cfg.bmstall); + unsafe { w.ss().bits(0) } + }); + spi.clkprescale() + .write(|w| unsafe { w.bits(spi_cfg.clk.prescale_val as u32) }); + + spi.fifo_clr().write(|w| { + w.rxfifo().set_bit(); + w.txfifo().set_bit() + }); + // Enable the peripheral as the last step as recommended in the + // programmers guide + spi.ctrl1().modify(|_, w| w.enable().set_bit()); + Spi { + id: SpiI::ID, + reg_block: spi.reg_block(), + cfg: spi_cfg, + sys_clk: sys_clk.into(), + fill_word: Default::default(), + bmstall: spi_cfg.bmstall, + blockmode: spi_cfg.blockmode, + word: PhantomData, + } + } + + #[inline(always)] + pub fn reg_block_mut(&mut self) -> &'static mut SpiRegBlock { + unsafe { &mut *(self.reg_block) } + } + + #[inline(always)] + pub fn reg_block(&self) -> &'static SpiRegBlock { + unsafe { &*(self.reg_block) } } #[inline] pub fn cfg_clock(&mut self, cfg: SpiClkConfig) { - self.spi + self.reg_block() .ctrl0() .modify(|_, w| unsafe { w.scrdv().bits(cfg.scrdv) }); - self.spi + self.reg_block() .clkprescale() .write(|w| unsafe { w.bits(cfg.prescale_val as u32) }); } @@ -687,7 +600,7 @@ where #[inline] pub fn cfg_mode(&mut self, mode: Mode) { let (cpo_bit, cph_bit) = mode_to_cpo_cph_bit(mode); - self.spi.ctrl0().modify(|_, w| { + self.reg_block().ctrl0().modify(|_, w| { w.spo().bit(cpo_bit); w.sph().bit(cph_bit) }); @@ -699,27 +612,24 @@ where } #[inline] - pub fn clear_tx_fifo(&self) { - self.spi.fifo_clr().write(|w| w.txfifo().set_bit()); + pub fn clear_tx_fifo(&mut self) { + self.reg_block().fifo_clr().write(|w| w.txfifo().set_bit()); } #[inline] - pub fn clear_rx_fifo(&self) { - self.spi.fifo_clr().write(|w| w.rxfifo().set_bit()); + pub fn clear_rx_fifo(&mut self) { + self.reg_block().fifo_clr().write(|w| w.rxfifo().set_bit()); } #[inline] pub fn perid(&self) -> u32 { - self.spi.perid().read().bits() + self.reg_block().perid().read().bits() } /// Configure the hardware chip select given a hardware chip select ID. #[inline] pub fn cfg_hw_cs(&mut self, hw_cs: HwChipSelectId) { - if hw_cs == HwChipSelectId::Invalid { - return; - } - self.spi.ctrl1().modify(|_, w| { + self.reg_block_mut().ctrl1().modify(|_, w| { w.sod().clear_bit(); unsafe { w.ss().bits(hw_cs as u8); @@ -730,7 +640,8 @@ where /// Configure the hardware chip select given a physical hardware CS pin. #[inline] - pub fn cfg_hw_cs_with_pin>(&mut self, _: &HwCs) { + pub fn cfg_hw_cs_with_pin(&mut self, _: &HwCs) { + // TODO: Error handling. self.cfg_hw_cs(HwCs::CS_ID); } @@ -738,7 +649,7 @@ where /// external chip select handling, for example with GPIO pins. #[inline] pub fn cfg_hw_cs_disable(&mut self) { - self.spi.ctrl1().modify(|_, w| { + self.reg_block().ctrl1().modify(|_, w| { w.sod().set_bit(); w }); @@ -747,35 +658,33 @@ where /// Utility function to configure all relevant transfer parameters in one go. /// This is useful if multiple devices with different clock and mode configurations /// are connected to one bus. - pub fn cfg_transfer>( - &mut self, - transfer_cfg: &TransferConfigWithHwcs, - ) { - if let Some(trans_clk_div) = transfer_cfg.cfg.clk_cfg { + pub fn cfg_transfer(&mut self, transfer_cfg: &TransferConfig) { + if let Some(trans_clk_div) = transfer_cfg.clk_cfg { self.cfg_clock(trans_clk_div); } - if let Some(mode) = transfer_cfg.cfg.mode { + if let Some(mode) = transfer_cfg.mode { self.cfg_mode(mode); } - self.blockmode = transfer_cfg.cfg.blockmode; - self.spi.ctrl1().modify(|_, w| { - if transfer_cfg.cfg.sod { + self.blockmode = transfer_cfg.blockmode; + self.reg_block().ctrl1().modify(|_, w| { + if transfer_cfg.sod { w.sod().set_bit(); } else if transfer_cfg.hw_cs.is_some() { w.sod().clear_bit(); unsafe { - w.ss().bits(HwCs::CS_ID as u8); + w.ss().bits(transfer_cfg.hw_cs.unwrap() as u8); } } else { w.sod().clear_bit(); } - w.blockmode().bit(transfer_cfg.cfg.blockmode); - w.bmstall().bit(transfer_cfg.cfg.bmstall) + w.blockmode().bit(transfer_cfg.blockmode); + w.bmstall().bit(transfer_cfg.bmstall) }); } - fn flush_internal(&self) { - let mut status_reg = self.spi.status().read(); + fn flush_internal(&mut self) { + let reg_block_mut = self.reg_block_mut(); + let mut status_reg = reg_block_mut.status().read(); while status_reg.tfe().bit_is_clear() || status_reg.rne().bit_is_set() || status_reg.busy().bit_is_set() @@ -783,11 +692,11 @@ where if status_reg.rne().bit_is_set() { self.read_fifo_unchecked(); } - status_reg = self.spi.status().read(); + status_reg = reg_block_mut.status().read(); } } - fn transfer_preparation(&self, words: &[Word]) -> Result<(), Infallible> { + fn transfer_preparation(&mut self, words: &[Word]) -> Result<(), Infallible> { if words.is_empty() { return Ok(()); } @@ -797,9 +706,10 @@ where // The FIFO can hold a guaranteed amount of data, so we can pump it on transfer // initialization. Returns the amount of written bytes. - fn initial_send_fifo_pumping_with_words(&self, words: &[Word]) -> usize { + fn initial_send_fifo_pumping_with_words(&mut self, words: &[Word]) -> usize { + let reg_block_mut = self.reg_block_mut(); if self.blockmode { - self.spi.ctrl1().modify(|_, w| w.mtxpause().set_bit()); + reg_block_mut.ctrl1().modify(|_, w| w.mtxpause().set_bit()); } // Fill the first half of the write FIFO let mut current_write_idx = 0; @@ -813,16 +723,19 @@ where current_write_idx += 1; } if self.blockmode { - self.spi.ctrl1().modify(|_, w| w.mtxpause().clear_bit()); + reg_block_mut + .ctrl1() + .modify(|_, w| w.mtxpause().clear_bit()); } current_write_idx } // The FIFO can hold a guaranteed amount of data, so we can pump it on transfer // initialization. - fn initial_send_fifo_pumping_with_fill_words(&self, send_len: usize) -> usize { + fn initial_send_fifo_pumping_with_fill_words(&mut self, send_len: usize) -> usize { + let reg_block_mut = self.reg_block_mut(); if self.blockmode { - self.spi.ctrl1().modify(|_, w| w.mtxpause().set_bit()); + reg_block_mut.ctrl1().modify(|_, w| w.mtxpause().set_bit()); } // Fill the first half of the write FIFO let mut current_write_idx = 0; @@ -836,19 +749,21 @@ where current_write_idx += 1; } if self.blockmode { - self.spi.ctrl1().modify(|_, w| w.mtxpause().clear_bit()); + reg_block_mut + .ctrl1() + .modify(|_, w| w.mtxpause().clear_bit()); } current_write_idx } } -impl SpiLowLevel for SpiBase +impl SpiLowLevel for Spi where >::Error: core::fmt::Debug, { #[inline(always)] - fn write_fifo(&self, data: u32) -> nb::Result<(), Infallible> { - if self.spi.status().read().tnf().bit_is_clear() { + fn write_fifo(&mut self, data: u32) -> nb::Result<(), Infallible> { + if self.reg_block_mut().status().read().tnf().bit_is_clear() { return Err(nb::Error::WouldBlock); } self.write_fifo_unchecked(data); @@ -856,29 +771,31 @@ where } #[inline(always)] - fn write_fifo_unchecked(&self, data: u32) { - self.spi.data().write(|w| unsafe { w.bits(data) }); + fn write_fifo_unchecked(&mut self, data: u32) { + self.reg_block_mut() + .data() + .write(|w| unsafe { w.bits(data) }); } #[inline(always)] - fn read_fifo(&self) -> nb::Result { - if self.spi.status().read().rne().bit_is_clear() { + fn read_fifo(&mut self) -> nb::Result { + if self.reg_block_mut().status().read().rne().bit_is_clear() { return Err(nb::Error::WouldBlock); } Ok(self.read_fifo_unchecked()) } #[inline(always)] - fn read_fifo_unchecked(&self) -> u32 { - self.spi.data().read().bits() + fn read_fifo_unchecked(&mut self) -> u32 { + self.reg_block_mut().data().read().bits() } } -impl embedded_hal::spi::ErrorType for SpiBase { +impl embedded_hal::spi::ErrorType for Spi { type Error = Infallible; } -impl embedded_hal::spi::SpiBus for SpiBase +impl embedded_hal::spi::SpiBus for Spi where >::Error: core::fmt::Debug, { @@ -919,7 +836,7 @@ where } current_write_idx += 1; // Ignore received words. - if self.spi.status().read().rne().bit_is_set() { + if self.reg_block().status().read().rne().bit_is_set() { self.clear_rx_fifo(); } } @@ -984,277 +901,41 @@ where } } -impl< - SpiI: Instance, - Sck: PinSck, - Miso: PinMiso, - Mosi: PinMosi, - Word: WordProvider, - > Spi -where - >::Error: core::fmt::Debug, -{ - /// Create a new SPI struct - /// - /// You can delete the pin type information by calling the - /// [`downgrade`](Self::downgrade) function - /// - /// ## Arguments - /// * `syscfg` - Can be passed optionally to enable the peripheral clock - /// * `sys_clk` - System clock - /// * `spi` - SPI bus to use - /// * `pins` - Pins to be used for SPI transactions. These pins are consumed - /// to ensure the pins can not be used for other purposes anymore - /// * `spi_cfg` - Configuration specific to the SPI bus - pub fn new( - syscfg: &mut pac::Sysconfig, - sys_clk: impl Into, - spi: SpiI, - pins: (Sck, Miso, Mosi), - spi_cfg: SpiConfig, - ) -> Self { - enable_peripheral_clock(syscfg, SpiI::PERIPH_SEL); - let SpiConfig { - clk, - init_mode, - blockmode, - bmstall, - ms, - slave_output_disable, - loopback_mode, - master_delayer_capture, - } = spi_cfg; - - let (cpo_bit, cph_bit) = mode_to_cpo_cph_bit(init_mode); - spi.ctrl0().write(|w| { - unsafe { - w.size().bits(Word::word_reg()); - w.scrdv().bits(clk.scrdv); - // Clear clock phase and polarity. Will be set to correct value for each - // transfer - w.spo().bit(cpo_bit); - w.sph().bit(cph_bit) - } - }); - - spi.ctrl1().write(|w| { - w.lbm().bit(loopback_mode); - w.sod().bit(slave_output_disable); - w.ms().bit(ms); - w.mdlycap().bit(master_delayer_capture); - w.blockmode().bit(blockmode); - w.bmstall().bit(bmstall); - unsafe { w.ss().bits(0) } - }); - spi.clkprescale() - .write(|w| unsafe { w.bits(clk.prescale_val as u32) }); - - spi.fifo_clr().write(|w| { - w.rxfifo().set_bit(); - w.txfifo().set_bit() - }); - // Enable the peripheral as the last step as recommended in the - // programmers guide - spi.ctrl1().modify(|_, w| w.enable().set_bit()); - Spi { - inner: SpiBase { - spi, - cfg: spi_cfg, - sys_clk: sys_clk.into(), - fill_word: Default::default(), - bmstall, - blockmode, - word: PhantomData, - }, - pins, - } - } - - delegate::delegate! { - to self.inner { - #[inline] - pub fn cfg_clock(&mut self, cfg: SpiClkConfig); - - #[inline] - pub fn cfg_clock_from_div(&mut self, div: u16) -> Result<(), SpiClkConfigError>; - - #[inline] - pub fn cfg_mode(&mut self, mode: Mode); - - #[inline] - pub fn perid(&self) -> u32; - - #[inline] - pub fn fill_word(&self) -> Word; - - #[inline] - pub fn spi(&self) -> &SpiI; - - /// Configure the hardware chip select given a hardware chip select ID. - #[inline] - pub fn cfg_hw_cs(&mut self, hw_cs: HwChipSelectId); - - /// Configure the hardware chip select given a physical hardware CS pin. - #[inline] - pub fn cfg_hw_cs_with_pin>(&mut self, _hwcs: &HwCs); - - /// Disables the hardware chip select functionality. This can be used when performing - /// external chip select handling, for example with GPIO pins. - #[inline] - pub fn cfg_hw_cs_disable(&mut self); - - /// Utility function to configure all relevant transfer parameters in one go. - /// This is useful if multiple devices with different clock and mode configurations - /// are connected to one bus. - pub fn cfg_transfer>( - &mut self, transfer_cfg: &TransferConfigWithHwcs - ); - - /// Low level function to write a word to the SPI FIFO but also checks whether - /// there is actually data in the FIFO. - /// - /// Uses the [nb] API to allow usage in blocking and non-blocking contexts. - #[inline(always)] - pub fn write_fifo(&self, data: u32) -> nb::Result<(), Infallible>; - - /// Low level function to write a word to the SPI FIFO. - /// - /// This does not necesarily mean there is a space in the FIFO available. - /// Use [Self::write_fifo] function to write a word into the FIFO reliably using the - /// [nb] API. - #[inline(always)] - pub fn write_fifo_unchecked(&self, data: u32); - - /// Low level function to read a word from the SPI FIFO. Must be preceeded by a - /// [Self::write_fifo] call. - /// - /// Uses the [nb] API to allow usage in blocking and non-blocking contexts. - #[inline(always)] - pub fn read_fifo(&self) -> nb::Result; - - /// Low level function to read a word from from the SPI FIFO. - /// - /// This does not necesarily mean there is a word in the FIFO available. - /// Use the [Self::read_fifo] function to read a word from the FIFO reliably using the [nb] - /// API. - /// You might also need to mask the value to ignore the BMSTART/BMSTOP bit. - #[inline(always)] - pub fn read_fifo_unchecked(&self) -> u32; - } - } - - pub fn set_fill_word(&mut self, fill_word: Word) { - self.inner.fill_word = fill_word; - } - - /// Releases the SPI peripheral and associated pins - pub fn release(self) -> (SpiI, (Sck, Miso, Mosi), SpiConfig) { - (self.inner.spi, self.pins, self.inner.cfg) - } - - pub fn downgrade(self) -> SpiBase { - self.inner - } -} - -impl< - SpiI: Instance, - Sck: PinSck, - Miso: PinMiso, - Mosi: PinMosi, - Word: WordProvider, - > SpiLowLevel for Spi -where - >::Error: core::fmt::Debug, -{ - delegate::delegate! { - to self.inner { - fn write_fifo(&self, data: u32) -> nb::Result<(), Infallible>; - fn write_fifo_unchecked(&self, data: u32); - fn read_fifo(&self) -> nb::Result; - fn read_fifo_unchecked(&self) -> u32; - } - } -} - -impl< - SpiI: Instance, - Word: WordProvider, - Sck: PinSck, - Miso: PinMiso, - Mosi: PinMosi, - > embedded_hal::spi::ErrorType for Spi -{ - type Error = Infallible; -} - -impl< - SpiI: Instance, - Word: WordProvider, - Sck: PinSck, - Miso: PinMiso, - Mosi: PinMosi, - > embedded_hal::spi::SpiBus for Spi -where - >::Error: core::fmt::Debug, -{ - delegate::delegate! { - to self.inner { - fn read(&mut self, words: &mut [Word]) -> Result<(), Self::Error>; - fn write(&mut self, words: &[Word]) -> Result<(), Self::Error>; - fn transfer(&mut self, read: &mut [Word], write: &[Word]) -> Result<(), Self::Error>; - fn transfer_in_place(&mut self, words: &mut [Word]) -> Result<(), Self::Error>; - fn flush(&mut self) -> Result<(), Self::Error>; - } - } -} - /// Changing the word size also requires a type conversion -impl, Miso: PinMiso, Mosi: PinMosi> - From> for Spi -{ - fn from(old_spi: Spi) -> Self { +impl From> for Spi { + fn from(old_spi: Spi) -> Self { old_spi - .inner - .spi + .reg_block() .ctrl0() .modify(|_, w| unsafe { w.size().bits(WordSize::SixteenBits as u8) }); Spi { - inner: SpiBase { - spi: old_spi.inner.spi, - cfg: old_spi.inner.cfg, - blockmode: old_spi.inner.blockmode, - fill_word: Default::default(), - bmstall: old_spi.inner.bmstall, - sys_clk: old_spi.inner.sys_clk, - word: PhantomData, - }, - pins: old_spi.pins, + id: old_spi.id, + reg_block: old_spi.reg_block, + cfg: old_spi.cfg, + blockmode: old_spi.blockmode, + fill_word: Default::default(), + bmstall: old_spi.bmstall, + sys_clk: old_spi.sys_clk, + word: PhantomData, } } } -/// Changing the word size also requires a type conversion -impl, Miso: PinMiso, Mosi: PinMosi> - From> for Spi -{ - fn from(old_spi: Spi) -> Self { +impl From> for Spi { + fn from(old_spi: Spi) -> Self { old_spi - .inner - .spi + .reg_block() .ctrl0() .modify(|_, w| unsafe { w.size().bits(WordSize::EightBits as u8) }); Spi { - inner: SpiBase { - spi: old_spi.inner.spi, - cfg: old_spi.inner.cfg, - blockmode: old_spi.inner.blockmode, - bmstall: old_spi.inner.bmstall, - sys_clk: old_spi.inner.sys_clk, - fill_word: Default::default(), - word: PhantomData, - }, - pins: old_spi.pins, + id: old_spi.id, + reg_block: old_spi.reg_block, + cfg: old_spi.cfg, + blockmode: old_spi.blockmode, + fill_word: Default::default(), + bmstall: old_spi.bmstall, + sys_clk: old_spi.sys_clk, + word: PhantomData, } } } diff --git a/va108xx-hal/src/spi/pins.rs b/va108xx-hal/src/spi/pins.rs new file mode 100644 index 0000000..515873b --- /dev/null +++ b/va108xx-hal/src/spi/pins.rs @@ -0,0 +1,416 @@ +use vorago_shared_periphs::FunSel; + +use crate::{ + pins::{ + Pa10, Pa11, Pa12, Pa13, Pa14, Pa15, Pa16, Pa17, Pa18, Pa19, Pa20, Pa21, Pa22, Pa23, Pa24, + Pa25, Pa26, Pa27, Pa28, Pa29, Pa30, Pa31, Pb0, Pb1, Pb10, Pb11, Pb12, Pb13, Pb14, Pb15, + Pb16, Pb17, Pb18, Pb19, Pb2, Pb22, Pb23, Pb3, Pb4, Pb5, Pb6, Pb7, Pb8, Pb9, Pin, + }, + sealed::Sealed, +}; + +use super::{HwChipSelectId, SpiId}; + +pub trait PinSck: Sealed { + const SPI_ID: SpiId; + const FUN_SEL: FunSel; +} + +pub trait PinMosi: Sealed { + const SPI_ID: SpiId; + const FUN_SEL: FunSel; +} + +pub trait PinMiso: Sealed { + const SPI_ID: SpiId; + const FUN_SEL: FunSel; +} + +pub trait HwCsProvider: Sealed { + const SPI_ID: SpiId; + const FUN_SEL: FunSel; + const CS_ID: HwChipSelectId; +} + +macro_rules! hw_cs_pins { + ($SpiId:path, $(($Px:ident, $FunSel:path, $HwCsIdent:path),)+) => { + $( + impl HwCsProvider for Pin<$Px> { + const SPI_ID: SpiId = $SpiId; + const FUN_SEL: FunSel = $FunSel; + const CS_ID: HwChipSelectId = $HwCsIdent; + } + )+ + }; +} +macro_rules! hw_cs_multi_pin { + ( + // name of the newtype wrapper struct + $name:ident, + // Pb0 + $pin_id:path, + // SpiId::B + $spi_id:path, + // FunSel::Sel1 + $fun_sel:path, + // HwChipSelectId::Id2 + $cs_id:path + ) => { + #[doc = concat!( + "Newtype wrapper to use [Pin] [`", stringify!($pin_ty), + "`] as a HW CS pin for [`", stringify!($spi_id), + "`] with [`", stringify!($cs_id), "`]." + )] + pub struct $name(Pin<$pin_id>); + + impl $name { + pub fn new(pin: Pin<$pin_id>) -> Self { + Self(pin) + } + } + + impl Sealed for $name {} + + impl HwCsProvider for $name { + const SPI_ID: SpiId = $spi_id; + const FUN_SEL: FunSel = $fun_sel; + const CS_ID: HwChipSelectId = $cs_id; + } + }; +} + +// SPIA + +impl PinSck for Pin { + const SPI_ID: SpiId = SpiId::A; + const FUN_SEL: FunSel = FunSel::Sel1; +} +impl PinMosi for Pin { + const SPI_ID: SpiId = SpiId::A; + const FUN_SEL: FunSel = FunSel::Sel1; +} +impl PinMiso for Pin { + const SPI_ID: SpiId = SpiId::A; + const FUN_SEL: FunSel = FunSel::Sel1; +} + +pub type SpiAPortASck = Pin; +pub type SpiAPortAMosi = Pin; +pub type SpiAPortAMiso = Pin; + +impl PinSck for Pin { + const SPI_ID: SpiId = SpiId::A; + const FUN_SEL: FunSel = FunSel::Sel2; +} +impl PinMosi for Pin { + const SPI_ID: SpiId = SpiId::A; + const FUN_SEL: FunSel = FunSel::Sel2; +} +impl PinMiso for Pin { + const SPI_ID: SpiId = SpiId::A; + const FUN_SEL: FunSel = FunSel::Sel2; +} + +pub type SpiAPortBSck = Pin; +pub type SpiAPortBMosi = Pin; +pub type SpiAPortBMiso = Pin; + +hw_cs_pins!( + SpiId::A, + (Pb0, FunSel::Sel2, HwChipSelectId::Id1), + (Pb1, FunSel::Sel2, HwChipSelectId::Id2), + (Pb2, FunSel::Sel2, HwChipSelectId::Id3), + (Pb3, FunSel::Sel2, HwChipSelectId::Id4), + (Pb4, FunSel::Sel2, HwChipSelectId::Id5), + (Pb5, FunSel::Sel2, HwChipSelectId::Id6), + (Pb6, FunSel::Sel2, HwChipSelectId::Id0), + (Pa24, FunSel::Sel1, HwChipSelectId::Id4), + (Pa25, FunSel::Sel1, HwChipSelectId::Id3), + (Pa26, FunSel::Sel1, HwChipSelectId::Id2), + (Pa27, FunSel::Sel1, HwChipSelectId::Id1), + (Pa28, FunSel::Sel1, HwChipSelectId::Id0), +); + +hw_cs_multi_pin!( + PinPb0SpiaHwCsId1, + Pb0, + SpiId::A, + FunSel::Sel2, + HwChipSelectId::Id1 +); +hw_cs_multi_pin!( + PinPb1SpiaHwCsId2, + Pb1, + SpiId::A, + FunSel::Sel2, + HwChipSelectId::Id2 +); +hw_cs_multi_pin!( + PinPb2SpiaHwCsId3, + Pb2, + SpiId::A, + FunSel::Sel2, + HwChipSelectId::Id3 +); + +hw_cs_multi_pin!( + PinPa21SpiaHwCsId7, + Pa21, + SpiId::A, + FunSel::Sel1, + HwChipSelectId::Id7 +); +hw_cs_multi_pin!( + PinPa22SpiaHwCsId6, + Pa22, + SpiId::A, + FunSel::Sel1, + HwChipSelectId::Id6 +); +hw_cs_multi_pin!( + PinPa23SpiaHwCsId5, + Pa23, + SpiId::A, + FunSel::Sel1, + HwChipSelectId::Id5 +); + +// SPIB + +impl PinSck for Pin { + const SPI_ID: SpiId = SpiId::B; + const FUN_SEL: FunSel = FunSel::Sel2; +} +impl PinMosi for Pin { + const SPI_ID: SpiId = SpiId::B; + const FUN_SEL: FunSel = FunSel::Sel2; +} +impl PinMosi for Pin { + const SPI_ID: SpiId = SpiId::B; + const FUN_SEL: FunSel = FunSel::Sel2; +} + +pub type SpiBPortASck = Pin; +pub type SpiBPortAMosi = Pin; +pub type SpiBPortAMiso = Pin; + +impl PinSck for Pin { + const SPI_ID: SpiId = SpiId::B; + const FUN_SEL: FunSel = FunSel::Sel1; +} +impl PinMosi for Pin { + const SPI_ID: SpiId = SpiId::B; + const FUN_SEL: FunSel = FunSel::Sel1; +} +impl PinMosi for Pin { + const SPI_ID: SpiId = SpiId::B; + const FUN_SEL: FunSel = FunSel::Sel1; +} + +impl PinSck for Pin { + const SPI_ID: SpiId = SpiId::B; + const FUN_SEL: FunSel = FunSel::Sel1; +} +impl PinMosi for Pin { + const SPI_ID: SpiId = SpiId::B; + const FUN_SEL: FunSel = FunSel::Sel1; +} +impl PinMosi for Pin { + const SPI_ID: SpiId = SpiId::B; + const FUN_SEL: FunSel = FunSel::Sel1; +} + +// TODO: Need to deal with these duplications.. +hw_cs_pins!( + SpiId::B, + (Pb16, FunSel::Sel1, HwChipSelectId::Id0), + (Pb15, FunSel::Sel1, HwChipSelectId::Id1), + (Pb14, FunSel::Sel1, HwChipSelectId::Id2), + (Pb13, FunSel::Sel1, HwChipSelectId::Id3), + (Pa17, FunSel::Sel2, HwChipSelectId::Id0), + (Pa16, FunSel::Sel2, HwChipSelectId::Id1), + (Pa15, FunSel::Sel2, HwChipSelectId::Id2), + (Pa14, FunSel::Sel2, HwChipSelectId::Id3), + (Pa13, FunSel::Sel2, HwChipSelectId::Id4), + (Pa12, FunSel::Sel2, HwChipSelectId::Id5), + (Pa11, FunSel::Sel2, HwChipSelectId::Id6), + (Pa10, FunSel::Sel2, HwChipSelectId::Id7), + (Pa23, FunSel::Sel2, HwChipSelectId::Id5), + (Pa22, FunSel::Sel2, HwChipSelectId::Id6), + (Pa21, FunSel::Sel2, HwChipSelectId::Id7), +); + +hw_cs_multi_pin!( + PinPb0SpibHwCsId2, + Pb0, + SpiId::B, + FunSel::Sel1, + HwChipSelectId::Id2 +); +hw_cs_multi_pin!( + PinPb1SpibHwCsId1, + Pb1, + SpiId::B, + FunSel::Sel1, + HwChipSelectId::Id1 +); +hw_cs_multi_pin!( + PinPb2SpibHwCsId0, + Pb2, + SpiId::B, + FunSel::Sel1, + HwChipSelectId::Id0 +); + +hw_cs_multi_pin!( + PinPb10SpibHwCsId6, + Pb10, + SpiId::B, + FunSel::Sel1, + HwChipSelectId::Id6 +); +hw_cs_multi_pin!( + PinPb11SpibHwCsId5, + Pb11, + SpiId::B, + FunSel::Sel1, + HwChipSelectId::Id5 +); +hw_cs_multi_pin!( + PinPb12SpibHwCsId4, + Pb12, + SpiId::B, + FunSel::Sel1, + HwChipSelectId::Id4 +); + +hw_cs_multi_pin!( + PinPb10SpibHwCsId2, + Pb10, + SpiId::B, + FunSel::Sel2, + HwChipSelectId::Id2 +); +hw_cs_multi_pin!( + PinPb11SpibHwCsId1, + Pb11, + SpiId::B, + FunSel::Sel2, + HwChipSelectId::Id1 +); +hw_cs_multi_pin!( + PinPb12SpibHwCsId0, + Pb12, + SpiId::B, + FunSel::Sel2, + HwChipSelectId::Id0 +); + +hw_cs_multi_pin!( + PinPa21SpibHwCsId7, + Pa21, + SpiId::B, + FunSel::Sel2, + HwChipSelectId::Id7 +); +hw_cs_multi_pin!( + PinPa22SpibHwCsId6, + Pa22, + SpiId::B, + FunSel::Sel2, + HwChipSelectId::Id6 +); +hw_cs_multi_pin!( + PinPa23SpibHwCsId5, + Pa23, + SpiId::B, + FunSel::Sel2, + HwChipSelectId::Id5 +); + +// SPIC + +// Dummy pin defintion for the ROM SCK. +pub struct RomSck; +// Dummy pin defintion for the ROM MOSI. +pub struct RomMosi; +// Dummy pin defintion for the ROM MISO. +pub struct RomMiso; +// Dummy pin defintion for the ROM chip select. +pub struct RomCs; + +impl Sealed for RomSck {} +impl PinSck for RomSck { + const SPI_ID: SpiId = SpiId::C; + /// Function select does not make sense here, just select default value. + const FUN_SEL: FunSel = FunSel::Sel0; +} +impl Sealed for RomMosi {} +impl PinMosi for RomMosi { + const SPI_ID: SpiId = SpiId::C; + /// Function select does not make sense here, just select default value. + const FUN_SEL: FunSel = FunSel::Sel0; +} +impl Sealed for RomMiso {} +impl PinMiso for RomMiso { + const SPI_ID: SpiId = SpiId::C; + /// Function select does not make sense here, just select default value. + const FUN_SEL: FunSel = FunSel::Sel0; +} +impl Sealed for RomCs {} + +hw_cs_pins!( + SpiId::C, + (Pb9, FunSel::Sel3, HwChipSelectId::Id1), + (Pb8, FunSel::Sel3, HwChipSelectId::Id2), + (Pb7, FunSel::Sel3, HwChipSelectId::Id3), + (Pb23, FunSel::Sel3, HwChipSelectId::Id2), + (Pb22, FunSel::Sel3, HwChipSelectId::Id1), + (Pa20, FunSel::Sel1, HwChipSelectId::Id1), + (Pa19, FunSel::Sel1, HwChipSelectId::Id2), + (Pb18, FunSel::Sel1, HwChipSelectId::Id3), +); + +hw_cs_multi_pin!( + PinPa21SpicHwCsId3, + Pa21, + SpiId::C, + FunSel::Sel3, + HwChipSelectId::Id3 +); +hw_cs_multi_pin!( + PinPa22SpicHwCsId2, + Pa22, + SpiId::C, + FunSel::Sel3, + HwChipSelectId::Id2 +); +hw_cs_multi_pin!( + PinPa23SpicHwCsId1, + Pa23, + SpiId::C, + FunSel::Sel3, + HwChipSelectId::Id1 +); + +hw_cs_multi_pin!( + PinPa20SpicHwCsId1, + Pa20, + SpiId::C, + FunSel::Sel1, + HwChipSelectId::Id1 +); +hw_cs_multi_pin!( + PinPa20SpicHwCsId4, + Pa20, + SpiId::C, + FunSel::Sel3, + HwChipSelectId::Id4 +); + +impl HwCsProvider for RomCs { + const CS_ID: HwChipSelectId = HwChipSelectId::Id0; + const SPI_ID: SpiId = SpiId::C; + /// Function select does not make sense here, just select default value. + const FUN_SEL: FunSel = FunSel::Sel0; +} diff --git a/va108xx-hal/src/sysconfig.rs b/va108xx-hal/src/sysconfig.rs index 5d04355..e50abc6 100644 --- a/va108xx-hal/src/sysconfig.rs +++ b/va108xx-hal/src/sysconfig.rs @@ -1,4 +1,4 @@ -use crate::{pac, PeripheralSelect}; +use crate::PeripheralSelect; #[derive(PartialEq, Eq, Debug)] #[cfg_attr(feature = "defmt", derive(defmt::Format))] @@ -8,10 +8,8 @@ pub struct InvalidCounterResetVal(pub(crate) ()); /// /// Returns [InvalidCounterResetVal] if the scrub rate is 0 /// (equivalent to disabling) or larger than 24 bits -pub fn enable_rom_scrubbing( - syscfg: &mut pac::Sysconfig, - scrub_rate: u32, -) -> Result<(), InvalidCounterResetVal> { +pub fn enable_rom_scrubbing(scrub_rate: u32) -> Result<(), InvalidCounterResetVal> { + let syscfg = unsafe { va108xx::Sysconfig::steal() }; if scrub_rate == 0 || scrub_rate > u32::pow(2, 24) { return Err(InvalidCounterResetVal(())); } @@ -19,7 +17,8 @@ pub fn enable_rom_scrubbing( Ok(()) } -pub fn disable_rom_scrubbing(syscfg: &mut pac::Sysconfig) { +pub fn disable_rom_scrubbing() { + let syscfg = unsafe { va108xx::Sysconfig::steal() }; syscfg.rom_scrub().write(|w| unsafe { w.bits(0) }); } @@ -27,10 +26,8 @@ pub fn disable_rom_scrubbing(syscfg: &mut pac::Sysconfig) { /// /// Returns [InvalidCounterResetVal] if the scrub rate is 0 /// (equivalent to disabling) or larger than 24 bits -pub fn enable_ram_scrubbing( - syscfg: &mut pac::Sysconfig, - scrub_rate: u32, -) -> Result<(), InvalidCounterResetVal> { +pub fn enable_ram_scrubbing(scrub_rate: u32) -> Result<(), InvalidCounterResetVal> { + let syscfg = unsafe { va108xx::Sysconfig::steal() }; if scrub_rate == 0 || scrub_rate > u32::pow(2, 24) { return Err(InvalidCounterResetVal(())); } @@ -38,20 +35,29 @@ pub fn enable_ram_scrubbing( Ok(()) } -pub fn disable_ram_scrubbing(syscfg: &mut pac::Sysconfig) { +pub fn disable_ram_scrubbing() { + let syscfg = unsafe { va108xx::Sysconfig::steal() }; syscfg.ram_scrub().write(|w| unsafe { w.bits(0) }); } -/// Clear the reset bit. This register is active low, so doing this will hold the peripheral -/// in a reset state -pub fn clear_reset_bit(syscfg: &mut pac::Sysconfig, periph_sel: PeripheralSelect) { +#[inline] +pub fn assert_peripheral_reset(periph_sel: PeripheralSelect) { + let syscfg = unsafe { va108xx::Sysconfig::steal() }; syscfg .peripheral_reset() .modify(|r, w| unsafe { w.bits(r.bits() & !(1 << periph_sel as u8)) }); } -pub fn set_reset_bit(syscfg: &mut pac::Sysconfig, periph_sel: PeripheralSelect) { +#[inline] +pub fn deassert_peripheral_reset(periph_sel: PeripheralSelect) { + let syscfg = unsafe { va108xx::Sysconfig::steal() }; syscfg .peripheral_reset() .modify(|r, w| unsafe { w.bits(r.bits() | (1 << periph_sel as u8)) }); } + +pub fn reset_peripheral_for_cycles(periph_sel: PeripheralSelect, cycles: u32) { + assert_peripheral_reset(periph_sel); + cortex_m::asm::delay(cycles); + deassert_peripheral_reset(periph_sel); +} diff --git a/va108xx-hal/src/timer.rs b/va108xx-hal/src/timer.rs index d356671..2a43b07 100644 --- a/va108xx-hal/src/timer.rs +++ b/va108xx-hal/src/timer.rs @@ -8,20 +8,23 @@ pub use crate::InterruptConfig; use crate::{ clock::{enable_peripheral_clock, PeripheralClocks}, enable_nvic_interrupt, - gpio::{ - AltFunc1, AltFunc2, AltFunc3, DynPinId, Pin, PinId, PA0, PA1, PA10, PA11, PA12, PA13, PA14, - PA15, PA2, PA24, PA25, PA26, PA27, PA28, PA29, PA3, PA30, PA31, PA4, PA5, PA6, PA7, PA8, - PA9, PB0, PB1, PB10, PB11, PB12, PB13, PB14, PB15, PB16, PB17, PB18, PB19, PB2, PB20, PB21, - PB22, PB23, PB3, PB4, PB5, PB6, - }, pac::{self, tim0}, + pins::{ + Pa0, Pa1, Pa10, Pa11, Pa12, Pa13, Pa14, Pa15, Pa2, Pa24, Pa25, Pa26, Pa27, Pa28, Pa29, Pa3, + Pa30, Pa31, Pa4, Pa5, Pa6, Pa7, Pa8, Pa9, Pb0, Pb1, Pb10, Pb11, Pb12, Pb13, Pb14, Pb15, + Pb16, Pb17, Pb18, Pb19, Pb2, Pb20, Pb21, Pb22, Pb23, Pb3, Pb4, Pb5, Pb6, + }, + sealed::Sealed, time::Hertz, - timer, - typelevel::Sealed, }; use core::cell::Cell; use critical_section::Mutex; use fugit::RateExtU32; +use vorago_shared_periphs::{ + gpio::{Pin, PinId}, + ioconfig::regs::FunSel, + Port, +}; const IRQ_DST_NONE: u32 = 0xffffffff; pub static MS_COUNTER: Mutex> = Mutex::new(Cell::new(0)); @@ -163,18 +166,21 @@ impl CascadeSource { //================================================================================================== pub trait TimPin { - const DYN: DynPinId; + const PORT: Port; + const OFFSET: usize; + const FUN_SEL: FunSel; + const TIM: Tim; } -pub trait ValidTim { +pub trait TimPeripheralMarker { // TIM ID ranging from 0 to 23 for 24 TIM peripherals - const TIM_ID: u8; + const TIM: Tim; } macro_rules! tim_marker { ($TIMX:path, $ID:expr) => { - impl ValidTim for $TIMX { - const TIM_ID: u8 = $ID; + impl TimPeripheralMarker for $TIMX { + const TIM: Tim = Tim($ID); } }; } @@ -204,8 +210,9 @@ tim_marker!(pac::Tim21, 21); tim_marker!(pac::Tim22, 22); tim_marker!(pac::Tim23, 23); -pub trait ValidTimAndPin: Sealed {} +pub trait ValidTimAndPin: Sealed {} +/* macro_rules! pin_and_tim { ($PAX:ident, $ALTFUNC:ident, $ID:expr, $TIMX:path) => { impl TimPin for Pin<$PAX, $ALTFUNC> @@ -274,6 +281,81 @@ pin_and_tim!(PB3, AltFunc3, 3, pac::Tim3); pin_and_tim!(PB2, AltFunc3, 2, pac::Tim2); pin_and_tim!(PB1, AltFunc3, 1, pac::Tim1); pin_and_tim!(PB0, AltFunc3, 0, pac::Tim0); +*/ + +macro_rules! pin_and_tim { + ($Px:ident, $FunSel:path, $ID:expr) => { + impl TimPin for Pin<$Px> + where + $Px: PinId, + { + const PORT: Port = $Px::PORT; + const OFFSET: usize = $Px::OFFSET; + const FUN_SEL: FunSel = $FunSel; + const TIM: Tim = Tim($ID); + } + + /* + impl ValidTimAndPin for (Pin<$Px>, $TIMX) + where + Pin<$PAX, $ALTFUNC>: TimPin, + $PAX: PinId, + { + } + + impl Sealed for (Pin<$PAX, $ALTFUNC>, $TIMX) {} + */ + }; +} + +pin_and_tim!(Pa0, FunSel::Sel1, 0); +pin_and_tim!(Pa1, FunSel::Sel1, 1); +pin_and_tim!(Pa2, FunSel::Sel1, 2); +pin_and_tim!(Pa3, FunSel::Sel1, 3); +pin_and_tim!(Pa4, FunSel::Sel1, 4); +pin_and_tim!(Pa5, FunSel::Sel1, 5); +pin_and_tim!(Pa6, FunSel::Sel1, 6); +pin_and_tim!(Pa7, FunSel::Sel1, 7); +pin_and_tim!(Pa8, FunSel::Sel1, 8); +pin_and_tim!(Pa9, FunSel::Sel1, 9); +pin_and_tim!(Pa10, FunSel::Sel1, 10); +pin_and_tim!(Pa11, FunSel::Sel1, 11); +pin_and_tim!(Pa12, FunSel::Sel1, 12); +pin_and_tim!(Pa13, FunSel::Sel1, 13); +pin_and_tim!(Pa14, FunSel::Sel1, 14); +pin_and_tim!(Pa15, FunSel::Sel1, 15); + +pin_and_tim!(Pa24, FunSel::Sel2, 16); +pin_and_tim!(Pa25, FunSel::Sel2, 17); +pin_and_tim!(Pa26, FunSel::Sel2, 18); +pin_and_tim!(Pa27, FunSel::Sel2, 19); +pin_and_tim!(Pa28, FunSel::Sel2, 20); +pin_and_tim!(Pa29, FunSel::Sel2, 21); +pin_and_tim!(Pa30, FunSel::Sel2, 22); +pin_and_tim!(Pa31, FunSel::Sel2, 23); + +pin_and_tim!(Pb0, FunSel::Sel3, 0); +pin_and_tim!(Pb1, FunSel::Sel3, 1); +pin_and_tim!(Pb2, FunSel::Sel3, 2); +pin_and_tim!(Pb3, FunSel::Sel3, 3); +pin_and_tim!(Pb4, FunSel::Sel3, 4); +pin_and_tim!(Pb5, FunSel::Sel3, 5); +pin_and_tim!(Pb6, FunSel::Sel3, 6); + +pin_and_tim!(Pb10, FunSel::Sel3, 10); +pin_and_tim!(Pb11, FunSel::Sel3, 11); +pin_and_tim!(Pb12, FunSel::Sel3, 12); +pin_and_tim!(Pb13, FunSel::Sel3, 13); +pin_and_tim!(Pb14, FunSel::Sel3, 14); +pin_and_tim!(Pb15, FunSel::Sel3, 15); +pin_and_tim!(Pb16, FunSel::Sel3, 16); +pin_and_tim!(Pb17, FunSel::Sel3, 17); +pin_and_tim!(Pb18, FunSel::Sel3, 18); +pin_and_tim!(Pb19, FunSel::Sel3, 19); +pin_and_tim!(Pb20, FunSel::Sel3, 20); +pin_and_tim!(Pb21, FunSel::Sel3, 21); +pin_and_tim!(Pb22, FunSel::Sel3, 22); +pin_and_tim!(Pb23, FunSel::Sel3, 23); //================================================================================================== // Register Interface for TIM registers and TIM pins @@ -337,16 +419,23 @@ pub unsafe trait TimRegInterface { } } -unsafe impl TimRegInterface for Tim { +#[derive(Debug)] +pub struct Tim(u8); + +impl Tim { + pub const fn raw_id(&self) -> u8 { + self.0 + } +} + +unsafe impl TimRegInterface for Tim { fn tim_id(&self) -> u8 { - Tim::TIM_ID + self.0 } } pub(crate) struct TimDynRegister { pub(crate) tim_id: u8, - #[allow(dead_code)] - pub(crate) pin_id: DynPinId, } unsafe impl TimRegInterface for TimDynRegister { @@ -361,44 +450,28 @@ unsafe impl TimRegInterface for TimDynRegister { //================================================================================================== /// Hardware timers -pub struct CountdownTimer { +pub struct CountdownTimer { tim: Tim, curr_freq: Hertz, - irq_cfg: Option, sys_clk: Hertz, rst_val: u32, last_cnt: u32, listening: bool, } -#[inline(always)] -pub fn enable_tim_clk(syscfg: &mut pac::Sysconfig, idx: u8) { - syscfg - .tim_clk_enable() - .modify(|r, w| unsafe { w.bits(r.bits() | (1 << idx)) }); -} - -#[inline(always)] -pub fn disable_tim_clk(syscfg: &mut pac::Sysconfig, idx: u8) { - syscfg - .tim_clk_enable() - .modify(|r, w| unsafe { w.bits(r.bits() & !(1 << idx)) }); -} - -unsafe impl TimRegInterface for CountdownTimer { +unsafe impl TimRegInterface for CountdownTimer { fn tim_id(&self) -> u8 { - TIM::TIM_ID + self.tim.0 } } -impl CountdownTimer { +impl CountdownTimer { /// Configures a TIM peripheral as a periodic count down timer - pub fn new(syscfg: &mut pac::Sysconfig, sys_clk: impl Into, tim: Tim) -> Self { - enable_tim_clk(syscfg, Tim::TIM_ID); + pub fn new(sys_clk: impl Into, tim_id: u8) -> Self { + enable_tim_clk(tim_id); let cd_timer = CountdownTimer { - tim, + tim: Tim(tim_id), sys_clk: sys_clk.into(), - irq_cfg: None, rst_val: 0, curr_freq: 0.Hz(), listening: false, @@ -414,44 +487,36 @@ impl CountdownTimer { /// Listen for events. Depending on the IRQ configuration, this also activates the IRQ in the /// IRQSEL peripheral for the provided interrupt and unmasks the interrupt - pub fn listen( - &mut self, - event: Event, - irq_cfg: InterruptConfig, - irq_sel: Option<&mut pac::Irqsel>, - sys_cfg: Option<&mut pac::Sysconfig>, - ) { + pub fn listen(&mut self, event: Event, irq_cfg: InterruptConfig) { + enable_peripheral_clock(PeripheralClocks::Irqsel); + if irq_cfg.route { + let irqsel = unsafe { pac::Irqsel::steal() }; + irqsel + .tim0(self.tim_id() as usize) + .write(|w| unsafe { w.bits(irq_cfg.id as u32) }); + } + if irq_cfg.enable_in_nvic { + unsafe { enable_nvic_interrupt(irq_cfg.id) }; + } match event { Event::TimeOut => { cortex_m::peripheral::NVIC::mask(irq_cfg.id); self.irq_cfg = Some(irq_cfg); - if irq_cfg.route { - if let Some(sys_cfg) = sys_cfg { - enable_peripheral_clock(sys_cfg, PeripheralClocks::Irqsel); - } - if let Some(irq_sel) = irq_sel { - irq_sel - .tim0(Tim::TIM_ID as usize) - .write(|w| unsafe { w.bits(irq_cfg.id as u32) }); - } - } self.listening = true; } } } - pub fn unlisten( - &mut self, - event: Event, - syscfg: &mut pac::Sysconfig, - irqsel: &mut pac::Irqsel, - ) { + pub fn unlisten(&mut self, event: Event, unroute_irq: bool) { match event { Event::TimeOut => { - enable_peripheral_clock(syscfg, PeripheralClocks::Irqsel); - irqsel - .tim0(Tim::TIM_ID as usize) - .write(|w| unsafe { w.bits(IRQ_DST_NONE) }); + enable_peripheral_clock(PeripheralClocks::Irqsel); + if unroute_irq { + let irqsel = unsafe { pac::Irqsel::steal() }; + irqsel + .tim0(self.tim_id() as usize) + .write(|w| unsafe { w.bits(IRQ_DST_NONE) }); + } self.disable_interrupt(); self.listening = false; } @@ -481,7 +546,7 @@ impl CountdownTimer { .write(|w| w.enable().clear_bit()); syscfg .tim_clk_enable() - .modify(|r, w| unsafe { w.bits(r.bits() & !(1 << Tim::TIM_ID)) }); + .modify(|r, w| unsafe { w.bits(r.bits() & !(1 << self.tim_id())) }); self.tim } @@ -628,7 +693,7 @@ impl CountdownTimer { } /// CountDown implementation for TIMx -impl CountdownTimer { +impl CountdownTimer { #[inline] pub fn start(&mut self, timeout: T) where @@ -664,7 +729,7 @@ impl CountdownTimer { } } -impl embedded_hal::delay::DelayNs for CountdownTimer { +impl embedded_hal::delay::DelayNs for CountdownTimer { fn delay_ns(&mut self, ns: u32) { let ticks = (u64::from(ns)) * (u64::from(self.sys_clk.raw())) / 1_000_000_000; @@ -718,27 +783,11 @@ impl embedded_hal::delay::DelayNs for CountdownTimer { } } -// Set up a millisecond timer on TIM0. Please note that the user still has to provide an IRQ handler -// which should call [default_ms_irq_handler]. -pub fn set_up_ms_tick( - irq_cfg: InterruptConfig, - sys_cfg: &mut pac::Sysconfig, - irq_sel: Option<&mut pac::Irqsel>, +pub fn set_up_ms_delay_provider( sys_clk: impl Into, - tim0: TIM, -) -> CountdownTimer { - let mut ms_timer = CountdownTimer::new(sys_cfg, sys_clk, tim0); - ms_timer.listen(timer::Event::TimeOut, irq_cfg, irq_sel, Some(sys_cfg)); - ms_timer.start(1000.Hz()); - ms_timer -} - -pub fn set_up_ms_delay_provider( - sys_cfg: &mut pac::Sysconfig, - sys_clk: impl Into, - tim: TIM, -) -> CountdownTimer { - let mut provider = CountdownTimer::new(sys_cfg, sys_clk, tim); + tim: impl TimPeripheralMarker, +) -> CountdownTimer { + let mut provider = CountdownTimer::new(sys_clk, tim); provider.start(1000.Hz()); provider } @@ -762,6 +811,7 @@ pub fn get_ms_ticks() -> u32 { // Delay implementations //================================================================================================== +/* pub struct DelayMs(CountdownTimer); impl DelayMs { @@ -787,3 +837,20 @@ impl embedded_hal::delay::DelayNs for DelayMs { } } } +*/ + +#[inline(always)] +pub fn enable_tim_clk(idx: u8) { + let syscfg = unsafe { va108xx::Sysconfig::steal() }; + syscfg + .tim_clk_enable() + .modify(|r, w| unsafe { w.bits(r.bits() | (1 << idx)) }); +} + +#[inline(always)] +pub fn disable_tim_clk(idx: u8) { + let syscfg = unsafe { va108xx::Sysconfig::steal() }; + syscfg + .tim_clk_enable() + .modify(|r, w| unsafe { w.bits(r.bits() & !(1 << idx)) }); +} diff --git a/va108xx-hal/src/typelevel.rs b/va108xx-hal/src/typelevel.rs deleted file mode 100644 index 7803c20..0000000 --- a/va108xx-hal/src/typelevel.rs +++ /dev/null @@ -1,155 +0,0 @@ -//! Module supporting type-level programming -//! -//! This module is identical to the -//! [atsamd typelevel](https://docs.rs/atsamd-hal/latest/atsamd_hal/typelevel/index.html). - -use core::ops::{Add, Sub}; - -use typenum::{Add1, Bit, Sub1, UInt, Unsigned, B1, U0}; - -mod private { - /// Super trait used to mark traits with an exhaustive set of - /// implementations - pub trait Sealed {} - - impl Sealed for u8 {} - impl Sealed for i8 {} - impl Sealed for u16 {} - impl Sealed for i16 {} - impl Sealed for u32 {} - impl Sealed for i32 {} - impl Sealed for f32 {} - - /// Mapping from an instance of a countable type to its successor - pub trait Increment { - /// Successor type of `Self` - type Inc; - /// Consume an instance of `Self` and return its successor - fn inc(self) -> Self::Inc; - } - - /// Mapping from an instance of a countable type to its predecessor - pub trait Decrement { - /// Predecessor type of `Self` - type Dec; - /// Consume an instance of `Self` and return its predecessor - fn dec(self) -> Self::Dec; - } -} - -pub(crate) use private::Decrement as PrivateDecrement; -pub(crate) use private::Increment as PrivateIncrement; -pub(crate) use private::Sealed; - -/// Type-level version of the [`None`] variant -#[derive(Default)] -pub struct NoneT; - -impl Sealed for NoneT {} - -//============================================================================== -// Is -//============================================================================== - -/// Marker trait for type identity -/// -/// This trait is used as part of the [`AnyKind`] trait pattern. It represents -/// the concept of type identity, because all implementors have -/// `::Type == Self`. When used as a trait bound with a specific -/// type, it guarantees that the corresponding type parameter is exactly the -/// specific type. Stated differently, it guarantees that `T == Specific` in -/// the following example. -/// -/// ```ignore -/// where T: Is -/// ``` -/// -/// Moreover, the super traits guarantee that any instance of or reference to a -/// type `T` can be converted into the `Specific` type. -/// -/// ```ignore -/// fn example(mut any: T) -/// where -/// T: Is, -/// { -/// let specific_mut: &mut Specific = any.as_mut(); -/// let specific_ref: &Specific = any.as_ref(); -/// let specific: Specific = any.into(); -/// } -/// ``` -/// -/// [`AnyKind`]: #anykind-trait-pattern -pub trait Is -where - Self: Sealed, - Self: From>, - Self: Into>, - Self: AsRef>, - Self: AsMut>, -{ - type Type; -} - -/// Type alias for [`Is::Type`] -pub type IsType = ::Type; - -impl Is for T -where - T: Sealed + AsRef + AsMut, -{ - type Type = T; -} - -//============================================================================== -// Counting -//============================================================================== - -/// Implement `Sealed` for [`U0`] -impl Sealed for U0 {} - -/// Implement `Sealed` for all type-level, [`Unsigned`] integers *except* [`U0`] -impl Sealed for UInt {} - -/// Trait mapping each countable type to its successor -/// -/// This trait maps each countable type to its corresponding successor type. The -/// actual implementation of this trait is contained within `PrivateIncrement`. -/// Access to `PrivateIncrement` is restricted, so that safe HAL APIs can be -/// built with it. -pub trait Increment: PrivateIncrement {} - -impl Increment for T {} - -/// Trait mapping each countable type to its predecessor -/// -/// This trait maps each countable type to its corresponding predecessor type. -/// The actual implementation of this trait is contained within -/// `PrivateDecrement`. Access to `PrivateDecrement` is restricted, so that safe -/// HAL APIs can be built with it. -pub trait Decrement: PrivateDecrement {} - -impl Decrement for T {} - -impl PrivateIncrement for N -where - N: Unsigned + Add, - Add1: Unsigned, -{ - type Inc = Add1; - #[inline] - fn inc(self) -> Self::Inc { - Self::Inc::default() - } -} - -impl PrivateDecrement for N -where - N: Unsigned + Sub, - Sub1: Unsigned, -{ - type Dec = Sub1; - #[inline] - fn dec(self) -> Self::Dec { - Self::Dec::default() - } -} diff --git a/va108xx-hal/src/uart/mod.rs b/va108xx-hal/src/uart/mod.rs index 929b6c2..f070994 100644 --- a/va108xx-hal/src/uart/mod.rs +++ b/va108xx-hal/src/uart/mod.rs @@ -14,16 +14,17 @@ //! - [Flashloader exposing a CCSDS interface via UART](https://egit.irs.uni-stuttgart.de/rust/va108xx-rs/src/branch/main/flashloader) use core::{convert::Infallible, ops::Deref}; use fugit::RateExtU32; +use vorago_shared_periphs::{gpio::Pin, FunSel}; pub use crate::InterruptConfig; use crate::{ clock::enable_peripheral_clock, enable_nvic_interrupt, - gpio::pin::{ - AltFunc1, AltFunc2, AltFunc3, Pin, PA16, PA17, PA18, PA19, PA2, PA26, PA27, PA3, PA30, - PA31, PA8, PA9, PB18, PB19, PB20, PB21, PB22, PB23, PB6, PB7, PB8, PB9, - }, pac::{self, uarta as uart_base}, + pins::{ + Pa16, Pa17, Pa18, Pa19, Pa2, Pa26, Pa27, Pa3, Pa30, Pa31, Pa8, Pa9, Pb18, Pb19, Pb20, Pb21, + Pb22, Pb23, Pb6, Pb7, Pb8, Pb9, + }, time::Hertz, PeripheralSelect, }; @@ -31,31 +32,136 @@ use embedded_hal_nb::serial::Read; #[derive(Debug, Clone, Copy)] #[cfg_attr(feature = "defmt", derive(defmt::Format))] -pub enum Bank { +pub enum UartId { A = 0, B = 1, } +impl UartId { + // TODO: Safety docs. + pub const unsafe fn reg_block(&self) -> &'static uart_base::RegisterBlock { + match self { + UartId::A => unsafe { &*pac::Uarta::PTR }, + UartId::B => unsafe { &*pac::Uartb::PTR }, + } + } +} + //================================================================================================== // Type-Level support //================================================================================================== -pub trait Pins {} +pub trait TxPin { + const UART_ID: UartId; + const FUN_SEL: FunSel; +} +pub trait RxPin { + const UART_ID: UartId; + const FUN_SEL: FunSel; +} -impl Pins for (Pin, Pin) {} -impl Pins for (Pin, Pin) {} -impl Pins for (Pin, Pin) {} +// UART A pins -impl Pins for (Pin, Pin) {} -impl Pins for (Pin, Pin) {} +impl TxPin for Pin { + const UART_ID: UartId = UartId::A; + const FUN_SEL: FunSel = FunSel::Sel2; +} +impl RxPin for Pin { + const UART_ID: UartId = UartId::A; + const FUN_SEL: FunSel = FunSel::Sel2; +} -impl Pins for (Pin, Pin) {} -impl Pins for (Pin, Pin) {} -impl Pins for (Pin, Pin) {} +impl TxPin for Pin { + const UART_ID: UartId = UartId::A; + const FUN_SEL: FunSel = FunSel::Sel3; +} +impl RxPin for Pin { + const UART_ID: UartId = UartId::A; + const FUN_SEL: FunSel = FunSel::Sel3; +} -impl Pins for (Pin, Pin) {} -impl Pins for (Pin, Pin) {} -impl Pins for (Pin, Pin) {} +impl TxPin for Pin { + const UART_ID: UartId = UartId::A; + const FUN_SEL: FunSel = FunSel::Sel3; +} +impl RxPin for Pin { + const UART_ID: UartId = UartId::A; + const FUN_SEL: FunSel = FunSel::Sel3; +} + +impl TxPin for Pin { + const UART_ID: UartId = UartId::A; + const FUN_SEL: FunSel = FunSel::Sel1; +} +impl RxPin for Pin { + const UART_ID: UartId = UartId::A; + const FUN_SEL: FunSel = FunSel::Sel1; +} + +impl TxPin for Pin { + const UART_ID: UartId = UartId::A; + const FUN_SEL: FunSel = FunSel::Sel1; +} +impl RxPin for Pin { + const UART_ID: UartId = UartId::A; + const FUN_SEL: FunSel = FunSel::Sel1; +} + +// UART B pins + +impl TxPin for Pin { + const UART_ID: UartId = UartId::B; + const FUN_SEL: FunSel = FunSel::Sel2; +} +impl RxPin for Pin { + const UART_ID: UartId = UartId::B; + const FUN_SEL: FunSel = FunSel::Sel2; +} + +impl TxPin for Pin { + const UART_ID: UartId = UartId::B; + const FUN_SEL: FunSel = FunSel::Sel3; +} +impl RxPin for Pin { + const UART_ID: UartId = UartId::B; + const FUN_SEL: FunSel = FunSel::Sel3; +} + +impl TxPin for Pin { + const UART_ID: UartId = UartId::B; + const FUN_SEL: FunSel = FunSel::Sel3; +} +impl RxPin for Pin { + const UART_ID: UartId = UartId::B; + const FUN_SEL: FunSel = FunSel::Sel3; +} + +impl TxPin for Pin { + const UART_ID: UartId = UartId::B; + const FUN_SEL: FunSel = FunSel::Sel1; +} +impl RxPin for Pin { + const UART_ID: UartId = UartId::B; + const FUN_SEL: FunSel = FunSel::Sel1; +} + +impl TxPin for Pin { + const UART_ID: UartId = UartId::B; + const FUN_SEL: FunSel = FunSel::Sel2; +} +impl RxPin for Pin { + const UART_ID: UartId = UartId::B; + const FUN_SEL: FunSel = FunSel::Sel2; +} + +impl TxPin for Pin { + const UART_ID: UartId = UartId::B; + const FUN_SEL: FunSel = FunSel::Sel1; +} +impl RxPin for Pin { + const UART_ID: UartId = UartId::B; + const FUN_SEL: FunSel = FunSel::Sel1; +} //================================================================================================== // Regular Definitions @@ -336,8 +442,8 @@ pub struct BufferTooShortError { // UART peripheral wrapper //================================================================================================== -pub trait Instance: Deref { - const IDX: u8; +pub trait UartPeripheralMarker: Deref { + const ID: UartId; const PERIPH_SEL: PeripheralSelect; const PTR: *const uart_base::RegisterBlock; @@ -364,8 +470,8 @@ pub trait Instance: Deref { } } -impl Instance for pac::Uarta { - const IDX: u8 = 0; +impl UartPeripheralMarker for pac::Uarta { + const ID: UartId = UartId::A; const PERIPH_SEL: PeripheralSelect = PeripheralSelect::Uart0; const PTR: *const uart_base::RegisterBlock = Self::PTR; @@ -376,8 +482,8 @@ impl Instance for pac::Uarta { } } -impl Instance for pac::Uartb { - const IDX: u8 = 1; +impl UartPeripheralMarker for pac::Uartb { + const ID: UartId = UartId::B; const PERIPH_SEL: PeripheralSelect = PeripheralSelect::Uart1; const PTR: *const uart_base::RegisterBlock = Self::PTR; @@ -388,35 +494,60 @@ impl Instance for pac::Uartb { } } -impl Bank { - /// Retrieve the peripheral register block. - /// - /// # Safety - /// - /// Circumvents the HAL safety guarantees. - pub unsafe fn reg_block(&self) -> &'static uart_base::RegisterBlock { - match self { - Bank::A => unsafe { pac::Uarta::reg_block() }, - Bank::B => unsafe { pac::Uartb::reg_block() }, - } - } -} - //================================================================================================== // UART implementation //================================================================================================== /// Type erased variant of a UART. Can be created with the [`Uart::downgrade`] function. -pub struct UartBase { - uart: Uart, - tx: Tx, - rx: Rx, +pub struct Uart { + tx: Tx, + rx: Rx, } -impl UartBase { - /// This function assumes that the peripheral clock was alredy enabled - /// in the SYSCONFIG register - fn init(self, config: Config, sys_clk: Hertz) -> Self { +impl Uart { + /// Calls [Self::new] with the interrupt configuration to some valid value. + pub fn new_with_interrupt( + sys_clk: Hertz, + uart: UartI, + pins: (Tx, Rx), + config: Config, + irq_cfg: InterruptConfig, + ) -> Self { + Self::new(sys_clk, uart, pins, config, Some(irq_cfg)) + } + + /// Calls [Self::new] with the interrupt configuration to [None]. + pub fn new_without_interrupt( + sys_clk: Hertz, + uart: UartI, + pins: (Tx, Rx), + config: Config, + ) -> Self { + Self::new(sys_clk, uart, pins, config, None) + } + + /// Create a new UART peripheral with an interrupt configuration. + /// + /// # Arguments + /// + /// - `syscfg`: The system configuration register block + /// - `sys_clk`: The system clock frequency + /// - `uart`: The concrete UART peripheral instance. + /// - `pins`: UART TX and RX pin tuple. + /// - `config`: UART specific configuration parameters like baudrate. + /// - `irq_cfg`: Optional interrupt configuration. This should be a valid value if the plan + /// is to use TX or RX functionality relying on interrupts. If only the blocking API without + /// any interrupt support is used, this can be [None]. + pub fn new( + sys_clk: Hertz, + uart: UartI, + pins: (TxPinI, RxPinI), + config: Config, + opt_irq_cfg: Option, + ) -> Self { + crate::clock::enable_peripheral_clock(UartI::PERIPH_SEL); + + let reg_block = unsafe { UartI::reg_block() }; let baud_multiplier = match config.baud8 { false => 16, true => 8, @@ -430,7 +561,7 @@ impl UartBase { // Calculations here are derived from chapter 4.8.5 (p.79) of the datasheet. let x = sys_clk.raw() as f32 / (config.baudrate.raw() * baud_multiplier) as f32; let integer_part = x as u32; - self.uart.clkscale().write(|w| unsafe { + reg_block.clkscale().write(|w| unsafe { w.frac().bits(frac as u8); w.int().bits(integer_part) }); @@ -446,7 +577,7 @@ impl UartBase { }; let wordsize = config.wordsize as u8; let baud8 = config.baud8; - self.uart.ctrl().write(|w| { + reg_block.ctrl().write(|w| { w.paren().bit(paren); w.pareven().bit(pareven); w.stopbits().bit(stopbits); @@ -455,59 +586,101 @@ impl UartBase { }); let (txenb, rxenb) = (config.enable_tx, config.enable_rx); // Clear the FIFO - self.uart.fifo_clr().write(|w| { + reg_block.fifo_clr().write(|w| { w.rxfifo().set_bit(); w.txfifo().set_bit() }); - self.uart.enable().write(|w| { + reg_block.enable().write(|w| { w.rxenable().bit(rxenb); w.txenable().bit(txenb) }); - self + + if let Some(irq_cfg) = opt_irq_cfg { + if irq_cfg.route { + enable_peripheral_clock(PeripheralSelect::Irqsel); + unsafe { pac::Irqsel::steal() } + .uart0(UartI::ID as usize) + .write(|w| unsafe { w.bits(irq_cfg.id as u32) }); + } + if irq_cfg.enable_in_nvic { + // Safety: User has specifically configured this. + unsafe { enable_nvic_interrupt(irq_cfg.id) }; + } + } + + // TODO: Validity checks. + Uart { + tx: Tx::new(UartI::ID), + rx: Rx::new(UartI::ID), + } } #[inline] pub fn enable_rx(&mut self) { - self.uart.enable().modify(|_, w| w.rxenable().set_bit()); + self.tx + .regs_priv() + .enable() + .modify(|_, w| w.rxenable().set_bit()); } #[inline] pub fn disable_rx(&mut self) { - self.uart.enable().modify(|_, w| w.rxenable().clear_bit()); + self.tx + .regs_priv() + .enable() + .modify(|_, w| w.rxenable().clear_bit()); } #[inline] pub fn enable_tx(&mut self) { - self.uart.enable().modify(|_, w| w.txenable().set_bit()); + self.tx + .regs_priv() + .enable() + .modify(|_, w| w.txenable().set_bit()); } #[inline] pub fn disable_tx(&mut self) { - self.uart.enable().modify(|_, w| w.txenable().clear_bit()); + self.tx + .regs_priv() + .enable() + .modify(|_, w| w.txenable().clear_bit()); } #[inline] pub fn clear_rx_fifo(&mut self) { - self.uart.fifo_clr().write(|w| w.rxfifo().set_bit()); + self.tx + .regs_priv() + .fifo_clr() + .write(|w| w.rxfifo().set_bit()); } #[inline] pub fn clear_tx_fifo(&mut self) { - self.uart.fifo_clr().write(|w| w.txfifo().set_bit()); + self.tx + .regs_priv() + .fifo_clr() + .write(|w| w.txfifo().set_bit()); } #[inline] pub fn clear_rx_status(&mut self) { - self.uart.fifo_clr().write(|w| w.rxsts().set_bit()); + self.tx + .regs_priv() + .fifo_clr() + .write(|w| w.rxsts().set_bit()); } #[inline] pub fn clear_tx_status(&mut self) { - self.uart.fifo_clr().write(|w| w.txsts().set_bit()); + self.tx + .regs_priv() + .fifo_clr() + .write(|w| w.txsts().set_bit()); } pub fn listen(&self, event: Event) { - self.uart.irq_enb().modify(|_, w| match event { + self.tx.regs_priv().irq_enb().modify(|_, w| match event { Event::RxError => w.irq_rx_status().set_bit(), Event::RxFifoHalfFull => w.irq_rx().set_bit(), Event::RxTimeout => w.irq_rx_to().set_bit(), @@ -519,7 +692,7 @@ impl UartBase { } pub fn unlisten(&self, event: Event) { - self.uart.irq_enb().modify(|_, w| match event { + self.tx.regs_priv().irq_enb().modify(|_, w| match event { Event::RxError => w.irq_rx_status().clear_bit(), Event::RxFifoHalfFull => w.irq_rx().clear_bit(), Event::RxTimeout => w.irq_rx_to().clear_bit(), @@ -530,44 +703,31 @@ impl UartBase { }); } - pub fn release(self) -> Uart { - // Clear the FIFO - self.uart.fifo_clr().write(|w| { - w.rxfifo().set_bit(); - w.txfifo().set_bit() - }); - self.uart.enable().write(|w| { - w.rxenable().clear_bit(); - w.txenable().clear_bit() - }); - self.uart - } - /// Poll receiver errors. pub fn poll_rx_errors(&self) -> Option { self.rx.poll_errors() } - pub fn split(self) -> (Tx, Rx) { + pub fn split(self) -> (Tx, Rx) { (self.tx, self.rx) } } -impl embedded_io::ErrorType for UartBase { +impl embedded_io::ErrorType for Uart { type Error = Infallible; } -impl embedded_hal_nb::serial::ErrorType for UartBase { +impl embedded_hal_nb::serial::ErrorType for Uart { type Error = Infallible; } -impl embedded_hal_nb::serial::Read for UartBase { +impl embedded_hal_nb::serial::Read for Uart { fn read(&mut self) -> nb::Result { self.rx.read() } } -impl embedded_hal_nb::serial::Write for UartBase { +impl embedded_hal_nb::serial::Write for Uart { fn write(&mut self, word: u8) -> nb::Result<(), Self::Error> { self.tx.write(word).map_err(|e| { if let nb::Error::Other(_) = e { @@ -587,163 +747,6 @@ impl embedded_hal_nb::serial::Write for UartBase { } } -/// Serial abstraction. Entry point to create a new UART -pub struct Uart { - inner: UartBase, - pins: Pins, -} - -impl Uart -where - UartInstance: Instance, - PinsInstance: Pins, -{ - /// Calls [Self::new] with the interrupt configuration to some valid value. - pub fn new_with_interrupt( - syscfg: &mut va108xx::Sysconfig, - sys_clk: impl Into, - uart: UartInstance, - pins: PinsInstance, - config: impl Into, - irq_cfg: InterruptConfig, - ) -> Self { - Self::new(syscfg, sys_clk, uart, pins, config, Some(irq_cfg)) - } - - /// Calls [Self::new] with the interrupt configuration to [None]. - pub fn new_without_interrupt( - syscfg: &mut va108xx::Sysconfig, - sys_clk: impl Into, - uart: UartInstance, - pins: PinsInstance, - config: impl Into, - ) -> Self { - Self::new(syscfg, sys_clk, uart, pins, config, None) - } - - /// Create a new UART peripheral with an interrupt configuration. - /// - /// # Arguments - /// - /// - `syscfg`: The system configuration register block - /// - `sys_clk`: The system clock frequency - /// - `uart`: The concrete UART peripheral instance. - /// - `pins`: UART TX and RX pin tuple. - /// - `config`: UART specific configuration parameters like baudrate. - /// - `irq_cfg`: Optional interrupt configuration. This should be a valid value if the plan - /// is to use TX or RX functionality relying on interrupts. If only the blocking API without - /// any interrupt support is used, this can be [None]. - pub fn new( - syscfg: &mut va108xx::Sysconfig, - sys_clk: impl Into, - uart: UartInstance, - pins: PinsInstance, - config: impl Into, - opt_irq_cfg: Option, - ) -> Self { - crate::clock::enable_peripheral_clock(syscfg, UartInstance::PERIPH_SEL); - let uart = Uart { - inner: UartBase { - uart, - tx: Tx::new(unsafe { UartInstance::steal() }), - rx: Rx::new(unsafe { UartInstance::steal() }), - }, - pins, - } - .init(config.into(), sys_clk.into()); - - if let Some(irq_cfg) = opt_irq_cfg { - if irq_cfg.route { - enable_peripheral_clock(syscfg, PeripheralSelect::Irqsel); - unsafe { pac::Irqsel::steal() } - .uart0(UartInstance::IDX as usize) - .write(|w| unsafe { w.bits(irq_cfg.id as u32) }); - } - if irq_cfg.enable_in_nvic { - // Safety: User has specifically configured this. - unsafe { enable_nvic_interrupt(irq_cfg.id) }; - } - } - uart - } - - /// This function assumes that the peripheral clock was alredy enabled - /// in the SYSCONFIG register - fn init(mut self, config: Config, sys_clk: Hertz) -> Self { - self.inner = self.inner.init(config, sys_clk); - self - } - - /// Poll receiver errors. - pub fn poll_rx_errors(&self) -> Option { - self.inner.poll_rx_errors() - } - - #[inline] - pub fn enable_rx(&mut self) { - self.inner.enable_rx(); - } - - #[inline] - pub fn disable_rx(&mut self) { - self.inner.enable_rx(); - } - - #[inline] - pub fn enable_tx(&mut self) { - self.inner.enable_tx(); - } - - #[inline] - pub fn disable_tx(&mut self) { - self.inner.disable_tx(); - } - - #[inline] - pub fn clear_rx_fifo(&mut self) { - self.inner.clear_rx_fifo(); - } - - #[inline] - pub fn clear_tx_fifo(&mut self) { - self.inner.clear_tx_fifo(); - } - - #[inline] - pub fn clear_rx_status(&mut self) { - self.inner.clear_rx_status(); - } - - #[inline] - pub fn clear_tx_status(&mut self) { - self.inner.clear_tx_status(); - } - - pub fn listen(&self, event: Event) { - self.inner.listen(event); - } - - pub fn unlisten(&self, event: Event) { - self.inner.unlisten(event); - } - - pub fn release(self) -> (UartInstance, PinsInstance) { - (self.inner.release(), self.pins) - } - - pub fn downgrade(self) -> UartBase { - UartBase { - uart: self.inner.uart, - tx: self.inner.tx, - rx: self.inner.rx, - } - } - - pub fn split(self) -> (Tx, Rx) { - self.inner.split() - } -} - #[inline(always)] pub fn enable_rx(uart: &uart_base::RegisterBlock) { uart.enable().modify(|_, w| w.rxenable().set_bit()); @@ -775,12 +778,12 @@ pub fn disable_rx_interrupts(uart: &uart_base::RegisterBlock) { /// Serial receiver. /// /// Can be created by using the [Uart::split] or [UartBase::split] API. -pub struct Rx(Uart); +pub struct Rx(UartId); -impl Rx { +impl Rx { #[inline(always)] - const fn new(uart: Uart) -> Self { - Self(uart) + const fn new(id: UartId) -> Self { + Self(id) } /// Direct access to the peripheral structure. @@ -789,15 +792,19 @@ impl Rx { /// /// You must ensure that only registers related to the operation of the RX side are used. #[inline(always)] - pub const unsafe fn uart(&self) -> &Uart { - &self.0 + pub const unsafe fn regs(&self) -> &'static uart_base::RegisterBlock { + self.regs_priv() + } + + #[inline(always)] + const fn regs_priv(&self) -> &'static uart_base::RegisterBlock { + unsafe { self.0.reg_block() } } pub fn poll_errors(&self) -> Option { let mut errors = UartErrors::default(); - let uart = unsafe { &(*Uart::ptr()) }; - let status_reader = uart.rxstatus().read(); + let status_reader = self.regs_priv().rxstatus().read(); if status_reader.rxovr().bit_is_set() { errors.overflow = true; } else if status_reader.rxfrm().bit_is_set() { @@ -812,26 +819,26 @@ impl Rx { #[inline] pub fn clear_fifo(&self) { - self.0.fifo_clr().write(|w| w.rxfifo().set_bit()); + self.regs_priv().fifo_clr().write(|w| w.rxfifo().set_bit()); } #[inline] pub fn disable_interrupts(&mut self) { - disable_rx_interrupts(unsafe { Uart::reg_block() }); + disable_rx_interrupts(self.regs_priv()); } #[inline] pub fn enable_interrupts(&mut self) { - enable_rx_interrupts(unsafe { Uart::reg_block() }); + enable_rx_interrupts(self.regs_priv()); } #[inline] pub fn enable(&mut self) { - enable_rx(unsafe { Uart::reg_block() }); + enable_rx(self.regs_priv()); } #[inline] pub fn disable(&mut self) { - disable_rx(unsafe { Uart::reg_block() }); + disable_rx(self.regs_priv()); } /// Low level function to read a word from the UART FIFO. @@ -841,8 +848,8 @@ impl Rx { /// Please note that you might have to mask the returned value with 0xff to retrieve the actual /// value if you use the manual parity mode. See chapter 4.6.2 for more information. #[inline(always)] - pub fn read_fifo(&self) -> nb::Result { - if self.0.rxstatus().read().rdavl().bit_is_clear() { + pub fn read_fifo(&mut self) -> nb::Result { + if self.regs_priv().rxstatus().read().rdavl().bit_is_clear() { return Err(nb::Error::WouldBlock); } Ok(self.read_fifo_unchecked()) @@ -857,29 +864,24 @@ impl Rx { /// Please note that you might have to mask the returned value with 0xff to retrieve the actual /// value if you use the manual parity mode. See chapter 4.6.2 for more information. #[inline(always)] - pub fn read_fifo_unchecked(&self) -> u32 { - self.0.data().read().bits() + pub fn read_fifo_unchecked(&mut self) -> u32 { + self.regs_priv().data().read().bits() } - pub fn into_rx_with_irq(self) -> RxWithInterrupt { + pub fn into_rx_with_irq(self) -> RxWithInterrupt { RxWithInterrupt::new(self) } - - #[inline(always)] - pub fn release(self) -> Uart { - self.0 - } } -impl embedded_io::ErrorType for Rx { +impl embedded_io::ErrorType for Rx { type Error = Infallible; } -impl embedded_hal_nb::serial::ErrorType for Rx { +impl embedded_hal_nb::serial::ErrorType for Rx { type Error = Infallible; } -impl embedded_hal_nb::serial::Read for Rx { +impl embedded_hal_nb::serial::Read for Rx { fn read(&mut self) -> nb::Result { self.read_fifo().map(|val| (val & 0xff) as u8).map_err(|e| { if let nb::Error::Other(_) = e { @@ -890,14 +892,14 @@ impl embedded_hal_nb::serial::Read for Rx { } } -impl embedded_io::Read for Rx { +impl embedded_io::Read for Rx { fn read(&mut self, buf: &mut [u8]) -> Result { if buf.is_empty() { return Ok(0); } let mut read = 0; loop { - if self.0.rxstatus().read().rdavl().bit_is_set() { + if self.regs_priv().rxstatus().read().rdavl().bit_is_set() { break; } } @@ -946,22 +948,22 @@ pub fn disable_tx_interrupts(uart: &uart_base::RegisterBlock) { /// Serial transmitter /// /// Can be created by using the [Uart::split] or [UartBase::split] API. -pub struct Tx(Uart); +pub struct Tx(UartId); -impl Tx { +impl Tx { /// Retrieve a TX pin without expecting an explicit UART structure /// /// # Safety /// /// Circumvents the HAL safety guarantees. #[inline(always)] - pub unsafe fn steal() -> Self { - Self(Uart::steal()) + pub unsafe fn steal(id: UartId) -> Self { + Self::new(id) } #[inline(always)] - fn new(uart: Uart) -> Self { - Self(uart) + fn new(id: UartId) -> Self { + Self(id) } /// Direct access to the peripheral structure. @@ -970,23 +972,32 @@ impl Tx { /// /// You must ensure that only registers related to the operation of the TX side are used. #[inline(always)] - pub const unsafe fn uart(&self) -> &Uart { - &self.0 + pub unsafe fn regs(&self) -> &'static uart_base::RegisterBlock { + &self.0.reg_block() + } + + #[inline(always)] + fn regs_priv(&self) -> &'static uart_base::RegisterBlock { + unsafe { self.regs() } } #[inline] - pub fn clear_fifo(&self) { - self.0.fifo_clr().write(|w| w.txfifo().set_bit()); + pub fn clear_fifo(&mut self) { + self.regs_priv().fifo_clr().write(|w| w.txfifo().set_bit()); } #[inline] pub fn enable(&mut self) { - self.0.enable().modify(|_, w| w.txenable().set_bit()); + self.regs_priv() + .enable() + .modify(|_, w| w.txenable().set_bit()); } #[inline] pub fn disable(&mut self) { - self.0.enable().modify(|_, w| w.txenable().clear_bit()); + self.regs_priv() + .enable() + .modify(|_, w| w.txenable().clear_bit()); } /// Enables the IRQ_TX, IRQ_TX_STATUS and IRQ_TX_EMPTY interrupts. @@ -998,7 +1009,7 @@ impl Tx { #[inline] pub fn enable_interrupts(&self) { // Safety: We own the UART structure - enable_tx_interrupts(unsafe { Uart::reg_block() }); + enable_tx_interrupts(self.regs_priv()); } /// Disables the IRQ_TX, IRQ_TX_STATUS and IRQ_TX_EMPTY interrupts. @@ -1007,7 +1018,7 @@ impl Tx { #[inline] pub fn disable_interrupts(&self) { // Safety: We own the UART structure - disable_tx_interrupts(unsafe { Uart::reg_block() }); + disable_tx_interrupts(self.regs_priv()); } /// Low level function to write a word to the UART FIFO. @@ -1017,8 +1028,8 @@ impl Tx { /// Please note that you might have to mask the returned value with 0xff to retrieve the actual /// value if you use the manual parity mode. See chapter 11.4.1 for more information. #[inline(always)] - pub fn write_fifo(&self, data: u32) -> nb::Result<(), Infallible> { - if self.0.txstatus().read().wrrdy().bit_is_clear() { + pub fn write_fifo(&mut self, data: u32) -> nb::Result<(), Infallible> { + if self.regs_priv().txstatus().read().wrrdy().bit_is_clear() { return Err(nb::Error::WouldBlock); } self.write_fifo_unchecked(data); @@ -1032,31 +1043,31 @@ impl Tx { /// Use the [Self::write_fifo] function to write a word to the FIFO reliably using the [nb] /// API. #[inline(always)] - pub fn write_fifo_unchecked(&self, data: u32) { - self.0.data().write(|w| unsafe { w.bits(data) }); + pub fn write_fifo_unchecked(&mut self, data: u32) { + self.regs_priv().data().write(|w| unsafe { w.bits(data) }); } - pub fn into_async(self) -> TxAsync { + pub fn into_async(self) -> TxAsync { TxAsync::new(self) } } -impl embedded_io::ErrorType for Tx { +impl embedded_io::ErrorType for Tx { type Error = Infallible; } -impl embedded_hal_nb::serial::ErrorType for Tx { +impl embedded_hal_nb::serial::ErrorType for Tx { type Error = Infallible; } -impl embedded_hal_nb::serial::Write for Tx { +impl embedded_hal_nb::serial::Write for Tx { fn write(&mut self, word: u8) -> nb::Result<(), Self::Error> { self.write_fifo(word as u32) } fn flush(&mut self) -> nb::Result<(), Self::Error> { // SAFETY: Only TX related registers are used. - let reader = unsafe { &(*Uart::ptr()) }.txstatus().read(); + let reader = self.regs_priv().txstatus().read(); if reader.wrbusy().bit_is_set() { return Err(nb::Error::WouldBlock); } @@ -1064,13 +1075,13 @@ impl embedded_hal_nb::serial::Write for Tx { } } -impl embedded_io::Write for Tx { +impl embedded_io::Write for Tx { fn write(&mut self, buf: &[u8]) -> Result { if buf.is_empty() { return Ok(0); } loop { - if self.0.txstatus().read().wrrdy().bit_is_set() { + if self.regs_priv().txstatus().read().wrrdy().bit_is_set() { break; } } @@ -1106,10 +1117,10 @@ impl embedded_io::Write for Tx { /// then call the [Self::on_interrupt_max_size_or_timeout_based] in the interrupt service /// routine. You have to call [Self::read_fixed_len_or_timeout_based_using_irq] in the ISR to /// start reading the next packet. -pub struct RxWithInterrupt(Rx); +pub struct RxWithInterrupt(Rx); -impl RxWithInterrupt { - pub fn new(rx: Rx) -> Self { +impl RxWithInterrupt { + pub fn new(rx: Rx) -> Self { Self(rx) } @@ -1121,8 +1132,8 @@ impl RxWithInterrupt { } #[inline(always)] - pub fn uart(&self) -> &Uart { - &self.0 .0 + pub fn rx(&self) -> &Rx { + &self.0 } /// This function is used together with the [Self::on_interrupt_max_size_or_timeout_based] @@ -1148,7 +1159,7 @@ impl RxWithInterrupt { #[inline] fn enable_rx_irq_sources(&mut self, timeout: bool) { - self.uart().irq_enb().modify(|_, w| { + self.rx().regs_priv().irq_enb().modify(|_, w| { if timeout { w.irq_rx_to().set_bit(); } @@ -1159,7 +1170,7 @@ impl RxWithInterrupt { #[inline] fn disable_rx_irq_sources(&mut self) { - self.uart().irq_enb().modify(|_, w| { + self.rx().regs_priv().irq_enb().modify(|_, w| { w.irq_rx_to().clear_bit(); w.irq_rx_status().clear_bit(); w.irq_rx().clear_bit() @@ -1182,18 +1193,19 @@ impl RxWithInterrupt { pub fn on_interrupt(&mut self, buf: &mut [u8; 16]) -> IrqResult { let mut result = IrqResult::default(); - let irq_end = self.uart().irq_end().read(); - let enb_status = self.uart().enable().read(); + let rx = self.rx(); + let irq_end = rx.regs_priv().irq_end().read(); + let enb_status = rx.regs_priv().enable().read(); let rx_enabled = enb_status.rxenable().bit_is_set(); // Half-Full interrupt. We have a guaranteed amount of data we can read. if irq_end.irq_rx().bit_is_set() { - let available_bytes = self.uart().rxfifoirqtrg().read().bits() as usize; + let available_bytes = rx.regs_priv().rxfifoirqtrg().read().bits() as usize; // If this interrupt bit is set, the trigger level is available at the very least. // Read everything as fast as possible for _ in 0..available_bytes { - buf[result.bytes_read] = (self.uart().data().read().bits() & 0xff) as u8; + buf[result.bytes_read] = (rx.regs_priv().data().read().bits() & 0xff) as u8; result.bytes_read += 1; } } @@ -1213,7 +1225,8 @@ impl RxWithInterrupt { } // Clear the interrupt status bits - self.uart() + self.0 + .regs_priv() .irq_clr() .write(|w| unsafe { w.bits(irq_end.bits()) }); result @@ -1243,9 +1256,10 @@ impl RxWithInterrupt { }); } let mut result = IrqResultMaxSizeOrTimeout::default(); + let rx = self.rx(); - let irq_end = self.uart().irq_end().read(); - let enb_status = self.uart().enable().read(); + let irq_end = rx.regs_priv().irq_end().read(); + let enb_status = rx.regs_priv().enable().read(); let rx_enabled = enb_status.rxenable().bit_is_set(); // Half-Full interrupt. We have a guaranteed amount of data we can read. @@ -1254,7 +1268,7 @@ impl RxWithInterrupt { // We use this trick/hack because the timeout feature of the peripheral relies on data // being in the RX FIFO. If data continues arriving, another half-full IRQ will fire. // If not, the last byte(s) is/are emptied by the timeout interrupt. - let available_bytes = self.uart().rxfifoirqtrg().read().bits() as usize; + let available_bytes = rx.regs_priv().rxfifoirqtrg().read().bits() as usize; let bytes_to_read = core::cmp::min( available_bytes.saturating_sub(1), @@ -1264,7 +1278,7 @@ impl RxWithInterrupt { // If this interrupt bit is set, the trigger level is available at the very least. // Read everything as fast as possible for _ in 0..bytes_to_read { - buf[context.rx_idx] = (self.uart().data().read().bits() & 0xff) as u8; + buf[context.rx_idx] = (rx.regs_priv().data().read().bits() & 0xff) as u8; context.rx_idx += 1; } @@ -1299,14 +1313,14 @@ impl RxWithInterrupt { } // Clear the interrupt status bits - self.uart() + rx.regs_priv() .irq_clr() .write(|w| unsafe { w.bits(irq_end.bits()) }); Ok(result) } fn check_for_errors(&self, errors: &mut Option) { - let rx_status = self.uart().rxstatus().read(); + let rx_status = self.rx().regs_priv().rxstatus().read(); if rx_status.rxovr().bit_is_set() || rx_status.rxfrm().bit_is_set() @@ -1344,8 +1358,9 @@ impl RxWithInterrupt { /// This API allows creating multiple UART instances when releasing the TX structure as well. /// The user must ensure that these instances are not used to create multiple overlapping /// UART drivers. - pub unsafe fn release(self) -> Uart { - self.0.release() + pub unsafe fn release(mut self) -> Rx { + self.disable_rx_irq_sources(); + self.0 } } diff --git a/va108xx-hal/src/uart/rx_asynch.rs b/va108xx-hal/src/uart/rx_asynch.rs index 215bfae..0ef374c 100644 --- a/va108xx-hal/src/uart/rx_asynch.rs +++ b/va108xx-hal/src/uart/rx_asynch.rs @@ -26,7 +26,7 @@ use embedded_io::ErrorType; use portable_atomic::AtomicBool; use va108xx::uarta as uart_base; -use super::{Bank, Instance, Rx, UartErrors}; +use super::{Rx, UartErrors, UartId, UartPeripheralMarker}; static UART_RX_WAKERS: [AtomicWaker; 2] = [const { AtomicWaker::new() }; 2]; static RX_READ_ACTIVE: [AtomicBool; 2] = [const { AtomicBool::new(false) }; 2]; @@ -37,10 +37,10 @@ struct RxFuture { } impl RxFuture { - pub fn new(_rx: &mut Rx) -> Self { - RX_READ_ACTIVE[Uart::IDX as usize].store(true, Ordering::Relaxed); + pub fn new(rx: &mut Rx) -> Self { + RX_READ_ACTIVE[rx.0 as usize].store(true, Ordering::Relaxed); Self { - uart_idx: Uart::IDX as usize, + uart_idx: rx.0 as usize, } } } @@ -92,12 +92,12 @@ fn on_interrupt_handle_rx_errors(uart: &'static uart_base::RegisterBlock) -> Opt } fn on_interrupt_rx_common_post_processing( - bank: Bank, + id: UartId, rx_enabled: bool, read_some_data: bool, irq_end: u32, ) -> Option { - let idx = bank as usize; + let idx = id as usize; if read_some_data { RX_HAS_DATA[idx].store(true, Ordering::Relaxed); if RX_READ_ACTIVE[idx].load(Ordering::Relaxed) { @@ -106,7 +106,7 @@ fn on_interrupt_rx_common_post_processing( } let mut errors = None; - let uart_regs = unsafe { bank.reg_block() }; + let uart_regs = unsafe { id.reg_block() }; // Check for RX errors if rx_enabled { errors = on_interrupt_handle_rx_errors(uart_regs); @@ -123,7 +123,7 @@ fn on_interrupt_rx_common_post_processing( /// asynchronous reception. This variant will overwrite old data in the ring buffer in case /// the ring buffer is full. pub fn on_interrupt_rx_overwriting( - bank: Bank, + bank: UartId, prod: &mut heapless::spsc::Producer, shared_consumer: &Mutex>>>, ) -> Result<(), AsyncUartErrors> { @@ -131,7 +131,7 @@ pub fn on_interrupt_rx_overwriting( } pub fn on_interrupt_rx_async_heapless_queue_overwriting( - bank: Bank, + bank: UartId, prod: &mut heapless::spsc::Producer, shared_consumer: &Mutex>>>, ) -> Result<(), AsyncUartErrors> { @@ -194,14 +194,14 @@ pub fn on_interrupt_rx_async_heapless_queue_overwriting( /// /// Should be called in the user interrupt handler to enable asynchronous reception. pub fn on_interrupt_rx( - bank: Bank, + bank: UartId, prod: &mut heapless::spsc::Producer<'_, u8, N>, ) -> Result<(), AsyncUartErrors> { on_interrupt_rx_async_heapless_queue(bank, prod) } pub fn on_interrupt_rx_async_heapless_queue( - bank: Bank, + bank: UartId, prod: &mut heapless::spsc::Producer<'_, u8, N>, ) -> Result<(), AsyncUartErrors> { let uart = unsafe { bank.reg_block() }; @@ -259,33 +259,33 @@ impl Drop for ActiveReadGuard { } } -struct RxAsyncInner { - rx: Rx, +struct RxAsyncInner { + rx: Rx, pub queue: heapless::spsc::Consumer<'static, u8, N>, } /// Core data structure to allow asynchronous UART reception. /// /// If the ring buffer becomes full, data will be lost. -pub struct RxAsync(Option>); +pub struct RxAsync(Option>); -impl ErrorType for RxAsync { +impl ErrorType for RxAsync { /// Error reporting is done using the result of the interrupt functions. type Error = Infallible; } -fn stop_async_rx(rx: &mut Rx) { +fn stop_async_rx(rx: &mut Rx) { rx.disable_interrupts(); rx.disable(); rx.clear_fifo(); } -impl RxAsync { +impl RxAsync { /// Create a new asynchronous receiver. /// /// The passed [heapless::spsc::Consumer] will be used to asynchronously receive data which /// is filled by the interrupt handler [on_interrupt_rx]. - pub fn new(mut rx: Rx, queue: heapless::spsc::Consumer<'static, u8, N>) -> Self { + pub fn new(mut rx: Rx, queue: heapless::spsc::Consumer<'static, u8, N>) -> Self { rx.disable_interrupts(); rx.disable(); rx.clear_fifo(); @@ -301,27 +301,28 @@ impl RxAsync { stop_async_rx(&mut self.0.as_mut().unwrap().rx); } - pub fn release(mut self) -> (Rx, heapless::spsc::Consumer<'static, u8, N>) { + pub fn release(mut self) -> (Rx, heapless::spsc::Consumer<'static, u8, N>) { self.stop(); let inner = self.0.take().unwrap(); (inner.rx, inner.queue) } } -impl Drop for RxAsync { +impl Drop for RxAsync { fn drop(&mut self) { self.stop(); } } -impl embedded_io_async::Read for RxAsync { +impl embedded_io_async::Read for RxAsync { async fn read(&mut self, buf: &mut [u8]) -> Result { + let inner = self.0.as_ref().unwrap(); // Need to wait for the IRQ to read data and set this flag. If the queue is not // empty, we can read data immediately. - if self.0.as_ref().unwrap().queue.len() == 0 { - RX_HAS_DATA[Uart::IDX as usize].store(false, Ordering::Relaxed); + if inner.queue.len() == 0 { + RX_HAS_DATA[inner.rx.0 as usize].store(false, Ordering::Relaxed); } - let _guard = ActiveReadGuard(Uart::IDX as usize); + let _guard = ActiveReadGuard(inner.rx.0 as usize); let mut handle_data_in_queue = |consumer: &mut heapless::spsc::Consumer<'static, u8, N>| { let data_to_read = consumer.len().min(buf.len()); for byte in buf.iter_mut().take(data_to_read) { @@ -343,8 +344,8 @@ impl embedded_io_async::Read for RxAsync { - rx: Rx, +struct RxAsyncOverwritingInner { + rx: Rx, pub shared_consumer: &'static Mutex>>>, } @@ -352,23 +353,21 @@ struct RxAsyncOverwritingInner { /// /// If the ring buffer becomes full, the oldest data will be overwritten when using the /// [on_interrupt_rx_overwriting] interrupt handlers. -pub struct RxAsyncOverwriting( - Option>, -); +pub struct RxAsyncOverwriting(Option>); -impl ErrorType for RxAsyncOverwriting { +impl ErrorType for RxAsyncOverwriting { /// Error reporting is done using the result of the interrupt functions. type Error = Infallible; } -impl RxAsyncOverwriting { +impl RxAsyncOverwriting { /// Create a new asynchronous receiver. /// /// The passed shared [heapless::spsc::Consumer] will be used to asynchronously receive data /// which is filled by the interrupt handler. The shared property allows using it in the /// interrupt handler to overwrite old data. pub fn new( - mut rx: Rx, + mut rx: Rx, shared_consumer: &'static Mutex>>>, ) -> Self { rx.disable_interrupts(); @@ -389,32 +388,34 @@ impl RxAsyncOverwriting { stop_async_rx(&mut self.0.as_mut().unwrap().rx); } - pub fn release(mut self) -> Rx { + pub fn release(mut self) -> Rx { self.stop(); let inner = self.0.take().unwrap(); inner.rx } } -impl Drop for RxAsyncOverwriting { +impl Drop for RxAsyncOverwriting { fn drop(&mut self) { self.stop(); } } -impl embedded_io_async::Read for RxAsyncOverwriting { +impl embedded_io_async::Read for RxAsyncOverwriting { async fn read(&mut self, buf: &mut [u8]) -> Result { + let inner = self.0.as_ref().unwrap(); + let id = inner.rx.0 as usize; // Need to wait for the IRQ to read data and set this flag. If the queue is not // empty, we can read data immediately. critical_section::with(|cs| { - let queue = self.0.as_ref().unwrap().shared_consumer.borrow(cs); + let queue = inner.shared_consumer.borrow(cs); if queue.borrow().as_ref().unwrap().len() == 0 { - RX_HAS_DATA[Uart::IDX as usize].store(false, Ordering::Relaxed); + RX_HAS_DATA[id as usize].store(false, Ordering::Relaxed); } }); - let _guard = ActiveReadGuard(Uart::IDX as usize); - let mut handle_data_in_queue = |inner: &mut RxAsyncOverwritingInner| { + let _guard = ActiveReadGuard(id as usize); + let mut handle_data_in_queue = |inner: &mut RxAsyncOverwritingInner| { critical_section::with(|cs| { let mut consumer_ref = inner.shared_consumer.borrow(cs).borrow_mut(); let consumer = consumer_ref.as_mut().unwrap(); diff --git a/va108xx-hal/src/uart/tx_asynch.rs b/va108xx-hal/src/uart/tx_asynch.rs index 3dc36f7..988053b 100644 --- a/va108xx-hal/src/uart/tx_asynch.rs +++ b/va108xx-hal/src/uart/tx_asynch.rs @@ -32,7 +32,7 @@ static TX_DONE: [AtomicBool; 2] = [const { AtomicBool::new(false) }; 2]; /// /// The user has to call this once in the interrupt handler responsible for the TX interrupts on /// the given UART bank. -pub fn on_interrupt_tx(bank: Bank) { +pub fn on_interrupt_tx(bank: UartId) { let uart = unsafe { bank.reg_block() }; let idx = bank as usize; let irq_enb = uart.irq_enb().read(); @@ -153,20 +153,20 @@ impl TxFuture { /// /// This function stores the raw pointer of the passed data slice. The user MUST ensure /// that the slice outlives the data structure. - pub unsafe fn new(tx: &mut Tx, data: &[u8]) -> Self { - TX_DONE[Uart::IDX as usize].store(false, core::sync::atomic::Ordering::Relaxed); + pub unsafe fn new(tx: &mut Tx, data: &[u8]) -> Self { + TX_DONE[tx.0 as usize].store(false, core::sync::atomic::Ordering::Relaxed); tx.disable_interrupts(); tx.disable(); tx.clear_fifo(); - let uart_tx = unsafe { tx.uart() }; + let uart_tx = tx.regs_priv(); let init_fill_count = core::cmp::min(data.len(), 16); // We fill the FIFO. for data in data.iter().take(init_fill_count) { uart_tx.data().write(|w| unsafe { w.bits(*data as u32) }); } critical_section::with(|cs| { - let context_ref = TX_CONTEXTS[Uart::IDX as usize].borrow(cs); + let context_ref = TX_CONTEXTS[tx.0 as usize].borrow(cs); let mut context = context_ref.borrow_mut(); context.slice.set(data); context.progress = init_fill_count; @@ -177,7 +177,7 @@ impl TxFuture { tx.enable(); }); Self { - uart_idx: Uart::IDX as usize, + uart_idx: tx.0 as usize, } } } @@ -213,17 +213,15 @@ impl Drop for TxFuture { } } -pub struct TxAsync { - tx: Tx, -} +pub struct TxAsync(Tx); -impl TxAsync { - pub fn new(tx: Tx) -> Self { - Self { tx } +impl TxAsync { + pub fn new(tx: Tx) -> Self { + Self(tx) } - pub fn release(self) -> Tx { - self.tx + pub fn release(self) -> Tx { + self.0 } } @@ -238,17 +236,17 @@ impl embedded_io_async::Error for TxOverrunError { } } -impl embedded_io::ErrorType for TxAsync { +impl embedded_io::ErrorType for TxAsync { type Error = TxOverrunError; } -impl Write for TxAsync { +impl Write for TxAsync { /// Write a buffer asynchronously. /// /// This implementation is not side effect free, and a started future might have already /// written part of the passed buffer. async fn write(&mut self, buf: &[u8]) -> Result { - let fut = unsafe { TxFuture::new(&mut self.tx, buf) }; + let fut = unsafe { TxFuture::new(&mut self.0, buf) }; fut.await } } diff --git a/vorago-shared-peripherals/Cargo.toml b/vorago-shared-peripherals/Cargo.toml deleted file mode 100644 index dd0ac93..0000000 --- a/vorago-shared-peripherals/Cargo.toml +++ /dev/null @@ -1,7 +0,0 @@ -[package] -name = "vorago-shared-peripherals" -version = "0.1.0" -edition = "2024" - -[dependencies] -derive-mmio = { path = "../../../ROMEO/derive-mmio" } diff --git a/vorago-shared-peripherals/src/ioconfig.rs b/vorago-shared-peripherals/src/ioconfig.rs deleted file mode 100644 index 8b71e1c..0000000 --- a/vorago-shared-peripherals/src/ioconfig.rs +++ /dev/null @@ -1,3 +0,0 @@ -pub struct Config { - -} diff --git a/vorago-shared-peripherals/src/lib.rs b/vorago-shared-peripherals/src/lib.rs deleted file mode 100644 index 7cc1f14..0000000 --- a/vorago-shared-peripherals/src/lib.rs +++ /dev/null @@ -1,2 +0,0 @@ -#![no_std] -pub mod ioconfig; diff --git a/vorago-shared-periphs/Cargo.toml b/vorago-shared-periphs/Cargo.toml new file mode 100644 index 0000000..1cbe8ea --- /dev/null +++ b/vorago-shared-periphs/Cargo.toml @@ -0,0 +1,21 @@ +[package] +name = "vorago-shared-periphs" +version = "0.1.0" +edition = "2024" + +[dependencies] +cfg-if = "1" +derive-mmio = { path = "../../../ROMEO/derive-mmio" } +bitbybit = "1.3" +arbitrary-int = "1.3" +static_assertions = "1.1" +embedded-hal = { version = "1.0" } +thiserror = { version = "2", default-features = false } +paste = "1" +defmt = { version = "1", optional = true } + +[features] +vor1x = ["_family-selected"] +vor4x = ["_family-selected"] + +_family-selected = [] diff --git a/vorago-shared-periphs/src/gpio/ll.rs b/vorago-shared-periphs/src/gpio/ll.rs new file mode 100644 index 0000000..c378020 --- /dev/null +++ b/vorago-shared-periphs/src/gpio/ll.rs @@ -0,0 +1,185 @@ +pub use embedded_hal::digital::PinState; + +pub use crate::InvalidOffsetError; +pub use crate::Port; +pub use crate::ioconfig::regs::Pull; +use crate::ioconfig::regs::{FunSel, IoConfig, MmioIoConfig}; + +pub struct LowLevelGpio { + gpio: super::regs::MmioGpio<'static>, + ioconfig: MmioIoConfig<'static>, + port: Port, + offset: usize, +} + +impl LowLevelGpio { + pub fn new(port: Port, offset: usize) -> Result { + if offset >= port.max_offset() { + return Err(InvalidOffsetError { + offset, + port: Port::A, + }); + } + Ok(LowLevelGpio { + gpio: super::regs::Gpio::new_mmio(port), + ioconfig: IoConfig::new_mmio(), + port, + offset, + }) + } + + #[inline] + pub fn port(&self) -> Port { + self.port + } + + #[inline] + pub fn offset(&self) -> usize { + self.offset + } + + pub fn configure_as_input_floating(&mut self) { + unsafe { + self.ioconfig + .modify_pin_config_unchecked(self.port, self.offset, |mut config| { + config.set_funsel(FunSel::Sel0); + config.set_io_disable(false); + config.set_invert_input(false); + config.set_open_drain(false); + config.set_pull_enable(false); + config.set_pull_when_output_active(false); + config.set_invert_output(false); + config.set_input_enable_when_output(false); + config + }); + } + self.gpio.modify_dir(|mut dir| { + dir &= !(1 << self.offset); + dir + }); + } + + pub fn configure_as_input_with_pull(&mut self, pull: Pull) { + unsafe { + self.ioconfig + .modify_pin_config_unchecked(self.port, self.offset, |mut config| { + config.set_funsel(FunSel::Sel0); + config.set_io_disable(false); + config.set_invert_input(false); + config.set_open_drain(false); + config.set_pull_enable(true); + config.set_pull_dir(pull); + config.set_pull_when_output_active(false); + config.set_invert_output(false); + config.set_input_enable_when_output(false); + config + }); + } + self.gpio.modify_dir(|mut dir| { + dir &= !(1 << self.offset); + dir + }); + } + + pub fn configure_as_output_push_pull(&mut self, init_level: PinState) { + unsafe { + self.ioconfig + .modify_pin_config_unchecked(self.port, self.offset, |mut config| { + config.set_funsel(FunSel::Sel0); + config.set_io_disable(false); + config.set_invert_input(false); + config.set_open_drain(false); + config.set_pull_enable(false); + config.set_pull_when_output_active(false); + config.set_invert_output(false); + config.set_input_enable_when_output(true); + config + }); + } + match init_level { + PinState::Low => self.gpio.write_clr_out(1 << self.offset), + PinState::High => self.gpio.write_set_out(1 << self.offset), + } + self.gpio.modify_dir(|mut dir| { + dir |= 1 << self.offset; + dir + }); + } + + pub fn configure_as_output_open_drain(&mut self, init_level: PinState) { + unsafe { + self.ioconfig + .modify_pin_config_unchecked(self.port, self.offset, |mut config| { + config.set_funsel(FunSel::Sel0); + config.set_io_disable(false); + config.set_invert_input(false); + config.set_open_drain(true); + config.set_pull_enable(true); + config.set_pull_dir(Pull::Up); + config.set_pull_when_output_active(false); + config.set_invert_output(false); + config.set_input_enable_when_output(true); + config + }); + } + match init_level { + PinState::Low => self.gpio.write_clr_out(1 << self.offset), + PinState::High => self.gpio.write_set_out(1 << self.offset), + } + self.gpio.modify_dir(|mut dir| { + dir |= 1 << self.offset; + dir + }); + } + + pub fn configure_as_peripheral_pin(&mut self, fun_sel: FunSel, pull: Option) { + unsafe { + self.ioconfig + .modify_pin_config_unchecked(self.port, self.offset, |mut config| { + config.set_funsel(fun_sel); + config.set_io_disable(false); + config.set_invert_input(false); + config.set_open_drain(false); + config.set_pull_enable(pull.is_some()); + config.set_pull_dir(pull.unwrap_or(Pull::Up)); + config.set_invert_output(false); + config + }); + } + } + + #[inline] + pub fn is_high(&self) -> bool { + (self.gpio.read_data_in() >> self.offset) & 1 == 1 + } + + #[inline] + pub fn is_low(&self) -> bool { + !self.is_high() + } + + #[inline] + pub fn set_high(&mut self) { + self.gpio.write_set_out(1 << self.offset); + } + + #[inline] + pub fn set_low(&mut self) { + self.gpio.write_clr_out(1 << self.offset); + } + + #[inline] + pub fn is_set_high(&self) -> bool { + (self.gpio.read_data_out() >> self.offset) & 1 == 1 + } + + #[inline] + pub fn is_set_low(&self) -> bool { + !self.is_set_high() + } + + #[inline] + pub fn toggle(&mut self) { + self.gpio.write_tog_out(1 << self.offset); + } +} diff --git a/vorago-shared-periphs/src/gpio/mod.rs b/vorago-shared-periphs/src/gpio/mod.rs new file mode 100644 index 0000000..af2c191 --- /dev/null +++ b/vorago-shared-periphs/src/gpio/mod.rs @@ -0,0 +1,290 @@ +use core::convert::Infallible; + +pub use embedded_hal::digital::PinState; +pub use ll::{Port, Pull}; + +use crate::ioconfig::regs::FunSel; + +pub mod ll; +pub mod regs; + +pub trait PinId { + const PORT: Port; + const OFFSET: usize; +} + +pub struct Pin { + phantom: core::marker::PhantomData, +} + +impl Pin { + #[allow(clippy::new_without_default)] + pub const fn new() -> Self { + Self { + phantom: core::marker::PhantomData, + } + } +} + +pub struct Output(ll::LowLevelGpio); + +impl Output { + pub fn new(_pin: Pin, init_level: PinState) -> Self { + let mut ll = ll::LowLevelGpio::new(I::PORT, I::OFFSET).unwrap(); + ll.configure_as_output_push_pull(init_level); + Output(ll) + } + + #[inline] + pub fn port(&self) -> Port { + self.0.port() + } + + #[inline] + pub fn offset(&self) -> usize { + self.0.offset() + } + + #[inline] + pub fn set_high(&mut self) { + self.0.set_high(); + } + + #[inline] + pub fn set_low(&mut self) { + self.0.set_low(); + } + + #[inline] + pub fn is_set_high(&self) -> bool { + self.0.is_set_high() + } + + #[inline] + pub fn is_set_low(&self) -> bool { + self.0.is_set_low() + } +} + +impl embedded_hal::digital::ErrorType for Output { + type Error = Infallible; +} + +impl embedded_hal::digital::OutputPin for Output { + fn set_low(&mut self) -> Result<(), Self::Error> { + self.0.set_low(); + Ok(()) + } + + fn set_high(&mut self) -> Result<(), Self::Error> { + self.0.set_high(); + Ok(()) + } +} + +impl embedded_hal::digital::StatefulOutputPin for Output { + fn is_set_high(&mut self) -> Result { + Ok(self.0.is_set_high()) + } + + fn is_set_low(&mut self) -> Result { + Ok(self.0.is_set_low()) + } + + /// Toggle pin output with dedicated HW feature. + fn toggle(&mut self) -> Result<(), Self::Error> { + self.0.toggle(); + Ok(()) + } +} + +pub struct Input(ll::LowLevelGpio); + +impl Input { + pub fn new_floating(_pin: Pin) -> Self { + let mut ll = ll::LowLevelGpio::new(I::PORT, I::OFFSET).unwrap(); + ll.configure_as_input_floating(); + Input(ll) + } + + pub fn new_with_pull(_pin: Pin, pull: Pull) -> Self { + let mut ll = ll::LowLevelGpio::new(I::PORT, I::OFFSET).unwrap(); + ll.configure_as_input_with_pull(pull); + Input(ll) + } + + #[inline] + pub fn port(&self) -> Port { + self.0.port() + } + + #[inline] + pub fn offset(&self) -> usize { + self.0.offset() + } +} + +impl embedded_hal::digital::ErrorType for Input { + type Error = Infallible; +} + +impl embedded_hal::digital::InputPin for Input { + fn is_low(&mut self) -> Result { + Ok(self.0.is_low()) + } + + fn is_high(&mut self) -> Result { + Ok(self.0.is_high()) + } +} + +pub enum PinMode { + InputFloating, + InputWithPull(Pull), + OutputPushPull, + OutputOpenDrain, +} + +impl PinMode { + pub fn is_input(&self) -> bool { + matches!(self, PinMode::InputFloating | PinMode::InputWithPull(_)) + } + + pub fn is_output(&self) -> bool { + !self.is_input() + } +} + +pub struct Flex { + ll: ll::LowLevelGpio, + mode: PinMode, +} + +impl Flex { + pub fn new(_pin: Pin) -> Self { + let mut ll = ll::LowLevelGpio::new(I::PORT, I::OFFSET).unwrap(); + ll.configure_as_input_floating(); + Flex { + ll, + mode: PinMode::InputFloating, + } + } + + #[inline] + pub fn port(&self) -> Port { + self.ll.port() + } + + #[inline] + pub fn offset(&self) -> usize { + self.ll.offset() + } + + /// Reads the input state of the pin, regardless of configured mode. + #[inline] + pub fn is_low(&self) -> bool { + self.ll.is_low() + } + + /// Reads the input state of the pin, regardless of configured mode. + #[inline] + pub fn is_high(&self) -> bool { + self.ll.is_high() + } + + /// If the pin is configured as an input pin, this function does nothing. + #[inline] + pub fn set_low(&mut self) { + if !self.mode.is_input() { + return; + } + self.ll.set_low(); + } + + /// If the pin is configured as an input pin, this function does nothing. + #[inline] + pub fn set_high(&mut self) { + if !self.mode.is_input() { + return; + } + self.ll.set_high(); + } +} + +impl embedded_hal::digital::ErrorType for Flex { + type Error = Infallible; +} + +impl embedded_hal::digital::InputPin for Flex { + /// Reads the input state of the pin, regardless of configured mode. + fn is_low(&mut self) -> Result { + Ok(self.ll.is_low()) + } + + /// Reads the input state of the pin, regardless of configured mode. + fn is_high(&mut self) -> Result { + Ok(self.ll.is_high()) + } +} + +impl embedded_hal::digital::OutputPin for Flex { + /// If the pin is configured as an input pin, this function does nothing. + fn set_low(&mut self) -> Result<(), Self::Error> { + self.set_low(); + Ok(()) + } + + /// If the pin is configured as an input pin, this function does nothing. + fn set_high(&mut self) -> Result<(), Self::Error> { + self.set_high(); + Ok(()) + } +} + +impl embedded_hal::digital::StatefulOutputPin for Flex { + /// If the pin is not configured as a stateful output pin like Output Push-Pull, the result + /// of this function is undefined. + fn is_set_high(&mut self) -> Result { + Ok(self.ll.is_set_high()) + } + + /// If the pin is not configured as a stateful output pin like Output Push-Pull, the result + /// of this function is undefined. + fn is_set_low(&mut self) -> Result { + Ok(self.ll.is_set_low()) + } + + /// Toggle pin output. + /// + /// If the pin is not configured as a stateful output pin like Output Push-Pull, the result + /// of this function is undefined. + fn toggle(&mut self) -> Result<(), Self::Error> { + self.ll.toggle(); + Ok(()) + } +} + +pub struct IoPeriphPin { + ll: ll::LowLevelGpio, + fun_sel: FunSel, +} + +impl IoPeriphPin { + pub fn new(_pin: Pin, fun_sel: FunSel, pull: Option) -> Self { + let mut ll = ll::LowLevelGpio::new(I::PORT, I::OFFSET).unwrap(); + ll.configure_as_peripheral_pin(fun_sel, pull); + IoPeriphPin { ll, fun_sel } + } + + pub fn port(&self) -> Port { + self.ll.port() + } + + pub fn offset(&self) -> usize { + self.ll.offset() + } + + pub fn fun_sel(&self) -> FunSel { + self.fun_sel + } +} diff --git a/vorago-shared-periphs/src/gpio/regs.rs b/vorago-shared-periphs/src/gpio/regs.rs new file mode 100644 index 0000000..b23beac --- /dev/null +++ b/vorago-shared-periphs/src/gpio/regs.rs @@ -0,0 +1,123 @@ +use crate::Port; + +cfg_if::cfg_if! { + if #[cfg(feature = "vor1x")] { + /// PORT A base address. + pub const GPIO_0_BASE: usize = 0x5000_0000; + /// PORT B base address. + pub const GPIO_1_BASE: usize = 0x5000_1000; + } else if #[cfg(feature = "vor4x")] { + /// PORT A base address. + pub const GPIO_0_BASE: usize = 0x4001_2000; + /// PORT B base address. + pub const GPIO_1_BASE: usize = 0x4001_2400; + /// PORT C base address. + pub const GPIO_2_BASE: usize = 0x4001_2800; + /// PORT D base address. + pub const GPIO_3_BASE: usize = 0x4001_2C00; + /// PORT E base address. + pub const GPIO_4_BASE: usize = 0x4001_3000; + /// PORT F base address. + pub const GPIO_5_BASE: usize = 0x4001_3400; + /// PORT G base address. + pub const GPIO_6_BASE: usize = 0x4001_3800; + } +} + +#[derive(derive_mmio::Mmio)] +#[mmio(no_ctors)] +#[repr(C)] +pub struct Gpio { + #[mmio(PureRead)] + data_in: u32, + #[mmio(PureRead)] + data_in_raw: u32, + data_out: u32, + data_out_raw: u32, + #[mmio(Write)] + set_out: u32, + #[mmio(Write)] + clr_out: u32, + #[mmio(Write)] + tog_out: u32, + data_mask: u32, + /// Direction bits. 1 for output, 0 for input. + dir: u32, + pulse: u32, + pulsebase: u32, + delay1: u32, + delay2: u32, + irq_sen: u32, + irq_edge: u32, + irq_evt: u32, + irq_enb: u32, + #[mmio(PureRead)] + irq_raw: u32, + #[mmio(PureRead)] + irq_end: u32, + #[mmio(PureRead)] + edge_status: u32, + + #[cfg(feature = "vor1x")] + _reserved: [u32; 0x3eb], + #[cfg(feature = "vor4x")] + _reserved: [u32; 0xeb], + + /// Peripheral ID. Vorago 1x reset value: 0x0040_07e1. Vorago 4x reset value: 0x0210_07E9. + perid: u32, +} + +cfg_if::cfg_if! { + if #[cfg(feature = "vor1x")] { + static_assertions::const_assert_eq!(core::mem::size_of::(), 0x1000); + } else if #[cfg(feature = "vor4x")] { + static_assertions::const_assert_eq!(core::mem::size_of::(), 0x400); + } +} + +impl Gpio { + const fn new_mmio_at(base: usize) -> MmioGpio<'static> { + MmioGpio { + ptr: base as *mut _, + phantom: core::marker::PhantomData, + } + } + + pub const fn new_mmio(port: Port) -> MmioGpio<'static> { + match port { + Port::A => Self::new_mmio_at(GPIO_0_BASE), + Port::B => Self::new_mmio_at(GPIO_1_BASE), + #[cfg(feature = "vor4x")] + Port::C => Self::new_mmio_at(GPIO_2_BASE), + #[cfg(feature = "vor4x")] + Port::D => Self::new_mmio_at(GPIO_3_BASE), + #[cfg(feature = "vor4x")] + Port::E => Self::new_mmio_at(GPIO_4_BASE), + #[cfg(feature = "vor4x")] + Port::F => Self::new_mmio_at(GPIO_5_BASE), + #[cfg(feature = "vor4x")] + Port::G => Self::new_mmio_at(GPIO_6_BASE), + } + } +} + +impl MmioGpio<'_> { + pub fn port(&self) -> Port { + match unsafe { self.ptr() } as usize { + GPIO_0_BASE => Port::A, + GPIO_1_BASE => Port::B, + #[cfg(feature = "vor4x")] + GPIO_2_BASE => Port::C, + #[cfg(feature = "vor4x")] + GPIO_3_BASE => Port::D, + #[cfg(feature = "vor4x")] + GPIO_4_BASE => Port::E, + #[cfg(feature = "vor4x")] + GPIO_5_BASE => Port::F, + #[cfg(feature = "vor4x")] + GPIO_6_BASE => Port::G, + // Constructors were disabled, so this should really not happen. + _ => panic!("unexpected base address of GPIO register block"), + } + } +} diff --git a/vorago-shared-periphs/src/ioconfig/mod.rs b/vorago-shared-periphs/src/ioconfig/mod.rs new file mode 100644 index 0000000..0a86d1c --- /dev/null +++ b/vorago-shared-periphs/src/ioconfig/mod.rs @@ -0,0 +1 @@ +pub mod regs; diff --git a/vorago-shared-periphs/src/ioconfig/regs.rs b/vorago-shared-periphs/src/ioconfig/regs.rs new file mode 100644 index 0000000..cdfcc48 --- /dev/null +++ b/vorago-shared-periphs/src/ioconfig/regs.rs @@ -0,0 +1,241 @@ +use core::marker::PhantomData; + +use crate::{InvalidOffsetError, NUM_PORT_A, NUM_PORT_B}; +#[cfg(feature = "vor4x")] +use crate::{NUM_PORT_DEFAULT, NUM_PORT_G}; + +#[cfg(feature = "vor1x")] +pub const BASE_ADDR: usize = 0x4000_2000; +#[cfg(feature = "vor4x")] +pub const BASE_ADDR: usize = 0x4001_1000; + +#[bitbybit::bitenum(u3)] +pub enum FilterType { + SysClk = 0, + DirectInput = 1, + FilterOneCycle = 2, + FilterTwoCycles = 3, + FilterThreeCycles = 4, + FilterFourCycles = 5, +} + +#[derive(Debug, PartialEq, Eq)] +#[bitbybit::bitenum(u3, exhaustive = true)] +#[cfg_attr(feature = "defmt", derive(defmt::Format))] +pub enum FilterClkSel { + SysClk = 0, + Clk1 = 1, + Clk2 = 2, + Clk3 = 3, + Clk4 = 4, + Clk5 = 5, + Clk6 = 6, + Clk7 = 7, +} + +#[derive(Debug, PartialEq, Eq)] +#[bitbybit::bitenum(u1, exhaustive = true)] +#[cfg_attr(feature = "defmt", derive(defmt::Format))] +pub enum Pull { + Up = 0, + Down = 1, +} + +#[derive(Debug, Eq, PartialEq)] +#[bitbybit::bitenum(u2, exhaustive = true)] +#[cfg_attr(feature = "defmt", derive(defmt::Format))] +pub enum FunSel { + Sel0 = 0b00, + Sel1 = 0b01, + Sel2 = 0b10, + Sel3 = 0b11, +} + +#[bitbybit::bitfield(u32)] +pub struct Config { + #[bit(16, rw)] + io_disable: bool, + #[bits(13..=14, rw)] + funsel: FunSel, + #[bit(12, rw)] + pull_when_output_active: bool, + #[bit(11, rw)] + pull_enable: bool, + #[bit(10, rw)] + pull_dir: Pull, + #[bit(9, rw)] + invert_output: bool, + #[bit(8, rw)] + open_drain: bool, + /// IEWO bit. Allows monitoring of output values. + #[bit(7, rw)] + input_enable_when_output: bool, + #[bit(6, rw)] + invert_input: bool, + #[bits(3..=5, rw)] + filter_sel: FilterClkSel, + #[bits(0..=2, rw)] + filter_type: Option, +} + +#[derive(derive_mmio::Mmio)] +#[mmio(no_ctors)] +#[repr(C)] +pub struct IoConfig { + port_a: [Config; NUM_PORT_A], + port_b: [Config; NUM_PORT_B], + #[cfg(feature = "vor4x")] + port_c: [Config; NUM_PORT_DEFAULT], + #[cfg(feature = "vor4x")] + port_d: [Config; NUM_PORT_DEFAULT], + #[cfg(feature = "vor4x")] + port_e: [Config; NUM_PORT_DEFAULT], + #[cfg(feature = "vor4x")] + port_f: [Config; NUM_PORT_DEFAULT], + #[cfg(feature = "vor4x")] + port_g: [Config; NUM_PORT_G], + #[cfg(feature = "vor4x")] + _reserved0: [u32; 0x8], + #[cfg(feature = "vor4x")] + #[mmio(PureRead)] + clk_div_0: u32, + #[cfg(feature = "vor4x")] + clk_div_1: u32, + #[cfg(feature = "vor4x")] + clk_div_2: u32, + #[cfg(feature = "vor4x")] + clk_div_3: u32, + #[cfg(feature = "vor4x")] + clk_div_4: u32, + #[cfg(feature = "vor4x")] + clk_div_5: u32, + #[cfg(feature = "vor4x")] + clk_div_6: u32, + #[cfg(feature = "vor4x")] + clk_div_7: u32, + #[cfg(feature = "vor4x")] + _reserved1: [u32; 0x387], + #[cfg(feature = "vor1x")] + _reserved1: [u32; 0x3c7], + #[mmio(PureRead)] + /// Reset value: 0x0282_07E9 for Vorago 4x, and 0x0182_07E1 for Vorago 1x + perid: u32, +} + +static_assertions::const_assert_eq!(core::mem::size_of::(), 0x1000); + +impl IoConfig { + pub const fn new_mmio() -> MmioIoConfig<'static> { + MmioIoConfig { + ptr: BASE_ADDR as *mut _, + phantom: PhantomData, + } + } +} + +impl MmioIoConfig<'_> { + pub fn read_pin_config( + &self, + port: crate::Port, + offset: usize, + ) -> Result { + if offset >= port.max_offset() { + return Err(InvalidOffsetError { port, offset }); + } + Ok(unsafe { self.read_pin_config_unchecked(port, offset) }) + } + + /// This function does NOT perform any bounds checking. + /// + /// # Safety + /// + /// Calling this function with an invalid offset can lead to undefined behaviour. + pub unsafe fn read_pin_config_unchecked(&self, port: crate::Port, offset: usize) -> Config { + match port { + crate::Port::A => unsafe { self.read_port_a_unchecked(offset) }, + crate::Port::B => unsafe { self.read_port_b_unchecked(offset) }, + #[cfg(feature = "vor4x")] + crate::Port::C => unsafe { self.read_port_c_unchecked(offset) }, + #[cfg(feature = "vor4x")] + crate::Port::D => unsafe { self.read_port_d_unchecked(offset) }, + #[cfg(feature = "vor4x")] + crate::Port::E => unsafe { self.read_port_e_unchecked(offset) }, + #[cfg(feature = "vor4x")] + crate::Port::F => unsafe { self.read_port_f_unchecked(offset) }, + #[cfg(feature = "vor4x")] + crate::Port::G => unsafe { self.read_port_g_unchecked(offset) }, + } + } + + pub fn modify_pin_config Config>( + &mut self, + port: crate::Port, + offset: usize, + f: F, + ) -> Result<(), InvalidOffsetError> { + if offset >= port.max_offset() { + return Err(InvalidOffsetError { port, offset }); + } + unsafe { self.modify_pin_config_unchecked(port, offset, f) }; + Ok(()) + } + + /// This function does NOT perform any bounds checking. + /// + /// # Safety + /// + /// Calling this function with an invalid offset can lead to undefined behaviour. + pub unsafe fn modify_pin_config_unchecked Config>( + &mut self, + port: crate::Port, + offset: usize, + mut f: F, + ) { + unsafe { + let config = self.read_pin_config_unchecked(port, offset); + self.write_pin_config_unchecked(port, offset, f(config)) + } + } + + pub fn write_pin_config( + &mut self, + port: crate::Port, + offset: usize, + config: Config, + ) -> Result<(), InvalidOffsetError> { + if offset >= port.max_offset() { + return Err(InvalidOffsetError { port, offset }); + } + unsafe { + self.write_pin_config_unchecked(port, offset, config); + } + Ok(()) + } + + /// This function does NOT perform any bounds checking. + /// + /// # Safety + /// + /// Calling this function with an invalid offset can lead to undefined behaviour. + pub unsafe fn write_pin_config_unchecked( + &mut self, + port: crate::Port, + offset: usize, + config: Config, + ) { + match port { + crate::Port::A => unsafe { self.write_port_a_unchecked(offset, config) }, + crate::Port::B => unsafe { self.write_port_b_unchecked(offset, config) }, + #[cfg(feature = "vor4x")] + crate::Port::C => unsafe { self.write_port_c_unchecked(offset, config) }, + #[cfg(feature = "vor4x")] + crate::Port::D => unsafe { self.write_port_d_unchecked(offset, config) }, + #[cfg(feature = "vor4x")] + crate::Port::E => unsafe { self.write_port_e_unchecked(offset, config) }, + #[cfg(feature = "vor4x")] + crate::Port::F => unsafe { self.write_port_f_unchecked(offset, config) }, + #[cfg(feature = "vor4x")] + crate::Port::G => unsafe { self.write_port_g_unchecked(offset, config) }, + } + } +} diff --git a/vorago-shared-periphs/src/lib.rs b/vorago-shared-periphs/src/lib.rs new file mode 100644 index 0000000..b0d0ac3 --- /dev/null +++ b/vorago-shared-periphs/src/lib.rs @@ -0,0 +1,69 @@ +#![no_std] +pub mod gpio; +pub mod ioconfig; + +#[cfg(not(feature = "_family-selected"))] +compile_error!("no Vorago CPU family was select. Choices: vor1x or vor4x"); + +pub use ioconfig::regs::FunSel; + +cfg_if::cfg_if! { + if #[cfg(feature = "vor1x")] { + /// Number of GPIO ports and IOCONFIG registers for PORT A + pub const NUM_PORT_A: usize = 32; + /// Number of GPIO ports and IOCONFIG registers for PORT B + pub const NUM_PORT_B: usize = 24; + } else if #[cfg(feature = "vor4x")] { + /// Number of GPIO ports and IOCONFIG registers for PORT C to Port F + pub const NUM_PORT_DEFAULT: usize = 16; + /// Number of GPIO ports and IOCONFIG registers for PORT A + pub const NUM_PORT_A: usize = NUM_PORT_DEFAULT; + /// Number of GPIO ports and IOCONFIG registers for PORT B + pub const NUM_PORT_B: usize = NUM_PORT_DEFAULT; + /// Number of GPIO ports and IOCONFIG registers for PORT G + pub const NUM_PORT_G: usize = 8; + } +} + +#[derive(Debug, Clone, Copy, PartialEq, Eq)] +#[cfg_attr(feature = "defmt", derive(defmt::Format))] +pub enum Port { + A = 0, + B = 1, + #[cfg(feature = "vor4x")] + C = 2, + #[cfg(feature = "vor4x")] + D = 3, + #[cfg(feature = "vor4x")] + E = 4, + #[cfg(feature = "vor4x")] + F = 5, + #[cfg(feature = "vor4x")] + G = 6, +} + +impl Port { + pub fn max_offset(&self) -> usize { + match self { + Port::A => NUM_PORT_A, + Port::B => NUM_PORT_B, + #[cfg(feature = "vor4x")] + Port::C | Port::D | Port::E | Port::F => NUM_PORT_DEFAULT, + #[cfg(feature = "vor4x")] + Port::G => NUM_PORT_G, + } + } +} + +#[derive(Debug, thiserror::Error)] +#[cfg_attr(feature = "defmt", derive(defmt::Format))] +#[error("invalid GPIO offset {offset} for port {port:?}")] +pub struct InvalidOffsetError { + offset: usize, + port: Port, +} + +#[allow(dead_code)] +pub(crate) mod sealed { + pub trait Sealed {} +}