pio i2s

4 files changed, 430 insertions, 18 deletions

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<const IDX: usize>;
+
+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<PIO, const IDX: usize> = 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<P = impl pio::PioPin> + 'static,
+        data: impl Peripheral<P = impl pio::PioPin> + 'static,
+        ws: impl Peripheral<P = impl pio::PioPin> + 'static,
+    ) -> Self
+    where
+        SmLookup<IDX>: 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 = <SmLookup<IDX> as GetSm<PIO, IDX>>::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<const BUFFER_SIZE: usize>(
+        &mut self,
+        dma: impl Peripheral<P = impl embassy_rp::dma::Channel>,
+        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<const PROGRAM_SIZE: usize>(
+    warmup: Option<WarmupConfig>,
+) -> molybdos::pio::Program<PROGRAM_SIZE> {
+    let mut asm =
+        molybdos::pio::Assembler::<PROGRAM_SIZE>::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<USB>;
         ADC_IRQ_FIFO => adc::InterruptHandler;
+        I2C0_IRQ => i2c::InterruptHandler<I2C0>;
+        I2C1_IRQ => i2c::InterruptHandler<I2C1>;
+        PIO0_IRQ_0 => embassy_rp::pio::InterruptHandler<PIO0>;
     }
 }
 
-static BME_SPI: StaticCell<Mutex<CriticalSectionRawMutex, Spi<SPI1, Async>>> = StaticCell::new();
-static SD_SPI: StaticCell<Mutex<CriticalSectionRawMutex, Spi<SPI0, Async>>> = StaticCell::new();
+static BME_SPI: StaticCell<Mutex<CriticalSectionRawMutex, spi::Spi<SPI1, Async>>> =
+    StaticCell::new();
+static SD_SPI: StaticCell<Mutex<CriticalSectionRawMutex, spi::Spi<SPI0, Async>>> =
+    StaticCell::new();
 static ADC: StaticCell<Mutex<CriticalSectionRawMutex, Adc<adc::Async>>> = StaticCell::new();
+static SENSOR_I2C: StaticCell<Mutex<CriticalSectionRawMutex, SensorI2c>> = StaticCell::new();
+
+static PIO0: StaticCell<embassy_rp::pio::Pio<'static, PIO0>> = StaticCell::new();
 
-pub struct Split<BmeSpi, BmeSpiMutex, SdSpi, SdSpiMutex>
-where
+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<SdSpiMutex, SdSpi>,
     pub sd_cs:   StaticOutput,
     pub bme_spi: &'static Mutex<BmeSpiMutex, BmeSpi>,
     pub bme_cs:  StaticOutput,
+    // pub sensor_i2c: &'static Mutex<SensorI2cMutex, SensorI2c>,
     pub wdt:     Watchdog,
     pub usb:     molybdos::pal::UsbDriver,
 
-    pub adc: &'static Mutex<CriticalSectionRawMutex, Adc<'static, adc::Async>>,
+    pub i2s:     i2s::I2S<'static, 'static, embassy_rp::peripherals::PIO0, 0>,
+    pub i2s_dma: DMA_CH5,
+    pub adc:     &'static Mutex<CriticalSectionRawMutex, Adc<'static, adc::Async>>,
     // pub leds: [StaticOutput; crate::N_LED],
 }
 
 pub fn split(
     config: embassy_rp::config::Config,
-) -> Split<crate::BMESpi, CriticalSectionRawMutex, crate::SdSpi, CriticalSectionRawMutex> {
-    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<u8, 48>),
 }
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<ThreadModeRawMutex, 1024> = 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<PIO0, 0>, 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()));
+        }
+    }
+}