refactor feedback Trait fn, fix max_packet_size calculation in descriptors

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

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

examples/lpc55s28-evk/Cargo.lock

@@ -2,6 +2,15 @@
 # It is not intended for manual editing.
 version = 4
 
+[[package]]
+name = "aho-corasick"
+version = "1.1.4"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "ddd31a130427c27518df266943a5308ed92d4b226cc639f5a8f1002816174301"
+dependencies = [
+ "memchr",
+]
+
 [[package]]
 name = "atomic-polyfill"
 version = "1.0.3"
@@ -26,6 +35,17 @@ dependencies = [
  "rustc_version 0.2.3",
 ]
 
+[[package]]
+name = "bbqueue"
+version = "0.7.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "68917624e17aad88607cb5a5936f6da9b607c48c711e4e9ed101e7189aed28c2"
+dependencies = [
+ "const-init",
+ "critical-section",
+ "maitake-sync",
+]
+
 [[package]]
 name = "bitfield"
 version = "0.13.2"
@@ -63,6 +83,22 @@ dependencies = [
  "embedded-io",
 ]
 
+[[package]]
+name = "cc"
+version = "1.2.61"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "d16d90359e986641506914ba71350897565610e87ce0ad9e6f28569db3dd5c6d"
+dependencies = [
+ "find-msvc-tools",
+ "shlex",
+]
+
+[[package]]
+name = "cfg-if"
+version = "1.0.4"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "9330f8b2ff13f34540b44e946ef35111825727b38d33286ef986142615121801"
+
 [[package]]
 name = "cipher"
 version = "0.4.4"
@@ -73,6 +109,22 @@ dependencies = [
  "inout",
 ]
 
+[[package]]
+name = "const-init"
+version = "1.0.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "4bd422bfb4f24a97243f60b6a4443e63d810c925d8da4bb2d8fde26a7c1d57ec"
+
+[[package]]
+name = "cordyceps"
+version = "0.3.4"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "688d7fbb8092b8de775ef2536f36c8c31f2bc4006ece2e8d8ad2d17d00ce0a2a"
+dependencies = [
+ "loom",
+ "tracing",
+]
+
 [[package]]
 name = "cortex-m"
 version = "0.7.7"
@@ -214,6 +266,27 @@ dependencies = [
  "num",
 ]
 
+[[package]]
+name = "find-msvc-tools"
+version = "0.1.9"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "5baebc0774151f905a1a2cc41989300b1e6fbb29aff0ceffa1064fdd3088d582"
+
+[[package]]
+name = "generator"
+version = "0.8.8"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "52f04ae4152da20c76fe800fa48659201d5cf627c5149ca0b707b69d7eef6cf9"
+dependencies = [
+ "cc",
+ "cfg-if",
+ "libc",
+ "log",
+ "rustversion",
+ "windows-link",
+ "windows-result",
+]
+
 [[package]]
 name = "generic-array"
 version = "0.14.7"
@@ -275,16 +348,6 @@ dependencies = [
  "stable_deref_trait",
 ]
 
-[[package]]
-name = "heapless"
-version = "0.9.3"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "25ba4bd83f9415b58b4ed8dc5714c76e626a105be4646c02630ad730ad3b5aa4"
-dependencies = [
- "hash32 0.3.1",
- "stable_deref_trait",
-]
-
 [[package]]
 name = "inout"
 version = "0.1.4"
@@ -294,6 +357,18 @@ dependencies = [
  "generic-array 0.14.7",
 ]
 
+[[package]]
+name = "lazy_static"
+version = "1.5.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "bbd2bcb4c963f2ddae06a2efc7e9f3591312473c50c6685e1f298068316e66fe"
+
+[[package]]
+name = "libc"
+version = "0.2.186"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "68ab91017fe16c622486840e4c83c9a37afeff978bd239b5293d61ece587de66"
+
 [[package]]
 name = "lock_api"
 version = "0.4.14"
@@ -320,6 +395,19 @@ dependencies = [
  "log",
 ]
 
+[[package]]
+name = "loom"
+version = "0.7.2"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "419e0dc8046cb947daa77eb95ae174acfbddb7673b4151f56d1eed8e93fbfaca"
+dependencies = [
+ "cfg-if",
+ "generator",
+ "scoped-tls",
+ "tracing",
+ "tracing-subscriber",
+]
+
 [[package]]
 name = "lpc55-hal"
 version = "0.5.0"
@@ -354,13 +442,13 @@ dependencies = [
 name = "lpc55s28-evk"
 version = "0.1.0"
 dependencies = [
+ "bbqueue",
  "cortex-m",
  "cortex-m-rt",
  "defmt 1.0.1",
  "defmt-rtt",
  "embedded-hal 1.0.0",
  "embedded-io",
- "heapless 0.9.3",
  "log-to-defmt",
  "lpc55-hal",
  "nb 1.1.0",
@@ -371,6 +459,37 @@ dependencies = [
  "usbd-uac2",
 ]
 
+[[package]]
+name = "maitake-sync"
+version = "0.3.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "3d77c365d697828821727b9bc09e6bc3c518b8c63804e79e1be5a5ae091a7c5f"
+dependencies = [
+ "cordyceps",
+ "critical-section",
+ "loom",
+ "mutex-traits",
+ "mycelium-bitfield",
+ "pin-project",
+ "portable-atomic",
+ "tracing",
+]
+
+[[package]]
+name = "matchers"
+version = "0.2.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "d1525a2a28c7f4fa0fc98bb91ae755d1e2d1505079e05539e35bc876b5d65ae9"
+dependencies = [
+ "regex-automata",
+]
+
+[[package]]
+name = "memchr"
+version = "2.8.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "f8ca58f447f06ed17d5fc4043ce1b10dd205e060fb3ce5b979b8ed8e59ff3f79"
+
 [[package]]
 name = "modular-bitfield"
 version = "0.13.1"
@@ -392,6 +511,18 @@ dependencies = [
  "syn",
 ]
 
+[[package]]
+name = "mutex-traits"
+version = "1.0.1"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "3929f2b5633d29cf7b6624992e5f3c1e9334f1193423e12d17be4faf678cde3f"
+
+[[package]]
+name = "mycelium-bitfield"
+version = "0.1.5"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "24e0cc5e2c585acbd15c5ce911dff71e1f4d5313f43345873311c4f5efd741cc"
+
 [[package]]
 name = "nb"
 version = "0.1.3"
@@ -407,6 +538,15 @@ version = "1.1.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "8d5439c4ad607c3c23abf66de8c8bf57ba8adcd1f129e699851a6e43935d339d"
 
+[[package]]
+name = "nu-ansi-term"
+version = "0.50.3"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "7957b9740744892f114936ab4a57b3f487491bbeafaf8083688b16841a4240e5"
+dependencies = [
+ "windows-sys",
+]
+
 [[package]]
 name = "num"
 version = "0.3.1"
@@ -469,6 +609,12 @@ dependencies = [
  "autocfg",
 ]
 
+[[package]]
+name = "once_cell"
+version = "1.21.4"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "9f7c3e4beb33f85d45ae3e3a1792185706c8e16d043238c593331cc7cd313b50"
+
 [[package]]
 name = "panic-halt"
 version = "1.0.0"
@@ -485,6 +631,32 @@ dependencies = [
  "defmt 1.0.1",
 ]
 
+[[package]]
+name = "pin-project"
+version = "1.1.12"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "cbf0d9e68100b3a7989b4901972f265cd542e560a3a8a724e1e20322f4d06ce9"
+dependencies = [
+ "pin-project-internal",
+]
+
+[[package]]
+name = "pin-project-internal"
+version = "1.1.12"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "a990e22f43e84855daf260dded30524ef4a9021cc7541c26540500a50b624389"
+dependencies = [
+ "proc-macro2",
+ "quote",
+ "syn",
+]
+
+[[package]]
+name = "pin-project-lite"
+version = "0.2.17"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "a89322df9ebe1c1578d689c92318e070967d1042b512afbe49518723f4e6d5cd"
+
 [[package]]
 name = "portable-atomic"
 version = "1.13.1"
@@ -537,6 +709,23 @@ version = "0.6.4"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "ec0be4795e2f6a28069bec0b5ff3e2ac9bafc99e6a9a7dc3547996c5c816922c"
 
+[[package]]
+name = "regex-automata"
+version = "0.4.14"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "6e1dd4122fc1595e8162618945476892eefca7b88c52820e74af6262213cae8f"
+dependencies = [
+ "aho-corasick",
+ "memchr",
+ "regex-syntax",
+]
+
+[[package]]
+name = "regex-syntax"
+version = "0.8.10"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "dc897dd8d9e8bd1ed8cdad82b5966c3e0ecae09fb1907d58efaa013543185d0a"
+
 [[package]]
 name = "rustc_version"
 version = "0.2.3"
@@ -561,6 +750,12 @@ version = "1.0.22"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "b39cdef0fa800fc44525c84ccb54a029961a8215f9619753635a9c0d2538d46d"
 
+[[package]]
+name = "scoped-tls"
+version = "1.0.1"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "e1cf6437eb19a8f4a6cc0f7dca544973b0b78843adbfeb3683d1a94a0024a294"
+
 [[package]]
 name = "scopeguard"
 version = "1.2.0"
@@ -588,6 +783,27 @@ version = "0.7.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "388a1df253eca08550bef6c72392cfe7c30914bf41df5269b68cbd6ff8f570a3"
 
+[[package]]
+name = "sharded-slab"
+version = "0.1.7"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "f40ca3c46823713e0d4209592e8d6e826aa57e928f09752619fc696c499637f6"
+dependencies = [
+ "lazy_static",
+]
+
+[[package]]
+name = "shlex"
+version = "1.3.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "0fda2ff0d084019ba4d7c6f371c95d8fd75ce3524c3cb8fb653a3023f6323e64"
+
+[[package]]
+name = "smallvec"
+version = "1.15.1"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "67b1b7a3b5fe4f1376887184045fcf45c69e92af734b7aaddc05fb777b6fbd03"
+
 [[package]]
 name = "spin"
 version = "0.9.8"
@@ -649,6 +865,76 @@ dependencies = [
  "syn",
 ]
 
+[[package]]
+name = "thread_local"
+version = "1.1.9"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "f60246a4944f24f6e018aa17cdeffb7818b76356965d03b07d6a9886e8962185"
+dependencies = [
+ "cfg-if",
+]
+
+[[package]]
+name = "tracing"
+version = "0.1.44"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "63e71662fa4b2a2c3a26f570f037eb95bb1f85397f3cd8076caed2f026a6d100"
+dependencies = [
+ "pin-project-lite",
+ "tracing-attributes",
+ "tracing-core",
+]
+
+[[package]]
+name = "tracing-attributes"
+version = "0.1.31"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "7490cfa5ec963746568740651ac6781f701c9c5ea257c58e057f3ba8cf69e8da"
+dependencies = [
+ "proc-macro2",
+ "quote",
+ "syn",
+]
+
+[[package]]
+name = "tracing-core"
+version = "0.1.36"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "db97caf9d906fbde555dd62fa95ddba9eecfd14cb388e4f491a66d74cd5fb79a"
+dependencies = [
+ "once_cell",
+ "valuable",
+]
+
+[[package]]
+name = "tracing-log"
+version = "0.2.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "ee855f1f400bd0e5c02d150ae5de3840039a3f54b025156404e34c23c03f47c3"
+dependencies = [
+ "log",
+ "once_cell",
+ "tracing-core",
+]
+
+[[package]]
+name = "tracing-subscriber"
+version = "0.3.23"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "cb7f578e5945fb242538965c2d0b04418d38ec25c79d160cd279bf0731c8d319"
+dependencies = [
+ "matchers",
+ "nu-ansi-term",
+ "once_cell",
+ "regex-automata",
+ "sharded-slab",
+ "smallvec",
+ "thread_local",
+ "tracing",
+ "tracing-core",
+ "tracing-log",
+]
+
 [[package]]
 name = "typenum"
 version = "1.20.0"
@@ -684,6 +970,12 @@ dependencies = [
  "usb-device",
 ]
 
+[[package]]
+name = "valuable"
+version = "0.1.1"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "ba73ea9cf16a25df0c8caa16c51acb937d5712a8429db78a3ee29d5dcacd3a65"
+
 [[package]]
 name = "vcell"
 version = "0.1.3"
@@ -710,3 +1002,27 @@ checksum = "de437e2a6208b014ab52972a27e59b33fa2920d3e00fe05026167a1c509d19cc"
 dependencies = [
  "vcell",
 ]
+
+[[package]]
+name = "windows-link"
+version = "0.2.1"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "f0805222e57f7521d6a62e36fa9163bc891acd422f971defe97d64e70d0a4fe5"
+
+[[package]]
+name = "windows-result"
+version = "0.4.1"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "7781fa89eaf60850ac3d2da7af8e5242a5ea78d1a11c49bf2910bb5a73853eb5"
+dependencies = [
+ "windows-link",
+]
+
+[[package]]
+name = "windows-sys"
+version = "0.61.2"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "ae137229bcbd6cdf0f7b80a31df61766145077ddf49416a728b02cb3921ff3fc"
+dependencies = [
+ "windows-link",
+]

examples/lpc55s28-evk/Cargo.toml

@@ -9,13 +9,13 @@ usbfs = []
 usbhs = []
 
 [dependencies]
+bbqueue = "0.7.0"
 cortex-m = { version = "0.7.7", features = ["critical-section-single-core"] }
 cortex-m-rt = "0.7.5"
 defmt = "1.0.1"
 defmt-rtt = "1.1.0"
 embedded-hal = "1.0.0"
 embedded-io = "0.7.1"
-heapless = "0.9.3"
 log-to-defmt = "0.1.0"
 lpc55-hal = { version = "0.5.0", path = "../lpc55-hal" }
 nb = "1.1.0"

examples/lpc55s28-evk/src/hw.rs

@@ -1,8 +1,22 @@
 //! Contains hardware setup unrelated to Usb Audio Class implementation
 
+use crate::hal;
+use core::cell::{OnceCell, UnsafeCell};
+use core::mem::MaybeUninit;
+use core::ptr::null_mut;
+
 use crate::Syscon;
 use crate::{MCLK_FREQ, SAMPLE_RATE, pac};
 use defmt::debug;
+use hal::{
+    Iocon, Pin,
+    drivers::pins,
+    prelude::*,
+    traits::wg::digital::v2::{OutputPin, ToggleableOutputPin},
+    typestates::pin::{gpio::direction::Output, state::Gpio},
+};
+use lpc55_hal::Enabled;
+use static_cell::StaticCell;
 pub(crate) struct PllConstants {
     pub m: u16,   // 1-65535
     pub n: u8,    // 1-255
@@ -79,7 +93,7 @@ pub(crate) fn init_audio_pll() {
         .xo32m_ctrl
         .modify(|_, w| w.enable_system_clk_out().enable());
 
-    debug!("init pll: {}", AUDIO_PLL);
+    debug!("init pll0: {}", AUDIO_PLL);
     pmc.pdruncfg0
         .modify(|_, w| w.pden_pll0().poweredoff().pden_pll0_sscg().poweredoff());
     syscon.pll0clksel.write(|w| w.sel().enum_0x1()); // clk_in
@@ -132,6 +146,61 @@ pub(crate) fn init_audio_pll() {
     debug!("pll0 locked after {} tries", i);
 }
 
+const SYS_PLL: PllConstants = PllConstants::new(4, 75, 1); // 150MHz
+
+pub(crate) fn init_sys_pll1() {
+    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 pll1: {}", SYS_PLL);
+    pmc.pdruncfg0.modify(|_, w| w.pden_pll1().poweredoff());
+    syscon.pll1clksel.write(|w| w.sel().enum_0x1()); // clk_in
+    syscon.pll1ctrl.write(|w| unsafe {
+        w.clken()
+            .enable()
+            .seli()
+            .bits(SYS_PLL.seli)
+            .selp()
+            .bits(SYS_PLL.selp)
+    });
+
+    syscon
+        .pll1ndec
+        .write(|w| unsafe { w.ndiv().bits(SYS_PLL.n) });
+    syscon.pll1ndec.write(|w| unsafe {
+        w.ndiv().bits(SYS_PLL.n).nreq().set_bit() // latch
+    });
+    syscon
+        .pll1mdec
+        .write(|w| unsafe { w.mdiv().bits(SYS_PLL.m) });
+    syscon
+        .pll1pdec
+        .write(|w| unsafe { w.pdiv().bits(SYS_PLL.p) });
+    syscon.pll1pdec.write(|w| unsafe {
+        w.pdiv().bits(SYS_PLL.p).preq().set_bit() // latch
+    });
+
+    pmc.pdruncfg0.modify(|_, w| w.pden_pll1().poweredon());
+    debug!("pll1 wait for lock");
+    let mut i = 0usize;
+    while syscon.pll1stat.read().lock().bit_is_clear() {
+        i += 1;
+    }
+    debug!("pll1 locked after {} tries", i);
+    // switch system clock to pll1
+    syscon.fmccr.modify(|_, w| w.flashtim().flashtim11());
+    syscon.mainclkselb.modify(|_, w| w.sel().enum_0x2()); // pll1
+}
+
 pub struct I2sTx {
     pub i2s: pac::I2S7,
 }
@@ -224,3 +293,74 @@ pub fn init_i2s(mut fc7: pac::FLEXCOMM7, i2s7: pac::I2S7, syscon: &mut Syscon) -
 
     I2sTx { i2s: regs }
 }
+
+pub struct SharedLed<T: OutputPin> {
+    inner: UnsafeCell<T>,
+}
+unsafe impl<T: OutputPin> Sync for SharedLed<T> {}
+impl<T: OutputPin> SharedLed<T> {
+    pub fn new(inner: T) -> Self {
+        Self {
+            inner: UnsafeCell::new(inner),
+        }
+    }
+    pub fn on(&self) {
+        unsafe {
+            (*self.inner.get()).set_low().ok();
+        }
+    }
+    pub fn off(&self) {
+        unsafe {
+            (*self.inner.get()).set_high().ok();
+        }
+    }
+}
+impl<T: OutputPin + ToggleableOutputPin> SharedLed<T> {
+    pub fn toggle(&self) {
+        unsafe {
+            (*self.inner.get()).toggle().ok();
+        }
+    }
+}
+
+type RedLed = Pin<pins::Pio1_6, Gpio<Output>>;
+type GreenLed = Pin<pins::Pio1_7, Gpio<Output>>;
+type BlueLed = Pin<pins::Pio1_4, Gpio<Output>>;
+pub static RED_LED: MaybeUninit<SharedLed<RedLed>> = MaybeUninit::uninit();
+pub static GREEN_LED: MaybeUninit<SharedLed<GreenLed>> = MaybeUninit::uninit();
+pub static BLUE_LED: MaybeUninit<SharedLed<BlueLed>> = MaybeUninit::uninit();
+
+pub fn init_leds(iocon: &mut Iocon<Enabled>, gpio: &mut hal::Gpio<Enabled>) {
+    let red_led = SharedLed::new(
+        pins::Pio1_6::take()
+            .unwrap()
+            .into_gpio_pin(iocon, gpio)
+            .into_output_low(),
+    );
+    let green_led = SharedLed::new(
+        pins::Pio1_7::take()
+            .unwrap()
+            .into_gpio_pin(iocon, gpio)
+            .into_output_low(),
+    );
+    let blue_led = SharedLed::new(
+        pins::Pio1_4::take()
+            .unwrap()
+            .into_gpio_pin(iocon, gpio)
+            .into_output_low(),
+    );
+    unsafe {
+        core::ptr::write(RED_LED.as_ptr() as *mut SharedLed<RedLed>, red_led);
+        core::ptr::write(GREEN_LED.as_ptr() as *mut SharedLed<GreenLed>, green_led);
+        core::ptr::write(BLUE_LED.as_ptr() as *mut SharedLed<BlueLed>, blue_led);
+    }
+}
+pub fn red_led() -> &'static SharedLed<RedLed> {
+    unsafe { &*RED_LED.as_ptr() }
+}
+pub fn green_led() -> &'static SharedLed<GreenLed> {
+    unsafe { &*GREEN_LED.as_ptr() }
+}
+pub fn blue_led() -> &'static SharedLed<BlueLed> {
+    unsafe { &*BLUE_LED.as_ptr() }
+}

examples/lpc55s28-evk/src/main.rs

@@ -17,8 +17,12 @@ fn panic() -> ! {
     panic_probe::hard_fault()
 }
 
-use core::ptr::null_mut;
+use bbqueue::{
-use core::sync::atomic::{AtomicBool, AtomicI32, Ordering};
+    nicknames::Churrasco,
+    prod_cons::stream::{StreamConsumer, StreamProducer},
+    traits::bbqhdl::BbqHandle,
+};
+use core::sync::atomic::{AtomicBool, AtomicI32, AtomicU32, Ordering};
 use cortex_m_rt::entry;
 use defmt::debug;
 use defmt_rtt as _;
@@ -29,10 +33,8 @@ use hal::{
     prelude::*,
     time::Hertz,
 };
-use heapless::spsc::{Consumer, Producer, Queue};
+use lpc55_hal as hal;
-use lpc55_hal::{self as hal};
 use pac::interrupt;
-use static_cell::StaticCell;
 use usb_device::{
     bus::{self},
     device::{StringDescriptors, UsbDeviceBuilder, UsbVidPid},
@@ -45,7 +47,7 @@ use usbd_uac2::{
     descriptors::{ChannelConfig, ClockType, FormatType1, LockDelay},
 };
 
-use crate::hw::I2sTx;
+use crate::hw::{I2sTx, blue_led, green_led, red_led};
 
 mod hw;
 mod wm8904;
@@ -53,7 +55,7 @@ mod wm8904;
 const CODEC_I2C_ADDR: u8 = 0b0011010;
 const FIFO_LENGTH: usize = 256; // frames
 const MCLK_FREQ: u32 = 12288000;
-const SAMPLE_RATE: u32 = 48000; // example implementation runs okay at 48k but not 96k
+const SAMPLE_RATE: u32 = 96000;
 type SampleType = (i32, i32);
 
 struct Clock {}
@@ -63,8 +65,8 @@ impl Clock {
 impl UsbAudioClockImpl for Clock {
     const CLOCK_TYPE: usbd_uac2::descriptors::ClockType = ClockType::InternalFixed;
     const SOF_SYNC: bool = false;
-    fn get_sample_rate(&self) -> core::result::Result<u32, usbd_uac2::UsbAudioClassError> {
+    fn get_sample_rate(&self) -> u32 {
-        Ok(Clock::RATES[0].min)
+        Clock::RATES[0].min
     }
     fn get_rates(
         &self,
@@ -76,35 +78,63 @@ impl UsbAudioClockImpl for Clock {
     }
 }
 
-static FIFO_CONSUMER_STORE: StaticCell<Consumer<SampleType>> = StaticCell::new();
+const BYTES_PER_FRAME: usize = 8;
-static mut FIFO_CONSUMER: *mut Consumer<SampleType> = null_mut();
+const QUEUE_BYTES: usize = FIFO_LENGTH * BYTES_PER_FRAME;
+// We use bbqueue here for performance in the USB driver that runs almost entirely in interrupt free critical section.
+static QUEUE: Churrasco<QUEUE_BYTES> = Churrasco::new();
+// Used for feedback calculation of current fifo state
+static PRODUCED: AtomicU32 = AtomicU32::new(0);
+static CONSUMED: AtomicU32 = AtomicU32::new(0);
+
+#[inline]
+fn try_write_one_frame<T: BbqHandle>(
+    cons: &mut StreamConsumer<T>,
+    i2s: &pac::i2s7::RegisterBlock,
+) -> bool {
+    if let Ok(rgr) = cons.read() {
+        if rgr.len() >= BYTES_PER_FRAME {
+            let l = u32::from_le_bytes(rgr[0..4].try_into().unwrap());
+            let r = u32::from_le_bytes(rgr[4..8].try_into().unwrap());
+
+            i2s.fifowr.write(|w| unsafe { w.bits(l) });
+            i2s.fifowr.write(|w| unsafe { w.bits(r) });
+
+            // consume exactly one frame (8 bytes)
+            rgr.release(BYTES_PER_FRAME);
+            CONSUMED.fetch_add(BYTES_PER_FRAME as u32, Ordering::Relaxed);
+            return true;
+        } else {
+            // Not enough bytes for a full frame: leave it in the queue.
+            return false;
+        }
+    }
+    false
+}
 
 #[interrupt]
 fn FLEXCOMM7() {
     let i2s = unsafe { &*pac::I2S7::ptr() };
-
+    // refill until fifo has >6 words
-    // refil the buffer to 4 frames / 8 samples
+    let mut cons = QUEUE.stream_consumer();
-    let fifo = unsafe { &mut *FIFO_CONSUMER };
     while i2s.fifostat.read().txlvl().bits() <= 6 {
-        if let Some((l, r)) = fifo.dequeue() {
+        if !try_write_one_frame(&mut cons, i2s) {
-            i2s.fifowr.write(|w| unsafe { w.bits(l as u32) });
+            // No complete frame available: write silence to keep FIFO above threshold
-            i2s.fifowr.write(|w| unsafe { w.bits(r as u32) });
+            defmt::error!("underflow");
-        } else {
+            red_led().toggle();
-            // Queue underflow
-            defmt::error!("queue underflow");
             i2s.fifowr.write(|w| unsafe { w.bits(0) });
             i2s.fifowr.write(|w| unsafe { w.bits(0) });
+            break;
         }
     }
 }
 
-struct Audio<'a> {
+struct Audio<T: BbqHandle> {
     running: AtomicBool,
     i2s: I2sTx,
-    producer: Producer<'a, SampleType>,
+    producer: StreamProducer<T>,
     integrator: AtomicI32,
 }
-impl<'a> Audio<'a> {
+impl<T: BbqHandle> Audio<T> {
     fn start(&self) {
         self.running.store(true, Ordering::Relaxed);
         defmt::info!("playback starting, enabling interrupts");
@@ -117,6 +147,7 @@ impl<'a> Audio<'a> {
         // FIFO level interrupt enable
         self.i2s.i2s.fifointenset.modify(|_, w| w.txlvl().enabled());
         unsafe { pac::NVIC::unmask(pac::Interrupt::FLEXCOMM7) };
+        green_led().on();
     }
     fn stop(&self) {
         // If we don't disable interrupts while stopped, we will underflow constantly and continuously refill the fifo with 0s
@@ -125,9 +156,10 @@ impl<'a> Audio<'a> {
         self.running.store(true, Ordering::Relaxed);
         defmt::info!("playback stopped");
         pac::NVIC::mask(pac::Interrupt::FLEXCOMM7);
+        green_led().off();
     }
 }
-impl<'a, B: bus::UsbBus> UsbAudioClass<'a, B> for Audio<'_> {
+impl<T: BbqHandle, B: bus::UsbBus> UsbAudioClass<'_, B> for Audio<T> {
     fn alternate_setting_changed(&mut self, _terminal: usb_device::UsbDirection, alt_setting: u8) {
         // alt setting 0 means stopped
         match alt_setting {
@@ -138,10 +170,10 @@ impl<'a, B: bus::UsbBus> UsbAudioClass<'a, B> for Audio<'_> {
     }
     fn audio_data_rx(
         &mut self,
-        ep: &usb_device::endpoint::Endpoint<'a, B, usb_device::endpoint::Out>,
+        ep: &usb_device::endpoint::Endpoint<'_, 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.
+        // Buffer must fit 125us 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 mut buf = [0; SAMPLE_RATE as usize / 8000 * core::mem::size_of::<SampleType>()];
         let len = match ep.read(&mut buf) {
             Ok(len) => len,
             Err(_) => {
@@ -150,64 +182,55 @@ impl<'a, B: bus::UsbBus> UsbAudioClass<'a, B> for Audio<'_> {
             }
         };
         let buf = &buf[..len];
-        // Translate the raw USB data into audio frames
+
-        for sample in buf
+        if let Ok(mut wg) = self.producer.grant_exact(buf.len()) {
-            .chunks_exact(core::mem::size_of::<SampleType>())
+            wg.copy_from_slice(buf);
-            .map(|b| {
+            wg.commit(buf.len());
-                // TODO: implement SampleType::from
+            PRODUCED.fetch_add(buf.len() as u32, Ordering::Relaxed);
-                (
+        } else {
-                    i32::from_le_bytes(b[..4].try_into().unwrap()),
+            blue_led().on();
-                    i32::from_le_bytes(b[4..].try_into().unwrap()),
+            defmt::error!("overflowed bbq, asked {}", buf.len());
-                )
-            })
-        {
-            if self.producer.enqueue(sample).is_err() {
-                defmt::error!("overflowed fifo, len: {}", self.producer.len());
-            }
         }
     }
 
     /// Provide rate feedback to the host, so that it doesn't over- or underflow
     /// our queue.
-    fn feedback(&mut self) -> Option<UsbIsochronousFeedback> {
+    fn feedback(&mut self, nominal_rate: UsbIsochronousFeedback) -> Option<UsbIsochronousFeedback> {
-        // Samples per USB interval (1ms)
+        let target = FIFO_LENGTH as i32 / 2 - nominal_rate.int as i32;
-        const FRAME_SAMPLES: i32 = SAMPLE_RATE as i32 / 1000;
 
-        // Keep FIFO around half full, minus one USB packet worth
+        let fill = PRODUCED
-        const TARGET: i32 = FIFO_LENGTH as i32 / 2 - FRAME_SAMPLES;
+            .load(Ordering::Acquire)
+            .wrapping_sub(CONSUMED.load(Ordering::Acquire)) as i32
+            / BYTES_PER_FRAME as i32;
 
-        // 16.16 fixed-point nominal feedback value
+        let error = fill - target;
-        const NOMINAL: i32 = FRAME_SAMPLES << 16;
-        const MAX_ERROR: i32 = FRAME_SAMPLES / 2;
 
-        let queuelen = self.producer.len() as i32;
+        // Clamp startup excursions.
+        let error = error.clamp(-(nominal_rate.int as i32 * 4), nominal_rate.int as i32 * 4);
 
-        let error = (queuelen - TARGET).clamp(-MAX_ERROR, MAX_ERROR);
+        let mut integrator = self.integrator.load(Ordering::Relaxed);
+        integrator += error / 32;
+        integrator = integrator.clamp(-1024, 1024);
+        self.integrator.store(integrator, Ordering::Relaxed);
 
-        // slow down accumulation of I
+        // gains
-        const I_ACCUM_DIV: i32 = 8;
+        let p = error << 8;
-        let i_delta = error / I_ACCUM_DIV;
+        let i = integrator;
-        let integrator = self.integrator.fetch_add(i_delta, Ordering::Relaxed) + i_delta;
 
-        // Integrator saturates at 1024xFRAME_SAMPLES
-        let i_limit = FRAME_SAMPLES << 10;
-        let integrator = integrator.clamp(-i_limit, i_limit);
-
-        // Gains
-        let p = error << 7;
-        let i = integrator << 3;
-
-        // Total correction in 16.16 space
         let correction = -(p + i);
+        let nominal_v = nominal_rate.to_u32_12_13() as i32;
 
-        let v = NOMINAL + correction;
+        let mut v = nominal_v + correction;
+
+        // Tight clamp around nominal.
+        v = v.clamp(nominal_v - (1 << 14), nominal_v + (1 << 14));
 
         defmt::debug!(
-            "q:{} err:{} i:{} fb:{}",
+            "fill:{} err:{} int:{} fb:{=u32:x}",
-            queuelen,
+            fill,
             error,
             integrator,
-            v >> 16
+            v
         );
 
         Some(UsbIsochronousFeedback::new(v as u32))
@@ -227,10 +250,7 @@ fn main() -> ! {
 
     debug!("start");
 
-    let mut red_led = pins::Pio1_6::take()
+    hw::init_leds(&mut iocon, &mut gpio);
-        .unwrap()
-        .into_gpio_pin(&mut iocon, &mut gpio)
-        .into_output_low(); // start turned off
 
     debug!("iocon");
     let usb0_vbus_pin = pins::Pio0_22::take()
@@ -249,7 +269,7 @@ fn main() -> ! {
     );
 
     debug!("clocks");
-    // Run the system clock at 96MHz. The lpc55-hal will run it from the FRO.
+    // Run the system clock at 96MHz. The lpc55-hal will run it from the FRO. But we won't actually use these clocks, we just need the guards...
     let clocks = hal::ClockRequirements::default()
         .system_frequency(96.MHz())
         .configure(&mut anactrl, &mut pmc, &mut syscon)
@@ -259,6 +279,8 @@ fn main() -> ! {
             .0
             .enabled(&mut syscon, clocks.support_1mhz_fro_token().unwrap()),
     );
+    // Start PLL1 at 150MHz as main system clock
+    hw::init_sys_pll1();
     // Start PLL0 at 24.576MHz as the audio clock. The FRO cannot evenly divide any common audio frequencies.
     hw::init_audio_pll();
 
@@ -280,44 +302,44 @@ fn main() -> ! {
     };
 
     #[cfg(feature = "usbhs")]
-    let usb_peripheral = hal.usbhs.enabled_as_device(
+    let (usb_speed, usb_peripheral) = (
+        UsbSpeed::High,
+        hal.usbhs.enabled_as_device(
             &mut anactrl,
             &mut pmc,
             &mut syscon,
             &mut _delay_timer,
             clocks.support_usbhs_token().unwrap(),
+        ),
     );
     #[cfg(feature = "usbfs")]
-    let usb_peripheral = hal.usbfs.enabled_as_device(
+    let (usb_speed, usb_peripheral) = (
+        UsbSpeed::Full,
+        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);
 
     defmt::debug!("codec init");
     wm8904::init_codec(&mut i2c_bus);
-    let queue = cortex_m::singleton!(
-        : Queue<SampleType, FIFO_LENGTH>
-        = Queue::new()
-    )
-    .unwrap();
-    let (producer, consumer) = queue.split();
 
-    let consumer_ref = FIFO_CONSUMER_STORE.init(consumer);
+    // let consumer_ref = FIFO_CONSUMER_STORE.init(consumer);
-    unsafe { FIFO_CONSUMER = consumer_ref as *mut _ };
+    // unsafe { FIFO_CONSUMER = consumer_ref as *mut _ };
 
     let mut clock = Clock {};
     let mut audio = Audio {
         i2s: i2s_peripheral,
-        producer,
+        producer: QUEUE.stream_producer(),
         running: AtomicBool::new(false),
         integrator: AtomicI32::new(0),
     };
 
-    let config = AudioClassConfig::new(UsbSpeed::High, FunctionCode::Other, &mut clock, &mut audio)
+    let config = AudioClassConfig::new(usb_speed, FunctionCode::Other, &mut clock, &mut audio)
         .with_output_config(TerminalConfig::new(
             4,
             1,
@@ -353,6 +375,5 @@ fn main() -> ! {
 
     loop {
         usb_dev.poll(&mut [&mut uac2]);
-        red_led.set_high().ok(); // Turn off
     }
 }

src/lib.rs

@@ -106,8 +106,8 @@ impl<T: RangeType + PartialOrd> PartialOrd for RangeEntry<T> {
 /// Fixed point 10.14, packed to the least significant 3-bytes of a 4-byte USB feedback endpoint response
 #[derive(Copy, Clone)]
 pub struct UsbIsochronousFeedback {
-    int: u16,
+    pub int: u16,
-    frac: u16,
+    pub frac: u16,
 }
 
 impl UsbIsochronousFeedback {
@@ -164,13 +164,12 @@ pub trait UsbAudioClass<'a, B: UsbBus> {
     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
+    /// return the correct feedback payload. Feedback always runs at 1ms (in
-    /// timing reference. Better to track sample timing using other means (even
+    /// this implementation), and will be passed the nominal frame size.
-    /// `audio_data_rx`).
     ///
     /// Required for isochronous asynchronous mode to work properly. If None is
     /// returned, no IN packet will be emitted at feedback time.
-    fn feedback(&mut self) -> Option<UsbIsochronousFeedback> {
+    fn feedback(&mut self, nominal_rate: UsbIsochronousFeedback) -> Option<UsbIsochronousFeedback> {
         None
     }
 
@@ -191,8 +190,13 @@ pub trait UsbAudioClass<'a, B: UsbBus> {
 pub trait UsbAudioClockImpl {
     const CLOCK_TYPE: ClockType;
     const SOF_SYNC: bool;
-    /// Called when the host requests the current sample rate. Returns the sample rate in Hz.
+    /// Called when the host or class needs the current sample rate. Returns the
-    fn get_sample_rate(&self) -> core::result::Result<u32, UsbAudioClassError>;
+    /// sample rate in Hz. It should be cheap and infallible as it gets called in every cycle
+    /// of the feedback loop. Use clock validity to signal to the host if the clock is not usable.
+    ///
+    /// Should never return 0 as it may be used in divides in the feedback loop
+    /// and that would cause a hard fault.
+    fn get_sample_rate(&self) -> u32;
     /// Called when the host requests to set the sample rate. Not necessarily called at all startups,
     /// so alt_setting should start/stop the clock. Not required for 'fixed' clocks.
     fn set_sample_rate(
@@ -468,9 +472,9 @@ impl<'a, B: UsbBus, CS: UsbAudioClockImpl, AU: UsbAudioClass<'a, B>>
     /// Allocate the various USB IDs, and build the class implementation
     pub fn build(self, alloc: &'a UsbBusAllocator<B>) -> Result<AudioClass<'a, B, CS, AU>> {
         let speed = self.speed;
-        let interval = match speed {
+        let (interval, fb_interval, audio_rate) = match speed {
-            UsbSpeed::Full => 1,
+            UsbSpeed::Full => (1, 1, 1000),
-            UsbSpeed::High | UsbSpeed::Super => 4, // rate = 2^(4-1) * 125us = 1ms, same as full speed
+            UsbSpeed::High | UsbSpeed::Super => (1, 4, 8000), //
             UsbSpeed::Low => return Err(Error::InvalidSpeed),
         };
         let max_rate = self
@@ -498,6 +502,7 @@ impl<'a, B: UsbBus, CS: UsbAudioClockImpl, AU: UsbAudioClass<'a, B>>
             out_ep: 0,
             fb_ep: 0,
             speed,
+            audio_rate,
         };
 
         if let Some(config) = self.output_config {
@@ -505,14 +510,14 @@ impl<'a, B: UsbBus, CS: UsbAudioClockImpl, AU: UsbAudioClass<'a, B>>
             let endpoint = alloc.isochronous(
                 config.sync_type,
                 IsochronousUsageType::Data,
-                ((config.bytes_per_frame() * max_rate) / 1000) as u16,
+                (max_rate.div_ceil(audio_rate) * config.bytes_per_frame()) as u16,
                 interval,
             );
             let feedback_ep = alloc.isochronous(
                 IsochronousSynchronizationType::NoSynchronization,
                 IsochronousUsageType::Feedback,
                 4,
-                interval,
+                fb_interval,
             );
             let alt_setting = DEFAULT_ALTERNATE_SETTING;
             ac.out_iface = interface.into();
@@ -532,7 +537,7 @@ impl<'a, B: UsbBus, CS: UsbAudioClockImpl, AU: UsbAudioClass<'a, B>>
             let endpoint = alloc.isochronous(
                 config.sync_type,
                 IsochronousUsageType::Data,
-                ((config.bytes_per_frame() * max_rate) / 1000) as u16,
+                (max_rate.div_ceil(audio_rate) * config.bytes_per_frame()) as u16,
                 interval,
             );
             let alt_setting = DEFAULT_ALTERNATE_SETTING;
@@ -664,6 +669,7 @@ pub struct AudioClass<'a, B: UsbBus, CS: UsbAudioClockImpl, AU: UsbAudioClass<'a
     out_ep: usize,
     fb_ep: usize,
     speed: UsbSpeed,
+    audio_rate: u32, // audio packet rate in hz
 }
 
 impl<'a, B: UsbBus, CS: UsbAudioClockImpl, AU: UsbAudioClass<'a, B>> UsbClass<B>
@@ -811,22 +817,28 @@ impl<'a, B: UsbBus, CS: UsbAudioClockImpl, AU: UsbAudioClass<'a, B>> UsbClass<B>
                 .audio_data_rx(&self.output.as_ref().unwrap().endpoint);
             let new_count = COUNTER.fetch_add(1, core::sync::atomic::Ordering::Relaxed);
 
-            if new_count.is_multiple_of(1 as usize) {
+            if let Some(fb_ep) = self.feedback.as_ref() {
-                if let Some(fb) = self.audio_impl.feedback() {
+                if new_count.is_multiple_of(1 << (fb_ep.interval() - 1) as usize) {
+                    let cur_rate = self.clock_impl.get_sample_rate();
+                    let numerator = (cur_rate as u64) << 16;
+                    let raw = (numerator + self.audio_rate as u64 / 2) / (self.audio_rate as u64);
+                    let nominal_rate = UsbIsochronousFeedback {
+                        int: (raw >> 16) as u16,
+                        frac: (raw & 0xffff) as u16,
+                    };
+
+                    if let Some(fb) = self.audio_impl.feedback(nominal_rate) {
                         debug!("  emitting feedback IN {:08x}", fb.to_u32_12_13());
                         let r = match self.speed {
-                        UsbSpeed::Low | UsbSpeed::Full => {
+                            UsbSpeed::Low | UsbSpeed::Full => fb_ep.write(&fb.to_bytes_10_14()),
-                            self.feedback.as_ref().unwrap().write(&fb.to_bytes_10_14())
+                            UsbSpeed::High | UsbSpeed::Super => fb_ep.write(&fb.to_bytes_12_13()),
-                        }
-                        UsbSpeed::High | UsbSpeed::Super => {
-                            self.feedback.as_ref().unwrap().write(&fb.to_bytes_12_13())
-                        }
                         };
                         if let Err(e) = r {
                             warn!("  feedback IN failed {:?}", e);
                         }
                     }
                 }
+            }
         } else {
             debug!("  unexpected OUT on {}", addr);
         }
@@ -1118,14 +1130,13 @@ impl<'a, B: UsbBus, CS: UsbAudioClockImpl, AU: UsbAudioClass<'a, B>> AudioClass<
                         channel
                     );
                 }
-                xfer.accept(|mut buf| match self.clock_impl.get_sample_rate() {
+                xfer.accept(|mut buf| {
-                    Ok(rate) => {
+                    let rate = self.clock_impl.get_sample_rate();
+
                     debug!("    {}", rate);
                     buf.write_u32::<LittleEndian>(rate)
                         .map_err(|_e| UsbError::BufferOverflow)?;
                     Ok(4)
-                    }
-                    Err(_e) => Err(UsbError::InvalidState),
                 })
                 .ok();
             }