Merge pull request #1667 from quentinmit/nrf-pdm

nrf/pdm: Add continuous sampling API

4 files changed, 333 insertions, 23 deletions

diff --git a/embassy-nrf/src/pdm.rs b/embassy-nrf/src/pdm.rs
index efa1fbccc..217884d1c 100644
--- a/embassy-nrf/src/pdm.rs
+++ b/embassy-nrf/src/pdm.rs
@@ -8,12 +8,22 @@ use core::task::Poll;
 
 use embassy_hal_common::drop::OnDrop;
 use embassy_hal_common::{into_ref, PeripheralRef};
+use fixed::types::I7F1;
 use futures::future::poll_fn;
 
 use crate::chip::EASY_DMA_SIZE;
 use crate::gpio::sealed::Pin;
 use crate::gpio::{AnyPin, Pin as GpioPin};
 use crate::interrupt::typelevel::Interrupt;
+use crate::pac::pdm::mode::{EDGE_A, OPERATION_A};
+pub use crate::pac::pdm::pdmclkctrl::FREQ_A as Frequency;
+#[cfg(any(
+    feature = "nrf52840",
+    feature = "nrf52833",
+    feature = "_nrf5340-app",
+    feature = "_nrf9160",
+))]
+pub use crate::pac::pdm::ratio::RATIO_A as Ratio;
 use crate::{interrupt, Peripheral};
 
 /// Interrupt handler.
@@ -23,7 +33,20 @@ pub struct InterruptHandler<T: Instance> {
 
 impl<T: Instance> interrupt::typelevel::Handler<T::Interrupt> for InterruptHandler<T> {
     unsafe fn on_interrupt() {
-        T::regs().intenclr.write(|w| w.end().clear());
+        let r = T::regs();
+
+        if r.events_end.read().bits() != 0 {
+            r.intenclr.write(|w| w.end().clear());
+        }
+
+        if r.events_started.read().bits() != 0 {
+            r.intenclr.write(|w| w.started().clear());
+        }
+
+        if r.events_stopped.read().bits() != 0 {
+            r.intenclr.write(|w| w.stopped().clear());
+        }
+
         T::state().waker.wake();
     }
 }
@@ -44,10 +67,24 @@ pub enum Error {
     BufferZeroLength,
     /// PDM is not running
     NotRunning,
+    /// PDM is already running
+    AlreadyRunning,
 }
 
 static DUMMY_BUFFER: [i16; 1] = [0; 1];
 
+/// The state of a continuously running sampler. While it reflects
+/// the progress of a sampler, it also signals what should be done
+/// next. For example, if the sampler has stopped then the Pdm implementation
+/// can then tear down its infrastructure.
+#[derive(PartialEq)]
+pub enum SamplerState {
+    /// The sampler processed the samples and is ready for more.
+    Sampled,
+    /// The sampler is done processing samples.
+    Stopped,
+}
+
 impl<'d, T: Instance> Pdm<'d, T> {
     /// Create PDM driver
     pub fn new(
@@ -79,18 +116,24 @@ impl<'d, T: Instance> Pdm<'d, T> {
         r.psel.clk.write(|w| unsafe { w.bits(clk.psel_bits()) });
 
         // configure
-        // use default for
-        // - gain right
-        // - gain left
-        // - clk
-        // - ratio
+        r.pdmclkctrl.write(|w| w.freq().variant(config.frequency));
+        #[cfg(any(
+            feature = "nrf52840",
+            feature = "nrf52833",
+            feature = "_nrf5340-app",
+            feature = "_nrf9160",
+        ))]
+        r.ratio.write(|w| w.ratio().variant(config.ratio));
         r.mode.write(|w| {
-            w.edge().bit(config.edge == Edge::LeftRising);
-            w.operation().bit(config.operation_mode == OperationMode::Mono);
+            w.operation().variant(config.operation_mode.into());
+            w.edge().variant(config.edge.into());
             w
         });
-        r.gainl.write(|w| w.gainl().default_gain());
-        r.gainr.write(|w| w.gainr().default_gain());
+
+        Self::_set_gain(r, config.gain_left, config.gain_right);
+
+        // Disable all events interrupts
+        r.intenclr.write(|w| unsafe { w.bits(0x003F_FFFF) });
 
         // IRQ
         T::Interrupt::unpend();
@@ -101,6 +144,25 @@ impl<'d, T: Instance> Pdm<'d, T> {
         Self { _peri: pdm }
     }
 
+    fn _set_gain(r: &crate::pac::pdm::RegisterBlock, gain_left: I7F1, gain_right: I7F1) {
+        let gain_left = gain_left
+            .saturating_add(I7F1::from_bits(40))
+            .saturating_to_num::<u8>()
+            .clamp(0, 0x50);
+        let gain_right = gain_right
+            .saturating_add(I7F1::from_bits(40))
+            .saturating_to_num::<u8>()
+            .clamp(0, 0x50);
+
+        r.gainl.write(|w| unsafe { w.gainl().bits(gain_left) });
+        r.gainr.write(|w| unsafe { w.gainr().bits(gain_right) });
+    }
+
+    /// Adjust the gain of the PDM microphone on the fly
+    pub fn set_gain(&mut self, gain_left: I7F1, gain_right: I7F1) {
+        Self::_set_gain(T::regs(), gain_left, gain_right)
+    }
+
     /// Start sampling microphon data into a dummy buffer
     /// Usefull to start the microphon and keep it active between recording samples
     pub async fn start(&mut self) {
@@ -198,6 +260,108 @@ impl<'d, T: Instance> Pdm<'d, T> {
 
         compiler_fence(Ordering::SeqCst);
     }
+
+    /// Continuous sampling with double buffers.
+    ///
+    /// A sampler closure is provided that receives the buffer of samples, noting
+    /// that the size of this buffer can be less than the original buffer's size.
+    /// A command is return from the closure that indicates whether the sampling
+    /// should continue or stop.
+    ///
+    /// NOTE: The time spent within the callback supplied should not exceed the time
+    /// taken to acquire the samples into a single buffer. You should measure the
+    /// time taken by the callback and set the sample buffer size accordingly.
+    /// Exceeding this time can lead to samples becoming dropped.
+    pub async fn run_task_sampler<S, const N: usize>(
+        &mut self,
+        bufs: &mut [[i16; N]; 2],
+        mut sampler: S,
+    ) -> Result<(), Error>
+    where
+        S: FnMut(&[i16; N]) -> SamplerState,
+    {
+        let r = T::regs();
+
+        if r.events_started.read().bits() != 0 {
+            return Err(Error::AlreadyRunning);
+        }
+
+        r.sample
+            .ptr
+            .write(|w| unsafe { w.sampleptr().bits(bufs[0].as_mut_ptr() as u32) });
+        r.sample.maxcnt.write(|w| unsafe { w.buffsize().bits(N as _) });
+
+        // Reset and enable the events
+        r.events_end.reset();
+        r.events_started.reset();
+        r.events_stopped.reset();
+        r.intenset.write(|w| {
+            w.end().set();
+            w.started().set();
+            w.stopped().set();
+            w
+        });
+
+        // Don't reorder the start event before the previous writes. Hopefully self
+        // wouldn't happen anyway.
+        compiler_fence(Ordering::SeqCst);
+
+        r.tasks_start.write(|w| unsafe { w.bits(1) });
+
+        let mut current_buffer = 0;
+
+        let mut done = false;
+
+        let drop = OnDrop::new(|| {
+            r.tasks_stop.write(|w| unsafe { w.bits(1) });
+            // N.B. It would be better if this were async, but Drop only support sync code.
+            while r.events_stopped.read().bits() != 0 {}
+        });
+
+        // Wait for events and complete when the sampler indicates it has had enough.
+        poll_fn(|cx| {
+            let r = T::regs();
+
+            T::state().waker.register(cx.waker());
+
+            if r.events_end.read().bits() != 0 {
+                compiler_fence(Ordering::SeqCst);
+
+                r.events_end.reset();
+                r.intenset.write(|w| w.end().set());
+
+                if !done {
+                    // Discard the last buffer after the user requested a stop.
+                    if sampler(&bufs[current_buffer]) == SamplerState::Sampled {
+                        let next_buffer = 1 - current_buffer;
+                        current_buffer = next_buffer;
+                    } else {
+                        r.tasks_stop.write(|w| unsafe { w.bits(1) });
+                        done = true;
+                    };
+                };
+            }
+
+            if r.events_started.read().bits() != 0 {
+                r.events_started.reset();
+                r.intenset.write(|w| w.started().set());
+
+                let next_buffer = 1 - current_buffer;
+                r.sample
+                    .ptr
+                    .write(|w| unsafe { w.sampleptr().bits(bufs[next_buffer].as_mut_ptr() as u32) });
+            }
+
+            if r.events_stopped.read().bits() != 0 {
+                return Poll::Ready(());
+            }
+
+            Poll::Pending
+        })
+        .await;
+        drop.defuse();
+        Ok(())
+    }
 }
 
 /// PDM microphone driver Config
@@ -206,6 +370,20 @@ pub struct Config {
     pub operation_mode: OperationMode,
     /// On which edge the left channel should be samples
     pub edge: Edge,
+    /// Clock frequency
+    pub frequency: Frequency,
+    /// Clock ratio
+    #[cfg(any(
+        feature = "nrf52840",
+        feature = "nrf52833",
+        feature = "_nrf5340-app",
+        feature = "_nrf9160",
+    ))]
+    pub ratio: Ratio,
+    /// Gain left in dB
+    pub gain_left: I7F1,
+    /// Gain right in dB
+    pub gain_right: I7F1,
 }
 
 impl Default for Config {
@@ -213,6 +391,16 @@ impl Default for Config {
         Self {
             operation_mode: OperationMode::Mono,
             edge: Edge::LeftFalling,
+            frequency: Frequency::DEFAULT,
+            #[cfg(any(
+                feature = "nrf52840",
+                feature = "nrf52833",
+                feature = "_nrf5340-app",
+                feature = "_nrf9160",
+            ))]
+            ratio: Ratio::RATIO80,
+            gain_left: I7F1::ZERO,
+            gain_right: I7F1::ZERO,
         }
     }
 }
@@ -226,6 +414,15 @@ pub enum OperationMode {
     Stereo,
 }
 
+impl From<OperationMode> for OPERATION_A {
+    fn from(mode: OperationMode) -> Self {
+        match mode {
+            OperationMode::Mono => OPERATION_A::MONO,
+            OperationMode::Stereo => OPERATION_A::STEREO,
+        }
+    }
+}
+
 /// PDM edge polarity
 #[derive(PartialEq)]
 pub enum Edge {
@@ -235,6 +432,15 @@ pub enum Edge {
     LeftFalling,
 }
 
+impl From<Edge> for EDGE_A {
+    fn from(edge: Edge) -> Self {
+        match edge {
+            Edge::LeftRising => EDGE_A::LEFT_RISING,
+            Edge::LeftFalling => EDGE_A::LEFT_FALLING,
+        }
+    }
+}
+
 impl<'d, T: Instance> Drop for Pdm<'d, T> {
     fn drop(&mut self) {
         let r = T::regs();
diff --git a/examples/nrf52840/Cargo.toml b/examples/nrf52840/Cargo.toml
index 7b9c371bb..9b41ec5ab 100644
--- a/examples/nrf52840/Cargo.toml
+++ b/examples/nrf52840/Cargo.toml
@@ -43,6 +43,7 @@ embassy-net-esp-hosted = { version = "0.1.0", path = "../../embassy-net-esp-host
 defmt = "0.3"
 defmt-rtt = "0.4"
 
+fixed = "1.10.0"
 static_cell = "1.1"
 cortex-m = { version = "0.7.6", features = ["inline-asm", "critical-section-single-core"] }
 cortex-m-rt = "0.7.0"
@@ -53,6 +54,8 @@ embedded-storage = "0.3.0"
 usbd-hid = "0.6.0"
 serde = { version = "1.0.136", default-features = false }
 embedded-hal-async = { version = "0.2.0-alpha.2", optional = true }
+num-integer = { version = "0.1.45", default-features = false }
+microfft = "0.5.0"
 
 [patch.crates-io]
 lora-phy = { git = "https://github.com/embassy-rs/lora-phy", rev = "ad289428fd44b02788e2fa2116445cc8f640a265" }
diff --git a/examples/nrf52840/src/bin/pdm.rs b/examples/nrf52840/src/bin/pdm.rs
index 6b41320ca..444b9137f 100644
--- a/examples/nrf52840/src/bin/pdm.rs
+++ b/examples/nrf52840/src/bin/pdm.rs
@@ -7,6 +7,8 @@ use embassy_executor::Spawner;
 use embassy_nrf::pdm::{self, Config, Pdm};
 use embassy_nrf::{bind_interrupts, peripherals};
 use embassy_time::{Duration, Timer};
+use fixed::types::I7F1;
+use num_integer::Roots;
 use {defmt_rtt as _, panic_probe as _};
 
 bind_interrupts!(struct Irqs {
@@ -20,18 +22,36 @@ async fn main(_p: Spawner) {
     let mut pdm = Pdm::new(p.PDM, Irqs, p.P0_01, p.P0_00, config);
 
     loop {
-        pdm.start().await;
-
-        // wait some time till the microphon settled
-        Timer::after(Duration::from_millis(1000)).await;
-
-        const SAMPLES: usize = 2048;
-        let mut buf = [0i16; SAMPLES];
-        pdm.sample(&mut buf).await.unwrap();
-
-        info!("samples: {:?}", &buf);
-
-        pdm.stop().await;
-        Timer::after(Duration::from_millis(100)).await;
+        for gain in [I7F1::from_num(-20), I7F1::from_num(0), I7F1::from_num(20)] {
+            pdm.set_gain(gain, gain);
+            info!("Gain = {} dB", defmt::Debug2Format(&gain));
+            pdm.start().await;
+
+            // wait some time till the microphon settled
+            Timer::after(Duration::from_millis(1000)).await;
+
+            const SAMPLES: usize = 2048;
+            let mut buf = [0i16; SAMPLES];
+            pdm.sample(&mut buf).await.unwrap();
+
+            let mean = (buf.iter().map(|v| i32::from(*v)).sum::<i32>() / buf.len() as i32) as i16;
+            info!(
+                "{} samples, min {=i16}, max {=i16}, mean {=i16}, AC RMS {=i16}",
+                buf.len(),
+                buf.iter().min().unwrap(),
+                buf.iter().max().unwrap(),
+                mean,
+                (buf.iter()
+                    .map(|v| i32::from(*v - mean).pow(2))
+                    .fold(0i32, |a, b| a.saturating_add(b))
+                    / buf.len() as i32)
+                    .sqrt() as i16,
+            );
+
+            info!("samples: {:?}", &buf);
+
+            pdm.stop().await;
+            Timer::after(Duration::from_millis(100)).await;
+        }
     }
 }
diff --git a/examples/nrf52840/src/bin/pdm_continuous.rs b/examples/nrf52840/src/bin/pdm_continuous.rs
new file mode 100644
index 000000000..7d8531475
--- /dev/null
+++ b/examples/nrf52840/src/bin/pdm_continuous.rs
@@ -0,0 +1,81 @@
+#![no_std]
+#![no_main]
+#![feature(type_alias_impl_trait)]
+
+use core::cmp::Ordering;
+
+use defmt::info;
+use embassy_executor::Spawner;
+use embassy_nrf::pdm::{self, Config, Frequency, OperationMode, Pdm, Ratio, SamplerState};
+use embassy_nrf::{bind_interrupts, peripherals};
+use fixed::types::I7F1;
+use microfft::real::rfft_1024;
+use num_integer::Roots;
+use {defmt_rtt as _, panic_probe as _};
+
+// Demonstrates both continuous sampling and scanning multiple channels driven by a PPI linked timer
+
+bind_interrupts!(struct Irqs {
+    PDM => pdm::InterruptHandler<peripherals::PDM>;
+});
+
+#[embassy_executor::main]
+async fn main(_p: Spawner) {
+    let mut p = embassy_nrf::init(Default::default());
+    let mut config = Config::default();
+    // Pins are correct for the onboard microphone on the Feather nRF52840 Sense.
+    config.frequency = Frequency::_1280K; // 16 kHz sample rate
+    config.ratio = Ratio::RATIO80;
+    config.operation_mode = OperationMode::Mono;
+    config.gain_left = I7F1::from_bits(5); // 2.5 dB
+    let mut pdm = Pdm::new(p.PDM, Irqs, &mut p.P0_00, &mut p.P0_01, config);
+
+    let mut bufs = [[0; 1024]; 2];
+
+    pdm.run_task_sampler(&mut bufs, move |buf| {
+        // NOTE: It is important that the time spent within this callback
+        // does not exceed the time taken to acquire the 1500 samples we
+        // have in this example, which would be 10us + 2us per
+        // sample * 1500 = 18ms. You need to measure the time taken here
+        // and set the sample buffer size accordingly. Exceeding this
+        // time can lead to the peripheral re-writing the other buffer.
+        let mean = (buf.iter().map(|v| i32::from(*v)).sum::<i32>() / buf.len() as i32) as i16;
+        let (peak_freq_index, peak_mag) = fft_peak_freq(&buf);
+        let peak_freq = peak_freq_index * 16000 / buf.len();
+        info!(
+            "{} samples, min {=i16}, max {=i16}, mean {=i16}, AC RMS {=i16}, peak {} @ {} Hz",
+            buf.len(),
+            buf.iter().min().unwrap(),
+            buf.iter().max().unwrap(),
+            mean,
+            (buf.iter()
+                .map(|v| i32::from(*v - mean).pow(2))
+                .fold(0i32, |a, b| a.saturating_add(b))
+                / buf.len() as i32)
+                .sqrt() as i16,
+            peak_mag,
+            peak_freq,
+        );
+        SamplerState::Sampled
+    })
+    .await
+    .unwrap();
+}
+
+fn fft_peak_freq(input: &[i16; 1024]) -> (usize, u32) {
+    let mut f = [0f32; 1024];
+    for i in 0..input.len() {
+        f[i] = (input[i] as f32) / 32768.0;
+    }
+    // N.B. rfft_1024 does the FFT in-place so result is actually also a reference to f.
+    let result = rfft_1024(&mut f);
+    result[0].im = 0.0;
+
+    result
+        .iter()
+        .map(|c| c.norm_sqr())
+        .enumerate()
+        .max_by(|(_, a), (_, b)| a.partial_cmp(b).unwrap_or(Ordering::Equal))
+        .map(|(i, v)| (i, ((v * 32768.0) as u32).sqrt()))
+        .unwrap()
+}