Interrupts, async, sine oscillator

3 files changed, 329 insertions, 101 deletions

diff --git a/embassy-nrf/src/i2s.rs b/embassy-nrf/src/i2s.rs
index fb6fa4bdf..f5e36f0dd 100644
--- a/embassy-nrf/src/i2s.rs
+++ b/embassy-nrf/src/i2s.rs
@@ -2,17 +2,18 @@
 
 //! I2S
 
-//use core::future::poll_fn;
-//use core::sync::atomic::{compiler_fence, Ordering};
-//use core::task::Poll;
+use core::future::poll_fn;
+use core::sync::atomic::{compiler_fence, Ordering};
+use core::task::Poll;
 
-//use embassy_hal_common::drop::OnDrop;
+use embassy_cortex_m::interrupt::{InterruptExt, Priority};
+use embassy_hal_common::drop::OnDrop;
 use embassy_hal_common::{into_ref, PeripheralRef};
 
 //use crate::gpio::sealed::Pin as _;
 use crate::gpio::{AnyPin, Pin as GpioPin};
 use crate::interrupt::Interrupt;
-use crate::pac::i2s::CONFIG;
+use crate::pac::i2s::{RegisterBlock, CONFIG, PSEL};
 use crate::Peripheral;
 
 // TODO: Define those in lib.rs somewhere else
@@ -35,10 +36,39 @@ pub enum Error {
     // TODO: add other error variants.
 }
 
+pub const MODE_MASTER_8000: Mode = Mode::Master {
+    freq: MckFreq::_32MDiv125,
+    ratio: Ratio::_32x,
+}; // error = 0
+pub const MODE_MASTER_11025: Mode = Mode::Master {
+    freq: MckFreq::_32MDiv15,
+    ratio: Ratio::_192x,
+}; // error = 86
+pub const MODE_MASTER_16000: Mode = Mode::Master {
+    freq: MckFreq::_32MDiv21,
+    ratio: Ratio::_96x,
+}; // error = 127
+pub const MODE_MASTER_22050: Mode = Mode::Master {
+    freq: MckFreq::_32MDiv15,
+    ratio: Ratio::_96x,
+}; // error = 172
+pub const MODE_MASTER_32000: Mode = Mode::Master {
+    freq: MckFreq::_32MDiv21,
+    ratio: Ratio::_48x,
+}; // error = 254
+pub const MODE_MASTER_44100: Mode = Mode::Master {
+    freq: MckFreq::_32MDiv15,
+    ratio: Ratio::_48x,
+}; // error = 344
+pub const MODE_MASTER_48000: Mode = Mode::Master {
+    freq: MckFreq::_32MDiv21,
+    ratio: Ratio::_32x,
+}; // error = 381
+
 #[derive(Clone)]
 #[non_exhaustive]
 pub struct Config {
-    pub ratio: Ratio,
+    pub mode: Mode,
     pub swidth: SampleWidth,
     pub align: Align,
     pub format: Format,
@@ -48,7 +78,7 @@ pub struct Config {
 impl Default for Config {
     fn default() -> Self {
         Self {
-            ratio: Ratio::_32x,
+            mode: MODE_MASTER_32000,
             swidth: SampleWidth::_16bit,
             align: Align::Left,
             format: Format::I2S,
@@ -57,6 +87,66 @@ impl Default for Config {
     }
 }
 
+/// I2S Mode
+#[derive(Debug, Eq, PartialEq, Clone, Copy)]
+pub enum Mode {
+    Master { freq: MckFreq, ratio: Ratio },
+    Slave,
+}
+
+impl Mode {
+    pub fn sample_rate(&self) -> Option<u32> {
+        match self {
+            Mode::Master { freq, ratio } => Some(freq.to_frequency() / ratio.to_divisor()),
+            Mode::Slave => None,
+        }
+    }
+}
+
+/// Master clock generator frequency.
+#[derive(Debug, Eq, PartialEq, Clone, Copy)]
+pub enum MckFreq {
+    _32MDiv8,
+    _32MDiv10,
+    _32MDiv11,
+    _32MDiv15,
+    _32MDiv16,
+    _32MDiv21,
+    _32MDiv23,
+    _32MDiv30,
+    _32MDiv31,
+    _32MDiv32,
+    _32MDiv42,
+    _32MDiv63,
+    _32MDiv125,
+}
+
+impl MckFreq {
+    const REGISTER_VALUES: &[u32] = &[
+        0x20000000, 0x18000000, 0x16000000, 0x11000000, 0x10000000, 0x0C000000, 0x0B000000, 0x08800000, 0x08400000,
+        0x08000000, 0x06000000, 0x04100000, 0x020C0000,
+    ];
+
+    const FREQUENCIES: &[u32] = &[
+        4000000, 3200000, 2909090, 2133333, 2000000, 1523809, 1391304, 1066666, 1032258, 1000000, 761904, 507936,
+        256000,
+    ];
+
+    pub fn to_register_value(&self) -> u32 {
+        Self::REGISTER_VALUES[usize::from(*self)]
+    }
+
+    pub fn to_frequency(&self) -> u32 {
+        Self::FREQUENCIES[usize::from(*self)]
+    }
+}
+
+impl From<MckFreq> for usize {
+    fn from(variant: MckFreq) -> Self {
+        variant as _
+    }
+}
+
 /// MCK / LRCK ratio.
 #[derive(Debug, Eq, PartialEq, Clone, Copy)]
 pub enum Ratio {
@@ -71,6 +161,14 @@ pub enum Ratio {
     _512x,
 }
 
+impl Ratio {
+    const RATIOS: &[u32] = &[32, 48, 64, 96, 128, 192, 256, 384, 512];
+
+    pub fn to_divisor(&self) -> u32 {
+        Self::RATIOS[u8::from(*self) as usize]
+    }
+}
+
 impl From<Ratio> for u8 {
     fn from(variant: Ratio) -> Self {
         variant as _
@@ -136,31 +234,6 @@ impl From<Channels> for u8 {
     }
 }
 
-/// I2S Mode
-#[derive(Debug, Eq, PartialEq, Clone, Copy)]
-pub enum Mode {
-    Controller,
-    Peripheral,
-}
-
-// /// Master clock generator frequency.
-// #[derive(Debug, Eq, PartialEq, Clone, Copy)]
-// pub enum MckFreq {
-//     _32MDiv8 = 0x20000000,
-//     _32MDiv10 = 0x18000000,
-//     _32MDiv11 = 0x16000000,
-//     _32MDiv15 = 0x11000000,
-//     _32MDiv16 = 0x10000000,
-//     _32MDiv21 = 0x0C000000,
-//     _32MDiv23 = 0x0B000000,
-//     _32MDiv30 = 0x08800000,
-//     _32MDiv31 = 0x08400000,
-//     _32MDiv32 = 0x08000000,
-//     _32MDiv42 = 0x06000000,
-//     _32MDiv63 = 0x04100000,
-//     _32MDiv125 = 0x020C0000,
-// }
-
 /// Interface to the UARTE peripheral using EasyDMA to offload the transmission and reception workload.
 ///
 /// For more details about EasyDMA, consult the module documentation.
@@ -185,7 +258,7 @@ impl<'d, T: Instance> I2S<'d, T> {
     /// Create a new I2S
     pub fn new(
         i2s: impl Peripheral<P = T> + 'd,
-        // irq: impl Peripheral<P = T::Interrupt> + 'd,
+        irq: impl Peripheral<P = T::Interrupt> + 'd,
         mck: impl Peripheral<P = impl GpioPin> + 'd,
         sck: impl Peripheral<P = impl GpioPin> + 'd,
         lrck: impl Peripheral<P = impl GpioPin> + 'd,
@@ -196,7 +269,7 @@ impl<'d, T: Instance> I2S<'d, T> {
         into_ref!(mck, sck, lrck, sdin, sdout);
         Self::new_inner(
             i2s,
-            // irq,
+            irq,
             mck.map_into(),
             sck.map_into(),
             lrck.map_into(),
@@ -208,7 +281,7 @@ impl<'d, T: Instance> I2S<'d, T> {
 
     fn new_inner(
         i2s: impl Peripheral<P = T> + 'd,
-        // irq: impl Peripheral<P = T::Interrupt> + 'd,
+        irq: impl Peripheral<P = T::Interrupt> + 'd,
         mck: PeripheralRef<'d, AnyPin>,
         sck: PeripheralRef<'d, AnyPin>,
         lrck: PeripheralRef<'d, AnyPin>,
@@ -216,36 +289,12 @@ impl<'d, T: Instance> I2S<'d, T> {
         sdout: PeripheralRef<'d, AnyPin>,
         config: Config,
     ) -> Self {
-        into_ref!(i2s, /* irq, */ mck, sck, lrck, sdin, sdout);
+        into_ref!(i2s, irq, mck, sck, lrck, sdin, sdout);
 
         let r = T::regs();
-
         Self::apply_config(&r.config, &config);
-
-        r.psel.mck.write(|w| {
-            unsafe { w.bits(mck.psel_bits()) };
-            w.connect().connected()
-        });
-
-        r.psel.sck.write(|w| {
-            unsafe { w.bits(sck.psel_bits()) };
-            w.connect().connected()
-        });
-
-        r.psel.lrck.write(|w| {
-            unsafe { w.bits(lrck.psel_bits()) };
-            w.connect().connected()
-        });
-
-        r.psel.sdin.write(|w| {
-            unsafe { w.bits(sdin.psel_bits()) };
-            w.connect().connected()
-        });
-
-        r.psel.sdout.write(|w| {
-            unsafe { w.bits(sdout.psel_bits()) };
-            w.connect().connected()
-        });
+        Self::select_pins(&r.psel, mck, sck, lrck, sdin, sdout);
+        Self::setup_interrupt(irq, r);
 
         r.enable.write(|w| w.enable().enabled());
 
@@ -322,19 +371,87 @@ impl<'d, T: Instance> I2S<'d, T> {
         self.input.rx(buffer).await
     }
 
+    fn on_interrupt(_: *mut ()) {
+        let r = T::regs();
+        let s = T::state();
+
+        if r.events_txptrupd.read().bits() != 0 {
+            s.tx_waker.wake();
+            r.intenclr.write(|w| w.txptrupd().clear());
+        }
+
+        if r.events_rxptrupd.read().bits() != 0 {
+            s.rx_waker.wake();
+            r.intenclr.write(|w| w.rxptrupd().clear());
+        }
+    }
+
     fn apply_config(c: &CONFIG, config: &Config) {
-        // TODO support slave too
-        c.mcken.write(|w| w.mcken().enabled());
-        c.mckfreq.write(|w| w.mckfreq()._32mdiv16());
-        c.mode.write(|w| w.mode().master());
+        match config.mode {
+            Mode::Master { freq, ratio } => {
+                c.mode.write(|w| w.mode().master());
+                c.mcken.write(|w| w.mcken().enabled());
+                c.mckfreq
+                    .write(|w| unsafe { w.mckfreq().bits(freq.to_register_value()) });
+                c.ratio.write(|w| unsafe { w.ratio().bits(ratio.into()) });
+            }
+            Mode::Slave => {
+                c.mode.write(|w| w.mode().slave());
+            }
+        };
 
-        c.ratio.write(|w| unsafe { w.ratio().bits(config.ratio.into()) });
         c.swidth.write(|w| unsafe { w.swidth().bits(config.swidth.into()) });
         c.align.write(|w| w.align().bit(config.align.into()));
         c.format.write(|w| w.format().bit(config.format.into()));
         c.channels
             .write(|w| unsafe { w.channels().bits(config.channels.into()) });
     }
+
+    fn select_pins(
+        psel: &PSEL,
+        mck: PeripheralRef<'d, AnyPin>,
+        sck: PeripheralRef<'d, AnyPin>,
+        lrck: PeripheralRef<'d, AnyPin>,
+        sdin: PeripheralRef<'d, AnyPin>,
+        sdout: PeripheralRef<'d, AnyPin>,
+    ) {
+        psel.mck.write(|w| {
+            unsafe { w.bits(mck.psel_bits()) };
+            w.connect().connected()
+        });
+
+        psel.sck.write(|w| {
+            unsafe { w.bits(sck.psel_bits()) };
+            w.connect().connected()
+        });
+
+        psel.lrck.write(|w| {
+            unsafe { w.bits(lrck.psel_bits()) };
+            w.connect().connected()
+        });
+
+        psel.sdin.write(|w| {
+            unsafe { w.bits(sdin.psel_bits()) };
+            w.connect().connected()
+        });
+
+        psel.sdout.write(|w| {
+            unsafe { w.bits(sdout.psel_bits()) };
+            w.connect().connected()
+        });
+    }
+
+    fn setup_interrupt(irq: PeripheralRef<'d, T::Interrupt>, r: &RegisterBlock) {
+        irq.set_handler(Self::on_interrupt);
+        irq.set_priority(Priority::P1); // TODO review priorities
+        irq.unpend();
+        irq.enable();
+
+        r.intenclr.write(|w| w.rxptrupd().clear());
+        r.intenclr.write(|w| w.txptrupd().clear());
+        r.events_rxptrupd.reset();
+        r.events_txptrupd.reset();
+    }
 }
 
 impl<'d, T: Instance> I2sOutput<'d, T> {
@@ -360,15 +477,40 @@ impl<'d, T: Instance> I2sOutput<'d, T> {
         }
 
         let r = T::regs();
-        let _s = T::state();
+        let s = T::state();
 
-        // TODO we can not progress until the last buffer written in TXD.PTR
-        // has started the transmission.
-        // We can use some sync primitive from `embassy-sync`.
+        let drop = OnDrop::new(move || {
+            trace!("write drop: stopping");
+
+            r.intenclr.write(|w| w.txptrupd().clear());
+            r.events_txptrupd.reset();
+            r.config.txen.write(|w| w.txen().disabled());
+
+            // TX is stopped almost instantly, spinning is fine.
+            while r.events_txptrupd.read().bits() == 0 {}
+            trace!("write drop: stopped");
+        });
 
         r.txd.ptr.write(|w| unsafe { w.ptr().bits(ptr as u32) });
         r.rxtxd.maxcnt.write(|w| unsafe { w.bits(maxcnt) });
 
+        r.intenset.write(|w| w.txptrupd().set());
+
+        compiler_fence(Ordering::SeqCst);
+
+        poll_fn(|cx| {
+            s.tx_waker.register(cx.waker());
+            if r.events_txptrupd.read().bits() != 0 {
+                Poll::Ready(())
+            } else {
+                Poll::Pending
+            }
+        })
+        .await;
+
+        compiler_fence(Ordering::SeqCst);
+        drop.defuse();
+
         Ok(())
     }
 }
@@ -451,23 +593,19 @@ impl Buffer for &[i32] {
 }
 
 pub(crate) mod sealed {
-    use core::sync::atomic::AtomicU8;
-
     use embassy_sync::waitqueue::AtomicWaker;
 
     //use super::*;
 
     pub struct State {
-        pub input_waker: AtomicWaker,
-        pub output_waker: AtomicWaker,
-        pub buffers_refcount: AtomicU8,
+        pub rx_waker: AtomicWaker,
+        pub tx_waker: AtomicWaker,
     }
     impl State {
         pub const fn new() -> Self {
             Self {
-                input_waker: AtomicWaker::new(),
-                output_waker: AtomicWaker::new(),
-                buffers_refcount: AtomicU8::new(0),
+                rx_waker: AtomicWaker::new(),
+                tx_waker: AtomicWaker::new(),
             }
         }
     }
diff --git a/embassy-nrf/src/lib.rs b/embassy-nrf/src/lib.rs
index bdb911ecd..dc018e08e 100644
--- a/embassy-nrf/src/lib.rs
+++ b/embassy-nrf/src/lib.rs
@@ -74,7 +74,7 @@ pub mod buffered_uarte;
 pub mod gpio;
 #[cfg(feature = "gpiote")]
 pub mod gpiote;
-#[cfg(any(feature = "nrf52832", feature = "nrf52833", feature = "nrf52840",))]
+#[cfg(any(feature = "nrf52832", feature = "nrf52833", feature = "nrf52840"))]
 pub mod i2s;
 #[cfg(not(any(feature = "_nrf5340", feature = "_nrf9160")))]
 pub mod nvmc;
diff --git a/examples/nrf/src/bin/i2s.rs b/examples/nrf/src/bin/i2s.rs
index e8ddb4a40..53ccb3b85 100644
--- a/examples/nrf/src/bin/i2s.rs
+++ b/examples/nrf/src/bin/i2s.rs
@@ -4,43 +4,133 @@
 #![no_main]
 #![feature(type_alias_impl_trait)]
 
-//use defmt::*;
+use core::f32::consts::PI;
+
+use defmt::{error, info};
 use embassy_executor::Spawner;
-use embassy_nrf::i2s;
+use embassy_nrf::i2s::{MckFreq, Mode, Ratio, MODE_MASTER_16000, MODE_MASTER_8000};
+use embassy_nrf::{i2s, interrupt};
 use {defmt_rtt as _, panic_probe as _};
 
 #[repr(align(4))]
-pub struct Aligned<T: ?Sized>(T);
+pub struct AlignedBuffer<T: ?Sized>(T);
+
+impl<T> AsRef<T> for AlignedBuffer<T> {
+    fn as_ref(&self) -> &T {
+        &self.0
+    }
+}
+
+impl<T> AsMut<T> for AlignedBuffer<T> {
+    fn as_mut(&mut self) -> &mut T {
+        &mut self.0
+    }
+}
 
 #[embassy_executor::main]
 async fn main(_spawner: Spawner) {
     let p = embassy_nrf::init(Default::default());
-    let config = i2s::Config::default();
+    let mut config = i2s::Config::default();
+    // config.mode = MODE_MASTER_16000;
+    config.mode = Mode::Master {
+        freq: MckFreq::_32MDiv10,
+        ratio: Ratio::_256x,
+    }; // 12500 Hz
+    let sample_rate = config.mode.sample_rate().expect("I2S Master");
+    let inv_sample_rate = 1.0 / sample_rate as f32;
 
-    let mut i2s = i2s::I2S::new(p.I2S, p.P0_28, p.P0_29, p.P0_31, p.P0_11, p.P0_30, config);
+    info!("Sample rate: {}", sample_rate);
 
-    let mut signal_buf: Aligned<[i16; 32]> = Aligned([0i16; 32]);
-    let len = signal_buf.0.len() / 2;
-    for x in 0..len {
-        signal_buf.0[2 * x] = triangle_wave(x as i32, len, 2048, 0, 1) as i16;
-        signal_buf.0[2 * x + 1] = triangle_wave(x as i32, len, 2048, 0, 1) as i16;
-    }
+    let irq = interrupt::take!(I2S);
+    let mut i2s = i2s::I2S::new(p.I2S, irq, p.P0_28, p.P0_29, p.P0_31, p.P0_11, p.P0_30, config);
+
+    const BUF_SAMPLES: usize = 250;
+    const BUF_SIZE: usize = BUF_SAMPLES * 2;
+    let mut buf = AlignedBuffer([0i16; BUF_SIZE]);
+
+    let mut carrier = SineOsc::new();
+    carrier.set_frequency(300.0, inv_sample_rate);
+
+    let mut modulator = SineOsc::new();
+    modulator.set_frequency(0.01, inv_sample_rate);
+    modulator.set_amplitude(0.2);
 
     i2s.set_tx_enabled(true);
     i2s.start();
 
     loop {
-        match i2s.tx(signal_buf.0.as_slice()).await {
-            Ok(_) => todo!(),
-            Err(_) => todo!(),
-        };
+        for sample in buf.as_mut().chunks_mut(2) {
+            let signal = carrier.generate();
+            // let modulation = bipolar_to_unipolar(modulator.generate());
+            // carrier.set_frequency(200.0 + 100.0 * modulation, inv_sample_rate);
+            // carrier.set_amplitude((modulation);
+            let value = (i16::MAX as f32 * signal) as i16;
+            sample[0] = value;
+            sample[1] = value;
+            // info!("{}", signal);
+        }
+
+        if let Err(err) = i2s.tx(buf.as_ref().as_slice()).await {
+            error!("{}", err);
+        }
+    }
+}
+
+struct SineOsc {
+    amplitude: f32,
+    modulo: f32,
+    phase_inc: f32,
+}
+
+impl SineOsc {
+    const B: f32 = 4.0 / PI;
+    const C: f32 = -4.0 / (PI * PI);
+    const P: f32 = 0.225;
+
+    pub fn new() -> Self {
+        Self {
+            amplitude: 1.0,
+            modulo: 0.0,
+            phase_inc: 0.0,
+        }
+    }
+
+    pub fn set_frequency(&mut self, freq: f32, inv_sample_rate: f32) {
+        self.phase_inc = freq * inv_sample_rate;
+    }
+
+    pub fn set_amplitude(&mut self, amplitude: f32) {
+        self.amplitude = amplitude;
+    }
+
+    pub fn generate(&mut self) -> f32 {
+        let signal = self.parabolic_sin(self.modulo);
+        self.modulo += self.phase_inc;
+        if self.modulo < 0.0 {
+            self.modulo += 1.0;
+        } else if self.modulo > 1.0 {
+            self.modulo -= 1.0;
+        }
+        signal * self.amplitude
+    }
+
+    fn parabolic_sin(&mut self, modulo: f32) -> f32 {
+        let angle = PI - modulo * 2.0 * PI;
+        let y = Self::B * angle + Self::C * angle * abs(angle);
+        Self::P * (y * abs(y) - y) + y
+    }
+}
+
+#[inline]
+fn abs(value: f32) -> f32 {
+    if value < 0.0 {
+        -value
+    } else {
+        value
     }
 }
 
-fn triangle_wave(x: i32, length: usize, amplitude: i32, phase: i32, periods: i32) -> i32 {
-    let length = length as i32;
-    amplitude
-        - ((2 * periods * (x + phase + length / (4 * periods)) * amplitude / length) % (2 * amplitude) - amplitude)
-            .abs()
-        - amplitude / 2
+#[inline]
+fn bipolar_to_unipolar(value: f32) -> f32 {
+    (value + 1.0) / 2.0
 }