diff --git a/firmware/src/dma.rs b/firmware/src/dma.rs index 191ef5a..ce2be7a 100644 --- a/firmware/src/dma.rs +++ b/firmware/src/dma.rs @@ -294,6 +294,8 @@ impl DmaRing { } pub fn run(&self) { + self.dma.inta0.write(|w| unsafe { w.bits(1 << 19) }); + self.dma.errint0.write(|w| unsafe { w.bits(1 << 19) }); self.dma.enableset0.write(|w| unsafe { w.bits(1 << 19) }); } diff --git a/firmware/src/main.rs b/firmware/src/main.rs index 2212878..04b91be 100644 --- a/firmware/src/main.rs +++ b/firmware/src/main.rs @@ -8,6 +8,8 @@ fn panic() -> ! { } use atomic::Atomic; +use core::ptr; +use core::sync::atomic::AtomicPtr; use core::sync::atomic::{AtomicBool, AtomicI32, AtomicUsize, Ordering}; use cortex_m_rt::entry; use defmt; @@ -117,47 +119,29 @@ struct ClockSelPins { #[derive(Default)] struct PerfCounters { - state: Atomic, + // state: Atomic, received_frames: AtomicUsize, played_frames: AtomicUsize, - min_fill: AtomicUsize, - avg_fill: AtomicUsize, + // min_fill: AtomicUsize, + // avg_fill: AtomicUsize, queue_underflows: AtomicUsize, queue_overflows: AtomicUsize, audio_underflows: AtomicUsize, - integrator: AtomicI32, - p: AtomicI32, - i: AtomicI32, - fb: AtomicI32, + // p: AtomicI32, + // fb: AtomicI32, } impl PerfCounters { fn reset(&self) { self.received_frames.store(0, Ordering::Relaxed); self.played_frames.store(0, Ordering::Relaxed); - self.min_fill - .store(N_SLOTS * MAX_BYTES_PER_SLOT, Ordering::Relaxed); - self.avg_fill.store(0 as usize, Ordering::Relaxed); + // self.min_fill + // .store(N_SLOTS * MAX_BYTES_PER_SLOT, Ordering::Relaxed); + // self.avg_fill.store(0 as usize, Ordering::Relaxed); self.queue_underflows.store(0, Ordering::Relaxed); self.queue_overflows.store(0, Ordering::Relaxed); self.audio_underflows.store(0, Ordering::Relaxed); - self.p.store(0, Ordering::Relaxed); - self.i.store(0, Ordering::Relaxed); - // FB loop will have to take care of the fb value - } - fn build_report(&self) -> AudioTelemetryReport { - AudioTelemetryReport { - state: self.state.load(Ordering::Relaxed) as u8, - average_buffer_fill: self.avg_fill.load(Ordering::Relaxed) as u16, - frame_count: self.played_frames.load(Ordering::Relaxed) as i32, - dac_underflow_count: self.audio_underflows.load(Ordering::Relaxed) as u16, - usb_underflow_count: self.queue_underflows.load(Ordering::Relaxed) as u16, - dac_overflow_count: self.queue_overflows.load(Ordering::Relaxed) as u16, - integrator: self.integrator.load(Ordering::Relaxed), - p: self.p.load(Ordering::Relaxed), - i: self.i.load(Ordering::Relaxed), - fb: u32::cast_signed(self.fb.load(Ordering::Relaxed) as u32), - } + // self.p.store(0, Ordering::Relaxed); } } @@ -165,11 +149,11 @@ impl defmt::Format for PerfCounters { fn format(&self, fmt: defmt::Formatter) { defmt::write!( fmt, - "frames: {}/{} min_fill: {} avg fill: {} a_underflows: {} q_underflows: {} q_overflows: {}", + "frames: {}/{} a_underflows: {} q_underflows: {} q_overflows: {}", self.played_frames.load(Ordering::Relaxed), self.received_frames.load(Ordering::Relaxed), - self.min_fill.load(Ordering::Relaxed), - self.avg_fill.load(Ordering::Relaxed), + // self.min_fill.load(Ordering::Relaxed), + // self.avg_fill.load(Ordering::Relaxed), self.audio_underflows.load(Ordering::Relaxed), self.queue_underflows.load(Ordering::Relaxed), self.queue_overflows.load(Ordering::Relaxed) @@ -178,27 +162,141 @@ impl defmt::Format for PerfCounters { } static PERF: PerfCounters = PerfCounters { - state: Atomic::new(AudioState::Stopped), + // state: Atomic::new(AudioState::Stopped), received_frames: AtomicUsize::new(0), // received from USB played_frames: AtomicUsize::new(0), // played audio frames - min_fill: AtomicUsize::new(0), // not recording this for now, need to figure out how to make it meaningful, since the queue starts empty - avg_fill: AtomicUsize::new(0), + // min_fill: AtomicUsize::new(0), // not recording this for now, need to figure out how to make it meaningful, since the queue starts empty + // avg_fill: 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), - integrator: AtomicI32::new(0), - p: AtomicI32::new(0), - i: AtomicI32::new(0), - fb: AtomicI32::new(0), + // p: AtomicI32::new(0), + // fb: AtomicI32::new(0), }; +fn build_telemetry_report, I>( + perf: &PerfCounters, + audio: &Audio, +) -> AudioTelemetryReport { + AudioTelemetryReport { + state: audio.state.load(Ordering::Relaxed) as u8, + average_buffer_fill: audio.fb.avg_fill as u16, + frame_count: perf.played_frames.load(Ordering::Relaxed).cast_signed() as i32, + dac_underflow_count: perf.audio_underflows.load(Ordering::Relaxed) as u16, + usb_underflow_count: perf.queue_underflows.load(Ordering::Relaxed) as u16, + dac_overflow_count: perf.queue_overflows.load(Ordering::Relaxed) as u16, + cur_rate: audio.cur_rate.cast_signed(), + fb_rate_estimate: audio.fb.current_freq_estimate() as i32, + } +} + +#[derive(Clone, Copy, Debug)] +pub struct FeedbackConfig { + /// Nominal USB rate in Q12.13 fixed-point format (or target sample rate context). + pub nominal_rate: u32, + /// Target ring buffer fill level in bytes. + pub target_fill_bytes: i32, + /// Deadband threshold in bytes (e.g., 4). + pub deadband_bytes: i32, + /// Maximum allowed feedback deviation permille denominator (e.g., 500 => 0.2%). + pub max_deviation_divider: i32, + /// Master toggle for feedback correction. + pub correction_enabled: bool, +} + +impl FeedbackConfig { + pub fn new(nominal_rate: u32, target_fill_bytes: usize) -> Self { + Self { + nominal_rate: nominal_rate, + target_fill_bytes: target_fill_bytes as i32, + deadband_bytes: 4, + max_deviation_divider: 500, // 0.2% + correction_enabled: true, + } + } + pub fn for_rate(nominal_rate: u32) -> Self { + let target_fill_bytes = (bytes_per_slot(nominal_rate) * N_SLOTS) / 2; + Self::new(nominal_rate, target_fill_bytes) + } +} + +pub struct FeedbackLoop { + pub config: FeedbackConfig, + pub avg_fill: usize, + last_freq: u32, +} + +impl FeedbackLoop { + pub fn new(config: FeedbackConfig) -> Self { + Self { + config, + avg_fill: config.target_fill_bytes as usize, + last_freq: config.nominal_rate, + } + } + + pub fn reset(&mut self) { + self.avg_fill = self.config.target_fill_bytes as usize; + self.last_freq = self.config.nominal_rate + } + + pub fn update_config(&mut self, config: FeedbackConfig) { + self.config = config; + self.reset(); + } + + pub fn compute( + &mut self, + nominal_rate: UsbIsochronousFeedback, + current_fill_bytes: usize, + ) -> UsbIsochronousFeedback { + if !self.config.correction_enabled { + return nominal_rate; + } + + let current_bytes = current_fill_bytes as i32; + + if current_bytes == 0 { + defmt::error!("[fb] dma underrun detected!"); + PERF.queue_underflows.fetch_add(1, Ordering::Relaxed); + return nominal_rate; + } + + self.avg_fill = ((self.avg_fill << 6) - self.avg_fill + current_bytes as usize) >> 6; + + let target_fill = self.config.target_fill_bytes; + let raw_error = current_bytes - target_fill; + + let nominal_v = nominal_rate.to_u32_12_13() as i32; + let max_allowed_deviation = nominal_v / self.config.max_deviation_divider; + + let error_permille = (raw_error * 1000) / target_fill; + let p_term = (-((error_permille as i64) * (nominal_v as i64)) / (10 * 256000)) as i32; + + let mut v = nominal_v + p_term; + v = v.clamp( + nominal_v - max_allowed_deviation, + nominal_v + max_allowed_deviation, + ); + self.last_freq = v as u32; + UsbIsochronousFeedback::new(v as u32) + } + + pub fn current_freq_estimate(&self) -> f32 { + // Divides by 8 (frame-to-microframe) and 65536.0 (Q16.16 shift) + (self.last_freq as f32 / (65536.0)) * USB_FRAME_RATE as f32 + } +} + static NODATA_FLAG: AtomicBool = AtomicBool::new(false); static DMA_RING: StaticCell> = StaticCell::new(); -static mut DMA_RING_REF: Option<&'static DmaRing> = None; +static DMA_RING_PTR: AtomicPtr> = + AtomicPtr::new(ptr::null_mut()); #[inline] fn dma_ring() -> &'static DmaRing { - unsafe { DMA_RING_REF.unwrap() } + let ptr = DMA_RING_PTR.load(Ordering::Acquire); + unsafe { &*ptr } } fn cur_fill() -> usize { @@ -209,14 +307,21 @@ fn cur_fill() -> usize { produced_bytes.wrapping_sub(consumed_bytes) as usize } +/// current fill target (based on current slot size) fn cur_fill_target() -> i32 { (dma_ring().slot_size() * N_SLOTS) as i32 / 2 } +/// frames per slot (based on current slot size) fn frames_per_slot() -> usize { dma_ring().slot_size() / BYTES_PER_FRAME } +/// bytes per slot (based on provided rate) +fn bytes_per_slot(rate: u32) -> usize { + (rate as usize / DMA_RATE) * BYTES_PER_FRAME +} + // 50% fn queue_running_up_threshold() -> usize { (frames_per_slot() * N_SLOTS) / 2 @@ -276,39 +381,13 @@ fn FLEXCOMM7() { .modify(|_, w| w.txerr().set_bit()) } -struct FeedbackState { - correction_enabled: AtomicBool, - integrator: AtomicI32, - filtered_fill: AtomicI32, -} -impl FeedbackState { - fn start(&mut self) { - self.correction_enabled.store(true, Ordering::Relaxed); - } - fn reset(&mut self) { - self.correction_enabled.store(false, Ordering::Relaxed); - self.integrator.store(0, Ordering::Relaxed); - self.filtered_fill - .store(cur_fill_target(), Ordering::Relaxed); - } -} -impl Default for FeedbackState { - fn default() -> Self { - Self { - correction_enabled: AtomicBool::new(false), - integrator: AtomicI32::new(0), - filtered_fill: AtomicI32::new(cur_fill_target()), - } - } -} - struct Audio<'a, D: Dac, I> { state: Atomic, alt_setting: u8, i2s: I2sTx, dac: D, dma: &'a DmaRing, - fb: FeedbackState, + fb: FeedbackLoop, nodata_timeout_frame: AtomicUsize, cur_rate: u32, clock_pins: ClockSelPins, @@ -332,8 +411,8 @@ impl, I> Audio<'_, D, I> { AudioState::NoData => self.nodata(), AudioState::Stopping => self.stopping(), } - self.state.store(state, Ordering::SeqCst); - PERF.state.store(state, Ordering::Relaxed); + self.state.store(state, Ordering::Release); + // PERF.state.store(state, Ordering::Relaxed); } fn init(&mut self) { @@ -376,6 +455,7 @@ impl, I> Audio<'_, D, I> { }); unsafe { pac::NVIC::unmask(pac::Interrupt::FLEXCOMM7) }; + self.dac.init(); } ///Transition -> Stopped: @@ -415,7 +495,7 @@ impl, I> Audio<'_, D, I> { ///Transition -> Running ///Unmask I2S ISR, start feedback fn run(&mut self) { - self.fb.start(); + self.i2s.i2s.fifostat.write(|w| w.txerr().set_bit()); // FIFO threshold trigger enable self.i2s .i2s @@ -425,14 +505,14 @@ impl, I> Audio<'_, D, I> { .i2s .fifocfg .modify(|_, w| w.enabletx().enabled().dmatx().enabled()); - dma_ring().run(); - // clear tx error status - self.i2s.i2s.fifostat.write(|w| w.txerr().set_bit()); - // enable tx error interrupt - self.i2s.i2s.fifointenset.write(|w| w.txerr().enabled()); unsafe { pac::NVIC::unmask(pac::Interrupt::DMA0); } + dma_ring().run(); + // clear tx error status + // self.i2s.i2s.fifostat.write(|w| w.txerr().set_bit()); + // enable tx error interrupt + self.i2s.i2s.fifointenset.write(|w| w.txerr().enabled()); } ///Transition->NoData ///store framecount at transition so we can time out recovery @@ -472,13 +552,15 @@ impl, I> ClockSource for Audio<'_, D, I> { sample_rate: u32, ) -> core::result::Result<(), usbd_uac2::UsbAudioClassError> { defmt::info!("[clock] changing rate to {}", sample_rate); - if self.state.load(Ordering::SeqCst) != AudioState::Stopped { + if self.state.load(Ordering::Acquire) != AudioState::Stopped { defmt::warn!("[clock] changing rate when not stopped, stopping first"); self.stop(); } - let slot_bytes = (self.cur_rate as usize / DMA_RATE) * BYTES_PER_FRAME; - dma_ring().set_slot_size(slot_bytes); self.cur_rate = sample_rate; + let slot_bytes = bytes_per_slot(self.cur_rate); + dma_ring().set_slot_size(slot_bytes); + self.fb + .update_config(FeedbackConfig::for_rate(self.cur_rate)); if 24_576_000u32.is_multiple_of(sample_rate) { defmt::info!("[clock] 24M osc selected"); self.clock_pins.sel_22m.set_low().ok(); @@ -506,7 +588,7 @@ impl, I> ClockSource for Audio<'_, D, I> { } impl, I, B: bus::UsbBus> AudioHandler<'_, B> for Audio<'_, D, I> { fn alternate_setting_changed(&mut self, _terminal: usb_device::UsbDirection, alt_setting: u8) { - let state = self.state.load(Ordering::Relaxed); + let state = self.state.load(Ordering::Acquire); match (alt_setting, state) { (0, AudioState::Armed | AudioState::Prefill | AudioState::NoData) => { self.transition(AudioState::Stopped) @@ -525,7 +607,7 @@ impl, I, B: bus::UsbBus> AudioHandler<'_, B> for Audio<'_, D, I> { &mut self, ep: &usb_device::endpoint::Endpoint<'_, B, usb_device::endpoint::Out>, ) { - let state = self.state.load(Ordering::Relaxed); + let state = self.state.load(Ordering::Acquire); let mut buf = [0; (MAX_SAMPLE_RATE.div_ceil(USB_FRAME_RATE) + 1) as usize * BYTES_PER_FRAME]; let len = match ep.read(&mut buf) { @@ -584,7 +666,7 @@ impl, I, B: bus::UsbBus> AudioHandler<'_, B> for Audio<'_, D, I> { } } // Any data in NoData moves us into LowData. But maybe it should be more like prefill? - AudioState::NoData => self.transition(AudioState::LowData), + AudioState::NoData => self.transition(AudioState::Prefill), } } fn audio_data_tx( @@ -593,11 +675,7 @@ impl, I, B: bus::UsbBus> AudioHandler<'_, B> for Audio<'_, D, I> { ) { } fn feedback(&mut self, nominal_rate: UsbIsochronousFeedback) -> Option { - if !self.fb.correction_enabled.load(Ordering::Relaxed) { - return Some(nominal_rate); - } - - let current_bytes = cur_fill() as i32; + let current_bytes = cur_fill(); if current_bytes == 0 { defmt::error!("[fb] dma underrun detected!"); @@ -605,44 +683,11 @@ impl, I, B: bus::UsbBus> AudioHandler<'_, B> for Audio<'_, D, I> { return Some(nominal_rate); } - PERF.avg_fill - .fetch_update(Ordering::SeqCst, Ordering::SeqCst, |v| { - Some(((v << 6) - v + current_bytes as usize) >> 6) - }) - .ok(); + let v = self.fb.compute(nominal_rate, current_bytes); + // PERF.avg_fill.store(self.fb.avg_fill, Ordering::Relaxed); + // PERF.fb.store(v as i32, Ordering::Relaxed); - let raw_error = current_bytes - cur_fill_target(); - let i_error = if raw_error.abs() <= 4 { 0 } else { raw_error }; // deadband - let current_i = self.fb.integrator.load(Ordering::Relaxed); - let leak = current_i >> 7; - let new_i = current_i - .saturating_sub(leak) - .saturating_add(i_error) - .clamp(-5000, 5000); - self.fb.integrator.store(new_i, Ordering::Relaxed); - PERF.integrator.store(new_i, Ordering::Relaxed); - - let nominal_v = nominal_rate.to_u32_12_13() as i32; - let max_allowed_deviation = nominal_v / 500; // 0.2% - - // 3. SEPARATE GAINS FOR P AND I - // For P: Keep your working math (converting raw error to a permille equivalent scale) - let error_permille = (raw_error * 1000) / cur_fill_target(); - let p_term = (-((error_permille as i64) * (nominal_v as i64)) / (10 * 256000)) as i32; - let i_term = (-((new_i as i64) * (nominal_v as i64)) / (256000 * 1000)) as i32; - let i_term = 0; - - PERF.p.store(p_term, Ordering::Relaxed); - PERF.i.store(i_term, Ordering::Relaxed); - - let mut v = nominal_v + p_term + i_term; - v = v.clamp( - nominal_v - max_allowed_deviation, - nominal_v + max_allowed_deviation, - ); - PERF.fb.store(v, Ordering::Relaxed); - - Some(UsbIsochronousFeedback::new(v as u32)) + Some(v) } } @@ -832,15 +877,17 @@ fn main() -> ! { ) .unwrap(); let dma_ref = DMA_RING.init(dma); - unsafe { DMA_RING_REF = Some(dma_ref) }; + DMA_RING_PTR.store(dma_ref as *const _ as *mut _, Ordering::Release); + // unsafe { DMA_RING_PTR = Some(dma_ref) }; defmt::info!("audio init"); + let mut audio = Audio { state: Atomic::new(AudioState::Stopped), i2s: i2s_peripheral, dac: dac_impl, dma: dma_ring(), - fb: FeedbackState::default(), + fb: FeedbackLoop::new(FeedbackConfig::for_rate(SAMPLE_RATES[0].min)), alt_setting: 0, nodata_timeout_frame: AtomicUsize::new(0), cur_rate: SAMPLE_RATES[0].min, @@ -871,47 +918,45 @@ fn main() -> ! { .unwrap() .build(); + // 1. Initialize the HID timer conditionally outside the closure #[cfg(feature = "hid")] - let mut poll_all = { - let mut hid_update_timer = Timer::new( + let mut hid_update_timer = { + let mut timer = Timer::new( hal.ctimer .1 .enabled(&mut syscon, clocks.support_1mhz_fro_token().unwrap()), ); - hid_update_timer.start(Microseconds::new(HID_INTERVAL_MS as u32 * 1000)); + timer.start(Microseconds::new(HID_INTERVAL_MS as u32 * 1000)); + timer + }; - move || { - usb_dev.poll(&mut [&mut uac2, &mut hid]); - // DMA ring is empty; if altsetting = 0 then -> stopped else NoData - // TODO: handle NoData state - if NODATA_FLAG.swap(false, Ordering::Acquire) { - match uac2.handler().state.load(Ordering::Acquire) { - AudioState::Stopping => uac2.handler().transition(AudioState::Stopped), - _ => uac2.handler().transition(AudioState::Stopped), - } - } - if hid_update_timer.wait().is_ok() { - let report = PERF.build_report(); - match hid.push_input(&report) { - Ok(_) => {} - Err(UsbError::WouldBlock) => {} - Err(e) => defmt::error!("Failed to send HID report: {:?}", e), - } - // lpc55 ctimer is not Periodic, so restart it - hid_update_timer.start(Microseconds::new(HID_INTERVAL_MS as u32 * 1000)); + // 2. Single consolidated poll_all closure + let mut poll_all = move || { + // Poll active USB classes + #[cfg(feature = "hid")] + usb_dev.poll(&mut [&mut uac2, &mut hid]); + + #[cfg(not(feature = "hid"))] + usb_dev.poll(&mut [&mut uac2]); + + // TODO: we should handle unexpected NODATA differently from 'Stopping' + if NODATA_FLAG.swap(false, Ordering::AcqRel) { + match uac2.handler().state.load(Ordering::Acquire) { + AudioState::Stopping => uac2.handler().transition(AudioState::Stopped), + _ => uac2.handler().transition(AudioState::Stopped), } } - }; - #[cfg(not(feature = "hid"))] - let mut poll_all = { - move || { - usb_dev.poll(&mut [&mut uac2]); - if NODATA_FLAG.swap(false, Ordering::Acquire) { - match uac2.handler().state.load(Ordering::Acquire) { - AudioState::Stopping => uac2.handler().transition(AudioState::Stopped), - _ => uac2.handler().transition(AudioState::Stopped), + + #[cfg(feature = "hid")] + if hid_update_timer.wait().is_ok() { + let report = build_telemetry_report(&PERF, uac2.handler()); + if let Err(e) = hid.push_input(&report) { + if e != UsbError::WouldBlock { + defmt::error!("Failed to send HID report: {:?}", e); } } + // LPC55 CTIMER is not periodic; restart manually + hid_update_timer.start(Microseconds::new(HID_INTERVAL_MS as u32 * 1000)); } }; @@ -919,6 +964,5 @@ fn main() -> ! { loop { poll_all(); - // usb_dev.poll(&mut [&mut uac2]); } } diff --git a/shared/src/lib.rs b/shared/src/lib.rs index e56e654..2bcb36e 100644 --- a/shared/src/lib.rs +++ b/shared/src/lib.rs @@ -99,9 +99,8 @@ pub struct AudioTelemetrySnapshot { pub dac_underflow_count: u16, pub usb_underflow_count: u16, pub dac_overflow_count: u16, - pub p: i32, - pub i: i32, - pub fb: i32, + pub cur_rate: u32, + pub fb_rate_estimate: f32, } pub mod hid { @@ -123,10 +122,8 @@ pub mod hid { dac_underflow_count=input; usb_underflow_count=input; dac_overflow_count=input; - p=input; - i=input; - fb=input; - integrator=input; + cur_rate=input; + fb_rate_estimate=input; } )] #[repr(C, packed)] @@ -138,10 +135,8 @@ pub mod hid { pub dac_underflow_count: u16, pub usb_underflow_count: u16, pub dac_overflow_count: u16, - pub p: i32, - pub i: i32, - pub fb: i32, - pub integrator: i32, + pub cur_rate: i32, + pub fb_rate_estimate: i32, } impl From for AudioTelemetrySnapshot { @@ -149,13 +144,12 @@ pub mod hid { AudioTelemetrySnapshot { state: AudioState::try_from(value.state).expect("Invalid AudioState"), average_buffer_fill: value.average_buffer_fill, - frame_count: i32::cast_unsigned(value.frame_count), // on firmware side is usize == u32 + frame_count: value.frame_count.cast_unsigned(), // on firmware side is usize == u32 dac_underflow_count: value.dac_underflow_count, usb_underflow_count: value.usb_underflow_count, dac_overflow_count: value.dac_overflow_count, - p: value.p, - i: value.i, - fb: value.fb, + cur_rate: value.cur_rate.cast_unsigned(), + fb_rate_estimate: (value.fb_rate_estimate as f32), } } }