diff --git a/va108xx-hal/src/clock.rs b/va108xx-hal/src/clock.rs index 1e34bdb..702cc49 100644 --- a/va108xx-hal/src/clock.rs +++ b/va108xx-hal/src/clock.rs @@ -8,8 +8,6 @@ use once_cell::unsync::OnceCell; static SYS_CLOCK: Mutex> = Mutex::new(OnceCell::new()); -pub type PeripheralClocks = PeripheralSelect; - #[derive(Debug, PartialEq, Eq)] #[cfg_attr(feature = "defmt", derive(defmt::Format))] pub enum FilterClkSel { @@ -64,18 +62,4 @@ pub fn set_clk_div_register(syscfg: &mut va108xx::Sysconfig, clk_sel: FilterClkS } } -#[inline] -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(clock: PeripheralClocks) { - let syscfg = unsafe { va108xx::Sysconfig::steal() }; - syscfg - .peripheral_clk_enable() - .modify(|r, w| unsafe { w.bits(r.bits() & !(1 << clock as u8)) }); -} +pub use vorago_shared_periphs::sysconfig::{disable_peripheral_clock, enable_peripheral_clock}; diff --git a/va108xx-hal/src/gpio/asynch.rs b/va108xx-hal/src/gpio/asynch.rs deleted file mode 100644 index 36c4ab8..0000000 --- a/va108xx-hal/src/gpio/asynch.rs +++ /dev/null @@ -1,366 +0,0 @@ -//! # Async GPIO functionality for the VA108xx family. -//! -//! This module provides the [InputPinAsync] and [InputDynPinAsync] which both implement -//! the [embedded_hal_async::digital::Wait] trait. These types allow for asynchronous waiting -//! on GPIO pins. Please note that this module does not specify/declare the interrupt handlers -//! which must be provided for async support to work. However, it provides the -//! [on_interrupt_for_async_gpio_for_port] generic interrupt handler. This should be called in all -//! IRQ functions which handle any GPIO interrupts with the corresponding [Port] argument. -//! -//! # Example -//! -//! - [Async GPIO example](https://egit.irs.uni-stuttgart.de/rust/va108xx-rs/src/branch/main/examples/embassy/src/bin/async-gpio.rs) -use core::future::Future; - -use embassy_sync::waitqueue::AtomicWaker; -use embedded_hal_async::digital::Wait; -use portable_atomic::AtomicBool; -use va108xx::{self as pac}; - -use crate::InterruptConfig; - -use super::{ - pin, DynPin, DynPinId, InputConfig, InterruptEdge, InvalidPinTypeError, Pin, PinId, Port, - NUM_PINS_PORT_A, NUM_PINS_PORT_B, -}; - -static WAKERS_FOR_PORT_A: [AtomicWaker; NUM_PINS_PORT_A] = - [const { AtomicWaker::new() }; NUM_PINS_PORT_A]; -static WAKERS_FOR_PORT_B: [AtomicWaker; NUM_PINS_PORT_B] = - [const { AtomicWaker::new() }; NUM_PINS_PORT_B]; -static EDGE_DETECTION_PORT_A: [AtomicBool; NUM_PINS_PORT_A] = - [const { AtomicBool::new(false) }; NUM_PINS_PORT_A]; -static EDGE_DETECTION_PORT_B: [AtomicBool; NUM_PINS_PORT_B] = - [const { AtomicBool::new(false) }; NUM_PINS_PORT_B]; - -/// Generic interrupt handler for GPIO interrupts on a specific port to support async functionalities -/// -/// This function should be called in all interrupt handlers which handle any GPIO interrupts -/// matching the [Port] argument. -/// The handler will wake the corresponding wakers for the pins that triggered an interrupts -/// as well as update the static edge detection structures. This allows the pin future tocomplete -/// complete async operations. -pub fn on_interrupt_for_async_gpio_for_port(port: Port) { - let periphs = unsafe { pac::Peripherals::steal() }; - - let (irq_enb, edge_status, wakers, edge_detection) = match port { - Port::A => ( - periphs.porta.irq_enb().read().bits(), - periphs.porta.edge_status().read().bits(), - WAKERS_FOR_PORT_A.as_ref(), - EDGE_DETECTION_PORT_A.as_ref(), - ), - Port::B => ( - periphs.portb.irq_enb().read().bits(), - periphs.portb.edge_status().read().bits(), - WAKERS_FOR_PORT_B.as_ref(), - EDGE_DETECTION_PORT_B.as_ref(), - ), - }; - - on_interrupt_for_port(irq_enb, edge_status, wakers, edge_detection); -} - -#[inline] -fn on_interrupt_for_port( - mut irq_enb: u32, - edge_status: u32, - wakers: &'static [AtomicWaker], - edge_detection: &'static [AtomicBool], -) { - while irq_enb != 0 { - let bit_pos = irq_enb.trailing_zeros() as usize; - let bit_mask = 1 << bit_pos; - - wakers[bit_pos].wake(); - - if edge_status & bit_mask != 0 { - edge_detection[bit_pos].store(true, core::sync::atomic::Ordering::Relaxed); - - // Clear the processed bit - irq_enb &= !bit_mask; - } - } -} - -/// Input pin future which implements the [Future] trait. -/// -/// Generally, you want to use the [InputPinAsync] or [InputDynPinAsync] types instead of this -/// which also implements the [embedded_hal_async::digital::Wait] trait. However, access to this -/// struture is granted to allow writing custom async structures. -pub struct InputPinFuture { - pin_id: DynPinId, - waker_group: &'static [AtomicWaker], - edge_detection_group: &'static [AtomicBool], -} - -impl InputPinFuture { - #[inline] - pub fn pin_group_to_waker_and_edge_detection_group( - group: Port, - ) -> (&'static [AtomicWaker], &'static [AtomicBool]) { - match group { - Port::A => (WAKERS_FOR_PORT_A.as_ref(), EDGE_DETECTION_PORT_A.as_ref()), - Port::B => (WAKERS_FOR_PORT_B.as_ref(), EDGE_DETECTION_PORT_B.as_ref()), - } - } - - pub fn new_with_dyn_pin( - pin: &mut DynPin, - irq: pac::Interrupt, - edge: InterruptEdge, - ) -> Result { - if !pin.is_input_pin() { - return Err(InvalidPinTypeError(pin.mode())); - } - - let (waker_group, edge_detection_group) = - Self::pin_group_to_waker_and_edge_detection_group(pin.id().port()); - edge_detection_group[pin.id().num() as usize] - .store(false, core::sync::atomic::Ordering::Relaxed); - pin.configure_edge_interrupt(edge).unwrap(); - pin.enable_interrupt(InterruptConfig::new(irq, true, true)); - Ok(Self { - pin_id: pin.id(), - waker_group, - edge_detection_group, - }) - } - - pub fn new_with_pin( - pin: &mut Pin>, - irq: pac::Interrupt, - edge: InterruptEdge, - ) -> Self { - let (waker_group, edge_detection_group) = - Self::pin_group_to_waker_and_edge_detection_group(pin.id().port()); - edge_detection_group[pin.id().num() as usize] - .store(false, core::sync::atomic::Ordering::Relaxed); - pin.configure_edge_interrupt(edge); - pin.enable_interrupt(InterruptConfig::new(irq, true, true)); - Self { - pin_id: pin.id(), - edge_detection_group, - waker_group, - } - } -} - -impl Drop for InputPinFuture { - fn drop(&mut self) { - // The API ensures that we actually own the pin, so stealing it here is okay. - unsafe { DynPin::steal(self.pin_id) }.disable_interrupt(false); - } -} - -impl Future for InputPinFuture { - type Output = (); - fn poll( - self: core::pin::Pin<&mut Self>, - cx: &mut core::task::Context<'_>, - ) -> core::task::Poll { - let idx = self.pin_id.num() as usize; - self.waker_group[idx].register(cx.waker()); - if self.edge_detection_group[idx].swap(false, core::sync::atomic::Ordering::Relaxed) { - return core::task::Poll::Ready(()); - } - core::task::Poll::Pending - } -} - -pub struct InputDynPinAsync { - pin: DynPin, - irq: pac::Interrupt, -} - -impl InputDynPinAsync { - /// Create a new asynchronous input pin from a [DynPin]. The interrupt ID to be used must be - /// passed as well and is used to route and enable the interrupt. - /// - /// Please note that the interrupt handler itself must be provided by the user and the - /// generic [on_interrupt_for_async_gpio_for_port] function must be called inside that function - /// for the asynchronous functionality to work. - pub fn new(pin: DynPin, irq: pac::Interrupt) -> Result { - if !pin.is_input_pin() { - return Err(InvalidPinTypeError(pin.mode())); - } - Ok(Self { pin, irq }) - } - - /// Asynchronously wait until the pin is high. - /// - /// This returns immediately if the pin is already high. - pub async fn wait_for_high(&mut self) { - // Unwrap okay, checked pin in constructor. - let fut = - InputPinFuture::new_with_dyn_pin(&mut self.pin, self.irq, InterruptEdge::LowToHigh) - .unwrap(); - if self.pin.is_high().unwrap() { - return; - } - fut.await; - } - - /// Asynchronously wait until the pin is low. - /// - /// This returns immediately if the pin is already high. - pub async fn wait_for_low(&mut self) { - // Unwrap okay, checked pin in constructor. - let fut = - InputPinFuture::new_with_dyn_pin(&mut self.pin, self.irq, InterruptEdge::HighToLow) - .unwrap(); - if self.pin.is_low().unwrap() { - return; - } - fut.await; - } - - /// Asynchronously wait until the pin sees a falling edge. - pub async fn wait_for_falling_edge(&mut self) { - // Unwrap okay, checked pin in constructor. - InputPinFuture::new_with_dyn_pin(&mut self.pin, self.irq, InterruptEdge::HighToLow) - .unwrap() - .await; - } - - /// Asynchronously wait until the pin sees a rising edge. - pub async fn wait_for_rising_edge(&mut self) { - // Unwrap okay, checked pin in constructor. - InputPinFuture::new_with_dyn_pin(&mut self.pin, self.irq, InterruptEdge::LowToHigh) - .unwrap() - .await; - } - - /// Asynchronously wait until the pin sees any edge (either rising or falling). - pub async fn wait_for_any_edge(&mut self) { - // Unwrap okay, checked pin in constructor. - InputPinFuture::new_with_dyn_pin(&mut self.pin, self.irq, InterruptEdge::BothEdges) - .unwrap() - .await; - } - - pub fn release(self) -> DynPin { - self.pin - } -} - -impl embedded_hal::digital::ErrorType for InputDynPinAsync { - type Error = core::convert::Infallible; -} - -impl Wait for InputDynPinAsync { - async fn wait_for_high(&mut self) -> Result<(), Self::Error> { - self.wait_for_high().await; - Ok(()) - } - - async fn wait_for_low(&mut self) -> Result<(), Self::Error> { - self.wait_for_low().await; - Ok(()) - } - - async fn wait_for_rising_edge(&mut self) -> Result<(), Self::Error> { - self.wait_for_rising_edge().await; - Ok(()) - } - - async fn wait_for_falling_edge(&mut self) -> Result<(), Self::Error> { - self.wait_for_falling_edge().await; - Ok(()) - } - - async fn wait_for_any_edge(&mut self) -> Result<(), Self::Error> { - self.wait_for_any_edge().await; - Ok(()) - } -} - -pub struct InputPinAsync { - pin: Pin>, - irq: pac::Interrupt, -} - -impl InputPinAsync { - /// Create a new asynchronous input pin from a typed [Pin]. The interrupt ID to be used must be - /// passed as well and is used to route and enable the interrupt. - /// - /// Please note that the interrupt handler itself must be provided by the user and the - /// generic [on_interrupt_for_async_gpio_for_port] function must be called inside that function - /// for the asynchronous functionality to work. - pub fn new(pin: Pin>, irq: pac::Interrupt) -> Self { - Self { pin, irq } - } - - /// Asynchronously wait until the pin is high. - /// - /// This returns immediately if the pin is already high. - pub async fn wait_for_high(&mut self) { - let fut = InputPinFuture::new_with_pin(&mut self.pin, self.irq, InterruptEdge::LowToHigh); - if self.pin.is_high() { - return; - } - fut.await; - } - - /// Asynchronously wait until the pin is low. - /// - /// This returns immediately if the pin is already high. - pub async fn wait_for_low(&mut self) { - let fut = InputPinFuture::new_with_pin(&mut self.pin, self.irq, InterruptEdge::HighToLow); - if self.pin.is_low() { - return; - } - fut.await; - } - - /// Asynchronously wait until the pin sees falling edge. - pub async fn wait_for_falling_edge(&mut self) { - // Unwrap okay, checked pin in constructor. - InputPinFuture::new_with_pin(&mut self.pin, self.irq, InterruptEdge::HighToLow).await; - } - - /// Asynchronously wait until the pin sees rising edge. - pub async fn wait_for_rising_edge(&mut self) { - // Unwrap okay, checked pin in constructor. - InputPinFuture::new_with_pin(&mut self.pin, self.irq, InterruptEdge::LowToHigh).await; - } - - /// Asynchronously wait until the pin sees any edge (either rising or falling). - pub async fn wait_for_any_edge(&mut self) { - InputPinFuture::new_with_pin(&mut self.pin, self.irq, InterruptEdge::BothEdges).await; - } - - pub fn release(self) -> Pin> { - self.pin - } -} -impl embedded_hal::digital::ErrorType for InputPinAsync { - type Error = core::convert::Infallible; -} - -impl Wait for InputPinAsync { - async fn wait_for_high(&mut self) -> Result<(), Self::Error> { - self.wait_for_high().await; - Ok(()) - } - - async fn wait_for_low(&mut self) -> Result<(), Self::Error> { - self.wait_for_low().await; - Ok(()) - } - - async fn wait_for_rising_edge(&mut self) -> Result<(), Self::Error> { - self.wait_for_rising_edge().await; - Ok(()) - } - - async fn wait_for_falling_edge(&mut self) -> Result<(), Self::Error> { - self.wait_for_falling_edge().await; - Ok(()) - } - - async fn wait_for_any_edge(&mut self) -> Result<(), Self::Error> { - self.wait_for_any_edge().await; - Ok(()) - } -} diff --git a/va108xx-hal/src/gpio/mod.rs b/va108xx-hal/src/gpio/mod.rs index be6f024..ce5391b 100644 --- a/va108xx-hal/src/gpio/mod.rs +++ b/va108xx-hal/src/gpio/mod.rs @@ -1,7 +1,9 @@ //! GPIO support module. pub use vorago_shared_periphs::gpio::*; +pub use vorago_shared_periphs::gpio::asynch; /// Low-level GPIO access. pub use vorago_shared_periphs::gpio::ll; -pub mod asynch; +/// GPIO register definitions. +pub use vorago_shared_periphs::gpio::regs; diff --git a/va108xx-hal/src/lib.rs b/va108xx-hal/src/lib.rs index 3aa94c5..ca23b3a 100644 --- a/va108xx-hal/src/lib.rs +++ b/va108xx-hal/src/lib.rs @@ -22,25 +22,9 @@ pub use vorago_shared_periphs::FunSel; /// This is the NONE destination reigster value for the IRQSEL peripheral. pub const IRQ_DST_NONE: u32 = 0xffffffff; -#[derive(Debug, Copy, Clone, PartialEq, Eq)] -#[cfg_attr(feature = "defmt", derive(defmt::Format))] -pub enum PeripheralSelect { - PortA = 0, - PortB = 1, - Spi0 = 4, - Spi1 = 5, - Spi2 = 6, - Uart0 = 8, - Uart1 = 9, - I2c0 = 16, - I2c1 = 17, - Irqsel = 21, - Ioconfig = 22, - Utility = 23, - Gpio = 24, -} - -pub type IrqCfg = InterruptConfig; +pub use vorago_shared_periphs::{ + disable_nvic_interrupt, enable_nvic_interrupt, InterruptConfig, PeripheralSelect, +}; #[derive(Debug, PartialEq, Eq, thiserror::Error)] #[cfg_attr(feature = "defmt", derive(defmt::Format))] @@ -71,24 +55,6 @@ pub fn port_function_select( Ok(()) } -/// Enable a specific interrupt using the NVIC peripheral. -/// -/// # Safety -/// -/// This function is `unsafe` because it can break mask-based critical sections. -#[inline] -pub unsafe fn enable_nvic_interrupt(irq: pac::Interrupt) { - unsafe { - cortex_m::peripheral::NVIC::unmask(irq); - } -} - -/// Disable a specific interrupt using the NVIC peripheral. -#[inline] -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 index 654b887..ce07e71 100644 --- a/va108xx-hal/src/pins.rs +++ b/va108xx-hal/src/pins.rs @@ -1,4 +1,4 @@ -pub use vorago_shared_periphs::gpio::{Pin, PinIdProvider, Port}; +pub use vorago_shared_periphs::gpio::{Pin, PinId, PinMarker, Port}; use crate::{sysconfig::reset_peripheral_for_cycles, PeripheralSelect}; @@ -11,15 +11,14 @@ macro_rules! pin_id { pub enum $Id {} impl $crate::sealed::Sealed for $Id {} - impl PinId for $Id { - const PORT: Port = $Port; - const OFFSET: usize = $num; + impl PinMarker for $Id { + const ID: PinId = PinId::new($Port, $num); } } }; } -impl crate::sealed::Sealed for Pin {} +impl crate::sealed::Sealed for Pin {} pin_id!(Pa0, Port::A, 0); pin_id!(Pa1, Port::A, 1); diff --git a/va108xx-hal/src/spi/pins.rs b/va108xx-hal/src/spi/pins.rs index a2afcb1..590a1bb 100644 --- a/va108xx-hal/src/spi/pins.rs +++ b/va108xx-hal/src/spi/pins.rs @@ -57,10 +57,10 @@ macro_rules! hw_cs_multi_pin { $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), "`]." - )] + "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 { diff --git a/va108xx-hal/src/sysconfig.rs b/va108xx-hal/src/sysconfig.rs index e50abc6..4122e27 100644 --- a/va108xx-hal/src/sysconfig.rs +++ b/va108xx-hal/src/sysconfig.rs @@ -61,3 +61,5 @@ pub fn reset_peripheral_for_cycles(periph_sel: PeripheralSelect, cycles: u32) { cortex_m::asm::delay(cycles); deassert_peripheral_reset(periph_sel); } + +pub use vorago_shared_periphs::sysconfig::{disable_peripheral_clock, enable_peripheral_clock}; diff --git a/va108xx-hal/src/timer.rs b/va108xx-hal/src/timer.rs index f8155c3..d7ca759 100644 --- a/va108xx-hal/src/timer.rs +++ b/va108xx-hal/src/timer.rs @@ -20,7 +20,7 @@ use core::cell::Cell; use critical_section::Mutex; use fugit::RateExtU32; use vorago_shared_periphs::{ - gpio::{Pin, PinId}, + gpio::{Pin, PinMarker}, ioconfig::regs::FunSel, Port, }; @@ -216,10 +216,10 @@ macro_rules! pin_and_tim { ($Px:ident, $FunSel:path, $ID:expr) => { impl TimPin for Pin<$Px> where - $Px: PinId, + $Px: PinMarker, { - const PORT: Port = $Px::PORT; - const OFFSET: usize = $Px::OFFSET; + const PORT: Port = $Px::ID.port(); + const OFFSET: usize = $Px::ID.offset(); const FUN_SEL: FunSel = $FunSel; const TIM_ID: TimId = TimId($ID); } diff --git a/va108xx-hal/src/uart/mod.rs b/va108xx-hal/src/uart/mod.rs index f4bd925..0e6f7d2 100644 --- a/va108xx-hal/src/uart/mod.rs +++ b/va108xx-hal/src/uart/mod.rs @@ -14,9 +14,8 @@ //! - [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}; +use vorago_shared_periphs::{gpio::Pin, FunSel, InterruptConfig}; -pub use crate::InterruptConfig; use crate::{ clock::enable_peripheral_clock, enable_nvic_interrupt, diff --git a/vorago-shared-periphs/src/gpio/asynch.rs b/vorago-shared-periphs/src/gpio/asynch.rs index 3adf288..b8ac438 100644 --- a/vorago-shared-periphs/src/gpio/asynch.rs +++ b/vorago-shared-periphs/src/gpio/asynch.rs @@ -19,7 +19,10 @@ use portable_atomic::AtomicBool; use crate::{InterruptConfig, NUM_PORT_A, NUM_PORT_B}; pub use super::ll::InterruptEdge; -use super::{Input, Pin, PinIdProvider, Port, ll::PinId}; +use super::{ + Input, Port, + ll::{LowLevelGpio, PinId}, +}; static WAKERS_FOR_PORT_A: [AtomicWaker; NUM_PORT_A] = [const { AtomicWaker::new() }; NUM_PORT_A]; static WAKERS_FOR_PORT_B: [AtomicWaker; NUM_PORT_B] = [const { AtomicWaker::new() }; NUM_PORT_B]; @@ -98,15 +101,14 @@ impl InputPinFuture { edge: InterruptEdge, ) -> Self { let (waker_group, edge_detection_group) = - Self::pin_group_to_waker_and_edge_detection_group(pin.port()); - edge_detection_group[pin.offset() as usize] + Self::pin_group_to_waker_and_edge_detection_group(pin.id().port()); + edge_detection_group[pin.id().offset() as usize] .store(false, core::sync::atomic::Ordering::Relaxed); - pin.configure_edge_interrupt(edge).unwrap(); + pin.configure_edge_interrupt(edge); #[cfg(feature = "vor1x")] pin.enable_interrupt(InterruptConfig::new(irq, true, true)); Self { - port: pin.port(), - offset: pin.offset(), + id: pin.id(), waker_group, edge_detection_group, } @@ -115,8 +117,8 @@ impl InputPinFuture { impl Drop for InputPinFuture { fn drop(&mut self) { - // The API ensures that we actually own the pin, so stealing it here is okay. - unsafe { DynPin::steal(self.pin_id) }.disable_interrupt(false); + let mut ll = LowLevelGpio::new(self.id); + ll.disable_interrupt(false); } } @@ -126,7 +128,7 @@ impl Future for InputPinFuture { self: core::pin::Pin<&mut Self>, cx: &mut core::task::Context<'_>, ) -> core::task::Poll { - let idx = self.pin_id.num() as usize; + let idx = self.id.offset() as usize; self.waker_group[idx].register(cx.waker()); if self.edge_detection_group[idx].swap(false, core::sync::atomic::Ordering::Relaxed) { return core::task::Poll::Ready(()); @@ -159,9 +161,8 @@ impl InputPinAsync { pub async fn wait_for_high(&mut self) { // Unwrap okay, checked pin in constructor. let fut = - InputPinFuture::new_with_input_pin(&mut self.pin, self.irq, InterruptEdge::LowToHigh) - .unwrap(); - if self.pin.is_high().unwrap() { + InputPinFuture::new_with_input_pin(&mut self.pin, self.irq, InterruptEdge::LowToHigh); + if self.pin.is_high() { return; } fut.await; @@ -173,9 +174,8 @@ impl InputPinAsync { pub async fn wait_for_low(&mut self) { // Unwrap okay, checked pin in constructor. let fut = - InputPinFuture::new_with_input_pin(&mut self.pin, self.irq, InterruptEdge::HighToLow) - .unwrap(); - if self.pin.is_low().unwrap() { + InputPinFuture::new_with_input_pin(&mut self.pin, self.irq, InterruptEdge::HighToLow); + if self.pin.is_low() { return; } fut.await; @@ -184,25 +184,19 @@ impl InputPinAsync { /// Asynchronously wait until the pin sees a falling edge. pub async fn wait_for_falling_edge(&mut self) { // Unwrap okay, checked pin in constructor. - InputPinFuture::new_with_input_pin(&mut self.pin, self.irq, InterruptEdge::HighToLow) - .unwrap() - .await; + InputPinFuture::new_with_input_pin(&mut self.pin, self.irq, InterruptEdge::HighToLow).await; } /// Asynchronously wait until the pin sees a rising edge. pub async fn wait_for_rising_edge(&mut self) { // Unwrap okay, checked pin in constructor. - InputPinFuture::new_with_input_pin(&mut self.pin, self.irq, InterruptEdge::LowToHigh) - .unwrap() - .await; + InputPinFuture::new_with_input_pin(&mut self.pin, self.irq, InterruptEdge::LowToHigh).await; } /// Asynchronously wait until the pin sees any edge (either rising or falling). pub async fn wait_for_any_edge(&mut self) { // Unwrap okay, checked pin in constructor. - InputPinFuture::new_with_input_pin(&mut self.pin, self.irq, InterruptEdge::BothEdges) - .unwrap() - .await; + InputPinFuture::new_with_input_pin(&mut self.pin, self.irq, InterruptEdge::BothEdges).await; } pub fn release(self) -> Input { @@ -240,93 +234,3 @@ impl Wait for InputPinAsync { Ok(()) } } - -pub struct InputPinAsync { - pin: Pin>, - irq: pac::Interrupt, -} - -impl InputPinAsync { - /// Create a new asynchronous input pin from a typed [Pin]. The interrupt ID to be used must be - /// passed as well and is used to route and enable the interrupt. - /// - /// Please note that the interrupt handler itself must be provided by the user and the - /// generic [on_interrupt_for_async_gpio_for_port] function must be called inside that function - /// for the asynchronous functionality to work. - pub fn new(pin: Pin>, irq: pac::Interrupt) -> Self { - Self { pin, irq } - } - - /// Asynchronously wait until the pin is high. - /// - /// This returns immediately if the pin is already high. - pub async fn wait_for_high(&mut self) { - let fut = InputPinFuture::new_with_pin(&mut self.pin, self.irq, InterruptEdge::LowToHigh); - if self.pin.is_high() { - return; - } - fut.await; - } - - /// Asynchronously wait until the pin is low. - /// - /// This returns immediately if the pin is already high. - pub async fn wait_for_low(&mut self) { - let fut = InputPinFuture::new_with_pin(&mut self.pin, self.irq, InterruptEdge::HighToLow); - if self.pin.is_low() { - return; - } - fut.await; - } - - /// Asynchronously wait until the pin sees falling edge. - pub async fn wait_for_falling_edge(&mut self) { - // Unwrap okay, checked pin in constructor. - InputPinFuture::new_with_pin(&mut self.pin, self.irq, InterruptEdge::HighToLow).await; - } - - /// Asynchronously wait until the pin sees rising edge. - pub async fn wait_for_rising_edge(&mut self) { - // Unwrap okay, checked pin in constructor. - InputPinFuture::new_with_pin(&mut self.pin, self.irq, InterruptEdge::LowToHigh).await; - } - - /// Asynchronously wait until the pin sees any edge (either rising or falling). - pub async fn wait_for_any_edge(&mut self) { - InputPinFuture::new_with_pin(&mut self.pin, self.irq, InterruptEdge::BothEdges).await; - } - - pub fn release(self) -> Pin> { - self.pin - } -} -impl embedded_hal::digital::ErrorType for InputPinAsync { - type Error = core::convert::Infallible; -} - -impl Wait for InputPinAsync { - async fn wait_for_high(&mut self) -> Result<(), Self::Error> { - self.wait_for_high().await; - Ok(()) - } - - async fn wait_for_low(&mut self) -> Result<(), Self::Error> { - self.wait_for_low().await; - Ok(()) - } - - async fn wait_for_rising_edge(&mut self) -> Result<(), Self::Error> { - self.wait_for_rising_edge().await; - Ok(()) - } - - async fn wait_for_falling_edge(&mut self) -> Result<(), Self::Error> { - self.wait_for_falling_edge().await; - Ok(()) - } - - async fn wait_for_any_edge(&mut self) -> Result<(), Self::Error> { - self.wait_for_any_edge().await; - Ok(()) - } -} diff --git a/vorago-shared-periphs/src/gpio/ll.rs b/vorago-shared-periphs/src/gpio/ll.rs index 38ace2c..3c24a4c 100644 --- a/vorago-shared-periphs/src/gpio/ll.rs +++ b/vorago-shared-periphs/src/gpio/ll.rs @@ -1,5 +1,8 @@ pub use embedded_hal::digital::PinState; +#[cfg(feature = "vor1x")] +use crate::{PeripheralSelect, sysconfig::enable_peripheral_clock}; + pub use crate::InvalidOffsetError; pub use crate::Port; pub use crate::ioconfig::regs::Pull; @@ -20,7 +23,7 @@ pub enum InterruptLevel { High = 1, } -#[derive(Debug, PartialEq, Eq)] +#[derive(Debug, PartialEq, Eq, Clone, Copy)] #[cfg_attr(feature = "defmt", derive(defmt::Format))] pub struct PinId { port: Port, @@ -40,7 +43,7 @@ impl PinId { } pub const fn offset(&self) -> usize { - self.port + self.offset } } @@ -64,10 +67,22 @@ impl LowLevelGpio { self.id } + #[inline] + pub fn port(&self) -> Port { + self.id.port() + } + + #[inline] + pub fn offset(&self) -> usize { + self.id.offset() + } + pub fn configure_as_input_floating(&mut self) { unsafe { - self.ioconfig - .modify_pin_config_unchecked(self.port, self.offset, |mut config| { + self.ioconfig.modify_pin_config_unchecked( + self.id.port(), + self.id.offset(), + |mut config| { config.set_funsel(FunSel::Sel0); config.set_io_disable(false); config.set_invert_input(false); @@ -77,18 +92,21 @@ impl LowLevelGpio { config.set_invert_output(false); config.set_input_enable_when_output(false); config - }); + }, + ); } self.gpio.modify_dir(|mut dir| { - dir &= !self.mask_32(); + dir &= !(1 << self.id.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| { + self.ioconfig.modify_pin_config_unchecked( + self.id.port(), + self.id.offset(), + |mut config| { config.set_funsel(FunSel::Sel0); config.set_io_disable(false); config.set_invert_input(false); @@ -99,18 +117,21 @@ impl LowLevelGpio { config.set_invert_output(false); config.set_input_enable_when_output(false); config - }); + }, + ); } self.gpio.modify_dir(|mut dir| { - dir &= !self.mask_32(); + dir &= !(1 << self.id.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| { + self.ioconfig.modify_pin_config_unchecked( + self.id.port(), + self.id.offset(), + |mut config| { config.set_funsel(FunSel::Sel0); config.set_io_disable(false); config.set_invert_input(false); @@ -120,22 +141,25 @@ impl LowLevelGpio { 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), + PinState::Low => self.gpio.write_clr_out(self.mask_32()), + PinState::High => self.gpio.write_set_out(self.mask_32()), } self.gpio.modify_dir(|mut dir| { - dir |= self.mask_32(); + dir |= 1 << self.id.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| { + self.ioconfig.modify_pin_config_unchecked( + self.id.port(), + self.id.offset(), + |mut config| { config.set_funsel(FunSel::Sel0); config.set_io_disable(false); config.set_invert_input(false); @@ -146,22 +170,26 @@ impl LowLevelGpio { config.set_invert_output(false); config.set_input_enable_when_output(true); config - }); + }, + ); } + let mask32 = self.mask_32(); match init_level { - PinState::Low => self.gpio.write_clr_out(1 << self.offset), - PinState::High => self.gpio.write_set_out(1 << self.offset), + PinState::Low => self.gpio.write_clr_out(mask32), + PinState::High => self.gpio.write_set_out(mask32), } self.gpio.modify_dir(|mut dir| { - dir |= 1 << self.offset; + dir |= mask32; 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| { + self.ioconfig.modify_pin_config_unchecked( + self.id.port(), + self.id.offset(), + |mut config| { config.set_funsel(fun_sel); config.set_io_disable(false); config.set_invert_input(false); @@ -170,13 +198,14 @@ impl LowLevelGpio { 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 + (self.gpio.read_data_in() >> self.offset()) & 1 == 1 } #[inline] @@ -196,7 +225,7 @@ impl LowLevelGpio { #[inline] pub fn is_set_high(&self) -> bool { - (self.gpio.read_data_out() >> self.offset) & 1 == 1 + (self.gpio.read_data_out() >> self.offset()) & 1 == 1 } #[inline] @@ -218,7 +247,19 @@ impl LowLevelGpio { unsafe { crate::enable_nvic_interrupt(irq_cfg.id) }; } self.gpio.modify_irq_enb(|mut value| { - value |= self.mask_32(); + value |= 1 << self.id.offset; + value + }); + } + + #[cfg(feature = "vor1x")] + pub fn disable_interrupt(&mut self, reset_irqsel: bool) { + if reset_irqsel { + self.reset_irqsel(); + } + // We only manipulate our own bit. + self.gpio.modify_irq_enb(|mut value| { + value &= !(1 << self.id.offset); value }); } @@ -227,36 +268,75 @@ impl LowLevelGpio { /// When using edge mode, it is possible to generate interrupts on both edges as well #[inline] pub fn configure_edge_interrupt(&mut self, edge_type: InterruptEdge) { - unsafe { - self.gpio.modify_irq_sen(|mut value| { - value &= !self.mask_32(); - value - }); - match edge_type { - InterruptEdge::HighToLow => { - self.gpio.modify_irq_evt(|mut value| { - value &= !self.mask_32(); - value - }); - } - InterruptEdge::LowToHigh => { - self.gpio.modify_irq_evt(|mut value| { - value |= self.mask_32(); - value - }); - } - InterruptEdge::BothEdges => { - self.gpio.modify_irq_edge(|mut value| { - value |= self.mask_32(); - value - }); - } + let mask32 = self.mask_32(); + self.gpio.modify_irq_sen(|mut value| { + value &= !mask32; + value + }); + match edge_type { + InterruptEdge::HighToLow => { + self.gpio.modify_irq_evt(|mut value| { + value &= !mask32; + value + }); + } + InterruptEdge::LowToHigh => { + self.gpio.modify_irq_evt(|mut value| { + value |= mask32; + value + }); + } + InterruptEdge::BothEdges => { + self.gpio.modify_irq_edge(|mut value| { + value |= mask32; + value + }); + } + } + } + + #[cfg(feature = "vor1x")] + /// Configure the IRQSEL peripheral for this particular pin with the given interrupt ID. + pub fn configure_irqsel(&mut self, id: va108xx::Interrupt) { + let irqsel = unsafe { va108xx::Irqsel::steal() }; + enable_peripheral_clock(PeripheralSelect::Irqsel); + match self.id().port() { + // Set the correct interrupt number in the IRQSEL register + super::Port::A => { + irqsel + .porta0(self.id().offset() as usize) + .write(|w| unsafe { w.bits(id as u32) }); + } + super::Port::B => { + irqsel + .portb0(self.id().offset() as usize) + .write(|w| unsafe { w.bits(id as u32) }); + } + } + } + + #[cfg(feature = "vor1x")] + /// Reset the IRQSEL peripheral value for this particular pin. + pub fn reset_irqsel(&mut self) { + let irqsel = unsafe { va108xx::Irqsel::steal() }; + enable_peripheral_clock(PeripheralSelect::Irqsel); + match self.id().port() { + // Set the correct interrupt number in the IRQSEL register + super::Port::A => { + irqsel + .porta0(self.id().offset() as usize) + .write(|w| unsafe { w.bits(u32::MAX) }); + } + super::Port::B => { + irqsel + .portb0(self.id().offset() as usize) + .write(|w| unsafe { w.bits(u32::MAX) }); } } } #[inline(always)] pub const fn mask_32(&self) -> u32 { - 1 << self.offset + 1 << self.id.offset() } } diff --git a/vorago-shared-periphs/src/gpio/mod.rs b/vorago-shared-periphs/src/gpio/mod.rs index 8a6e331..c420d70 100644 --- a/vorago-shared-periphs/src/gpio/mod.rs +++ b/vorago-shared-periphs/src/gpio/mod.rs @@ -1,6 +1,7 @@ use core::convert::Infallible; pub use embedded_hal::digital::PinState; +pub use ll::PinId; pub use ll::{Port, Pull}; use crate::ioconfig::regs::FunSel; @@ -9,15 +10,15 @@ pub mod asynch; pub mod ll; pub mod regs; -pub trait PinIdProvider { +pub trait PinMarker { const ID: ll::PinId; } -pub struct Pin { +pub struct Pin { phantom: core::marker::PhantomData, } -impl Pin { +impl Pin { #[allow(clippy::new_without_default)] pub const fn new() -> Self { Self { @@ -29,7 +30,7 @@ impl Pin { pub struct Output(ll::LowLevelGpio); impl Output { - pub fn new(_pin: Pin, init_level: PinState) -> Self { + pub fn new(_pin: Pin, init_level: PinState) -> Self { let mut ll = ll::LowLevelGpio::new(I::ID); ll.configure_as_output_push_pull(init_level); Output(ll) @@ -101,35 +102,43 @@ impl embedded_hal::digital::StatefulOutputPin for Output { pub struct Input(ll::LowLevelGpio); impl Input { - pub fn new_floating(_pin: Pin) -> Self { + pub fn new_floating(_pin: Pin) -> Self { let mut ll = ll::LowLevelGpio::new(I::ID); ll.configure_as_input_floating(); Input(ll) } - pub fn new_with_pull(_pin: Pin, pull: Pull) -> Self { + pub fn new_with_pull(_pin: Pin, pull: Pull) -> Self { let mut ll = ll::LowLevelGpio::new(I::ID); 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() + pub fn id(&self) -> PinId { + self.0.id() } #[cfg(feature = "vor1x")] + #[inline] pub fn enable_interrupt(&mut self, irq_cfg: crate::InterruptConfig) { self.0.enable_interrupt(irq_cfg); } + + #[inline] pub fn configure_edge_interrupt(&mut self, edge: ll::InterruptEdge) { self.0.configure_edge_interrupt(edge); } + + #[inline] + pub fn is_low(&self) -> bool { + self.0.is_low() + } + + #[inline] + pub fn is_high(&self) -> bool { + self.0.is_high() + } } impl embedded_hal::digital::ErrorType for Input { @@ -169,7 +178,7 @@ pub struct Flex { } impl Flex { - pub fn new(_pin: Pin) -> Self { + pub fn new(_pin: Pin) -> Self { let mut ll = ll::LowLevelGpio::new(I::ID); ll.configure_as_input_floating(); Flex { @@ -278,7 +287,7 @@ pub struct IoPeriphPin { } impl IoPeriphPin { - pub fn new(_pin: Pin, fun_sel: FunSel, pull: Option) -> Self { + pub fn new(_pin: Pin, fun_sel: FunSel, pull: Option) -> Self { let mut ll = ll::LowLevelGpio::new(I::ID); ll.configure_as_peripheral_pin(fun_sel, pull); IoPeriphPin { ll, fun_sel } diff --git a/vorago-shared-periphs/src/ioconfig/regs.rs b/vorago-shared-periphs/src/ioconfig/regs.rs index cdfcc48..7161fb5 100644 --- a/vorago-shared-periphs/src/ioconfig/regs.rs +++ b/vorago-shared-periphs/src/ioconfig/regs.rs @@ -167,7 +167,7 @@ impl MmioIoConfig<'_> { } } - pub fn modify_pin_config Config>( + pub fn modify_pin_config Config>( &mut self, port: crate::Port, offset: usize, @@ -185,7 +185,7 @@ impl MmioIoConfig<'_> { /// # Safety /// /// Calling this function with an invalid offset can lead to undefined behaviour. - pub unsafe fn modify_pin_config_unchecked Config>( + pub unsafe fn modify_pin_config_unchecked Config>( &mut self, port: crate::Port, offset: usize, diff --git a/vorago-shared-periphs/src/lib.rs b/vorago-shared-periphs/src/lib.rs index f1ecc1c..91c6ae1 100644 --- a/vorago-shared-periphs/src/lib.rs +++ b/vorago-shared-periphs/src/lib.rs @@ -1,12 +1,32 @@ #![no_std] pub mod gpio; pub mod ioconfig; +pub mod sysconfig; #[cfg(not(feature = "_family-selected"))] compile_error!("no Vorago CPU family was select. Choices: vor1x or vor4x"); pub use ioconfig::regs::FunSel; +#[cfg(feature = "vor1x")] +#[derive(Debug, Copy, Clone, PartialEq, Eq)] +#[cfg_attr(feature = "defmt", derive(defmt::Format))] +pub enum PeripheralSelect { + PortA = 0, + PortB = 1, + Spi0 = 4, + Spi1 = 5, + Spi2 = 6, + Uart0 = 8, + Uart1 = 9, + I2c0 = 16, + I2c1 = 17, + Irqsel = 21, + Ioconfig = 22, + Utility = 23, + Gpio = 24, +} + cfg_if::cfg_if! { if #[cfg(feature = "vor1x")] { /// Number of GPIO ports and IOCONFIG registers for PORT A @@ -43,7 +63,7 @@ pub enum Port { } impl Port { - pub fn max_offset(&self) -> usize { + pub const fn max_offset(&self) -> usize { match self { Port::A => NUM_PORT_A, Port::B => NUM_PORT_B, @@ -60,7 +80,7 @@ impl Port { /// /// Circumvents ownership and safety guarantees by the HAL. pub unsafe fn steal_gpio(&self) -> gpio::regs::MmioGpio<'static> { - gpio::regs::Gpio::new_mmio(self) + gpio::regs::Gpio::new_mmio(*self) } } diff --git a/vorago-shared-periphs/src/sysconfig.rs b/vorago-shared-periphs/src/sysconfig.rs new file mode 100644 index 0000000..317e503 --- /dev/null +++ b/vorago-shared-periphs/src/sysconfig.rs @@ -0,0 +1,17 @@ +#[cfg(feature = "vor1x")] +#[inline] +pub fn enable_peripheral_clock(clock: crate::PeripheralSelect) { + let syscfg = unsafe { va108xx::Sysconfig::steal() }; + syscfg + .peripheral_clk_enable() + .modify(|r, w| unsafe { w.bits(r.bits() | (1 << clock as u8)) }); +} + +#[cfg(feature = "vor1x")] +#[inline] +pub fn disable_peripheral_clock(clock: crate::PeripheralSelect) { + let syscfg = unsafe { va108xx::Sysconfig::steal() }; + syscfg + .peripheral_clk_enable() + .modify(|r, w| unsafe { w.bits(r.bits() & !(1 << clock as u8)) }); +}