From 122a31d20877005c7201d4e7c98da5544666dd1d Mon Sep 17 00:00:00 2001 From: Christian Perez Llamas <932644+chris-zen@users.noreply.github.com> Date: Sat, 12 Nov 2022 18:48:57 +0100 Subject: [PATCH] Interrupts, async, sine oscillator --- embassy-nrf/src/i2s.rs | 298 ++++++++++++++++++++++++++---------- embassy-nrf/src/lib.rs | 2 +- examples/nrf/src/bin/i2s.rs | 132 +++++++++++++--- 3 files changed, 330 insertions(+), 102 deletions(-) diff --git a/embassy-nrf/src/i2s.rs b/embassy-nrf/src/i2s.rs index fb6fa4bd..f5e36f0d 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 { + 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 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 for u8 { fn from(variant: Ratio) -> Self { variant as _ @@ -136,31 +234,6 @@ impl From 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

+ 'd, - // irq: impl Peripheral

+ 'd, + irq: impl Peripheral

+ 'd, mck: impl Peripheral

+ 'd, sck: impl Peripheral

+ 'd, lrck: impl Peripheral

+ '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

+ 'd, - // irq: impl Peripheral

+ 'd, + irq: impl Peripheral

+ '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 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()); + 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) { + 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 bdb911ec..dc018e08 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 e8ddb4a4..53ccb3b8 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); +pub struct AlignedBuffer(T); + +impl AsRef for AlignedBuffer { + fn as_ref(&self) -> &T { + &self.0 + } +} + +impl AsMut for AlignedBuffer { + 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); + } } } -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 +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 + } +} + +#[inline] +fn bipolar_to_unipolar(value: f32) -> f32 { + (value + 1.0) / 2.0 }