audio works

kernel/src/audio.rs

@@ -1,51 +1,92 @@
 use crate::Audio;
 use core::sync::atomic::{AtomicBool, Ordering};
+use embassy_futures::join::join;
 use embassy_rp::{
     Peri,
+    clocks::clk_sys_freq,
     dma::{AnyChannel, Channel},
     gpio::Level,
     pio::{
         Common, Config, Direction, FifoJoin, Instance, LoadedProgram, PioPin, ShiftConfig,
-        ShiftDirection, StateMachine, program::pio_asm,
+        StateMachine, program::pio_asm,
     },
     pio_programs::clock_divider::calculate_pio_clock_divider,
 };
-use embassy_time::Timer;
+use fixed::traits::ToFixed;
 
-const AUDIO_BUFFER_LEN: usize = 1024;
-const _: () = assert!(AUDIO_BUFFER_LEN == abi_sys::AUDIO_BUFFER_LEN);
-pub static mut AUDIO_BUFFER: [u8; AUDIO_BUFFER_LEN] = [0; AUDIO_BUFFER_LEN];
-static mut AUDIO_BUFFER_1: [u8; AUDIO_BUFFER_LEN] = [0; AUDIO_BUFFER_LEN];
+pub const SAMPLE_RATE_HZ: u32 = 22_050;
+const AUDIO_BUFFER_SAMPLES: usize = 1024;
+const _: () = assert!(AUDIO_BUFFER_SAMPLES == abi_sys::AUDIO_BUFFER_SAMPLES);
+
+// 8bit stereo interleaved PCM audio buffers
+pub static mut AUDIO_BUFFER: [u8; AUDIO_BUFFER_SAMPLES * 2] = [0; AUDIO_BUFFER_SAMPLES * 2];
+static mut AUDIO_BUFFER_1: [u8; AUDIO_BUFFER_SAMPLES * 2] = [0; AUDIO_BUFFER_SAMPLES * 2];
 
 pub static AUDIO_BUFFER_READY: AtomicBool = AtomicBool::new(true);
 
-pub const SAMPLE_RATE_HZ: u32 = 8_000;
-
 #[embassy_executor::task]
 pub async fn audio_handler(mut audio: Audio) {
+    const SILENCE_VALUE: u8 = u8::MAX / 2;
+
     let prg = PioPwmAudioProgram8Bit::new(&mut audio.pio);
-    defmt::info!("loaded prg");
+    let mut pwm_pio_left =
+        PioPwmAudio::new(audio.dma0, &mut audio.pio, audio.sm0, audio.left, &prg);
+    let mut pwm_pio_right =
+        PioPwmAudio::new(audio.dma1, &mut audio.pio, audio.sm1, audio.right, &prg);
 
-    let mut pwm_pio = PioPwmAudio::new(audio.dma, &mut audio.pio, audio.sm0, audio.left, &prg);
-    defmt::info!("cfgd sm");
-
-    pwm_pio.configure(SAMPLE_RATE_HZ);
-    pwm_pio.start();
     loop {
-        pwm_pio.write(unsafe { &AUDIO_BUFFER_1 }).await;
+        write_samples(&mut pwm_pio_left, &mut pwm_pio_right, unsafe {
+            &AUDIO_BUFFER_1
+        })
+        .await;
+        unsafe { &mut AUDIO_BUFFER_1 }.fill(SILENCE_VALUE);
 
         unsafe { core::mem::swap(&mut AUDIO_BUFFER, &mut AUDIO_BUFFER_1) };
         AUDIO_BUFFER_READY.store(true, Ordering::Release)
     }
 }
 
+async fn write_samples<PIO: Instance>(
+    left: &mut PioPwmAudio<'static, PIO, 0>,
+    right: &mut PioPwmAudio<'static, PIO, 1>,
+    buf: &[u8],
+) {
+    // pack two samples per word
+    let mut packed_buf_left: [u32; AUDIO_BUFFER_SAMPLES / 2] = [0; AUDIO_BUFFER_SAMPLES / 2];
+    let mut packed_buf_right: [u32; AUDIO_BUFFER_SAMPLES / 2] = [0; AUDIO_BUFFER_SAMPLES / 2];
+
+    for ((pl, pr), sample) in packed_buf_left
+        .iter_mut()
+        .zip(packed_buf_right.iter_mut())
+        .zip(buf.chunks(4))
+    {
+        *pl = pack_u8_samples(sample[0], sample[2]);
+        *pr = pack_u8_samples(sample[1], sample[3]);
+    }
+
+    let left_fut = left
+        .sm
+        .tx()
+        .dma_push(left.dma.reborrow(), &packed_buf_left, false);
+
+    let right_fut = right
+        .sm
+        .tx()
+        .dma_push(right.dma.reborrow(), &packed_buf_right, false);
+
+    join(left_fut, right_fut).await;
+}
+
 struct PioPwmAudioProgram8Bit<'d, PIO: Instance>(LoadedProgram<'d, PIO>);
 
+/// Writes one sample to pwm as high and low time
 impl<'d, PIO: Instance> PioPwmAudioProgram8Bit<'d, PIO> {
     fn new(common: &mut Common<'d, PIO>) -> Self {
+        // only uses 16 bits top for high, bottom for low
+        // allows two samples per word
         let prg = pio_asm!(
-            "out x, 8", // samples <<
-            "out y, 8", // samples max >>
+            "out x, 8", // pwm high time
+            "out y, 8", // pwm low time
             "loop_high:",
             "set pins, 1",       // keep pin high
             "jmp x-- loop_high", // decrement X until 0
@@ -62,7 +103,6 @@ impl<'d, PIO: Instance> PioPwmAudioProgram8Bit<'d, PIO> {
 
 struct PioPwmAudio<'d, PIO: Instance, const SM: usize> {
     dma: Peri<'d, AnyChannel>,
-    cfg: Config<'d, PIO>,
     sm: StateMachine<'d, PIO, SM>,
 }
 
@@ -87,51 +127,24 @@ impl<'d, PIO: Instance, const SM: usize> PioPwmAudio<'d, PIO, SM> {
         cfg.use_program(&prg.0, &[]);
         sm.set_config(&cfg);
 
+        let target_clock = (u8::MAX as u32 + 1) * SAMPLE_RATE_HZ;
+        let divider = (clk_sys_freq() / (target_clock * 2)).to_fixed();
+        sm.set_clock_divider(divider);
+
+        sm.set_enable(true);
+
         Self {
             dma: dma.into(),
-            cfg,
             sm,
         }
     }
-
-    fn configure(&mut self, sample_rate: u32) {
-        let cycles_per_sample = u8::MAX as u32 + 2; // X_max + Y_max + movs
-        let target_pio_hz = cycles_per_sample * sample_rate; // ~11.3 MHz
-
-        let divider = calculate_pio_clock_divider(target_pio_hz);
-        self.cfg.clock_divider = divider;
-        self.sm.set_clock_divider(divider);
-    }
-
-    async fn write(&mut self, buf: &[u8]) {
-        let mut packed_buf = [0_u32; AUDIO_BUFFER_LEN / 4];
-
-        for (packed_sample, sample) in packed_buf.iter_mut().zip(buf.chunks(2)) {
-            *packed_sample = pack_two_samples(sample[0], sample[1]);
-        }
-
-        self.sm
-            .tx()
-            .dma_push(self.dma.reborrow(), &packed_buf, false)
-            .await
-    }
-
-    fn start(&mut self) {
-        self.sm.set_enable(true);
-    }
-
-    fn stop(&mut self) {
-        self.sm.set_enable(false);
-    }
 }
 
-fn pack_two_samples(s1: u8, s2: u8) -> u32 {
-    let x = ((s1 as u16) << 8 | (255 - s1) as u16); // original
-    let y = ((s2 as u16) << 8 | (255 - s2) as u16);
-
-    // Scale to full 16-bit for higher volume:
-    let x_scaled = ((x as u32) << 8) | (x as u32 >> 8);
-    let y_scaled = ((y as u32) << 8) | (y as u32 >> 8);
-
-    (x_scaled << 16) | y_scaled
+/// packs two u8 samples into 32bit word
+fn pack_u8_samples(sample1: u8, sample2: u8) -> u32 {
+    (u8_pcm_to_pwm(sample1) as u32) << 16 | u8_pcm_to_pwm(sample2) as u32
+}
+
+fn u8_pcm_to_pwm(sample: u8) -> u16 {
+    ((sample as u16) << 8) | ((u8::MAX - sample) as u16)
 }