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

@@ -0,0 +1,226 @@
+//! 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
+    pub p: u8,    // 1-31
+    pub selp: u8, // 5 bits
+    pub seli: u8, // 6 bits
+}
+
+impl PllConstants {
+    pub(crate) const fn new(n: u8, m: u16, p: u8) -> Self {
+        assert!(n != 0, "1 <= N <= 255");
+        assert!(m != 0, "1 <= M <= 65535");
+        assert!(p != 0 && p <= 31, "1 <= P <= 31");
+
+        // Following ripped from lpc55-hal and made const
+        // UM 4.6.6.3.2
+        let selp = {
+            let v = (m >> 2) + 1;
+            if v < 31 { v } else { 31 }
+        } as u8;
+
+        let seli = {
+            let v = match m {
+                m if m >= 8000 => 1,
+                m if m >= 122 => 8000 / m,
+                _ => 2 * (m >> 2) + 3,
+            };
+
+            if v < 63 { v } else { 63 }
+        } as u8;
+        // let seli = min(2*(m >> 2) + 3, 63);
+        Self {
+            n,
+            m,
+            p,
+            selp,
+            seli,
+        }
+    }
+}
+impl defmt::Format for PllConstants {
+    fn format(&self, fmt: defmt::Formatter) {
+        let factor = f32::from(self.m) / (f32::from(self.n) * 2.0 * f32::from(self.p));
+
+        defmt::write!(
+            fmt,
+            "m: {} n: {} p: {} selp: {} seli: {} fout: fin * {}",
+            self.m,
+            self.n,
+            self.p,
+            self.selp,
+            self.seli,
+            factor
+        );
+    }
+}
+
+// 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,113 +45,20 @@ use usbd_uac2::{
     descriptors::{ChannelConfig, ClockType, FormatType1, LockDelay},
 };
 
+use crate::hw::I2sTx;
+
+mod hw;
 mod wm8904;
 
-struct PllConstants {
-    m: u16,   // 1-65535
-    n: u8,    // 1-255
-    p: u8,    // 1-31
-    selp: u8, // 5 bits
-    seli: u8, // 6 bits
-}
-
-impl PllConstants {
-    const fn new(n: u8, m: u16, p: u8) -> Self {
-        assert!(n != 0, "1 <= N <= 255");
-        assert!(m != 0, "1 <= M <= 65535");
-        assert!(p != 0 && p <= 31, "1 <= P <= 31");
-
-        // Following ripped from lpc55-hal and made const
-        // UM 4.6.6.3.2
-        let selp = {
-            let v = (m >> 2) + 1;
-            if v < 31 { v } else { 31 }
-        } as u8;
-
-        let seli = {
-            let v = match m {
-                m if m >= 8000 => 1,
-                m if m >= 122 => 8000 / m,
-                _ => 2 * (m >> 2) + 3,
-            };
-
-            if v < 63 { v } else { 63 }
-        } as u8;
-        // let seli = min(2*(m >> 2) + 3, 63);
-        Self {
-            n,
-            m,
-            p,
-            selp,
-            seli,
-        }
-    }
-}
-
-impl defmt::Format for PllConstants {
-    fn format(&self, fmt: defmt::Formatter) {
-        let factor = f32::from(self.m) / (f32::from(self.n) * 2.0 * f32::from(self.p));
-
-        defmt::write!(
-            fmt,
-            "m: {} n: {} p: {} selp: {} seli: {} fout: fin * {}",
-            self.m,
-            self.n,
-            self.p,
-            self.selp,
-            self.seli,
-            factor
-        );
-    }
-}
-
 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 MCLK_FREQ: u32 = 12288000;
-const AUDIO_PLL: PllConstants = PllConstants::new(125, 3072, 8);
+const SAMPLE_RATE: u32 = 48000; // example implementation runs okay at 48k but not 96k
-const FIFO_LENGTH: usize = 2048; // frames
-const MCLK_FREQ: u32 = 24576000;
-const SAMPLE_RATE: u32 = 48000;
 type SampleType = (i32, i32);
 
-// const SINE_LUT: [i32; 32] = [
-//     0, 1636536, 3210180, 4660460, 5931640, 6974871, 7750062, 8227422, 8388607, 8227422, 7750062,
-//     6974871, 5931640, 4660460, 3210180, 1636536, 0, -1636536, -3210180, -4660460, -5931640,
-//     -6974871, -7750062, -8227422, -8388607, -8227422, -7750062, -6974871, -5931640, -4660460,
-//     -3210180, -1636536,
-// ];
-
-// pub fn i2s_sine_test(i2s: &pac::I2S7) -> ! {
-//     let mut idx = 0;
-//     let mut count = 0usize;
-
-//     defmt::debug!("starting sine test");
-
-//     loop {
-//         if i2s.fifostat.read().txnotfull().bit_is_set() {
-//             let sample = SINE_LUT[idx] * 32;
-
-//             // ✅ Left channel
-//             i2s.fifowr.write(|w| unsafe { w.bits(sample as u32) });
-
-//             // wait for space if needed
-//             while !i2s.fifostat.read().txnotfull().bit_is_set() {}
-
-//             // ✅ Right channel
-//             i2s.fifowr.write(|w| unsafe { w.bits(sample as u32) });
-
-//             idx = (idx + 1) & (SINE_LUT.len() - 1);
-//             count += 1;
-//             if count.is_multiple_of(48000) {
-//                 defmt::debug!("frames sent: {}", count)
-//             }
-//         }
-//     }
-// }
-
 struct Clock {}
 impl Clock {
-    const RATES: [RangeEntry<u32>; 1] = [RangeEntry::new_fixed(48000)];
+    const RATES: [RangeEntry<u32>; 1] = [RangeEntry::new_fixed(SAMPLE_RATE)];
 }
 impl UsbAudioClockImpl for Clock {
     const CLOCK_TYPE: usbd_uac2::descriptors::ClockType = ClockType::InternalFixed;
@@ -159,29 +76,6 @@ impl UsbAudioClockImpl for Clock {
     }
 }
 
-#[derive(Default)]
-struct PerfCounters {
-    frames: AtomicUsize,
-    avg_fill: AtomicI32, // i32 to simplify averaging
-    queue_underflows: AtomicUsize,
-    queue_overflows: AtomicUsize,
-    audio_underflows: AtomicUsize,
-}
-
-impl defmt::Format for PerfCounters {
-    fn format(&self, fmt: defmt::Formatter) {
-        defmt::write!(
-            fmt,
-            "frames: {} avg fill: {} a_underflows: {} q_underflows: {} q_overflows: {}",
-            self.frames.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();
 
@@ -190,12 +84,14 @@ fn FLEXCOMM7() {
     let i2s = unsafe { &*pac::I2S7::ptr() };
 
     // 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 {
+            // Queue underflow
+            defmt::error!("queue underflow");
             i2s.fifowr.write(|w| unsafe { w.bits(0) });
             i2s.fifowr.write(|w| unsafe { w.bits(0) });
         }
@@ -205,310 +101,119 @@ fn FLEXCOMM7() {
 struct Audio<'a> {
     running: AtomicBool,
     i2s: I2sTx,
-    producer: UnsafeCell<heapless::spsc::Producer<'a, SampleType>>,
+    producer: Producer<'a, SampleType>,
-    perf: PerfCounters,
     integrator: AtomicI32,
 }
 impl<'a> Audio<'a> {
-    // fn poll(&self) {
-    //     if !self.running.load(Ordering::Relaxed) {
-    //         return;
-    //     }
-
-    //     if self.i2s.i2s.fifostat.read().txerr().bit_is_set() {
-    //         self.i2s.i2s.fifostat.modify(|_, w| w.txerr().set_bit());
-    //         // defmt::error!("fifo tx error, txlvl: {}", stat.txlvl().bits());
-    //     }
-    //     if self.i2s.i2s.fifostat.read().txlvl().bits() == 0 {
-    //         self.perf.audio_underflows.fetch_add(1, Ordering::Relaxed);
-    //     }
-    //     while self.i2s.i2s.fifostat.read().txlvl().bits() <= 6 {
-    //         // fifo is 8 deep at 32 bit samples
-    //         let fifo = self.consumer.get();
-    //         if let Some(sample) = unsafe { (*fifo).dequeue() } {
-    //             self.i2s
-    //                 .i2s
-    //                 .fifowr
-    //                 .write(|w| unsafe { w.bits(sample.0 as u32) });
-    //             self.i2s
-    //                 .i2s
-    //                 .fifowr
-    //                 .write(|w| unsafe { w.bits(sample.1 as u32) });
-    //         } else {
-    //             if self.running.load(Ordering::Relaxed) {
-    //                 self.perf.queue_underflows.fetch_add(1, Ordering::Relaxed);
-    //             }
-    //             break;
-    //             // self.i2s.i2s.fifowr.write(|w| unsafe { w.bits(0 as u32) });
-    //             // self.i2s.i2s.fifowr.write(|w| unsafe { w.bits(0 as u32) });
-    //         }
-    //         self.perf.frames.fetch_add(1, Ordering::Relaxed);
-    //     }
-    // }
     fn start(&self) {
         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: {}", self.perf);
+        defmt::info!("playback stopped");
         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) } {
+        {
-                self.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() as i32 };
+    /// Provide rate feedback to the host, so that it doesn't over- or underflow
-        let error = (queuelen - 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 = self.perf.avg_fill.load(Ordering::Relaxed);
+        let p = error << 7;
-        let new = ((ema * 1023) + queuelen + 512) >> 10;
+        let i = integrator << 3;
-        self.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(0).halt().run().reset().released()); // div by 1 = PLL0 fout
-        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(7) }); // Clock source is MCLK (24MHz on FRO96) / 8 = 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();
@@ -525,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()
@@ -535,20 +240,17 @@ 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");
-    // TODO: figure out how to configure the PLL for a more suitable audio clock.
+    // Run the system clock at 96MHz. The lpc55-hal will run it from the FRO.
     let clocks = hal::ClockRequirements::default()
-        // .system_frequency(24.mhz())
-        // .system_frequency(72.mhz())
         .system_frequency(96.MHz())
         .configure(&mut anactrl, &mut pmc, &mut syscon)
         .unwrap();
@@ -557,9 +259,8 @@ fn main() -> ! {
             .0
             .enabled(&mut syscon, clocks.support_1mhz_fro_token().unwrap()),
     );
-
+    // Start PLL0 at 24.576MHz as the audio clock. The FRO cannot evenly divide any common audio frequencies.
-    debug!("pll0");
+    hw::init_audio_pll();
-    init_audio_pll();
 
     debug!("peripherals");
 
@@ -575,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,
@@ -585,46 +287,37 @@ 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();
 
-    let consumer_ref = unsafe { FIFO_CONSUMER_STORE.init(consumer) };
+    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),
-        perf: PerfCounters::default(),
         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,
@@ -645,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)
@@ -660,7 +353,6 @@ fn main() -> ! {
 
     loop {
         usb_dev.poll(&mut [&mut uac2]);
-        // audio.poll();
         red_led.set_high().ok(); // Turn off
     }
 }

src/lib.rs

@@ -6,18 +6,16 @@ mod cursor;
 pub mod descriptors;
 mod log;
 
-use core::cell::OnceCell;
 use core::cmp::Ordering;
 use core::marker::PhantomData;
 use core::sync::atomic::AtomicUsize;
 
-use byteorder_embedded_io::{LittleEndian, WriteBytesExt};
+use byteorder_embedded_io::{LittleEndian, ReadBytesExt, WriteBytesExt};
 use constants::*;
 use descriptors::*;
 use log::*;
 
-use modular_bitfield::prelude::*;
+use num_traits::ConstZero;
-use num_traits::{ConstZero, Zero};
 use usb_device::control::{Recipient, Request, RequestType};
 use usb_device::device::DEFAULT_ALTERNATE_SETTING;
 use usb_device::endpoint::{self, Endpoint, EndpointDirection, In, Out};
@@ -163,7 +161,7 @@ impl UsbIsochronousFeedback {
 pub trait UsbAudioClass<'a, B: UsbBus> {
     /// Called when audio data is received from the host. `ep` is ready for
     /// `ep.read()`.
-    fn audio_data_rx(&self, ep: &Endpoint<'a, B, endpoint::Out>) {}
+    fn audio_data_rx(&mut self, ep: &Endpoint<'a, B, endpoint::Out>) {}
 
     /// Called when it's time to send an isochronous feedback update. Should
     /// return the correct feedback payload. Should not be considered a great
@@ -172,20 +170,14 @@ pub trait UsbAudioClass<'a, B: UsbBus> {
     ///
     /// Required for isochronous asynchronous mode to work properly. If None is
     /// returned, no IN packet will be emitted at feedback time.
-    fn feedback(&self) -> Option<UsbIsochronousFeedback> {
+    fn feedback(&mut self) -> Option<UsbIsochronousFeedback> {
         None
     }
 
     /// Called when the alternate setting of `terminal`'s interface is changed,
     /// before the `AudioStream` is updated. Currently not very useful since we
     /// don't implement alternate settings.
-    fn alternate_setting_changed<CS: UsbAudioClockImpl, AU: UsbAudioClass<'a, B>>(
+    fn alternate_setting_changed(&mut self, terminal: UsbDirection, alt_setting: u8) {}
-        &self,
-        ac: &mut AudioClass<'a, B, CS, AU>,
-        terminal: UsbDirection,
-        alt_setting: u8,
-    ) {
-    }
 }
 
 /// A trait for implementing Sampling Frequency Control for USB Audio Clock Sources
@@ -201,8 +193,8 @@ pub trait UsbAudioClockImpl {
     const SOF_SYNC: bool;
     /// Called when the host requests the current sample rate. Returns the sample rate in Hz.
     fn get_sample_rate(&self) -> core::result::Result<u32, UsbAudioClassError>;
-    /// Called when the host requests to set the sample rate. Should reconfigure the clock source
+    /// Called when the host requests to set the sample rate. Not necessarily called at all startups,
-    /// if necessary.
+    /// so alt_setting should start/stop the clock. Not required for 'fixed' clocks.
     fn set_sample_rate(
         &mut self,
         sample_rate: u32,
@@ -238,6 +230,13 @@ pub trait UsbAudioClockImpl {
     /// ref: USB Audio Class Specification 2.0 5.2.1 & 5.2.3.3
     fn get_rates(&self) -> core::result::Result<&[RangeEntry<u32>], UsbAudioClassError>;
 
+    /// Called when the audio device's AltSetting is changed. Usually 0 signals shutdown of the
+    /// streaming audio and 1 signals start of streaming. This should be used to start the clock
+    /// (and stop it if desired). If unimplemented, does nothing - keep the clock running at all times.
+    fn alt_setting(&mut self, alt_setting: u8) -> core::result::Result<(), UsbAudioClassError> {
+        Ok(())
+    }
+
     /// Build the ClockSource descriptor. It is not intended to override this method.
     ///
     /// Assumes access control based on clock type. Internal fixed/variable are read only,
@@ -320,7 +319,7 @@ impl<D: EndpointDirection> TerminalConfig<D> {
         self.format.bytes_per_sample as u32 * self.num_channels as u32
     }
 }
-impl<'a> TerminalConfigurationDescriptors for TerminalConfig<In> {
+impl<'a> TerminalConfigurationDescriptors for TerminalConfig<Out> {
     fn get_configuration_descriptors(&self) -> (InputTerminal, OutputTerminal) {
         let input_terminal = InputTerminal {
             id: self.base_id,
@@ -357,7 +356,7 @@ impl<'a> TerminalConfigurationDescriptors for TerminalConfig<In> {
     // fn get_interface_descriptor(&self, id: InterfaceIndex) )
 }
 
-impl<'a> TerminalConfigurationDescriptors for TerminalConfig<Out> {
+impl<'a> TerminalConfigurationDescriptors for TerminalConfig<In> {
     fn get_configuration_descriptors(&self) -> (InputTerminal, OutputTerminal) {
         let output_terminal = OutputTerminal {
             id: self.base_id,
@@ -422,10 +421,10 @@ pub enum UsbSpeed {
 pub struct AudioClassConfig<'a, B: UsbBus, CS: UsbAudioClockImpl, AU: UsbAudioClass<'a, B>> {
     pub speed: UsbSpeed,
     pub device_category: FunctionCode,
-    pub clock_impl: &'a CS,
+    pub clock_impl: &'a mut CS,
-    pub audio_impl: &'a AU,
+    pub audio_impl: &'a mut AU,
-    pub input_config: Option<TerminalConfig<Out>>,
+    pub input_config: Option<TerminalConfig<In>>,
-    pub output_config: Option<TerminalConfig<In>>,
+    pub output_config: Option<TerminalConfig<Out>>,
     pub additional_descriptors: Option<&'a [AudioClassDescriptor]>,
     _bus: PhantomData<B>,
 }
@@ -436,8 +435,8 @@ impl<'a, B: UsbBus, CS: UsbAudioClockImpl, AU: UsbAudioClass<'a, B>>
     pub fn new(
         speed: UsbSpeed,
         device_category: FunctionCode,
-        clock_impl: &'a CS,
+        clock_impl: &'a mut CS,
-        audio_impl: &'a AU,
+        audio_impl: &'a mut AU,
     ) -> Self {
         Self {
             speed,
@@ -450,11 +449,11 @@ impl<'a, B: UsbBus, CS: UsbAudioClockImpl, AU: UsbAudioClass<'a, B>>
             _bus: PhantomData,
         }
     }
-    pub fn with_input_config(mut self, input_config: TerminalConfig<Out>) -> Self {
+    pub fn with_input_config(mut self, input_config: TerminalConfig<In>) -> Self {
         self.input_config = Some(input_config);
         self
     }
-    pub fn with_output_config(mut self, output_config: TerminalConfig<In>) -> Self {
+    pub fn with_output_config(mut self, output_config: TerminalConfig<Out>) -> Self {
         self.output_config = Some(output_config);
         self
     }
@@ -516,10 +515,10 @@ impl<'a, B: UsbBus, CS: UsbAudioClockImpl, AU: UsbAudioClass<'a, B>>
                 interval,
             );
             let alt_setting = DEFAULT_ALTERNATE_SETTING;
-            ac.in_iface = interface.into();
+            ac.out_iface = interface.into();
-            ac.in_ep = endpoint.address().index();
+            ac.out_ep = endpoint.address().index();
             ac.fb_ep = feedback_ep.address().index();
-            ac.input = Some(AudioStream {
+            ac.output = Some(AudioStream {
                 config,
                 interface,
                 endpoint,
@@ -537,9 +536,10 @@ impl<'a, B: UsbBus, CS: UsbAudioClockImpl, AU: UsbAudioClass<'a, B>>
                 interval,
             );
             let alt_setting = DEFAULT_ALTERNATE_SETTING;
-            ac.out_iface = interface.into();
+            ac.in_iface = interface.into();
-            ac.out_ep = endpoint.address().index();
+            ac.in_ep = endpoint.address().index();
-            ac.output = Some(AudioStream {
+
+            ac.input = Some(AudioStream {
                 config,
                 interface,
                 endpoint,
@@ -651,8 +651,8 @@ impl<'a, B: UsbBus, D: EndpointDirection> AudioStream<'a, B, D> {
 
 pub struct AudioClass<'a, B: UsbBus, CS: UsbAudioClockImpl, AU: UsbAudioClass<'a, B>> {
     control_iface: InterfaceNumber,
-    clock_impl: &'a CS,
+    clock_impl: &'a mut CS,
-    audio_impl: &'a AU,
+    audio_impl: &'a mut AU,
     output: Option<AudioStream<'a, B, Out>>,
     input: Option<AudioStream<'a, B, In>>,
     feedback: Option<Endpoint<'a, B, In>>,
@@ -675,7 +675,7 @@ impl<'a, B: UsbBus, CS: UsbAudioClockImpl, AU: UsbAudioClass<'a, B>> UsbClass<B>
     ) -> usb_device::Result<()> {
         info!("  AudioClass::get_configuration_descriptors");
         // Control + 0-2 streaming
-        let n_interfaces = 1 + (self.input.is_some() as u8) + (self.input.is_some() as u8);
+        let n_interfaces = 1 + (self.input.is_some() as u8) + (self.output.is_some() as u8);
 
         debug!("writer.iad()");
         // UAC2 4.6 Interface Association Descriptor
@@ -835,12 +835,15 @@ impl<'a, B: UsbBus, CS: UsbAudioClockImpl, AU: UsbAudioClass<'a, B>> UsbClass<B>
     fn poll(&mut self) {
         debug!("poll");
         // no streaming in alt 0
-        if self.output.as_ref().unwrap().alt_setting != 1 {
+        if self.output.as_ref().is_none_or(|o| o.alt_setting != 0)
+            || self.input.as_ref().is_none_or(|i| i.alt_setting != 0)
+        {
             return;
         }
         loop {
+            if let Some(o) = self.output.as_ref() {
                 let mut buf = [0; 1024];
-            match self.output.as_ref().unwrap().endpoint.read(&mut buf) {
+                match o.endpoint.read(&mut buf) {
                     Ok(len) if len > 0 => {
                         debug!("EP OUT data {:?}", len);
                     }
@@ -855,6 +858,7 @@ impl<'a, B: UsbBus, CS: UsbAudioClockImpl, AU: UsbAudioClass<'a, B>> UsbClass<B>
                 }
             }
         }
+    }
 }
 
 impl<'a, B: UsbBus, CS: UsbAudioClockImpl, AU: UsbAudioClass<'a, B>> AudioClass<'a, B, CS, AU> {
@@ -917,16 +921,18 @@ impl<'a, B: UsbBus, CS: UsbAudioClockImpl, AU: UsbAudioClass<'a, B>> AudioClass<
         if self.input.is_some() && iface == self.in_iface {
             let old_alt = self.input.as_ref().unwrap().alt_setting;
             if old_alt != alt_setting {
+                self.clock_impl.alt_setting(alt_setting).ok();
                 self.audio_impl
-                    .alternate_setting_changed(self, UsbDirection::In, alt_setting);
+                    .alternate_setting_changed(UsbDirection::In, alt_setting);
                 self.input.as_mut().unwrap().alt_setting = alt_setting;
                 xfer.accept().ok();
             }
         } else if self.output.is_some() && iface == self.out_iface {
             let old_alt = self.output.as_ref().unwrap().alt_setting;
             if old_alt != alt_setting {
+                self.clock_impl.alt_setting(alt_setting).ok();
                 self.audio_impl
-                    .alternate_setting_changed(self, UsbDirection::Out, alt_setting);
+                    .alternate_setting_changed(UsbDirection::Out, alt_setting);
                 self.output.as_mut().unwrap().alt_setting = alt_setting;
                 xfer.accept().ok();
             }
@@ -1023,6 +1029,10 @@ impl<'a, B: UsbBus, CS: UsbAudioClockImpl, AU: UsbAudioClass<'a, B>> AudioClass<
         channel: u8,
         control: u8,
     ) {
+        match entity {
+            1 => return self.set_clock_cur(xfer, channel, control),
+            _ => {}
+        }
         debug!("   Unimplemented.");
     }
 
@@ -1131,7 +1141,8 @@ impl<'a, B: UsbBus, CS: UsbAudioClockImpl, AU: UsbAudioClass<'a, B>> AudioClass<
                     Ok(valid) => {
                         debug!("    {}", valid);
                         buf.write_u8(valid as u8)
-                            .map_err(|_e| UsbError::BufferOverflow)?;
+                            .map_err(|_e| UsbError::BufferOverflow)
+                            .ok();
                         Ok(1)
                     }
                     Err(_e) => Err(UsbError::InvalidState),
@@ -1143,6 +1154,32 @@ impl<'a, B: UsbBus, CS: UsbAudioClockImpl, AU: UsbAudioClass<'a, B>> AudioClass<
             }
         }
     }
+    fn set_clock_cur(&mut self, xfer: ControlOut<B>, channel: u8, control: u8) {
+        match control.try_into() {
+            Ok(ClockSourceControlSelector::SamFreqControl) => {
+                debug!("   SamplingFreqControl");
+                if channel != 0 {
+                    error!(
+                        "   Invalid channel {} for SamplingFreqControl GET CUR. Ignoring.",
+                        channel
+                    );
+                }
+                match xfer.data().read_u32::<LittleEndian>() {
+                    Ok(rate) => {
+                        debug!("   SET SamplingFreqControl CUR {}", rate);
+                        self.clock_impl.set_sample_rate(rate).ok();
+                        xfer.accept().ok();
+                    }
+                    Err(e) => {
+                        error!("   SET SamplingFreqControl CUR ERROR BAD DATA");
+                    }
+                }
+            }
+            _ => {
+                debug!("   Unimplemented.");
+            }
+        }
+    }
     fn get_clock_range(&mut self, xfer: ControlIn<B>, channel: u8, control: u8) {
         match control.try_into() {
             Ok(ClockSourceControlSelector::SamFreqControl) => {