diff --git a/embassy-macros/src/lib.rs b/embassy-macros/src/lib.rs index b11fc4ae..c2e2d9e2 100644 --- a/embassy-macros/src/lib.rs +++ b/embassy-macros/src/lib.rs @@ -98,3 +98,65 @@ pub fn task(args: TokenStream, item: TokenStream) -> TokenStream { }; result.into() } + +#[proc_macro] +pub fn interrupt_declare(item: TokenStream) -> TokenStream { + let name = syn::parse_macro_input!(item as syn::Ident); + let name = format_ident!("{}", name); + let name_interrupt = format_ident!("{}Interrupt", name); + let name_handler = format!("__EMBASSY_{}_HANDLER", name); + + let result = quote! { + #[allow(non_camel_case_types)] + pub struct #name_interrupt(()); + unsafe impl OwnedInterrupt for #name_interrupt { + type Priority = Priority; + fn number(&self) -> u8 { + Interrupt::#name as u8 + } + unsafe fn __handler(&self) -> &'static ::core::sync::atomic::AtomicPtr { + #[export_name = #name_handler] + static HANDLER: ::core::sync::atomic::AtomicPtr = ::core::sync::atomic::AtomicPtr::new(::core::ptr::null_mut()); + &HANDLER + } + } + }; + result.into() +} + +#[proc_macro] +pub fn interrupt_take(item: TokenStream) -> TokenStream { + let name = syn::parse_macro_input!(item as syn::Ident); + let name = format!("{}", name); + let name_interrupt = format_ident!("{}Interrupt", name); + let name_handler = format!("__EMBASSY_{}_HANDLER", name); + + let result = quote! { + { + #[allow(non_snake_case)] + #[export_name = #name] + pub unsafe extern "C" fn trampoline() { + extern "C" { + #[link_name = #name_handler] + static HANDLER: ::core::sync::atomic::AtomicPtr; + } + + let p = HANDLER.load(::core::sync::atomic::Ordering::Acquire); + if !p.is_null() { + let f: fn() = ::core::mem::transmute(p); + f() + } + } + + static TAKEN: ::core::sync::atomic::AtomicBool = ::core::sync::atomic::AtomicBool::new(false); + + if TAKEN.compare_exchange(false, true, ::core::sync::atomic::Ordering::AcqRel, ::core::sync::atomic::Ordering::Acquire).is_err() { + panic!("IRQ Already taken"); + } + + let irq: interrupt::#name_interrupt = unsafe { ::core::mem::transmute(()) }; + irq + } + }; + result.into() +} diff --git a/embassy-nrf/Cargo.toml b/embassy-nrf/Cargo.toml index 7fe285c6..eb5370ea 100644 --- a/embassy-nrf/Cargo.toml +++ b/embassy-nrf/Cargo.toml @@ -26,6 +26,7 @@ log = { version = "0.4.11", optional = true } cortex-m-rt = "0.6.13" cortex-m = { version = "0.6.4" } embedded-hal = { version = "0.2.4" } +embedded-dma = { version = "0.1.2" } nrf52810-pac = { version = "0.9.0", optional = true } nrf52811-pac = { version = "0.9.1", optional = true } diff --git a/embassy-nrf/src/buffered_uarte.rs b/embassy-nrf/src/buffered_uarte.rs new file mode 100644 index 00000000..db6a83fb --- /dev/null +++ b/embassy-nrf/src/buffered_uarte.rs @@ -0,0 +1,557 @@ +//! HAL interface to the UARTE peripheral +//! +//! See product specification: +//! +//! - nrf52832: Section 35 +//! - nrf52840: Section 6.34 +use core::cell::UnsafeCell; +use core::cmp::min; +use core::marker::PhantomPinned; +use core::ops::Deref; +use core::pin::Pin; +use core::ptr; +use core::sync::atomic::{compiler_fence, Ordering}; +use core::task::{Context, Poll}; + +use embedded_hal::digital::v2::OutputPin; + +use crate::hal::gpio::{Floating, Input, Output, Pin as GpioPin, Port as GpioPort, PushPull}; +use crate::interrupt; +use crate::interrupt::{CriticalSection, OwnedInterrupt}; +#[cfg(any(feature = "52833", feature = "52840", feature = "9160"))] +use crate::pac::UARTE1; +use crate::pac::{uarte0, UARTE0}; + +// Re-export SVD variants to allow user to directly set values +pub use uarte0::{baudrate::BAUDRATE_A as Baudrate, config::PARITY_A as Parity}; + +use embassy::io::{AsyncBufRead, AsyncWrite, Result}; +use embassy::util::WakerStore; + +use crate::fmt::{assert, panic, todo, *}; + +//use crate::trace; + +const RINGBUF_SIZE: usize = 512; +struct RingBuf { + buf: [u8; RINGBUF_SIZE], + start: usize, + end: usize, + empty: bool, +} + +impl RingBuf { + fn new() -> Self { + RingBuf { + buf: [0; RINGBUF_SIZE], + start: 0, + end: 0, + empty: true, + } + } + + fn push_buf(&mut self) -> &mut [u8] { + if self.start == self.end && !self.empty { + trace!(" ringbuf: push_buf empty"); + return &mut self.buf[..0]; + } + + let n = if self.start <= self.end { + RINGBUF_SIZE - self.end + } else { + self.start - self.end + }; + + trace!(" ringbuf: push_buf {:?}..{:?}", self.end, self.end + n); + &mut self.buf[self.end..self.end + n] + } + + fn push(&mut self, n: usize) { + trace!(" ringbuf: push {:?}", n); + if n == 0 { + return; + } + + self.end = Self::wrap(self.end + n); + self.empty = false; + } + + fn pop_buf(&mut self) -> &mut [u8] { + if self.empty { + trace!(" ringbuf: pop_buf empty"); + return &mut self.buf[..0]; + } + + let n = if self.end <= self.start { + RINGBUF_SIZE - self.start + } else { + self.end - self.start + }; + + trace!(" ringbuf: pop_buf {:?}..{:?}", self.start, self.start + n); + &mut self.buf[self.start..self.start + n] + } + + fn pop(&mut self, n: usize) { + trace!(" ringbuf: pop {:?}", n); + if n == 0 { + return; + } + + self.start = Self::wrap(self.start + n); + self.empty = self.start == self.end; + } + + fn wrap(n: usize) -> usize { + assert!(n <= RINGBUF_SIZE); + if n == RINGBUF_SIZE { + 0 + } else { + n + } + } +} + +#[derive(Copy, Clone, Debug, PartialEq)] +enum RxState { + Idle, + Receiving, + ReceivingReady, + Stopping, +} +#[derive(Copy, Clone, Debug, PartialEq)] +enum TxState { + Idle, + Transmitting(usize), +} + +/// Interface to a UARTE instance +/// +/// This is a very basic interface that comes with the following limitations: +/// - The UARTE instances share the same address space with instances of UART. +/// You need to make sure that conflicting instances +/// are disabled before using `Uarte`. See product specification: +/// - nrf52832: Section 15.2 +/// - nrf52840: Section 6.1.2 +pub struct BufferedUarte { + started: bool, + state: UnsafeCell>, +} + +// public because it needs to be used in Instance::{get_state, set_state}, but +// should not be used outside the module +#[doc(hidden)] +pub struct UarteState { + inner: T, + irq: T::Interrupt, + + rx: RingBuf, + rx_state: RxState, + rx_waker: WakerStore, + + tx: RingBuf, + tx_state: TxState, + tx_waker: WakerStore, + + _pin: PhantomPinned, +} + +#[cfg(any(feature = "52833", feature = "52840"))] +fn port_bit(port: GpioPort) -> bool { + match port { + GpioPort::Port0 => false, + GpioPort::Port1 => true, + } +} + +impl BufferedUarte { + pub fn new( + uarte: T, + irq: T::Interrupt, + mut pins: Pins, + parity: Parity, + baudrate: Baudrate, + ) -> Self { + // Select pins + uarte.psel.rxd.write(|w| { + let w = unsafe { w.pin().bits(pins.rxd.pin()) }; + #[cfg(any(feature = "52833", feature = "52840"))] + let w = w.port().bit(port_bit(pins.rxd.port())); + w.connect().connected() + }); + pins.txd.set_high().unwrap(); + uarte.psel.txd.write(|w| { + let w = unsafe { w.pin().bits(pins.txd.pin()) }; + #[cfg(any(feature = "52833", feature = "52840"))] + let w = w.port().bit(port_bit(pins.txd.port())); + w.connect().connected() + }); + + // Optional pins + uarte.psel.cts.write(|w| { + if let Some(ref pin) = pins.cts { + let w = unsafe { w.pin().bits(pin.pin()) }; + #[cfg(any(feature = "52833", feature = "52840"))] + let w = w.port().bit(port_bit(pin.port())); + w.connect().connected() + } else { + w.connect().disconnected() + } + }); + + uarte.psel.rts.write(|w| { + if let Some(ref pin) = pins.rts { + let w = unsafe { w.pin().bits(pin.pin()) }; + #[cfg(any(feature = "52833", feature = "52840"))] + let w = w.port().bit(port_bit(pin.port())); + w.connect().connected() + } else { + w.connect().disconnected() + } + }); + + // Enable UARTE instance + uarte.enable.write(|w| w.enable().enabled()); + + // Enable interrupts + uarte.intenset.write(|w| w.endrx().set().endtx().set()); + + // Configure + let hardware_flow_control = pins.rts.is_some() && pins.cts.is_some(); + uarte + .config + .write(|w| w.hwfc().bit(hardware_flow_control).parity().variant(parity)); + + // Configure frequency + uarte.baudrate.write(|w| w.baudrate().variant(baudrate)); + + BufferedUarte { + started: false, + state: UnsafeCell::new(UarteState { + inner: uarte, + irq, + + rx: RingBuf::new(), + rx_state: RxState::Idle, + rx_waker: WakerStore::new(), + + tx: RingBuf::new(), + tx_state: TxState::Idle, + tx_waker: WakerStore::new(), + + _pin: PhantomPinned, + }), + } + } + + fn with_state<'a, R>( + self: Pin<&'a mut Self>, + f: impl FnOnce(Pin<&'a mut UarteState>) -> R, + ) -> R { + let Self { state, started } = unsafe { self.get_unchecked_mut() }; + + interrupt::free(|cs| { + let ptr = state.get(); + + if !*started { + T::set_state(cs, ptr); + + *started = true; + + // safety: safe because critical section ensures only one *mut UartState + // exists at the same time. + unsafe { Pin::new_unchecked(&mut *ptr) }.start(); + } + + // safety: safe because critical section ensures only one *mut UartState + // exists at the same time. + f(unsafe { Pin::new_unchecked(&mut *ptr) }) + }) + } +} + +impl Drop for BufferedUarte { + fn drop(&mut self) { + // stop DMA before dropping, because DMA is using the buffer in `self`. + todo!() + } +} + +impl AsyncBufRead for BufferedUarte { + fn poll_fill_buf(self: Pin<&mut Self>, cx: &mut Context<'_>) -> Poll> { + self.with_state(|s| s.poll_fill_buf(cx)) + } + + fn consume(self: Pin<&mut Self>, amt: usize) { + self.with_state(|s| s.consume(amt)) + } +} + +impl AsyncWrite for BufferedUarte { + fn poll_write(self: Pin<&mut Self>, cx: &mut Context<'_>, buf: &[u8]) -> Poll> { + self.with_state(|s| s.poll_write(cx, buf)) + } +} + +impl UarteState { + pub fn start(self: Pin<&mut Self>) { + self.irq.set_handler(|| unsafe { + interrupt::free(|cs| T::get_state(cs).as_mut().unwrap().on_interrupt()); + }); + + self.irq.pend(); + self.irq.enable(); + } + + fn poll_fill_buf(self: Pin<&mut Self>, cx: &mut Context<'_>) -> Poll> { + let this = unsafe { self.get_unchecked_mut() }; + + // Conservative compiler fence to prevent optimizations that do not + // take in to account actions by DMA. The fence has been placed here, + // before any DMA action has started + compiler_fence(Ordering::SeqCst); + trace!("poll_read"); + + // We have data ready in buffer? Return it. + let buf = this.rx.pop_buf(); + if buf.len() != 0 { + trace!(" got {:?} {:?}", buf.as_ptr() as u32, buf.len()); + return Poll::Ready(Ok(buf)); + } + + trace!(" empty"); + + if this.rx_state == RxState::ReceivingReady { + trace!(" stopping"); + this.rx_state = RxState::Stopping; + this.inner.tasks_stoprx.write(|w| unsafe { w.bits(1) }); + } + + this.rx_waker.store(cx.waker()); + Poll::Pending + } + + fn consume(self: Pin<&mut Self>, amt: usize) { + let this = unsafe { self.get_unchecked_mut() }; + trace!("consume {:?}", amt); + this.rx.pop(amt); + this.irq.pend(); + } + + fn poll_write(self: Pin<&mut Self>, cx: &mut Context<'_>, buf: &[u8]) -> Poll> { + let this = unsafe { self.get_unchecked_mut() }; + + trace!("poll_write: {:?}", buf.len()); + + let tx_buf = this.tx.push_buf(); + if tx_buf.len() == 0 { + trace!("poll_write: pending"); + this.tx_waker.store(cx.waker()); + return Poll::Pending; + } + + let n = min(tx_buf.len(), buf.len()); + tx_buf[..n].copy_from_slice(&buf[..n]); + this.tx.push(n); + + trace!("poll_write: queued {:?}", n); + + // Conservative compiler fence to prevent optimizations that do not + // take in to account actions by DMA. The fence has been placed here, + // before any DMA action has started + compiler_fence(Ordering::SeqCst); + + this.irq.pend(); + + Poll::Ready(Ok(n)) + } + + fn on_interrupt(&mut self) { + trace!("irq: start"); + let mut more_work = true; + while more_work { + more_work = false; + match self.rx_state { + RxState::Idle => { + trace!(" irq_rx: in state idle"); + + if self.inner.events_rxdrdy.read().bits() != 0 { + trace!(" irq_rx: rxdrdy?????"); + self.inner.events_rxdrdy.reset(); + } + + if self.inner.events_endrx.read().bits() != 0 { + panic!("unexpected endrx"); + } + + let buf = self.rx.push_buf(); + if buf.len() != 0 { + trace!(" irq_rx: starting {:?}", buf.len()); + self.rx_state = RxState::Receiving; + + // Set up the DMA read + self.inner.rxd.ptr.write(|w| + // The PTR field is a full 32 bits wide and accepts the full range + // of values. + unsafe { w.ptr().bits(buf.as_ptr() as u32) }); + self.inner.rxd.maxcnt.write(|w| + // We're giving it the length of the buffer, so no danger of + // accessing invalid memory. We have verified that the length of the + // buffer fits in an `u8`, so the cast to `u8` is also fine. + // + // The MAXCNT field is at least 8 bits wide and accepts the full + // range of values. + unsafe { w.maxcnt().bits(buf.len() as _) }); + trace!(" irq_rx: buf {:?} {:?}", buf.as_ptr() as u32, buf.len()); + + // Enable RXRDY interrupt. + self.inner.events_rxdrdy.reset(); + self.inner.intenset.write(|w| w.rxdrdy().set()); + + // Start UARTE Receive transaction + self.inner.tasks_startrx.write(|w| + // `1` is a valid value to write to task registers. + unsafe { w.bits(1) }); + } + } + RxState::Receiving => { + trace!(" irq_rx: in state receiving"); + if self.inner.events_rxdrdy.read().bits() != 0 { + trace!(" irq_rx: rxdrdy"); + + // Disable the RXRDY event interrupt + // RXRDY is triggered for every byte, but we only care about whether we have + // some bytes or not. So as soon as we have at least one, disable it, to avoid + // wasting CPU cycles in interrupts. + self.inner.intenclr.write(|w| w.rxdrdy().clear()); + + self.inner.events_rxdrdy.reset(); + + self.rx_waker.wake(); + self.rx_state = RxState::ReceivingReady; + more_work = true; // in case we also have endrx pending + } + } + RxState::ReceivingReady | RxState::Stopping => { + trace!(" irq_rx: in state ReceivingReady"); + + if self.inner.events_rxdrdy.read().bits() != 0 { + trace!(" irq_rx: rxdrdy"); + self.inner.events_rxdrdy.reset(); + } + + if self.inner.events_endrx.read().bits() != 0 { + let n: usize = self.inner.rxd.amount.read().amount().bits() as usize; + trace!(" irq_rx: endrx {:?}", n); + self.rx.push(n); + + self.inner.events_endrx.reset(); + + self.rx_waker.wake(); + self.rx_state = RxState::Idle; + more_work = true; // start another rx if possible + } + } + } + } + + more_work = true; + while more_work { + more_work = false; + match self.tx_state { + TxState::Idle => { + trace!(" irq_tx: in state Idle"); + let buf = self.tx.pop_buf(); + if buf.len() != 0 { + trace!(" irq_tx: starting {:?}", buf.len()); + self.tx_state = TxState::Transmitting(buf.len()); + + // Set up the DMA write + self.inner.txd.ptr.write(|w| + // The PTR field is a full 32 bits wide and accepts the full range + // of values. + unsafe { w.ptr().bits(buf.as_ptr() as u32) }); + self.inner.txd.maxcnt.write(|w| + // We're giving it the length of the buffer, so no danger of + // accessing invalid memory. We have verified that the length of the + // buffer fits in an `u8`, so the cast to `u8` is also fine. + // + // The MAXCNT field is 8 bits wide and accepts the full range of + // values. + unsafe { w.maxcnt().bits(buf.len() as _) }); + + // Start UARTE Transmit transaction + self.inner.tasks_starttx.write(|w| + // `1` is a valid value to write to task registers. + unsafe { w.bits(1) }); + } + } + TxState::Transmitting(n) => { + trace!(" irq_tx: in state Transmitting"); + if self.inner.events_endtx.read().bits() != 0 { + self.inner.events_endtx.reset(); + + trace!(" irq_tx: endtx {:?}", n); + self.tx.pop(n); + self.tx_waker.wake(); + self.tx_state = TxState::Idle; + more_work = true; // start another tx if possible + } + } + } + } + trace!("irq: end"); + } +} + +pub struct Pins { + pub rxd: GpioPin>, + pub txd: GpioPin>, + pub cts: Option>>, + pub rts: Option>>, +} + +mod private { + pub trait Sealed {} + + impl Sealed for crate::pac::UARTE0 {} + #[cfg(any(feature = "52833", feature = "52840", feature = "9160"))] + impl Sealed for crate::pac::UARTE1 {} +} + +pub trait Instance: Deref + Sized + private::Sealed { + type Interrupt: OwnedInterrupt; + + #[doc(hidden)] + fn get_state(_cs: &CriticalSection) -> *mut UarteState; + + #[doc(hidden)] + fn set_state(_cs: &CriticalSection, state: *mut UarteState); +} + +static mut UARTE0_STATE: *mut UarteState = ptr::null_mut(); +#[cfg(any(feature = "52833", feature = "52840", feature = "9160"))] +static mut UARTE1_STATE: *mut UarteState = ptr::null_mut(); + +impl Instance for UARTE0 { + type Interrupt = interrupt::UARTE0_UART0Interrupt; + + fn get_state(_cs: &CriticalSection) -> *mut UarteState { + unsafe { UARTE0_STATE } // Safe because of CriticalSection + } + fn set_state(_cs: &CriticalSection, state: *mut UarteState) { + unsafe { UARTE0_STATE = state } // Safe because of CriticalSection + } +} + +#[cfg(any(feature = "52833", feature = "52840", feature = "9160"))] +impl Instance for UARTE1 { + type Interrupt = interrupt::UARTE1Interrupt; + + fn get_state(_cs: &CriticalSection) -> *mut UarteState { + unsafe { UARTE1_STATE } // Safe because of CriticalSection + } + fn set_state(_cs: &CriticalSection, state: *mut UarteState) { + unsafe { UARTE1_STATE = state } // Safe because of CriticalSection + } +} diff --git a/embassy-nrf/src/gpiote.rs b/embassy-nrf/src/gpiote.rs index 069e9140..353f6a94 100644 --- a/embassy-nrf/src/gpiote.rs +++ b/embassy-nrf/src/gpiote.rs @@ -7,6 +7,7 @@ use embassy::util::Signal; use crate::hal::gpio::{Input, Level, Output, Pin, Port}; use crate::interrupt; +use crate::interrupt::OwnedInterrupt; use crate::pac::generic::Reg; use crate::pac::gpiote::_TASKS_OUT; #[cfg(any(feature = "52833", feature = "52840"))] @@ -58,7 +59,7 @@ pub enum NewChannelError { } impl Gpiote { - pub fn new(gpiote: GPIOTE) -> Self { + pub fn new(gpiote: GPIOTE, irq: interrupt::GPIOTEInterrupt) -> Self { #[cfg(any(feature = "52833", feature = "52840"))] let ports = unsafe { &[&*P0::ptr(), &*P1::ptr()] }; #[cfg(not(any(feature = "52833", feature = "52840")))] @@ -74,8 +75,9 @@ impl Gpiote { // Enable interrupts gpiote.events_port.write(|w| w); gpiote.intenset.write(|w| w.port().set()); - interrupt::unpend(interrupt::GPIOTE); - interrupt::enable(interrupt::GPIOTE); + irq.set_handler(Self::on_irq); + irq.unpend(); + irq.enable(); Self { inner: gpiote, @@ -293,6 +295,39 @@ impl Gpiote { }) }) } + + unsafe fn on_irq() { + let s = &(*INSTANCE); + + for i in 0..8 { + if s.inner.events_in[i].read().bits() != 0 { + s.inner.events_in[i].write(|w| w); + s.channel_signals[i].signal(()); + } + } + + if s.inner.events_port.read().bits() != 0 { + s.inner.events_port.write(|w| w); + + #[cfg(any(feature = "52833", feature = "52840"))] + let ports = &[&*P0::ptr(), &*P1::ptr()]; + #[cfg(not(any(feature = "52833", feature = "52840")))] + let ports = &[&*P0::ptr()]; + + let mut work = true; + while work { + work = false; + for (port, &p) in ports.iter().enumerate() { + for pin in BitIter(p.latch.read().bits()) { + work = true; + p.pin_cnf[pin as usize].modify(|_, w| w.sense().disabled()); + p.latch.write(|w| w.bits(1 << pin)); + s.port_signals[port * 32 + pin as usize].signal(()); + } + } + } + } + } } pub struct PortInputFuture<'a, T> { @@ -413,40 +448,6 @@ impl<'a> OutputChannel<'a> { } } -#[interrupt] -unsafe fn GPIOTE() { - let s = &(*INSTANCE); - - for i in 0..8 { - if s.inner.events_in[i].read().bits() != 0 { - s.inner.events_in[i].write(|w| w); - s.channel_signals[i].signal(()); - } - } - - if s.inner.events_port.read().bits() != 0 { - s.inner.events_port.write(|w| w); - - #[cfg(any(feature = "52833", feature = "52840"))] - let ports = &[&*P0::ptr(), &*P1::ptr()]; - #[cfg(not(any(feature = "52833", feature = "52840")))] - let ports = &[&*P0::ptr()]; - - let mut work = true; - while work { - work = false; - for (port, &p) in ports.iter().enumerate() { - for pin in BitIter(p.latch.read().bits()) { - work = true; - p.pin_cnf[pin as usize].modify(|_, w| w.sense().disabled()); - p.latch.write(|w| w.bits(1 << pin)); - s.port_signals[port * 32 + pin as usize].signal(()); - } - } - } - } -} - struct BitIter(u32); impl Iterator for BitIter { diff --git a/embassy-nrf/src/interrupt.rs b/embassy-nrf/src/interrupt.rs index 17fc9ab3..90b56857 100644 --- a/embassy-nrf/src/interrupt.rs +++ b/embassy-nrf/src/interrupt.rs @@ -5,12 +5,13 @@ use core::sync::atomic::{compiler_fence, Ordering}; -use crate::pac::{NVIC, NVIC_PRIO_BITS}; +use crate::pac::NVIC_PRIO_BITS; // Re-exports pub use crate::pac::Interrupt; pub use crate::pac::Interrupt::*; // needed for cortex-m-rt #[interrupt] pub use cortex_m::interrupt::{CriticalSection, Mutex}; +pub use embassy::interrupt::{declare, take, OwnedInterrupt}; #[derive(Debug, Copy, Clone, Eq, PartialEq, Ord, PartialOrd)] #[cfg_attr(feature = "defmt", derive(defmt::Format))] @@ -26,14 +27,8 @@ pub enum Priority { Level7 = 7, } -impl Priority { - #[inline] - fn to_nvic(self) -> u8 { - (self as u8) << (8 - NVIC_PRIO_BITS) - } - - #[inline] - fn from_nvic(priority: u8) -> Self { +impl From for Priority { + fn from(priority: u8) -> Self { match priority >> (8 - NVIC_PRIO_BITS) { 0 => Self::Level0, 1 => Self::Level1, @@ -48,6 +43,12 @@ impl Priority { } } +impl From for u8 { + fn from(p: Priority) -> Self { + (p as u8) << (8 - NVIC_PRIO_BITS) + } +} + #[inline] pub fn free(f: F) -> R where @@ -77,53 +78,204 @@ where } } -#[inline] -pub fn enable(irq: Interrupt) { - unsafe { - NVIC::unmask(irq); - } +#[cfg(feature = "52810")] +mod irqs { + use super::*; + declare!(POWER_CLOCK); + declare!(RADIO); + declare!(UARTE0_UART0); + declare!(TWIM0_TWIS0_TWI0); + declare!(SPIM0_SPIS0_SPI0); + declare!(GPIOTE); + declare!(SAADC); + declare!(TIMER0); + declare!(TIMER1); + declare!(TIMER2); + declare!(RTC0); + declare!(TEMP); + declare!(RNG); + declare!(ECB); + declare!(CCM_AAR); + declare!(WDT); + declare!(RTC1); + declare!(QDEC); + declare!(COMP); + declare!(SWI0_EGU0); + declare!(SWI1_EGU1); + declare!(SWI2); + declare!(SWI3); + declare!(SWI4); + declare!(SWI5); + declare!(PWM0); + declare!(PDM); } -#[inline] -pub fn disable(irq: Interrupt) { - NVIC::mask(irq); +#[cfg(feature = "52811")] +mod irqs { + use super::*; + declare!(POWER_CLOCK); + declare!(RADIO); + declare!(UARTE0_UART0); + declare!(TWIM0_TWIS0_TWI0_SPIM1_SPIS1_SPI1); + declare!(SPIM0_SPIS0_SPI0); + declare!(GPIOTE); + declare!(SAADC); + declare!(TIMER0); + declare!(TIMER1); + declare!(TIMER2); + declare!(RTC0); + declare!(TEMP); + declare!(RNG); + declare!(ECB); + declare!(CCM_AAR); + declare!(WDT); + declare!(RTC1); + declare!(QDEC); + declare!(COMP); + declare!(SWI0_EGU0); + declare!(SWI1_EGU1); + declare!(SWI2); + declare!(SWI3); + declare!(SWI4); + declare!(SWI5); + declare!(PWM0); + declare!(PDM); } -#[inline] -pub fn is_active(irq: Interrupt) -> bool { - NVIC::is_active(irq) +#[cfg(feature = "52832")] +mod irqs { + use super::*; + declare!(POWER_CLOCK); + declare!(RADIO); + declare!(UARTE0_UART0); + declare!(SPIM0_SPIS0_TWIM0_TWIS0_SPI0_TWI0); + declare!(SPIM1_SPIS1_TWIM1_TWIS1_SPI1_TWI1); + declare!(NFCT); + declare!(GPIOTE); + declare!(SAADC); + declare!(TIMER0); + declare!(TIMER1); + declare!(TIMER2); + declare!(RTC0); + declare!(TEMP); + declare!(RNG); + declare!(ECB); + declare!(CCM_AAR); + declare!(WDT); + declare!(RTC1); + declare!(QDEC); + declare!(COMP_LPCOMP); + declare!(SWI0_EGU0); + declare!(SWI1_EGU1); + declare!(SWI2_EGU2); + declare!(SWI3_EGU3); + declare!(SWI4_EGU4); + declare!(SWI5_EGU5); + declare!(TIMER3); + declare!(TIMER4); + declare!(PWM0); + declare!(PDM); + declare!(MWU); + declare!(PWM1); + declare!(PWM2); + declare!(SPIM2_SPIS2_SPI2); + declare!(RTC2); + declare!(I2S); + declare!(FPU); } -#[inline] -pub fn is_enabled(irq: Interrupt) -> bool { - NVIC::is_enabled(irq) +#[cfg(feature = "52833")] +mod irqs { + use super::*; + declare!(POWER_CLOCK); + declare!(RADIO); + declare!(UARTE0_UART0); + declare!(SPIM0_SPIS0_TWIM0_TWIS0_SPI0_TWI0); + declare!(SPIM1_SPIS1_TWIM1_TWIS1_SPI1_TWI1); + declare!(NFCT); + declare!(GPIOTE); + declare!(SAADC); + declare!(TIMER0); + declare!(TIMER1); + declare!(TIMER2); + declare!(RTC0); + declare!(TEMP); + declare!(RNG); + declare!(ECB); + declare!(CCM_AAR); + declare!(WDT); + declare!(RTC1); + declare!(QDEC); + declare!(COMP_LPCOMP); + declare!(SWI0_EGU0); + declare!(SWI1_EGU1); + declare!(SWI2_EGU2); + declare!(SWI3_EGU3); + declare!(SWI4_EGU4); + declare!(SWI5_EGU5); + declare!(TIMER3); + declare!(TIMER4); + declare!(PWM0); + declare!(PDM); + declare!(MWU); + declare!(PWM1); + declare!(PWM2); + declare!(SPIM2_SPIS2_SPI2); + declare!(RTC2); + declare!(I2S); + declare!(FPU); + declare!(USBD); + declare!(UARTE1); + declare!(PWM3); + declare!(SPIM3); } -#[inline] -pub fn is_pending(irq: Interrupt) -> bool { - NVIC::is_pending(irq) +#[cfg(feature = "52840")] +mod irqs { + use super::*; + declare!(POWER_CLOCK); + declare!(RADIO); + declare!(UARTE0_UART0); + declare!(SPIM0_SPIS0_TWIM0_TWIS0_SPI0_TWI0); + declare!(SPIM1_SPIS1_TWIM1_TWIS1_SPI1_TWI1); + declare!(NFCT); + declare!(GPIOTE); + declare!(SAADC); + declare!(TIMER0); + declare!(TIMER1); + declare!(TIMER2); + declare!(RTC0); + declare!(TEMP); + declare!(RNG); + declare!(ECB); + declare!(CCM_AAR); + declare!(WDT); + declare!(RTC1); + declare!(QDEC); + declare!(COMP_LPCOMP); + declare!(SWI0_EGU0); + declare!(SWI1_EGU1); + declare!(SWI2_EGU2); + declare!(SWI3_EGU3); + declare!(SWI4_EGU4); + declare!(SWI5_EGU5); + declare!(TIMER3); + declare!(TIMER4); + declare!(PWM0); + declare!(PDM); + declare!(MWU); + declare!(PWM1); + declare!(PWM2); + declare!(SPIM2_SPIS2_SPI2); + declare!(RTC2); + declare!(I2S); + declare!(FPU); + declare!(USBD); + declare!(UARTE1); + declare!(QSPI); + declare!(CRYPTOCELL); + declare!(PWM3); + declare!(SPIM3); } -#[inline] -pub fn pend(irq: Interrupt) { - NVIC::pend(irq) -} - -#[inline] -pub fn unpend(irq: Interrupt) { - NVIC::unpend(irq) -} - -#[inline] -pub fn get_priority(irq: Interrupt) -> Priority { - Priority::from_nvic(NVIC::get_priority(irq)) -} - -#[inline] -pub fn set_priority(irq: Interrupt, prio: Priority) { - unsafe { - cortex_m::peripheral::Peripherals::steal() - .NVIC - .set_priority(irq, prio.to_nvic()) - } -} +pub use irqs::*; diff --git a/embassy-nrf/src/lib.rs b/embassy-nrf/src/lib.rs index 7b3368d6..e97002a2 100644 --- a/embassy-nrf/src/lib.rs +++ b/embassy-nrf/src/lib.rs @@ -51,6 +51,7 @@ pub use nrf52840_hal as hal; // This mod MUST go first, so that the others see its macros. pub(crate) mod fmt; +pub mod buffered_uarte; pub mod gpiote; pub mod interrupt; #[cfg(feature = "52840")] diff --git a/embassy-nrf/src/qspi.rs b/embassy-nrf/src/qspi.rs index 79fc7029..c9c907cd 100644 --- a/embassy-nrf/src/qspi.rs +++ b/embassy-nrf/src/qspi.rs @@ -2,6 +2,7 @@ use crate::fmt::{assert, assert_eq, panic, *}; use core::future::Future; use crate::hal::gpio::{Output, Pin as GpioPin, Port as GpioPort, PushPull}; +use crate::interrupt::{OwnedInterrupt, QSPIInterrupt}; use crate::pac::{Interrupt, QSPI}; pub use crate::pac::qspi::ifconfig0::ADDRMODE_A as AddressMode; @@ -22,8 +23,6 @@ pub use crate::pac::qspi::ifconfig0::WRITEOC_A as WriteOpcode; use embassy::flash::{Error, Flash}; use embassy::util::{DropBomb, Signal}; -use crate::interrupt; - pub struct Pins { pub sck: GpioPin>, pub csn: GpioPin>, @@ -59,7 +58,7 @@ fn port_bit(port: GpioPort) -> bool { } impl Qspi { - pub fn new(qspi: QSPI, config: Config) -> Self { + pub fn new(qspi: QSPI, irq: QSPIInterrupt, config: Config) -> Self { qspi.psel.sck.write(|w| { let pin = &config.pins.sck; let w = unsafe { w.pin().bits(pin.pin()) }; @@ -146,9 +145,10 @@ impl Qspi { // Enable READY interrupt SIGNAL.reset(); qspi.intenset.write(|w| w.ready().set()); - interrupt::set_priority(Interrupt::QSPI, interrupt::Priority::Level7); - interrupt::unpend(Interrupt::QSPI); - interrupt::enable(Interrupt::QSPI); + + irq.set_handler(irq_handler); + irq.unpend(); + irq.enable(); Self { inner: qspi } } @@ -347,8 +347,7 @@ impl Flash for Qspi { static SIGNAL: Signal<()> = Signal::new(); -#[interrupt] -unsafe fn QSPI() { +unsafe fn irq_handler() { let p = crate::pac::Peripherals::steal().QSPI; if p.events_ready.read().events_ready().bit_is_set() { p.events_ready.reset(); diff --git a/embassy-nrf/src/rtc.rs b/embassy-nrf/src/rtc.rs index 66e2f552..d65b8d47 100644 --- a/embassy-nrf/src/rtc.rs +++ b/embassy-nrf/src/rtc.rs @@ -2,10 +2,10 @@ use core::cell::Cell; use core::ops::Deref; use core::sync::atomic::{AtomicU32, Ordering}; -use embassy::time::Clock; +use embassy::time::{Clock, Instant}; use crate::interrupt; -use crate::interrupt::{CriticalSection, Mutex}; +use crate::interrupt::{CriticalSection, Mutex, OwnedInterrupt}; use crate::pac::{rtc0, Interrupt, RTC0, RTC1}; #[cfg(any(feature = "52832", feature = "52833", feature = "52840"))] @@ -56,8 +56,9 @@ impl AlarmState { const ALARM_COUNT: usize = 3; -pub struct RTC { +pub struct RTC { rtc: T, + irq: T::Interrupt, /// Number of 2^23 periods elapsed since boot. /// @@ -75,13 +76,14 @@ pub struct RTC { alarms: Mutex<[AlarmState; ALARM_COUNT]>, } -unsafe impl Send for RTC {} -unsafe impl Sync for RTC {} +unsafe impl Send for RTC {} +unsafe impl Sync for RTC {} impl RTC { - pub fn new(rtc: T) -> Self { + pub fn new(rtc: T, irq: T::Interrupt) -> Self { Self { rtc, + irq, period: AtomicU32::new(0), alarms: Mutex::new([AlarmState::new(), AlarmState::new(), AlarmState::new()]), } @@ -103,7 +105,10 @@ impl RTC { while self.rtc.counter.read().bits() != 0 {} T::set_rtc_instance(self); - interrupt::enable(T::INTERRUPT); + self.irq + .set_handler(|| T::get_rtc_instance().on_interrupt()); + self.irq.unpend(); + self.irq.enable(); } fn on_interrupt(&self) { @@ -234,18 +239,18 @@ impl embassy::time::Alarm for Alarm { /// Implemented by all RTC instances. pub trait Instance: Deref + Sized + 'static { /// The interrupt associated with this RTC instance. - const INTERRUPT: Interrupt; + type Interrupt: OwnedInterrupt; fn set_rtc_instance(rtc: &'static RTC); fn get_rtc_instance() -> &'static RTC; } macro_rules! impl_instance { - ($name:ident, $static_name:ident) => { + ($name:ident, $irq_name:path, $static_name:ident) => { static mut $static_name: Option<&'static RTC<$name>> = None; impl Instance for $name { - const INTERRUPT: Interrupt = Interrupt::$name; + type Interrupt = $irq_name; fn set_rtc_instance(rtc: &'static RTC) { unsafe { $static_name = Some(rtc) } } @@ -253,16 +258,11 @@ macro_rules! impl_instance { unsafe { $static_name.unwrap() } } } - - #[interrupt] - fn $name() { - $name::get_rtc_instance().on_interrupt(); - } }; } -impl_instance!(RTC0, RTC0_INSTANCE); -impl_instance!(RTC1, RTC1_INSTANCE); +impl_instance!(RTC0, interrupt::RTC0Interrupt, RTC0_INSTANCE); +impl_instance!(RTC1, interrupt::RTC1Interrupt, RTC1_INSTANCE); #[cfg(any(feature = "52832", feature = "52833", feature = "52840"))] -impl_instance!(RTC2, RTC2_INSTANCE); +impl_instance!(RTC2, interrupt::RTC2Interrupt, RTC2_INSTANCE); diff --git a/embassy-nrf/src/uarte.rs b/embassy-nrf/src/uarte.rs index 3a33e759..f0337be8 100644 --- a/embassy-nrf/src/uarte.rs +++ b/embassy-nrf/src/uarte.rs @@ -1,160 +1,46 @@ -//! HAL interface to the UARTE peripheral +//! Async low power UARTE. //! -//! See product specification: -//! -//! - nrf52832: Section 35 -//! - nrf52840: Section 6.34 -use core::cell::UnsafeCell; -use core::cmp::min; -use core::marker::PhantomPinned; +//! The peripheral is automatically enabled and disabled as required to save power. +//! Lowest power consumption can only be guaranteed if the send receive futures +//! are dropped correctly (e.g. not using `mem::forget()`). + +use core::future::Future; use core::ops::Deref; -use core::pin::Pin; -use core::ptr; use core::sync::atomic::{compiler_fence, Ordering}; use core::task::{Context, Poll}; -use embedded_hal::digital::v2::OutputPin; +use embassy::util::Signal; +use embedded_dma::{ReadBuffer, WriteBuffer}; -use crate::hal::gpio::{Floating, Input, Output, Pin as GpioPin, Port as GpioPort, PushPull}; +use crate::fmt::{assert, *}; +#[cfg(any(feature = "52833", feature = "52840"))] +use crate::hal::gpio::Port as GpioPort; +use crate::hal::pac; +use crate::hal::prelude::*; +use crate::hal::target_constants::EASY_DMA_SIZE; use crate::interrupt; -use crate::interrupt::CriticalSection; -#[cfg(any(feature = "52833", feature = "52840", feature = "9160"))] -use crate::pac::UARTE1; -use crate::pac::{uarte0, Interrupt, UARTE0}; +use crate::interrupt::OwnedInterrupt; -// Re-export SVD variants to allow user to directly set values -pub use uarte0::{baudrate::BAUDRATE_A as Baudrate, config::PARITY_A as Parity}; +pub use crate::hal::uarte::Pins; +// Re-export SVD variants to allow user to directly set values. +pub use pac::uarte0::{baudrate::BAUDRATE_A as Baudrate, config::PARITY_A as Parity}; -use embassy::io::{AsyncBufRead, AsyncWrite, Result}; -use embassy::util::WakerStore; - -use crate::fmt::{assert, panic, todo, *}; - -//use crate::trace; - -const RINGBUF_SIZE: usize = 512; -struct RingBuf { - buf: [u8; RINGBUF_SIZE], - start: usize, - end: usize, - empty: bool, +/// Interface to the UARTE peripheral +pub struct Uarte +where + T: Instance, +{ + instance: T, + irq: T::Interrupt, + pins: Pins, } -impl RingBuf { - fn new() -> Self { - RingBuf { - buf: [0; RINGBUF_SIZE], - start: 0, - end: 0, - empty: true, - } - } - - fn push_buf(&mut self) -> &mut [u8] { - if self.start == self.end && !self.empty { - trace!(" ringbuf: push_buf empty"); - return &mut self.buf[..0]; - } - - let n = if self.start <= self.end { - RINGBUF_SIZE - self.end - } else { - self.start - self.end - }; - - trace!(" ringbuf: push_buf {:?}..{:?}", self.end, self.end + n); - &mut self.buf[self.end..self.end + n] - } - - fn push(&mut self, n: usize) { - trace!(" ringbuf: push {:?}", n); - if n == 0 { - return; - } - - self.end = Self::wrap(self.end + n); - self.empty = false; - } - - fn pop_buf(&mut self) -> &mut [u8] { - if self.empty { - trace!(" ringbuf: pop_buf empty"); - return &mut self.buf[..0]; - } - - let n = if self.end <= self.start { - RINGBUF_SIZE - self.start - } else { - self.end - self.start - }; - - trace!(" ringbuf: pop_buf {:?}..{:?}", self.start, self.start + n); - &mut self.buf[self.start..self.start + n] - } - - fn pop(&mut self, n: usize) { - trace!(" ringbuf: pop {:?}", n); - if n == 0 { - return; - } - - self.start = Self::wrap(self.start + n); - self.empty = self.start == self.end; - } - - fn wrap(n: usize) -> usize { - assert!(n <= RINGBUF_SIZE); - if n == RINGBUF_SIZE { - 0 - } else { - n - } - } -} - -#[derive(Copy, Clone, Debug, PartialEq)] -enum RxState { - Idle, - Receiving, - ReceivingReady, - Stopping, -} -#[derive(Copy, Clone, Debug, PartialEq)] -enum TxState { - Idle, - Transmitting(usize), -} - -/// Interface to a UARTE instance -/// -/// This is a very basic interface that comes with the following limitations: -/// - The UARTE instances share the same address space with instances of UART. -/// You need to make sure that conflicting instances -/// are disabled before using `Uarte`. See product specification: -/// - nrf52832: Section 15.2 -/// - nrf52840: Section 6.1.2 -pub struct Uarte { - started: bool, - state: UnsafeCell>, -} - -// public because it needs to be used in Instance::{get_state, set_state}, but -// should not be used outside the module -#[doc(hidden)] -pub struct UarteState { - inner: T, - - rx: RingBuf, - rx_state: RxState, - rx_waker: WakerStore, - - tx: RingBuf, - tx_state: TxState, - tx_waker: WakerStore, - - _pin: PhantomPinned, +pub struct State { + tx_done: Signal<()>, + rx_done: Signal, } +// TODO: Remove when https://github.com/nrf-rs/nrf-hal/pull/276 has landed #[cfg(any(feature = "52833", feature = "52840"))] fn port_bit(port: GpioPort) -> bool { match port { @@ -163,15 +49,35 @@ fn port_bit(port: GpioPort) -> bool { } } -impl Uarte { - pub fn new(uarte: T, mut pins: Pins, parity: Parity, baudrate: Baudrate) -> Self { - // Select pins +impl Uarte +where + T: Instance, +{ + /// Creates the interface to a UARTE instance. + /// Sets the baud rate, parity and assigns the pins to the UARTE peripheral. + /// + /// # Unsafe + /// + /// The returned API is safe unless you use `mem::forget` (or similar safe mechanisms) + /// on stack allocated buffers which which have been passed to [`send()`](Uarte::send) + /// or [`receive`](Uarte::receive). + #[allow(unused_unsafe)] + pub unsafe fn new( + uarte: T, + irq: T::Interrupt, + mut pins: Pins, + parity: Parity, + baudrate: Baudrate, + ) -> Self { + assert!(uarte.enable.read().enable().is_disabled()); + uarte.psel.rxd.write(|w| { let w = unsafe { w.pin().bits(pins.rxd.pin()) }; #[cfg(any(feature = "52833", feature = "52840"))] let w = w.port().bit(port_bit(pins.rxd.port())); w.connect().connected() }); + pins.txd.set_high().unwrap(); uarte.psel.txd.write(|w| { let w = unsafe { w.pin().bits(pins.txd.pin()) }; @@ -203,359 +109,310 @@ impl Uarte { } }); - // Enable UARTE instance - uarte.enable.write(|w| w.enable().enabled()); + uarte.baudrate.write(|w| w.baudrate().variant(baudrate)); + uarte.config.write(|w| w.parity().variant(parity)); // Enable interrupts - uarte.intenset.write(|w| w.endrx().set().endtx().set()); - - // Configure - let hardware_flow_control = pins.rts.is_some() && pins.cts.is_some(); + uarte.events_endtx.reset(); + uarte.events_endrx.reset(); uarte - .config - .write(|w| w.hwfc().bit(hardware_flow_control).parity().variant(parity)); + .intenset + .write(|w| w.endtx().set().txstopped().set().endrx().set().rxto().set()); - // Configure frequency - uarte.baudrate.write(|w| w.baudrate().variant(baudrate)); + // Register ISR + irq.set_handler(Self::on_irq); + irq.unpend(); + irq.enable(); Uarte { - started: false, - state: UnsafeCell::new(UarteState { - inner: uarte, - - rx: RingBuf::new(), - rx_state: RxState::Idle, - rx_waker: WakerStore::new(), - - tx: RingBuf::new(), - tx_state: TxState::Idle, - tx_waker: WakerStore::new(), - - _pin: PhantomPinned, - }), + instance: uarte, + irq, + pins, } } - fn with_state<'a, R>( - self: Pin<&'a mut Self>, - f: impl FnOnce(Pin<&'a mut UarteState>) -> R, - ) -> R { - let Self { state, started } = unsafe { self.get_unchecked_mut() }; + pub fn free(self) -> (T, T::Interrupt, Pins) { + (self.instance, self.irq, self.pins) + } - interrupt::free(|cs| { - let ptr = state.get(); + fn enable(&mut self) { + trace!("enable"); + self.instance.enable.write(|w| w.enable().enabled()); + } - if !*started { - T::set_state(cs, ptr); + /// Sends serial data. + /// + /// `tx_buffer` is marked as static as per `embedded-dma` requirements. + /// It it safe to use a buffer with a non static lifetime if memory is not + /// reused until the future has finished. + pub fn send<'a, B>(&'a mut self, tx_buffer: B) -> SendFuture<'a, T, B> + where + B: ReadBuffer, + { + // Panic if TX is running which can happen if the user has called + // `mem::forget()` on a previous future after polling it once. + assert!(!self.tx_started()); - *started = true; + self.enable(); - // safety: safe because critical section ensures only one *mut UartState - // exists at the same time. - unsafe { Pin::new_unchecked(&mut *ptr) }.start(); - } + SendFuture { + uarte: self, + buf: tx_buffer, + } + } - // safety: safe because critical section ensures only one *mut UartState - // exists at the same time. - f(unsafe { Pin::new_unchecked(&mut *ptr) }) - }) + fn tx_started(&self) -> bool { + self.instance.events_txstarted.read().bits() != 0 + } + + /// Receives serial data. + /// + /// The future is pending until the buffer is completely filled. + /// A common pattern is to use [`stop()`](ReceiveFuture::stop) to cancel + /// unfinished transfers after a timeout to prevent lockup when no more data + /// is incoming. + /// + /// `rx_buffer` is marked as static as per `embedded-dma` requirements. + /// It it safe to use a buffer with a non static lifetime if memory is not + /// reused until the future has finished. + pub fn receive<'a, B>(&'a mut self, rx_buffer: B) -> ReceiveFuture<'a, T, B> + where + B: WriteBuffer, + { + // Panic if RX is running which can happen if the user has called + // `mem::forget()` on a previous future after polling it once. + assert!(!self.rx_started()); + + self.enable(); + + ReceiveFuture { + uarte: self, + buf: Some(rx_buffer), + } + } + + fn rx_started(&self) -> bool { + self.instance.events_rxstarted.read().bits() != 0 + } + + unsafe fn on_irq() { + let uarte = &*pac::UARTE0::ptr(); + + let mut try_disable = false; + + if uarte.events_endtx.read().bits() != 0 { + uarte.events_endtx.reset(); + trace!("endtx"); + compiler_fence(Ordering::SeqCst); + T::state().tx_done.signal(()); + } + + if uarte.events_txstopped.read().bits() != 0 { + uarte.events_txstopped.reset(); + trace!("txstopped"); + try_disable = true; + } + + if uarte.events_endrx.read().bits() != 0 { + uarte.events_endrx.reset(); + trace!("endrx"); + let len = uarte.rxd.amount.read().bits(); + compiler_fence(Ordering::SeqCst); + T::state().rx_done.signal(len); + } + + if uarte.events_rxto.read().bits() != 0 { + uarte.events_rxto.reset(); + trace!("rxto"); + try_disable = true; + } + + // Disable the peripheral if not active. + if try_disable + && uarte.events_txstarted.read().bits() == 0 + && uarte.events_rxstarted.read().bits() == 0 + { + trace!("disable"); + uarte.enable.write(|w| w.enable().disabled()); + } } } -impl Drop for Uarte { - fn drop(&mut self) { - // stop DMA before dropping, because DMA is using the buffer in `self`. - todo!() +/// Future for the [`Uarte::send()`] method. +pub struct SendFuture<'a, T, B> +where + T: Instance, +{ + uarte: &'a Uarte, + buf: B, +} + +impl<'a, T, B> Drop for SendFuture<'a, T, B> +where + T: Instance, +{ + fn drop(self: &mut Self) { + if self.uarte.tx_started() { + trace!("stoptx"); + + // Stop the transmitter to minimize the current consumption. + self.uarte.instance.events_txstarted.reset(); + self.uarte + .instance + .tasks_stoptx + .write(|w| unsafe { w.bits(1) }); + T::state().tx_done.blocking_wait(); + } } } -impl AsyncBufRead for Uarte { - fn poll_fill_buf(self: Pin<&mut Self>, cx: &mut Context<'_>) -> Poll> { - self.with_state(|s| s.poll_fill_buf(cx)) - } +impl<'a, T, B> Future for SendFuture<'a, T, B> +where + T: Instance, + B: ReadBuffer, +{ + type Output = (); - fn consume(self: Pin<&mut Self>, amt: usize) { - self.with_state(|s| s.consume(amt)) + fn poll(self: core::pin::Pin<&mut Self>, cx: &mut Context<'_>) -> Poll<()> { + let Self { uarte, buf } = unsafe { self.get_unchecked_mut() }; + + if !uarte.tx_started() { + let uarte = &uarte.instance; + + T::state().tx_done.reset(); + + let (ptr, len) = unsafe { buf.read_buffer() }; + assert!(len <= EASY_DMA_SIZE); + // TODO: panic if buffer is not in SRAM + + compiler_fence(Ordering::SeqCst); + uarte.txd.ptr.write(|w| unsafe { w.ptr().bits(ptr as u32) }); + uarte + .txd + .maxcnt + .write(|w| unsafe { w.maxcnt().bits(len as _) }); + + trace!("starttx"); + uarte.tasks_starttx.write(|w| unsafe { w.bits(1) }); + } + + T::state().tx_done.poll_wait(cx) } } -impl AsyncWrite for Uarte { - fn poll_write(self: Pin<&mut Self>, cx: &mut Context<'_>, buf: &[u8]) -> Poll> { - self.with_state(|s| s.poll_write(cx, buf)) +/// Future for the [`Uarte::receive()`] method. +pub struct ReceiveFuture<'a, T, B> +where + T: Instance, +{ + uarte: &'a Uarte, + buf: Option, +} + +impl<'a, T, B> Drop for ReceiveFuture<'a, T, B> +where + T: Instance, +{ + fn drop(self: &mut Self) { + if self.uarte.rx_started() { + trace!("stoprx"); + + self.uarte.instance.events_rxstarted.reset(); + self.uarte + .instance + .tasks_stoprx + .write(|w| unsafe { w.bits(1) }); + T::state().rx_done.blocking_wait(); + } } } -impl UarteState { - pub fn start(self: Pin<&mut Self>) { - interrupt::set_priority(T::interrupt(), interrupt::Priority::Level7); - interrupt::enable(T::interrupt()); - interrupt::pend(T::interrupt()); - } +impl<'a, T, B> Future for ReceiveFuture<'a, T, B> +where + T: Instance, + B: WriteBuffer, +{ + type Output = B; - fn poll_fill_buf(self: Pin<&mut Self>, cx: &mut Context<'_>) -> Poll> { - let this = unsafe { self.get_unchecked_mut() }; + fn poll(self: core::pin::Pin<&mut Self>, cx: &mut Context<'_>) -> Poll { + let Self { uarte, buf } = unsafe { self.get_unchecked_mut() }; - // Conservative compiler fence to prevent optimizations that do not - // take in to account actions by DMA. The fence has been placed here, - // before any DMA action has started - compiler_fence(Ordering::SeqCst); - trace!("poll_read"); + if !uarte.rx_started() { + let uarte = &uarte.instance; - // We have data ready in buffer? Return it. - let buf = this.rx.pop_buf(); - if buf.len() != 0 { - trace!(" got {:?} {:?}", buf.as_ptr() as u32, buf.len()); - return Poll::Ready(Ok(buf)); + T::state().rx_done.reset(); + + let (ptr, len) = unsafe { buf.as_mut().unwrap().write_buffer() }; + assert!(len <= EASY_DMA_SIZE); + + compiler_fence(Ordering::SeqCst); + uarte.rxd.ptr.write(|w| unsafe { w.ptr().bits(ptr as u32) }); + uarte + .rxd + .maxcnt + .write(|w| unsafe { w.maxcnt().bits(len as _) }); + + trace!("startrx"); + uarte.tasks_startrx.write(|w| unsafe { w.bits(1) }); } - trace!(" empty"); - - if this.rx_state == RxState::ReceivingReady { - trace!(" stopping"); - this.rx_state = RxState::Stopping; - this.inner.tasks_stoprx.write(|w| unsafe { w.bits(1) }); - } - - this.rx_waker.store(cx.waker()); - Poll::Pending - } - - fn consume(self: Pin<&mut Self>, amt: usize) { - let this = unsafe { self.get_unchecked_mut() }; - trace!("consume {:?}", amt); - this.rx.pop(amt); - interrupt::pend(T::interrupt()); - } - - fn poll_write(self: Pin<&mut Self>, cx: &mut Context<'_>, buf: &[u8]) -> Poll> { - let this = unsafe { self.get_unchecked_mut() }; - - trace!("poll_write: {:?}", buf.len()); - - let tx_buf = this.tx.push_buf(); - if tx_buf.len() == 0 { - trace!("poll_write: pending"); - this.tx_waker.store(cx.waker()); - return Poll::Pending; - } - - let n = min(tx_buf.len(), buf.len()); - tx_buf[..n].copy_from_slice(&buf[..n]); - this.tx.push(n); - - trace!("poll_write: queued {:?}", n); - - // Conservative compiler fence to prevent optimizations that do not - // take in to account actions by DMA. The fence has been placed here, - // before any DMA action has started - compiler_fence(Ordering::SeqCst); - - interrupt::pend(T::interrupt()); - - Poll::Ready(Ok(n)) - } - - fn on_interrupt(&mut self) { - trace!("irq: start"); - let mut more_work = true; - while more_work { - more_work = false; - match self.rx_state { - RxState::Idle => { - trace!(" irq_rx: in state idle"); - - if self.inner.events_rxdrdy.read().bits() != 0 { - trace!(" irq_rx: rxdrdy?????"); - self.inner.events_rxdrdy.reset(); - } - - if self.inner.events_endrx.read().bits() != 0 { - panic!("unexpected endrx"); - } - - let buf = self.rx.push_buf(); - if buf.len() != 0 { - trace!(" irq_rx: starting {:?}", buf.len()); - self.rx_state = RxState::Receiving; - - // Set up the DMA read - self.inner.rxd.ptr.write(|w| - // The PTR field is a full 32 bits wide and accepts the full range - // of values. - unsafe { w.ptr().bits(buf.as_ptr() as u32) }); - self.inner.rxd.maxcnt.write(|w| - // We're giving it the length of the buffer, so no danger of - // accessing invalid memory. We have verified that the length of the - // buffer fits in an `u8`, so the cast to `u8` is also fine. - // - // The MAXCNT field is at least 8 bits wide and accepts the full - // range of values. - unsafe { w.maxcnt().bits(buf.len() as _) }); - trace!(" irq_rx: buf {:?} {:?}", buf.as_ptr() as u32, buf.len()); - - // Enable RXRDY interrupt. - self.inner.events_rxdrdy.reset(); - self.inner.intenset.write(|w| w.rxdrdy().set()); - - // Start UARTE Receive transaction - self.inner.tasks_startrx.write(|w| - // `1` is a valid value to write to task registers. - unsafe { w.bits(1) }); - } - } - RxState::Receiving => { - trace!(" irq_rx: in state receiving"); - if self.inner.events_rxdrdy.read().bits() != 0 { - trace!(" irq_rx: rxdrdy"); - - // Disable the RXRDY event interrupt - // RXRDY is triggered for every byte, but we only care about whether we have - // some bytes or not. So as soon as we have at least one, disable it, to avoid - // wasting CPU cycles in interrupts. - self.inner.intenclr.write(|w| w.rxdrdy().clear()); - - self.inner.events_rxdrdy.reset(); - - self.rx_waker.wake(); - self.rx_state = RxState::ReceivingReady; - more_work = true; // in case we also have endrx pending - } - } - RxState::ReceivingReady | RxState::Stopping => { - trace!(" irq_rx: in state ReceivingReady"); - - if self.inner.events_rxdrdy.read().bits() != 0 { - trace!(" irq_rx: rxdrdy"); - self.inner.events_rxdrdy.reset(); - } - - if self.inner.events_endrx.read().bits() != 0 { - let n: usize = self.inner.rxd.amount.read().amount().bits() as usize; - trace!(" irq_rx: endrx {:?}", n); - self.rx.push(n); - - self.inner.events_endrx.reset(); - - self.rx_waker.wake(); - self.rx_state = RxState::Idle; - more_work = true; // start another rx if possible - } - } - } - } - - more_work = true; - while more_work { - more_work = false; - match self.tx_state { - TxState::Idle => { - trace!(" irq_tx: in state Idle"); - let buf = self.tx.pop_buf(); - if buf.len() != 0 { - trace!(" irq_tx: starting {:?}", buf.len()); - self.tx_state = TxState::Transmitting(buf.len()); - - // Set up the DMA write - self.inner.txd.ptr.write(|w| - // The PTR field is a full 32 bits wide and accepts the full range - // of values. - unsafe { w.ptr().bits(buf.as_ptr() as u32) }); - self.inner.txd.maxcnt.write(|w| - // We're giving it the length of the buffer, so no danger of - // accessing invalid memory. We have verified that the length of the - // buffer fits in an `u8`, so the cast to `u8` is also fine. - // - // The MAXCNT field is 8 bits wide and accepts the full range of - // values. - unsafe { w.maxcnt().bits(buf.len() as _) }); - - // Start UARTE Transmit transaction - self.inner.tasks_starttx.write(|w| - // `1` is a valid value to write to task registers. - unsafe { w.bits(1) }); - } - } - TxState::Transmitting(n) => { - trace!(" irq_tx: in state Transmitting"); - if self.inner.events_endtx.read().bits() != 0 { - self.inner.events_endtx.reset(); - - trace!(" irq_tx: endtx {:?}", n); - self.tx.pop(n); - self.tx_waker.wake(); - self.tx_state = TxState::Idle; - more_work = true; // start another tx if possible - } - } - } - } - trace!("irq: end"); + T::state() + .rx_done + .poll_wait(cx) + .map(|_| buf.take().unwrap()) } } -pub struct Pins { - pub rxd: GpioPin>, - pub txd: GpioPin>, - pub cts: Option>>, - pub rts: Option>>, +/// Future for the [`receive()`] method. +impl<'a, T, B> ReceiveFuture<'a, T, B> +where + T: Instance, +{ + /// Stops the ongoing reception and returns the number of bytes received. + pub async fn stop(mut self) -> (B, usize) { + let buf = self.buf.take().unwrap(); + drop(self); + let len = T::state().rx_done.wait().await; + (buf, len as _) + } } mod private { pub trait Sealed {} - - impl Sealed for crate::pac::UARTE0 {} - #[cfg(any(feature = "52833", feature = "52840", feature = "9160"))] - impl Sealed for crate::pac::UARTE1 {} } -pub trait Instance: Deref + Sized + private::Sealed { - fn interrupt() -> Interrupt; +pub trait Instance: Deref + Sized + private::Sealed { + type Interrupt: OwnedInterrupt; #[doc(hidden)] - fn get_state(_cs: &CriticalSection) -> *mut UarteState; - - #[doc(hidden)] - fn set_state(_cs: &CriticalSection, state: *mut UarteState); + fn state() -> &'static State; } -#[interrupt] -unsafe fn UARTE0_UART0() { - interrupt::free(|cs| UARTE0::get_state(cs).as_mut().unwrap().on_interrupt()); -} +static UARTE0_STATE: State = State { + tx_done: Signal::new(), + rx_done: Signal::new(), +}; +impl private::Sealed for pac::UARTE0 {} +impl Instance for pac::UARTE0 { + type Interrupt = interrupt::UARTE0_UART0Interrupt; -#[cfg(any(feature = "52833", feature = "52840", feature = "9160"))] -#[interrupt] -unsafe fn UARTE1() { - interrupt::free(|cs| UARTE1::get_state(cs).as_mut().unwrap().on_interrupt()); -} - -static mut UARTE0_STATE: *mut UarteState = ptr::null_mut(); -#[cfg(any(feature = "52833", feature = "52840", feature = "9160"))] -static mut UARTE1_STATE: *mut UarteState = ptr::null_mut(); - -impl Instance for UARTE0 { - fn interrupt() -> Interrupt { - Interrupt::UARTE0_UART0 - } - - fn get_state(_cs: &CriticalSection) -> *mut UarteState { - unsafe { UARTE0_STATE } // Safe because of CriticalSection - } - fn set_state(_cs: &CriticalSection, state: *mut UarteState) { - unsafe { UARTE0_STATE = state } // Safe because of CriticalSection + fn state() -> &'static State { + &UARTE0_STATE } } #[cfg(any(feature = "52833", feature = "52840", feature = "9160"))] -impl Instance for UARTE1 { - fn interrupt() -> Interrupt { - Interrupt::UARTE1 - } +static UARTE1_STATE: State = State { + tx_done: Signal::new(), + rx_done: Signal::new(), +}; +#[cfg(any(feature = "52833", feature = "52840", feature = "9160"))] +impl private::Sealed for pac::UARTE1 {} +#[cfg(any(feature = "52833", feature = "52840", feature = "9160"))] +impl Instance for pac::UARTE1 { + type Interrupt = interrupt::UARTE1Interrupt; - fn get_state(_cs: &CriticalSection) -> *mut UarteState { - unsafe { UARTE1_STATE } // Safe because of CriticalSection - } - fn set_state(_cs: &CriticalSection, state: *mut UarteState) { - unsafe { UARTE1_STATE = state } // Safe because of CriticalSection + fn state() -> &'static State { + &UARTE1_STATE } } diff --git a/embassy/src/executor/mod.rs b/embassy/src/executor/mod.rs index c1ec3832..5e835476 100644 --- a/embassy/src/executor/mod.rs +++ b/embassy/src/executor/mod.rs @@ -105,8 +105,8 @@ impl Task { if task .header .state - .compare_and_swap(0, state, Ordering::AcqRel) - == 0 + .compare_exchange(0, state, Ordering::AcqRel, Ordering::Acquire) + .is_ok() { // Initialize the task task.header.poll_fn.write(Self::poll); @@ -214,9 +214,12 @@ impl Executor { /// Runs the executor until the queue is empty. pub fn run(&self) { unsafe { - self.timer_queue.dequeue_expired(Instant::now(), |p| { - self.enqueue(p); - }); + if self.alarm.is_some() { + self.timer_queue.dequeue_expired(Instant::now(), |p| { + let header = &*p; + header.enqueue(); + }); + } self.run_queue.dequeue_all(|p| { let header = &*p; diff --git a/embassy/src/executor/timer.rs b/embassy/src/executor/timer.rs index 05c14b88..56236a05 100644 --- a/embassy/src/executor/timer.rs +++ b/embassy/src/executor/timer.rs @@ -7,16 +7,21 @@ use crate::time::{Duration, Instant}; pub struct Timer { expires_at: Instant, + yielded_once: bool, } impl Timer { pub fn at(expires_at: Instant) -> Self { - Self { expires_at } + Self { + expires_at, + yielded_once: false, + } } pub fn after(duration: Duration) -> Self { Self { expires_at: Instant::now() + duration, + yielded_once: false, } } } @@ -25,11 +30,12 @@ impl Unpin for Timer {} impl Future for Timer { type Output = (); - fn poll(self: Pin<&mut Self>, cx: &mut Context<'_>) -> Poll { - if self.expires_at <= Instant::now() { + fn poll(mut self: Pin<&mut Self>, cx: &mut Context<'_>) -> Poll { + if self.yielded_once && self.expires_at <= Instant::now() { Poll::Ready(()) } else { unsafe { super::register_timer(self.expires_at, cx.waker()) }; + self.yielded_once = true; Poll::Pending } } diff --git a/embassy/src/interrupt.rs b/embassy/src/interrupt.rs new file mode 100644 index 00000000..fee52b32 --- /dev/null +++ b/embassy/src/interrupt.rs @@ -0,0 +1,76 @@ +use core::mem; +use core::ptr; +use core::sync::atomic::{AtomicBool, AtomicPtr, Ordering}; +use cortex_m::peripheral::NVIC; + +pub use embassy_macros::interrupt_declare as declare; +pub use embassy_macros::interrupt_take as take; + +struct NrWrap(u8); +unsafe impl cortex_m::interrupt::Nr for NrWrap { + fn nr(&self) -> u8 { + self.0 + } +} + +pub unsafe trait OwnedInterrupt { + type Priority: From + Into + Copy; + fn number(&self) -> u8; + #[doc(hidden)] + unsafe fn __handler(&self) -> &'static AtomicPtr; + + fn set_handler(&self, handler: unsafe fn()) { + unsafe { self.__handler() }.store(handler as *mut u32, Ordering::Release); + } + + #[inline] + fn enable(&self) { + unsafe { + NVIC::unmask(NrWrap(self.number())); + } + } + + #[inline] + fn disable(&self) { + NVIC::mask(NrWrap(self.number())); + } + + #[inline] + fn is_active(&self) -> bool { + NVIC::is_active(NrWrap(self.number())) + } + + #[inline] + fn is_enabled(&self) -> bool { + NVIC::is_enabled(NrWrap(self.number())) + } + + #[inline] + fn is_pending(&self) -> bool { + NVIC::is_pending(NrWrap(self.number())) + } + + #[inline] + fn pend(&self) { + NVIC::pend(NrWrap(self.number())) + } + + #[inline] + fn unpend(&self) { + NVIC::unpend(NrWrap(self.number())) + } + + #[inline] + fn get_priority(&self) -> Self::Priority { + Self::Priority::from(NVIC::get_priority(NrWrap(self.number()))) + } + + #[inline] + fn set_priority(&self, prio: Self::Priority) { + unsafe { + cortex_m::peripheral::Peripherals::steal() + .NVIC + .set_priority(NrWrap(self.number()), prio.into()) + } + } +} diff --git a/embassy/src/lib.rs b/embassy/src/lib.rs index 49ba8eea..bc06ebd1 100644 --- a/embassy/src/lib.rs +++ b/embassy/src/lib.rs @@ -9,6 +9,7 @@ pub(crate) mod fmt; pub mod executor; pub mod flash; +pub mod interrupt; pub mod io; pub mod rand; pub mod time; diff --git a/embassy/src/util/forever.rs b/embassy/src/util/forever.rs index 80dc0eda..9b1f8bb3 100644 --- a/embassy/src/util/forever.rs +++ b/embassy/src/util/forever.rs @@ -19,7 +19,11 @@ impl Forever { } pub fn put(&'static self, val: T) -> &'static mut T { - if self.used.compare_and_swap(false, true, Ordering::SeqCst) { + if self + .used + .compare_exchange(false, true, Ordering::AcqRel, Ordering::Acquire) + .is_err() + { panic!("Forever.put() called multiple times"); } diff --git a/embassy/src/util/signal.rs b/embassy/src/util/signal.rs index bde10c56..cb641061 100644 --- a/embassy/src/util/signal.rs +++ b/embassy/src/util/signal.rs @@ -62,4 +62,13 @@ impl Signal { pub fn wait(&self) -> impl Future + '_ { futures::future::poll_fn(move |cx| self.poll_wait(cx)) } + + /// Blocks until the signal has been received. + /// + /// Returns immediately when [`poll_wait()`] has not been called before. + pub fn blocking_wait(&self) { + while cortex_m::interrupt::free(|_| { + matches!(unsafe { &*self.state.get() }, State::Waiting(_)) + }) {} + } } diff --git a/examples/src/bin/buffered_uart.rs b/examples/src/bin/buffered_uart.rs new file mode 100644 index 00000000..6e15fbcf --- /dev/null +++ b/examples/src/bin/buffered_uart.rs @@ -0,0 +1,84 @@ +#![no_std] +#![no_main] +#![feature(type_alias_impl_trait)] + +#[path = "../example_common.rs"] +mod example_common; +use example_common::*; + +use cortex_m_rt::entry; +use defmt::panic; +use futures::pin_mut; +use nrf52840_hal::gpio; + +use embassy::executor::{task, Executor}; +use embassy::io::{AsyncBufRead, AsyncBufReadExt, AsyncWrite, AsyncWriteExt}; +use embassy::util::Forever; +use embassy_nrf::buffered_uarte; +use embassy_nrf::interrupt; + +#[task] +async fn run() { + let p = unwrap!(embassy_nrf::pac::Peripherals::take()); + + let port0 = gpio::p0::Parts::new(p.P0); + + let pins = buffered_uarte::Pins { + rxd: port0.p0_08.into_floating_input().degrade(), + txd: port0 + .p0_06 + .into_push_pull_output(gpio::Level::Low) + .degrade(), + cts: None, + rts: None, + }; + + let irq = interrupt::take!(UARTE0_UART0); + let u = buffered_uarte::BufferedUarte::new( + p.UARTE0, + irq, + pins, + buffered_uarte::Parity::EXCLUDED, + buffered_uarte::Baudrate::BAUD115200, + ); + pin_mut!(u); + + info!("uarte initialized!"); + + unwrap!(u.write_all(b"Hello!\r\n").await); + info!("wrote hello in uart!"); + + // Simple demo, reading 8-char chunks and echoing them back reversed. + loop { + info!("reading..."); + let mut buf = [0u8; 8]; + unwrap!(u.read_exact(&mut buf).await); + info!("read done, got {:[u8]}", buf); + + // Reverse buf + for i in 0..4 { + let tmp = buf[i]; + buf[i] = buf[7 - i]; + buf[7 - i] = tmp; + } + + info!("writing..."); + unwrap!(u.write_all(&buf).await); + info!("write done"); + } +} + +static EXECUTOR: Forever = Forever::new(); + +#[entry] +fn main() -> ! { + info!("Hello World!"); + + let executor = EXECUTOR.put(Executor::new(cortex_m::asm::sev)); + unwrap!(executor.spawn(run())); + + loop { + executor.run(); + cortex_m::asm::wfe(); + } +} diff --git a/examples/src/bin/executor_fairness_test.rs b/examples/src/bin/executor_fairness_test.rs new file mode 100644 index 00000000..9b2c1bd2 --- /dev/null +++ b/examples/src/bin/executor_fairness_test.rs @@ -0,0 +1,74 @@ +#![no_std] +#![no_main] +#![feature(type_alias_impl_trait)] + +#[path = "../example_common.rs"] +mod example_common; +use example_common::*; + +use core::task::Poll; +use cortex_m_rt::entry; +use defmt::panic; +use embassy::executor::{task, Executor}; +use embassy::time::{Duration, Instant, Timer}; +use embassy::util::Forever; +use embassy_nrf::pac; +use embassy_nrf::{interrupt, rtc}; +use nrf52840_hal::clocks; + +#[task] +async fn run1() { + loop { + info!("DING DONG"); + Timer::after(Duration::from_ticks(16000)).await; + } +} + +#[task] +async fn run2() { + loop { + Timer::at(Instant::from_ticks(0)).await; + } +} + +#[task] +async fn run3() { + futures::future::poll_fn(|cx| { + cx.waker().wake_by_ref(); + Poll::<()>::Pending + }) + .await; +} + +static RTC: Forever> = Forever::new(); +static ALARM: Forever> = Forever::new(); +static EXECUTOR: Forever = Forever::new(); + +#[entry] +fn main() -> ! { + info!("Hello World!"); + + let p = unwrap!(embassy_nrf::pac::Peripherals::take()); + + clocks::Clocks::new(p.CLOCK) + .enable_ext_hfosc() + .set_lfclk_src_external(clocks::LfOscConfiguration::NoExternalNoBypass) + .start_lfclk(); + + let rtc = RTC.put(rtc::RTC::new(p.RTC1, interrupt::take!(RTC1))); + rtc.start(); + + unsafe { embassy::time::set_clock(rtc) }; + + let alarm = ALARM.put(rtc.alarm0()); + let executor = EXECUTOR.put(Executor::new_with_alarm(alarm, cortex_m::asm::sev)); + + unwrap!(executor.spawn(run1())); + unwrap!(executor.spawn(run2())); + unwrap!(executor.spawn(run3())); + + loop { + executor.run(); + cortex_m::asm::wfe(); + } +} diff --git a/examples/src/bin/gpiote.rs b/examples/src/bin/gpiote.rs index edc3f5ef..afa1b85d 100644 --- a/examples/src/bin/gpiote.rs +++ b/examples/src/bin/gpiote.rs @@ -7,18 +7,20 @@ mod example_common; use example_common::*; use cortex_m_rt::entry; +use defmt::panic; use nrf52840_hal::gpio; use embassy::executor::{task, Executor}; use embassy::util::Forever; use embassy_nrf::gpiote; +use embassy_nrf::interrupt; #[task] async fn run() { let p = unwrap!(embassy_nrf::pac::Peripherals::take()); let port0 = gpio::p0::Parts::new(p.P0); - let g = gpiote::Gpiote::new(p.GPIOTE); + let g = gpiote::Gpiote::new(p.GPIOTE, interrupt::take!(GPIOTE)); info!("Starting!"); diff --git a/examples/src/bin/gpiote_port.rs b/examples/src/bin/gpiote_port.rs index 58531764..f5aa8132 100644 --- a/examples/src/bin/gpiote_port.rs +++ b/examples/src/bin/gpiote_port.rs @@ -8,11 +8,13 @@ use example_common::*; use core::mem; use cortex_m_rt::entry; +use defmt::panic; use nrf52840_hal::gpio; use embassy::executor::{task, Executor}; use embassy::util::Forever; use embassy_nrf::gpiote::{Gpiote, PortInputPolarity}; +use embassy_nrf::interrupt; async fn button(g: &Gpiote, n: usize, pin: gpio::Pin>) { loop { @@ -28,7 +30,7 @@ async fn run() { let p = unwrap!(embassy_nrf::pac::Peripherals::take()); let port0 = gpio::p0::Parts::new(p.P0); - let g = Gpiote::new(p.GPIOTE); + let g = Gpiote::new(p.GPIOTE, interrupt::take!(GPIOTE)); info!( "sizeof Signal<()> = {:usize}", mem::size_of::>() diff --git a/examples/src/bin/multiprio.rs b/examples/src/bin/multiprio.rs index be7e91a9..c821e3db 100644 --- a/examples/src/bin/multiprio.rs +++ b/examples/src/bin/multiprio.rs @@ -61,7 +61,9 @@ mod example_common; use example_common::*; +use cortex_m::peripheral::NVIC; use cortex_m_rt::entry; +use defmt::panic; use nrf52840_hal::clocks; use embassy::executor::{task, Executor}; @@ -130,7 +132,7 @@ fn main() -> ! { .set_lfclk_src_external(clocks::LfOscConfiguration::NoExternalNoBypass) .start_lfclk(); - let rtc = RTC.put(rtc::RTC::new(p.RTC1)); + let rtc = RTC.put(rtc::RTC::new(p.RTC1, interrupt::take!(RTC1))); rtc.start(); unsafe { embassy::time::set_clock(rtc) }; @@ -138,17 +140,20 @@ fn main() -> ! { let executor_low = EXECUTOR_LOW.put(Executor::new_with_alarm(alarm_low, cortex_m::asm::sev)); let alarm_med = ALARM_MED.put(rtc.alarm1()); let executor_med = EXECUTOR_MED.put(Executor::new_with_alarm(alarm_med, || { - interrupt::pend(interrupt::SWI0_EGU0) + NVIC::pend(interrupt::SWI0_EGU0) })); let alarm_high = ALARM_HIGH.put(rtc.alarm2()); let executor_high = EXECUTOR_HIGH.put(Executor::new_with_alarm(alarm_high, || { - interrupt::pend(interrupt::SWI1_EGU1) + NVIC::pend(interrupt::SWI1_EGU1) })); - interrupt::set_priority(interrupt::SWI0_EGU0, interrupt::Priority::Level7); - interrupt::set_priority(interrupt::SWI1_EGU1, interrupt::Priority::Level6); - interrupt::enable(interrupt::SWI0_EGU0); - interrupt::enable(interrupt::SWI1_EGU1); + unsafe { + let mut nvic: NVIC = core::mem::transmute(()); + nvic.set_priority(interrupt::SWI0_EGU0, 7 << 5); + nvic.set_priority(interrupt::SWI1_EGU1, 6 << 5); + NVIC::unmask(interrupt::SWI0_EGU0); + NVIC::unmask(interrupt::SWI1_EGU1); + } unwrap!(executor_low.spawn(run_low())); unwrap!(executor_med.spawn(run_med())); diff --git a/examples/src/bin/qspi.rs b/examples/src/bin/qspi.rs index 644018e2..a7d47f79 100644 --- a/examples/src/bin/qspi.rs +++ b/examples/src/bin/qspi.rs @@ -13,7 +13,7 @@ use nrf52840_hal::gpio; use embassy::executor::{task, Executor}; use embassy::flash::Flash; use embassy::util::Forever; -use embassy_nrf::qspi; +use embassy_nrf::{interrupt, qspi}; const PAGE_SIZE: usize = 4096; @@ -68,7 +68,8 @@ async fn run() { deep_power_down: None, }; - let mut q = qspi::Qspi::new(p.QSPI, config); + let irq = interrupt::take!(QSPI); + let mut q = qspi::Qspi::new(p.QSPI, irq, config); let mut id = [1; 3]; q.custom_instruction(0x9F, &[], &mut id).await.unwrap(); diff --git a/examples/src/bin/rtc_async.rs b/examples/src/bin/rtc_async.rs index aec70a07..dcdeb704 100644 --- a/examples/src/bin/rtc_async.rs +++ b/examples/src/bin/rtc_async.rs @@ -8,13 +8,13 @@ use example_common::*; use core::mem::MaybeUninit; use cortex_m_rt::entry; -use nrf52840_hal::clocks; - +use defmt::panic; use embassy::executor::{task, Executor}; use embassy::time::{Clock, Duration, Timer}; use embassy::util::Forever; use embassy_nrf::pac; -use embassy_nrf::rtc; +use embassy_nrf::{interrupt, rtc}; +use nrf52840_hal::clocks; #[task] async fn run1() { @@ -47,7 +47,7 @@ fn main() -> ! { .set_lfclk_src_external(clocks::LfOscConfiguration::NoExternalNoBypass) .start_lfclk(); - let rtc = RTC.put(rtc::RTC::new(p.RTC1)); + let rtc = RTC.put(rtc::RTC::new(p.RTC1, interrupt::take!(RTC1))); rtc.start(); unsafe { embassy::time::set_clock(rtc) }; diff --git a/examples/src/bin/rtc_raw.rs b/examples/src/bin/rtc_raw.rs index ad5fab24..43858546 100644 --- a/examples/src/bin/rtc_raw.rs +++ b/examples/src/bin/rtc_raw.rs @@ -8,8 +8,9 @@ use example_common::*; use core::mem::MaybeUninit; use cortex_m_rt::entry; +use defmt::panic; use embassy::time::{Alarm, Clock}; -use embassy_nrf::rtc; +use embassy_nrf::{interrupt, rtc}; use nrf52840_hal::clocks; static mut RTC: MaybeUninit> = MaybeUninit::uninit(); @@ -25,9 +26,11 @@ fn main() -> ! { .set_lfclk_src_external(clocks::LfOscConfiguration::NoExternalNoBypass) .start_lfclk(); + let irq = interrupt::take!(RTC1); + let rtc: &'static _ = unsafe { let ptr = RTC.as_mut_ptr(); - ptr.write(rtc::RTC::new(p.RTC1)); + ptr.write(rtc::RTC::new(p.RTC1, irq)); &*ptr }; diff --git a/examples/src/bin/uart.rs b/examples/src/bin/uart.rs index 553bbb35..10793668 100644 --- a/examples/src/bin/uart.rs +++ b/examples/src/bin/uart.rs @@ -7,18 +7,74 @@ mod example_common; use example_common::*; use cortex_m_rt::entry; -use futures::pin_mut; +use defmt::panic; +use embassy::executor::{task, Executor}; +use embassy::time::{Duration, Timer}; +use embassy::util::Forever; +use embassy_nrf::{interrupt, pac, rtc, uarte}; +use futures::future::{select, Either}; +use nrf52840_hal::clocks; use nrf52840_hal::gpio; -use embassy::executor::{task, Executor}; -use embassy::io::{AsyncBufRead, AsyncBufReadExt, AsyncWrite, AsyncWriteExt}; -use embassy::util::Forever; -use embassy_nrf::uarte; - #[task] -async fn run() { +async fn run(mut uart: uarte::Uarte) { + info!("uarte initialized!"); + + // Message must be in SRAM + let mut buf = [0; 8]; + buf.copy_from_slice(b"Hello!\r\n"); + + uart.send(&buf).await; + info!("wrote hello in uart!"); + + info!("reading..."); + loop { + let received = match select( + uart.receive(&mut buf), + Timer::after(Duration::from_millis(10)), + ) + .await + { + Either::Left((buf, _)) => buf, + Either::Right((_, read)) => { + let (buf, n) = read.stop().await; + &buf[..n] + } + }; + + if received.len() > 0 { + info!("read done, got {:[u8]}", received); + + // Echo back received data + uart.send(received).await; + } + } +} + +static RTC: Forever> = Forever::new(); +static ALARM: Forever> = Forever::new(); +static EXECUTOR: Forever = Forever::new(); + +#[entry] +fn main() -> ! { + info!("Hello World!"); + let p = unwrap!(embassy_nrf::pac::Peripherals::take()); + clocks::Clocks::new(p.CLOCK) + .enable_ext_hfosc() + .set_lfclk_src_external(clocks::LfOscConfiguration::NoExternalNoBypass) + .start_lfclk(); + + let rtc = RTC.put(rtc::RTC::new(p.RTC1, interrupt::take!(RTC1))); + rtc.start(); + + unsafe { embassy::time::set_clock(rtc) }; + + let alarm = ALARM.put(rtc.alarm0()); + let executor = EXECUTOR.put(Executor::new_with_alarm(alarm, cortex_m::asm::sev)); + + // Init UART let port0 = gpio::p0::Parts::new(p.P0); let pins = uarte::Pins { @@ -31,47 +87,18 @@ async fn run() { rts: None, }; - let u = uarte::Uarte::new( - p.UARTE0, - pins, - uarte::Parity::EXCLUDED, - uarte::Baudrate::BAUD115200, - ); - pin_mut!(u); + // NOTE(unsafe): Safe becasue we do not use `mem::forget` anywhere. + let uart = unsafe { + uarte::Uarte::new( + p.UARTE0, + interrupt::take!(UARTE0_UART0), + pins, + uarte::Parity::EXCLUDED, + uarte::Baudrate::BAUD115200, + ) + }; - info!("uarte initialized!"); - - unwrap!(u.write_all(b"Hello!\r\n").await); - info!("wrote hello in uart!"); - - // Simple demo, reading 8-char chunks and echoing them back reversed. - loop { - info!("reading..."); - let mut buf = [0u8; 8]; - unwrap!(u.read_exact(&mut buf).await); - info!("read done, got {:[u8]}", buf); - - // Reverse buf - for i in 0..4 { - let tmp = buf[i]; - buf[i] = buf[7 - i]; - buf[7 - i] = tmp; - } - - info!("writing..."); - unwrap!(u.write_all(&buf).await); - info!("write done"); - } -} - -static EXECUTOR: Forever = Forever::new(); - -#[entry] -fn main() -> ! { - info!("Hello World!"); - - let executor = EXECUTOR.put(Executor::new(cortex_m::asm::sev)); - unwrap!(executor.spawn(run())); + unwrap!(executor.spawn(run(uart))); loop { executor.run();