lpc55s28 example refactoring and improved feedback

examples/lpc55s28-evk/.cargo/config.toml

@@ -16,4 +16,4 @@ rustflags = [
 target = "thumbv8m.main-none-eabihf"
 
 [env]
-DEFMT_LOG = "debug"
+DEFMT_LOG = "info"

examples/lpc55s28-evk/Cargo.toml

@@ -3,6 +3,11 @@ name = "lpc55s28-evk"
 version = "0.1.0"
 edition = "2024"
 
+[features]
+default = ["usbhs"]
+usbfs = []
+usbhs = []
+
 [dependencies]
 cortex-m = { version = "0.7.7", features = ["critical-section-single-core"] }
 cortex-m-rt = "0.7.5"

examples/lpc55s28-evk/src/hw.rs

@@ -1,3 +1,8 @@
+//! Contains hardware setup unrelated to Usb Audio Class implementation
+
+use crate::Syscon;
+use crate::{MCLK_FREQ, SAMPLE_RATE, pac};
+use defmt::debug;
 pub(crate) struct PllConstants {
     pub m: u16,   // 1-65535
     pub n: u8,    // 1-255
@@ -54,3 +59,168 @@ impl defmt::Format for PllConstants {
         );
     }
 }
+
+// Fo = M/(N*2*P) * Fin
+// Fo = 3072/(125*2*8) * 16MHz = 24.576MHz
+const AUDIO_PLL: PllConstants = PllConstants::new(125, 3072, 8);
+
+// Set PLL0 to 24.576MHz, start, and wait for lock
+// This is not exposed by lpc55-hal, unfortunately. Copy their implementation here.
+pub(crate) fn init_audio_pll() {
+    let syscon = unsafe { &*pac::SYSCON::ptr() };
+    let pmc = unsafe { &*pac::PMC::ptr() };
+    let anactrl = unsafe { &*pac::ANACTRL::ptr() };
+
+    debug!("start clk_in");
+    pmc.pdruncfg0
+        .modify(|_, w| w.pden_xtal32m().poweredon().pden_ldoxo32m().poweredon());
+    syscon.clock_ctrl.modify(|_, w| w.clkin_ena().enable());
+    anactrl
+        .xo32m_ctrl
+        .modify(|_, w| w.enable_system_clk_out().enable());
+
+    debug!("init pll: {}", AUDIO_PLL);
+    pmc.pdruncfg0
+        .modify(|_, w| w.pden_pll0().poweredoff().pden_pll0_sscg().poweredoff());
+    syscon.pll0clksel.write(|w| w.sel().enum_0x1()); // clk_in
+    syscon.pll0ctrl.write(|w| unsafe {
+        w.clken()
+            .enable()
+            .seli()
+            .bits(AUDIO_PLL.seli)
+            .selp()
+            .bits(AUDIO_PLL.selp)
+    });
+
+    syscon
+        .pll0ndec
+        .write(|w| unsafe { w.ndiv().bits(AUDIO_PLL.n) });
+    syscon.pll0ndec.write(|w| unsafe {
+        w.ndiv().bits(AUDIO_PLL.n).nreq().set_bit() // latch
+    });
+
+    syscon
+        .pll0pdec
+        .write(|w| unsafe { w.pdiv().bits(AUDIO_PLL.p) });
+    syscon.pll0pdec.write(|w| unsafe {
+        w.pdiv().bits(AUDIO_PLL.p).preq().set_bit() // latch
+    });
+
+    syscon.pll0sscg0.write(|w| unsafe { w.md_lbs().bits(0) });
+
+    syscon
+        .pll0sscg1
+        .write(|w| unsafe { w.mdiv_ext().bits(AUDIO_PLL.m).sel_ext().set_bit() });
+    syscon.pll0sscg1.write(|w| unsafe {
+        w.mdiv_ext()
+            .bits(AUDIO_PLL.m)
+            .sel_ext()
+            .set_bit()
+            .mreq()
+            .set_bit() // latch
+            .md_req()
+            .set_bit() // latch
+    });
+
+    pmc.pdruncfg0
+        .modify(|_, w| w.pden_pll0().poweredon().pden_pll0_sscg().poweredon());
+    debug!("pll0 wait for lock");
+    let mut i = 0usize;
+    while syscon.pll0stat.read().lock().bit_is_clear() {
+        i += 1;
+    }
+    debug!("pll0 locked after {} tries", i);
+}
+
+pub struct I2sTx {
+    pub i2s: pac::I2S7,
+}
+
+pub fn init_i2s(mut fc7: pac::FLEXCOMM7, i2s7: pac::I2S7, syscon: &mut Syscon) -> I2sTx {
+    defmt::debug!("init i2s");
+    // Enable BOTH
+    syscon.reset(&mut fc7);
+    syscon.enable_clock(&mut fc7);
+
+    unsafe {
+        pac::IOCON::ptr().as_ref().unwrap().pio1_31.modify(|_, w| {
+            w.func()
+                .alt1()
+                .mode()
+                .inactive()
+                .slew()
+                .fast()
+                .invert()
+                .disabled()
+                .digimode()
+                .digital()
+                .od()
+                .normal()
+        });
+        pac::SYSCON::ptr()
+            .as_ref()
+            .unwrap()
+            .fcclksel7()
+            .modify(|_, w| w.sel().enum_0x5()); // MCLK
+        pac::SYSCON::ptr()
+            .as_ref()
+            .unwrap()
+            .mclkclksel
+            .modify(|_, w| w.sel().enum_0x1()); // PLL0
+        pac::SYSCON::ptr()
+            .as_ref()
+            .unwrap()
+            .mclkdiv
+            .modify(|_, w| w.div().bits(1).halt().run().reset().released()); // div by 2 = PLL0 fout / 2 = 12.288MHz, max for WM8904 @ 96k
+        pac::SYSCON::ptr()
+            .as_ref()
+            .unwrap()
+            .mclkio
+            .modify(|_, w| w.mclkio().output());
+    };
+
+    // Select I2S TX function
+    fc7.pselid.write(|w| w.persel().i2s_transmit());
+
+    let regs = i2s7;
+
+    // Enable TX FIFO only
+    regs.fifocfg.modify(|_, w| {
+        w.enabletx()
+            .enabled()
+            .enablerx()
+            .disabled()
+            .dmatx()
+            .disabled()
+            .txi2se0()
+            .zero()
+    });
+
+    // Flush
+    regs.fifocfg.modify(|_, w| w.emptytx().set_bit());
+
+    regs.cfg2
+        .modify(|_, w| unsafe { w.position().bits(0).framelen().bits(63) }); // framelen = 64
+
+    let bclk_div = (MCLK_FREQ / SAMPLE_RATE / 64) as u16;
+    regs.div
+        .modify(|_, w| unsafe { w.div().bits(bclk_div - 1) }); // Clock source is MCLK (12.288MHz) / 4 = 3MHz
+
+    // Config
+    regs.cfg1.modify(|_, w| unsafe {
+        w.mstslvcfg()
+            .normal_master()
+            .onechannel()
+            .dual_channel()
+            .datalen()
+            .bits(31)
+            .mainenable()
+            .enabled()
+            .mode()
+            .classic_mode()
+            .datapause()
+            .normal()
+    });
+
+    I2sTx { i2s: regs }
+}

examples/lpc55s28-evk/src/main.rs

@@ -1,25 +1,35 @@
+//! Interrupt driven example for the LPCXpresso55S28 demo board
+//!
+//! Uses the onboard WM8904 DAC at 48KHz. Clock is generated by PLL0. Simple PI feedback
+//! is implemented.
+//!
+//! Packets from USB are placed a `heapless::spsc::Queue`. They are consumed
+//! by the I2S FIFO in the FLEXCOMM7 interrupt.
 #![no_main]
 #![no_std]
 
+#[cfg(all(feature = "usbfs", feature = "usbhs"))]
+compile_error!("Choose one USB peripheral, usbfs and usbhs cannot be used together");
+
 extern crate panic_probe;
 #[defmt::panic_handler]
 fn panic() -> ! {
     panic_probe::hard_fault()
 }
 
-use core::cell::UnsafeCell;
 use core::ptr::null_mut;
-use core::sync::atomic::{AtomicBool, AtomicI32, AtomicUsize, Ordering};
+use core::sync::atomic::{AtomicBool, AtomicI32, Ordering};
 use cortex_m_rt::entry;
-use defmt;
 use defmt::debug;
 use defmt_rtt as _;
-use hal::Syscon;
-use hal::drivers::{Timer, UsbBus, pins};
-use hal::prelude::*;
 use hal::raw as pac;
-use hal::time::Hertz;
+use hal::{
-use heapless::spsc::Consumer;
+    Syscon,
+    drivers::{Timer, UsbBus, pins},
+    prelude::*,
+    time::Hertz,
+};
+use heapless::spsc::{Consumer, Producer, Queue};
 use lpc55_hal::{self as hal};
 use pac::interrupt;
 use static_cell::StaticCell;
@@ -35,16 +45,15 @@ use usbd_uac2::{
     descriptors::{ChannelConfig, ClockType, FormatType1, LockDelay},
 };
 
+use crate::hw::I2sTx;
+
 mod hw;
 mod wm8904;
 
 const CODEC_I2C_ADDR: u8 = 0b0011010;
-// Fo = M/(N*2*P) * Fin
+const FIFO_LENGTH: usize = 256; // frames
-// Fo = 3072/(125*2*8) * 16MHz = 24.576MHz
-const AUDIO_PLL: hw::PllConstants = hw::PllConstants::new(125, 3072, 8);
-const FIFO_LENGTH: usize = 2048; // frames
 const MCLK_FREQ: u32 = 12288000;
-const SAMPLE_RATE: u32 = 48000;
+const SAMPLE_RATE: u32 = 48000; // example implementation runs okay at 48k but not 96k
 type SampleType = (i32, i32);
 
 struct Clock {}
@@ -67,71 +76,22 @@ impl UsbAudioClockImpl for Clock {
     }
 }
 
-#[derive(Default)]
-struct PerfCounters {
-    frames: AtomicUsize,
-    min_fill: AtomicUsize,
-    avg_fill: AtomicUsize,
-    queue_underflows: AtomicUsize,
-    queue_overflows: AtomicUsize,
-    audio_underflows: AtomicUsize,
-}
-
-impl PerfCounters {
-    fn reset(&self) {
-        self.frames.store(0, Ordering::Relaxed);
-        self.min_fill.store(FIFO_LENGTH, Ordering::Relaxed);
-        self.avg_fill.store(FIFO_LENGTH / 2, Ordering::Relaxed);
-        self.queue_underflows.store(0, Ordering::Relaxed);
-        self.queue_overflows.store(0, Ordering::Relaxed);
-        self.audio_underflows.store(0, Ordering::Relaxed);
-    }
-}
-
-impl defmt::Format for PerfCounters {
-    fn format(&self, fmt: defmt::Formatter) {
-        defmt::write!(
-            fmt,
-            "frames: {} min_fill: {} avg fill: {} a_underflows: {} q_underflows: {} q_overflows: {}",
-            self.frames.load(Ordering::Relaxed),
-            self.min_fill.load(Ordering::Relaxed),
-            self.avg_fill.load(Ordering::Relaxed),
-            self.audio_underflows.load(Ordering::Relaxed),
-            self.queue_underflows.load(Ordering::Relaxed),
-            self.queue_overflows.load(Ordering::Relaxed)
-        )
-    }
-}
-
 static FIFO_CONSUMER_STORE: StaticCell<Consumer<SampleType>> = StaticCell::new();
 static mut FIFO_CONSUMER: *mut Consumer<SampleType> = null_mut();
 
-static PERF: PerfCounters = PerfCounters {
-    frames: AtomicUsize::new(0),
-    min_fill: AtomicUsize::new(FIFO_LENGTH),
-    avg_fill: AtomicUsize::new(FIFO_LENGTH / 2),
-    queue_underflows: AtomicUsize::new(0),
-    queue_overflows: AtomicUsize::new(0),
-    audio_underflows: AtomicUsize::new(0),
-};
-
 #[interrupt]
 fn FLEXCOMM7() {
     let i2s = unsafe { &*pac::I2S7::ptr() };
 
-    if i2s.fifostat.read().txlvl().bits() == 0 {
-        PERF.audio_underflows.fetch_add(1, Ordering::Relaxed);
-    }
-
     // refil the buffer to 4 frames / 8 samples
-    while i2s.fifostat.read().txlvl().bits() <= 6 {
     let fifo = unsafe { &mut *FIFO_CONSUMER };
+    while i2s.fifostat.read().txlvl().bits() <= 6 {
         if let Some((l, r)) = fifo.dequeue() {
             i2s.fifowr.write(|w| unsafe { w.bits(l as u32) });
             i2s.fifowr.write(|w| unsafe { w.bits(r as u32) });
         } else {
-            PERF.queue_underflows.fetch_add(1, Ordering::Relaxed);
+            // Queue underflow
-            PERF.min_fill.fetch_add(0, Ordering::Relaxed);
+            defmt::error!("queue underflow");
             i2s.fifowr.write(|w| unsafe { w.bits(0) });
             i2s.fifowr.write(|w| unsafe { w.bits(0) });
         }
@@ -141,7 +101,7 @@ fn FLEXCOMM7() {
 struct Audio<'a> {
     running: AtomicBool,
     i2s: I2sTx,
-    producer: UnsafeCell<heapless::spsc::Producer<'a, SampleType>>,
+    producer: Producer<'a, SampleType>,
     integrator: AtomicI32,
 }
 impl<'a> Audio<'a> {
@@ -149,267 +109,111 @@ impl<'a> Audio<'a> {
         self.running.store(true, Ordering::Relaxed);
         defmt::info!("playback starting, enabling interrupts");
         self.i2s.i2s.fifointenclr.write(|w| w.txlvl().set_bit());
-        // FIFO threshold trigger enable
+        // FIFO trigger threshold = <= 6 entries
         self.i2s
             .i2s
             .fifotrig
-            .modify(|_, w| unsafe { w.txlvl().bits(4).txlvlena().enabled() });
+            .modify(|_, w| unsafe { w.txlvl().bits(6).txlvlena().enabled() });
         // FIFO level interrupt enable
         self.i2s.i2s.fifointenset.modify(|_, w| w.txlvl().enabled());
         unsafe { pac::NVIC::unmask(pac::Interrupt::FLEXCOMM7) };
     }
     fn stop(&self) {
+        // If we don't disable interrupts while stopped, we will underflow constantly and continuously refill the fifo with 0s
+        // We could actually stop the I2S here, but sometimes that makes the DAC misbehave. The peripheral is configured to send
+        // 0s when the FIFO is empty, so this is fine.
         self.running.store(true, Ordering::Relaxed);
-        defmt::info!("playback stopped: {}", PERF);
+        defmt::info!("playback stopped");
-        PERF.reset();
         pac::NVIC::mask(pac::Interrupt::FLEXCOMM7);
     }
 }
 impl<'a, B: bus::UsbBus> UsbAudioClass<'a, B> for Audio<'_> {
-    fn alternate_setting_changed<CS: UsbAudioClockImpl, AU: UsbAudioClass<'a, B>>(
+    fn alternate_setting_changed(&mut self, _terminal: usb_device::UsbDirection, alt_setting: u8) {
-        &self,
+        // alt setting 0 means stopped
-        ac: &mut usbd_uac2::AudioClass<'a, B, CS, AU>,
-        terminal: usb_device::UsbDirection,
-        alt_setting: u8,
-    ) {
         match alt_setting {
             0 => self.stop(),
             1 => self.start(),
             _ => defmt::error!("unexpected alt setting {}", alt_setting),
         }
     }
-    fn audio_data_rx(&self, ep: &usb_device::endpoint::Endpoint<'a, B, usb_device::endpoint::Out>) {
+    fn audio_data_rx(
-        let mut buf = [0; 384];
+        &mut self,
+        ep: &usb_device::endpoint::Endpoint<'a, B, usb_device::endpoint::Out>,
+    ) {
+        // Buffer must fit 1ms of audio data (based on how `usbd_uac2` sets up the descriptors), calculate that size here.
+        let mut buf = [0; SAMPLE_RATE as usize / 1000 * core::mem::size_of::<SampleType>()];
         let len = match ep.read(&mut buf) {
             Ok(len) => len,
-            Err(e) => {
+            Err(_) => {
                 defmt::error!("usb error in rx callback");
                 return;
             }
         };
         let buf = &buf[..len];
-        for sample in buf.chunks_exact(8).map(|b| {
+        // Translate the raw USB data into audio frames
+        for sample in buf
+            .chunks_exact(core::mem::size_of::<SampleType>())
+            .map(|b| {
+                // TODO: implement SampleType::from
                 (
                     i32::from_le_bytes(b[..4].try_into().unwrap()),
                     i32::from_le_bytes(b[4..].try_into().unwrap()),
                 )
-        }) {
+            })
-            if let Err(e) = unsafe { (*self.producer.get()).enqueue(sample) } {
+        {
-                PERF.queue_overflows.fetch_add(1, Ordering::Relaxed);
+            if self.producer.enqueue(sample).is_err() {
-                // defmt::error!("overflowed fifo, len: {}", unsafe {
+                defmt::error!("overflowed fifo, len: {}", self.producer.len());
-                //     (*self.producer.get()).len()
-                // });
             }
         }
     }
-    fn feedback(&self) -> Option<UsbIsochronousFeedback> {
-        const TARGET: i32 = FIFO_LENGTH as i32 / 2 - 64;
-        const NOMINAL: i32 = 48 << 16;
 
-        let queuelen = unsafe { (*self.producer.get()).len() };
+    /// Provide rate feedback to the host, so that it doesn't over- or underflow
-        let error = (queuelen as i32 - TARGET).clamp(-32, 32);
+    /// our queue.
+    fn feedback(&mut self) -> Option<UsbIsochronousFeedback> {
+        // Samples per USB interval (1ms)
+        const FRAME_SAMPLES: i32 = SAMPLE_RATE as i32 / 1000;
 
-        // --- integrator ---
+        // Keep FIFO around half full, minus one USB packet worth
-        let scaled_error = error / 64;
+        const TARGET: i32 = FIFO_LENGTH as i32 / 2 - FRAME_SAMPLES;
 
-        let new_i = self.integrator.fetch_add(scaled_error, Ordering::Relaxed) + scaled_error;
+        // 16.16 fixed-point nominal feedback value
-        let clamped = new_i.clamp(-131072, 131072);
+        const NOMINAL: i32 = FRAME_SAMPLES << 16;
+        const MAX_ERROR: i32 = FRAME_SAMPLES / 2;
 
-        // leak + store final value
+        let queuelen = self.producer.len() as i32;
-        let leaked = clamped - (clamped >> 8);
-        self.integrator.store(leaked, Ordering::Relaxed);
 
-        // reset on large deviation
+        let error = (queuelen - TARGET).clamp(-MAX_ERROR, MAX_ERROR);
-        if error.abs() > 96 {
-            self.integrator.store(0, Ordering::Relaxed);
-        }
 
-        // --- gains ---
+        // slow down accumulation of I
-        let p = error / 128;
+        const I_ACCUM_DIV: i32 = 8;
-        let i = leaked / 32768;
+        let i_delta = error / I_ACCUM_DIV;
+        let integrator = self.integrator.fetch_add(i_delta, Ordering::Relaxed) + i_delta;
 
-        // correction
+        // Integrator saturates at 1024xFRAME_SAMPLES
-        let correction = (-(p + i)).clamp(-32, 32);
+        let i_limit = FRAME_SAMPLES << 10;
-        let v = NOMINAL + (correction << 10);
+        let integrator = integrator.clamp(-i_limit, i_limit);
 
-        // EMA (unchanged, already correct)
+        // Gains
-        let ema = PERF.avg_fill.load(Ordering::Relaxed);
+        let p = error << 7;
-        let new = ((ema * 1023) + queuelen + 512) >> 10;
+        let i = integrator << 3;
-        PERF.avg_fill.store(new, Ordering::Relaxed);
+
+        // Total correction in 16.16 space
+        let correction = -(p + i);
+
+        let v = NOMINAL + correction;
 
         defmt::debug!(
-            "q:{} p:{} i:{} err:{} fb:{}+{}",
+            "q:{} err:{} i:{} fb:{}",
             queuelen,
-            p,
-            i,
             error,
-            NOMINAL >> 16,
+            integrator,
-            correction
+            v >> 16
         );
 
         Some(UsbIsochronousFeedback::new(v as u32))
     }
 }
 
-// Set PLL0 to 24.576MHz, start, and wait for lock
-// This is not exposed by lpc55-hal, unfortunately. Copy their implementation here.
-fn init_audio_pll() {
-    let syscon = unsafe { &*pac::SYSCON::ptr() };
-    let pmc = unsafe { &*pac::PMC::ptr() };
-    let anactrl = unsafe { &*pac::ANACTRL::ptr() };
-
-    debug!("start clk_in");
-    pmc.pdruncfg0
-        .modify(|_, w| w.pden_xtal32m().poweredon().pden_ldoxo32m().poweredon());
-    syscon.clock_ctrl.modify(|_, w| w.clkin_ena().enable());
-    anactrl
-        .xo32m_ctrl
-        .modify(|_, w| w.enable_system_clk_out().enable());
-
-    debug!("init pll: {}", AUDIO_PLL);
-    pmc.pdruncfg0
-        .modify(|_, w| w.pden_pll0().poweredoff().pden_pll0_sscg().poweredoff());
-    syscon.pll0clksel.write(|w| w.sel().enum_0x1()); // clk_in
-    syscon.pll0ctrl.write(|w| unsafe {
-        w.clken()
-            .enable()
-            .seli()
-            .bits(AUDIO_PLL.seli)
-            .selp()
-            .bits(AUDIO_PLL.selp)
-    });
-
-    syscon
-        .pll0ndec
-        .write(|w| unsafe { w.ndiv().bits(AUDIO_PLL.n) });
-    syscon.pll0ndec.write(|w| unsafe {
-        w.ndiv().bits(AUDIO_PLL.n).nreq().set_bit() // latch
-    });
-
-    syscon
-        .pll0pdec
-        .write(|w| unsafe { w.pdiv().bits(AUDIO_PLL.p) });
-    syscon.pll0pdec.write(|w| unsafe {
-        w.pdiv().bits(AUDIO_PLL.p).preq().set_bit() // latch
-    });
-
-    syscon.pll0sscg0.write(|w| unsafe { w.md_lbs().bits(0) });
-
-    syscon
-        .pll0sscg1
-        .write(|w| unsafe { w.mdiv_ext().bits(AUDIO_PLL.m).sel_ext().set_bit() });
-    syscon.pll0sscg1.write(|w| unsafe {
-        w.mdiv_ext()
-            .bits(AUDIO_PLL.m)
-            .sel_ext()
-            .set_bit()
-            .mreq()
-            .set_bit() // latch
-            .md_req()
-            .set_bit() // latch
-    });
-
-    pmc.pdruncfg0
-        .modify(|_, w| w.pden_pll0().poweredon().pden_pll0_sscg().poweredon());
-    debug!("pll0 wait for lock");
-    let mut i = 0usize;
-    while syscon.pll0stat.read().lock().bit_is_clear() {
-        i += 1;
-    }
-    debug!("pll0 locked after {} tries", i);
-}
-
-pub struct I2sTx {
-    pub i2s: pac::I2S7,
-}
-
-pub fn init_i2s(mut fc7: pac::FLEXCOMM7, i2s7: pac::I2S7, syscon: &mut Syscon) -> I2sTx {
-    defmt::debug!("init i2s");
-    // Enable BOTH
-    syscon.reset(&mut fc7);
-    syscon.enable_clock(&mut fc7);
-
-    unsafe {
-        pac::IOCON::ptr().as_ref().unwrap().pio1_31.modify(|_, w| {
-            w.func()
-                .alt1()
-                .mode()
-                .inactive()
-                .slew()
-                .fast()
-                .invert()
-                .disabled()
-                .digimode()
-                .digital()
-                .od()
-                .normal()
-        });
-        pac::SYSCON::ptr()
-            .as_ref()
-            .unwrap()
-            .fcclksel7()
-            .modify(|_, w| w.sel().enum_0x5()); // MCLK
-        pac::SYSCON::ptr()
-            .as_ref()
-            .unwrap()
-            .mclkclksel
-            .modify(|_, w| w.sel().enum_0x1()); // PLL0
-        pac::SYSCON::ptr()
-            .as_ref()
-            .unwrap()
-            .mclkdiv
-            .modify(|_, w| w.div().bits(1).halt().run().reset().released()); // div by 2 = PLL0 fout / 2 = 12.288MHz
-        pac::SYSCON::ptr()
-            .as_ref()
-            .unwrap()
-            .mclkio
-            .modify(|_, w| w.mclkio().output());
-    };
-
-    // Select I2S TX function
-    fc7.pselid.write(|w| w.persel().i2s_transmit());
-
-    let regs = i2s7;
-
-    // Enable TX FIFO only
-    regs.fifocfg.modify(|_, w| {
-        w.enabletx()
-            .enabled()
-            .enablerx()
-            .disabled()
-            .dmatx()
-            .disabled()
-            .txi2se0()
-            .zero()
-    });
-
-    // Flush
-    regs.fifocfg.modify(|_, w| w.emptytx().set_bit());
-
-    regs.cfg2
-        .modify(|_, w| unsafe { w.position().bits(0).framelen().bits(63) });
-
-    regs.div.modify(|_, w| unsafe { w.div().bits(3) }); // Clock source is MCLK (24MHz on FRO96) / 4 = 3MHz
-
-    // Config
-    regs.cfg1.modify(|_, w| unsafe {
-        w.mstslvcfg()
-            .normal_master()
-            .onechannel()
-            .dual_channel()
-            .datalen()
-            .bits(31)
-            .mainenable()
-            .enabled()
-            .mode()
-            .classic_mode()
-            .datapause()
-            .normal()
-    });
-
-    I2sTx { i2s: regs }
-}
-
 #[entry]
 fn main() -> ! {
     let hal = hal::new();
@@ -426,7 +230,7 @@ fn main() -> ! {
     let mut red_led = pins::Pio1_6::take()
         .unwrap()
         .into_gpio_pin(&mut iocon, &mut gpio)
-        .into_output(hal::drivers::pins::Level::Low); // start turned on
+        .into_output_low(); // start turned off
 
     debug!("iocon");
     let usb0_vbus_pin = pins::Pio0_22::take()
@@ -436,16 +240,16 @@ fn main() -> ! {
         pins::Pio1_20::take().unwrap().into_i2c4_scl_pin(&mut iocon),
         pins::Pio1_21::take().unwrap().into_i2c4_sda_pin(&mut iocon),
     );
-    let codec_i2s_pins = (
+    // We can initialize and iocon these, but there is no peripheral, so they do not get used
+    let _codec_i2s_pins = (
         pins::Pio0_21::take().unwrap().into_spi7_sck_pin(&mut iocon),
         pins::Pio0_20::take().unwrap().into_i2s7_sda_pin(&mut iocon),
         pins::Pio0_19::take().unwrap().into_i2s7_ws_pin(&mut iocon),
         pins::Pio1_31::take().unwrap(), // MCLK
     );
 
-    // iocon.disabled(&mut syscon).release(); // save the environment :)
-
     debug!("clocks");
+    // Run the system clock at 96MHz. The lpc55-hal will run it from the FRO.
     let clocks = hal::ClockRequirements::default()
         .system_frequency(96.MHz())
         .configure(&mut anactrl, &mut pmc, &mut syscon)
@@ -455,7 +259,8 @@ fn main() -> ! {
             .0
             .enabled(&mut syscon, clocks.support_1mhz_fro_token().unwrap()),
     );
-    init_audio_pll();
+    // Start PLL0 at 24.576MHz as the audio clock. The FRO cannot evenly divide any common audio frequencies.
+    hw::init_audio_pll();
 
     debug!("peripherals");
 
@@ -471,9 +276,10 @@ fn main() -> ! {
 
     let i2s_peripheral = {
         let fc7 = hal.flexcomm.7.release();
-        init_i2s(fc7.0, fc7.2, &mut syscon)
+        hw::init_i2s(fc7.0, fc7.2, &mut syscon)
     };
 
+    #[cfg(feature = "usbhs")]
     let usb_peripheral = hal.usbhs.enabled_as_device(
         &mut anactrl,
         &mut pmc,
@@ -481,15 +287,21 @@ fn main() -> ! {
         &mut _delay_timer,
         clocks.support_usbhs_token().unwrap(),
     );
+    #[cfg(feature = "usbfs")]
+    let usb_peripheral = hal.usbfs.enabled_as_device(
+        &mut anactrl,
+        &mut pmc,
+        &mut syscon,
+        clocks.support_usbfs_token().unwrap(),
+    );
 
     let usb_bus = UsbBus::new(usb_peripheral, usb0_vbus_pin);
-    let clock = Clock {};
 
     defmt::debug!("codec init");
     wm8904::init_codec(&mut i2c_bus);
     let queue = cortex_m::singleton!(
-        : heapless::spsc::Queue<SampleType, FIFO_LENGTH>
+        : Queue<SampleType, FIFO_LENGTH>
-        = heapless::spsc::Queue::new()
+        = Queue::new()
     )
     .unwrap();
     let (producer, consumer) = queue.split();
@@ -497,29 +309,15 @@ fn main() -> ! {
     let consumer_ref = FIFO_CONSUMER_STORE.init(consumer);
     unsafe { FIFO_CONSUMER = consumer_ref as *mut _ };
 
-    // i2s_sine_test(&i2s_peripheral.i2s);
+    let mut clock = Clock {};
-    let audio = Audio {
+    let mut audio = Audio {
         i2s: i2s_peripheral,
-        producer: UnsafeCell::new(producer),
+        producer,
         running: AtomicBool::new(false),
         integrator: AtomicI32::new(0),
     };
 
-    let config = AudioClassConfig::new(UsbSpeed::High, FunctionCode::Other, &clock, &audio)
+    let config = AudioClassConfig::new(UsbSpeed::High, FunctionCode::Other, &mut clock, &mut audio)
-        .with_input_config(TerminalConfig::new(
-            2,
-            1,
-            2,
-            FormatType1 {
-                bit_resolution: 32,
-                bytes_per_sample: 4,
-            },
-            TerminalType::ExtLineConnector,
-            ChannelConfig::default_chans(2),
-            IsochronousSynchronizationType::Asynchronous,
-            LockDelay::Undefined(0),
-            None,
-        ))
         .with_output_config(TerminalConfig::new(
             4,
             1,
@@ -540,8 +338,8 @@ fn main() -> ! {
     let mut usb_dev = UsbDeviceBuilder::new(&usb_bus, UsbVidPid(0x1209, 0xcc1d))
         .composite_with_iads()
         .strings(&[StringDescriptors::default()
-            .manufacturer("VE7XEN")
+            .manufacturer("Generic")
-            .product("Guac")
+            .product("usbd_uac2 device")
             .serial_number("123456789")])
         .unwrap()
         .max_packet_size_0(64)
@@ -555,7 +353,6 @@ fn main() -> ! {
 
     loop {
         usb_dev.poll(&mut [&mut uac2]);
-        // audio.poll();
         red_led.set_high().ok(); // Turn off
     }
 }