multi-sample-rate support, evk fixes

This commit is contained in:
ktims committed 2026-08-26 14:28:14 -07:00
1 parent bdd946ad11
commit 34df570dda
5 files changed
+209 -133

No files matched your search

+150 -65
View File
@@ -69,24 +69,41 @@ mod dma;
mod hw;
mod traits;
#[cfg(not(feature = "evk"))]
const MAX_SAMPLE_RATE: u32 = 192000;
#[cfg(feature = "evk")]
const MAX_SAMPLE_RATE: u32 = 96000;
#[cfg(not(feature = "evk"))]
const SAMPLE_RATES: [RangeEntry<u32>; 6] = [
RangeEntry::new_fixed(44100),
RangeEntry::new_fixed(48000),
RangeEntry::new_fixed(44100 * 2),
RangeEntry::new_fixed(48000 * 2),
RangeEntry::new_fixed(44100 * 4),
RangeEntry::new_fixed(48000 * 4),
];
#[cfg(feature = "evk")]
const SAMPLE_RATES: [RangeEntry<u32>; 2] = [
RangeEntry::new_fixed(48000),
RangeEntry::new_fixed(48000 * 2),
];
const DMA_RATE: usize = 4000;
const BYTES_PER_SAMPLE: usize = 4; // 32 bit samples
const BYTES_PER_FRAME: usize = BYTES_PER_SAMPLE * 2; // 2 channels
const FRAMES_PER_SLOT: usize = SAMPLE_RATE as usize / 4000; // run the DMA at 4khz
const BYTES_PER_SLOT: usize = FRAMES_PER_SLOT * BYTES_PER_FRAME;
const MAX_FRAMES_PER_SLOT: usize = MAX_SAMPLE_RATE as usize / 4000; // run the DMA at 4khz
const MAX_BYTES_PER_SLOT: usize = MAX_FRAMES_PER_SLOT * BYTES_PER_FRAME;
const N_SLOTS: usize = 8;
const FILL_TARGET_BYTES: i32 = (BYTES_PER_SLOT * N_SLOTS) as i32 / 2;
// const FILL_TARGET_BYTES: i32 = (BYTES_PER_SLOT * N_SLOTS) as i32 / 2;
const USB_FRAME_RATE: u32 = 8000; // microframe rate: 8000 for HS, 1000 for FS
// In frames
const QUEUE_RUNNING_UP: usize = ((FRAMES_PER_SLOT * N_SLOTS) * 5) / 10; // 50%
const QUEUE_RUNNING_DOWN: usize = ((FRAMES_PER_SLOT * N_SLOTS) * 2) / 10; // 20%
const NODATA_TIMEOUT_FRAMES: usize = SAMPLE_RATE as usize / 100; // ~100ms
#[cfg(not(feature = "evk"))]
const MCLK_FREQ: u32 = 24576000;
#[cfg(feature = "evk")]
const MCLK_FREQ: u32 = 24576000 / 2;
// const QUEUE_RUNNING_UP: usize = ((FRAMES_PER_SLOT * N_SLOTS) * 5) / 10; // 50%
// const QUEUE_RUNNING_DOWN: usize = ((FRAMES_PER_SLOT * N_SLOTS) * 2) / 10; // 20%
// const NODATA_TIMEOUT_FRAMES: usize = SAMPLE_RATE as usize / 100; // ~100ms
const SAMPLE_RATE: u32 = 192000;
const HID_INTERVAL_MS: u8 = 10;
struct CodecPins {
@@ -119,9 +136,8 @@ impl PerfCounters {
self.received_frames.store(0, Ordering::Relaxed);
self.played_frames.store(0, Ordering::Relaxed);
self.min_fill
.store(N_SLOTS * BYTES_PER_SLOT, Ordering::Relaxed);
self.avg_fill
.store(FILL_TARGET_BYTES as usize, Ordering::Relaxed);
.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);
@@ -166,7 +182,7 @@ static PERF: PerfCounters = PerfCounters {
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(FILL_TARGET_BYTES as usize),
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),
@@ -178,10 +194,10 @@ static PERF: PerfCounters = PerfCounters {
static NODATA_FLAG: AtomicBool = AtomicBool::new(false);
static DMA_RING: StaticCell<DmaRing<N_SLOTS, BYTES_PER_SLOT>> = StaticCell::new();
static mut DMA_RING_REF: Option<&'static DmaRing<N_SLOTS, BYTES_PER_SLOT>> = None;
static DMA_RING: StaticCell<DmaRing<N_SLOTS, MAX_BYTES_PER_SLOT>> = StaticCell::new();
static mut DMA_RING_REF: Option<&'static DmaRing<N_SLOTS, MAX_BYTES_PER_SLOT>> = None;
#[inline]
fn dma_ring() -> &'static DmaRing<N_SLOTS, BYTES_PER_SLOT> {
fn dma_ring() -> &'static DmaRing<N_SLOTS, MAX_BYTES_PER_SLOT> {
unsafe { DMA_RING_REF.unwrap() }
}
@@ -193,6 +209,24 @@ fn cur_fill() -> usize {
produced_bytes.wrapping_sub(consumed_bytes) as usize
}
fn cur_fill_target() -> i32 {
(dma_ring().slot_size() * N_SLOTS) as i32 / 2
}
fn frames_per_slot() -> usize {
dma_ring().slot_size() / BYTES_PER_FRAME
}
// 50%
fn queue_running_up_threshold() -> usize {
(frames_per_slot() * N_SLOTS) / 2
}
// 20%
fn queue_running_down_threshold() -> usize {
(frames_per_slot() * N_SLOTS) / 5
}
#[interrupt]
fn DMA0() {
defmt::debug!("dma0");
@@ -224,9 +258,9 @@ fn DMA0() {
} else {
led1().toggle();
PERF.played_frames
.fetch_add(FRAMES_PER_SLOT, Ordering::Relaxed);
.fetch_add(frames_per_slot(), Ordering::Relaxed);
}
if cur_fill() <= BYTES_PER_SLOT {
if cur_fill() <= dma_ring().slot_size() {
led2().on();
NODATA_FLAG.store(true, Ordering::Release);
}
@@ -255,7 +289,7 @@ impl FeedbackState {
self.correction_enabled.store(false, Ordering::Relaxed);
self.integrator.store(0, Ordering::Relaxed);
self.filtered_fill
.store(FILL_TARGET_BYTES, Ordering::Relaxed);
.store(cur_fill_target(), Ordering::Relaxed);
}
}
impl Default for FeedbackState {
@@ -263,7 +297,7 @@ impl Default for FeedbackState {
Self {
correction_enabled: AtomicBool::new(false),
integrator: AtomicI32::new(0),
filtered_fill: AtomicI32::new(FILL_TARGET_BYTES),
filtered_fill: AtomicI32::new(cur_fill_target()),
}
}
}
@@ -273,7 +307,7 @@ struct Audio<'a, D: Dac<I>, I> {
alt_setting: u8,
i2s: I2sTx,
dac: D,
dma: &'a DmaRing<N_SLOTS, BYTES_PER_SLOT>,
dma: &'a DmaRing<N_SLOTS, MAX_BYTES_PER_SLOT>,
fb: FeedbackState,
nodata_timeout_frame: AtomicUsize,
cur_rate: u32,
@@ -281,7 +315,7 @@ struct Audio<'a, D: Dac<I>, I> {
_marker: core::marker::PhantomData<I>,
}
impl<D: Dac<I>, I> Audio<'_, D, I> {
const RATES: [RangeEntry<u32>; 1] = [RangeEntry::new_fixed(SAMPLE_RATE)];
const RATES: &'static [RangeEntry<u32>] = &SAMPLE_RATES;
/// Perform a state transition to `state`
fn transition(&mut self, state: AudioState) {
defmt::info!(
@@ -303,9 +337,8 @@ impl<D: Dac<I>, I> Audio<'_, D, I> {
}
fn init(&mut self) {
let regs = &self.i2s.i2s;
// Enable TX FIFO only
regs.fifocfg.modify(|_, w| {
self.i2s.i2s.fifocfg.modify(|_, w| {
w.enabletx()
.enabled()
.enablerx()
@@ -317,17 +350,17 @@ impl<D: Dac<I>, I> Audio<'_, D, I> {
});
// Flush
regs.fifocfg.modify(|_, w| w.emptytx().set_bit());
self.i2s.i2s.fifocfg.modify(|_, w| w.emptytx().set_bit());
regs.cfg2
self.i2s
.i2s
.cfg2
.modify(|_, w| unsafe { w.position().bits(0).framelen().bits(63) }); // framelen = 64
let bclk_div = (MCLK_FREQ / SAMPLE_RATE / 64) as u16;
regs.div
.modify(|_, w| unsafe { w.div().bits(bclk_div - 1) }); // Clock source is MCLK (12.288MHz) / 4 = 3MHz
self.update_bclk();
// Config
regs.cfg1.modify(|_, w| unsafe {
self.i2s.i2s.cfg1.modify(|_, w| unsafe {
w.mstslvcfg()
.normal_master()
.onechannel()
@@ -362,8 +395,8 @@ impl<D: Dac<I>, I> Audio<'_, D, I> {
// reset performance counters
PERF.reset();
// Stop the clocks
self.clock_pins.sel_22m.set_low().ok();
self.clock_pins.sel_24m.set_low().ok();
// self.clock_pins.sel_22m.set_low().ok();
// self.clock_pins.sel_24m.set_low().ok();
}
///Transition -> Armed
/// Start I2S peripheral and MCLK. Since we assume we have interrupts disabled at
@@ -404,17 +437,31 @@ impl<D: Dac<I>, I> Audio<'_, D, I> {
///Transition->NoData
///store framecount at transition so we can time out recovery
fn nodata(&mut self) {
self.nodata_timeout_frame.store(
PERF.queue_underflows.load(Ordering::Relaxed) + NODATA_TIMEOUT_FRAMES, // we underflow every frame, use it as a timeout counter
Ordering::Relaxed,
);
// TODO: Actually handle this
// self.nodata_timeout_frame.store(
// PERF.queue_underflows.load(Ordering::Relaxed) + NODATA_TIMEOUT_FRAMES, // we underflow every frame, use it as a timeout counter
// Ordering::Relaxed,
// );
}
/// Transition -> Stopping
/// just a marker that upcoming nodata is expected, do nothing
fn stopping(&mut self) {}
fn update_bclk(&mut self) {
let mclk_freq = if 24_576_000u32.is_multiple_of(self.cur_rate) {
24576000
} else {
22579200
};
let bclk_div = (mclk_freq / self.cur_rate / 64) as u16;
self.i2s
.i2s
.div
.modify(|_, w| unsafe { w.div().bits(bclk_div - 1) });
}
}
impl<D: Dac<I>, I> ClockSource for Audio<'_, D, I> {
const CLOCK_TYPE: usbd_uac2::descriptors::ClockType = ClockType::InternalFixed;
const CLOCK_TYPE: usbd_uac2::descriptors::ClockType = ClockType::InternalProgrammable;
const SOF_SYNC: bool = false;
fn sample_rate(&self) -> u32 {
@@ -424,25 +471,34 @@ impl<D: Dac<I>, I> ClockSource for Audio<'_, D, I> {
&mut self,
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 {
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;
if 24_576_000u32.is_multiple_of(sample_rate) {
defmt::info!("[clock] 24M clock selected");
defmt::info!("[clock] 24M osc selected");
self.clock_pins.sel_22m.set_low().ok();
// hal::wait_at_least(1);
self.clock_pins.sel_24m.set_high().ok();
} else {
defmt::info!("[clock] 22M clock selected");
defmt::info!("[clock] 22M osc selected");
self.clock_pins.sel_24m.set_low().ok();
// hal::wait_at_least(1);
self.clock_pins.sel_22m.set_high().ok();
};
self.dac.change_rate(sample_rate);
self.cur_rate = sample_rate;
self.update_bclk();
Ok(())
}
fn sample_rates(
&self,
) -> core::result::Result<&[usbd_uac2::RangeEntry<u32>], usbd_uac2::UsbAudioClassError> {
Ok(&Self::RATES)
defmt::debug!("[clock] sample_rates will return {:?}", &Self::RATES.len());
Ok(Self::RATES)
}
fn clock_validity(&self) -> Result<bool, UsbAudioClassError> {
Ok(true)
@@ -470,7 +526,8 @@ impl<D: Dac<I>, I, B: bus::UsbBus> AudioHandler<'_, B> for Audio<'_, D, I> {
ep: &usb_device::endpoint::Endpoint<'_, B, usb_device::endpoint::Out>,
) {
let state = self.state.load(Ordering::Relaxed);
let mut buf = [0; (SAMPLE_RATE.div_ceil(USB_FRAME_RATE) + 1) as usize * BYTES_PER_FRAME];
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) {
Ok(len) => len,
Err(_) => {
@@ -483,7 +540,7 @@ impl<D: Dac<I>, I, B: bus::UsbBus> AudioHandler<'_, B> for Audio<'_, D, I> {
if res.dropped != 0 {
// Overflow: some or all bytes couldn't be queued.
defmt::error!(
defmt::warn!(
"overflowed dma ring, asked {}, wrote {}, dropped {}",
buf.len(),
res.written,
@@ -505,7 +562,9 @@ impl<D: Dac<I>, I, B: bus::UsbBus> AudioHandler<'_, B> for Audio<'_, D, I> {
AudioState::Armed => self.transition(AudioState::Prefill),
// When prefilling, if we have received frames over the up threshold, move to running
AudioState::Prefill => {
if PERF.received_frames.load(Ordering::Relaxed) >= QUEUE_RUNNING_UP {
if PERF.received_frames.load(Ordering::Relaxed) >= queue_running_up_threshold()
// 50%
{
self.transition(AudioState::Running);
}
}
@@ -515,7 +574,7 @@ impl<D: Dac<I>, I, B: bus::UsbBus> AudioHandler<'_, B> for Audio<'_, D, I> {
AudioState::LowData => {
let fill = cur_fill() as usize;
// Do we check alt setting here? We shouldn't be receiving data at all if we are not in altSetting 1
if fill >= QUEUE_RUNNING_UP {
if fill >= queue_running_up_threshold() {
self.transition(AudioState::Running);
} else if fill == 0 && self.alt_setting == 0 {
self.transition(AudioState::Stopped);
@@ -551,7 +610,7 @@ impl<D: Dac<I>, I, B: bus::UsbBus> AudioHandler<'_, B> for Audio<'_, D, I> {
})
.ok();
let raw_error = current_bytes - FILL_TARGET_BYTES;
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;
@@ -567,7 +626,7 @@ impl<D: Dac<I>, I, B: bus::UsbBus> AudioHandler<'_, B> for Audio<'_, D, I> {
// 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) / FILL_TARGET_BYTES;
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;
@@ -596,6 +655,14 @@ pub fn init_i2s(mut fc7: pac::FLEXCOMM7, i2s7: pac::I2S7, syscon: &mut Syscon) -
syscon.reset(&mut fc7);
syscon.enable_clock(&mut fc7);
unsafe {
pac::SYSCON::ptr()
.as_ref()
.unwrap()
.fcclksel7()
.modify(|_, w| w.sel().enum_0x5()); // MCLK
}
#[cfg(not(feature = "evk"))]
unsafe {
pac::IOCON::ptr().as_ref().unwrap().pio0_23.modify(|_, w| {
w.func()
@@ -611,23 +678,29 @@ pub fn init_i2s(mut fc7: pac::FLEXCOMM7, i2s7: pac::I2S7, syscon: &mut Syscon) -
.od()
.normal()
});
pac::SYSCON::ptr()
.as_ref()
.unwrap()
.fcclksel7()
.modify(|_, w| w.sel().enum_0x5()); // MCLK
};
#[cfg(not(feature = "evk"))]
unsafe {
pac::SYSCON::ptr()
.as_ref()
.unwrap()
.mclkio
.modify(|_, w| w.mclkio().input());
}
};
#[cfg(feature = "evk")]
unsafe {
pac::IOCON::ptr().as_ref().unwrap().pio1_31.modify(|_, w| {
w.func()
.alt1()
.mode()
.inactive()
.slew()
.fast()
.invert()
.disabled()
.digimode()
.digital()
.od()
.normal()
});
pac::SYSCON::ptr()
.as_ref()
.unwrap()
@@ -750,9 +823,13 @@ fn main() -> ! {
defmt::info!("dma init");
let i2s_dma_addr = &i2s_peripheral.i2s.fifowr as *const _ as *mut u32;
let dma =
DmaRing::<N_SLOTS, BYTES_PER_SLOT>::new(hal.dma.release(), &mut syscon, i2s_dma_addr, 4)
.unwrap();
let dma = DmaRing::<N_SLOTS, MAX_BYTES_PER_SLOT>::new(
hal.dma.release(),
&mut syscon,
i2s_dma_addr,
4,
)
.unwrap();
let dma_ref = DMA_RING.init(dma);
unsafe { DMA_RING_REF = Some(dma_ref) };
@@ -765,7 +842,7 @@ fn main() -> ! {
fb: FeedbackState::default(),
alt_setting: 0,
nodata_timeout_frame: AtomicUsize::new(0),
cur_rate: SAMPLE_RATE,
cur_rate: SAMPLE_RATES[0].min,
clock_pins: clock_sel_pins,
_marker: core::marker::PhantomData,
};
@@ -825,8 +902,16 @@ fn main() -> ! {
}
};
#[cfg(not(feature = "hid"))]
let poll_all = || {
usb_dev.poll(&mut [&mut uac2]);
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),
}
}
}
};
defmt::info!("main loop");