From 2b013f065bf373171fc144d8bc5f75277a6222b6 Mon Sep 17 00:00:00 2001 From: Keenan Tims Date: Wed, 6 May 2026 18:41:22 -0700 Subject: [PATCH] working pretty smoothly at 48k! --- examples/lpc55s28-evk/src/main.rs | 436 ++++++++++++++++++++++++------ src/lib.rs | 84 +++++- 2 files changed, 435 insertions(+), 85 deletions(-) diff --git a/examples/lpc55s28-evk/src/main.rs b/examples/lpc55s28-evk/src/main.rs index b12148b..64da435 100644 --- a/examples/lpc55s28-evk/src/main.rs +++ b/examples/lpc55s28-evk/src/main.rs @@ -7,73 +7,136 @@ fn panic() -> ! { panic_probe::hard_fault() } -use core::cell::RefCell; - +use core::cell::UnsafeCell; +use core::ptr::null_mut; +use core::sync::atomic::{AtomicBool, AtomicI32, AtomicUsize, Ordering}; use cortex_m_rt::entry; use defmt; use defmt::debug; use defmt_rtt as _; -use embedded_io::Write; use hal::Syscon; use hal::drivers::{Timer, UsbBus, pins}; -use hal::peripherals::flexcomm::Flexcomm7; use hal::prelude::*; use hal::raw as pac; use hal::time::Hertz; -use heapless::spsc::Queue; -use lpc55_hal::drivers::clocks::Pll; -use lpc55_hal::peripherals::syscon::ClockControl; -use lpc55_hal::raw::{FLEXCOMM7, I2S7}; +use heapless::spsc::Consumer; use lpc55_hal::{self as hal}; - +use pac::interrupt; +use static_cell::StaticCell; use usb_device::{ bus::{self}, device::{StringDescriptors, UsbDeviceBuilder, UsbVidPid}, endpoint::IsochronousSynchronizationType, }; +use usbd_uac2::UsbIsochronousFeedback; use usbd_uac2::{ self, AudioClassConfig, RangeEntry, TerminalConfig, UsbAudioClass, UsbAudioClockImpl, UsbSpeed, constants::{FunctionCode, TerminalType}, descriptors::{ChannelConfig, ClockType, FormatType1, LockDelay}, }; -const CODEC_I2C_ADDR: u8 = 0b0011010; +// mod wm8904; -const SINE_LUT: [i32; 32] = [ - 0, 1636536, 3210180, 4660460, 5931640, 6974871, 7750062, 8227422, 8388607, 8227422, 7750062, - 6974871, 5931640, 4660460, 3210180, 1636536, 0, -1636536, -3210180, -4660460, -5931640, - -6974871, -7750062, -8227422, -8388607, -8227422, -7750062, -6974871, -5931640, -4660460, - -3210180, -1636536, -]; +struct PllConstants { + m: u16, // 1-65535 + n: u8, // 1-255 + p: u8, // 1-31 + selp: u8, // 5 bits + seli: u8, // 6 bits +} -pub fn i2s_sine_test(i2s: &pac::I2S7) -> ! { - let mut idx = 0; - let mut count = 0usize; +impl PllConstants { + const fn new(n: u8, m: u16, p: u8) -> Self { + assert!(n != 0, "1 <= N <= 255"); + assert!(m != 0, "1 <= M <= 65535"); + assert!(p != 0 && p <= 31, "1 <= P <= 31"); - defmt::debug!("starting sine test"); + // Following ripped from lpc55-hal and made const + // UM 4.6.6.3.2 + let selp = { + let v = (m >> 2) + 1; + if v < 31 { v } else { 31 } + } as u8; - loop { - if i2s.fifostat.read().txnotfull().bit_is_set() { - let sample = SINE_LUT[idx] * 32; + let seli = { + let v = match m { + m if m >= 8000 => 1, + m if m >= 122 => 8000 / m, + _ => 2 * (m >> 2) + 3, + }; - // ✅ Left channel - i2s.fifowr.write(|w| unsafe { w.bits(sample as u32) }); - - // wait for space if needed - while !i2s.fifostat.read().txnotfull().bit_is_set() {} - - // ✅ Right channel - i2s.fifowr.write(|w| unsafe { w.bits(sample as u32) }); - - idx = (idx + 1) & (SINE_LUT.len() - 1); - count += 1; - if count.is_multiple_of(48000) { - defmt::debug!("frames sent: {}", count) - } + if v < 63 { v } else { 63 } + } as u8; + // let seli = min(2*(m >> 2) + 3, 63); + Self { + n, + m, + p, + selp, + seli, } } } +impl defmt::Format for PllConstants { + fn format(&self, fmt: defmt::Formatter) { + let factor = f32::from(self.m) / (f32::from(self.n) * 2.0 * f32::from(self.p)); + + defmt::write!( + fmt, + "m: {} n: {} p: {} selp: {} seli: {} fout: fin * {}", + self.m, + self.n, + self.p, + self.selp, + self.seli, + factor + ); + } +} + +const CODEC_I2C_ADDR: u8 = 0b0011010; +// Fo = M/(N*2*P) * Fin +// Fo = 3072/(125*2*8) * 16MHz = 24.576MHz +const AUDIO_PLL: PllConstants = PllConstants::new(125, 3072, 8); +const FIFO_LENGTH: usize = 2048; // frames +type SampleType = (i32, i32); + +// const SINE_LUT: [i32; 32] = [ +// 0, 1636536, 3210180, 4660460, 5931640, 6974871, 7750062, 8227422, 8388607, 8227422, 7750062, +// 6974871, 5931640, 4660460, 3210180, 1636536, 0, -1636536, -3210180, -4660460, -5931640, +// -6974871, -7750062, -8227422, -8388607, -8227422, -7750062, -6974871, -5931640, -4660460, +// -3210180, -1636536, +// ]; + +// pub fn i2s_sine_test(i2s: &pac::I2S7) -> ! { +// let mut idx = 0; +// let mut count = 0usize; + +// defmt::debug!("starting sine test"); + +// loop { +// if i2s.fifostat.read().txnotfull().bit_is_set() { +// let sample = SINE_LUT[idx] * 32; + +// // ✅ Left channel +// i2s.fifowr.write(|w| unsafe { w.bits(sample as u32) }); + +// // wait for space if needed +// while !i2s.fifostat.read().txnotfull().bit_is_set() {} + +// // ✅ Right channel +// i2s.fifowr.write(|w| unsafe { w.bits(sample as u32) }); + +// idx = (idx + 1) & (SINE_LUT.len() - 1); +// count += 1; +// if count.is_multiple_of(48000) { +// defmt::debug!("frames sent: {}", count) +// } +// } +// } +// } + struct Clock {} impl Clock { const RATES: [RangeEntry; 1] = [RangeEntry::new_fixed(48000)]; @@ -94,41 +157,112 @@ impl UsbAudioClockImpl for Clock { } } -struct Audio { - running: RefCell, - i2s: I2sTx, - queue: RefCell>, +#[derive(Default)] +struct PerfCounters { + frames: AtomicUsize, + avg_fill: AtomicI32, // i32 to simplify averaging + queue_underflows: AtomicUsize, + queue_overflows: AtomicUsize, + audio_underflows: AtomicUsize, } -impl Audio { - fn poll(&self) { - if !*self.running.borrow() { - return; - } - let stat = self.i2s.i2s.fifostat.read(); - if stat.txerr().bit_is_set() { - self.i2s.i2s.fifostat.modify(|_, w| w.txerr().set_bit()); - // defmt::error!("fifo tx error, txlvl: {}", stat.txlvl().bits()); - } - if stat.txlvl().bits() <= 6 { - // fifo is 8 deep - if let Some(sample) = self.queue.borrow_mut().dequeue() { - self.i2s - .i2s - .fifowr - .write(|w| unsafe { w.bits(sample.0 as u32) }); - self.i2s - .i2s - .fifowr - .write(|w| unsafe { w.bits(sample.1 as u32) }); - } else { - // defmt::error!("queue underflow"); - self.i2s.i2s.fifowr.write(|w| unsafe { w.bits(0 as u32) }); - } +impl defmt::Format for PerfCounters { + fn format(&self, fmt: defmt::Formatter) { + defmt::write!( + fmt, + "frames: {} avg fill: {} a_underflows: {} q_underflows: {} q_overflows: {}", + self.frames.load(Ordering::Relaxed), + self.avg_fill.load(Ordering::Relaxed), + self.audio_underflows.load(Ordering::Relaxed), + self.queue_underflows.load(Ordering::Relaxed), + self.queue_overflows.load(Ordering::Relaxed) + ) + } +} + +static FIFO_CONSUMER_STORE: StaticCell> = StaticCell::new(); +static mut FIFO_CONSUMER: *mut Consumer = null_mut(); + +#[interrupt] +fn FLEXCOMM7() { + let i2s = unsafe { &*pac::I2S7::ptr() }; + + // refil the buffer to 4 frames / 8 samples + while i2s.fifostat.read().txlvl().bits() <= 6 { + let fifo = unsafe { &mut *FIFO_CONSUMER }; + if let Some((l, r)) = fifo.dequeue() { + i2s.fifowr.write(|w| unsafe { w.bits(l as u32) }); + i2s.fifowr.write(|w| unsafe { w.bits(r as u32) }); + } else { + i2s.fifowr.write(|w| unsafe { w.bits(0) }); + i2s.fifowr.write(|w| unsafe { w.bits(0) }); } } } -impl<'a, B: bus::UsbBus> UsbAudioClass<'a, B> for Audio { + +struct Audio<'a> { + running: AtomicBool, + i2s: I2sTx, + producer: UnsafeCell>, + perf: PerfCounters, + integrator: AtomicI32, +} +impl<'a> Audio<'a> { + // fn poll(&self) { + // if !self.running.load(Ordering::Relaxed) { + // return; + // } + + // if self.i2s.i2s.fifostat.read().txerr().bit_is_set() { + // self.i2s.i2s.fifostat.modify(|_, w| w.txerr().set_bit()); + // // defmt::error!("fifo tx error, txlvl: {}", stat.txlvl().bits()); + // } + // if self.i2s.i2s.fifostat.read().txlvl().bits() == 0 { + // self.perf.audio_underflows.fetch_add(1, Ordering::Relaxed); + // } + // while self.i2s.i2s.fifostat.read().txlvl().bits() <= 6 { + // // fifo is 8 deep at 32 bit samples + // let fifo = self.consumer.get(); + // if let Some(sample) = unsafe { (*fifo).dequeue() } { + // self.i2s + // .i2s + // .fifowr + // .write(|w| unsafe { w.bits(sample.0 as u32) }); + // self.i2s + // .i2s + // .fifowr + // .write(|w| unsafe { w.bits(sample.1 as u32) }); + // } else { + // if self.running.load(Ordering::Relaxed) { + // self.perf.queue_underflows.fetch_add(1, Ordering::Relaxed); + // } + // break; + // // self.i2s.i2s.fifowr.write(|w| unsafe { w.bits(0 as u32) }); + // // self.i2s.i2s.fifowr.write(|w| unsafe { w.bits(0 as u32) }); + // } + // self.perf.frames.fetch_add(1, Ordering::Relaxed); + // } + // } + fn start(&self) { + self.running.store(true, Ordering::Relaxed); + defmt::info!("playback starting, enabling interrupts"); + self.i2s.i2s.fifointenclr.write(|w| w.txlvl().set_bit()); + // FIFO threshold trigger enable + self.i2s + .i2s + .fifotrig + .modify(|_, w| unsafe { w.txlvl().bits(4).txlvlena().enabled() }); + // FIFO level interrupt enable + self.i2s.i2s.fifointenset.modify(|_, w| w.txlvl().enabled()); + unsafe { pac::NVIC::unmask(pac::Interrupt::FLEXCOMM7) }; + } + fn stop(&self) { + self.running.store(true, Ordering::Relaxed); + defmt::info!("playback stopped: {}", self.perf); + pac::NVIC::mask(pac::Interrupt::FLEXCOMM7); + } +} +impl<'a, B: bus::UsbBus> UsbAudioClass<'a, B> for Audio<'_> { fn alternate_setting_changed>( &self, ac: &mut usbd_uac2::AudioClass<'a, B, CS, AU>, @@ -136,8 +270,8 @@ impl<'a, B: bus::UsbBus> UsbAudioClass<'a, B> for Audio { alt_setting: u8, ) { match alt_setting { - 0 => *self.running.borrow_mut() = false, - 1 => *self.running.borrow_mut() = true, + 0 => self.stop(), + 1 => self.start(), _ => defmt::error!("unexpected alt setting {}", alt_setting), } } @@ -153,13 +287,133 @@ impl<'a, B: bus::UsbBus> UsbAudioClass<'a, B> for Audio { let buf = &buf[..len]; for sample in buf.chunks_exact(8).map(|b| { ( - u32::from_le_bytes(b[..4].try_into().unwrap()), - u32::from_le_bytes(b[4..].try_into().unwrap()), + i32::from_le_bytes(b[..4].try_into().unwrap()), + i32::from_le_bytes(b[4..].try_into().unwrap()), ) }) { - self.queue.borrow_mut().enqueue(sample).ok(); // TODO: ok is not ok here, it means we have overflowed the + if let Err(e) = unsafe { (*self.producer.get()).enqueue(sample) } { + self.perf.queue_overflows.fetch_add(1, Ordering::Relaxed); + // defmt::error!("overflowed fifo, len: {}", unsafe { + // (*self.producer.get()).len() + // }); + } } } + fn feedback(&self) -> Option { + const TARGET: i32 = FIFO_LENGTH as i32 / 2 - 64; + const NOMINAL: i32 = 48 << 16; + + let queuelen = unsafe { (*self.producer.get()).len() as i32 }; + let error = (queuelen - TARGET).clamp(-32, 32); + + // --- integrator --- + let scaled_error = error / 64; + + let new_i = self.integrator.fetch_add(scaled_error, Ordering::Relaxed) + scaled_error; + let clamped = new_i.clamp(-131072, 131072); + + // leak + store final value + let leaked = clamped - (clamped >> 8); + self.integrator.store(leaked, Ordering::Relaxed); + + // reset on large deviation + if error.abs() > 96 { + self.integrator.store(0, Ordering::Relaxed); + } + + // --- gains --- + let p = error / 128; + let i = leaked / 32768; + + // correction + let correction = (-(p + i)).clamp(-32, 32); + let v = NOMINAL + (correction << 10); + + // EMA (unchanged, already correct) + let ema = self.perf.avg_fill.load(Ordering::Relaxed); + let new = ((ema * 1023) + queuelen + 512) >> 10; + self.perf.avg_fill.store(new, Ordering::Relaxed); + + defmt::debug!( + "q:{} p:{} i:{} err:{} fb:{}+{}", + queuelen, + p, + i, + error, + NOMINAL >> 16, + correction + ); + + Some(UsbIsochronousFeedback::new(v as u32)) + } +} + +// Set PLL0 to 24.576MHz, start, and wait for lock +// This is not exposed by lpc55-hal, unfortunately. Copy their implementation here. +fn init_audio_pll() { + let syscon = unsafe { &*pac::SYSCON::ptr() }; + let pmc = unsafe { &*pac::PMC::ptr() }; + let anactrl = unsafe { &*pac::ANACTRL::ptr() }; + + debug!("start clk_in"); + pmc.pdruncfg0 + .modify(|_, w| w.pden_xtal32m().poweredon().pden_ldoxo32m().poweredon()); + syscon.clock_ctrl.modify(|_, w| w.clkin_ena().enable()); + anactrl + .xo32m_ctrl + .modify(|_, w| w.enable_system_clk_out().enable()); + + debug!("init pll: {}", AUDIO_PLL); + pmc.pdruncfg0 + .modify(|_, w| w.pden_pll0().poweredoff().pden_pll0_sscg().poweredoff()); + syscon.pll0clksel.write(|w| w.sel().enum_0x1()); // clk_in + syscon.pll0ctrl.write(|w| unsafe { + w.clken() + .enable() + .seli() + .bits(AUDIO_PLL.seli) + .selp() + .bits(AUDIO_PLL.selp) + }); + + syscon + .pll0ndec + .write(|w| unsafe { w.ndiv().bits(AUDIO_PLL.n) }); + syscon.pll0ndec.write(|w| unsafe { + w.ndiv().bits(AUDIO_PLL.n).nreq().set_bit() // latch + }); + + syscon + .pll0pdec + .write(|w| unsafe { w.pdiv().bits(AUDIO_PLL.p) }); + syscon.pll0pdec.write(|w| unsafe { + w.pdiv().bits(AUDIO_PLL.p).preq().set_bit() // latch + }); + + syscon.pll0sscg0.write(|w| unsafe { w.md_lbs().bits(0) }); + + syscon + .pll0sscg1 + .write(|w| unsafe { w.mdiv_ext().bits(AUDIO_PLL.m).sel_ext().set_bit() }); + syscon.pll0sscg1.write(|w| unsafe { + w.mdiv_ext() + .bits(AUDIO_PLL.m) + .sel_ext() + .set_bit() + .mreq() + .set_bit() // latch + .md_req() + .set_bit() // latch + }); + + pmc.pdruncfg0 + .modify(|_, w| w.pden_pll0().poweredon().pden_pll0_sscg().poweredon()); + debug!("pll0 wait for lock"); + let mut i = 0usize; + while syscon.pll0stat.read().lock().bit_is_clear() { + i += 1; + } + debug!("pll0 locked after {} tries", i); } // copied from NXP SDK WM8904_Init @@ -219,10 +473,10 @@ where } pub struct I2sTx { - pub i2s: I2S7, + pub i2s: pac::I2S7, } -pub fn init_i2s(mut fc7: FLEXCOMM7, mut i2s7: I2S7, syscon: &mut Syscon) -> I2sTx { +pub fn init_i2s(mut fc7: pac::FLEXCOMM7, i2s7: pac::I2S7, syscon: &mut Syscon) -> I2sTx { defmt::debug!("init i2s"); // Enable BOTH syscon.reset(&mut fc7); @@ -252,12 +506,12 @@ pub fn init_i2s(mut fc7: FLEXCOMM7, mut i2s7: I2S7, syscon: &mut Syscon) -> I2sT .as_ref() .unwrap() .mclkclksel - .modify(|_, w| w.sel().enum_0x0()); // FRO 96MHz + .modify(|_, w| w.sel().enum_0x1()); // PLL0 pac::SYSCON::ptr() .as_ref() .unwrap() .mclkdiv - .modify(|_, w| w.div().bits(3).halt().run().reset().released()); // div by 4 = 24MHz + .modify(|_, w| w.div().bits(0).halt().run().reset().released()); // div by 1 = PLL0 fout pac::SYSCON::ptr() .as_ref() .unwrap() @@ -285,6 +539,11 @@ pub fn init_i2s(mut fc7: FLEXCOMM7, mut i2s7: I2S7, syscon: &mut Syscon) -> I2sT // Flush regs.fifocfg.modify(|_, w| w.emptytx().set_bit()); + regs.cfg2 + .modify(|_, w| unsafe { w.position().bits(0).framelen().bits(63) }); + + regs.div.modify(|_, w| unsafe { w.div().bits(7) }); // Clock source is MCLK (24MHz on FRO96) / 8 = 3MHz + // Config regs.cfg1.modify(|_, w| unsafe { w.mstslvcfg() @@ -301,11 +560,6 @@ pub fn init_i2s(mut fc7: FLEXCOMM7, mut i2s7: I2S7, syscon: &mut Syscon) -> I2sT .normal() }); - regs.cfg2 - .modify(|_, w| unsafe { w.position().bits(0).framelen().bits(63) }); - - regs.div.modify(|_, w| unsafe { w.div().bits(7) }); // Clock source is MCLK (24MHz on FRO96) / 8 = 3MHz - I2sTx { i2s: regs } } @@ -358,6 +612,9 @@ fn main() -> ! { .enabled(&mut syscon, clocks.support_1mhz_fro_token().unwrap()), ); + debug!("pll0"); + init_audio_pll(); + debug!("peripherals"); let i2c_peripheral = hal @@ -388,12 +645,23 @@ fn main() -> ! { defmt::debug!("codec init"); init_codec(&mut i2c_bus); + let queue = cortex_m::singleton!( + : heapless::spsc::Queue + = heapless::spsc::Queue::new() + ) + .unwrap(); + let (producer, consumer) = queue.split(); + + let consumer_ref = unsafe { FIFO_CONSUMER_STORE.init(consumer) }; + unsafe { FIFO_CONSUMER = consumer_ref as *mut _ }; // i2s_sine_test(&i2s_peripheral.i2s); let audio = Audio { i2s: i2s_peripheral, - queue: RefCell::new(heapless::spsc::Queue::new()), - running: RefCell::new(false), + producer: UnsafeCell::new(producer), + running: AtomicBool::new(false), + perf: PerfCounters::default(), + integrator: AtomicI32::new(0), }; let config = AudioClassConfig::new(UsbSpeed::High, FunctionCode::Other, &clock, &audio) @@ -446,7 +714,7 @@ fn main() -> ! { loop { usb_dev.poll(&mut [&mut uac2]); - audio.poll(); + // audio.poll(); red_led.set_high().ok(); // Turn off } } diff --git a/src/lib.rs b/src/lib.rs index ff731a0..65670b3 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -6,14 +6,17 @@ mod cursor; pub mod descriptors; mod log; +use core::cell::OnceCell; use core::cmp::Ordering; use core::marker::PhantomData; +use core::sync::atomic::AtomicUsize; use byteorder_embedded_io::{LittleEndian, WriteBytesExt}; use constants::*; use descriptors::*; use log::*; +use modular_bitfield::prelude::*; use num_traits::{ConstZero, Zero}; use usb_device::control::{Recipient, Request, RequestType}; use usb_device::device::DEFAULT_ALTERNATE_SETTING; @@ -102,6 +105,53 @@ impl PartialOrd for RangeEntry { } } +/// 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, + frac: u16, +} + +impl UsbIsochronousFeedback { + /// Accepts all u16 values, saturating the output depending on format + pub fn new_frac(int: u16, frac: u16) -> Self { + Self { int, frac } + } + /// Assumed 16.16, not either of the USB formats + pub fn new(value: u32) -> Self { + Self { + int: (value >> 16) as u16, + frac: (value & 0xffff) as u16, + } + } + /// Serialize into a u32 in 16.16 representation for USB HS + pub fn to_u32_12_13(&self) -> u32 { + let int = (self.int as u32) << 16; + // ostensibly 13 bits, so should require << 3, but USB allows us to use + // these bits for 'extra precision'. So we may as well just treat it as + // 16.16. The application can << 3 if it wants to for some reason. + let frac = (self.frac as u32) & 0xffff; + + int | frac + } + /// Serialize into a u32 in 10.14 representation for USB FS (take the 3 LSB) + pub fn to_u32_10_14(&self) -> u32 { + let int = (self.int as u32) << 14; + let frac = (self.frac as u32) & 0x3fff; + + int | frac + } + /// Serialize into 16.16 little endian byte array for USB HS + pub fn to_bytes_12_13(&self) -> [u8; 4] { + self.to_u32_12_13().to_le_bytes() + } + /// Serialize into 10.14 little endian byte array for USB FS + pub fn to_bytes_10_14(&self) -> [u8; 3] { + let bytes = self.to_u32_10_14().to_le_bytes(); + [bytes[0], bytes[1], bytes[2]] + } +} + /// A trait for implementing USB Audio Class 2 devices /// /// Contains callback methods which will be called by the class driver. All @@ -115,6 +165,17 @@ pub trait UsbAudioClass<'a, B: UsbBus> { /// `ep.read()`. fn audio_data_rx(&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 + /// timing reference. Better to track sample timing using other means (even + /// `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(&self) -> Option { + None + } + /// Called when the alternate setting of `terminal`'s interface is changed, /// before the `AudioStream` is updated. Currently not very useful since we /// don't implement alternate settings. @@ -437,6 +498,7 @@ impl<'a, B: UsbBus, CS: UsbAudioClockImpl, AU: UsbAudioClass<'a, B>> in_ep: 0, out_ep: 0, fb_ep: 0, + speed, }; if let Some(config) = self.output_config { @@ -601,6 +663,7 @@ pub struct AudioClass<'a, B: UsbBus, CS: UsbAudioClockImpl, AU: UsbAudioClass<'a in_ep: usize, out_ep: usize, fb_ep: usize, + speed: UsbSpeed, } impl<'a, B: UsbBus, CS: UsbAudioClockImpl, AU: UsbAudioClass<'a, B>> UsbClass @@ -742,9 +805,28 @@ impl<'a, B: UsbBus, CS: UsbAudioClockImpl, AU: UsbAudioClass<'a, B>> UsbClass } fn endpoint_out(&mut self, addr: EndpointAddress) { debug!("EP {} out data", addr); + static COUNTER: AtomicUsize = AtomicUsize::new(0); if addr.index() == self.out_ep { self.audio_impl - .audio_data_rx(&self.output.as_ref().unwrap().endpoint) + .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) = self.audio_impl.feedback() { + debug!(" emitting feedback IN {:08x}", fb.to_u32_12_13()); + let r = match self.speed { + UsbSpeed::Low | UsbSpeed::Full => { + self.feedback.as_ref().unwrap().write(&fb.to_bytes_10_14()) + } + 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); }