diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index ae27f70..f4c6212 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -27,7 +27,6 @@ jobs: command: fmt args: --all -- --check - uses: actions-rs/clippy-check@v1 - continue-on-error: true with: token: ${{ secrets.GITHUB_TOKEN }} diff --git a/src/bin/dual-iir.rs b/src/bin/dual-iir.rs new file mode 100644 index 0000000..fd8f5e4 --- /dev/null +++ b/src/bin/dual-iir.rs @@ -0,0 +1,295 @@ +#![deny(warnings)] +#![no_std] +#![no_main] +#![cfg_attr(feature = "nightly", feature(core_intrinsics))] + +use stm32h7xx_hal as hal; + +#[macro_use] +extern crate log; + +use rtic::cyccnt::{Instant, U32Ext}; + +use heapless::{consts::*, String}; + +use stabilizer::{hardware, server}; + +use dsp::iir; +use hardware::{Adc0Input, Adc1Input, Dac0Output, Dac1Output, AFE0, AFE1}; + +const SCALE: f32 = ((1 << 15) - 1) as f32; + +const TCP_RX_BUFFER_SIZE: usize = 8192; +const TCP_TX_BUFFER_SIZE: usize = 8192; + +// The number of cascaded IIR biquads per channel. Select 1 or 2! +const IIR_CASCADE_LENGTH: usize = 1; + +#[rtic::app(device = stm32h7xx_hal::stm32, peripherals = true, monotonic = rtic::cyccnt::CYCCNT)] +const APP: () = { + struct Resources { + afes: (AFE0, AFE1), + adcs: (Adc0Input, Adc1Input), + dacs: (Dac0Output, Dac1Output), + net_interface: hardware::Ethernet, + + // Format: iir_state[ch][cascade-no][coeff] + #[init([[[0.; 5]; IIR_CASCADE_LENGTH]; 2])] + iir_state: [[iir::IIRState; IIR_CASCADE_LENGTH]; 2], + #[init([[iir::IIR { ba: [1., 0., 0., 0., 0.], y_offset: 0., y_min: -SCALE - 1., y_max: SCALE }; IIR_CASCADE_LENGTH]; 2])] + iir_ch: [[iir::IIR; IIR_CASCADE_LENGTH]; 2], + } + + #[init] + fn init(c: init::Context) -> init::LateResources { + // Configure the microcontroller + let (mut stabilizer, _pounder) = hardware::setup(c.core, c.device); + + // Enable ADC/DAC events + stabilizer.adcs.0.start(); + stabilizer.adcs.1.start(); + stabilizer.dacs.0.start(); + stabilizer.dacs.1.start(); + + // Start sampling ADCs. + stabilizer.adc_dac_timer.start(); + + init::LateResources { + afes: stabilizer.afes, + adcs: stabilizer.adcs, + dacs: stabilizer.dacs, + net_interface: stabilizer.net.interface, + } + } + + /// Main DSP processing routine for Stabilizer. + /// + /// # Note + /// Processing time for the DSP application code is bounded by the following constraints: + /// + /// DSP application code starts after the ADC has generated a batch of samples and must be + /// completed by the time the next batch of ADC samples has been acquired (plus the FIFO buffer + /// time). If this constraint is not met, firmware will panic due to an ADC input overrun. + /// + /// The DSP application code must also fill out the next DAC output buffer in time such that the + /// DAC can switch to it when it has completed the current buffer. If this constraint is not met + /// it's possible that old DAC codes will be generated on the output and the output samples will + /// be delayed by 1 batch. + /// + /// Because the ADC and DAC operate at the same rate, these two constraints actually implement + /// the same time bounds, meeting one also means the other is also met. + #[task(binds=DMA1_STR4, resources=[adcs, dacs, iir_state, iir_ch], priority=2)] + fn process(c: process::Context) { + let adc_samples = [ + c.resources.adcs.0.acquire_buffer(), + c.resources.adcs.1.acquire_buffer(), + ]; + + let dac_samples = [ + c.resources.dacs.0.acquire_buffer(), + c.resources.dacs.1.acquire_buffer(), + ]; + + for channel in 0..adc_samples.len() { + for sample in 0..adc_samples[0].len() { + let x = f32::from(adc_samples[channel][sample] as i16); + let mut y = x; + for i in 0..c.resources.iir_state[channel].len() { + y = c.resources.iir_ch[channel][i] + .update(&mut c.resources.iir_state[channel][i], y); + } + // Note(unsafe): The filter limits ensure that the value is in range. + // The truncation introduces 1/2 LSB distortion. + let y = unsafe { y.to_int_unchecked::() }; + // Convert to DAC code + dac_samples[channel][sample] = y as u16 ^ 0x8000; + } + } + } + + #[idle(resources=[net_interface, iir_state, iir_ch, afes])] + fn idle(mut c: idle::Context) -> ! { + let mut socket_set_entries: [_; 8] = Default::default(); + let mut sockets = + smoltcp::socket::SocketSet::new(&mut socket_set_entries[..]); + + let mut rx_storage = [0; TCP_RX_BUFFER_SIZE]; + let mut tx_storage = [0; TCP_TX_BUFFER_SIZE]; + let tcp_handle = { + let tcp_rx_buffer = + smoltcp::socket::TcpSocketBuffer::new(&mut rx_storage[..]); + let tcp_tx_buffer = + smoltcp::socket::TcpSocketBuffer::new(&mut tx_storage[..]); + let tcp_socket = + smoltcp::socket::TcpSocket::new(tcp_rx_buffer, tcp_tx_buffer); + sockets.add(tcp_socket) + }; + + let mut server = server::Server::new(); + + let mut time = 0u32; + let mut next_ms = Instant::now(); + + // TODO: Replace with reference to CPU clock from CCDR. + next_ms += 400_000.cycles(); + + loop { + let tick = Instant::now() > next_ms; + + if tick { + next_ms += 400_000.cycles(); + time += 1; + } + + { + let socket = + &mut *sockets.get::(tcp_handle); + if socket.state() == smoltcp::socket::TcpState::CloseWait { + socket.close(); + } else if !(socket.is_open() || socket.is_listening()) { + socket + .listen(1235) + .unwrap_or_else(|e| warn!("TCP listen error: {:?}", e)); + } else { + server.poll(socket, |req| { + info!("Got request: {:?}", req); + stabilizer::route_request!(req, + readable_attributes: [ + "stabilizer/iir/state": (|| { + let state = c.resources.iir_state.lock(|iir_state| + server::Status { + t: time, + x0: iir_state[0][0][0], + y0: iir_state[0][0][2], + x1: iir_state[1][0][0], + y1: iir_state[1][0][2], + }); + + Ok::(state) + }), + // "_b" means cascades 2nd IIR + "stabilizer/iir_b/state": (|| { let state = c.resources.iir_state.lock(|iir_state| + server::Status { + t: time, + x0: iir_state[0][IIR_CASCADE_LENGTH-1][0], + y0: iir_state[0][IIR_CASCADE_LENGTH-1][2], + x1: iir_state[1][IIR_CASCADE_LENGTH-1][0], + y1: iir_state[1][IIR_CASCADE_LENGTH-1][2], + }); + + Ok::(state) + }), + "stabilizer/afe0/gain": (|| c.resources.afes.0.get_gain()), + "stabilizer/afe1/gain": (|| c.resources.afes.1.get_gain()) + ], + + modifiable_attributes: [ + "stabilizer/iir0/state": server::IirRequest, (|req: server::IirRequest| { + c.resources.iir_ch.lock(|iir_ch| { + if req.channel > 1 { + return Err(()); + } + + iir_ch[req.channel as usize][0] = req.iir; + + Ok::(req) + }) + }), + "stabilizer/iir1/state": server::IirRequest, (|req: server::IirRequest| { + c.resources.iir_ch.lock(|iir_ch| { + if req.channel > 1 { + return Err(()); + } + + iir_ch[req.channel as usize][0] = req.iir; + + Ok::(req) + }) + }), + "stabilizer/iir_b0/state": server::IirRequest, (|req: server::IirRequest| { + c.resources.iir_ch.lock(|iir_ch| { + if req.channel > 1 { + return Err(()); + } + + iir_ch[req.channel as usize][IIR_CASCADE_LENGTH-1] = req.iir; + + Ok::(req) + }) + }), + "stabilizer/iir_b1/state": server::IirRequest,(|req: server::IirRequest| { + c.resources.iir_ch.lock(|iir_ch| { + if req.channel > 1 { + return Err(()); + } + + iir_ch[req.channel as usize][IIR_CASCADE_LENGTH-1] = req.iir; + + Ok::(req) + }) + }), + "stabilizer/afe0/gain": hardware::AfeGain, (|gain| { + c.resources.afes.0.set_gain(gain); + Ok::<(), ()>(()) + }), + "stabilizer/afe1/gain": hardware::AfeGain, (|gain| { + c.resources.afes.1.set_gain(gain); + Ok::<(), ()>(()) + }) + ] + ) + }); + } + } + + let sleep = match c.resources.net_interface.poll( + &mut sockets, + smoltcp::time::Instant::from_millis(time as i64), + ) { + Ok(changed) => !changed, + Err(smoltcp::Error::Unrecognized) => true, + Err(e) => { + info!("iface poll error: {:?}", e); + true + } + }; + + if sleep { + cortex_m::asm::wfi(); + } + } + } + + #[task(binds = ETH, priority = 1)] + fn eth(_: eth::Context) { + unsafe { hal::ethernet::interrupt_handler() } + } + + #[task(binds = SPI2, priority = 3)] + fn spi2(_: spi2::Context) { + panic!("ADC0 input overrun"); + } + + #[task(binds = SPI3, priority = 3)] + fn spi3(_: spi3::Context) { + panic!("ADC0 input overrun"); + } + + #[task(binds = SPI4, priority = 3)] + fn spi4(_: spi4::Context) { + panic!("DAC0 output error"); + } + + #[task(binds = SPI5, priority = 3)] + fn spi5(_: spi5::Context) { + panic!("DAC1 output error"); + } + + extern "C" { + // hw interrupt handlers for RTIC to use for scheduling tasks + // one per priority + fn DCMI(); + fn JPEG(); + fn SDMMC(); + } +}; diff --git a/src/bin/lockin.rs b/src/bin/lockin.rs new file mode 100644 index 0000000..810b8e5 --- /dev/null +++ b/src/bin/lockin.rs @@ -0,0 +1,326 @@ +#![deny(warnings)] +#![no_std] +#![no_main] +#![cfg_attr(feature = "nightly", feature(core_intrinsics))] + +use stm32h7xx_hal as hal; + +#[macro_use] +extern crate log; + +use rtic::cyccnt::{Instant, U32Ext}; + +use heapless::{consts::*, String}; + +use stabilizer::{hardware, server}; + +use dsp::iir; +use hardware::{Adc0Input, Adc1Input, Dac0Output, Dac1Output, AFE0, AFE1}; + +const SCALE: f32 = ((1 << 15) - 1) as f32; + +const TCP_RX_BUFFER_SIZE: usize = 8192; +const TCP_TX_BUFFER_SIZE: usize = 8192; + +// The number of cascaded IIR biquads per channel. Select 1 or 2! +const IIR_CASCADE_LENGTH: usize = 1; + +#[rtic::app(device = stm32h7xx_hal::stm32, peripherals = true, monotonic = rtic::cyccnt::CYCCNT)] +const APP: () = { + struct Resources { + afes: (AFE0, AFE1), + adcs: (Adc0Input, Adc1Input), + dacs: (Dac0Output, Dac1Output), + net_interface: hardware::Ethernet, + + // Format: iir_state[ch][cascade-no][coeff] + #[init([[[0.; 5]; IIR_CASCADE_LENGTH]; 2])] + iir_state: [[iir::IIRState; IIR_CASCADE_LENGTH]; 2], + #[init([[iir::IIR { ba: [1., 0., 0., 0., 0.], y_offset: 0., y_min: -SCALE - 1., y_max: SCALE }; IIR_CASCADE_LENGTH]; 2])] + iir_ch: [[iir::IIR; IIR_CASCADE_LENGTH]; 2], + } + + #[init] + fn init(c: init::Context) -> init::LateResources { + // Configure the microcontroller + let (mut stabilizer, _pounder) = hardware::setup(c.core, c.device); + + // Enable ADC/DAC events + stabilizer.adcs.0.start(); + stabilizer.adcs.1.start(); + stabilizer.dacs.0.start(); + stabilizer.dacs.1.start(); + + // Start sampling ADCs. + stabilizer.adc_dac_timer.start(); + + init::LateResources { + afes: stabilizer.afes, + adcs: stabilizer.adcs, + dacs: stabilizer.dacs, + net_interface: stabilizer.net.interface, + } + } + + /// Main DSP processing routine for Stabilizer. + /// + /// # Note + /// Processing time for the DSP application code is bounded by the following constraints: + /// + /// DSP application code starts after the ADC has generated a batch of samples and must be + /// completed by the time the next batch of ADC samples has been acquired (plus the FIFO buffer + /// time). If this constraint is not met, firmware will panic due to an ADC input overrun. + /// + /// The DSP application code must also fill out the next DAC output buffer in time such that the + /// DAC can switch to it when it has completed the current buffer. If this constraint is not met + /// it's possible that old DAC codes will be generated on the output and the output samples will + /// be delayed by 1 batch. + /// + /// Because the ADC and DAC operate at the same rate, these two constraints actually implement + /// the same time bounds, meeting one also means the other is also met. + #[task(binds=DMA1_STR4, resources=[adcs, dacs, iir_state, iir_ch], priority=2)] + fn process(c: process::Context) { + let adc_samples = [ + c.resources.adcs.0.acquire_buffer(), + c.resources.adcs.1.acquire_buffer(), + ]; + + let dac_samples = [ + c.resources.dacs.0.acquire_buffer(), + c.resources.dacs.1.acquire_buffer(), + ]; + + let iir_lockin = c.resources.iir_lockin; + let iir_state_lockin = c.resources.iir_state_lockin; + let iir_ch = c.resources.iir_ch; + let iir_state = c.resources.iir_state; + + let (pll_phase, pll_frequency) = c + .resources + .timestamp_handler + .update(c.resources.input_stamper.latest_timestamp()); + let frequency = pll_frequency.wrapping_mul(HARMONIC); + let mut phase = + PHASE_OFFSET.wrapping_add(pll_phase.wrapping_mul(HARMONIC)); + + for i in 0..adc_samples[0].len() { + let m = cossin((phase as i32).wrapping_neg()); + phase = phase.wrapping_add(frequency); + + let signal = Complex( + iir_lockin.update( + &mut iir_state_lockin[0], + ((adc_samples[0][i] as i64 * m.0 as i64) >> 16) as _, + ), + iir_lockin.update( + &mut iir_state_lockin[1], + ((adc_samples[0][i] as i64 * m.1 as i64) >> 16) as _, + ), + ); + + let mut magnitude = + (signal.0 * signal.0 + signal.1 * signal.1) as _; + let mut phase = atan2(signal.1, signal.0) as _; + + for j in 0..iir_state[0].len() { + magnitude = + iir_ch[0][j].update(&mut iir_state[0][j], magnitude); + phase = iir_ch[1][j].update(&mut iir_state[1][j], phase); + } + + // Note(unsafe): range clipping to i16 is ensured by IIR filters above. + unsafe { + dac_samples[0][i] = + magnitude.to_int_unchecked::() as u16 ^ 0x8000; + dac_samples[1][i] = + phase.to_int_unchecked::() as u16 ^ 0x8000; + } + } + } + + #[idle(resources=[net_interface, iir_state, iir_ch, afes])] + fn idle(mut c: idle::Context) -> ! { + let mut socket_set_entries: [_; 8] = Default::default(); + let mut sockets = + smoltcp::socket::SocketSet::new(&mut socket_set_entries[..]); + + let mut rx_storage = [0; TCP_RX_BUFFER_SIZE]; + let mut tx_storage = [0; TCP_TX_BUFFER_SIZE]; + let tcp_handle = { + let tcp_rx_buffer = + smoltcp::socket::TcpSocketBuffer::new(&mut rx_storage[..]); + let tcp_tx_buffer = + smoltcp::socket::TcpSocketBuffer::new(&mut tx_storage[..]); + let tcp_socket = + smoltcp::socket::TcpSocket::new(tcp_rx_buffer, tcp_tx_buffer); + sockets.add(tcp_socket) + }; + + let mut server = server::Server::new(); + + let mut time = 0u32; + let mut next_ms = Instant::now(); + + // TODO: Replace with reference to CPU clock from CCDR. + next_ms += 400_000.cycles(); + + loop { + let tick = Instant::now() > next_ms; + + if tick { + next_ms += 400_000.cycles(); + time += 1; + } + + { + let socket = + &mut *sockets.get::(tcp_handle); + if socket.state() == smoltcp::socket::TcpState::CloseWait { + socket.close(); + } else if !(socket.is_open() || socket.is_listening()) { + socket + .listen(1235) + .unwrap_or_else(|e| warn!("TCP listen error: {:?}", e)); + } else { + server.poll(socket, |req| { + info!("Got request: {:?}", req); + stabilizer::route_request!(req, + readable_attributes: [ + "stabilizer/iir/state": (|| { + let state = c.resources.iir_state.lock(|iir_state| + server::Status { + t: time, + x0: iir_state[0][0][0], + y0: iir_state[0][0][2], + x1: iir_state[1][0][0], + y1: iir_state[1][0][2], + }); + + Ok::(state) + }), + // "_b" means cascades 2nd IIR + "stabilizer/iir_b/state": (|| { let state = c.resources.iir_state.lock(|iir_state| + server::Status { + t: time, + x0: iir_state[0][IIR_CASCADE_LENGTH-1][0], + y0: iir_state[0][IIR_CASCADE_LENGTH-1][2], + x1: iir_state[1][IIR_CASCADE_LENGTH-1][0], + y1: iir_state[1][IIR_CASCADE_LENGTH-1][2], + }); + + Ok::(state) + }), + "stabilizer/afe0/gain": (|| c.resources.afes.0.get_gain()), + "stabilizer/afe1/gain": (|| c.resources.afes.1.get_gain()) + ], + + modifiable_attributes: [ + "stabilizer/iir0/state": server::IirRequest, (|req: server::IirRequest| { + c.resources.iir_ch.lock(|iir_ch| { + if req.channel > 1 { + return Err(()); + } + + iir_ch[req.channel as usize][0] = req.iir; + + Ok::(req) + }) + }), + "stabilizer/iir1/state": server::IirRequest, (|req: server::IirRequest| { + c.resources.iir_ch.lock(|iir_ch| { + if req.channel > 1 { + return Err(()); + } + + iir_ch[req.channel as usize][0] = req.iir; + + Ok::(req) + }) + }), + "stabilizer/iir_b0/state": server::IirRequest, (|req: server::IirRequest| { + c.resources.iir_ch.lock(|iir_ch| { + if req.channel > 1 { + return Err(()); + } + + iir_ch[req.channel as usize][IIR_CASCADE_LENGTH-1] = req.iir; + + Ok::(req) + }) + }), + "stabilizer/iir_b1/state": server::IirRequest,(|req: server::IirRequest| { + c.resources.iir_ch.lock(|iir_ch| { + if req.channel > 1 { + return Err(()); + } + + iir_ch[req.channel as usize][IIR_CASCADE_LENGTH-1] = req.iir; + + Ok::(req) + }) + }), + "stabilizer/afe0/gain": hardware::AfeGain, (|gain| { + c.resources.afes.0.set_gain(gain); + Ok::<(), ()>(()) + }), + "stabilizer/afe1/gain": hardware::AfeGain, (|gain| { + c.resources.afes.1.set_gain(gain); + Ok::<(), ()>(()) + }) + ] + ) + }); + } + } + + let sleep = match c.resources.net_interface.poll( + &mut sockets, + smoltcp::time::Instant::from_millis(time as i64), + ) { + Ok(changed) => !changed, + Err(smoltcp::Error::Unrecognized) => true, + Err(e) => { + info!("iface poll error: {:?}", e); + true + } + }; + + if sleep { + cortex_m::asm::wfi(); + } + } + } + + #[task(binds = ETH, priority = 1)] + fn eth(_: eth::Context) { + unsafe { hal::ethernet::interrupt_handler() } + } + + #[task(binds = SPI2, priority = 3)] + fn spi2(_: spi2::Context) { + panic!("ADC0 input overrun"); + } + + #[task(binds = SPI3, priority = 3)] + fn spi3(_: spi3::Context) { + panic!("ADC0 input overrun"); + } + + #[task(binds = SPI4, priority = 3)] + fn spi4(_: spi4::Context) { + panic!("DAC0 output error"); + } + + #[task(binds = SPI5, priority = 3)] + fn spi5(_: spi5::Context) { + panic!("DAC1 output error"); + } + + extern "C" { + // hw interrupt handlers for RTIC to use for scheduling tasks + // one per priority + fn DCMI(); + fn JPEG(); + fn SDMMC(); + } +}; diff --git a/src/adc.rs b/src/hardware/adc.rs similarity index 95% rename from src/adc.rs rename to src/hardware/adc.rs index 48792d9..188e436 100644 --- a/src/adc.rs +++ b/src/hardware/adc.rs @@ -72,9 +72,16 @@ ///! be used as a means to both detect and buffer ADC samples during the buffer swap-over. Because ///! of this, double-buffered mode does not offer any advantages over single-buffered mode (unless ///! double-buffered mode offers less overhead due to the DMA disable/enable procedure). -use super::{ - hal, timers, DMAReq, DmaConfig, MemoryToPeripheral, PeripheralToMemory, - Priority, TargetAddress, Transfer, SAMPLE_BUFFER_SIZE, +use stm32h7xx_hal as hal; + +use crate::SAMPLE_BUFFER_SIZE; + +use super::timers; +use hal::dma::{ + config::Priority, + dma::{DMAReq, DmaConfig}, + traits::TargetAddress, + MemoryToPeripheral, PeripheralToMemory, Transfer, }; // The following data is written by the timer ADC sample trigger into the SPI CR1 to start the @@ -174,13 +181,13 @@ macro_rules! adc_input { PeripheralToMemory, &'static mut [u16; SAMPLE_BUFFER_SIZE], >, - _trigger_transfer: Transfer< + trigger_transfer: Transfer< hal::dma::dma::$trigger_stream, [< $spi CR >], MemoryToPeripheral, &'static mut [u32; 1], >, - _flag_clear_transfer: Transfer< + clear_transfer: Transfer< hal::dma::dma::$clear_stream, [< $spi IFCR >], MemoryToPeripheral, @@ -227,7 +234,7 @@ macro_rules! adc_input { clear_channel.listen_dma(); clear_channel.to_output_compare(0); - let mut clear_transfer: Transfer< + let clear_transfer: Transfer< _, _, MemoryToPeripheral, @@ -264,7 +271,7 @@ macro_rules! adc_input { }; // Construct the trigger stream to write from memory to the peripheral. - let mut trigger_transfer: Transfer< + let trigger_transfer: Transfer< _, _, MemoryToPeripheral, @@ -299,7 +306,7 @@ macro_rules! adc_input { // The data transfer is always a transfer of data from the peripheral to a RAM // buffer. - let mut data_transfer: Transfer<_, _, PeripheralToMemory, _> = + let data_transfer: Transfer<_, _, PeripheralToMemory, _> = Transfer::init( data_stream, spi, @@ -310,29 +317,30 @@ macro_rules! adc_input { data_config, ); - data_transfer.start(|spi| { - // Allow the SPI RX FIFO to generate DMA transfer requests when data is - // available. - spi.enable_dma_rx(); - - // Each transaction is 1 word (16 bytes). - spi.inner().cr2.modify(|_, w| w.tsize().bits(1)); - spi.inner().cr1.modify(|_, w| w.spe().set_bit()); - }); - - clear_transfer.start(|_| {}); - trigger_transfer.start(|_| {}); - Self { // Note(unsafe): The ADC_BUF[$index][1] is "owned" by this peripheral. It // shall not be used anywhere else in the module. next_buffer: unsafe { Some(&mut ADC_BUF[$index][1]) }, transfer: data_transfer, - _trigger_transfer: trigger_transfer, - _flag_clear_transfer: clear_transfer, + trigger_transfer, + clear_transfer, } } + /// Enable the ADC DMA transfer sequence. + pub fn start(&mut self) { + self.transfer.start(|spi| { + spi.enable_dma_rx(); + + spi.inner().cr2.modify(|_, w| w.tsize().bits(1)); + spi.inner().cr1.modify(|_, w| w.spe().set_bit()); + }); + + self.clear_transfer.start(|_| {}); + self.trigger_transfer.start(|_| {}); + + } + /// Obtain a buffer filled with ADC samples. /// /// # Returns diff --git a/src/afe.rs b/src/hardware/afe.rs similarity index 100% rename from src/afe.rs rename to src/hardware/afe.rs diff --git a/src/hardware/configuration.rs b/src/hardware/configuration.rs new file mode 100644 index 0000000..18ca67f --- /dev/null +++ b/src/hardware/configuration.rs @@ -0,0 +1,822 @@ +///! Stabilizer hardware configuration +///! +///! This file contains all of the hardware-specific configuration of Stabilizer. +use crate::ADC_SAMPLE_TICKS; + +#[cfg(feature = "pounder_v1_1")] +use crate::SAMPLE_BUFFER_SIZE; + +#[cfg(feature = "pounder_v1_1")] +use core::convert::TryInto; + +use smoltcp::{iface::Routes, wire::Ipv4Address}; + +use stm32h7xx_hal::{ + self as hal, + ethernet::{self, PHY}, + prelude::*, +}; + +use embedded_hal::digital::v2::{InputPin, OutputPin}; + +use super::{ + adc, afe, dac, design_parameters, digital_input_stamper, eeprom, pounder, + timers, DdsOutput, Ethernet, AFE0, AFE1, +}; + +// Network storage definition for the ethernet interface. +struct NetStorage { + ip_addrs: [smoltcp::wire::IpCidr; 1], + neighbor_cache: + [Option<(smoltcp::wire::IpAddress, smoltcp::iface::Neighbor)>; 8], + routes_storage: [Option<(smoltcp::wire::IpCidr, smoltcp::iface::Route)>; 1], +} + +/// The available networking devices on Stabilizer. +pub struct NetworkDevices { + pub interface: Ethernet, + pub phy: ethernet::phy::LAN8742A, +} + +/// The available hardware interfaces on Stabilizer. +pub struct StabilizerDevices { + pub afes: (AFE0, AFE1), + pub adcs: (adc::Adc0Input, adc::Adc1Input), + pub dacs: (dac::Dac0Output, dac::Dac1Output), + pub timestamper: digital_input_stamper::InputStamper, + pub adc_dac_timer: timers::SamplingTimer, + pub timestamp_timer: timers::TimestampTimer, + pub net: NetworkDevices, +} + +/// The available Pounder-specific hardware interfaces. +pub struct PounderDevices { + pub pounder: pounder::PounderDevices, + pub dds_output: DdsOutput, + + #[cfg(feature = "pounder_v1_1")] + pub timestamper: pounder::timestamp::Timestamper, +} + +#[link_section = ".sram3.eth"] +/// Static storage for the ethernet DMA descriptor ring. +static mut DES_RING: ethernet::DesRing = ethernet::DesRing::new(); + +/// Static, global-scope network storage for the ethernet interface. +/// +/// This is a static singleton so that the network storage can be referenced from all contexts. +static mut NET_STORE: NetStorage = NetStorage { + // Placeholder for the real IP address, which is initialized at runtime. + ip_addrs: [smoltcp::wire::IpCidr::Ipv6( + smoltcp::wire::Ipv6Cidr::SOLICITED_NODE_PREFIX, + )], + neighbor_cache: [None; 8], + routes_storage: [None; 1], +}; + +/// Configure the stabilizer hardware for operation. +/// +/// # Args +/// * `core` - The RTIC core for configuring the cortex-M core of the device. +/// * `device` - The microcontroller peripherals to be configured. +/// +/// # Returns +/// (stabilizer, pounder) where `stabilizer` is a `StabilizerDevices` structure containing all +/// stabilizer hardware interfaces in a disabled state. `pounder` is an `Option` containing +/// `Some(devices)` if pounder is detected, where `devices` is a `PounderDevices` structure +/// containing all of the pounder hardware interfaces in a disabled state. +pub fn setup( + mut core: rtic::export::Peripherals, + device: stm32h7xx_hal::stm32::Peripherals, +) -> (StabilizerDevices, Option) { + let pwr = device.PWR.constrain(); + let vos = pwr.freeze(); + + // Enable SRAM3 for the ethernet descriptor ring. + device.RCC.ahb2enr.modify(|_, w| w.sram3en().set_bit()); + + // Clear reset flags. + device.RCC.rsr.write(|w| w.rmvf().set_bit()); + + // Select the PLLs for SPI. + device + .RCC + .d2ccip1r + .modify(|_, w| w.spi123sel().pll2_p().spi45sel().pll2_q()); + + let rcc = device.RCC.constrain(); + let ccdr = rcc + .use_hse(8.mhz()) + .sysclk(400.mhz()) + .hclk(200.mhz()) + .per_ck(100.mhz()) + .pll2_p_ck(100.mhz()) + .pll2_q_ck(100.mhz()) + .freeze(vos, &device.SYSCFG); + + #[cfg(feature = "semihosting")] + { + use cortex_m_log::log::{init as init_log, Logger}; + use cortex_m_log::printer::semihosting::{hio::HStdout, InterruptOk}; + use log::LevelFilter; + static mut LOGGER: Option>> = None; + let logger = Logger { + inner: InterruptOk::<_>::stdout().unwrap(), + level: LevelFilter::Info, + }; + let logger = unsafe { LOGGER.get_or_insert(logger) }; + + init_log(logger).unwrap(); + } + + let mut delay = hal::delay::Delay::new(core.SYST, ccdr.clocks); + + let gpioa = device.GPIOA.split(ccdr.peripheral.GPIOA); + let gpiob = device.GPIOB.split(ccdr.peripheral.GPIOB); + let gpioc = device.GPIOC.split(ccdr.peripheral.GPIOC); + let gpiod = device.GPIOD.split(ccdr.peripheral.GPIOD); + let gpioe = device.GPIOE.split(ccdr.peripheral.GPIOE); + let gpiof = device.GPIOF.split(ccdr.peripheral.GPIOF); + let mut gpiog = device.GPIOG.split(ccdr.peripheral.GPIOG); + + let dma_streams = + hal::dma::dma::StreamsTuple::new(device.DMA1, ccdr.peripheral.DMA1); + + // Configure timer 2 to trigger conversions for the ADC + let mut sampling_timer = { + // The timer frequency is manually adjusted below, so the 1KHz setting here is a + // dont-care. + let mut timer2 = + device + .TIM2 + .timer(1.khz(), ccdr.peripheral.TIM2, &ccdr.clocks); + + // Configure the timer to count at the designed tick rate. We will manually set the + // period below. + timer2.pause(); + timer2.reset_counter(); + timer2.set_tick_freq(design_parameters::TIMER_FREQUENCY); + + let mut sampling_timer = timers::SamplingTimer::new(timer2); + sampling_timer.set_period_ticks((ADC_SAMPLE_TICKS - 1) as u32); + + // The sampling timer is used as the master timer for the shadow-sampling timer. Thus, + // it generates a trigger whenever it is enabled. + + sampling_timer + }; + + let mut shadow_sampling_timer = { + // The timer frequency is manually adjusted below, so the 1KHz setting here is a + // dont-care. + let mut timer3 = + device + .TIM3 + .timer(1.khz(), ccdr.peripheral.TIM3, &ccdr.clocks); + + // Configure the timer to count at the designed tick rate. We will manually set the + // period below. + timer3.pause(); + timer3.reset_counter(); + timer3.set_tick_freq(design_parameters::TIMER_FREQUENCY); + + let mut shadow_sampling_timer = + timers::ShadowSamplingTimer::new(timer3); + shadow_sampling_timer.set_period_ticks(ADC_SAMPLE_TICKS - 1); + + // The shadow sampling timer is a slave-mode timer to the sampling timer. It should + // always be in-sync - thus, we configure it to operate in slave mode using "Trigger + // mode". + // For TIM3, TIM2 can be made the internal trigger connection using ITR1. Thus, the + // SamplingTimer start now gates the start of the ShadowSamplingTimer. + shadow_sampling_timer.set_slave_mode( + timers::TriggerSource::Trigger1, + timers::SlaveMode::Trigger, + ); + + shadow_sampling_timer + }; + + let sampling_timer_channels = sampling_timer.channels(); + let shadow_sampling_timer_channels = shadow_sampling_timer.channels(); + + let mut timestamp_timer = { + // The timer frequency is manually adjusted below, so the 1KHz setting here is a + // dont-care. + let mut timer5 = + device + .TIM5 + .timer(1.khz(), ccdr.peripheral.TIM5, &ccdr.clocks); + + // Configure the timer to count at the designed tick rate. We will manually set the + // period below. + timer5.pause(); + timer5.set_tick_freq(design_parameters::TIMER_FREQUENCY); + + // The timestamp timer must run at exactly a multiple of the sample timer based on the + // batch size. To accomodate this, we manually set the prescaler identical to the sample + // timer, but use a period that is longer. + let mut timer = timers::TimestampTimer::new(timer5); + + let period = digital_input_stamper::calculate_timestamp_timer_period(); + timer.set_period_ticks(period); + + timer + }; + + let timestamp_timer_channels = timestamp_timer.channels(); + + // Configure the SPI interfaces to the ADCs and DACs. + let adcs = { + let adc0 = { + let spi_miso = gpiob + .pb14 + .into_alternate_af5() + .set_speed(hal::gpio::Speed::VeryHigh); + let spi_sck = gpiob + .pb10 + .into_alternate_af5() + .set_speed(hal::gpio::Speed::VeryHigh); + let _spi_nss = gpiob + .pb9 + .into_alternate_af5() + .set_speed(hal::gpio::Speed::VeryHigh); + + let config = hal::spi::Config::new(hal::spi::Mode { + polarity: hal::spi::Polarity::IdleHigh, + phase: hal::spi::Phase::CaptureOnSecondTransition, + }) + .manage_cs() + .suspend_when_inactive() + .communication_mode(hal::spi::CommunicationMode::Receiver) + .cs_delay(design_parameters::ADC_SETUP_TIME); + + let spi: hal::spi::Spi<_, _, u16> = device.SPI2.spi( + (spi_sck, spi_miso, hal::spi::NoMosi), + config, + design_parameters::ADC_DAC_SCK_MAX, + ccdr.peripheral.SPI2, + &ccdr.clocks, + ); + + adc::Adc0Input::new( + spi, + dma_streams.0, + dma_streams.1, + dma_streams.2, + sampling_timer_channels.ch1, + shadow_sampling_timer_channels.ch1, + ) + }; + + let adc1 = { + let spi_miso = gpiob + .pb4 + .into_alternate_af6() + .set_speed(hal::gpio::Speed::VeryHigh); + let spi_sck = gpioc + .pc10 + .into_alternate_af6() + .set_speed(hal::gpio::Speed::VeryHigh); + let _spi_nss = gpioa + .pa15 + .into_alternate_af6() + .set_speed(hal::gpio::Speed::VeryHigh); + + let config = hal::spi::Config::new(hal::spi::Mode { + polarity: hal::spi::Polarity::IdleHigh, + phase: hal::spi::Phase::CaptureOnSecondTransition, + }) + .manage_cs() + .suspend_when_inactive() + .communication_mode(hal::spi::CommunicationMode::Receiver) + .cs_delay(design_parameters::ADC_SETUP_TIME); + + let spi: hal::spi::Spi<_, _, u16> = device.SPI3.spi( + (spi_sck, spi_miso, hal::spi::NoMosi), + config, + design_parameters::ADC_DAC_SCK_MAX, + ccdr.peripheral.SPI3, + &ccdr.clocks, + ); + + adc::Adc1Input::new( + spi, + dma_streams.3, + dma_streams.4, + dma_streams.5, + sampling_timer_channels.ch2, + shadow_sampling_timer_channels.ch2, + ) + }; + + (adc0, adc1) + }; + + let dacs = { + let _dac_clr_n = gpioe.pe12.into_push_pull_output().set_high().unwrap(); + let _dac0_ldac_n = + gpioe.pe11.into_push_pull_output().set_low().unwrap(); + let _dac1_ldac_n = + gpioe.pe15.into_push_pull_output().set_low().unwrap(); + + let dac0_spi = { + let spi_miso = gpioe + .pe5 + .into_alternate_af5() + .set_speed(hal::gpio::Speed::VeryHigh); + let spi_sck = gpioe + .pe2 + .into_alternate_af5() + .set_speed(hal::gpio::Speed::VeryHigh); + let _spi_nss = gpioe + .pe4 + .into_alternate_af5() + .set_speed(hal::gpio::Speed::VeryHigh); + + let config = hal::spi::Config::new(hal::spi::Mode { + polarity: hal::spi::Polarity::IdleHigh, + phase: hal::spi::Phase::CaptureOnSecondTransition, + }) + .manage_cs() + .suspend_when_inactive() + .communication_mode(hal::spi::CommunicationMode::Transmitter) + .swap_mosi_miso(); + + device.SPI4.spi( + (spi_sck, spi_miso, hal::spi::NoMosi), + config, + design_parameters::ADC_DAC_SCK_MAX, + ccdr.peripheral.SPI4, + &ccdr.clocks, + ) + }; + + let dac1_spi = { + let spi_miso = gpiof + .pf8 + .into_alternate_af5() + .set_speed(hal::gpio::Speed::VeryHigh); + let spi_sck = gpiof + .pf7 + .into_alternate_af5() + .set_speed(hal::gpio::Speed::VeryHigh); + let _spi_nss = gpiof + .pf6 + .into_alternate_af5() + .set_speed(hal::gpio::Speed::VeryHigh); + + let config = hal::spi::Config::new(hal::spi::Mode { + polarity: hal::spi::Polarity::IdleHigh, + phase: hal::spi::Phase::CaptureOnSecondTransition, + }) + .manage_cs() + .communication_mode(hal::spi::CommunicationMode::Transmitter) + .suspend_when_inactive() + .swap_mosi_miso(); + + device.SPI5.spi( + (spi_sck, spi_miso, hal::spi::NoMosi), + config, + design_parameters::ADC_DAC_SCK_MAX, + ccdr.peripheral.SPI5, + &ccdr.clocks, + ) + }; + + let dac0 = dac::Dac0Output::new( + dac0_spi, + dma_streams.6, + sampling_timer_channels.ch3, + ); + let dac1 = dac::Dac1Output::new( + dac1_spi, + dma_streams.7, + sampling_timer_channels.ch4, + ); + (dac0, dac1) + }; + + let afes = { + let afe0 = { + let a0_pin = gpiof.pf2.into_push_pull_output(); + let a1_pin = gpiof.pf5.into_push_pull_output(); + afe::ProgrammableGainAmplifier::new(a0_pin, a1_pin) + }; + + let afe1 = { + let a0_pin = gpiod.pd14.into_push_pull_output(); + let a1_pin = gpiod.pd15.into_push_pull_output(); + afe::ProgrammableGainAmplifier::new(a0_pin, a1_pin) + }; + + (afe0, afe1) + }; + + let input_stamper = { + let trigger = gpioa.pa3.into_alternate_af2(); + digital_input_stamper::InputStamper::new( + trigger, + timestamp_timer_channels.ch4, + ) + }; + + let mut eeprom_i2c = { + let sda = gpiof.pf0.into_alternate_af4().set_open_drain(); + let scl = gpiof.pf1.into_alternate_af4().set_open_drain(); + device.I2C2.i2c( + (scl, sda), + 100.khz(), + ccdr.peripheral.I2C2, + &ccdr.clocks, + ) + }; + + // Configure ethernet pins. + { + // Reset the PHY before configuring pins. + let mut eth_phy_nrst = gpioe.pe3.into_push_pull_output(); + eth_phy_nrst.set_low().unwrap(); + delay.delay_us(200u8); + eth_phy_nrst.set_high().unwrap(); + let _rmii_ref_clk = gpioa + .pa1 + .into_alternate_af11() + .set_speed(hal::gpio::Speed::VeryHigh); + let _rmii_mdio = gpioa + .pa2 + .into_alternate_af11() + .set_speed(hal::gpio::Speed::VeryHigh); + let _rmii_mdc = gpioc + .pc1 + .into_alternate_af11() + .set_speed(hal::gpio::Speed::VeryHigh); + let _rmii_crs_dv = gpioa + .pa7 + .into_alternate_af11() + .set_speed(hal::gpio::Speed::VeryHigh); + let _rmii_rxd0 = gpioc + .pc4 + .into_alternate_af11() + .set_speed(hal::gpio::Speed::VeryHigh); + let _rmii_rxd1 = gpioc + .pc5 + .into_alternate_af11() + .set_speed(hal::gpio::Speed::VeryHigh); + let _rmii_tx_en = gpiob + .pb11 + .into_alternate_af11() + .set_speed(hal::gpio::Speed::VeryHigh); + let _rmii_txd0 = gpiob + .pb12 + .into_alternate_af11() + .set_speed(hal::gpio::Speed::VeryHigh); + let _rmii_txd1 = gpiog + .pg14 + .into_alternate_af11() + .set_speed(hal::gpio::Speed::VeryHigh); + } + + let mac_addr = match eeprom::read_eui48(&mut eeprom_i2c) { + Err(_) => { + info!("Could not read EEPROM, using default MAC address"); + smoltcp::wire::EthernetAddress([0x10, 0xE2, 0xD5, 0x00, 0x03, 0x00]) + } + Ok(raw_mac) => smoltcp::wire::EthernetAddress(raw_mac), + }; + + let network_devices = { + // Configure the ethernet controller + let (eth_dma, eth_mac) = unsafe { + ethernet::new_unchecked( + device.ETHERNET_MAC, + device.ETHERNET_MTL, + device.ETHERNET_DMA, + &mut DES_RING, + mac_addr, + ccdr.peripheral.ETH1MAC, + &ccdr.clocks, + ) + }; + + // Reset and initialize the ethernet phy. + let mut lan8742a = + ethernet::phy::LAN8742A::new(eth_mac.set_phy_addr(0)); + lan8742a.phy_reset(); + lan8742a.phy_init(); + + unsafe { ethernet::enable_interrupt() }; + + let store = unsafe { &mut NET_STORE }; + + store.ip_addrs[0] = smoltcp::wire::IpCidr::new( + smoltcp::wire::IpAddress::v4(10, 0, 16, 99), + 24, + ); + + let default_v4_gw = Ipv4Address::new(10, 0, 16, 1); + let mut routes = Routes::new(&mut store.routes_storage[..]); + routes.add_default_ipv4_route(default_v4_gw).unwrap(); + + let neighbor_cache = + smoltcp::iface::NeighborCache::new(&mut store.neighbor_cache[..]); + + let interface = smoltcp::iface::EthernetInterfaceBuilder::new(eth_dma) + .ethernet_addr(mac_addr) + .neighbor_cache(neighbor_cache) + .ip_addrs(&mut store.ip_addrs[..]) + .routes(routes) + .finalize(); + + NetworkDevices { + interface, + phy: lan8742a, + } + }; + + let mut fp_led_0 = gpiod.pd5.into_push_pull_output(); + let mut fp_led_1 = gpiod.pd6.into_push_pull_output(); + let mut fp_led_2 = gpiog.pg4.into_push_pull_output(); + let mut fp_led_3 = gpiod.pd12.into_push_pull_output(); + + fp_led_0.set_low().unwrap(); + fp_led_1.set_low().unwrap(); + fp_led_2.set_low().unwrap(); + fp_led_3.set_low().unwrap(); + + // Measure the Pounder PGOOD output to detect if pounder is present on Stabilizer. + let pounder_pgood = gpiob.pb13.into_pull_down_input(); + delay.delay_ms(2u8); + let pounder = if pounder_pgood.is_high().unwrap() { + let ad9959 = { + let qspi_interface = { + // Instantiate the QUADSPI pins and peripheral interface. + let qspi_pins = { + let _qspi_ncs = gpioc + .pc11 + .into_alternate_af9() + .set_speed(hal::gpio::Speed::VeryHigh); + + let clk = gpiob + .pb2 + .into_alternate_af9() + .set_speed(hal::gpio::Speed::VeryHigh); + let io0 = gpioe + .pe7 + .into_alternate_af10() + .set_speed(hal::gpio::Speed::VeryHigh); + let io1 = gpioe + .pe8 + .into_alternate_af10() + .set_speed(hal::gpio::Speed::VeryHigh); + let io2 = gpioe + .pe9 + .into_alternate_af10() + .set_speed(hal::gpio::Speed::VeryHigh); + let io3 = gpioe + .pe10 + .into_alternate_af10() + .set_speed(hal::gpio::Speed::VeryHigh); + + (clk, io0, io1, io2, io3) + }; + + let qspi = hal::qspi::Qspi::bank2( + device.QUADSPI, + qspi_pins, + design_parameters::POUNDER_QSPI_FREQUENCY, + &ccdr.clocks, + ccdr.peripheral.QSPI, + ); + + pounder::QspiInterface::new(qspi).unwrap() + }; + + #[cfg(feature = "pounder_v1_1")] + let reset_pin = gpiog.pg6.into_push_pull_output(); + #[cfg(not(feature = "pounder_v1_1"))] + let reset_pin = gpioa.pa0.into_push_pull_output(); + + let mut io_update = gpiog.pg7.into_push_pull_output(); + + let ref_clk: hal::time::Hertz = + design_parameters::DDS_REF_CLK.into(); + + let ad9959 = ad9959::Ad9959::new( + qspi_interface, + reset_pin, + &mut io_update, + &mut delay, + ad9959::Mode::FourBitSerial, + ref_clk.0 as f32, + design_parameters::DDS_MULTIPLIER, + ) + .unwrap(); + + // Return IO_Update + gpiog.pg7 = io_update.into_analog(); + + ad9959 + }; + + let io_expander = { + let sda = gpiob.pb7.into_alternate_af4().set_open_drain(); + let scl = gpiob.pb8.into_alternate_af4().set_open_drain(); + let i2c1 = device.I2C1.i2c( + (scl, sda), + 100.khz(), + ccdr.peripheral.I2C1, + &ccdr.clocks, + ); + mcp23017::MCP23017::default(i2c1).unwrap() + }; + + let spi = { + let spi_mosi = gpiod + .pd7 + .into_alternate_af5() + .set_speed(hal::gpio::Speed::VeryHigh); + let spi_miso = gpioa + .pa6 + .into_alternate_af5() + .set_speed(hal::gpio::Speed::VeryHigh); + let spi_sck = gpiog + .pg11 + .into_alternate_af5() + .set_speed(hal::gpio::Speed::VeryHigh); + + let config = hal::spi::Config::new(hal::spi::Mode { + polarity: hal::spi::Polarity::IdleHigh, + phase: hal::spi::Phase::CaptureOnSecondTransition, + }); + + // The maximum frequency of this SPI must be limited due to capacitance on the MISO + // line causing a long RC decay. + device.SPI1.spi( + (spi_sck, spi_miso, spi_mosi), + config, + 5.mhz(), + ccdr.peripheral.SPI1, + &ccdr.clocks, + ) + }; + + let (adc1, adc2) = { + let (mut adc1, mut adc2) = hal::adc::adc12( + device.ADC1, + device.ADC2, + &mut delay, + ccdr.peripheral.ADC12, + &ccdr.clocks, + ); + + let adc1 = { + adc1.calibrate(); + adc1.enable() + }; + + let adc2 = { + adc2.calibrate(); + adc2.enable() + }; + + (adc1, adc2) + }; + + let adc1_in_p = gpiof.pf11.into_analog(); + let adc2_in_p = gpiof.pf14.into_analog(); + + let pounder_devices = pounder::PounderDevices::new( + io_expander, + spi, + adc1, + adc2, + adc1_in_p, + adc2_in_p, + ) + .unwrap(); + + let dds_output = { + let io_update_trigger = { + let _io_update = gpiog + .pg7 + .into_alternate_af2() + .set_speed(hal::gpio::Speed::VeryHigh); + + // Configure the IO_Update signal for the DDS. + let mut hrtimer = pounder::hrtimer::HighResTimerE::new( + device.HRTIM_TIME, + device.HRTIM_MASTER, + device.HRTIM_COMMON, + ccdr.clocks, + ccdr.peripheral.HRTIM, + ); + + // IO_Update occurs after a fixed delay from the QSPI write. Note that the timer + // is triggered after the QSPI write, which can take approximately 120nS, so + // there is additional margin. + hrtimer.configure_single_shot( + pounder::hrtimer::Channel::Two, + design_parameters::POUNDER_IO_UPDATE_DURATION, + design_parameters::POUNDER_IO_UPDATE_DELAY, + ); + + // Ensure that we have enough time for an IO-update every sample. + let sample_frequency = { + let timer_frequency: hal::time::Hertz = + design_parameters::TIMER_FREQUENCY.into(); + timer_frequency.0 as f32 / ADC_SAMPLE_TICKS as f32 + }; + + let sample_period = 1.0 / sample_frequency; + assert!( + sample_period > design_parameters::POUNDER_IO_UPDATE_DELAY + ); + + hrtimer + }; + + let (qspi, config) = ad9959.freeze(); + DdsOutput::new(qspi, io_update_trigger, config) + }; + + #[cfg(feature = "pounder_v1_1")] + let pounder_stamper = { + let dma2_streams = hal::dma::dma::StreamsTuple::new( + device.DMA2, + ccdr.peripheral.DMA2, + ); + + let etr_pin = gpioa.pa0.into_alternate_af3(); + + // The frequency in the constructor is dont-care, as we will modify the period + clock + // source manually below. + let tim8 = + device + .TIM8 + .timer(1.khz(), ccdr.peripheral.TIM8, &ccdr.clocks); + let mut timestamp_timer = timers::PounderTimestampTimer::new(tim8); + + // Pounder is configured to generate a 500MHz reference clock, so a 125MHz sync-clock is + // output. As a result, dividing the 125MHz sync-clk provides a 31.25MHz tick rate for + // the timestamp timer. 31.25MHz corresponds with a 32ns tick rate. + timestamp_timer.set_external_clock(timers::Prescaler::Div4); + timestamp_timer.start(); + + // We want the pounder timestamp timer to overflow once per batch. + let tick_ratio = { + let sync_clk_mhz: f32 = design_parameters::DDS_SYSTEM_CLK.0 + as f32 + / design_parameters::DDS_SYNC_CLK_DIV as f32; + sync_clk_mhz / design_parameters::TIMER_FREQUENCY.0 as f32 + }; + + let period = (tick_ratio + * ADC_SAMPLE_TICKS as f32 + * SAMPLE_BUFFER_SIZE as f32) as u32 + / 4; + timestamp_timer.set_period_ticks((period - 1).try_into().unwrap()); + let tim8_channels = timestamp_timer.channels(); + + pounder::timestamp::Timestamper::new( + timestamp_timer, + dma2_streams.0, + tim8_channels.ch1, + &mut sampling_timer, + etr_pin, + ) + }; + + Some(PounderDevices { + pounder: pounder_devices, + dds_output, + + #[cfg(feature = "pounder_v1_1")] + timestamper: pounder_stamper, + }) + } else { + None + }; + + let stabilizer = StabilizerDevices { + afes, + adcs, + dacs, + timestamper: input_stamper, + net: network_devices, + adc_dac_timer: sampling_timer, + timestamp_timer, + }; + + // info!("Version {} {}", build_info::PKG_VERSION, build_info::GIT_VERSION.unwrap()); + // info!("Built on {}", build_info::BUILT_TIME_UTC); + // info!("{} {}", build_info::RUSTC_VERSION, build_info::TARGET); + + // Enable the instruction cache. + core.SCB.enable_icache(); + + // Utilize the cycle counter for RTIC scheduling. + core.DWT.enable_cycle_counter(); + + (stabilizer, pounder) +} diff --git a/src/dac.rs b/src/hardware/dac.rs similarity index 96% rename from src/dac.rs rename to src/hardware/dac.rs index a22e5da..09d0497 100644 --- a/src/dac.rs +++ b/src/hardware/dac.rs @@ -50,9 +50,15 @@ ///! While double-buffered mode is used for DMA to avoid lost DAC-update events, there is no check ///! for re-use of a previously provided DAC output buffer. It is assumed that the DMA request is ///! served promptly after the transfer completes. -use super::{ - hal, timers, DMAReq, DmaConfig, MemoryToPeripheral, TargetAddress, - Transfer, SAMPLE_BUFFER_SIZE, +use stm32h7xx_hal as hal; + +use crate::SAMPLE_BUFFER_SIZE; + +use super::timers; +use hal::dma::{ + dma::{DMAReq, DmaConfig}, + traits::TargetAddress, + MemoryToPeripheral, Transfer, }; // The following global buffers are used for the DAC code DMA transfers. Two buffers are used for @@ -158,7 +164,7 @@ macro_rules! dac_output { } // Construct the trigger stream to write from memory to the peripheral. - let mut transfer: Transfer<_, _, MemoryToPeripheral, _> = + let transfer: Transfer<_, _, MemoryToPeripheral, _> = Transfer::init( stream, $spi::new(trigger_channel, spi), @@ -169,8 +175,6 @@ macro_rules! dac_output { trigger_config, ); - transfer.start(|spi| spi.start_dma()); - Self { transfer, // Note(unsafe): This buffer is only used once and provided for the next DMA transfer. @@ -178,6 +182,10 @@ macro_rules! dac_output { } } + pub fn start(&mut self) { + self.transfer.start(|spi| spi.start_dma()); + } + /// Acquire the next output buffer to populate it with DAC codes. pub fn acquire_buffer(&mut self) -> &mut [u16; SAMPLE_BUFFER_SIZE] { // Note: If a device hangs up, check that this conditional is passing correctly, as diff --git a/src/design_parameters.rs b/src/hardware/design_parameters.rs similarity index 92% rename from src/design_parameters.rs rename to src/hardware/design_parameters.rs index 3edf04b..3de7c15 100644 --- a/src/design_parameters.rs +++ b/src/hardware/design_parameters.rs @@ -1,4 +1,4 @@ -use super::hal::time::MegaHertz; +use stm32h7xx_hal::time::MegaHertz; /// The ADC setup time is the number of seconds after the CSn line goes low before the serial clock /// may begin. This is used for performing the internal ADC conversion. @@ -17,13 +17,13 @@ pub const POUNDER_QSPI_FREQUENCY: MegaHertz = MegaHertz(40); // Pounder Profile writes are always 16 bytes, with 2 cycles required per byte, coming out to a // total of 32 QSPI clock cycles. The QSPI is configured for 40MHz, so this comes out to an offset // of 800nS. We use 900ns to be safe. -pub const POUNDER_IO_UPDATE_DELAY: f32 = 900_e-9; +pub const POUNDER_IO_UPDATE_DELAY: f32 = 900e-9; /// The duration to assert IO_Update for the pounder DDS. // IO_Update should be latched for 4 SYNC_CLK cycles after the QSPI profile write. With pounder // SYNC_CLK running at 100MHz (1/4 of the pounder reference clock of 500MHz), this corresponds to // 32ns. To accomodate rounding errors, we use 50ns instead. -pub const POUNDER_IO_UPDATE_DURATION: f32 = 50_e-9; +pub const POUNDER_IO_UPDATE_DURATION: f32 = 50e-9; /// The DDS reference clock frequency in MHz. pub const DDS_REF_CLK: MegaHertz = MegaHertz(100); diff --git a/src/digital_input_stamper.rs b/src/hardware/digital_input_stamper.rs similarity index 97% rename from src/digital_input_stamper.rs rename to src/hardware/digital_input_stamper.rs index 648105f..4262800 100644 --- a/src/digital_input_stamper.rs +++ b/src/hardware/digital_input_stamper.rs @@ -24,7 +24,8 @@ ///! ///! This module only supports DI0 for timestamping due to trigger constraints on the DIx pins. If ///! timestamping is desired in DI1, a separate timer + capture channel will be necessary. -use super::{hal, timers, ADC_SAMPLE_TICKS, SAMPLE_BUFFER_SIZE}; +use super::{hal, timers}; +use crate::{ADC_SAMPLE_TICKS, SAMPLE_BUFFER_SIZE}; /// Calculate the period of the digital input timestamp timer. /// @@ -89,6 +90,7 @@ impl InputStamper { } /// Start to capture timestamps on DI0. + #[allow(dead_code)] pub fn start(&mut self) { self.capture_channel.enable(); } @@ -101,6 +103,7 @@ impl InputStamper { /// /// To prevent timestamp loss, the batch size and sampling rate must be adjusted such that at /// most one timestamp will occur in each data processing cycle. + #[allow(dead_code)] pub fn latest_timestamp(&mut self) -> Option { self.capture_channel .latest_capture() diff --git a/src/eeprom.rs b/src/hardware/eeprom.rs similarity index 93% rename from src/eeprom.rs rename to src/hardware/eeprom.rs index aa5caf7..d84dd70 100644 --- a/src/eeprom.rs +++ b/src/hardware/eeprom.rs @@ -2,7 +2,6 @@ use embedded_hal::blocking::i2c::WriteRead; const I2C_ADDR: u8 = 0x50; -#[allow(dead_code)] pub fn read_eui48(i2c: &mut T) -> Result<[u8; 6], T::Error> where T: WriteRead, diff --git a/src/hardware/mod.rs b/src/hardware/mod.rs new file mode 100644 index 0000000..6434dbc --- /dev/null +++ b/src/hardware/mod.rs @@ -0,0 +1,69 @@ +///! Module for all hardware-specific setup of Stabilizer +use stm32h7xx_hal as hal; + +#[cfg(feature = "semihosting")] +use panic_semihosting as _; + +#[cfg(not(any(feature = "nightly", feature = "semihosting")))] +use panic_halt as _; + +mod adc; +mod afe; +mod configuration; +mod dac; +mod design_parameters; +mod digital_input_stamper; +mod eeprom; +mod pounder; +mod timers; + +pub use adc::{Adc0Input, Adc1Input}; +pub use afe::Gain as AfeGain; +pub use dac::{Dac0Output, Dac1Output}; +pub use pounder::DdsOutput; + +// Type alias for the analog front-end (AFE) for ADC0. +pub type AFE0 = afe::ProgrammableGainAmplifier< + hal::gpio::gpiof::PF2>, + hal::gpio::gpiof::PF5>, +>; + +// Type alias for the analog front-end (AFE) for ADC1. +pub type AFE1 = afe::ProgrammableGainAmplifier< + hal::gpio::gpiod::PD14>, + hal::gpio::gpiod::PD15>, +>; + +// Type alias for the ethernet interface on Stabilizer. +pub type Ethernet = smoltcp::iface::EthernetInterface< + 'static, + 'static, + 'static, + hal::ethernet::EthernetDMA<'static>, +>; + +pub use configuration::{setup, PounderDevices, StabilizerDevices}; + +#[inline(never)] +#[panic_handler] +#[cfg(all(feature = "nightly", not(feature = "semihosting")))] +fn panic(_info: &core::panic::PanicInfo) -> ! { + let gpiod = unsafe { &*hal::stm32::GPIOD::ptr() }; + gpiod.odr.modify(|_, w| w.odr6().high().odr12().high()); // FP_LED_1, FP_LED_3 + #[cfg(feature = "nightly")] + core::intrinsics::abort(); + #[cfg(not(feature = "nightly"))] + unsafe { + core::intrinsics::abort(); + } +} + +#[cortex_m_rt::exception] +fn HardFault(ef: &cortex_m_rt::ExceptionFrame) -> ! { + panic!("HardFault at {:#?}", ef); +} + +#[cortex_m_rt::exception] +fn DefaultHandler(irqn: i16) { + panic!("Unhandled exception (IRQn = {})", irqn); +} diff --git a/src/pounder/attenuators.rs b/src/hardware/pounder/attenuators.rs similarity index 100% rename from src/pounder/attenuators.rs rename to src/hardware/pounder/attenuators.rs diff --git a/src/pounder/dds_output.rs b/src/hardware/pounder/dds_output.rs similarity index 98% rename from src/pounder/dds_output.rs rename to src/hardware/pounder/dds_output.rs index 36399f2..c7c5e6d 100644 --- a/src/pounder/dds_output.rs +++ b/src/hardware/pounder/dds_output.rs @@ -52,8 +52,7 @@ ///! compile-time-known register update sequence needed for the application, the serialization ///! process can be done once and then register values can be written into a pre-computed serialized ///! buffer to avoid the software overhead of much of the serialization process. -use super::QspiInterface; -use crate::hrtimer::HighResTimerE; +use super::{hrtimer::HighResTimerE, QspiInterface}; use ad9959::{Channel, DdsConfig, ProfileSerializer}; use stm32h7xx_hal as hal; @@ -90,6 +89,7 @@ impl DdsOutput { } /// Get a builder for serializing a Pounder DDS profile. + #[allow(dead_code)] pub fn builder(&mut self) -> ProfileBuilder { let builder = self.config.builder(); ProfileBuilder { @@ -145,6 +145,7 @@ impl<'a> ProfileBuilder<'a> { /// * `ftw` - If provided, indicates a frequency tuning word for the channels. /// * `pow` - If provided, indicates a phase offset word for the channels. /// * `acr` - If provided, indicates the amplitude control register for the channels. + #[allow(dead_code)] pub fn update_channels( mut self, channels: &[Channel], @@ -157,6 +158,7 @@ impl<'a> ProfileBuilder<'a> { } /// Write the profile to the DDS asynchronously. + #[allow(dead_code)] pub fn write_profile(mut self) { let profile = self.serializer.finalize(); self.dds_stream.write_profile(profile); diff --git a/src/hrtimer.rs b/src/hardware/pounder/hrtimer.rs similarity index 97% rename from src/hrtimer.rs rename to src/hardware/pounder/hrtimer.rs index 2831bbe..ef7086d 100644 --- a/src/hrtimer.rs +++ b/src/hardware/pounder/hrtimer.rs @@ -1,8 +1,11 @@ ///! The HRTimer (High Resolution Timer) is used to generate IO_Update pulses to the Pounder DDS. -use crate::hal; -use hal::rcc::{rec, CoreClocks, ResetEnable}; +use stm32h7xx_hal::{ + self as hal, + rcc::{rec, CoreClocks, ResetEnable}, +}; /// A HRTimer output channel. +#[allow(dead_code)] pub enum Channel { One, Two, diff --git a/src/pounder/mod.rs b/src/hardware/pounder/mod.rs similarity index 99% rename from src/pounder/mod.rs rename to src/hardware/pounder/mod.rs index 2811e92..3a85937 100644 --- a/src/pounder/mod.rs +++ b/src/hardware/pounder/mod.rs @@ -2,7 +2,10 @@ use serde::{Deserialize, Serialize}; mod attenuators; mod dds_output; +pub mod hrtimer; mod rf_power; + +#[cfg(feature = "pounder_v1_1")] pub mod timestamp; pub use dds_output::DdsOutput; @@ -27,13 +30,11 @@ const ATT_LE0_PIN: u8 = 8; pub enum Error { Spi, I2c, - Dds, Qspi, Bounds, InvalidAddress, InvalidChannel, Adc, - Access, } #[derive(Debug, Copy, Clone)] diff --git a/src/pounder/rf_power.rs b/src/hardware/pounder/rf_power.rs similarity index 100% rename from src/pounder/rf_power.rs rename to src/hardware/pounder/rf_power.rs diff --git a/src/pounder/timestamp.rs b/src/hardware/pounder/timestamp.rs similarity index 94% rename from src/pounder/timestamp.rs rename to src/hardware/pounder/timestamp.rs index f6c1a14..5ccf235 100644 --- a/src/pounder/timestamp.rs +++ b/src/hardware/pounder/timestamp.rs @@ -26,7 +26,7 @@ use stm32h7xx_hal as hal; use hal::dma::{dma::DmaConfig, PeripheralToMemory, Transfer}; -use crate::{timers, SAMPLE_BUFFER_SIZE}; +use crate::{hardware::timers, SAMPLE_BUFFER_SIZE}; // Three buffers are required for double buffered mode - 2 are owned by the DMA stream and 1 is the // working data provided to the application. These buffers must exist in a DMA-accessible memory @@ -89,7 +89,7 @@ impl Timestamper { input_capture.listen_dma(); // The data transfer is always a transfer of data from the peripheral to a RAM buffer. - let mut data_transfer: Transfer<_, _, PeripheralToMemory, _> = + let data_transfer: Transfer<_, _, PeripheralToMemory, _> = Transfer::init( stream, input_capture, @@ -100,8 +100,6 @@ impl Timestamper { config, ); - data_transfer.start(|capture_channel| capture_channel.enable()); - Self { timer: timestamp_timer, transfer: data_transfer, @@ -112,7 +110,15 @@ impl Timestamper { } } + /// Start the DMA transfer for collecting timestamps. + #[allow(dead_code)] + pub fn start(&mut self) { + self.transfer + .start(|capture_channel| capture_channel.enable()); + } + /// Update the period of the underlying timestamp timer. + #[allow(dead_code)] pub fn update_period(&mut self, period: u16) { self.timer.set_period_ticks(period); } @@ -121,6 +127,7 @@ impl Timestamper { /// /// # Returns /// A reference to the underlying buffer that has been filled with timestamps. + #[allow(dead_code)] pub fn acquire_buffer(&mut self) -> &[u16; SAMPLE_BUFFER_SIZE] { // Wait for the transfer to fully complete before continuing. // Note: If a device hangs up, check that this conditional is passing correctly, as there is diff --git a/src/timers.rs b/src/hardware/timers.rs similarity index 99% rename from src/timers.rs rename to src/hardware/timers.rs index 062eb4b..7199730 100644 --- a/src/timers.rs +++ b/src/hardware/timers.rs @@ -32,6 +32,7 @@ pub enum TriggerSource { } /// Prescalers for externally-supplied reference clocks. +#[allow(dead_code)] pub enum Prescaler { Div1 = 0b00, Div2 = 0b01, @@ -40,6 +41,7 @@ pub enum Prescaler { } /// Optional slave operation modes of a timer. +#[allow(dead_code)] pub enum SlaveMode { Disabled = 0, Trigger = 0b0110, diff --git a/src/lib.rs b/src/lib.rs new file mode 100644 index 0000000..b685f7a --- /dev/null +++ b/src/lib.rs @@ -0,0 +1,15 @@ +#![no_std] + +#[macro_use] +extern crate log; + +pub mod hardware; +pub mod server; + +// The number of ticks in the ADC sampling timer. The timer runs at 100MHz, so the step size is +// equal to 10ns per tick. +// Currently, the sample rate is equal to: Fsample = 100/256 MHz = 390.625 KHz +const ADC_SAMPLE_TICKS: u16 = 256; + +// The desired ADC sample processing buffer size. +const SAMPLE_BUFFER_SIZE: usize = 8; diff --git a/src/main.rs b/src/main.rs deleted file mode 100644 index 0930d5a..0000000 --- a/src/main.rs +++ /dev/null @@ -1,1268 +0,0 @@ -#![deny(warnings)] -#![allow(clippy::missing_safety_doc)] -#![no_std] -#![no_main] -#![cfg_attr(feature = "nightly", feature(asm))] -// Enable returning `!` -#![cfg_attr(feature = "nightly", feature(never_type))] -#![cfg_attr(feature = "nightly", feature(core_intrinsics))] - -#[inline(never)] -#[panic_handler] -#[cfg(all(feature = "nightly", not(feature = "semihosting")))] -fn panic(_info: &core::panic::PanicInfo) -> ! { - let gpiod = unsafe { &*hal::stm32::GPIOD::ptr() }; - gpiod.odr.modify(|_, w| w.odr6().high().odr12().high()); // FP_LED_1, FP_LED_3 - #[cfg(feature = "nightly")] - core::intrinsics::abort(); - #[cfg(not(feature = "nightly"))] - unsafe { - core::intrinsics::abort(); - } -} - -#[cfg(feature = "semihosting")] -extern crate panic_semihosting; - -#[cfg(not(any(feature = "nightly", feature = "semihosting")))] -extern crate panic_halt; - -#[macro_use] -extern crate log; - -#[allow(unused_imports)] -use core::convert::TryInto; - -// use core::sync::atomic::{AtomicU32, AtomicBool, Ordering}; -use cortex_m_rt::exception; -use rtic::cyccnt::{Instant, U32Ext}; -use stm32h7xx_hal as hal; -use stm32h7xx_hal::prelude::*; - -use embedded_hal::digital::v2::{InputPin, OutputPin}; - -use hal::{ - dma::{ - config::Priority, - dma::{DMAReq, DmaConfig}, - traits::TargetAddress, - MemoryToPeripheral, PeripheralToMemory, Transfer, - }, - ethernet::{self, PHY}, -}; - -use smoltcp as net; -use smoltcp::iface::Routes; -use smoltcp::wire::Ipv4Address; - -use heapless::{consts::*, String}; - -// The number of ticks in the ADC sampling timer. The timer runs at 100MHz, so the step size is -// equal to 10ns per tick. -// Currently, the sample rate is equal to: Fsample = 100/256 MHz = 390.625 KHz -const ADC_SAMPLE_TICKS_LOG2: u16 = 8; -const ADC_SAMPLE_TICKS: u16 = 1 << ADC_SAMPLE_TICKS_LOG2; - -// The desired ADC sample processing buffer size. -const SAMPLE_BUFFER_SIZE_LOG2: usize = 3; -const SAMPLE_BUFFER_SIZE: usize = 1 << SAMPLE_BUFFER_SIZE_LOG2; - -// The number of cascaded IIR biquads per channel. Select 1 or 2! -const IIR_CASCADE_LENGTH: usize = 1; - -// Frequency scaling factor for lock-in harmonic demodulation. -const HARMONIC: u32 = 1; -// Phase offset applied to the lock-in demodulation signal. -const PHASE_OFFSET: u32 = 0; - -#[link_section = ".sram3.eth"] -static mut DES_RING: ethernet::DesRing = ethernet::DesRing::new(); - -mod adc; -mod afe; -mod dac; -mod design_parameters; -mod digital_input_stamper; -mod eeprom; -mod hrtimer; -mod pounder; -mod server; -mod timers; - -use adc::{Adc0Input, Adc1Input}; -use dac::{Dac0Output, Dac1Output}; -use dsp::{ - iir, iir_int, - reciprocal_pll::TimestampHandler, - trig::{atan2, cossin}, - Complex, -}; -use pounder::DdsOutput; - -#[cfg(not(feature = "semihosting"))] -fn init_log() {} - -#[cfg(feature = "semihosting")] -fn init_log() { - use cortex_m_log::log::{init as init_log, Logger}; - use cortex_m_log::printer::semihosting::{hio::HStdout, InterruptOk}; - use log::LevelFilter; - static mut LOGGER: Option>> = None; - let logger = Logger { - inner: InterruptOk::<_>::stdout().unwrap(), - level: LevelFilter::Info, - }; - let logger = unsafe { LOGGER.get_or_insert(logger) }; - - init_log(logger).unwrap(); -} - -// Pull in build information (from `built` crate) -mod build_info { - #![allow(dead_code)] - // include!(concat!(env!("OUT_DIR"), "/built.rs")); -} - -pub struct NetStorage { - ip_addrs: [net::wire::IpCidr; 1], - neighbor_cache: [Option<(net::wire::IpAddress, net::iface::Neighbor)>; 8], - routes_storage: [Option<(smoltcp::wire::IpCidr, smoltcp::iface::Route)>; 1], -} - -static mut NET_STORE: NetStorage = NetStorage { - // Placeholder for the real IP address, which is initialized at runtime. - ip_addrs: [net::wire::IpCidr::Ipv6( - net::wire::Ipv6Cidr::SOLICITED_NODE_PREFIX, - )], - - neighbor_cache: [None; 8], - - routes_storage: [None; 1], -}; - -const SCALE: f32 = ((1 << 15) - 1) as f32; - -// static ETHERNET_PENDING: AtomicBool = AtomicBool::new(true); - -const TCP_RX_BUFFER_SIZE: usize = 8192; -const TCP_TX_BUFFER_SIZE: usize = 8192; - -type AFE0 = afe::ProgrammableGainAmplifier< - hal::gpio::gpiof::PF2>, - hal::gpio::gpiof::PF5>, ->; - -type AFE1 = afe::ProgrammableGainAmplifier< - hal::gpio::gpiod::PD14>, - hal::gpio::gpiod::PD15>, ->; - -macro_rules! route_request { - ($request:ident, - readable_attributes: [$($read_attribute:tt: $getter:tt),*], - modifiable_attributes: [$($write_attribute:tt: $TYPE:ty, $setter:tt),*]) => { - match $request.req { - server::AccessRequest::Read => { - match $request.attribute { - $( - $read_attribute => { - #[allow(clippy::redundant_closure_call)] - let value = match $getter() { - Ok(data) => data, - Err(_) => return server::Response::error($request.attribute, - "Failed to read attribute"), - }; - - let encoded_data: String = match serde_json_core::to_string(&value) { - Ok(data) => data, - Err(_) => return server::Response::error($request.attribute, - "Failed to encode attribute value"), - }; - - server::Response::success($request.attribute, &encoded_data) - }, - )* - _ => server::Response::error($request.attribute, "Unknown attribute") - } - }, - server::AccessRequest::Write => { - match $request.attribute { - $( - $write_attribute => { - let new_value = match serde_json_core::from_str::<$TYPE>(&$request.value) { - Ok(data) => data, - Err(_) => return server::Response::error($request.attribute, - "Failed to decode value"), - }; - - #[allow(clippy::redundant_closure_call)] - match $setter(new_value) { - Ok(_) => server::Response::success($request.attribute, &$request.value), - Err(_) => server::Response::error($request.attribute, - "Failed to set attribute"), - } - } - )* - _ => server::Response::error($request.attribute, "Unknown attribute") - } - } - } - } -} - -#[rtic::app(device = stm32h7xx_hal::stm32, peripherals = true, monotonic = rtic::cyccnt::CYCCNT)] -const APP: () = { - struct Resources { - afes: (AFE0, AFE1), - adcs: (Adc0Input, Adc1Input), - dacs: (Dac0Output, Dac1Output), - input_stamper: digital_input_stamper::InputStamper, - timestamp_handler: TimestampHandler, - iir_lockin: iir_int::IIR, - iir_state_lockin: [iir_int::IIRState; 2], - - eeprom_i2c: hal::i2c::I2c, - - dds_output: Option, - - // Note: It appears that rustfmt generates a format that GDB cannot recognize, which - // results in GDB breakpoints being set improperly. - #[rustfmt::skip] - net_interface: net::iface::EthernetInterface< - 'static, - 'static, - 'static, - ethernet::EthernetDMA<'static>>, - eth_mac: ethernet::phy::LAN8742A, - mac_addr: net::wire::EthernetAddress, - - pounder: Option, - - pounder_stamper: Option, - - // Format: iir_state[ch][cascade-no][coeff] - #[init([[[0.; 5]; IIR_CASCADE_LENGTH]; 2])] - iir_state: [[iir::IIRState; IIR_CASCADE_LENGTH]; 2], - #[init([[iir::IIR { ba: [1., 0., 0., 0., 0.], y_offset: 0., y_min: -SCALE - 1., y_max: SCALE }; IIR_CASCADE_LENGTH]; 2])] - iir_ch: [[iir::IIR; IIR_CASCADE_LENGTH]; 2], - } - - #[init] - fn init(c: init::Context) -> init::LateResources { - let dp = c.device; - let mut cp = c.core; - - let pwr = dp.PWR.constrain(); - let vos = pwr.freeze(); - - // Enable SRAM3 for the ethernet descriptor ring. - dp.RCC.ahb2enr.modify(|_, w| w.sram3en().set_bit()); - - // Clear reset flags. - dp.RCC.rsr.write(|w| w.rmvf().set_bit()); - - // Select the PLLs for SPI. - dp.RCC - .d2ccip1r - .modify(|_, w| w.spi123sel().pll2_p().spi45sel().pll2_q()); - - let rcc = dp.RCC.constrain(); - let ccdr = rcc - .use_hse(8.mhz()) - .sysclk(400.mhz()) - .hclk(200.mhz()) - .per_ck(100.mhz()) - .pll2_p_ck(100.mhz()) - .pll2_q_ck(100.mhz()) - .freeze(vos, &dp.SYSCFG); - - init_log(); - - let mut delay = hal::delay::Delay::new(cp.SYST, ccdr.clocks); - - let gpioa = dp.GPIOA.split(ccdr.peripheral.GPIOA); - let gpiob = dp.GPIOB.split(ccdr.peripheral.GPIOB); - let gpioc = dp.GPIOC.split(ccdr.peripheral.GPIOC); - let gpiod = dp.GPIOD.split(ccdr.peripheral.GPIOD); - let gpioe = dp.GPIOE.split(ccdr.peripheral.GPIOE); - let gpiof = dp.GPIOF.split(ccdr.peripheral.GPIOF); - let mut gpiog = dp.GPIOG.split(ccdr.peripheral.GPIOG); - - let afe0 = { - let a0_pin = gpiof.pf2.into_push_pull_output(); - let a1_pin = gpiof.pf5.into_push_pull_output(); - afe::ProgrammableGainAmplifier::new(a0_pin, a1_pin) - }; - - let afe1 = { - let a0_pin = gpiod.pd14.into_push_pull_output(); - let a1_pin = gpiod.pd15.into_push_pull_output(); - afe::ProgrammableGainAmplifier::new(a0_pin, a1_pin) - }; - - let dma_streams = - hal::dma::dma::StreamsTuple::new(dp.DMA1, ccdr.peripheral.DMA1); - - // Configure timer 2 to trigger conversions for the ADC - let mut sampling_timer = { - // The timer frequency is manually adjusted below, so the 1KHz setting here is a - // dont-care. - let mut timer2 = - dp.TIM2.timer(1.khz(), ccdr.peripheral.TIM2, &ccdr.clocks); - - // Configure the timer to count at the designed tick rate. We will manually set the - // period below. - timer2.pause(); - timer2.reset_counter(); - timer2.set_tick_freq(design_parameters::TIMER_FREQUENCY); - - let mut sampling_timer = timers::SamplingTimer::new(timer2); - sampling_timer.set_period_ticks((ADC_SAMPLE_TICKS - 1) as u32); - - // The sampling timer is used as the master timer for the shadow-sampling timer. Thus, - // it generates a trigger whenever it is enabled. - - sampling_timer - }; - - let mut shadow_sampling_timer = { - // The timer frequency is manually adjusted below, so the 1KHz setting here is a - // dont-care. - let mut timer3 = - dp.TIM3.timer(1.khz(), ccdr.peripheral.TIM3, &ccdr.clocks); - - // Configure the timer to count at the designed tick rate. We will manually set the - // period below. - timer3.pause(); - timer3.reset_counter(); - timer3.set_tick_freq(design_parameters::TIMER_FREQUENCY); - - let mut shadow_sampling_timer = - timers::ShadowSamplingTimer::new(timer3); - shadow_sampling_timer.set_period_ticks(ADC_SAMPLE_TICKS - 1); - - // The shadow sampling timer is a slave-mode timer to the sampling timer. It should - // always be in-sync - thus, we configure it to operate in slave mode using "Trigger - // mode". - // For TIM3, TIM2 can be made the internal trigger connection using ITR1. Thus, the - // SamplingTimer start now gates the start of the ShadowSamplingTimer. - shadow_sampling_timer.set_slave_mode( - timers::TriggerSource::Trigger1, - timers::SlaveMode::Trigger, - ); - - shadow_sampling_timer - }; - - let sampling_timer_channels = sampling_timer.channels(); - let shadow_sampling_timer_channels = shadow_sampling_timer.channels(); - - let mut timestamp_timer = { - // The timer frequency is manually adjusted below, so the 1KHz setting here is a - // dont-care. - let mut timer5 = - dp.TIM5.timer(1.khz(), ccdr.peripheral.TIM5, &ccdr.clocks); - - // Configure the timer to count at the designed tick rate. We will manually set the - // period below. - timer5.pause(); - timer5.set_tick_freq(design_parameters::TIMER_FREQUENCY); - - // The timestamp timer must run at exactly a multiple of the sample timer based on the - // batch size. To accomodate this, we manually set the prescaler identical to the sample - // timer, but use a period that is longer. - let mut timer = timers::TimestampTimer::new(timer5); - - let period = - digital_input_stamper::calculate_timestamp_timer_period(); - timer.set_period_ticks(period); - - timer - }; - - let timestamp_timer_channels = timestamp_timer.channels(); - - // Configure the SPI interfaces to the ADCs and DACs. - let adcs = { - let adc0 = { - let spi_miso = gpiob - .pb14 - .into_alternate_af5() - .set_speed(hal::gpio::Speed::VeryHigh); - let spi_sck = gpiob - .pb10 - .into_alternate_af5() - .set_speed(hal::gpio::Speed::VeryHigh); - let _spi_nss = gpiob - .pb9 - .into_alternate_af5() - .set_speed(hal::gpio::Speed::VeryHigh); - - let config = hal::spi::Config::new(hal::spi::Mode { - polarity: hal::spi::Polarity::IdleHigh, - phase: hal::spi::Phase::CaptureOnSecondTransition, - }) - .manage_cs() - .suspend_when_inactive() - .communication_mode(hal::spi::CommunicationMode::Receiver) - .cs_delay(design_parameters::ADC_SETUP_TIME); - - let spi: hal::spi::Spi<_, _, u16> = dp.SPI2.spi( - (spi_sck, spi_miso, hal::spi::NoMosi), - config, - design_parameters::ADC_DAC_SCK_MAX, - ccdr.peripheral.SPI2, - &ccdr.clocks, - ); - - Adc0Input::new( - spi, - dma_streams.0, - dma_streams.1, - dma_streams.2, - sampling_timer_channels.ch1, - shadow_sampling_timer_channels.ch1, - ) - }; - - let adc1 = { - let spi_miso = gpiob - .pb4 - .into_alternate_af6() - .set_speed(hal::gpio::Speed::VeryHigh); - let spi_sck = gpioc - .pc10 - .into_alternate_af6() - .set_speed(hal::gpio::Speed::VeryHigh); - let _spi_nss = gpioa - .pa15 - .into_alternate_af6() - .set_speed(hal::gpio::Speed::VeryHigh); - - let config = hal::spi::Config::new(hal::spi::Mode { - polarity: hal::spi::Polarity::IdleHigh, - phase: hal::spi::Phase::CaptureOnSecondTransition, - }) - .manage_cs() - .suspend_when_inactive() - .communication_mode(hal::spi::CommunicationMode::Receiver) - .cs_delay(design_parameters::ADC_SETUP_TIME); - - let spi: hal::spi::Spi<_, _, u16> = dp.SPI3.spi( - (spi_sck, spi_miso, hal::spi::NoMosi), - config, - design_parameters::ADC_DAC_SCK_MAX, - ccdr.peripheral.SPI3, - &ccdr.clocks, - ); - - Adc1Input::new( - spi, - dma_streams.3, - dma_streams.4, - dma_streams.5, - sampling_timer_channels.ch2, - shadow_sampling_timer_channels.ch2, - ) - }; - - (adc0, adc1) - }; - - let dacs = { - let _dac_clr_n = - gpioe.pe12.into_push_pull_output().set_high().unwrap(); - let _dac0_ldac_n = - gpioe.pe11.into_push_pull_output().set_low().unwrap(); - let _dac1_ldac_n = - gpioe.pe15.into_push_pull_output().set_low().unwrap(); - - let dac0_spi = { - let spi_miso = gpioe - .pe5 - .into_alternate_af5() - .set_speed(hal::gpio::Speed::VeryHigh); - let spi_sck = gpioe - .pe2 - .into_alternate_af5() - .set_speed(hal::gpio::Speed::VeryHigh); - let _spi_nss = gpioe - .pe4 - .into_alternate_af5() - .set_speed(hal::gpio::Speed::VeryHigh); - - let config = hal::spi::Config::new(hal::spi::Mode { - polarity: hal::spi::Polarity::IdleHigh, - phase: hal::spi::Phase::CaptureOnSecondTransition, - }) - .manage_cs() - .suspend_when_inactive() - .communication_mode(hal::spi::CommunicationMode::Transmitter) - .swap_mosi_miso(); - - dp.SPI4.spi( - (spi_sck, spi_miso, hal::spi::NoMosi), - config, - design_parameters::ADC_DAC_SCK_MAX, - ccdr.peripheral.SPI4, - &ccdr.clocks, - ) - }; - - let dac1_spi = { - let spi_miso = gpiof - .pf8 - .into_alternate_af5() - .set_speed(hal::gpio::Speed::VeryHigh); - let spi_sck = gpiof - .pf7 - .into_alternate_af5() - .set_speed(hal::gpio::Speed::VeryHigh); - let _spi_nss = gpiof - .pf6 - .into_alternate_af5() - .set_speed(hal::gpio::Speed::VeryHigh); - - let config = hal::spi::Config::new(hal::spi::Mode { - polarity: hal::spi::Polarity::IdleHigh, - phase: hal::spi::Phase::CaptureOnSecondTransition, - }) - .manage_cs() - .communication_mode(hal::spi::CommunicationMode::Transmitter) - .suspend_when_inactive() - .swap_mosi_miso(); - - dp.SPI5.spi( - (spi_sck, spi_miso, hal::spi::NoMosi), - config, - design_parameters::ADC_DAC_SCK_MAX, - ccdr.peripheral.SPI5, - &ccdr.clocks, - ) - }; - - let dac0 = Dac0Output::new( - dac0_spi, - dma_streams.6, - sampling_timer_channels.ch3, - ); - let dac1 = Dac1Output::new( - dac1_spi, - dma_streams.7, - sampling_timer_channels.ch4, - ); - (dac0, dac1) - }; - - let mut fp_led_0 = gpiod.pd5.into_push_pull_output(); - let mut fp_led_1 = gpiod.pd6.into_push_pull_output(); - let mut fp_led_2 = gpiog.pg4.into_push_pull_output(); - let mut fp_led_3 = gpiod.pd12.into_push_pull_output(); - - fp_led_0.set_low().unwrap(); - fp_led_1.set_low().unwrap(); - fp_led_2.set_low().unwrap(); - fp_led_3.set_low().unwrap(); - - // Measure the Pounder PGOOD output to detect if pounder is present on Stabilizer. - let pounder_pgood = gpiob.pb13.into_pull_down_input(); - delay.delay_ms(2u8); - let (pounder_devices, dds_output) = if pounder_pgood.is_high().unwrap() - { - let ad9959 = { - let qspi_interface = { - // Instantiate the QUADSPI pins and peripheral interface. - let qspi_pins = { - let _qspi_ncs = gpioc - .pc11 - .into_alternate_af9() - .set_speed(hal::gpio::Speed::VeryHigh); - - let clk = gpiob - .pb2 - .into_alternate_af9() - .set_speed(hal::gpio::Speed::VeryHigh); - let io0 = gpioe - .pe7 - .into_alternate_af10() - .set_speed(hal::gpio::Speed::VeryHigh); - let io1 = gpioe - .pe8 - .into_alternate_af10() - .set_speed(hal::gpio::Speed::VeryHigh); - let io2 = gpioe - .pe9 - .into_alternate_af10() - .set_speed(hal::gpio::Speed::VeryHigh); - let io3 = gpioe - .pe10 - .into_alternate_af10() - .set_speed(hal::gpio::Speed::VeryHigh); - - (clk, io0, io1, io2, io3) - }; - - let qspi = hal::qspi::Qspi::bank2( - dp.QUADSPI, - qspi_pins, - design_parameters::POUNDER_QSPI_FREQUENCY, - &ccdr.clocks, - ccdr.peripheral.QSPI, - ); - - pounder::QspiInterface::new(qspi).unwrap() - }; - - #[cfg(feature = "pounder_v1_1")] - let reset_pin = gpiog.pg6.into_push_pull_output(); - #[cfg(not(feature = "pounder_v1_1"))] - let reset_pin = gpioa.pa0.into_push_pull_output(); - - let mut io_update = gpiog.pg7.into_push_pull_output(); - - let ref_clk: hal::time::Hertz = - design_parameters::DDS_REF_CLK.into(); - - let ad9959 = ad9959::Ad9959::new( - qspi_interface, - reset_pin, - &mut io_update, - &mut delay, - ad9959::Mode::FourBitSerial, - ref_clk.0 as f32, - design_parameters::DDS_MULTIPLIER, - ) - .unwrap(); - - // Return IO_Update - gpiog.pg7 = io_update.into_analog(); - - ad9959 - }; - - let io_expander = { - let sda = gpiob.pb7.into_alternate_af4().set_open_drain(); - let scl = gpiob.pb8.into_alternate_af4().set_open_drain(); - let i2c1 = dp.I2C1.i2c( - (scl, sda), - 100.khz(), - ccdr.peripheral.I2C1, - &ccdr.clocks, - ); - mcp23017::MCP23017::default(i2c1).unwrap() - }; - - let spi = { - let spi_mosi = gpiod - .pd7 - .into_alternate_af5() - .set_speed(hal::gpio::Speed::VeryHigh); - let spi_miso = gpioa - .pa6 - .into_alternate_af5() - .set_speed(hal::gpio::Speed::VeryHigh); - let spi_sck = gpiog - .pg11 - .into_alternate_af5() - .set_speed(hal::gpio::Speed::VeryHigh); - - let config = hal::spi::Config::new(hal::spi::Mode { - polarity: hal::spi::Polarity::IdleHigh, - phase: hal::spi::Phase::CaptureOnSecondTransition, - }); - - // The maximum frequency of this SPI must be limited due to capacitance on the MISO - // line causing a long RC decay. - dp.SPI1.spi( - (spi_sck, spi_miso, spi_mosi), - config, - 5.mhz(), - ccdr.peripheral.SPI1, - &ccdr.clocks, - ) - }; - - let (adc1, adc2) = { - let (mut adc1, mut adc2) = hal::adc::adc12( - dp.ADC1, - dp.ADC2, - &mut delay, - ccdr.peripheral.ADC12, - &ccdr.clocks, - ); - - let adc1 = { - adc1.calibrate(); - adc1.enable() - }; - - let adc2 = { - adc2.calibrate(); - adc2.enable() - }; - - (adc1, adc2) - }; - - let adc1_in_p = gpiof.pf11.into_analog(); - let adc2_in_p = gpiof.pf14.into_analog(); - - let pounder_devices = pounder::PounderDevices::new( - io_expander, - spi, - adc1, - adc2, - adc1_in_p, - adc2_in_p, - ) - .unwrap(); - - let dds_output = { - let io_update_trigger = { - let _io_update = gpiog - .pg7 - .into_alternate_af2() - .set_speed(hal::gpio::Speed::VeryHigh); - - // Configure the IO_Update signal for the DDS. - let mut hrtimer = hrtimer::HighResTimerE::new( - dp.HRTIM_TIME, - dp.HRTIM_MASTER, - dp.HRTIM_COMMON, - ccdr.clocks, - ccdr.peripheral.HRTIM, - ); - - // IO_Update occurs after a fixed delay from the QSPI write. Note that the timer - // is triggered after the QSPI write, which can take approximately 120nS, so - // there is additional margin. - hrtimer.configure_single_shot( - hrtimer::Channel::Two, - design_parameters::POUNDER_IO_UPDATE_DURATION, - design_parameters::POUNDER_IO_UPDATE_DELAY, - ); - - // Ensure that we have enough time for an IO-update every sample. - let sample_frequency = { - let timer_frequency: hal::time::Hertz = - design_parameters::TIMER_FREQUENCY.into(); - timer_frequency.0 as f32 / ADC_SAMPLE_TICKS as f32 - }; - - let sample_period = 1.0 / sample_frequency; - assert!( - sample_period - > design_parameters::POUNDER_IO_UPDATE_DELAY - ); - - hrtimer - }; - - let (qspi, config) = ad9959.freeze(); - DdsOutput::new(qspi, io_update_trigger, config) - }; - - (Some(pounder_devices), Some(dds_output)) - } else { - (None, None) - }; - - let mut eeprom_i2c = { - let sda = gpiof.pf0.into_alternate_af4().set_open_drain(); - let scl = gpiof.pf1.into_alternate_af4().set_open_drain(); - dp.I2C2.i2c( - (scl, sda), - 100.khz(), - ccdr.peripheral.I2C2, - &ccdr.clocks, - ) - }; - - // Configure ethernet pins. - { - // Reset the PHY before configuring pins. - let mut eth_phy_nrst = gpioe.pe3.into_push_pull_output(); - eth_phy_nrst.set_low().unwrap(); - delay.delay_us(200u8); - eth_phy_nrst.set_high().unwrap(); - let _rmii_ref_clk = gpioa - .pa1 - .into_alternate_af11() - .set_speed(hal::gpio::Speed::VeryHigh); - let _rmii_mdio = gpioa - .pa2 - .into_alternate_af11() - .set_speed(hal::gpio::Speed::VeryHigh); - let _rmii_mdc = gpioc - .pc1 - .into_alternate_af11() - .set_speed(hal::gpio::Speed::VeryHigh); - let _rmii_crs_dv = gpioa - .pa7 - .into_alternate_af11() - .set_speed(hal::gpio::Speed::VeryHigh); - let _rmii_rxd0 = gpioc - .pc4 - .into_alternate_af11() - .set_speed(hal::gpio::Speed::VeryHigh); - let _rmii_rxd1 = gpioc - .pc5 - .into_alternate_af11() - .set_speed(hal::gpio::Speed::VeryHigh); - let _rmii_tx_en = gpiob - .pb11 - .into_alternate_af11() - .set_speed(hal::gpio::Speed::VeryHigh); - let _rmii_txd0 = gpiob - .pb12 - .into_alternate_af11() - .set_speed(hal::gpio::Speed::VeryHigh); - let _rmii_txd1 = gpiog - .pg14 - .into_alternate_af11() - .set_speed(hal::gpio::Speed::VeryHigh); - } - - let mac_addr = match eeprom::read_eui48(&mut eeprom_i2c) { - Err(_) => { - info!("Could not read EEPROM, using default MAC address"); - net::wire::EthernetAddress([0x10, 0xE2, 0xD5, 0x00, 0x03, 0x00]) - } - Ok(raw_mac) => net::wire::EthernetAddress(raw_mac), - }; - - let (network_interface, eth_mac) = { - // Configure the ethernet controller - let (eth_dma, eth_mac) = unsafe { - ethernet::new_unchecked( - dp.ETHERNET_MAC, - dp.ETHERNET_MTL, - dp.ETHERNET_DMA, - &mut DES_RING, - mac_addr, - ccdr.peripheral.ETH1MAC, - &ccdr.clocks, - ) - }; - - // Reset and initialize the ethernet phy. - let mut lan8742a = - ethernet::phy::LAN8742A::new(eth_mac.set_phy_addr(0)); - lan8742a.phy_reset(); - lan8742a.phy_init(); - - unsafe { ethernet::enable_interrupt() }; - - let store = unsafe { &mut NET_STORE }; - - store.ip_addrs[0] = net::wire::IpCidr::new( - net::wire::IpAddress::v4(10, 0, 16, 99), - 24, - ); - - let default_v4_gw = Ipv4Address::new(10, 0, 16, 1); - let mut routes = Routes::new(&mut store.routes_storage[..]); - routes.add_default_ipv4_route(default_v4_gw).unwrap(); - - let neighbor_cache = - net::iface::NeighborCache::new(&mut store.neighbor_cache[..]); - - let interface = net::iface::EthernetInterfaceBuilder::new(eth_dma) - .ethernet_addr(mac_addr) - .neighbor_cache(neighbor_cache) - .ip_addrs(&mut store.ip_addrs[..]) - .routes(routes) - .finalize(); - - (interface, lan8742a) - }; - - cp.SCB.enable_icache(); - - // info!("Version {} {}", build_info::PKG_VERSION, build_info::GIT_VERSION.unwrap()); - // info!("Built on {}", build_info::BUILT_TIME_UTC); - // info!("{} {}", build_info::RUSTC_VERSION, build_info::TARGET); - - // Utilize the cycle counter for RTIC scheduling. - cp.DWT.enable_cycle_counter(); - - let mut input_stamper = { - let trigger = gpioa.pa3.into_alternate_af2(); - digital_input_stamper::InputStamper::new( - trigger, - timestamp_timer_channels.ch4, - ) - }; - - #[cfg(feature = "pounder_v1_1")] - let pounder_stamper = { - let dma2_streams = - hal::dma::dma::StreamsTuple::new(dp.DMA2, ccdr.peripheral.DMA2); - - let etr_pin = gpioa.pa0.into_alternate_af3(); - - // The frequency in the constructor is dont-care, as we will modify the period + clock - // source manually below. - let tim8 = - dp.TIM8.timer(1.khz(), ccdr.peripheral.TIM8, &ccdr.clocks); - let mut timestamp_timer = timers::PounderTimestampTimer::new(tim8); - - // Pounder is configured to generate a 500MHz reference clock, so a 125MHz sync-clock is - // output. As a result, dividing the 125MHz sync-clk provides a 31.25MHz tick rate for - // the timestamp timer. 31.25MHz corresponds with a 32ns tick rate. - timestamp_timer.set_external_clock(timers::Prescaler::Div4); - timestamp_timer.start(); - - // We want the pounder timestamp timer to overflow once per batch. - let tick_ratio = { - let sync_clk_mhz: f32 = design_parameters::DDS_SYSTEM_CLK.0 - as f32 - / design_parameters::DDS_SYNC_CLK_DIV as f32; - sync_clk_mhz / design_parameters::TIMER_FREQUENCY.0 as f32 - }; - - let period = (tick_ratio - * ADC_SAMPLE_TICKS as f32 - * SAMPLE_BUFFER_SIZE as f32) as u32 - / 4; - timestamp_timer.set_period_ticks((period - 1).try_into().unwrap()); - let tim8_channels = timestamp_timer.channels(); - - let stamper = pounder::timestamp::Timestamper::new( - timestamp_timer, - dma2_streams.0, - tim8_channels.ch1, - &mut sampling_timer, - etr_pin, - ); - - Some(stamper) - }; - - #[cfg(not(feature = "pounder_v1_1"))] - let pounder_stamper = None; - - let timestamp_handler = TimestampHandler::new( - 4, - 3, - ADC_SAMPLE_TICKS_LOG2 as usize, - SAMPLE_BUFFER_SIZE_LOG2, - ); - let iir_lockin = iir_int::IIR::default(); - let iir_state_lockin = [iir_int::IIRState::default(); 2]; - - // Start sampling ADCs. - sampling_timer.start(); - timestamp_timer.start(); - input_stamper.start(); - - init::LateResources { - afes: (afe0, afe1), - - adcs, - dacs, - input_stamper, - dds_output, - pounder: pounder_devices, - pounder_stamper, - - timestamp_handler, - iir_lockin, - iir_state_lockin, - - eeprom_i2c, - net_interface: network_interface, - eth_mac, - mac_addr, - } - } - - /// Main DSP processing routine for Stabilizer. - /// - /// # Note - /// Processing time for the DSP application code is bounded by the following constraints: - /// - /// DSP application code starts after the ADC has generated a batch of samples and must be - /// completed by the time the next batch of ADC samples has been acquired (plus the FIFO buffer - /// time). If this constraint is not met, firmware will panic due to an ADC input overrun. - /// - /// The DSP application code must also fill out the next DAC output buffer in time such that the - /// DAC can switch to it when it has completed the current buffer. If this constraint is not met - /// it's possible that old DAC codes will be generated on the output and the output samples will - /// be delayed by 1 batch. - /// - /// Because the ADC and DAC operate at the same rate, these two constraints actually implement - /// the same time bounds, meeting one also means the other is also met. - #[task(binds=DMA1_STR4, resources=[pounder_stamper, adcs, dacs, iir_state, iir_ch, dds_output, input_stamper, timestamp_handler, iir_lockin, iir_state_lockin], priority=2)] - fn process(c: process::Context) { - if let Some(stamper) = c.resources.pounder_stamper { - let pounder_timestamps = stamper.acquire_buffer(); - info!("{:?}", pounder_timestamps); - } - - let adc_samples = [ - c.resources.adcs.0.acquire_buffer(), - c.resources.adcs.1.acquire_buffer(), - ]; - - let dac_samples = [ - c.resources.dacs.0.acquire_buffer(), - c.resources.dacs.1.acquire_buffer(), - ]; - - let iir_lockin = c.resources.iir_lockin; - let iir_state_lockin = c.resources.iir_state_lockin; - let iir_ch = c.resources.iir_ch; - let iir_state = c.resources.iir_state; - - let (pll_phase, pll_frequency) = c - .resources - .timestamp_handler - .update(c.resources.input_stamper.latest_timestamp()); - let frequency = pll_frequency.wrapping_mul(HARMONIC); - let mut phase = - PHASE_OFFSET.wrapping_add(pll_phase.wrapping_mul(HARMONIC)); - - for i in 0..adc_samples[0].len() { - let m = cossin((phase as i32).wrapping_neg()); - phase = phase.wrapping_add(frequency); - - let signal = Complex( - iir_lockin.update( - &mut iir_state_lockin[0], - ((adc_samples[0][i] as i64 * m.0 as i64) >> 16) as _, - ), - iir_lockin.update( - &mut iir_state_lockin[1], - ((adc_samples[0][i] as i64 * m.1 as i64) >> 16) as _, - ), - ); - - let mut magnitude = - (signal.0 * signal.0 + signal.1 * signal.1) as _; - let mut phase = atan2(signal.1, signal.0) as _; - - for j in 0..iir_state[0].len() { - magnitude = - iir_ch[0][j].update(&mut iir_state[0][j], magnitude); - phase = iir_ch[1][j].update(&mut iir_state[1][j], phase); - } - - // Note(unsafe): range clipping to i16 is ensured by IIR filters above. - unsafe { - dac_samples[0][i] = - magnitude.to_int_unchecked::() as u16 ^ 0x8000; - dac_samples[1][i] = - phase.to_int_unchecked::() as u16 ^ 0x8000; - } - } - - if let Some(dds_output) = c.resources.dds_output { - let builder = dds_output.builder().update_channels( - &[pounder::Channel::Out0.into()], - Some(u32::MAX / 4), - None, - None, - ); - - builder.write_profile(); - } - } - - #[idle(resources=[net_interface, pounder, mac_addr, eth_mac, iir_state, iir_ch, afes])] - fn idle(mut c: idle::Context) -> ! { - let mut socket_set_entries: [_; 8] = Default::default(); - let mut sockets = - net::socket::SocketSet::new(&mut socket_set_entries[..]); - - let mut rx_storage = [0; TCP_RX_BUFFER_SIZE]; - let mut tx_storage = [0; TCP_TX_BUFFER_SIZE]; - let tcp_handle = { - let tcp_rx_buffer = - net::socket::TcpSocketBuffer::new(&mut rx_storage[..]); - let tcp_tx_buffer = - net::socket::TcpSocketBuffer::new(&mut tx_storage[..]); - let tcp_socket = - net::socket::TcpSocket::new(tcp_rx_buffer, tcp_tx_buffer); - sockets.add(tcp_socket) - }; - - let mut server = server::Server::new(); - - let mut time = 0u32; - let mut next_ms = Instant::now(); - - // TODO: Replace with reference to CPU clock from CCDR. - next_ms += 400_000.cycles(); - - loop { - let tick = Instant::now() > next_ms; - - if tick { - next_ms += 400_000.cycles(); - time += 1; - } - - { - let socket = - &mut *sockets.get::(tcp_handle); - if socket.state() == net::socket::TcpState::CloseWait { - socket.close(); - } else if !(socket.is_open() || socket.is_listening()) { - socket - .listen(1235) - .unwrap_or_else(|e| warn!("TCP listen error: {:?}", e)); - } else { - server.poll(socket, |req| { - info!("Got request: {:?}", req); - route_request!(req, - readable_attributes: [ - "stabilizer/iir/state": (|| { - let state = c.resources.iir_state.lock(|iir_state| - server::Status { - t: time, - x0: iir_state[0][0][0], - y0: iir_state[0][0][2], - x1: iir_state[1][0][0], - y1: iir_state[1][0][2], - }); - - Ok::(state) - }), - // "_b" means cascades 2nd IIR - "stabilizer/iir_b/state": (|| { - let state = c.resources.iir_state.lock(|iir_state| - server::Status { - t: time, - x0: iir_state[0][IIR_CASCADE_LENGTH-1][0], - y0: iir_state[0][IIR_CASCADE_LENGTH-1][2], - x1: iir_state[1][IIR_CASCADE_LENGTH-1][0], - y1: iir_state[1][IIR_CASCADE_LENGTH-1][2], - }); - - Ok::(state) - }), - "stabilizer/afe0/gain": (|| c.resources.afes.0.get_gain()), - "stabilizer/afe1/gain": (|| c.resources.afes.1.get_gain()) - ], - - modifiable_attributes: [ - "stabilizer/iir0/state": server::IirRequest, (|req: server::IirRequest| { - c.resources.iir_ch.lock(|iir_ch| { - if req.channel > 1 { - return Err(()); - } - - iir_ch[req.channel as usize][0] = req.iir; - - Ok::(req) - }) - }), - "stabilizer/iir1/state": server::IirRequest, (|req: server::IirRequest| { - c.resources.iir_ch.lock(|iir_ch| { - if req.channel > 1 { - return Err(()); - } - - iir_ch[req.channel as usize][0] = req.iir; - - Ok::(req) - }) - }), - "stabilizer/iir_b0/state": server::IirRequest, (|req: server::IirRequest| { - c.resources.iir_ch.lock(|iir_ch| { - if req.channel > 1 { - return Err(()); - } - - iir_ch[req.channel as usize][IIR_CASCADE_LENGTH-1] = req.iir; - - Ok::(req) - }) - }), - "stabilizer/iir_b1/state": server::IirRequest,(|req: server::IirRequest| { - c.resources.iir_ch.lock(|iir_ch| { - if req.channel > 1 { - return Err(()); - } - - iir_ch[req.channel as usize][IIR_CASCADE_LENGTH-1] = req.iir; - - Ok::(req) - }) - }), - "stabilizer/afe0/gain": afe::Gain, (|gain| { - c.resources.afes.0.set_gain(gain); - Ok::<(), ()>(()) - }), - "stabilizer/afe1/gain": afe::Gain, (|gain| { - c.resources.afes.1.set_gain(gain); - Ok::<(), ()>(()) - }) - ] - ) - }); - } - } - - let sleep = match c.resources.net_interface.poll( - &mut sockets, - net::time::Instant::from_millis(time as i64), - ) { - Ok(changed) => !changed, - Err(net::Error::Unrecognized) => true, - Err(e) => { - info!("iface poll error: {:?}", e); - true - } - }; - - if sleep { - cortex_m::asm::wfi(); - } - } - } - - #[task(binds = ETH, priority = 1)] - fn eth(_: eth::Context) { - unsafe { ethernet::interrupt_handler() } - } - - #[task(binds = SPI2, priority = 3)] - fn spi2(_: spi2::Context) { - panic!("ADC0 input overrun"); - } - - #[task(binds = SPI3, priority = 3)] - fn spi3(_: spi3::Context) { - panic!("ADC0 input overrun"); - } - - #[task(binds = SPI4, priority = 3)] - fn spi4(_: spi4::Context) { - panic!("DAC0 output error"); - } - - #[task(binds = SPI5, priority = 3)] - fn spi5(_: spi5::Context) { - panic!("DAC1 output error"); - } - - extern "C" { - // hw interrupt handlers for RTIC to use for scheduling tasks - // one per priority - fn DCMI(); - fn JPEG(); - fn SDMMC(); - } -}; - -#[exception] -fn HardFault(ef: &cortex_m_rt::ExceptionFrame) -> ! { - panic!("HardFault at {:#?}", ef); -} - -#[exception] -fn DefaultHandler(irqn: i16) { - panic!("Unhandled exception (IRQn = {})", irqn); -} diff --git a/src/server.rs b/src/server.rs index 2803805..5e6950d 100644 --- a/src/server.rs +++ b/src/server.rs @@ -1,13 +1,64 @@ -use heapless::{consts::*, String, Vec}; - use core::fmt::Write; - +use heapless::{consts::*, String, Vec}; use serde::{Deserialize, Serialize}; - use serde_json_core::{de::from_slice, ser::to_string}; +use smoltcp as net; -use super::iir; -use super::net; +use dsp::iir; + +#[macro_export] +macro_rules! route_request { + ($request:ident, + readable_attributes: [$($read_attribute:tt: $getter:tt),*], + modifiable_attributes: [$($write_attribute:tt: $TYPE:ty, $setter:tt),*]) => { + match $request.req { + server::AccessRequest::Read => { + match $request.attribute { + $( + $read_attribute => { + #[allow(clippy::redundant_closure_call)] + let value = match $getter() { + Ok(data) => data, + Err(_) => return server::Response::error($request.attribute, + "Failed to read attribute"), + }; + + let encoded_data: String = match serde_json_core::to_string(&value) { + Ok(data) => data, + Err(_) => return server::Response::error($request.attribute, + "Failed to encode attribute value"), + }; + + server::Response::success($request.attribute, &encoded_data) + }, + )* + _ => server::Response::error($request.attribute, "Unknown attribute") + } + }, + server::AccessRequest::Write => { + match $request.attribute { + $( + $write_attribute => { + let new_value = match serde_json_core::from_str::<$TYPE>(&$request.value) { + Ok(data) => data, + Err(_) => return server::Response::error($request.attribute, + "Failed to decode value"), + }; + + #[allow(clippy::redundant_closure_call)] + match $setter(new_value) { + Ok(_) => server::Response::success($request.attribute, &$request.value), + Err(_) => server::Response::error($request.attribute, + "Failed to set attribute"), + } + } + )* + _ => server::Response::error($request.attribute, "Unknown attribute") + } + } + } + } +} #[derive(Deserialize, Serialize, Debug)] pub enum AccessRequest {