Move nRF's util into a separate crate

4 files changed, 335 insertions, 0 deletions

diff --git a/embassy-extras/src/fmt.rs b/embassy-extras/src/fmt.rs
new file mode 100644
index 000000000..1be1057a7
--- /dev/null
+++ b/embassy-extras/src/fmt.rs
@@ -0,0 +1,119 @@
+#![macro_use]
+#![allow(clippy::module_inception)]
+
+#[cfg(all(feature = "defmt", feature = "log"))]
+compile_error!("You may not enable both `defmt` and `log` features.");
+
+pub use fmt::*;
+
+#[cfg(feature = "defmt")]
+mod fmt {
+    pub use defmt::{
+        assert, assert_eq, assert_ne, debug, debug_assert, debug_assert_eq, debug_assert_ne, error,
+        info, panic, todo, trace, unreachable, unwrap, warn,
+    };
+}
+
+#[cfg(feature = "log")]
+mod fmt {
+    pub use core::{
+        assert, assert_eq, assert_ne, debug_assert, debug_assert_eq, debug_assert_ne, panic, todo,
+        unreachable,
+    };
+    pub use log::{debug, error, info, trace, warn};
+}
+
+#[cfg(not(any(feature = "defmt", feature = "log")))]
+mod fmt {
+    #![macro_use]
+
+    pub use core::{
+        assert, assert_eq, assert_ne, debug_assert, debug_assert_eq, debug_assert_ne, panic, todo,
+        unreachable,
+    };
+
+    #[macro_export]
+    macro_rules! trace {
+        ($($msg:expr),+ $(,)?) => {
+            ()
+        };
+    }
+
+    #[macro_export]
+    macro_rules! debug {
+        ($($msg:expr),+ $(,)?) => {
+            ()
+        };
+    }
+
+    #[macro_export]
+    macro_rules! info {
+        ($($msg:expr),+ $(,)?) => {
+            ()
+        };
+    }
+
+    #[macro_export]
+    macro_rules! warn {
+        ($($msg:expr),+ $(,)?) => {
+            ()
+        };
+    }
+
+    #[macro_export]
+    macro_rules! error {
+        ($($msg:expr),+ $(,)?) => {
+            ()
+        };
+    }
+}
+
+#[cfg(not(feature = "defmt"))]
+#[macro_export]
+macro_rules! unwrap {
+    ($arg:expr) => {
+        match $crate::fmt::Try::into_result($arg) {
+            ::core::result::Result::Ok(t) => t,
+            ::core::result::Result::Err(e) => {
+                ::core::panic!("unwrap of `{}` failed: {:?}", ::core::stringify!($arg), e);
+            }
+        }
+    };
+    ($arg:expr, $($msg:expr),+ $(,)? ) => {
+        match $crate::fmt::Try::into_result($arg) {
+            ::core::result::Result::Ok(t) => t,
+            ::core::result::Result::Err(e) => {
+                ::core::panic!("unwrap of `{}` failed: {}: {:?}", ::core::stringify!($arg), ::core::format_args!($($msg,)*), e);
+            }
+        }
+    }
+}
+
+#[derive(Debug, Copy, Clone, Eq, PartialEq)]
+pub struct NoneError;
+
+pub trait Try {
+    type Ok;
+    type Error;
+    fn into_result(self) -> Result<Self::Ok, Self::Error>;
+}
+
+impl<T> Try for Option<T> {
+    type Ok = T;
+    type Error = NoneError;
+
+    #[inline]
+    fn into_result(self) -> Result<T, NoneError> {
+        self.ok_or(NoneError)
+    }
+}
+
+impl<T, E> Try for Result<T, E> {
+    type Ok = T;
+    type Error = E;
+
+    #[inline]
+    fn into_result(self) -> Self {
+        self
+    }
+}
diff --git a/embassy-extras/src/lib.rs b/embassy-extras/src/lib.rs
new file mode 100644
index 000000000..4a95173cf
--- /dev/null
+++ b/embassy-extras/src/lib.rs
@@ -0,0 +1,17 @@
+#![no_std]
+
+// This mod MUST go first, so that the others see its macros.
+pub(crate) mod fmt;
+
+pub mod peripheral;
+pub mod ring_buffer;
+
+/// Low power blocking wait loop using WFE/SEV.
+pub fn low_power_wait_until(mut condition: impl FnMut() -> bool) {
+    while !condition() {
+        // WFE might "eat" an event that would have otherwise woken the executor.
+        cortex_m::asm::wfe();
+    }
+    // Retrigger an event to be transparent to the executor.
+    cortex_m::asm::sev();
+}
diff --git a/embassy-extras/src/peripheral.rs b/embassy-extras/src/peripheral.rs
new file mode 100644
index 000000000..e9357969d
--- /dev/null
+++ b/embassy-extras/src/peripheral.rs
@@ -0,0 +1,119 @@
+use core::cell::UnsafeCell;
+use core::marker::{PhantomData, PhantomPinned};
+use core::mem::MaybeUninit;
+use core::pin::Pin;
+use core::sync::atomic::{compiler_fence, Ordering};
+
+use embassy::interrupt::{Interrupt, InterruptExt};
+
+use crate::fmt::assert;
+
+pub trait PeripheralState {
+    type Interrupt: Interrupt;
+    fn on_interrupt(&mut self);
+}
+
+#[derive(Clone, Copy, PartialEq, Eq, Debug)]
+enum Life {
+    Ready,
+    Created,
+    Freed,
+}
+
+pub struct PeripheralMutex<S: PeripheralState> {
+    life: Life,
+
+    state: MaybeUninit<UnsafeCell<S>>, // Init if life != Freed
+    irq: MaybeUninit<S::Interrupt>,    // Init if life != Freed
+
+    _not_send: PhantomData<*mut ()>,
+    _pinned: PhantomPinned,
+}
+
+impl<S: PeripheralState> PeripheralMutex<S> {
+    pub fn new(state: S, irq: S::Interrupt) -> Self {
+        Self {
+            life: Life::Created,
+            state: MaybeUninit::new(UnsafeCell::new(state)),
+            irq: MaybeUninit::new(irq),
+            _not_send: PhantomData,
+            _pinned: PhantomPinned,
+        }
+    }
+
+    /// safety: self must be pinned.
+    unsafe fn setup(&mut self) {
+        assert!(self.life == Life::Created);
+
+        let irq = &mut *self.irq.as_mut_ptr();
+        irq.disable();
+        compiler_fence(Ordering::SeqCst);
+
+        irq.set_handler(|p| {
+            // Safety: it's OK to get a &mut to the state, since
+            // - We're in the IRQ, no one else can't preempt us
+            // - We can't have preempted a with() call because the irq is disabled during it.
+            let state = &mut *(p as *mut S);
+            state.on_interrupt();
+        });
+        irq.set_handler_context(self.state.as_mut_ptr() as *mut ());
+
+        compiler_fence(Ordering::SeqCst);
+        irq.enable();
+
+        self.life = Life::Ready;
+    }
+
+    pub fn with<R>(self: Pin<&mut Self>, f: impl FnOnce(&mut S, &mut S::Interrupt) -> R) -> R {
+        let this = unsafe { self.get_unchecked_mut() };
+        if this.life != Life::Ready {
+            unsafe { this.setup() }
+        }
+
+        let irq = unsafe { &mut *this.irq.as_mut_ptr() };
+
+        irq.disable();
+        compiler_fence(Ordering::SeqCst);
+
+        // Safety: it's OK to get a &mut to the state, since the irq is disabled.
+        let state = unsafe { &mut *(*this.state.as_ptr()).get() };
+
+        let r = f(state, irq);
+
+        compiler_fence(Ordering::SeqCst);
+        irq.enable();
+
+        r
+    }
+
+    pub fn try_free(self: Pin<&mut Self>) -> Option<(S, S::Interrupt)> {
+        let this = unsafe { self.get_unchecked_mut() };
+
+        if this.life != Life::Freed {
+            return None;
+        }
+
+        unsafe { &mut *this.irq.as_mut_ptr() }.disable();
+        compiler_fence(Ordering::SeqCst);
+
+        this.life = Life::Freed;
+
+        let state = unsafe { this.state.as_ptr().read().into_inner() };
+        let irq = unsafe { this.irq.as_ptr().read() };
+        Some((state, irq))
+    }
+
+    pub fn free(self: Pin<&mut Self>) -> (S, S::Interrupt) {
+        unwrap!(self.try_free())
+    }
+}
+
+impl<S: PeripheralState> Drop for PeripheralMutex<S> {
+    fn drop(&mut self) {
+        if self.life != Life::Freed {
+            let irq = unsafe { &mut *self.irq.as_mut_ptr() };
+            irq.disable();
+            irq.remove_handler();
+        }
+    }
+}
diff --git a/embassy-extras/src/ring_buffer.rs b/embassy-extras/src/ring_buffer.rs
new file mode 100644
index 000000000..f2b9f7359
--- /dev/null
+++ b/embassy-extras/src/ring_buffer.rs
@@ -0,0 +1,80 @@
+use crate::fmt::{assert, *};
+
+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
+        }
+    }
+}