From c15509282eeb620b6ce70059ea10c4657cff7474 Mon Sep 17 00:00:00 2001 From: Nathan Perry Date: Tue, 24 Sep 2024 00:13:11 -0400 Subject: pio i2s --- src/bringup/i2s.rs | 281 +++++++++++++++++++++++++++++++++++++++++++++++++++++ src/bringup/mod.rs | 87 ++++++++++++++--- src/lib.rs | 1 + src/main.rs | 81 +++++++++++++-- 4 files changed, 431 insertions(+), 19 deletions(-) create mode 100644 src/bringup/i2s.rs (limited to 'src') diff --git a/src/bringup/i2s.rs b/src/bringup/i2s.rs new file mode 100644 index 0000000..37d8921 --- /dev/null +++ b/src/bringup/i2s.rs @@ -0,0 +1,281 @@ +use core::{ + arch::asm, + mem, +}; + +use molybdos::{ + embassy_rp, + embassy_rp::{ + clocks::clk_sys_freq, + pio, + pio::{ + Direction, + 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: 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 DEFAULT_BUFFER_SIZE: usize = 960; + pub const SAMPLE_RATE_HZ: usize = 48_000 * 32; + + 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(); + + sm.set_pin_dirs(Direction::Out, &[&bclk, &ws]); + sm.set_pin_dirs(Direction::In, &[&data]); + + Self { + sm, + } + } + + pub async fn run( + &mut self, + dma: impl Peripheral

, + mut pipe: impl embedded_io_async::Write, + ) { + self.sm.set_enable(true); + defmt::info!("i2s state machine started"); + + let rx = self.sm.rx(); + let mut dma = dma.into_ref(); + + let mut front_buffer = [0u8; BUFFER_SIZE]; + let mut back_buffer = [0u8; BUFFER_SIZE]; + + let per_sample_dur = + core::time::Duration::from_secs(1) * BUFFER_SIZE as _ / (Self::SAMPLE_RATE_HZ as _); + + defmt::info!("ready to loop, sample_dur = {}us", per_sample_dur.as_micros()); + + let per_sample_dur = embassy_time::Duration::from_nanos(per_sample_dur.as_nanos() as _); + + loop { + defmt::info!("loop"); + + let transfer = rx.dma_pull(dma.reborrow(), &mut front_buffer); + let now = Instant::now(); + let deadline = now + per_sample_dur; + + 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; + defmt::info!("using clock freq: {}", clock_frequency); + + 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(); + defmt::info!( + "set clock divider: {} based on sys freq: {}Hz", + defmt::Debug2Format(&cfg.clock_divider), + clk_sys_freq() + ); + + 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.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); + } + } + + let mut wrap_target = asm.label(); + asm.bind(&mut wrap_target); + + asm.in_with_side_set(InSource::PINS, 1, 0); + asm.nop_with_side_set(1); + + let mut wrap = asm.label(); + asm.bind(&mut wrap); + + defmt::info!("trying assemble..."); + asm.assemble_with_wrap(wrap, wrap_target) +} diff --git a/src/bringup/mod.rs b/src/bringup/mod.rs index 71c2d4b..e59e8c3 100644 --- a/src/bringup/mod.rs +++ b/src/bringup/mod.rs @@ -8,7 +8,12 @@ use embassy_rp::{ Output, Pin, }, + i2c::{ + self, + I2c, + }, peripherals::{ + I2C0, SPI0, SPI1, USB, @@ -27,48 +32,87 @@ use embassy_sync::{ }, mutex::Mutex, }; +use molybdos::embassy_rp::peripherals::{ + DMA_CH5, + I2C1, + PIO0, +}; use static_cell::StaticCell; use tap::Pipe; +use crate::SensorI2c; use molybdos::pal::StaticOutput; +pub mod i2s; + +pub use i2s::{ + StaticI2S, + I2S, +}; + embassy_rp::bind_interrupts! { struct Irqs { USBCTRL_IRQ => embassy_rp::usb::InterruptHandler; ADC_IRQ_FIFO => adc::InterruptHandler; + I2C0_IRQ => i2c::InterruptHandler; + I2C1_IRQ => i2c::InterruptHandler; + PIO0_IRQ_0 => embassy_rp::pio::InterruptHandler; } } -static BME_SPI: StaticCell>> = StaticCell::new(); -static SD_SPI: StaticCell>> = StaticCell::new(); +static BME_SPI: StaticCell>> = + StaticCell::new(); +static SD_SPI: StaticCell>> = + StaticCell::new(); static ADC: StaticCell>> = StaticCell::new(); - -pub struct Split -where +static SENSOR_I2C: StaticCell> = StaticCell::new(); + +static PIO0: StaticCell> = StaticCell::new(); + +pub struct Split< + BmeSpi, + BmeSpiMutex, + SdSpi, + SdSpiMutex, + // SensorI2c, + // SensorI2cMutex, +> where SdSpiMutex: RawMutex + 'static, SdSpi: 'static, BmeSpiMutex: RawMutex + 'static, BmeSpi: 'static, + // SensorI2cMutex: RawMutex + 'static, + // SensorI2c: 'static, { pub sd_spi: &'static Mutex, pub sd_cs: StaticOutput, pub bme_spi: &'static Mutex, pub bme_cs: StaticOutput, + // pub sensor_i2c: &'static Mutex, pub wdt: Watchdog, pub usb: molybdos::pal::UsbDriver, - pub adc: &'static Mutex>, + pub i2s: i2s::I2S<'static, 'static, embassy_rp::peripherals::PIO0, 0>, + pub i2s_dma: DMA_CH5, + pub adc: &'static Mutex>, // pub leds: [StaticOutput; crate::N_LED], } pub fn split( config: embassy_rp::config::Config, -) -> Split { - let periphs = embassy_rp::init(config); +) -> Split< + crate::BMESpi, + CriticalSectionRawMutex, + crate::SdSpi, + CriticalSectionRawMutex, + // crate::SensorI2c, + // CriticalSectionRawMutex, +> { + let mut periphs = embassy_rp::init(config); let wdt = Watchdog::new(periphs.WATCHDOG); - let bme_cs = periphs.PIN_5.degrade().pipe(|cs| Output::new(cs, Level::High)); + let bme_cs = periphs.PIN_13.degrade().pipe(|cs| Output::new(cs, Level::High)); let bme_spi = { let sck = periphs.PIN_10; let mosi = periphs.PIN_11; @@ -83,7 +127,7 @@ pub fn split( BME_SPI.init(bme_spi) }; - let sd_cs = periphs.PIN_9.degrade().pipe(|cs| Output::new(cs, Level::High)); + let sd_cs = periphs.PIN_17.degrade().pipe(|cs| Output::new(cs, Level::High)); let sd_spi = { let mosi = periphs.PIN_19; let sck = periphs.PIN_18; @@ -99,20 +143,39 @@ pub fn split( SD_SPI.init(sd_spi) }; + // let i2c = { + // let mut conf = i2c::Config::default(); + // conf.frequency = 400_000; + // + // let i2c = SensorI2c::new_async(periphs.I2C1, periphs.PIN_23, periphs.PIN_22, Irqs, conf) + // .pipe(Mutex::new); + // SENSOR_I2C.init(i2c) + // }; + let usb_driver = embassy_rp::usb::Driver::new(periphs.USB, Irqs); let adc = Adc::new(periphs.ADC, Irqs, adc::Config::default()); - let adc = ADC.init(Mutex::new(adc)); + // { + // StaticOutput::new(periphs.PIN_24, Level::High); + // } + + let pio0 = embassy_rp::pio::Pio::new(periphs.PIO0, Irqs); + let pio = PIO0.init(pio0); + + let i2s = i2s::I2S::new(pio, periphs.PIN_24, periphs.PIN_23, periphs.PIN_25); + Split { sd_spi, sd_cs, bme_spi, bme_cs, wdt, + // sensor_i2c: i2c, usb: usb_driver, - + i2s, + i2s_dma: periphs.DMA_CH5, adc, } } diff --git a/src/lib.rs b/src/lib.rs index fd70da7..6a9d858 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -12,4 +12,5 @@ pub enum Uplink { )] pub enum Downlink { Pong, + PDM(molybdos::heapless::Vec), } diff --git a/src/main.rs b/src/main.rs index 37b413c..6cf46be 100644 --- a/src/main.rs +++ b/src/main.rs @@ -2,8 +2,11 @@ #![no_main] #![feature(impl_trait_in_assoc_type)] +use crate::usb::COBS_DOWNLINK; use molybdos::{ embassy_embedded_hal::shared_bus::asynch::spi::SpiDevice, + embassy_executor, + embassy_rp, embassy_rp::{ i2c::{ self, @@ -11,6 +14,8 @@ use molybdos::{ }, peripherals::{ I2C0, + I2C1, + PIO0, SPI0, SPI1, }, @@ -20,8 +25,17 @@ use molybdos::{ }, }, embassy_sync::{ - blocking_mutex::raw::CriticalSectionRawMutex, + blocking_mutex::raw::{ + CriticalSectionRawMutex, + ThreadModeRawMutex, + }, + pipe::Pipe, pubsub, + pubsub::PubSubBehavior, + }, + embedded_hal_async::spi::{ + Operation, + SpiDevice as _, }, embedded_sdmmc_async::{ VolumeIdx, @@ -29,8 +43,10 @@ use molybdos::{ }, genlog, genlog::defmt, + heapless, pal::StaticOutput, }; +use ocularium::Downlink; mod bringup; mod usb; @@ -40,13 +56,15 @@ pub type BMESpiDev = SpiDevice<'static, CriticalSectionRawMutex, BMESpi, StaticO pub type SdSpi = Spi<'static, SPI0, spi::Async>; pub type SdSpiDev = SpiDevice<'static, CriticalSectionRawMutex, SdSpi, StaticOutput>; -pub type SensorI2c = I2c<'static, I2C0, i2c::Async>; +pub type SensorI2c = I2c<'static, I2C1, i2c::Async>; + +pub static I2S_PIPE: Pipe = Pipe::new(); -#[embassy_executor::main] +#[::embassy_executor::main] async fn main(spawner: embassy_executor::Spawner) { molybdos::pal::heap::init(); - genlog::info!("boot"); + defmt::info!("boot"); let bringup::Split { sd_spi, @@ -55,9 +73,13 @@ async fn main(spawner: embassy_executor::Spawner) { bme_cs, wdt, usb, + i2s, + i2s_dma, .. } = bringup::split(Default::default()); + defmt::info!("split ok"); + // spawner.must_spawn(molybdos::pal::watchdog(wdt)); let (acm,) = molybdos::lib::usb::bringup!( @@ -81,18 +103,63 @@ async fn main(spawner: embassy_executor::Spawner) { &usb::COBS_DOWNLINK ); + let mut spidev = SpiDevice::new(bme_spi, bme_cs); + + let data = &mut [0u8]; + + let result = spidev + .transaction(&mut [Operation::Write(&[0x50 | (1 << 7)]), Operation::Read(data)]) + .await; + if let Err(e) = result { + defmt::error!("reading spi dev: {}", defmt::Debug2Format(&e)); + } else { + defmt::info!("read chipid: {:x}", data[0]); + } + #[cfg(not(feature = "disable_sd"))] { let vm = molybdos::rt::sd::bringup!(sd_spi, SdSpi, sd_cs); { - let vol = VolumeManager::open_volume(vm, VolumeIdx(0)).await.expect("opening volume"); - } + let vol = match VolumeManager::open_volume(vm, VolumeIdx(0)).await { + Ok(x) => x, + Err(e) => { + defmt::error!("opening volume: {}", defmt::Debug2Format(&e)); - // spawner.must_spawn(sd::run_sd(vm, pubsub::LED.sender().into())); + loop { + molybdos::embassy_time::Timer::after( + molybdos::embassy_time::Duration::from_secs(1), + ) + .await; + } + }, + }; + } } + spawner.must_spawn(drain_pipe()); + spawner.must_spawn(run_i2s(i2s, i2s_dma)); + loop { molybdos::embassy_time::Timer::after(molybdos::embassy_time::Duration::from_secs(1)).await; } } + +#[::embassy_executor::task] +async fn run_i2s(mut i2s: bringup::StaticI2S, dma: embassy_rp::peripherals::DMA_CH5) { + i2s.run::<960>(dma, &I2S_PIPE).await +} + +#[::embassy_executor::task] +async fn drain_pipe() { + let publ = COBS_DOWNLINK.publisher().expect("getting publisher"); + + loop { + let buf = &mut [0u8; 960]; + let n = I2S_PIPE.read(buf).await; + + for elem in (&buf[..n]).chunks(48) { + publ.publish_immediate(Downlink::PDM(heapless::Vec::from_slice(elem).unwrap())); + } + } +} -- cgit v1.3.1