From c15509282eeb620b6ce70059ea10c4657cff7474 Mon Sep 17 00:00:00 2001 From: Nathan Perry Date: Tue, 24 Sep 2024 00:13:11 -0400 Subject: pio i2s --- pio_i2s/src/lib.rs | 278 +++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 278 insertions(+) create mode 100644 pio_i2s/src/lib.rs (limited to 'pio_i2s/src') diff --git a/pio_i2s/src/lib.rs b/pio_i2s/src/lib.rs new file mode 100644 index 0000000..62ef3ac --- /dev/null +++ b/pio_i2s/src/lib.rs @@ -0,0 +1,278 @@ +#![no_std] + +use core::{ + arch::asm, + mem, + time::Duration, +}; +use ::{ + embassy_rp, + embassy_rp::{ + clocks::clk_sys_freq, + pio, + pio::{ + ShiftDirection, + StateMachine, + }, + Peripheral, + }, + embassy_time, + embassy_time::{ + Duration, + Instant, + }, + embedded_io_async, + fixed::prelude::ToFixed, + genlog::defmt, + pio::{ + InSource, + JmpCondition, + MovDestination, + MovOperation, + MovSource, + OutDestination, + SetDestination, + SideSet, + }, + pio_proc, +}; + +pub trait GetSm<'inner, PIO, const IDX: usize> +where + PIO: embassy_rp::pio::Instance, +{ + fn sm<'p>(pio: &'p mut pio::Pio<'inner, PIO>) -> &'p mut StateMachine<'inner, PIO, IDX>; +} + +pub struct SmLookup; + +macro_rules! impl_getsm { + ($idx:literal, $name:ident) => { + impl<'inner, PIO> GetSm<'inner, PIO, $idx> for SmLookup<$idx> + where + PIO: embassy_rp::pio::Instance, + { + fn sm<'p>( + pio: &'p mut pio::Pio<'inner, PIO>, + ) -> &'p mut StateMachine<'inner, PIO, $idx> { + &mut pio.$name + } + } + }; +} + +impl_getsm!(0, sm0); +impl_getsm!(1, sm1); +impl_getsm!(2, sm2); +impl_getsm!(3, sm3); + +pub type StaticI2S = I2S<'static, 'static, PIO, IDX>; + +pub struct I2S<'b, 'p, PIO, const IDX: usize> +where + PIO: pio::Instance, +{ + sm: &'b mut StateMachine<'p, PIO, IDX>, +} + +impl<'b, 'p, PIO, const IDX: usize> I2S<'b, 'p, PIO, IDX> +where + PIO: pio::Instance, +{ + pub const BIT_DEPTH: usize = 16; + pub const CHANNELS: usize = 1; + pub const DEFAULT_BUFFER_SIZE: usize = 960; + pub const SAMPLE_RATE_HZ: usize = 48_000; + + pub fn new( + pio: &'b mut pio::Pio<'p, PIO>, + bclk: impl Peripheral

+ 'static, + data: impl Peripheral

+ 'static, + ws: impl Peripheral

+ 'static, + ) -> Self + where + SmLookup: GetSm<'p, PIO, IDX>, + { + defmt::info!("start"); + + let bclk = pio.common.make_pio_pin(bclk); + let data = pio.common.make_pio_pin(data); + let ws = pio.common.make_pio_pin(ws); + + defmt::info!("pins done"); + + let program = program::<1024>(None); + defmt::info!("made program: {}", defmt::Debug2Format(&program)); + + let program = pio.common.try_load_program(&program); + + let program = match program { + Err(e) => { + defmt::panic!("loading pio: {}", e); + }, + Ok(program) => program, + }; + + defmt::info!("program loaded"); + + let config = Self::config(&program, bclk, data, ws); + + defmt::info!("config run"); + + let sm = as GetSm>::sm(pio); + sm.set_enable(false); + sm.set_config(&config); + sm.clear_fifos(); + + Self { + sm, + } + } + + pub async fn run( + &mut self, + dma: impl Peripheral

, + mut pipe: impl embedded_io_async::Write, + ) { + self.sm.set_enable(true); + + let tx = self.sm.tx(); + let mut dma = dma.into_ref(); + + let mut front_buffer = [0u8; BUFFER_SIZE]; + let mut back_buffer = [0u8; BUFFER_SIZE]; + + loop { + let transfer = tx.dma_push(dma.reborrow(), &mut front_buffer); + let now = Instant::now(); + let deadline = now + Duration::from_hz(Self::SAMPLE_RATE_HZ as _) * BUFFER_SIZE as _; + + if let Err(e) = + molybdos::embassy_time::with_deadline(deadline, pipe.write_all(&back_buffer)).await + { + defmt::error!("audio timeout"); + } + + transfer.await; + mem::swap(&mut back_buffer, &mut front_buffer); + } + } + + fn config<'pin>( + prog: &pio::LoadedProgram<'p, PIO>, + bclk: pio::Pin<'pin, PIO>, + data: pio::Pin<'pin, PIO>, + ws: pio::Pin<'pin, PIO>, + ) -> pio::Config<'p, PIO> + where + 'pin: 'p, + { + let clock_frequency: usize = Self::SAMPLE_RATE_HZ * Self::CHANNELS * Self::BIT_DEPTH; + + let mut cfg = pio::Config::default(); + + cfg.use_program(&prog, &[&bclk]); + cfg.set_in_pins(&[&data]); + cfg.set_set_pins(&[&ws]); + + cfg.fifo_join = pio::FifoJoin::RxOnly; + + cfg.shift_in = pio::ShiftConfig { + threshold: 32, + direction: ShiftDirection::Left, + auto_fill: true, + }; + + cfg.clock_divider = (clk_sys_freq() as f64 / clock_frequency as f64 / 2.).to_fixed(); + + cfg + } +} + +#[derive(Debug, Copy, Clone, PartialEq, Eq, PartialOrd, Ord)] +pub struct WarmupConfig { + clock_freq: usize, + warmup_period: embassy_time::Duration, +} + +/// I2S pio driver program. +/// +/// Pass `warmup` to have the PIO idle for a period before starting the clock. +pub fn program( + warmup: Option, +) -> molybdos::pio::Program { + let mut asm = + molybdos::pio::Assembler::::new_with_side_set(SideSet::new(true, 1, false)); + + asm.irq(true, false, 0, true); + asm.set(SetDestination::PINS, 1); + + // on boot, SPH0644LM4H wants 30ms warmup time with the clock running at full frequency. it's + // nice to do this in the PIO itself because it makes the host agnostic -- it can just wait + // until there is data available in the fifo. + if let Some(WarmupConfig { + clock_freq, + warmup_period, + }) = warmup + { + const SET_MAX_BITS: usize = 5; + + let warmup_period_clock_cycles = clock_freq / 1000 * warmup_period.as_millis() as usize; + debug_assert!(warmup_period_clock_cycles <= 1 << 31); + + asm.r#in(InSource::NULL, 32); + + let sig_bits = 32 - warmup_period_clock_cycles.leading_zeros(); + + for i in 0..(sig_bits / 5) { + const MASK: usize = 0b11111; + let shift = i * SET_MAX_BITS as u32; + + asm.set(SetDestination::X, ((warmup_period_clock_cycles >> shift) & MASK) as u8); + asm.r#in(InSource::X, SET_MAX_BITS as _); + } + + let rem_bits = sig_bits % SET_MAX_BITS as u32; + if rem_bits != 0 { + let mask = (1 << (rem_bits + 1)) - 1; + + asm.set( + SetDestination::X, + ((warmup_period_clock_cycles >> (sig_bits - rem_bits)) & mask) as u8, + ); + asm.r#in(InSource::X, SET_MAX_BITS as _); + } + + asm.mov(MovDestination::X, MovOperation::None, MovSource::ISR); + + let mut bootloop = asm.label(); + { + asm.nop_with_side_set(1); + asm.jmp_with_side_set(JmpCondition::XDecNonZero, &mut bootloop, 0); + } + } + + asm.irq_with_side_set(false, false, 0, true, 1); + + let mut wrap_target = asm.label(); + + asm.in_with_side_set(InSource::PINS, 1, 0); + asm.nop_with_side_set(1); + let wrap = asm.label(); + + defmt::info!("trying assemble..."); + asm.assemble_with_wrap(wrap_target, wrap) +} + +#[cfg(test)] +mod test { + use crate::{ + bringup::i2s::program, + program, + }; + + #[test] + fn test_program() { + program(None); + } +} -- cgit v1.3.1