From 8d807fa09bee71c124578334c1d4012bb5a7dec4 Mon Sep 17 00:00:00 2001 From: Ryan Summers Date: Mon, 9 Nov 2020 12:30:02 +0100 Subject: [PATCH] Adding WIP --- ad9959/src/lib.rs | 95 ++++++++--------------- src/hrtimer.rs | 60 +++++++++----- src/main.rs | 123 +++++++++-------------------- src/pounder/mod.rs | 189 ++++++++++++++++++--------------------------- 4 files changed, 185 insertions(+), 282 deletions(-) diff --git a/ad9959/src/lib.rs b/ad9959/src/lib.rs index 1a5de18..5a2d132 100644 --- a/ad9959/src/lib.rs +++ b/ad9959/src/lib.rs @@ -13,12 +13,10 @@ use embedded_hal::{blocking::delay::DelayMs, digital::v2::OutputPin}; /// /// The chip supports a number of serial interfaces to improve data throughput, including normal, /// dual, and quad SPI configurations. -pub struct Ad9959 { +pub struct Ad9959 { pub interface: INTERFACE, - delay: DELAY, reference_clock_frequency: f32, system_clock_multiplier: u8, - io_update: UPDATE, communication_mode: Mode, } @@ -31,8 +29,6 @@ pub trait Interface { fn write(&mut self, addr: u8, data: &[u8]) -> Result<(), Self::Error>; fn read(&mut self, addr: u8, dest: &mut [u8]) -> Result<(), Self::Error>; - - fn write_profile(&mut self, data: [u32; 4]) -> Result<(), Self::Error>; } /// Indicates various communication modes of the DDS. The value of this enumeration is equivalent to @@ -93,12 +89,7 @@ pub enum Error { Frequency, } -impl Ad9959 -where - INTERFACE: Interface, - DELAY: DelayMs, - UPDATE: OutputPin, -{ +impl Ad9959 { /// Construct and initialize the DDS. /// /// Args: @@ -110,36 +101,26 @@ where /// * `clock_frequency` - The clock frequency of the reference clock input. /// * `multiplier` - The desired clock multiplier for the system clock. This multiplies /// `clock_frequency` to generate the system clock. - pub fn new( - interface: INTERFACE, - reset_pin: &mut RST, - io_update: UPDATE, - delay: DELAY, + pub fn new( + interface: I, + reset_pin: &mut impl OutputPin, + delay: &mut impl DelayMs, desired_mode: Mode, clock_frequency: f32, multiplier: u8, - ) -> Result - where - RST: OutputPin, - { + ) -> Result { let mut ad9959 = Ad9959 { interface, - io_update, - delay, reference_clock_frequency: clock_frequency, system_clock_multiplier: 1, communication_mode: desired_mode, }; - ad9959.io_update.set_low().or_else(|_| Err(Error::Pin))?; - // Reset the AD9959 reset_pin.set_high().or_else(|_| Err(Error::Pin))?; // Delay for a clock cycle to allow the device to reset. - ad9959 - .delay - .delay_ms((1000.0 / clock_frequency as f32) as u8); + delay.delay_ms((1000.0 / clock_frequency as f32) as u8); reset_pin.set_low().or_else(|_| Err(Error::Pin))?; @@ -156,9 +137,6 @@ where .write(Register::CSR as u8, &csr) .map_err(|_| Error::Interface)?; - // Latch the configuration registers to make them active. - ad9959.latch_configuration()?; - ad9959 .interface .configure_mode(desired_mode) @@ -179,19 +157,6 @@ where Ok(ad9959) } - /// Latch the DDS configuration to ensure it is active on the output channels. - pub fn latch_configuration(&mut self) -> Result<(), Error> { - self.delay.delay_ms(2); - self.io_update.set_high().or_else(|_| Err(Error::Pin))?; - // The SYNC_CLK is 1/4 the system clock frequency. The IO_UPDATE pin must be latched for one - // full SYNC_CLK pulse to register. For safety, we latch for 5 here. - self.delay - .delay_ms((5000.0 / self.system_clock_frequency()) as u8); - self.io_update.set_low().or_else(|_| Err(Error::Pin))?; - - Ok(()) - } - /// Configure the internal system clock of the chip. /// /// Arguments: @@ -232,8 +197,6 @@ where .map_err(|_| Error::Interface)?; self.system_clock_multiplier = multiplier; - self.latch_configuration()?; - Ok(self.system_clock_frequency()) } @@ -337,10 +300,6 @@ where .write(register as u8, &data) .map_err(|_| Error::Interface)?; - // Latch the configuration and restore the previous CSR. Note that the re-enable of the - // channel happens immediately, so the CSR update does not need to be latched. - self.latch_configuration()?; - Ok(()) } @@ -529,43 +488,55 @@ where let tuning_word = u32::from_be_bytes(tuning_word); // Convert the tuning word into a frequency. - Ok((tuning_word as f32 * self.system_clock_frequency()) / (1u64 << 32) as f32) + Ok((tuning_word as f32 * self.system_clock_frequency()) + / (1u64 << 32) as f32) } - pub fn write_profile(&mut self, channel: Channel, freq: f32, turns: f32, amplitude: f32) -> Result<(), Error> { - + pub fn serialize_profile( + &self, + channel: Channel, + freq: f32, + turns: f32, + amplitude: f32, + ) -> Result<[u32; 4], Error> { let csr: u8 = *0x00_u8 .set_bits(1..=2, self.communication_mode as u8) .set_bit(4 + channel as usize, true); // The function for channel frequency is `f_out = FTW * f_s / 2^32`, where FTW is the // frequency tuning word and f_s is the system clock rate. - let tuning_word: u32 = ((freq * (1u64 << 32) as f32) / self.system_clock_frequency()) as u32; + let tuning_word: u32 = ((freq * (1u64 << 32) as f32) + / self.system_clock_frequency()) + as u32; let phase_offset: u16 = (turns * (1 << 14) as f32) as u16 & 0x3FFFu16; - let pow: u32 = *0u32.set_bits(24..32, Register::CPOW0 as u32) - .set_bits(8..24, phase_offset as u32) - .set_bits(0..8, Register::CFTW0 as u32); - + let pow: u32 = *0u32 + .set_bits(24..32, Register::CPOW0 as u32) + .set_bits(8..24, phase_offset as u32) + .set_bits(0..8, Register::CFTW0 as u32); // Enable the amplitude multiplier for the channel if required. The amplitude control has // full-scale at 0x3FF (amplitude of 1), so the multiplier should be disabled whenever // full-scale is used. let amplitude_control: u16 = (amplitude * (1 << 10) as f32) as u16; - let acr: u32 = *0u32.set_bits(24..32, Register::ACR as u32) + let acr: u32 = *0u32 + .set_bits(24..32, Register::ACR as u32) .set_bits(0..10, amplitude_control as u32 & 0x3FF) .set_bit(12, amplitude_control < (1 << 10)); let serialized: [u32; 4] = [ - u32::from_le_bytes([Register::CSR as u8, csr, Register::CSR as u8, csr]), + u32::from_le_bytes([ + Register::CSR as u8, + csr, + Register::CSR as u8, + csr, + ]), acr.to_be(), pow.to_be(), tuning_word.to_be(), ]; - self.interface.write_profile(serialized).map_err(|_| Error::Interface)?; - - Ok(()) + Ok(serialized) } } diff --git a/src/hrtimer.rs b/src/hrtimer.rs index ff074d1..4af2144 100644 --- a/src/hrtimer.rs +++ b/src/hrtimer.rs @@ -1,12 +1,12 @@ use crate::hal; -use hal::rcc::{CoreClocks, ResetEnable, rec}; +use hal::rcc::{rec, CoreClocks, ResetEnable}; pub enum Channel { One, Two, } -struct HighResTimerE { +pub struct HighResTimerE { master: hal::stm32::HRTIM_MASTER, timer: hal::stm32::HRTIM_TIME, common: hal::stm32::HRTIM_COMMON, @@ -15,15 +15,29 @@ struct HighResTimerE { } impl HighResTimerE { - pub fn new(timer_regs: hal::stm32::HRTIM_TIME, clocks: CoreClocks, prec: rec::Hrtim) -> Self { - let master = unsafe { &*hal::stm32::HRTIM_MASTER::ptr() }; - let common = unsafe { &*hal::stm32::HRTIM_COMMON::ptr() }; + pub fn new( + timer_regs: hal::stm32::HRTIM_TIME, + master_regs: hal::stm32::HRTIM_MASTER, + common_regs: hal::stm32::HRTIM_COMMON, + clocks: CoreClocks, + prec: rec::Hrtim, + ) -> Self { prec.reset().enable(); - Self { master, timer: timer_regs, common, clocks } + Self { + master: master_regs, + timer: timer_regs, + common: common_regs, + clocks, + } } - pub fn configure_single_shot(&mut self, channel: Channel, set_duration: f32, set_offset: f32) { + pub fn configure_single_shot( + &mut self, + channel: Channel, + set_duration: f32, + set_offset: f32, + ) { // Disable the timer before configuration. self.master.mcr.modify(|_, w| w.tecen().clear_bit()); @@ -32,12 +46,12 @@ impl HighResTimerE { // is the APB bus clock. let minimum_duration = set_duration + set_offset; - let source_frequency = self.clocks.timy_ker_ck; - let source_cycles = minimum_duration * source_frequency; + let source_frequency: u32 = self.clocks.timy_ker_ck().0; + let source_cycles = (minimum_duration * source_frequency as f32) as u32; // Determine the clock divider, which may be 1, 2, or 4. We will choose a clock divider that // allows us the highest resolution per tick, so lower dividers are favored. - let divider = if source_cycles < 0xFFDF { + let divider: u8 = if source_cycles < 0xFFDF { 1 } else if (source_cycles / 2) < 0xFFDF { 2 @@ -48,26 +62,32 @@ impl HighResTimerE { }; // The period register must be greater than or equal to 3 cycles. - assert!((source_cycles / divider) > 2); + let period = (source_cycles / divider as u32) as u16; + assert!(period > 2); // We now have the prescaler and the period registers. Configure the timer. - self.timer.timecr.modify(|_, w| unsafe{w.ck_pscx().bits(divider)}) - self.timer.perer.write(|w| unsafe{w.per().bits(source_cycles / divider)}); + self.timer + .timecr + .modify(|_, w| unsafe { w.ck_pscx().bits(divider) }); + self.timer.perer.write(|w| unsafe { w.perx().bits(period) }); // Configure the comparator 1 level. - self.timer.cmpe1r.write(|w| unsafe{w.cmp1().bits(set_offset * source_frequency)}); + let offset = (set_offset * source_frequency as f32) as u16; + self.timer + .cmp1er + .write(|w| unsafe { w.cmp1x().bits(offset) }); // Configure the set/reset signals. // Set on compare with CMP1, reset upon reaching PER match channel { Channel::One => { - self.timer.sete1r().write(|w| w.cmp1().set_bit()); - self.timer.resete1r().write(|w| w.per().set_bit()); - }, + self.timer.sete1r.write(|w| w.cmp1().set_bit()); + self.timer.rste1r.write(|w| w.per().set_bit()); + } Channel::Two => { - self.timer.sete2r().write(|w| w.cmp1().set_bit()); - self.timer.resete2r().write(|w| w.per().set_bit()); - }, + self.timer.sete2r.write(|w| w.cmp1().set_bit()); + self.timer.rste2r.write(|w| w.per().set_bit()); + } } // Enable the timer now that it is configured. diff --git a/src/main.rs b/src/main.rs index 2485133..db0558a 100644 --- a/src/main.rs +++ b/src/main.rs @@ -45,6 +45,7 @@ static mut DES_RING: ethernet::DesRing = ethernet::DesRing::new(); mod afe; mod eeprom; +mod hrtimer; mod iir; mod pounder; mod server; @@ -187,11 +188,12 @@ const APP: () = { 'static, 'static, 'static, - ethernet::EthernetDMA<'static>>, + ethernet::EthernetDMA<'static>, + >, eth_mac: ethernet::EthernetMAC, mac_addr: net::wire::EthernetAddress, - pounder: Option>, + pounder: Option, #[init([[0.; 5]; 2])] iir_state: [iir::IIRState; 2], @@ -439,41 +441,19 @@ const APP: () = { .set_speed(hal::gpio::Speed::VeryHigh); let qspi = - hal::qspi::Qspi::new(dp.QUADSPI, &mut clocks, 10.mhz()) + hal::qspi::Qspi::new(dp.QUADSPI, &mut clocks, 50.mhz()) .unwrap(); pounder::QspiInterface::new(qspi).unwrap() }; let mut reset_pin = gpioa.pa0.into_push_pull_output(); - // Configure the IO_Update signal for the DDS. - let mut hrtimer = HighResTimerE::new(dp.HRTIM_TIME, ccdr.clocks, ccdr.peripheral.HRTIM); - - // IO_Update should be latched for 50ns after the QSPI profile write. 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 10MHz, so this comes out to an - // offset of 3.2uS. - // TODO: This currently does not meet the 2uS timing that we have for profile - // updates, since we want to send a profile update for every DAC update. We should - // increase the QSPI clock frequency. - hrtimer.configure_single_shot(hrtimer::Channel::Two, 50e-9, 3.2e-6); - - let io_update = gpiog.pg7.into_push_pull_output(); - - let asm_delay = { - let frequency_hz = clocks.clocks.c_ck().0; - asm_delay::AsmDelay::new(asm_delay::bitrate::Hertz( - frequency_hz, - )) - }; - ad9959::Ad9959::new( qspi_interface, &mut reset_pin, - io_update, - asm_delay, + &mut delay, ad9959::Mode::FourBitSerial, - 100_000_000f32, + 100_000_000_f32, 5, ) .unwrap() @@ -533,10 +513,39 @@ const APP: () = { let adc1_in_p = gpiof.pf11.into_analog(); let adc2_in_p = gpiof.pf14.into_analog(); + 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, + clocks.clocks, + clocks.peripheral.HRTIM, + ); + + // IO_Update should be latched for 50ns after the QSPI profile write. 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 50MHz, so this comes out to an + // offset of 640nS. We use 900ns to be safe. + hrtimer.configure_single_shot( + hrtimer::Channel::Two, + 50_e-9, + 900_e-9, + ); + + hrtimer + }; + Some( pounder::PounderDevices::new( io_expander, ad9959, + io_update_trigger, spi, adc1, adc2, @@ -759,38 +768,6 @@ const APP: () = { // TODO: Replace with reference to CPU clock from CCDR. next_ms += 400_000.cycles(); - match c.resources.pounder { - Some(pounder) => { - pounder.ad9959.interface.start_stream(); - - let state = pounder::ChannelState { - parameters: pounder::DdsChannelState { - phase_offset: 0.0, - frequency: 100_000_000.0, - amplitude: 1.0, - enabled: true, - }, - attenuation: 0.0, - }; - - let state1 = pounder::ChannelState { - parameters: pounder::DdsChannelState { - phase_offset: 0.5, - frequency: 100_000_000.0, - amplitude: 1.0, - enabled: true, - }, - attenuation: 0.0, - }; - - pounder.set_channel_state(pounder::Channel::Out0, state).unwrap(); - pounder.set_channel_state(pounder::Channel::Out1, state1).unwrap(); - - pounder.ad9959.latch_configuration().unwrap(); - }, - _ => panic!("Failed"), - } - loop { let tick = Instant::now() > next_ms; @@ -827,34 +804,6 @@ const APP: () = { }), "stabilizer/afe0/gain": (|| c.resources.afe0.get_gain()), "stabilizer/afe1/gain": (|| c.resources.afe1.get_gain()), - "pounder/in0": (|| { - match c.resources.pounder { - Some(pounder) => - pounder.get_input_channel_state(pounder::Channel::In0), - _ => Err(pounder::Error::Access), - } - }), - "pounder/in1": (|| { - match c.resources.pounder { - Some(pounder) => - pounder.get_input_channel_state(pounder::Channel::In1), - _ => Err(pounder::Error::Access), - } - }), - "pounder/out0": (|| { - match c.resources.pounder { - Some(pounder) => - pounder.get_output_channel_state(pounder::Channel::Out0), - _ => Err(pounder::Error::Access), - } - }), - "pounder/out1": (|| { - match c.resources.pounder { - Some(pounder) => - pounder.get_output_channel_state(pounder::Channel::Out1), - _ => Err(pounder::Error::Access), - } - }), "pounder/dds/clock": (|| { match c.resources.pounder { Some(pounder) => pounder.get_dds_clock_config(), diff --git a/src/pounder/mod.rs b/src/pounder/mod.rs index 78062e5..1e3c8c9 100644 --- a/src/pounder/mod.rs +++ b/src/pounder/mod.rs @@ -4,6 +4,7 @@ mod attenuators; mod rf_power; use super::hal; +use super::hrtimer::HighResTimerE; use attenuators::AttenuatorInterface; use rf_power::PowerMeasurementInterface; @@ -111,9 +112,54 @@ impl QspiInterface { }) } - pub fn start_stream(&mut self) { + pub fn start_stream(&mut self) -> Result<(), Error> { + if self.qspi.is_busy() { + return Err(Error::Qspi); + } + + // Configure QSPI for infinite transaction mode using only a data phase (no instruction or + // address). + let qspi_regs = unsafe { &*hal::stm32::QUADSPI::ptr() }; + qspi_regs.fcr.modify(|_, w| w.ctcf().set_bit()); + + unsafe { + qspi_regs.dlr.write(|w| w.dl().bits(0xFFFF_FFFF)); + qspi_regs + .ccr + .modify(|_, w| w.imode().bits(0).fmode().bits(1)); + } + self.streaming = true; - self.qspi.enter_write_stream_mode().unwrap(); + + Ok(()) + } + + pub fn write_profile(&mut self, data: [u32; 4]) -> Result<(), Error> { + if self.streaming == false { + return Err(Error::Qspi); + } + + let qspi_regs = unsafe { &*hal::stm32::QUADSPI::ptr() }; + unsafe { + core::ptr::write_volatile( + &qspi_regs.dr as *const _ as *mut u32, + data[0], + ); + core::ptr::write_volatile( + &qspi_regs.dr as *const _ as *mut u32, + data[1], + ); + core::ptr::write_volatile( + &qspi_regs.dr as *const _ as *mut u32, + data[2], + ); + core::ptr::write_volatile( + &qspi_regs.dr as *const _ as *mut u32, + data[3], + ); + } + + Ok(()) } } @@ -223,11 +269,6 @@ impl ad9959::Interface for QspiInterface { } } - fn write_profile(&mut self, data: [u32; 4]) -> Result<(), Error> { - self.qspi.write_profile(data); - Ok(()) - } - fn read(&mut self, addr: u8, dest: &mut [u8]) -> Result<(), Error> { if (addr & 0x80) != 0 { return Err(Error::InvalidAddress); @@ -238,17 +279,16 @@ impl ad9959::Interface for QspiInterface { return Err(Error::Qspi); } - self.qspi.read(0x80_u8 | addr, dest).map_err(|_| Error::Qspi) + self.qspi + .read(0x80_u8 | addr, dest) + .map_err(|_| Error::Qspi) } } /// A structure containing implementation for Pounder hardware. -pub struct PounderDevices { - pub ad9959: ad9959::Ad9959< - QspiInterface, - DELAY, - hal::gpio::gpiog::PG7>, - >, +pub struct PounderDevices { + pub ad9959: ad9959::Ad9959, + pub io_update_trigger: HighResTimerE, mcp23017: mcp23017::MCP23017>, attenuator_spi: hal::spi::Spi, adc1: hal::adc::Adc, @@ -257,26 +297,21 @@ pub struct PounderDevices { adc2_in_p: hal::gpio::gpiof::PF14, } -impl PounderDevices -where - DELAY: embedded_hal::blocking::delay::DelayMs, -{ +impl PounderDevices { /// Construct and initialize pounder-specific hardware. /// /// Args: /// * `ad9959` - The DDS driver for the pounder hardware. /// * `attenuator_spi` - A SPI interface to control digital attenuators. + /// * `io_update_timer` - The HRTimer with the IO_update signal connected to the output. /// * `adc1` - The ADC1 peripheral for measuring power. /// * `adc2` - The ADC2 peripheral for measuring power. /// * `adc1_in_p` - The input channel for the RF power measurement on IN0. /// * `adc2_in_p` - The input channel for the RF power measurement on IN1. pub fn new( mcp23017: mcp23017::MCP23017>, - ad9959: ad9959::Ad9959< - QspiInterface, - DELAY, - hal::gpio::gpiog::PG7>, - >, + ad9959: ad9959::Ad9959, + io_update_trigger: HighResTimerE, attenuator_spi: hal::spi::Spi, adc1: hal::adc::Adc, adc2: hal::adc::Adc, @@ -285,6 +320,7 @@ where ) -> Result { let mut devices = Self { mcp23017, + io_update_trigger, ad9959, attenuator_spi, adc1, @@ -312,6 +348,9 @@ where // Select the on-board clock with a 4x prescaler (400MHz). devices.select_onboard_clock(4u8)?; + // Run the DDS in stream-only mode (no read support). + devices.ad9959.interface.start_stream(); + Ok(devices) } @@ -390,91 +429,6 @@ where }) } - /// Get the state of a Pounder input channel. - /// - /// Args: - /// * `channel` - The pounder channel to get the state of. Must be an input channel - /// - /// Returns: - /// The read-back channel input state. - pub fn get_input_channel_state( - &mut self, - channel: Channel, - ) -> Result { - match channel { - Channel::In0 | Channel::In1 => { - let channel_state = self.get_dds_channel_state(channel)?; - - let attenuation = self.get_attenuation(channel)?; - let power = self.measure_power(channel)?; - - Ok(InputChannelState { - attenuation, - power, - mixer: channel_state, - }) - } - _ => Err(Error::InvalidChannel), - } - } - - /// Get the state of a DDS channel. - /// - /// Args: - /// * `channel` - The pounder channel to get the state of. - /// - /// Returns: - /// The read-back channel state. - fn get_dds_channel_state( - &mut self, - channel: Channel, - ) -> Result { - let frequency = self - .ad9959 - .get_frequency(channel.into()) - .map_err(|_| Error::Dds)?; - let phase_offset = self - .ad9959 - .get_phase(channel.into()) - .map_err(|_| Error::Dds)?; - let amplitude = self - .ad9959 - .get_amplitude(channel.into()) - .map_err(|_| Error::Dds)?; - - Ok(DdsChannelState { - phase_offset, - frequency, - amplitude, - enabled: true, - }) - } - - /// Get the state of a DDS output channel. - /// - /// Args: - /// * `channel` - The pounder channel to get the output state of. Must be an output channel. - /// - /// Returns: - /// The read-back output channel state. - pub fn get_output_channel_state( - &mut self, - channel: Channel, - ) -> Result { - match channel { - Channel::Out0 | Channel::Out1 => { - let channel_state = self.get_dds_channel_state(channel)?; - let attenuation = self.get_attenuation(channel)?; - - Ok(OutputChannelState { - attenuation, - channel: channel_state, - }) - } - _ => Err(Error::InvalidChannel), - } - } - /// Configure a DDS channel. /// /// Args: @@ -485,16 +439,25 @@ where channel: Channel, state: ChannelState, ) -> Result<(), Error> { - self.ad9959.write_profile(channel.into(), state.parameters.frequency, - state.parameters.phase_offset, state.parameters.amplitude).map_err(|_| Error::Dds)?; + let profile = self + .ad9959 + .serialize_profile( + channel.into(), + state.parameters.frequency, + state.parameters.phase_offset, + state.parameters.amplitude, + ) + .map_err(|_| Error::Dds)?; + self.ad9959.interface.write_profile(profile).unwrap(); + self.io_update_trigger.trigger(); - //self.set_attenuation(channel, state.attenuation)?; + self.set_attenuation(channel, state.attenuation)?; Ok(()) } } -impl AttenuatorInterface for PounderDevices { +impl AttenuatorInterface for PounderDevices { /// Reset all of the attenuators to a power-on default state. fn reset_attenuators(&mut self) -> Result<(), Error> { self.mcp23017 @@ -566,7 +529,7 @@ impl AttenuatorInterface for PounderDevices { } } -impl PowerMeasurementInterface for PounderDevices { +impl PowerMeasurementInterface for PounderDevices { /// Sample an ADC channel. /// /// Args: