839 lines
28 KiB
Rust
839 lines
28 KiB
Rust
#![no_main]
|
|
#![no_std]
|
|
|
|
extern crate panic_probe;
|
|
#[defmt::panic_handler]
|
|
fn panic() -> ! {
|
|
panic_probe::hard_fault()
|
|
}
|
|
|
|
use atomic::Atomic;
|
|
use core::sync::atomic::{AtomicBool, AtomicI32, AtomicUsize, Ordering};
|
|
use cortex_m_rt::entry;
|
|
use defmt;
|
|
use defmt::debug;
|
|
use defmt_rtt as _;
|
|
use hal::Pin;
|
|
use hal::Syscon;
|
|
use hal::drivers::{Timer, UsbBus, pins, pins::direction::Output};
|
|
use hal::prelude::*;
|
|
use hal::raw as pac;
|
|
use hal::time::{Hertz, Microseconds};
|
|
use hal::typestates::pin::state::Gpio;
|
|
use lpc55_hal as hal;
|
|
use pac::interrupt;
|
|
use static_cell::StaticCell;
|
|
use usb_device::{
|
|
bus::{self},
|
|
device::{StringDescriptors, UsbVidPid},
|
|
};
|
|
#[cfg(feature = "hid")]
|
|
use usbd_hid::{descriptor::SerializedDescriptor, hid_class::HIDClass};
|
|
use usbd_uac2::{
|
|
self, AudioHandler, ClockSource, RangeEntry, TerminalConfig, UsbAudioClassConfig,
|
|
UsbAudioClassError, UsbIsochronousFeedback, UsbSpeed,
|
|
constants::{FunctionCode, TerminalType},
|
|
descriptors::ClockType,
|
|
};
|
|
|
|
use crate::dac::DacImpl;
|
|
use crate::dma::DmaRing;
|
|
use crate::hw::led1;
|
|
use crate::hw::led2;
|
|
use crate::traits::Dac;
|
|
use shared::AudioState;
|
|
use shared::hid::AudioTelemetryReport;
|
|
|
|
#[cfg(feature = "ak4490")]
|
|
pub mod dac {
|
|
mod ak4490;
|
|
pub use self::ak4490::Ak4490Dac as DacImpl;
|
|
}
|
|
#[cfg(feature = "cs4398")]
|
|
pub mod dac {
|
|
mod cs4398;
|
|
pub use self::cs4398::Cs4398Dac as DacImpl;
|
|
}
|
|
#[cfg(feature = "nodac")]
|
|
pub mod dac {
|
|
mod noop;
|
|
pub use self::noop::NoopDac as DacImpl;
|
|
}
|
|
#[cfg(feature = "wm8904")]
|
|
pub mod dac {
|
|
mod wm8904;
|
|
pub use self::wm8904::Wm8904Dac as DacImpl;
|
|
}
|
|
|
|
mod dma;
|
|
mod hw;
|
|
mod traits;
|
|
|
|
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 N_SLOTS: usize = 8;
|
|
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 SAMPLE_RATE: u32 = 192000;
|
|
const HID_INTERVAL_MS: u8 = 10;
|
|
|
|
struct CodecPins {
|
|
reset: Pin<pins::Pio0_3, Gpio<Output>>,
|
|
}
|
|
|
|
struct ClockSelPins {
|
|
sel_24m: Pin<pins::Pio0_27, Gpio<Output>>,
|
|
sel_22m: Pin<pins::Pio0_31, Gpio<Output>>,
|
|
}
|
|
|
|
#[derive(Default)]
|
|
struct PerfCounters {
|
|
state: Atomic<AudioState>,
|
|
received_frames: AtomicUsize,
|
|
played_frames: AtomicUsize,
|
|
min_fill: AtomicUsize,
|
|
avg_fill: AtomicUsize,
|
|
queue_underflows: AtomicUsize,
|
|
queue_overflows: AtomicUsize,
|
|
audio_underflows: AtomicUsize,
|
|
integrator: AtomicI32,
|
|
p: AtomicI32,
|
|
i: 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 * BYTES_PER_SLOT, Ordering::Relaxed);
|
|
self.avg_fill
|
|
.store(FILL_TARGET_BYTES 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),
|
|
}
|
|
}
|
|
}
|
|
|
|
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: {}",
|
|
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.audio_underflows.load(Ordering::Relaxed),
|
|
self.queue_underflows.load(Ordering::Relaxed),
|
|
self.queue_overflows.load(Ordering::Relaxed)
|
|
)
|
|
}
|
|
}
|
|
|
|
static PERF: PerfCounters = PerfCounters {
|
|
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(FILL_TARGET_BYTES as usize),
|
|
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),
|
|
};
|
|
|
|
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;
|
|
#[inline]
|
|
fn dma_ring() -> &'static DmaRing<N_SLOTS, BYTES_PER_SLOT> {
|
|
unsafe { DMA_RING_REF.unwrap() }
|
|
}
|
|
|
|
fn cur_fill() -> usize {
|
|
let produced_bytes = dma_ring().produced_bytes() as u32;
|
|
let consumed_bytes = dma_ring().consumed_bytes() as u32;
|
|
|
|
// Handle rollover properly
|
|
produced_bytes.wrapping_sub(consumed_bytes) as usize
|
|
}
|
|
|
|
#[interrupt]
|
|
fn DMA0() {
|
|
defmt::debug!("dma0");
|
|
let dma = unsafe { &*pac::DMA0::ptr() };
|
|
|
|
let inta = dma.inta0.read().bits();
|
|
let err = dma.errint0.read().bits();
|
|
|
|
// TODO: figure out how to track underflows properly
|
|
if (err & (1 << 19)) != 0 {
|
|
let live = dma.channel19.xfercfg.read().bits();
|
|
|
|
let desc = unsafe { &*dma_ring().channel_desc.get() };
|
|
let mem = desc.d[19];
|
|
defmt::error!(
|
|
"DMA error ch19: live={=u32:08x} INTA={=u32:x} ERR={=u32:x}\n desc: {}",
|
|
live,
|
|
inta,
|
|
err,
|
|
mem
|
|
);
|
|
dma.errint0.write(|w| unsafe { w.bits(1 << 19) });
|
|
}
|
|
|
|
if (inta & (1 << 19)) != 0 {
|
|
dma.inta0.write(|w| unsafe { w.bits(1 << 19) });
|
|
if dma_ring().advance_consumed(1).is_err() {
|
|
PERF.audio_underflows.fetch_add(1, Ordering::Relaxed);
|
|
} else {
|
|
led1().toggle();
|
|
PERF.played_frames
|
|
.fetch_add(FRAMES_PER_SLOT, Ordering::Relaxed);
|
|
}
|
|
if cur_fill() <= BYTES_PER_SLOT {
|
|
led2().on();
|
|
NODATA_FLAG.store(true, Ordering::Release);
|
|
}
|
|
}
|
|
}
|
|
|
|
#[interrupt]
|
|
fn FLEXCOMM7() {
|
|
// Count I2S TX FIFO error (should be underrun, assuming we set up our DMA trigger correctly)
|
|
PERF.audio_underflows.fetch_add(1, Ordering::Relaxed);
|
|
unsafe { &*pac::I2S7::ptr() }
|
|
.fifostat
|
|
.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(FILL_TARGET_BYTES, Ordering::Relaxed);
|
|
}
|
|
}
|
|
impl Default for FeedbackState {
|
|
fn default() -> Self {
|
|
Self {
|
|
correction_enabled: AtomicBool::new(false),
|
|
integrator: AtomicI32::new(0),
|
|
filtered_fill: AtomicI32::new(FILL_TARGET_BYTES),
|
|
}
|
|
}
|
|
}
|
|
|
|
struct Audio<'a, D: Dac<I>, I> {
|
|
state: Atomic<AudioState>,
|
|
alt_setting: u8,
|
|
i2s: I2sTx,
|
|
dac: D,
|
|
dma: &'a DmaRing<N_SLOTS, BYTES_PER_SLOT>,
|
|
fb: FeedbackState,
|
|
nodata_timeout_frame: AtomicUsize,
|
|
cur_rate: u32,
|
|
clock_pins: ClockSelPins,
|
|
_marker: core::marker::PhantomData<I>,
|
|
}
|
|
impl<D: Dac<I>, I> Audio<'_, D, I> {
|
|
const RATES: [RangeEntry<u32>; 1] = [RangeEntry::new_fixed(SAMPLE_RATE)];
|
|
/// Perform a state transition to `state`
|
|
fn transition(&mut self, state: AudioState) {
|
|
defmt::info!(
|
|
"AudioState {} -> {}",
|
|
self.state.load(Ordering::Relaxed),
|
|
state
|
|
);
|
|
match state {
|
|
AudioState::Stopped => self.stop(),
|
|
AudioState::Armed => self.arm(),
|
|
AudioState::Prefill => self.prefill(),
|
|
AudioState::Running => self.run(),
|
|
AudioState::LowData => {}
|
|
AudioState::NoData => self.nodata(),
|
|
AudioState::Stopping => self.stopping(),
|
|
}
|
|
self.state.store(state, Ordering::SeqCst);
|
|
PERF.state.store(state, Ordering::Relaxed);
|
|
}
|
|
|
|
fn init(&mut self) {
|
|
let regs = &self.i2s.i2s;
|
|
// Enable TX FIFO only
|
|
regs.fifocfg.modify(|_, w| {
|
|
w.enabletx()
|
|
.enabled()
|
|
.enablerx()
|
|
.disabled()
|
|
.dmatx()
|
|
.disabled()
|
|
.txi2se0()
|
|
.zero()
|
|
});
|
|
|
|
// Flush
|
|
regs.fifocfg.modify(|_, w| w.emptytx().set_bit());
|
|
|
|
regs.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
|
|
|
|
// Config
|
|
regs.cfg1.modify(|_, w| unsafe {
|
|
w.mstslvcfg()
|
|
.normal_master()
|
|
.onechannel()
|
|
.dual_channel()
|
|
.datalen()
|
|
.bits(31)
|
|
.mainenable()
|
|
.disabled()
|
|
.mode()
|
|
.classic_mode()
|
|
.datapause()
|
|
.normal()
|
|
});
|
|
|
|
unsafe { pac::NVIC::unmask(pac::Interrupt::FLEXCOMM7) };
|
|
self.dac.init();
|
|
}
|
|
///Transition -> Stopped:
|
|
///clear queue, mute DAC, mask I2S ISR, stop I2S peripheral, disable & reset feedback and performance queues
|
|
fn stop(&mut self) {
|
|
// Disable FIFO error interrupt
|
|
self.i2s.i2s.fifointenclr.write(|w| w.txerr().set_bit());
|
|
dma_ring().stop();
|
|
pac::NVIC::mask(pac::Interrupt::DMA0);
|
|
self.dac.mute();
|
|
// Clear any samples in the FIFO
|
|
self.i2s.i2s.fifocfg.modify(|_, w| w.emptytx().set_bit());
|
|
// Disable I2S
|
|
self.i2s.i2s.cfg1.modify(|_, w| w.mainenable().disabled());
|
|
// Reset feedback state
|
|
self.fb.reset();
|
|
// reset performance counters
|
|
PERF.reset();
|
|
// Stop the clocks
|
|
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
|
|
/// this point (as we came from Stopped), and the FIFO is empty, this will
|
|
/// play out 0s.
|
|
fn arm(&mut self) {
|
|
dma_ring().init();
|
|
self.set_sample_rate(self.cur_rate).ok();
|
|
self.i2s.i2s.cfg1.modify(|_, w| w.mainenable().enabled());
|
|
}
|
|
///Transition -> Prefill
|
|
/// Unmute DAC
|
|
fn prefill(&mut self) {
|
|
self.dac.unmute();
|
|
}
|
|
///Transition -> Running
|
|
///Unmask I2S ISR, start feedback
|
|
fn run(&mut self) {
|
|
self.fb.start();
|
|
// FIFO threshold trigger enable
|
|
self.i2s
|
|
.i2s
|
|
.fifotrig
|
|
.modify(|_, w| unsafe { w.txlvl().bits(6).txlvlena().enabled() });
|
|
self.i2s
|
|
.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);
|
|
}
|
|
}
|
|
///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,
|
|
);
|
|
}
|
|
/// Transition -> Stopping
|
|
/// just a marker that upcoming nodata is expected, do nothing
|
|
fn stopping(&mut self) {}
|
|
}
|
|
impl<D: Dac<I>, I> ClockSource for Audio<'_, D, I> {
|
|
const CLOCK_TYPE: usbd_uac2::descriptors::ClockType = ClockType::InternalFixed;
|
|
const SOF_SYNC: bool = false;
|
|
|
|
fn sample_rate(&self) -> u32 {
|
|
self.cur_rate
|
|
}
|
|
fn set_sample_rate(
|
|
&mut self,
|
|
sample_rate: u32,
|
|
) -> core::result::Result<(), usbd_uac2::UsbAudioClassError> {
|
|
if 24_576_000u32.is_multiple_of(sample_rate) {
|
|
defmt::info!("[clock] 24M clock 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");
|
|
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;
|
|
Ok(())
|
|
}
|
|
fn sample_rates(
|
|
&self,
|
|
) -> core::result::Result<&[usbd_uac2::RangeEntry<u32>], usbd_uac2::UsbAudioClassError> {
|
|
Ok(&Self::RATES)
|
|
}
|
|
fn clock_validity(&self) -> Result<bool, UsbAudioClassError> {
|
|
Ok(true)
|
|
}
|
|
}
|
|
impl<D: Dac<I>, 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);
|
|
match (alt_setting, state) {
|
|
(0, AudioState::Armed | AudioState::Prefill | AudioState::NoData) => {
|
|
self.transition(AudioState::Stopped)
|
|
}
|
|
(0, AudioState::Running) => self.transition(AudioState::Stopping),
|
|
(0, AudioState::Stopped | AudioState::Stopping) => {} // already stopped/ing
|
|
(1, AudioState::Stopped) => self.transition(AudioState::Armed),
|
|
(1, _) => {} // altSetting 1 in any other state is a no-op
|
|
(_, _) => {
|
|
defmt::error!("Invalid alt setting {}", alt_setting)
|
|
}
|
|
}
|
|
self.alt_setting = alt_setting;
|
|
}
|
|
fn audio_data_rx(
|
|
&mut self,
|
|
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 len = match ep.read(&mut buf) {
|
|
Ok(len) => len,
|
|
Err(_) => {
|
|
defmt::error!("usb error in rx callback");
|
|
return;
|
|
}
|
|
};
|
|
let buf = &buf[..len];
|
|
let res = self.dma.push(buf);
|
|
|
|
if res.dropped != 0 {
|
|
// Overflow: some or all bytes couldn't be queued.
|
|
defmt::error!(
|
|
"overflowed dma ring, asked {}, wrote {}, dropped {}",
|
|
buf.len(),
|
|
res.written,
|
|
res.dropped
|
|
);
|
|
PERF.queue_overflows
|
|
.fetch_add(res.dropped / BYTES_PER_FRAME, Ordering::Relaxed);
|
|
}
|
|
|
|
PERF.received_frames
|
|
.fetch_add(res.written / BYTES_PER_FRAME, Ordering::Relaxed);
|
|
|
|
// Valid states here are Armed, Prefill, Running, Draining and NoData
|
|
match state {
|
|
AudioState::Stopped | AudioState::Stopping => {
|
|
defmt::error!("Received audio data when stopped/stopping")
|
|
}
|
|
// When armed, data rx goes to prefill
|
|
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 {
|
|
self.transition(AudioState::Running);
|
|
}
|
|
}
|
|
// When running, USB RX is a no-op
|
|
AudioState::Running => {}
|
|
// If draining, check cur_fill, if it rises above QUEUE_RUNNING_UP, move back to running. If it drops to 0, move to NoData or Stopped
|
|
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 {
|
|
self.transition(AudioState::Running);
|
|
} else if fill == 0 && self.alt_setting == 0 {
|
|
self.transition(AudioState::Stopped);
|
|
} else if fill == 0 {
|
|
self.transition(AudioState::NoData);
|
|
}
|
|
}
|
|
// Any data in NoData moves us into LowData. But maybe it should be more like prefill?
|
|
AudioState::NoData => self.transition(AudioState::LowData),
|
|
}
|
|
}
|
|
fn audio_data_tx(
|
|
&mut self,
|
|
_ep: &usb_device::endpoint::Endpoint<'_, B, usb_device::endpoint::In>,
|
|
) {
|
|
}
|
|
fn feedback(&mut self, nominal_rate: UsbIsochronousFeedback) -> Option<UsbIsochronousFeedback> {
|
|
if !self.fb.correction_enabled.load(Ordering::Relaxed) {
|
|
return Some(nominal_rate);
|
|
}
|
|
|
|
let current_bytes = cur_fill() as i32;
|
|
|
|
if current_bytes == 0 {
|
|
defmt::error!("[fb] dma underrun detected!");
|
|
PERF.queue_underflows.fetch_add(1, Ordering::Relaxed);
|
|
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 raw_error = current_bytes - FILL_TARGET_BYTES;
|
|
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) / FILL_TARGET_BYTES;
|
|
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))
|
|
}
|
|
}
|
|
|
|
pub struct I2sTx {
|
|
pub i2s: pac::I2S7,
|
|
}
|
|
|
|
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);
|
|
syscon.enable_clock(&mut fc7);
|
|
|
|
unsafe {
|
|
pac::IOCON::ptr().as_ref().unwrap().pio0_23.modify(|_, w| {
|
|
w.func()
|
|
.alt1() // MCLK
|
|
.mode()
|
|
.inactive()
|
|
.slew()
|
|
.fast()
|
|
.invert()
|
|
.disabled()
|
|
.digimode()
|
|
.digital()
|
|
.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::SYSCON::ptr()
|
|
.as_ref()
|
|
.unwrap()
|
|
.mclkclksel
|
|
.modify(|_, w| w.sel().enum_0x1()); // PLL0
|
|
pac::SYSCON::ptr()
|
|
.as_ref()
|
|
.unwrap()
|
|
.mclkdiv
|
|
.modify(|_, w| w.div().bits(1).halt().run().reset().released()); // div by 2 = PLL0 fout / 2 = 12.288MHz, max for WM8904 @ 96k
|
|
pac::SYSCON::ptr()
|
|
.as_ref()
|
|
.unwrap()
|
|
.mclkio
|
|
.modify(|_, w| w.mclkio().output());
|
|
}
|
|
|
|
// Select I2S TX function
|
|
fc7.pselid.write(|w| w.persel().i2s_transmit());
|
|
|
|
let regs = i2s7;
|
|
I2sTx { i2s: regs }
|
|
}
|
|
|
|
#[entry]
|
|
fn main() -> ! {
|
|
let hal = hal::new();
|
|
|
|
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);
|
|
|
|
debug!("start");
|
|
|
|
debug!("iocon");
|
|
let usb0_vbus_pin = pins::Pio0_22::take()
|
|
.unwrap()
|
|
.into_usb0_vbus_pin(&mut iocon);
|
|
|
|
#[cfg(not(feature = "evk"))]
|
|
let codec_i2c_pins = (
|
|
pins::Pio0_16::take().unwrap().into_i2c4_scl_pin(&mut iocon),
|
|
pins::Pio0_5::take().unwrap().into_i2c4_sda_pin(&mut iocon),
|
|
);
|
|
#[cfg(feature = "evk")]
|
|
let codec_i2c_pins = (
|
|
pins::Pio1_20::take().unwrap().into_i2c4_scl_pin(&mut iocon),
|
|
pins::Pio1_21::take().unwrap().into_i2c4_sda_pin(&mut iocon),
|
|
);
|
|
let codec_i2s_pins = (
|
|
pins::Pio0_21::take().unwrap().into_spi7_sck_pin(&mut iocon),
|
|
pins::Pio0_20::take().unwrap().into_i2s7_sda_pin(&mut iocon),
|
|
pins::Pio0_19::take().unwrap().into_i2s7_ws_pin(&mut iocon),
|
|
pins::Pio0_23::take().unwrap(),
|
|
);
|
|
let codec_gpio_pins = CodecPins {
|
|
reset: pins::Pio0_3::take()
|
|
.unwrap()
|
|
.into_gpio_pin(&mut iocon, &mut gpio)
|
|
.into_output_low(),
|
|
};
|
|
let clock_sel_pins = ClockSelPins {
|
|
sel_24m: pins::Pio0_27::take()
|
|
.unwrap()
|
|
.into_gpio_pin(&mut iocon, &mut gpio)
|
|
.into_output_low(),
|
|
sel_22m: pins::Pio0_31::take()
|
|
.unwrap()
|
|
.into_gpio_pin(&mut iocon, &mut gpio)
|
|
.into_output_low(),
|
|
};
|
|
|
|
hw::init_leds(&mut iocon, &mut gpio);
|
|
|
|
// iocon.disabled(&mut syscon).release(); // save the environment :)
|
|
|
|
debug!("clocks");
|
|
let clocks = hal::ClockRequirements::default()
|
|
.system_frequency(96.MHz())
|
|
.configure(&mut anactrl, &mut pmc, &mut syscon)
|
|
.unwrap();
|
|
hw::init_sys_pll1();
|
|
#[cfg(feature = "evk")]
|
|
hw::init_audio_pll();
|
|
|
|
let mut delay_timer = Timer::new(
|
|
hal.ctimer
|
|
.0
|
|
.enabled(&mut syscon, clocks.support_1mhz_fro_token().unwrap()),
|
|
);
|
|
|
|
debug!("peripherals");
|
|
|
|
let i2c_peripheral = hal
|
|
.flexcomm
|
|
.4
|
|
.enabled_as_i2c(&mut syscon, &clocks.support_flexcomm_token().unwrap());
|
|
|
|
let i2c_bus = I2cMaster::new(
|
|
i2c_peripheral,
|
|
codec_i2c_pins,
|
|
Hertz::try_from(400.kHz()).unwrap(),
|
|
);
|
|
let dac_impl = DacImpl::new(i2c_bus, codec_gpio_pins);
|
|
|
|
let i2s_peripheral = {
|
|
let fc7 = hal.flexcomm.7.release();
|
|
init_i2s(fc7.0, fc7.2, &mut syscon)
|
|
};
|
|
|
|
let usb_peripheral = hal.usbhs.enabled_as_device(
|
|
&mut anactrl,
|
|
&mut pmc,
|
|
&mut syscon,
|
|
&mut delay_timer,
|
|
clocks.support_usbhs_token().unwrap(),
|
|
);
|
|
|
|
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_ref = DMA_RING.init(dma);
|
|
unsafe { DMA_RING_REF = 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(),
|
|
alt_setting: 0,
|
|
nodata_timeout_frame: AtomicUsize::new(0),
|
|
cur_rate: SAMPLE_RATE,
|
|
clock_pins: clock_sel_pins,
|
|
_marker: core::marker::PhantomData,
|
|
};
|
|
audio.init();
|
|
|
|
let usb_bus = UsbBus::new(usb_peripheral, usb0_vbus_pin);
|
|
|
|
let config = UsbAudioClassConfig::new(UsbSpeed::High, FunctionCode::Other, &mut audio)
|
|
.with_output_config(
|
|
TerminalConfig::builder()
|
|
.base_id(2)
|
|
.terminal_type(TerminalType::UsbUndefined)
|
|
.build(),
|
|
);
|
|
let mut uac2 = config.build(&usb_bus).unwrap();
|
|
#[cfg(feature = "hid")]
|
|
let mut hid = HIDClass::new_ep_in(&usb_bus, AudioTelemetryReport::desc(), HID_INTERVAL_MS);
|
|
|
|
let mut usb_dev = usbd_uac2::builder(&usb_bus, UsbVidPid(0x1209, 0xcc1d))
|
|
.strings(&[StringDescriptors::default()
|
|
.manufacturer("VE7XEN")
|
|
.product("Guac Tortilla")])
|
|
.unwrap()
|
|
.max_packet_size_0(64)
|
|
.unwrap()
|
|
.build();
|
|
|
|
#[cfg(feature = "hid")]
|
|
let mut poll_all = {
|
|
let mut hid_update_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));
|
|
|
|
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));
|
|
}
|
|
}
|
|
};
|
|
#[cfg(not(feature = "hid"))]
|
|
let poll_all = || {
|
|
usb_dev.poll(&mut [&mut uac2]);
|
|
};
|
|
|
|
defmt::info!("main loop");
|
|
|
|
loop {
|
|
poll_all();
|
|
// usb_dev.poll(&mut [&mut uac2]);
|
|
}
|
|
}
|