diff --git a/embassy-nrf/src/i2s.rs b/embassy-nrf/src/i2s.rs index 08d4093f..7e950775 100644 --- a/embassy-nrf/src/i2s.rs +++ b/embassy-nrf/src/i2s.rs @@ -19,6 +19,8 @@ use crate::pac::i2s::RegisterBlock; use crate::util::{slice_in_ram_or, slice_ptr_parts}; use crate::{Peripheral, EASY_DMA_SIZE}; +pub type DoubleBuffering = MultiBuffering; + #[derive(Debug, Clone, Copy, PartialEq, Eq)] #[cfg_attr(feature = "defmt", derive(defmt::Format))] #[non_exhaustive] @@ -379,27 +381,47 @@ impl<'d, T: Instance> I2S<'d, T> { } /// I2S output only - pub fn output(mut self, sdout: impl Peripheral

+ 'd) -> OutputStream<'d, T> { + pub fn output( + mut self, + sdout: impl Peripheral

+ 'd, + buffers: MultiBuffering, + ) -> OutputStream<'d, T, S, NB, NS> { self.sdout = Some(sdout.into_ref().map_into()); - OutputStream { _p: self.build() } + OutputStream { + _p: self.build(), + buffers, + } } /// I2S input only - pub fn input(mut self, sdin: impl Peripheral

+ 'd) -> InputStream<'d, T> { + pub fn input( + mut self, + sdin: impl Peripheral

+ 'd, + buffers: MultiBuffering, + ) -> InputStream<'d, T, S, NB, NS> { self.sdin = Some(sdin.into_ref().map_into()); - InputStream { _p: self.build() } + InputStream { + _p: self.build(), + buffers, + } } /// I2S full duplex (input and output) - pub fn full_duplex( + pub fn full_duplex( mut self, sdin: impl Peripheral

+ 'd, sdout: impl Peripheral

+ 'd, - ) -> FullDuplexStream<'d, T> { + buffers_out: MultiBuffering, + buffers_in: MultiBuffering, + ) -> FullDuplexStream<'d, T, S, NB, NS> { self.sdout = Some(sdout.into_ref().map_into()); self.sdin = Some(sdin.into_ref().map_into()); - FullDuplexStream { _p: self.build() } + FullDuplexStream { + _p: self.build(), + buffers_out, + buffers_in, + } } fn build(self) -> PeripheralRef<'d, T> { @@ -651,14 +673,19 @@ impl<'d, T: Instance> I2S<'d, T> { } /// I2S output -pub struct OutputStream<'d, T: Instance> { +pub struct OutputStream<'d, T: Instance, S: Sample, const NB: usize, const NS: usize> { _p: PeripheralRef<'d, T>, + buffers: MultiBuffering, } -impl<'d, T: Instance> OutputStream<'d, T> { +impl<'d, T: Instance, S: Sample, const NB: usize, const NS: usize> OutputStream<'d, T, S, NB, NS> { + /// Get a mutable reference to the current buffer. + pub fn buffer(&mut self) -> &mut [S] { + self.buffers.get_mut() + } + /// Prepare the initial buffer and start the I2S transfer. - #[allow(unused_mut)] - pub async fn start(&mut self, buffer: &[S]) -> Result<(), Error> + pub async fn start(&mut self) -> Result<(), Error> where S: Sample, { @@ -672,7 +699,7 @@ impl<'d, T: Instance> OutputStream<'d, T> { device.enable(); device.enable_tx(); - device.update_tx(buffer as *const [S])?; + device.update_tx(self.buffers.switch())?; s.started.store(true, Ordering::Relaxed); @@ -689,28 +716,30 @@ impl<'d, T: Instance> OutputStream<'d, T> { I2S::::stop().await } - /// Sets the given `buffer` for transmission in the DMA. - /// Buffer address must be 4 byte aligned and located in RAM. - /// The buffer must not be written while being used by the DMA, - /// which takes two other `send`s being awaited. - #[allow(unused_mut)] - pub async fn send_from_ram(&mut self, buffer: &[S]) -> Result<(), Error> + /// Sends the current buffer for transmission in the DMA. + /// Switches to use the next available buffer. + pub async fn send(&mut self) -> Result<(), Error> where S: Sample, { - I2S::::send_from_ram(buffer as *const [S]).await + I2S::::send_from_ram(self.buffers.switch()).await } } /// I2S input -pub struct InputStream<'d, T: Instance> { +pub struct InputStream<'d, T: Instance, S: Sample, const NB: usize, const NS: usize> { _p: PeripheralRef<'d, T>, + buffers: MultiBuffering, } -impl<'d, T: Instance> InputStream<'d, T> { +impl<'d, T: Instance, S: Sample, const NB: usize, const NS: usize> InputStream<'d, T, S, NB, NS> { + /// Get a mutable reference to the current buffer. + pub fn buffer(&mut self) -> &mut [S] { + self.buffers.get_mut() + } + /// Prepare the initial buffer and start the I2S transfer. - #[allow(unused_mut)] - pub async fn start(&mut self, buffer: &mut [S]) -> Result<(), Error> + pub async fn start(&mut self) -> Result<(), Error> where S: Sample, { @@ -724,7 +753,7 @@ impl<'d, T: Instance> InputStream<'d, T> { device.enable(); device.enable_rx(); - device.update_rx(buffer as *mut [S])?; + device.update_rx(self.buffers.switch())?; s.started.store(true, Ordering::Relaxed); @@ -741,28 +770,32 @@ impl<'d, T: Instance> InputStream<'d, T> { I2S::::stop().await } - /// Sets the given `buffer` for reception from the DMA. - /// Buffer address must be 4 byte aligned and located in RAM. - /// The buffer must not be read while being used by the DMA, - /// which takes two other `receive`s being awaited. + /// Sets the current buffer for reception from the DMA. + /// Switches to use the next available buffer. #[allow(unused_mut)] - pub async fn receive_from_ram(&mut self, buffer: &mut [S]) -> Result<(), Error> + pub async fn receive(&mut self) -> Result<(), Error> where S: Sample, { - I2S::::receive_from_ram(buffer as *mut [S]).await + I2S::::receive_from_ram(self.buffers.switch_mut()).await } } /// I2S full duplex stream (input & output) -pub struct FullDuplexStream<'d, T: Instance> { +pub struct FullDuplexStream<'d, T: Instance, S: Sample, const NB: usize, const NS: usize> { _p: PeripheralRef<'d, T>, + buffers_out: MultiBuffering, + buffers_in: MultiBuffering, } -impl<'d, T: Instance> FullDuplexStream<'d, T> { +impl<'d, T: Instance, S: Sample, const NB: usize, const NS: usize> FullDuplexStream<'d, T, S, NB, NS> { + /// Get the current output and input buffers. + pub fn buffers(&mut self) -> (&mut [S], &[S]) { + (self.buffers_out.get_mut(), self.buffers_in.get()) + } + /// Prepare the initial buffers and start the I2S transfer. - #[allow(unused_mut)] - pub async fn start(&mut self, buffer_in: &mut [S], buffer_out: &[S]) -> Result<(), Error> + pub async fn start(&mut self) -> Result<(), Error> where S: Sample, { @@ -777,8 +810,8 @@ impl<'d, T: Instance> FullDuplexStream<'d, T> { device.enable_tx(); device.enable_rx(); - device.update_tx(buffer_out as *const [S])?; - device.update_rx(buffer_in as *mut [S])?; + device.update_tx(self.buffers_out.switch())?; + device.update_rx(self.buffers_in.switch_mut())?; s.started.store(true, Ordering::Relaxed); @@ -796,17 +829,14 @@ impl<'d, T: Instance> FullDuplexStream<'d, T> { I2S::::stop().await } - /// Sets the given `buffer_out` and `buffer_in` for transmission/reception from the DMA. - /// Buffer address must be 4 byte aligned and located in RAM. - /// The buffers must not be written/read while being used by the DMA, - /// which takes two other `send_and_receive` operations being awaited. - #[allow(unused_mut)] - pub async fn send_and_receive_from_ram(&mut self, buffer_in: &mut [S], buffer_out: &[S]) -> Result<(), Error> + /// Sets the current buffers for output and input for transmission/reception from the DMA. + /// Switch to use the next available buffers for output/input. + pub async fn send_and_receive(&mut self) -> Result<(), Error> where S: Sample, { - I2S::::send_from_ram(buffer_out as *const [S]).await?; - I2S::::receive_from_ram(buffer_in as *mut [S]).await?; + I2S::::send_from_ram(self.buffers_out.switch()).await?; + I2S::::receive_from_ram(self.buffers_in.switch_mut()).await?; Ok(()) } } @@ -992,7 +1022,7 @@ impl Sample for i32 { const SCALE: Self = 1 << (Self::WIDTH - 1); } -/// A 4-bytes aligned [Buffer]. +/// A 4-bytes aligned buffer. #[derive(Clone, Copy)] #[repr(align(4))] pub struct AlignedBuffer([T; N]); @@ -1022,6 +1052,43 @@ impl DerefMut for AlignedBuffer { } } +pub struct MultiBuffering { + buffers: [AlignedBuffer; NB], + index: usize, +} + +impl MultiBuffering { + pub fn new() -> Self { + assert!(NB > 1); + Self { + buffers: [AlignedBuffer::::default(); NB], + index: 0, + } + } + + fn get(&self) -> &[S] { + &self.buffers[self.index] + } + + fn get_mut(&mut self) -> &mut [S] { + &mut self.buffers[self.index] + } + + /// Advance to use the next buffer and return a non mutable pointer to the previous one. + fn switch(&mut self) -> *const [S] { + let prev_index = self.index; + self.index = (self.index + 1) % NB; + self.buffers[prev_index].deref() as *const [S] + } + + /// Advance to use the next buffer and return a mutable pointer to the previous one. + fn switch_mut(&mut self) -> *mut [S] { + let prev_index = self.index; + self.index = (self.index + 1) % NB; + self.buffers[prev_index].deref_mut() as *mut [S] + } +} + pub(crate) mod sealed { use core::sync::atomic::AtomicBool; diff --git a/examples/nrf/src/bin/i2s_effect.rs b/examples/nrf/src/bin/i2s_effect.rs new file mode 100644 index 00000000..3cca005b --- /dev/null +++ b/examples/nrf/src/bin/i2s_effect.rs @@ -0,0 +1,117 @@ +#![no_std] +#![no_main] +#![feature(type_alias_impl_trait)] + +use core::f32::consts::PI; + +use defmt::{error, info}; +use embassy_executor::Spawner; +use embassy_nrf::i2s::{self, Channels, Config, MasterClock, MultiBuffering, Sample as _, SampleWidth, I2S}; +use embassy_nrf::interrupt; +use {defmt_rtt as _, panic_probe as _}; + +type Sample = i16; + +const NUM_BUFFERS: usize = 2; +const NUM_SAMPLES: usize = 4; + +#[embassy_executor::main] +async fn main(_spawner: Spawner) { + let p = embassy_nrf::init(Default::default()); + + let master_clock: MasterClock = i2s::ExactSampleRate::_50000.into(); + + let sample_rate = master_clock.sample_rate(); + info!("Sample rate: {}", sample_rate); + + let config = Config::default() + .sample_width(SampleWidth::_16bit) + .channels(Channels::MonoLeft); + + let irq = interrupt::take!(I2S); + let buffers_out = MultiBuffering::::new(); + let buffers_in = MultiBuffering::::new(); + let mut full_duplex_stream = I2S::master(p.I2S, irq, p.P0_25, p.P0_26, p.P0_27, master_clock, config).full_duplex( + p.P0_29, + p.P0_28, + buffers_out, + buffers_in, + ); + + let mut modulator = SineOsc::new(); + modulator.set_frequency(8.0, 1.0 / sample_rate as f32); + modulator.set_amplitude(1.0); + + full_duplex_stream.start().await.expect("I2S Start"); + + loop { + let (buff_out, buff_in) = full_duplex_stream.buffers(); + for i in 0..NUM_SAMPLES { + let modulation = (Sample::SCALE as f32 * bipolar_to_unipolar(modulator.generate())) as Sample; + buff_out[i] = buff_in[i] * modulation; + } + + if let Err(err) = full_duplex_stream.send_and_receive().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 + } +} + +#[inline] +fn bipolar_to_unipolar(value: f32) -> f32 { + (value + 1.0) / 2.0 +} diff --git a/examples/nrf/src/bin/i2s_monitor.rs b/examples/nrf/src/bin/i2s_monitor.rs new file mode 100644 index 00000000..48eb7d58 --- /dev/null +++ b/examples/nrf/src/bin/i2s_monitor.rs @@ -0,0 +1,115 @@ +#![no_std] +#![no_main] +#![feature(type_alias_impl_trait)] + +use defmt::{debug, error, info}; +use embassy_executor::Spawner; +use embassy_nrf::i2s::{self, Channels, Config, DoubleBuffering, MasterClock, Sample as _, SampleWidth, I2S}; +use embassy_nrf::interrupt; +use embassy_nrf::pwm::{Prescaler, SimplePwm}; +use {defmt_rtt as _, panic_probe as _}; + +type Sample = i16; + +const NUM_SAMPLES: usize = 500; + +#[embassy_executor::main] +async fn main(_spawner: Spawner) { + let p = embassy_nrf::init(Default::default()); + + let master_clock: MasterClock = i2s::ExactSampleRate::_50000.into(); + + let sample_rate = master_clock.sample_rate(); + info!("Sample rate: {}", sample_rate); + + let config = Config::default() + .sample_width(SampleWidth::_16bit) + .channels(Channels::MonoLeft); + + let irq = interrupt::take!(I2S); + let buffers = DoubleBuffering::::new(); + let mut input_stream = + I2S::master(p.I2S, irq, p.P0_25, p.P0_26, p.P0_27, master_clock, config).input(p.P0_29, buffers); + + // Configure the PWM to use the pins corresponding to the RGB leds + let mut pwm = SimplePwm::new_3ch(p.PWM0, p.P0_23, p.P0_22, p.P0_24); + pwm.set_prescaler(Prescaler::Div1); + pwm.set_max_duty(255); + + let mut rms_online = RmsOnline::::default(); + + input_stream.start().await.expect("I2S Start"); + + loop { + let rms = rms_online.process(input_stream.buffer()); + let rgb = rgb_from_rms(rms); + + debug!("RMS: {}, RGB: {:?}", rms, rgb); + for i in 0..3 { + pwm.set_duty(i, rgb[i].into()); + } + + if let Err(err) = input_stream.receive().await { + error!("{}", err); + } + } +} + +/// RMS from 0.0 until 0.75 will give green with a proportional intensity +/// RMS from 0.75 until 0.9 will give a blend between orange and red proportionally to the intensity +/// RMS above 0.9 will give a red with a proportional intensity +fn rgb_from_rms(rms: f32) -> [u8; 3] { + if rms < 0.75 { + let intensity = rms / 0.75; + [0, (intensity * 165.0) as u8, 0] + } else if rms < 0.9 { + let intensity = (rms - 0.75) / 0.15; + [200, 165 - (165.0 * intensity) as u8, 0] + } else { + let intensity = (rms - 0.9) / 0.1; + [200 + (55.0 * intensity) as u8, 0, 0] + } +} + +pub struct RmsOnline { + pub squares: [f32; N], + pub head: usize, +} + +impl Default for RmsOnline { + fn default() -> Self { + RmsOnline { + squares: [0.0; N], + head: 0, + } + } +} + +impl RmsOnline { + pub fn reset(&mut self) { + self.squares = [0.0; N]; + self.head = 0; + } + + pub fn process(&mut self, buf: &[Sample]) -> f32 { + buf.iter() + .for_each(|sample| self.push(*sample as f32 / Sample::SCALE as f32)); + + let sum_of_squares = self.squares.iter().fold(0.0, |acc, v| acc + *v); + Self::approx_sqrt(sum_of_squares / N as f32) + } + + pub fn push(&mut self, signal: f32) { + let square = signal * signal; + self.squares[self.head] = square; + self.head = (self.head + 1) % N; + } + + /// Approximated sqrt taken from [micromath] + /// + /// [micromath]: https://docs.rs/micromath/latest/src/micromath/float/sqrt.rs.html#11-17 + /// + fn approx_sqrt(value: f32) -> f32 { + f32::from_bits((value.to_bits() + 0x3f80_0000) >> 1) + } +} diff --git a/examples/nrf/src/bin/i2s_waveform.rs b/examples/nrf/src/bin/i2s_waveform.rs index 13b1300e..1b0e8ebc 100644 --- a/examples/nrf/src/bin/i2s_waveform.rs +++ b/examples/nrf/src/bin/i2s_waveform.rs @@ -6,13 +6,12 @@ use core::f32::consts::PI; use defmt::{error, info}; use embassy_executor::Spawner; -use embassy_nrf::i2s::{self, Channels, Config, MasterClock, Sample as _, SampleWidth, I2S}; +use embassy_nrf::i2s::{self, Channels, Config, DoubleBuffering, MasterClock, Sample as _, SampleWidth, I2S}; use embassy_nrf::interrupt; use {defmt_rtt as _, panic_probe as _}; type Sample = i16; -const NUM_BUFFERS: usize = 2; const NUM_SAMPLES: usize = 50; #[embassy_executor::main] @@ -29,29 +28,22 @@ async fn main(_spawner: Spawner) { .channels(Channels::MonoLeft); let irq = interrupt::take!(I2S); - let mut output_stream = I2S::master(p.I2S, irq, p.P0_25, p.P0_26, p.P0_27, master_clock, config).output(p.P0_28); - - let mut buffers: [i2s::AlignedBuffer; NUM_BUFFERS] = - [i2s::AlignedBuffer::default(); NUM_BUFFERS]; + let buffers = DoubleBuffering::::new(); + let mut output_stream = + I2S::master(p.I2S, irq, p.P0_25, p.P0_26, p.P0_27, master_clock, config).output(p.P0_28, buffers); let mut waveform = Waveform::new(1.0 / sample_rate as f32); - waveform.process(&mut buffers[0]); + waveform.process(output_stream.buffer()); - output_stream.start(&buffers[0]).await.expect("I2S Start"); + output_stream.start().await.expect("I2S Start"); - let mut index = 1; loop { - waveform.process(&mut buffers[index]); + waveform.process(output_stream.buffer()); - if let Err(err) = output_stream.send_from_ram(&buffers[index]).await { + if let Err(err) = output_stream.send().await { error!("{}", err); } - - index += 1; - if index >= NUM_BUFFERS { - index = 0; - } } } @@ -68,7 +60,7 @@ impl Waveform { carrier.set_frequency(110.0, inv_sample_rate); let mut freq_mod = SineOsc::new(); - freq_mod.set_frequency(8.0, inv_sample_rate); + freq_mod.set_frequency(1.0, inv_sample_rate); freq_mod.set_amplitude(1.0); let mut amp_mod = SineOsc::new();