Stabilizer asynchronous batch sampling support

Cargo.toml

@@ -54,7 +54,8 @@ path = "ad9959"
 
 [dependencies.stm32h7xx-hal]
 features = ["stm32h743v", "rt", "unproven", "ethernet", "quadspi"]
-git = "https://github.com/stm32-rs/stm32h7xx-hal"
+git = "https://github.com/quartiq/stm32h7xx-hal"
+branch = "feature/stabilizer-dma"
 
 [features]
 semihosting = ["panic-semihosting", "cortex-m-log/semihosting"]
@@ -64,7 +65,7 @@ nightly = ["cortex-m/inline-asm"]
 [profile.dev]
 codegen-units = 1
 incremental = false
-opt-level = 3
+opt-level = 1
 
 [profile.release]
 opt-level = 3

openocd.gdb

@@ -26,3 +26,6 @@ set var $t0=*$cc
 continue
 end
 #set var $t0=*$cc
+
+source ../../PyCortexMDebug/cmdebug/svd_gdb.py
+svd_load ~/Downloads/STM32H743x.svd

src/adc.rs

@@ -0,0 +1,227 @@
+use super::{
+    hal, DMAReq, DmaConfig, MemoryToPeripheral, PeripheralToMemory, Priority,
+    Stream, TargetAddress, Transfer,
+};
+
+const INPUT_BUFFER_SIZE: usize = 1;
+
+#[link_section = ".axisram.buffers"]
+static mut SPI_START: [u16; 1] = [0x00];
+
+#[link_section = ".axisram.buffers"]
+static mut ADC0_BUF0: [u16; INPUT_BUFFER_SIZE] = [0; INPUT_BUFFER_SIZE];
+
+#[link_section = ".axisram.buffers"]
+static mut ADC0_BUF1: [u16; INPUT_BUFFER_SIZE] = [0; INPUT_BUFFER_SIZE];
+
+#[link_section = ".axisram.buffers"]
+static mut ADC1_BUF0: [u16; INPUT_BUFFER_SIZE] = [0; INPUT_BUFFER_SIZE];
+
+#[link_section = ".axisram.buffers"]
+static mut ADC1_BUF1: [u16; INPUT_BUFFER_SIZE] = [0; INPUT_BUFFER_SIZE];
+
+struct SPI2 {}
+
+impl SPI2 {
+    pub fn new() -> Self {
+        Self {}
+    }
+}
+
+unsafe impl TargetAddress<MemoryToPeripheral> for SPI2 {
+    type MemSize = u16;
+
+    const REQUEST_LINE: Option<u8> = Some(DMAReq::TIM2_UP as u8);
+
+    fn address(&self) -> u32 {
+        let regs = unsafe { &*hal::stm32::SPI2::ptr() };
+        &regs.txdr as *const _ as u32
+    }
+}
+
+struct SPI3 {}
+
+impl SPI3 {
+    pub fn new() -> Self {
+        Self {}
+    }
+}
+
+unsafe impl TargetAddress<MemoryToPeripheral> for SPI3 {
+    type MemSize = u16;
+
+    const REQUEST_LINE: Option<u8> = Some(DMAReq::TIM2_UP as u8);
+
+    fn address(&self) -> u32 {
+        let regs = unsafe { &*hal::stm32::SPI3::ptr() };
+        &regs.txdr as *const _ as u32
+    }
+}
+
+pub struct AdcInputs {
+    adc0: Adc0Input,
+    adc1: Adc1Input,
+}
+
+impl AdcInputs {
+    pub fn new(adc0: Adc0Input, adc1: Adc1Input) -> Self {
+        Self { adc0, adc1 }
+    }
+
+    pub fn transfer_complete_handler(
+        &mut self,
+    ) -> (&[u16; INPUT_BUFFER_SIZE], &[u16; INPUT_BUFFER_SIZE]) {
+        let adc0_buffer = self.adc0.transfer_complete_handler();
+        let adc1_buffer = self.adc1.transfer_complete_handler();
+        (adc0_buffer, adc1_buffer)
+    }
+}
+
+pub struct Adc0Input {
+    next_buffer: Option<&'static mut [u16; INPUT_BUFFER_SIZE]>,
+    transfer: Transfer<
+        hal::dma::dma::Stream1<hal::stm32::DMA1>,
+        hal::spi::Spi<hal::stm32::SPI2, hal::spi::Disabled, u16>,
+        PeripheralToMemory,
+        &'static mut [u16; INPUT_BUFFER_SIZE],
+    >,
+}
+
+impl Adc0Input {
+    pub fn new(
+        spi: hal::spi::Spi<hal::stm32::SPI2, hal::spi::Enabled, u16>,
+        trigger_stream: hal::dma::dma::Stream0<hal::stm32::DMA1>,
+        data_stream: hal::dma::dma::Stream1<hal::stm32::DMA1>,
+    ) -> Self {
+        let trigger_config = DmaConfig::default()
+            .memory_increment(false)
+            .peripheral_increment(false)
+            .priority(Priority::High)
+            .circular_buffer(true);
+
+        let mut trigger_transfer: Transfer<_, _, MemoryToPeripheral, _> =
+            Transfer::init(
+                trigger_stream,
+                &SPI2::new(),
+                unsafe { &mut SPI_START },
+                None,
+                trigger_config,
+            );
+
+        let data_config = DmaConfig::default()
+            .memory_increment(true)
+            .priority(Priority::VeryHigh)
+            .peripheral_increment(false);
+
+        let mut spi = spi.disable();
+        spi.listen(hal::spi::Event::Error);
+
+        let mut data_transfer: Transfer<_, _, PeripheralToMemory, _> =
+            Transfer::init(
+                data_stream,
+                &spi,
+                unsafe { &mut ADC0_BUF0 },
+                None,
+                data_config,
+            );
+
+        spi.enable_dma_rx();
+        spi.enable_dma_tx();
+
+        let spi = spi.enable();
+        spi.inner().cr1.modify(|_, w| w.cstart().started());
+
+        data_transfer.start();
+        trigger_transfer.start();
+
+        Self {
+            next_buffer: unsafe { Some(&mut ADC0_BUF1) },
+            transfer: data_transfer,
+        }
+    }
+
+    pub fn transfer_complete_handler(&mut self) -> &[u16; INPUT_BUFFER_SIZE] {
+        let next_buffer = self.next_buffer.take().unwrap();
+        while hal::dma::dma::Stream1::<hal::stm32::DMA1>::is_enabled() {}
+        self.transfer.clear_interrupts();
+        let (prev_buffer, _) =
+            self.transfer.next_transfer(next_buffer).unwrap();
+        self.next_buffer.replace(prev_buffer);
+        self.next_buffer.as_ref().unwrap()
+    }
+}
+
+pub struct Adc1Input {
+    next_buffer: Option<&'static mut [u16; INPUT_BUFFER_SIZE]>,
+    transfer: Transfer<
+        hal::dma::dma::Stream3<hal::stm32::DMA1>,
+        hal::spi::Spi<hal::stm32::SPI3, hal::spi::Disabled, u16>,
+        PeripheralToMemory,
+        &'static mut [u16; INPUT_BUFFER_SIZE],
+    >,
+}
+
+impl Adc1Input {
+    pub fn new(
+        spi: hal::spi::Spi<hal::stm32::SPI3, hal::spi::Enabled, u16>,
+        trigger_stream: hal::dma::dma::Stream2<hal::stm32::DMA1>,
+        data_stream: hal::dma::dma::Stream3<hal::stm32::DMA1>,
+    ) -> Self {
+        let trigger_config = DmaConfig::default()
+            .memory_increment(false)
+            .peripheral_increment(false)
+            .priority(Priority::High)
+            .circular_buffer(true);
+
+        let mut trigger_transfer: Transfer<_, _, MemoryToPeripheral, _> =
+            Transfer::init(
+                trigger_stream,
+                &SPI3::new(),
+                unsafe { &mut SPI_START },
+                None,
+                trigger_config,
+            );
+
+        let data_config = DmaConfig::default()
+            .memory_increment(true)
+            .transfer_complete_interrupt(true)
+            .priority(Priority::VeryHigh)
+            .peripheral_increment(false);
+
+        let mut spi = spi.disable();
+        spi.listen(hal::spi::Event::Error);
+
+        let mut data_transfer: Transfer<_, _, PeripheralToMemory, _> =
+            Transfer::init(
+                data_stream,
+                &spi,
+                unsafe { &mut ADC1_BUF0 },
+                None,
+                data_config,
+            );
+
+        spi.enable_dma_rx();
+        spi.enable_dma_tx();
+
+        let spi = spi.enable();
+        spi.inner().cr1.modify(|_, w| w.cstart().started());
+
+        data_transfer.start();
+        trigger_transfer.start();
+
+        Self {
+            next_buffer: unsafe { Some(&mut ADC1_BUF1) },
+            transfer: data_transfer,
+        }
+    }
+
+    pub fn transfer_complete_handler(&mut self) -> &[u16; INPUT_BUFFER_SIZE] {
+        let next_buffer = self.next_buffer.take().unwrap();
+        while hal::dma::dma::Stream3::<hal::stm32::DMA1>::is_enabled() {}
+        self.transfer.clear_interrupts();
+        let (prev_buffer, _) =
+            self.transfer.next_transfer(next_buffer).unwrap();
+        self.next_buffer.replace(prev_buffer);
+        self.next_buffer.as_ref().unwrap()
+    }
+}

src/dac.rs

@@ -0,0 +1,62 @@
+use super::hal;
+use heapless::consts;
+
+pub struct DacOutputs {
+    dac0_spi: hal::spi::Spi<hal::stm32::SPI4, hal::spi::Enabled, u16>,
+    dac1_spi: hal::spi::Spi<hal::stm32::SPI5, hal::spi::Enabled, u16>,
+    outputs: heapless::spsc::Queue<(u16, u16), consts::U32>,
+    timer: hal::timer::Timer<hal::stm32::TIM3>,
+}
+
+impl DacOutputs {
+    pub fn new(
+        dac0_spi: hal::spi::Spi<hal::stm32::SPI4, hal::spi::Enabled, u16>,
+        dac1_spi: hal::spi::Spi<hal::stm32::SPI5, hal::spi::Enabled, u16>,
+        mut timer: hal::timer::Timer<hal::stm32::TIM3>,
+    ) -> Self {
+        dac0_spi.inner().cr1.modify(|_, w| w.cstart().started());
+        dac1_spi.inner().cr1.modify(|_, w| w.cstart().started());
+        timer.pause();
+        timer.reset_counter();
+        timer.clear_irq();
+        timer.listen(hal::timer::Event::TimeOut);
+
+        Self {
+            dac0_spi,
+            dac1_spi,
+            outputs: heapless::spsc::Queue::new(),
+            timer,
+        }
+    }
+
+    pub fn push(&mut self, dac0_value: u16, dac1_value: u16) {
+        self.outputs.enqueue((dac0_value, dac1_value)).unwrap();
+        self.timer.resume();
+    }
+
+    pub fn update(&mut self) {
+        self.timer.clear_irq();
+        match self.outputs.dequeue() {
+            Some((dac0, dac1)) => self.write(dac0, dac1),
+            None => {
+                self.timer.pause();
+                self.timer.reset_counter();
+                self.timer.clear_irq();
+            }
+        };
+    }
+
+    pub fn write(&mut self, dac0_value: u16, dac1_value: u16) {
+        unsafe {
+            core::ptr::write_volatile(
+                &self.dac0_spi.inner().txdr as *const _ as *mut u16,
+                dac0_value,
+            );
+
+            core::ptr::write_volatile(
+                &self.dac1_spi.inner().txdr as *const _ as *mut u16,
+                dac1_value,
+            );
+        }
+    }
+}

src/iir.rs

@@ -105,4 +105,14 @@ impl IIR {
         xy[xy.len() / 2] = y0;
         y0
     }
+
+    pub fn update_from_adc_sample(
+        &mut self,
+        sample: u16,
+        state: &mut IIRState,
+    ) -> u16 {
+        let x0 = f32::from(sample as i16);
+        let y0 = self.update(state, x0);
+        y0 as i16 as u16 ^ 0x8000
+    }
 }