ktims/guac-rs
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

switch to bbqueue, simplified feedback

Browse Source
This commit is contained in:
Keenan Tims
2026-05-08 13:02:41 -07:00
parent 4d45da6bdf
commit 7d3e58c9ad
3 changed files with 436 additions and 96 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main.rs
+106 -83
View File
@@ -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<u32, usbd_uac2::UsbAudioClassError> {
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<Consumer<SampleType>> = StaticCell::new();
static mut FIFO_CONSUMER: *mut Consumer<SampleType> = 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<QUEUE_BYTES> = 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<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);
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<T: BbqHandle> {
running: AtomicBool,
i2s: I2sTx,
producer: heapless::spsc::Producer<'a, SampleType>,
producer: StreamProducer<T>,
integrator: AtomicI32,
filtered_fill: 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");
@@ -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<T: BbqHandle, B: bus::UsbBus> UsbAudioClass<'_, B> for Audio<T> {
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<UsbIsochronousFeedback> {
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<UsbIsochronousFeedback> {
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<SampleType, FIFO_LENGTH>
= 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),