From ace4f40f8089f8cebbac023a4a1b960d976d2776 Mon Sep 17 00:00:00 2001 From: Dario Nieuwenhuis Date: Sun, 3 Jan 2021 01:40:40 +0100 Subject: [PATCH] Introduce "peripheral" abstraction to share state between main and interrupt. --- embassy-nrf-examples/.cargo/config | 6 +- embassy-nrf-examples/src/bin/buffered_uart.rs | 11 +- embassy-nrf/src/buffered_uarte.rs | 315 ++++++------------ embassy-nrf/src/lib.rs | 1 + embassy-nrf/src/util/mod.rs | 2 + embassy-nrf/src/util/peripheral.rs | 107 ++++++ embassy-nrf/src/util/ring_buffer.rs | 80 +++++ 7 files changed, 304 insertions(+), 218 deletions(-) create mode 100644 embassy-nrf/src/util/mod.rs create mode 100644 embassy-nrf/src/util/peripheral.rs create mode 100644 embassy-nrf/src/util/ring_buffer.rs diff --git a/embassy-nrf-examples/.cargo/config b/embassy-nrf-examples/.cargo/config index 3f319ae5..59128887 100644 --- a/embassy-nrf-examples/.cargo/config +++ b/embassy-nrf-examples/.cargo/config @@ -20,8 +20,4 @@ rustflags = [ ] [build] -# Pick ONE of these compilation targets -# target = "thumbv6m-none-eabi" # Cortex-M0 and Cortex-M0+ -# target = "thumbv7m-none-eabi" # Cortex-M3 -# target = "thumbv7em-none-eabi" # Cortex-M4 and Cortex-M7 (no FPU) -target = "thumbv7em-none-eabihf" # Cortex-M4F and Cortex-M7F (with FPU) +target = "thumbv7em-none-eabi" diff --git a/embassy-nrf-examples/src/bin/buffered_uart.rs b/embassy-nrf-examples/src/bin/buffered_uart.rs index 6e15fbcf..68a76f71 100644 --- a/embassy-nrf-examples/src/bin/buffered_uart.rs +++ b/embassy-nrf-examples/src/bin/buffered_uart.rs @@ -8,15 +8,17 @@ 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::io::{AsyncBufReadExt, AsyncWriteExt}; use embassy::util::Forever; use embassy_nrf::buffered_uarte; use embassy_nrf::interrupt; +static mut TX_BUFFER: [u8; 4096] = [0; 4096]; +static mut RX_BUFFER: [u8; 4096] = [0; 4096]; + #[task] async fn run() { let p = unwrap!(embassy_nrf::pac::Peripherals::take()); @@ -34,14 +36,15 @@ async fn run() { }; let irq = interrupt::take!(UARTE0_UART0); - let u = buffered_uarte::BufferedUarte::new( + let mut u = buffered_uarte::BufferedUarte::new( p.UARTE0, irq, + unsafe { &mut RX_BUFFER }, + unsafe { &mut TX_BUFFER }, pins, buffered_uarte::Parity::EXCLUDED, buffered_uarte::Baudrate::BAUD115200, ); - pin_mut!(u); info!("uarte initialized!"); diff --git a/embassy-nrf/src/buffered_uarte.rs b/embassy-nrf/src/buffered_uarte.rs index 9863e3fa..4749c3b4 100644 --- a/embassy-nrf/src/buffered_uarte.rs +++ b/embassy-nrf/src/buffered_uarte.rs @@ -4,107 +4,29 @@ //! //! - nrf52832: Section 35 //! - nrf52840: Section 6.34 -use core::cell::UnsafeCell; use core::cmp::min; -use core::marker::PhantomPinned; +use core::marker::PhantomData; +use core::mem; use core::ops::Deref; use core::pin::Pin; -use core::ptr; use core::sync::atomic::{compiler_fence, Ordering}; use core::task::{Context, Poll}; use embassy::io::{AsyncBufRead, AsyncWrite, Result}; use embassy::util::WakerRegistration; use embedded_hal::digital::v2::OutputPin; -use crate::fmt::{assert, panic, todo, *}; +use crate::fmt::{panic, todo, *}; use crate::hal::gpio::Port as GpioPort; -use crate::interrupt::{self, CriticalSection, OwnedInterrupt}; +use crate::interrupt::{self, OwnedInterrupt}; +use crate::pac; use crate::pac::uarte0; +use crate::util::peripheral; +use crate::util::ring_buffer::RingBuffer; // Re-export SVD variants to allow user to directly set values pub use crate::hal::uarte::Pins; pub use uarte0::{baudrate::BAUDRATE_A as Baudrate, config::PARITY_A as Parity}; -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, @@ -126,28 +48,12 @@ enum TxState { /// are disabled before using `Uarte`. See product specification: /// - nrf52832: Section 15.2 /// - nrf52840: Section 6.1.2 -pub struct BufferedUarte { - started: bool, - state: UnsafeCell>, +pub struct BufferedUarte<'a, T: Instance> { + reg: peripheral::Registration>, + wtf: PhantomData<&'a ()>, } -// 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: WakerRegistration, - - tx: RingBuf, - tx_state: TxState, - tx_waker: WakerRegistration, - - _pin: PhantomPinned, -} +impl<'a, T: Instance> Unpin for BufferedUarte<'a, T> {} #[cfg(any(feature = "52833", feature = "52840"))] fn port_bit(port: GpioPort) -> bool { @@ -157,10 +63,12 @@ fn port_bit(port: GpioPort) -> bool { } } -impl BufferedUarte { +impl<'a, T: Instance> BufferedUarte<'a, T> { pub fn new( uarte: T, irq: T::Interrupt, + rx_buffer: &'a mut [u8], + tx_buffer: &'a mut [u8], mut pins: Pins, parity: Parity, baudrate: Baudrate, @@ -218,87 +126,79 @@ impl BufferedUarte { // Configure frequency uarte.baudrate.write(|w| w.baudrate().variant(baudrate)); + irq.pend(); + BufferedUarte { - started: false, - state: UnsafeCell::new(UarteState { - inner: uarte, + reg: peripheral::Registration::new( irq, + State { + inner: uarte, - rx: RingBuf::new(), - rx_state: RxState::Idle, - rx_waker: WakerRegistration::new(), + rx: RingBuffer::new(rx_buffer), + rx_state: RxState::Idle, + rx_waker: WakerRegistration::new(), - tx: RingBuf::new(), - tx_state: TxState::Idle, - tx_waker: WakerRegistration::new(), - - _pin: PhantomPinned, - }), + tx: RingBuffer::new(tx_buffer), + tx_state: TxState::Idle, + tx_waker: WakerRegistration::new(), + }, + ), + wtf: PhantomData, } } - - 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 { +impl<'a, T: Instance> Drop for BufferedUarte<'a, T> { fn drop(&mut self) { // stop DMA before dropping, because DMA is using the buffer in `self`. todo!() } } -impl AsyncBufRead for BufferedUarte { +impl<'a, T: Instance> AsyncBufRead for BufferedUarte<'a, T> { fn poll_fill_buf(self: Pin<&mut Self>, cx: &mut Context<'_>) -> Poll> { - self.with_state(|s| s.poll_fill_buf(cx)) + let this = unsafe { self.get_unchecked_mut() }; + this.reg.with(|state, _| { + let z: Poll> = state.poll_fill_buf(cx); + let z: Poll> = unsafe { mem::transmute(z) }; + z + }) } 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() }; + this.reg.with(|state, irq| state.consume(irq, amt)) + } +} +impl<'a, T: Instance> AsyncWrite for BufferedUarte<'a, T> { + fn poll_write(self: Pin<&mut Self>, cx: &mut Context<'_>, buf: &[u8]) -> Poll> { + let this = unsafe { self.get_unchecked_mut() }; + this.reg.with(|state, irq| state.poll_write(irq, cx, buf)) + } +} + +// ==================================== +// ==================================== +// ==================================== + +// public because it needs to be used in Instance trait, but +// should not be used outside the module +#[doc(hidden)] +pub struct State<'a, T: Instance> { + inner: T, + + rx: RingBuffer<'a>, + rx_state: RxState, + rx_waker: WakerRegistration, + + tx: RingBuffer<'a>, + tx_state: TxState, + tx_waker: WakerRegistration, +} + +impl<'a, T: Instance> State<'a, T> { + fn poll_fill_buf(&mut self, cx: &mut Context<'_>) -> Poll> { // 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 @@ -306,7 +206,7 @@ impl UarteState { trace!("poll_read"); // We have data ready in buffer? Return it. - let buf = this.rx.pop_buf(); + let buf = self.rx.pop_buf(); if buf.len() != 0 { trace!(" got {:?} {:?}", buf.as_ptr() as u32, buf.len()); return Poll::Ready(Ok(buf)); @@ -314,38 +214,40 @@ impl UarteState { trace!(" empty"); - if this.rx_state == RxState::ReceivingReady { + if self.rx_state == RxState::ReceivingReady { trace!(" stopping"); - this.rx_state = RxState::Stopping; - this.inner.tasks_stoprx.write(|w| unsafe { w.bits(1) }); + self.rx_state = RxState::Stopping; + self.inner.tasks_stoprx.write(|w| unsafe { w.bits(1) }); } - this.rx_waker.register(cx.waker()); + self.rx_waker.register(cx.waker()); Poll::Pending } - fn consume(self: Pin<&mut Self>, amt: usize) { - let this = unsafe { self.get_unchecked_mut() }; + fn consume(&mut self, irq: &mut T::Interrupt, amt: usize) { trace!("consume {:?}", amt); - this.rx.pop(amt); - this.irq.pend(); + self.rx.pop(amt); + irq.pend(); } - fn poll_write(self: Pin<&mut Self>, cx: &mut Context<'_>, buf: &[u8]) -> Poll> { - let this = unsafe { self.get_unchecked_mut() }; - + fn poll_write( + &mut self, + irq: &mut T::Interrupt, + cx: &mut Context<'_>, + buf: &[u8], + ) -> Poll> { trace!("poll_write: {:?}", buf.len()); - let tx_buf = this.tx.push_buf(); + let tx_buf = self.tx.push_buf(); if tx_buf.len() == 0 { trace!("poll_write: pending"); - this.tx_waker.register(cx.waker()); + self.tx_waker.register(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); + self.tx.push(n); trace!("poll_write: queued {:?}", n); @@ -354,10 +256,17 @@ impl UarteState { // before any DMA action has started compiler_fence(Ordering::SeqCst); - this.irq.pend(); + irq.pend(); Poll::Ready(Ok(n)) } +} + +impl<'a, T: Instance> peripheral::State for State<'a, T> { + type Interrupt = T::Interrupt; + fn store<'b>() -> &'b peripheral::Store { + unsafe { mem::transmute(T::storage()) } + } fn on_interrupt(&mut self) { trace!("irq: start"); @@ -505,39 +414,27 @@ mod private { impl Sealed for crate::pac::UARTE1 {} } -pub trait Instance: Deref + Sized + private::Sealed { +pub trait Instance: + Deref + Sized + private::Sealed + 'static +{ type Interrupt: OwnedInterrupt; - - #[doc(hidden)] - fn get_state(_cs: &CriticalSection) -> *mut UarteState; - - #[doc(hidden)] - fn set_state(_cs: &CriticalSection, state: *mut UarteState); + fn storage() -> &'static peripheral::Store>; } -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 crate::pac::UARTE0 { +impl Instance for pac::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 + fn storage() -> &'static peripheral::Store> { + static STORAGE: peripheral::Store> = + peripheral::Store::uninit(); + &STORAGE } } -#[cfg(any(feature = "52833", feature = "52840", feature = "9160"))] -impl Instance for crate::pac::UARTE1 { +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 storage() -> &'static peripheral::Store> { + static STORAGE: peripheral::Store> = + peripheral::Store::uninit(); + &STORAGE } } diff --git a/embassy-nrf/src/lib.rs b/embassy-nrf/src/lib.rs index e97002a2..ac237176 100644 --- a/embassy-nrf/src/lib.rs +++ b/embassy-nrf/src/lib.rs @@ -50,6 +50,7 @@ pub use nrf52840_hal as hal; // This mod MUST go first, so that the others see its macros. pub(crate) mod fmt; +pub(crate) mod util; pub mod buffered_uarte; pub mod gpiote; diff --git a/embassy-nrf/src/util/mod.rs b/embassy-nrf/src/util/mod.rs new file mode 100644 index 00000000..2fd5453d --- /dev/null +++ b/embassy-nrf/src/util/mod.rs @@ -0,0 +1,2 @@ +pub mod peripheral; +pub mod ring_buffer; diff --git a/embassy-nrf/src/util/peripheral.rs b/embassy-nrf/src/util/peripheral.rs new file mode 100644 index 00000000..85de3419 --- /dev/null +++ b/embassy-nrf/src/util/peripheral.rs @@ -0,0 +1,107 @@ +use core::mem; +use core::mem::MaybeUninit; +use core::ptr; +use core::sync::atomic::{compiler_fence, Ordering}; +use core::{cell::UnsafeCell, marker::PhantomData}; + +use crate::interrupt::OwnedInterrupt; + +pub struct Store(MaybeUninit>); +impl Store { + pub const fn uninit() -> Self { + Self(MaybeUninit::uninit()) + } + + unsafe fn as_mut_ptr(&self) -> *mut T { + (*self.0.as_ptr()).get() + } + + unsafe fn as_mut(&self) -> &mut T { + &mut *self.as_mut_ptr() + } + + unsafe fn write(&self, val: T) { + ptr::write(self.as_mut_ptr(), val) + } + + unsafe fn drop_in_place(&self) { + ptr::drop_in_place(self.as_mut_ptr()) + } + + unsafe fn read(&self) -> T { + ptr::read(self.as_mut_ptr()) + } +} +unsafe impl Send for Store {} +unsafe impl Sync for Store {} + +pub trait State: Sized { + type Interrupt: OwnedInterrupt; + fn on_interrupt(&mut self); + #[doc(hidden)] + fn store<'a>() -> &'a Store; +} + +pub struct Registration { + irq: P::Interrupt, + not_send: PhantomData<*mut P>, +} + +impl Registration

{ + pub fn new(irq: P::Interrupt, state: P) -> Self { + // safety: + // - No other PeripheralRegistration can already exist because we have the owned interrupt + // - therefore, storage is uninitialized + // - therefore it's safe to overwrite it without dropping the previous contents + unsafe { P::store().write(state) } + + irq.set_handler(|| { + // safety: + // - If a PeripheralRegistration instance exists, P::storage() is initialized. + // - It's OK to get a &mut to it since the irq is disabled. + unsafe { P::store().as_mut() }.on_interrupt(); + }); + + compiler_fence(Ordering::SeqCst); + irq.enable(); + + Self { + irq, + not_send: PhantomData, + } + } + + pub fn with(&mut self, f: impl FnOnce(&mut P, &mut P::Interrupt) -> R) -> R { + self.irq.disable(); + compiler_fence(Ordering::SeqCst); + + // safety: + // - If a PeripheralRegistration instance exists, P::storage() is initialized. + // - It's OK to get a &mut to it since the irq is disabled. + let r = f(unsafe { P::store().as_mut() }, &mut self.irq); + + compiler_fence(Ordering::SeqCst); + self.irq.enable(); + + r + } + + pub fn free(self) -> (P::Interrupt, P) { + let irq = unsafe { ptr::read(&self.irq) }; + irq.disable(); + irq.set_handler(|| ()); + mem::forget(self); + let storage = P::store(); + (irq, unsafe { storage.read() }) + } +} + +impl Drop for Registration

{ + fn drop(&mut self) { + self.irq.disable(); + self.irq.set_handler(|| ()); + + let storage = P::store(); + unsafe { storage.drop_in_place() }; + } +} diff --git a/embassy-nrf/src/util/ring_buffer.rs b/embassy-nrf/src/util/ring_buffer.rs new file mode 100644 index 00000000..f395785a --- /dev/null +++ b/embassy-nrf/src/util/ring_buffer.rs @@ -0,0 +1,80 @@ +use crate::fmt::{assert, panic, todo, *}; + +pub struct RingBuffer<'a> { + buf: &'a mut [u8], + start: usize, + end: usize, + empty: bool, +} + +impl<'a> RingBuffer<'a> { + pub fn new(buf: &'a mut [u8]) -> Self { + Self { + buf, + start: 0, + end: 0, + empty: true, + } + } + + pub 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 { + self.buf.len() - self.end + } else { + self.start - self.end + }; + + trace!(" ringbuf: push_buf {:?}..{:?}", self.end, self.end + n); + &mut self.buf[self.end..self.end + n] + } + + pub fn push(&mut self, n: usize) { + trace!(" ringbuf: push {:?}", n); + if n == 0 { + return; + } + + self.end = self.wrap(self.end + n); + self.empty = false; + } + + pub 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 { + self.buf.len() - self.start + } else { + self.end - self.start + }; + + trace!(" ringbuf: pop_buf {:?}..{:?}", self.start, self.start + n); + &mut self.buf[self.start..self.start + n] + } + + pub 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(&self, n: usize) -> usize { + assert!(n <= self.buf.len()); + if n == self.buf.len() { + 0 + } else { + n + } + } +}