blackbox-rs 0.1.0

//! Stock Blackbox audio bring-up: SAI1_A + SAI1_B + SAI2_A, raw registers.
//!
//! Ported from the recovered stock firmware (README §SAI). The HAL can't express the
//! external-sync OLM topology, so the SAI blocks + DMA streams are programmed with the
//! bit-exact register values read back from working silicon.
//!
//! Topology:
//!   * SAI1_A: master TX → CS42528 CX_SDIN1, OLM #1 DAC1-6 packed 20-bit
//!   * SAI2_A: slave TX (external sync from SAI1) → CX_SDIN4, DAC7-8
//!   * SAI1_B: slave RX (internal sync from SAI1_A) → CX_SDOUT, ADC1-2
//!
//! DMA2 stream1 (master TX) raises HT/TC; our `audio_dma_isr` ping-pong refills the freed
//! half of the TX buffer. embassy owns every DMA `#[interrupt]` vector, so we relocate the
//! vector table to RAM and patch IRQ 57 → our handler. Streams 4/5 run circular, serviced
//! inside the stream-1 ISR; their NVIC lines stay masked.
//!
//! Buffers live in D2 SRAM1 (0x3000_0000), DMA-reachable from DMA2's domain. D-cache is off,
//! so they're coherent without maintenance; the MPU still marks the region non-cacheable.

use core::ptr;
use core::sync::atomic::{AtomicU32, AtomicU8, Ordering};

use embassy_stm32::gpio::Output as GpioOut;
use embassy_stm32::i2c::{I2c, Master};
use embassy_stm32::mode::Blocking;
use embassy_stm32::pac;
use embassy_time::Timer;

const CODEC_ADDR: u8 = 0x4C;

/// CS42528 bring-up over I2C1. Control port acks without MCLK; clock/I²S regs take audible
/// effect once SAI feeds 12.288 MHz MCLK (so call before `bringup`).
pub async fn init_codec(i2c: &mut I2c<'_, Blocking, Master>, reset: &mut GpioOut<'_>) -> bool {
    reset.set_low(); // PG13 active-low reset
    Timer::after_millis(10).await;
    reset.set_high();
    Timer::after_millis(20).await;

    // README stock boot sequence + unmute (0x0E); stock leaves channels muted.
    let seq: [[u8; 2]; 6] = [
        [0x02, 0x80], // Power Control
        [0x03, 0x08], // Functional Mode
        [0x04, 0x44], // Interface Format: I²S, DAC OLM #1 20-bit
        [0x05, 0x04], // Misc Control
        [0x06, 0x02], // Clock Control: MCLK from OMCK, 12.288 MHz
        [0x0E, 0x00], // unmute all DAC channels
    ];
    let mut ok = true;
    for reg in seq {
        ok &= i2c.blocking_write(CODEC_ADDR, &reg).is_ok();
    }
    ok
}

const DMA2_STREAM1_IRQ: usize = 57;

const TX_WORDS: usize = 0x400;
const RX_WORDS: usize = 0x200;
const OLM_WORDS_PER_FRAME: usize = 4;
/// Frames refilled per interrupt (one half-buffer).
pub const FRAMES_PER_REFILL: usize = (TX_WORDS / 2) / OLM_WORDS_PER_FRAME;
const FULL_SCALE_20BIT: f32 = 0x0007_FFFF as f32;

const SAMPLE_RATE: u32 = 48_000;

const SAI1_A_DR: u32 = 0x4001_5820;
const SAI1_B_DR: u32 = 0x4001_5840;
const SAI2_A_DR: u32 = 0x4001_5C20;

// D2 SRAM1: TX1 4 KB, RX 2 KB, TX2 2 KB.
const AUDIO_TX1: *mut u32 = 0x3000_0000 as *mut u32;
const AUDIO_RX: *mut u32 = 0x3000_1000 as *mut u32;
const AUDIO_TX2: *mut u32 = 0x3000_1800 as *mut u32;

const SAI_GCR: u32 = 0x0000_0010;
const SAI_BLOCK_RUN_MASK: u32 = 0x0003_0000; // SAIEN | DMAEN

const DMAMUX_REQ_SAI1_A: u32 = 0x57;
const DMAMUX_REQ_SAI1_B: u32 = 0x58;
const DMAMUX_REQ_SAI2_A: u32 = 0x59;

const DMA_TX_CR: u32 = 0x0003_555F;
const DMA_RX_CR: u32 = 0x0003_551F;
const DMA_TX_FCR: u32 = 0x0000_0088;
const DMA_RX_FCR: u32 = 0x0000_00A0;

#[derive(Clone, Copy)]
struct SaiBlockConfig {
    cr1: u32,
    cr2: u32,
    frcr: u32,
    slotr: u32,
    im: u32,
}
impl SaiBlockConfig {
    const fn disabled_cr1(self) -> u32 {
        self.cr1 & !SAI_BLOCK_RUN_MASK
    }
}

#[derive(Clone, Copy)]
struct DmaStreamConfig {
    stream: usize,
    words: u32,
    peripheral: u32,
    memory: *mut u32,
    fcr: u32,
    cr: u32,
}

const SAI1_A: SaiBlockConfig = SaiBlockConfig { cr1: 0x0013_22E0, cr2: 0x0000_0001, frcr: 0x0002_3F7F, slotr: 0xFFFF_0380, im: 0x0000_0005 };
const SAI1_B: SaiBlockConfig = SaiBlockConfig { cr1: 0x0013_06E3, cr2: 0x0000_0001, frcr: 0x0005_1F3F, slotr: 0xFFFF_0180, im: 0x0000_0061 };
const SAI2_A: SaiBlockConfig = SaiBlockConfig { cr1: 0x0013_2AE2, cr2: 0x0000_0001, frcr: 0x0002_3F7F, slotr: 0xFFFF_0380, im: 0x0000_0061 };

const DMA_SAI1_B_RX: DmaStreamConfig = DmaStreamConfig { stream: 5, words: RX_WORDS as u32, peripheral: SAI1_B_DR, memory: AUDIO_RX, fcr: DMA_RX_FCR, cr: DMA_RX_CR };
const DMA_SAI2_A_TX: DmaStreamConfig = DmaStreamConfig { stream: 4, words: TX_WORDS as u32, peripheral: SAI2_A_DR, memory: AUDIO_TX2, fcr: DMA_TX_FCR, cr: DMA_TX_CR };
const DMA_SAI1_A_TX: DmaStreamConfig = DmaStreamConfig { stream: 1, words: TX_WORDS as u32, peripheral: SAI1_A_DR, memory: AUDIO_TX1, fcr: DMA_TX_FCR, cr: DMA_TX_CR };

const DAC_CHANNELS: usize = 8;

/// A CS42528 DAC output channel. DAC1-6 stream on SAI1_A (SDIN1), DAC7-8 on SAI2_A (SDIN4).
#[derive(Clone, Copy, PartialEq, Eq, defmt::Format)]
pub enum Dac {
    Dac1,
    Dac2,
    Dac3,
    Dac4,
    Dac5,
    Dac6,
    Dac7,
    Dac8,
}

impl Dac {
    pub const ALL: [Dac; DAC_CHANNELS] = [
        Dac::Dac1, Dac::Dac2, Dac::Dac3, Dac::Dac4, Dac::Dac5, Dac::Dac6, Dac::Dac7, Dac::Dac8,
    ];

    pub const fn index(self) -> usize {
        self as usize
    }
    pub const fn label(self) -> &'static str {
        match self {
            Dac::Dac1 => "DAC1",
            Dac::Dac2 => "DAC2",
            Dac::Dac3 => "DAC3",
            Dac::Dac4 => "DAC4",
            Dac::Dac5 => "DAC5",
            Dac::Dac6 => "DAC6",
            Dac::Dac7 => "DAC7",
            Dac::Dac8 => "DAC8",
        }
    }
}

/// A stereo output port, aliasing its two DAC channels.
///
/// Mapping (8 DACs → 4 stereo pairs): Phones = DAC1/2, Jack1 = DAC3/4, Jack2 = DAC5/6,
/// Jack3 = DAC7/8. **Assumed** — the recovered dump doesn't pin the jack wiring; adjust
/// `left`/`right` if the hardware says otherwise.
#[derive(Clone, Copy, PartialEq, Eq, defmt::Format)]
pub enum Output {
    Phones,
    Jack1,
    Jack2,
    Jack3,
}

impl Output {
    pub const ALL: [Output; 4] = [Output::Phones, Output::Jack1, Output::Jack2, Output::Jack3];

    pub const fn label(self) -> &'static str {
        match self {
            Output::Phones => "PHONES",
            Output::Jack1 => "JACK1",
            Output::Jack2 => "JACK2",
            Output::Jack3 => "JACK3",
        }
    }

    pub const fn left(self) -> Dac {
        match self {
            Output::Phones => Dac::Dac1,
            Output::Jack1 => Dac::Dac3,
            Output::Jack2 => Dac::Dac5,
            Output::Jack3 => Dac::Dac7,
        }
    }

    pub const fn right(self) -> Dac {
        match self {
            Output::Phones => Dac::Dac2,
            Output::Jack1 => Dac::Dac4,
            Output::Jack2 => Dac::Dac6,
            Output::Jack3 => Dac::Dac8,
        }
    }

    /// The two DAC channels of this port: `[left, right]`.
    pub const fn channels(self) -> [Dac; 2] {
        [self.left(), self.right()]
    }
}

/// A CS42528 ADC input channel (captured on SAI1_B RX). No read path is wired yet — present
/// so callers can name the inputs.
#[derive(Clone, Copy, PartialEq, Eq, defmt::Format)]
pub enum AudioIn {
    Adc1,
    Adc2,
}

// ---- sine source (replaces the stock render engine) ----
static PHASE: AtomicU32 = AtomicU32::new(0);
static mut SINE_LUT: [i32; 256] = [0; 256];
static PHASE_INC: AtomicU32 = AtomicU32::new(0);
/// Bit `n` set → the sine is rendered onto `Dac::ALL[n]`.
static OUTPUT_MASK: AtomicU8 = AtomicU8::new(0);

/// DMA2_STREAM1 ISR: clear ALL five stream-1 flags (level-sensitive line storms otherwise),
/// then refill the half the DMA just finished reading. HT = first half drained, TC = second.
extern "C" fn audio_dma_isr() {
    let lisr = pac::DMA2.isr(0).read();
    pac::DMA2.ifcr(0).write(|w| {
        w.set_tcif(1, true);
        w.set_htif(1, true);
        w.set_teif(1, true);
        w.set_dmeif(1, true);
        w.set_feif(1, true);
    });
    if lisr.htif(1) {
        fill_half(0);
    }
    if lisr.tcif(1) {
        fill_half(1);
    }
}

/// Render one half-buffer of sine onto every DAC selected in `OUTPUT_MASK` — DAC1-6 into
/// TX1 (SDIN1), DAC7-8 into TX2 (SDIN4). Unselected channels stay silent.
fn fill_half(half: usize) {
    let lut = unsafe { &*ptr::addr_of!(SINE_LUT) };
    let inc = PHASE_INC.load(Ordering::Relaxed);
    let mask = OUTPUT_MASK.load(Ordering::Relaxed);
    let mut phase = PHASE.load(Ordering::Relaxed);
    let base = half * (TX_WORDS / 2);
    for f in 0..FRAMES_PER_REFILL {
        let s = lut[(phase >> 24) as usize & 0xFF];
        phase = phase.wrapping_add(inc);
        let mut d = [0i32; DAC_CHANNELS];
        for (ch, slot) in d.iter_mut().enumerate() {
            if mask & (1 << ch) != 0 {
                *slot = s;
            }
        }
        let w1 = pack_dac6(&d);
        let w2 = pack_dac2(&d);
        let idx = base + f * OLM_WORDS_PER_FRAME;
        for k in 0..OLM_WORDS_PER_FRAME {
            // SAFETY: AUDIO_TX1/TX2 are reserved non-cacheable D2 SRAM1.
            unsafe {
                ptr::write_volatile(AUDIO_TX1.add(idx + k), w1[k]);
                ptr::write_volatile(AUDIO_TX2.add(idx + k), w2[k]);
            }
        }
    }
    PHASE.store(phase, Ordering::Relaxed);
    cortex_m::asm::dsb(); // retire writes before the DMA reads them
}

/// Bring up the SAI/codec audio path and play a sine on the phones (DAC1/2).
/// Call [`init_codec`] first so the codec is configured before MCLK arrives.
pub fn play_sine(freq_hz: u32) {
    play_sine_out(freq_hz, Output::Phones);
}

/// Like [`play_sine`] but routes the tone to a stereo [`Output`] port (Phones / Jack1-3).
pub fn play_sine_out(freq_hz: u32, out: Output) {
    play_sine_on(freq_hz, &out.channels());
}

/// Like [`play_sine`] but routes the tone to a chosen set of DAC channels.
pub fn play_sine_on(freq_hz: u32, dacs: &[Dac]) {
    let mut mask = 0u8;
    for d in dacs {
        mask |= 1 << d.index();
    }
    OUTPUT_MASK.store(mask, Ordering::Relaxed);
    bringup(freq_hz);
    unmask_master_irq(); // arm the master-TX ISR last → tone starts
}

fn bringup(freq_hz: u32) {
    mask_master_irq();

    // Build the sine LUT (half-scale headroom) + phase step.
    let lut = unsafe { &mut *ptr::addr_of_mut!(SINE_LUT) };
    let mut i = 0;
    while i < 256 {
        let ph = i as f32 / 256.0 * core::f32::consts::TAU;
        lut[i] = (libm::sinf(ph) * 0.5 * FULL_SCALE_20BIT) as i32;
        i += 1;
    }
    PHASE_INC.store((((freq_hz as u64) << 32) / SAMPLE_RATE as u64) as u32, Ordering::Relaxed);

    // D2 SRAM1/2 clock — gated off at reset (README); read back to order it.
    pac::RCC.ahb2enr().modify(|w| {
        w.set_sram1en(true);
        w.set_sram2en(true);
    });
    let _ = pac::RCC.ahb2enr().read();

    // Kernel clocks: SAI1 + SAI2/3 ← PLL2_P (12.288 MHz).
    pac::RCC.d2ccip1r().modify(|w| {
        w.set_sai1sel(pac::rcc::vals::Saisel::PLL2_P);
        w.set_sai23sel(pac::rcc::vals::Saisel::PLL2_P);
    });

    enable_and_reset(|v| pac::RCC.apb2enr().modify(|w| w.set_sai1en(v)), |v| pac::RCC.apb2rstr().modify(|w| w.set_sai1rst(v)));
    enable_and_reset(|v| pac::RCC.apb2enr().modify(|w| w.set_sai2en(v)), |v| pac::RCC.apb2rstr().modify(|w| w.set_sai2rst(v)));
    enable_and_reset(|v| pac::RCC.ahb1enr().modify(|w| w.set_dma2en(v)), |v| pac::RCC.ahb1rstr().modify(|w| w.set_dma2rst(v)));

    // Pins: SAI1 PE2 MCLK_A, PE5 SCK_A, PE4 FS_A, PB2 SD_A, PE3 SD_B — AF6; SAI2 PD11/12/13 — AF10.
    config_sai_pin(pac::GPIOE, 2, 6);
    config_sai_pin(pac::GPIOE, 5, 6);
    config_sai_pin(pac::GPIOE, 4, 6);
    config_sai_pin(pac::GPIOB, 2, 6);
    config_sai_pin(pac::GPIOE, 3, 6);
    config_sai_pin(pac::GPIOD, 11, 10);
    config_sai_pin(pac::GPIOD, 12, 10);
    config_sai_pin(pac::GPIOD, 13, 10);

    init_audio_buffers();

    use pac::{dma, dmamux, sai};
    pac::SAI1.gcr().write_value(sai::regs::Gcr(SAI_GCR));
    pac::SAI2.gcr().write_value(sai::regs::Gcr(SAI_GCR));

    // Program all blocks disabled first.
    configure_sai_block(pac::SAI1.ch(0), SAI1_A, false);
    configure_sai_block(pac::SAI1.ch(1), SAI1_B, false);
    configure_sai_block(pac::SAI2.ch(0), SAI2_A, false);

    pac::DMA2.st(1).cr().write_value(dma::regs::Cr(0));
    pac::DMA2.st(4).cr().write_value(dma::regs::Cr(0));
    pac::DMA2.st(5).cr().write_value(dma::regs::Cr(0));
    while pac::DMA2.st(1).cr().read().en() || pac::DMA2.st(4).cr().read().en() || pac::DMA2.st(5).cr().read().en() {}

    pac::DMA2.ifcr(0).write_value(dma::regs::Ixr(0xFFFF_FFFF));
    pac::DMA2.ifcr(1).write_value(dma::regs::Ixr(0xFFFF_FFFF));

    // DMAMUX1 ch 8-15 feed DMA2; stock maps stream1/4/5 → ch 9/12/13.
    pac::DMAMUX1.ccr(9).write_value(dmamux::regs::Ccr(DMAMUX_REQ_SAI1_A));
    pac::DMAMUX1.ccr(12).write_value(dmamux::regs::Ccr(DMAMUX_REQ_SAI2_A));
    pac::DMAMUX1.ccr(13).write_value(dmamux::regs::Ccr(DMAMUX_REQ_SAI1_B));

    configure_dma_stream(DMA_SAI1_B_RX);
    configure_dma_stream(DMA_SAI2_A_TX);
    configure_dma_stream(DMA_SAI1_A_TX);

    // Enable blocks: slaves first, master last (master starts the clocks).
    configure_sai_block(pac::SAI1.ch(1), SAI1_B, true);
    configure_sai_block(pac::SAI2.ch(0), SAI2_A, true);
    configure_sai_block(pac::SAI1.ch(0), SAI1_A, true);

    install_isr();

    // embassy enabled the NVIC line for every DMA stream; disable all DMA2 lines so the
    // unserviced ones don't storm. Master (57) re-enabled by unmask_master_irq() last.
    // H743 vector table isn't contiguous: DMA2_STREAM0..4 = IRQ 56..60, STREAM5..7 = 68..70.
    const NVIC_ICER1: *mut u32 = 0xE000_E184 as *mut u32;
    const NVIC_ICER2: *mut u32 = 0xE000_E188 as *mut u32;
    let icer1 = (1u32 << (56 - 32)) | (1 << (57 - 32)) | (1 << (58 - 32)) | (1 << (59 - 32)) | (1 << (60 - 32));
    let icer2 = (1u32 << (68 - 64)) | (1 << (69 - 64)) | (1 << (70 - 64));
    unsafe {
        ptr::write_volatile(NVIC_ICER1, icer1);
        ptr::write_volatile(NVIC_ICER2, icer2);
    }
    cortex_m::asm::dsb();

    defmt::info!("sai: stock topology up — SAI1_A DAC1-6 + SAI2_A DAC7-8 + SAI1_B RX, {} Hz", SAMPLE_RATE);
}

/// Enable the master-TX DMA IRQ (call last, once the LUT/phase are ready).
pub fn unmask_master_irq() {
    const NVIC_ISER1: *mut u32 = 0xE000_E104 as *mut u32;
    unsafe { ptr::write_volatile(NVIC_ISER1, 1 << (DMA2_STREAM1_IRQ - 32)) };
}

fn mask_master_irq() {
    const NVIC_ICER1: *mut u32 = 0xE000_E184 as *mut u32;
    unsafe { ptr::write_volatile(NVIC_ICER1, 1 << (DMA2_STREAM1_IRQ - 32)) };
    cortex_m::asm::dsb();
    cortex_m::asm::isb();
}

// Relocate the vector table to RAM and patch IRQ 57 → audio_dma_isr (embassy owns the
// flash-resident DMA vectors, so we can't use a cortex-m-rt #[interrupt]).
#[repr(align(1024))]
struct VectorTable([u32; 256]);
static mut RAM_VTABLE: VectorTable = VectorTable([0; 256]);

fn install_isr() {
    let scb = unsafe { &*cortex_m::peripheral::SCB::PTR };
    let src = scb.vtor.read() as *const u32; // current table base (flash)
    let table = unsafe { &mut *ptr::addr_of_mut!(RAM_VTABLE) };
    for i in 0..256 {
        table.0[i] = unsafe { ptr::read_volatile(src.add(i)) };
    }
    table.0[16 + DMA2_STREAM1_IRQ] = audio_dma_isr as usize as u32;
    cortex_m::asm::dsb();
    unsafe { scb.vtor.write(table.0.as_ptr() as u32) };
    cortex_m::asm::dsb();
    cortex_m::asm::isb();
}

fn config_sai_pin(port: pac::gpio::Gpio, pin: usize, af: u8) {
    use pac::gpio::vals;
    port.afr(pin / 8).modify(|w| w.set_afr(pin % 8, af));
    port.ospeedr().modify(|w| w.set_ospeedr(pin, vals::Ospeedr::MEDIUM_SPEED));
    port.pupdr().modify(|w| w.set_pupdr(pin, vals::Pupdr::FLOATING));
    port.otyper().modify(|w| w.set_ot(pin, vals::Ot::PUSH_PULL));
    port.moder().modify(|w| w.set_moder(pin, vals::Moder::ALTERNATE));
}

fn enable_and_reset(enable: impl Fn(bool), reset: impl Fn(bool)) {
    enable(true);
    let _ = pac::RCC.ahb1enr().read(); // settle
    reset(true);
    reset(false);
}

fn init_audio_buffers() {
    for i in 0..RX_WORDS {
        unsafe { ptr::write_volatile(AUDIO_RX.add(i), 0) };
    }
    for i in 0..TX_WORDS {
        unsafe {
            ptr::write_volatile(AUDIO_TX1.add(i), 0);
            ptr::write_volatile(AUDIO_TX2.add(i), 0);
        }
    }
}

/// Pack DAC1-6 into the SDIN1 OLM frame. Slot order is DAC1,3,5 then DAC2,4,6.
fn pack_dac6(s: &[i32; DAC_CHANNELS]) -> [u32; 4] {
    let mut w = [0u32; 4];
    let m = |x: i32| (x as u32) & 0x000F_FFFF;
    put_20(&mut w, 0, m(s[0]));
    put_20(&mut w, 20, m(s[2]));
    put_20(&mut w, 40, m(s[4]));
    put_20(&mut w, 64, m(s[1]));
    put_20(&mut w, 84, m(s[3]));
    put_20(&mut w, 104, m(s[5]));
    w
}

/// Pack DAC7 (`s[6]`) and DAC8 (`s[7]`) into the SDIN4 OLM frame.
fn pack_dac2(s: &[i32; DAC_CHANNELS]) -> [u32; 4] {
    let mut w = [0u32; 4];
    let m = |x: i32| (x as u32) & 0x000F_FFFF;
    put_20(&mut w, 0, m(s[6])); // DAC7
    put_20(&mut w, 64, m(s[7])); // DAC8
    w
}

/// Place a masked 20-bit sample at `start_bit` in the 128-bit MSB-first OLM frame.
fn put_20(words: &mut [u32; 4], start_bit: usize, sample: u32) {
    let w = start_bit / 32;
    let off = start_bit % 32;
    if off <= 12 {
        words[w] |= sample << (12 - off);
    } else {
        words[w] |= sample >> (off - 12);
        words[w + 1] |= sample << (44 - off);
    }
}

fn configure_sai_block(block: pac::sai::Ch, config: SaiBlockConfig, enable: bool) {
    use pac::sai::regs;
    let cr1 = if enable { config.cr1 } else { config.disabled_cr1() };
    block.cr1().write_value(regs::Cr1(cr1));
    block.cr2().write_value(regs::Cr2(config.cr2));
    block.frcr().write_value(regs::Frcr(config.frcr));
    block.slotr().write_value(regs::Slotr(config.slotr));
    block.im().write_value(regs::Im(config.im));
}

fn configure_dma_stream(config: DmaStreamConfig) {
    use pac::dma::regs;
    let st = pac::DMA2.st(config.stream);
    st.ndtr().write_value(regs::Ndtr(config.words));
    st.par().write_value(config.peripheral);
    st.m0ar().write_value(config.memory as u32);
    st.m1ar().write_value(0);
    st.fcr().write_value(regs::Fcr(config.fcr));
    st.cr().write_value(regs::Cr(config.cr));
}