From 7d3e58c9ad1c6c590d1dea5268b3c77617a564a2 Mon Sep 17 00:00:00 2001 From: Keenan Tims Date: Fri, 8 May 2026 13:02:41 -0700 Subject: [PATCH] switch to bbqueue, simplified feedback --- Cargo.lock | 338 ++++++++++++++++++++++++++++++++++++++++++++++++++-- Cargo.toml | 5 +- src/main.rs | 189 ++++++++++++++++------------- 3 files changed, 436 insertions(+), 96 deletions(-) diff --git a/Cargo.lock b/Cargo.lock index b1b304c..f7a32ef 100644 --- a/Cargo.lock +++ b/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" @@ -238,13 +311,13 @@ dependencies = [ name = "guac" 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", @@ -296,16 +369,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" @@ -315,6 +378,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" @@ -341,6 +416,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" @@ -371,6 +459,37 @@ dependencies = [ "vcell", ] +[[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", +] diff --git a/Cargo.toml b/Cargo.toml index a0053bc..e48f8a9 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -4,17 +4,18 @@ version = "0.1.0" edition = "2024" [features] -default = ["ak4490"] +default = ["ak4490", "dma"] ak4490 = [] +dma = [] [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 = "../usbd_uac2/examples/lpc55-hal" } nb = "1.1.0" diff --git a/src/main.rs b/src/main.rs index 7a5e2bf..3ebe6aa 100644 --- a/src/main.rs +++ b/src/main.rs @@ -7,8 +7,10 @@ fn panic() -> ! { panic_probe::hard_fault() } -use core::ptr::null_mut; -use core::sync::atomic::{AtomicBool, AtomicI32, AtomicUsize, Ordering}; +use bbqueue::nicknames::Churrasco; +use bbqueue::prod_cons::stream::{StreamConsumer, StreamProducer}; +use bbqueue::traits::bbqhdl::BbqHandle; +use core::sync::atomic::{AtomicBool, AtomicI32, AtomicU32, AtomicUsize, Ordering}; use cortex_m_rt::entry; use defmt; use defmt::debug; @@ -20,10 +22,8 @@ use hal::prelude::*; use hal::raw as pac; use hal::time::Hertz; use hal::typestates::pin::state::Gpio; -use heapless::spsc::Consumer; -use lpc55_hal::{self as hal}; +use lpc55_hal as hal; use pac::interrupt; -use static_cell::StaticCell; use usb_device::{ bus::{self}, device::{StringDescriptors, UsbDeviceBuilder, UsbVidPid}, @@ -44,9 +44,10 @@ mod hw; // Fo = M/(N*2*P) * Fin // Fo = 3072/(125*2*8) * 16MHz = 24.576MHz -const FIFO_LENGTH: usize = 128; // frames +// +const FIFO_LENGTH: usize = 256; // frames const MCLK_FREQ: u32 = 24576000; -const SAMPLE_RATE: u32 = 48000; +const SAMPLE_RATE: u32 = 88200; type SampleType = (i32, i32); struct CodecPins { @@ -68,8 +69,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 { - Ok(self.cur_rate) + fn get_sample_rate(&self) -> u32 { + self.cur_rate } fn set_sample_rate( &mut self, @@ -81,7 +82,7 @@ impl UsbAudioClockImpl for Clock { // hal::wait_at_least(1); self.pins.sel_24m.set_high().ok(); } else { - defmt::info!("[clock] 24M clock selected"); + defmt::info!("[clock] 22M clock selected"); self.pins.sel_24m.set_low().ok(); // hal::wait_at_least(1); self.pins.sel_22m.set_high().ok(); @@ -154,19 +155,57 @@ impl defmt::Format for PerfCounters { } } -static FIFO_CONSUMER_STORE: StaticCell> = StaticCell::new(); -static mut FIFO_CONSUMER: *mut Consumer = null_mut(); +const BYTES_PER_FRAME: usize = 8; +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 = Churrasco::new(); +// Used for feedback calculation of current fifo state +static PRODUCED: AtomicU32 = AtomicU32::new(0); +static CONSUMED: AtomicU32 = AtomicU32::new(0); static PERF: PerfCounters = PerfCounters { received_frames: AtomicUsize::new(0), played_frames: AtomicUsize::new(0), - min_fill: AtomicUsize::new(FIFO_LENGTH), + min_fill: AtomicUsize::new(FIFO_LENGTH), // not recording this for now, need to figure out how to make it meaningful, since the queue starts empty avg_fill: AtomicUsize::new(FIFO_LENGTH / 2), - queue_underflows: AtomicUsize::new(0), + queue_underflows: AtomicUsize::new(0), // ditto here, since we underflow at startup, but we record this one as it can be trended queue_overflows: AtomicUsize::new(0), audio_underflows: AtomicUsize::new(0), }; +fn cur_fill() -> u32 { + PRODUCED + .load(Ordering::Acquire) + .wrapping_sub(CONSUMED.load(Ordering::Acquire)) + / BYTES_PER_FRAME as u32 +} + +#[inline] +fn try_write_one_frame( + cons: &mut StreamConsumer, + 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); + PERF.played_frames.fetch_add(1, Ordering::Relaxed); + 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() }; @@ -176,28 +215,26 @@ fn FLEXCOMM7() { } // refil the buffer to 4 frames / 8 samples - let fifo = unsafe { &mut *FIFO_CONSUMER }; + let mut cons = QUEUE.stream_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) }); - PERF.min_fill.fetch_min(fifo.len(), Ordering::Relaxed); - PERF.played_frames.fetch_add(1, Ordering::Relaxed); - } else { + if !try_write_one_frame(&mut cons, i2s) { + // No complete frame available: write silence to keep FIFO above threshold PERF.queue_underflows.fetch_add(1, Ordering::Relaxed); i2s.fifowr.write(|w| unsafe { w.bits(0) }); i2s.fifowr.write(|w| unsafe { w.bits(0) }); + break; } } } -struct Audio<'a> { +struct Audio { running: AtomicBool, i2s: I2sTx, - producer: heapless::spsc::Producer<'a, SampleType>, + producer: StreamProducer, integrator: AtomicI32, + filtered_fill: AtomicI32, } -impl<'a> Audio<'a> { +impl Audio { fn start(&self) { self.running.store(true, Ordering::Relaxed); defmt::info!("playback starting, enabling interrupts"); @@ -218,8 +255,8 @@ impl<'a> Audio<'a> { pac::NVIC::mask(pac::Interrupt::FLEXCOMM7); } } -impl<'a, B: bus::UsbBus> UsbAudioClass<'a, B> for Audio<'_> { - fn alternate_setting_changed(&mut self, terminal: usb_device::UsbDirection, alt_setting: u8) { +impl UsbAudioClass<'_, B> for Audio { + fn alternate_setting_changed(&mut self, _terminal: usb_device::UsbDirection, alt_setting: u8) { match alt_setting { 0 => self.stop(), 1 => self.start(), @@ -228,7 +265,7 @@ 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>, ) { let mut buf = [0; SAMPLE_RATE as usize / 1000 * 64]; let len = match ep.read(&mut buf) { @@ -239,64 +276,58 @@ impl<'a, B: bus::UsbBus> UsbAudioClass<'a, B> for Audio<'_> { } }; let buf = &buf[..len]; - for sample in buf.chunks_exact(8).map(|b| { - PERF.received_frames.fetch_add(1, Ordering::Relaxed); - ( - i32::from_le_bytes(b[..4].try_into().unwrap()), - i32::from_le_bytes(b[4..].try_into().unwrap()), - ) - }) { - if let Err(e) = self.producer.enqueue(sample) { - PERF.queue_overflows.fetch_add(1, Ordering::Relaxed); - // defmt::error!("overflowed fifo, len: {}", unsafe { - // (*self.producer.get()).len() - // }); - } + if let Ok(mut wg) = self.producer.grant_exact(buf.len()) { + wg.copy_from_slice(buf); + wg.commit(buf.len()); + PRODUCED.fetch_add(buf.len() as u32, Ordering::Relaxed); + PERF.received_frames + .fetch_add(buf.len() / BYTES_PER_FRAME, Ordering::Relaxed); + } else { + PERF.queue_overflows.fetch_add(1, Ordering::Relaxed); + defmt::error!("overflowed bbq, asked {}", buf.len()); } } - fn feedback(&mut self) -> Option { - const TARGET: i32 = FIFO_LENGTH as i32 / 2 - 64; - const NOMINAL: i32 = (SAMPLE_RATE as i32 / 1000) << 16; + fn feedback(&mut self, nominal_rate: UsbIsochronousFeedback) -> Option { + let target = FIFO_LENGTH as i32 / 2 - nominal_rate.int as i32; - let queuelen = self.producer.len(); - let error = (queuelen as i32 - TARGET).clamp(-32, 32); + let fill = cur_fill() as i32; + let prev = self.filtered_fill.load(Ordering::Relaxed); + let filtered = prev + ((fill - prev) >> 4); // ~1/16 smoothing + self.filtered_fill.store(filtered, Ordering::Relaxed); - // --- integrator --- - let scaled_error = error / 64; + let error = filtered - target; - let new_i = self.integrator.fetch_add(scaled_error, Ordering::Relaxed) + scaled_error; - let clamped = new_i.clamp(-131072, 131072); + // Clamp startup excursions. + let error = error.clamp(-(nominal_rate.int as i32 * 4), nominal_rate.int as i32 * 4); - // leak + store final value - let leaked = clamped - (clamped >> 8); - self.integrator.store(leaked, Ordering::Relaxed); - - // reset on large deviation - if error.abs() > 96 { + // Reset integrator when the error is small + if error.abs() < 2 { self.integrator.store(0, Ordering::Relaxed); } + let mut integrator = self.integrator.load(Ordering::Relaxed); + integrator = integrator - (integrator >> 6); // ~1/64 leak, reduce windup - // --- gains --- - let p = error / 128; - let i = leaked / 32768; + integrator = integrator.clamp(-256, 256); + self.integrator.store(integrator, Ordering::Relaxed); - // correction - let correction = (-(p + i)).clamp(-32, 32); - let v = NOMINAL + (correction << 10); + // gains + let p = error << 3; + let i = integrator * 0; // disabled for now - // EMA (unchanged, already correct) - let ema = PERF.avg_fill.load(Ordering::Relaxed); - let new = ((ema * 1023) + queuelen + 512) >> 10; - PERF.avg_fill.store(new, Ordering::Relaxed); + let correction = -((p + i) >> 2); + let nominal_v = nominal_rate.to_u32_12_13() as i32; + + let mut v = nominal_v + correction; + + // Tight clamp around nominal. + v = v.clamp(nominal_v - (1 << 12), nominal_v + (1 << 12)); defmt::debug!( - "q:{} p:{} i:{} err:{} fb:{}+{}", - queuelen, - p, - i, + "fill:{} err:{} int:{} fb:{=u32:x}", + fill, error, - NOMINAL >> 16, - correction + integrator, + v ); Some(UsbIsochronousFeedback::new(v as u32)) @@ -393,7 +424,6 @@ fn main() -> ! { let mut anactrl = hal.anactrl; let mut pmc = hal.pmc; let mut syscon = hal.syscon; - let mut gpio = hal.gpio.enabled(&mut syscon); let mut iocon = hal.iocon.enabled(&mut syscon); @@ -459,6 +489,7 @@ fn main() -> ! { .flexcomm .4 .enabled_as_i2c(&mut syscon, &clocks.support_flexcomm_token().unwrap()); + let mut i2c_bus = I2cMaster::new( i2c_peripheral, codec_i2c_pins, @@ -486,21 +517,13 @@ fn main() -> ! { defmt::debug!("codec init"); dac::init_dac(&mut i2c_bus, codec_gpio_pins); - let queue = cortex_m::singleton!( - : heapless::spsc::Queue - = heapless::spsc::Queue::new() - ) - .unwrap(); - let (producer, consumer) = queue.split(); - - let consumer_ref = FIFO_CONSUMER_STORE.init(consumer); - unsafe { FIFO_CONSUMER = consumer_ref as *mut _ }; let mut audio = Audio { i2s: i2s_peripheral, - producer, + producer: QUEUE.stream_producer(), running: AtomicBool::new(false), integrator: AtomicI32::new(0), + filtered_fill: AtomicI32::new(0), }; let config = AudioClassConfig::new(UsbSpeed::High, FunctionCode::Other, &mut clock, &mut audio) @@ -512,7 +535,7 @@ fn main() -> ! { bit_resolution: 32, bytes_per_sample: 4, }, - TerminalType::ExtLineConnector, + TerminalType::OutHeadphones, ChannelConfig::default_chans(2), IsochronousSynchronizationType::Asynchronous, LockDelay::Undefined(0),