From 9d90d7b0d2d3f0b97453541c32b64226985733ba Mon Sep 17 00:00:00 2001
From: Ryan Summers <ryan.summers@vertigo-designs.com>
Date: Mon, 18 Jan 2021 17:20:33 +0100
Subject: [PATCH 01/71] Adding WIP apps

---
 Cargo.toml              |   8 ++
 src/{main.rs => iir.rs} |   0
 src/lockin.rs           | 280 ++++++++++++++++++++++++++++++++++++++++
 3 files changed, 288 insertions(+)
 rename src/{main.rs => iir.rs} (100%)
 create mode 100644 src/lockin.rs

diff --git a/Cargo.toml b/Cargo.toml
index f4f90fa..64979a8 100644
--- a/Cargo.toml
+++ b/Cargo.toml
@@ -75,3 +75,11 @@ opt-level = 3
 debug = true
 lto = true
 codegen-units = 1
+
+[[bin]]
+name = "iir"
+path = "src/iir.rs"
+
+[[bin]]
+name = "lockin"
+path = "src/lockin.rs"
diff --git a/src/main.rs b/src/iir.rs
similarity index 100%
rename from src/main.rs
rename to src/iir.rs
diff --git a/src/lockin.rs b/src/lockin.rs
new file mode 100644
index 0000000..29cdb1f
--- /dev/null
+++ b/src/lockin.rs
@@ -0,0 +1,280 @@
+#![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};
+
+// 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;
+
+// A constant sinusoid to send on the DAC output.
+const DAC_SEQUENCE: [8; i16] = [0, 0.707, 1, 0.707, 0, -0.707, -1, -0.707];
+
+#[macro_use]
+mod server;
+mod hardware;
+use hardware::{Adc0Input, Adc1Input, Dac0Output, Dac1Output, AFE0, AFE1};
+use dsp::iir;
+
+const SCALE: f32 = ((1 << 15) - 1) as f32;
+
+const TCP_RX_BUFFER_SIZE: usize = 8192;
+const TCP_TX_BUFFER_SIZE: usize = 8192;
+
+#[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][coeff]
+        #[init([[0.; 5]; 2])]
+        iir_state: [iir::IIRState; 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(),
+        ];
+
+        // DAC0 always generates a fixed sinusoidal output.
+        for value in DAC_SEQUENCE.iter() {
+            let y = value * i16::MAX;
+            // Note(unsafe): The DAC_SEQUENCE values are guaranteed to be normalized.
+            let y = unsafe { y.to_int_unchecked::<i16>() };
+
+            // Convert to DAC code
+            dac_samples[0][sample] = y as u16 ^ 0x8000;
+        }
+
+        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::<i16>() };
+                // 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::<smoltcp::socket::TcpSocket>(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);
+                        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],
+                                            y0: iir_state[0][2],
+                                            x1: iir_state[1][0],
+                                            y1: iir_state[1][2],
+                                    });
+
+                                    Ok::<server::Status, ()>(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] = req.iir;
+
+                                        Ok::<server::IirRequest, ()>(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] = req.iir;
+
+                                        Ok::<server::IirRequest, ()>(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();
+    }
+};

From ac06f811ab4bf88e791ec33cd10b06a2cd785835 Mon Sep 17 00:00:00 2001
From: Ryan Summers <ryan.summers@vertigo-designs.com>
Date: Mon, 18 Jan 2021 18:02:00 +0100
Subject: [PATCH 02/71] Adding framework for initial lockin demo

---
 src/hardware/adc.rs |  2 +
 src/lockin.rs       | 91 ++++++++++++++++++---------------------------
 2 files changed, 39 insertions(+), 54 deletions(-)

diff --git a/src/hardware/adc.rs b/src/hardware/adc.rs
index 188e436..d0d7d4f 100644
--- a/src/hardware/adc.rs
+++ b/src/hardware/adc.rs
@@ -328,6 +328,7 @@ macro_rules! adc_input {
                 }
 
                 /// Enable the ADC DMA transfer sequence.
+                #[allow(dead_code)]
                 pub fn start(&mut self) {
                     self.transfer.start(|spi| {
                         spi.enable_dma_rx();
@@ -345,6 +346,7 @@ macro_rules! adc_input {
                 ///
                 /// # Returns
                 /// A reference to the underlying buffer that has been filled with ADC samples.
+                #[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 no
diff --git a/src/lockin.rs b/src/lockin.rs
index 29cdb1f..9de067e 100644
--- a/src/lockin.rs
+++ b/src/lockin.rs
@@ -21,12 +21,12 @@ const ADC_SAMPLE_TICKS: u16 = 256;
 const SAMPLE_BUFFER_SIZE: usize = 8;
 
 // A constant sinusoid to send on the DAC output.
-const DAC_SEQUENCE: [8; i16] = [0, 0.707, 1, 0.707, 0, -0.707, -1, -0.707];
+const DAC_SEQUENCE: [f32; 8] = [0.0, 0.707, 1.0, 0.707, 0.0, -0.707, -1.0, -0.707];
 
 #[macro_use]
 mod server;
 mod hardware;
-use hardware::{Adc0Input, Adc1Input, Dac0Output, Dac1Output, AFE0, AFE1};
+use hardware::{Adc1Input, Dac0Output, Dac1Output, AFE0, AFE1};
 use dsp::iir;
 
 const SCALE: f32 = ((1 << 15) - 1) as f32;
@@ -38,15 +38,15 @@ const TCP_TX_BUFFER_SIZE: usize = 8192;
 const APP: () = {
     struct Resources {
         afes: (AFE0, AFE1),
-        adcs: (Adc0Input, Adc1Input),
+        adc1: Adc1Input,
         dacs: (Dac0Output, Dac1Output),
         net_interface: hardware::Ethernet,
 
-        // Format: iir_state[ch][coeff]
-        #[init([[0.; 5]; 2])]
-        iir_state: [iir::IIRState; 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([0.; 5])]
+        iir_state: iir::IIRState,
+
+        #[init(iir::IIR { ba: [1., 0., 0., 0., 0.], y_offset: 0., y_min: -SCALE - 1., y_max: SCALE })]
+        iir: iir::IIR,
     }
 
     #[init]
@@ -55,7 +55,6 @@ const APP: () = {
         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();
@@ -65,7 +64,7 @@ const APP: () = {
 
         init::LateResources {
             afes: stabilizer.afes,
-            adcs: stabilizer.adcs,
+            adc1: stabilizer.adcs.1,
             dacs: stabilizer.dacs,
             net_interface: stabilizer.net.interface,
         }
@@ -87,45 +86,41 @@ const APP: () = {
     ///
     /// 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)]
+    #[task(binds=DMA1_STR4, resources=[adc1, dacs, iir_state, iir], priority=2)]
     fn process(c: process::Context) {
-        let adc_samples = [
-            c.resources.adcs.0.acquire_buffer(),
-            c.resources.adcs.1.acquire_buffer(),
-        ];
+        let _adc_samples = c.resources.adc1.acquire_buffer();
         let dac_samples = [
             c.resources.dacs.0.acquire_buffer(),
             c.resources.dacs.1.acquire_buffer(),
         ];
 
         // DAC0 always generates a fixed sinusoidal output.
-        for value in DAC_SEQUENCE.iter() {
-            let y = value * i16::MAX;
+        for (i, value) in DAC_SEQUENCE.iter().enumerate() {
+            let y = value * i16::MAX as f32;
             // Note(unsafe): The DAC_SEQUENCE values are guaranteed to be normalized.
             let y = unsafe { y.to_int_unchecked::<i16>() };
 
             // Convert to DAC code
-            dac_samples[0][sample] = y as u16 ^ 0x8000;
+            dac_samples[0][i] = y as u16 ^ 0x8000;
         }
 
-        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::<i16>() };
-                // Convert to DAC code
-                dac_samples[channel][sample] = y as u16 ^ 0x8000;
-            }
+        // TODO: Introduce a "dummy" PLL here.
+
+        // TODO: Demodulate the ADC0 input samples with the dummy PLL.
+
+        // TODO: Filter the demodulated ADC values
+
+        // TODO: Compute phase of the last sample
+
+        // TODO: Place last sample phase into DAC1s output buffer.
+        let y = 0.0;
+
+        for value in dac_samples[1].iter_mut() {
+            *value = y as u16 ^ 0x8000
         }
     }
 
-    #[idle(resources=[net_interface, iir_state, iir_ch, afes])]
+    #[idle(resources=[net_interface, iir_state, iir, afes])]
     fn idle(mut c: idle::Context) -> ! {
         let mut socket_set_entries: [_; 8] = Default::default();
         let mut sockets =
@@ -177,10 +172,10 @@ const APP: () = {
                                     let state = c.resources.iir_state.lock(|iir_state|
                                         server::Status {
                                             t: time,
-                                            x0: iir_state[0][0],
-                                            y0: iir_state[0][2],
-                                            x1: iir_state[1][0],
-                                            y1: iir_state[1][2],
+                                            x0: iir_state[0],
+                                            y0: iir_state[2],
+                                            x1: iir_state[0],
+                                            y1: iir_state[2],
                                     });
 
                                     Ok::<server::Status, ()>(state)
@@ -190,29 +185,17 @@ const APP: () = {
                             ],
 
                             modifiable_attributes: [
-                                "stabilizer/iir0/state": server::IirRequest, (|req: server::IirRequest| {
-                                    c.resources.iir_ch.lock(|iir_ch| {
-                                        if req.channel > 1 {
+                                "stabilizer/iir/state": server::IirRequest, (|req: server::IirRequest| {
+                                    c.resources.iir.lock(|iir| {
+                                        if req.channel >= 1 {
                                             return Err(());
                                         }
 
-                                        iir_ch[req.channel as usize] = req.iir;
+                                        *iir = req.iir;
 
                                         Ok::<server::IirRequest, ()>(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] = req.iir;
-
-                                        Ok::<server::IirRequest, ()>(req)
-                                    })
-                                }),
-                                }),
                                 "stabilizer/afe0/gain": hardware::AfeGain, (|gain| {
                                     c.resources.afes.0.set_gain(gain);
                                     Ok::<(), ()>(())
@@ -257,7 +240,7 @@ const APP: () = {
 
     #[task(binds = SPI3, priority = 3)]
     fn spi3(_: spi3::Context) {
-        panic!("ADC0 input overrun");
+        panic!("ADC1 input overrun");
     }
 
     #[task(binds = SPI4, priority = 3)]

From 7134df974944c9b05188d853290a907860abb757 Mon Sep 17 00:00:00 2001
From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com>
Date: Wed, 20 Jan 2021 04:01:23 +0000
Subject: [PATCH 03/71] build(deps): bump smoltcp from 0.6.0 to 0.7.0

Bumps [smoltcp](https://github.com/smoltcp-rs/smoltcp) from 0.6.0 to 0.7.0.
- [Release notes](https://github.com/smoltcp-rs/smoltcp/releases)
- [Changelog](https://github.com/smoltcp-rs/smoltcp/blob/master/CHANGELOG.md)
- [Commits](https://github.com/smoltcp-rs/smoltcp/compare/v0.6.0...v0.7.0)

Signed-off-by: dependabot[bot] <support@github.com>
---
 Cargo.lock | 15 +++++++++++++--
 Cargo.toml |  2 +-
 2 files changed, 14 insertions(+), 3 deletions(-)

diff --git a/Cargo.lock b/Cargo.lock
index 76fb832..748e98d 100644
--- a/Cargo.lock
+++ b/Cargo.lock
@@ -827,6 +827,17 @@ dependencies = [
  "managed",
 ]
 
+[[package]]
+name = "smoltcp"
+version = "0.7.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "ab527c390c7e107f687bd92a886a083fde61b8cdc700b37f3d7e4346ffd8fae1"
+dependencies = [
+ "bitflags",
+ "byteorder",
+ "managed",
+]
+
 [[package]]
 name = "stabilizer"
 version = "0.4.1"
@@ -849,7 +860,7 @@ dependencies = [
  "paste",
  "serde",
  "serde-json-core",
- "smoltcp",
+ "smoltcp 0.7.0",
  "stm32h7xx-hal",
 ]
 
@@ -884,7 +895,7 @@ dependencies = [
  "embedded-hal",
  "nb 1.0.0",
  "paste",
- "smoltcp",
+ "smoltcp 0.6.0",
  "stm32h7",
  "void",
 ]
diff --git a/Cargo.toml b/Cargo.toml
index f4f90fa..8e20684 100644
--- a/Cargo.toml
+++ b/Cargo.toml
@@ -50,7 +50,7 @@ ad9959 = { path = "ad9959" }
 git = "https://github.com/mrd0ll4r/mcp23017.git"
 
 [dependencies.smoltcp]
-version = "0.6"
+version = "0.7"
 features = ["ethernet", "proto-ipv4", "socket-tcp", "proto-ipv6"]
 default-features = false
 

From f1f15aca65f8fe4677205adff8ef433aa6ba0c1c Mon Sep 17 00:00:00 2001
From: Ryan Summers <ryan.summers@vertigo-designs.com>
Date: Wed, 20 Jan 2021 12:49:07 +0100
Subject: [PATCH 04/71] Refactoring app structure

---
 src/{iir.rs => bin/dual-iir.rs} | 19 +++++--------------
 src/{ => bin}/lockin.rs         |  0
 src/lib.rs                      | 16 ++++++++++++++++
 src/server.rs                   | 10 ++++------
 4 files changed, 25 insertions(+), 20 deletions(-)
 rename src/{iir.rs => bin/dual-iir.rs} (96%)
 rename src/{ => bin}/lockin.rs (100%)
 create mode 100644 src/lib.rs

diff --git a/src/iir.rs b/src/bin/dual-iir.rs
similarity index 96%
rename from src/iir.rs
rename to src/bin/dual-iir.rs
index 3b86c37..fd8f5e4 100644
--- a/src/iir.rs
+++ b/src/bin/dual-iir.rs
@@ -12,20 +12,8 @@ use rtic::cyccnt::{Instant, U32Ext};
 
 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: u16 = 256;
+use stabilizer::{hardware, server};
 
-// The desired ADC sample processing buffer size.
-const SAMPLE_BUFFER_SIZE: usize = 8;
-
-// The number of cascaded IIR biquads per channel. Select 1 or 2!
-const IIR_CASCADE_LENGTH: usize = 1;
-
-#[macro_use]
-mod server;
-mod hardware;
 use dsp::iir;
 use hardware::{Adc0Input, Adc1Input, Dac0Output, Dac1Output, AFE0, AFE1};
 
@@ -34,6 +22,9 @@ 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 {
@@ -162,7 +153,7 @@ const APP: () = {
                 } else {
                     server.poll(socket, |req| {
                         info!("Got request: {:?}", req);
-                        route_request!(req,
+                        stabilizer::route_request!(req,
                             readable_attributes: [
                                 "stabilizer/iir/state": (|| {
                                     let state = c.resources.iir_state.lock(|iir_state|
diff --git a/src/lockin.rs b/src/bin/lockin.rs
similarity index 100%
rename from src/lockin.rs
rename to src/bin/lockin.rs
diff --git a/src/lib.rs b/src/lib.rs
new file mode 100644
index 0000000..a51024a
--- /dev/null
+++ b/src/lib.rs
@@ -0,0 +1,16 @@
+#![no_std]
+
+pub mod server;
+
+#[macro_use]
+extern crate log;
+
+pub mod hardware;
+
+// 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/server.rs b/src/server.rs
index f434060..5e6950d 100644
--- a/src/server.rs
+++ b/src/server.rs
@@ -1,14 +1,12 @@
-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 super::iir;
 use smoltcp as net;
 
+use dsp::iir;
+
+#[macro_export]
 macro_rules! route_request {
     ($request:ident,
             readable_attributes: [$($read_attribute:tt: $getter:tt),*],

From b2cbc6791df6c9fcef2fbafd12f25b3cc59ed8f9 Mon Sep 17 00:00:00 2001
From: Ryan Summers <ryan.summers@vertigo-designs.com>
Date: Wed, 20 Jan 2021 12:55:45 +0100
Subject: [PATCH 05/71] Restructuring

---
 Cargo.toml        |  8 --------
 src/bin/lockin.rs | 18 +++++-------------
 2 files changed, 5 insertions(+), 21 deletions(-)

diff --git a/Cargo.toml b/Cargo.toml
index 64979a8..f4f90fa 100644
--- a/Cargo.toml
+++ b/Cargo.toml
@@ -75,11 +75,3 @@ opt-level = 3
 debug = true
 lto = true
 codegen-units = 1
-
-[[bin]]
-name = "iir"
-path = "src/iir.rs"
-
-[[bin]]
-name = "lockin"
-path = "src/lockin.rs"
diff --git a/src/bin/lockin.rs b/src/bin/lockin.rs
index 9de067e..6b07901 100644
--- a/src/bin/lockin.rs
+++ b/src/bin/lockin.rs
@@ -12,21 +12,13 @@ use rtic::cyccnt::{Instant, U32Ext};
 
 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: u16 = 256;
-
-// The desired ADC sample processing buffer size.
-const SAMPLE_BUFFER_SIZE: usize = 8;
-
 // A constant sinusoid to send on the DAC output.
 const DAC_SEQUENCE: [f32; 8] = [0.0, 0.707, 1.0, 0.707, 0.0, -0.707, -1.0, -0.707];
 
-#[macro_use]
-mod server;
-mod hardware;
-use hardware::{Adc1Input, Dac0Output, Dac1Output, AFE0, AFE1};
+use stabilizer::{
+    hardware::{self, Adc1Input, Dac0Output, Dac1Output, AFE0, AFE1},
+    server
+};
 use dsp::iir;
 
 const SCALE: f32 = ((1 << 15) - 1) as f32;
@@ -166,7 +158,7 @@ const APP: () = {
                 } else {
                     server.poll(socket, |req| {
                         info!("Got request: {:?}", req);
-                        route_request!(req,
+                        stabilizer::route_request!(req,
                             readable_attributes: [
                                 "stabilizer/iir/state": (|| {
                                     let state = c.resources.iir_state.lock(|iir_state|

From 2ef27b8187c3e09f03dc2945e34c98749914bc20 Mon Sep 17 00:00:00 2001
From: Ryan Summers <ryan.summers@vertigo-designs.com>
Date: Wed, 20 Jan 2021 12:55:55 +0100
Subject: [PATCH 06/71] Formatting

---
 src/bin/lockin.rs | 7 ++++---
 1 file changed, 4 insertions(+), 3 deletions(-)

diff --git a/src/bin/lockin.rs b/src/bin/lockin.rs
index 6b07901..3bd70fd 100644
--- a/src/bin/lockin.rs
+++ b/src/bin/lockin.rs
@@ -13,13 +13,14 @@ use rtic::cyccnt::{Instant, U32Ext};
 use heapless::{consts::*, String};
 
 // A constant sinusoid to send on the DAC output.
-const DAC_SEQUENCE: [f32; 8] = [0.0, 0.707, 1.0, 0.707, 0.0, -0.707, -1.0, -0.707];
+const DAC_SEQUENCE: [f32; 8] =
+    [0.0, 0.707, 1.0, 0.707, 0.0, -0.707, -1.0, -0.707];
 
+use dsp::iir;
 use stabilizer::{
     hardware::{self, Adc1Input, Dac0Output, Dac1Output, AFE0, AFE1},
-    server
+    server,
 };
-use dsp::iir;
 
 const SCALE: f32 = ((1 << 15) - 1) as f32;
 

From 7c5a74c35ef95f70b91a5536d152cd495cedff0b Mon Sep 17 00:00:00 2001
From: Ryan Summers <ryan.summers@vertigo-designs.com>
Date: Tue, 26 Jan 2021 10:52:35 +0100
Subject: [PATCH 07/71] Renaming internal lockin

---
 src/bin/{lockin.rs => lockin-internal-demo.rs} | 0
 1 file changed, 0 insertions(+), 0 deletions(-)
 rename src/bin/{lockin.rs => lockin-internal-demo.rs} (100%)

diff --git a/src/bin/lockin.rs b/src/bin/lockin-internal-demo.rs
similarity index 100%
rename from src/bin/lockin.rs
rename to src/bin/lockin-internal-demo.rs

From e161f498228b24379fb6a67c1a2d372edad39dba Mon Sep 17 00:00:00 2001
From: Ryan Summers <ryan.summers@vertigo-designs.com>
Date: Tue, 26 Jan 2021 12:21:44 +0100
Subject: [PATCH 08/71] Adding WIP lockin demo

---
 src/bin/lockin-internal-demo.rs | 64 ++++++++++++++++++++++++++-------
 src/lib.rs                      |  1 -
 2 files changed, 51 insertions(+), 14 deletions(-)

diff --git a/src/bin/lockin-internal-demo.rs b/src/bin/lockin-internal-demo.rs
index 3bd70fd..113fafd 100644
--- a/src/bin/lockin-internal-demo.rs
+++ b/src/bin/lockin-internal-demo.rs
@@ -16,10 +16,10 @@ use heapless::{consts::*, String};
 const DAC_SEQUENCE: [f32; 8] =
     [0.0, 0.707, 1.0, 0.707, 0.0, -0.707, -1.0, -0.707];
 
-use dsp::iir;
+use dsp::{iir, iir_int, lockin::Lockin, reciprocal_pll::TimestampHandler};
+use hardware::{Adc1Input, Dac0Output, Dac1Output, AFE0, AFE1};
 use stabilizer::{
-    hardware::{self, Adc1Input, Dac0Output, Dac1Output, AFE0, AFE1},
-    server,
+    hardware, server, ADC_SAMPLE_TICKS_LOG2, SAMPLE_BUFFER_SIZE_LOG2,
 };
 
 const SCALE: f32 = ((1 << 15) - 1) as f32;
@@ -40,6 +40,9 @@ const APP: () = {
 
         #[init(iir::IIR { ba: [1., 0., 0., 0., 0.], y_offset: 0., y_min: -SCALE - 1., y_max: SCALE })]
         iir: iir::IIR,
+
+        pll: TimestampHandler,
+        lockin: Lockin,
     }
 
     #[init]
@@ -47,6 +50,17 @@ const APP: () = {
         // Configure the microcontroller
         let (mut stabilizer, _pounder) = hardware::setup(c.core, c.device);
 
+        let pll = TimestampHandler::new(
+            4, // relative PLL frequency bandwidth: 2**-4, TODO: expose
+            3, // relative PLL phase bandwidth: 2**-3, TODO: expose
+            ADC_SAMPLE_TICKS_LOG2 as usize,
+            SAMPLE_BUFFER_SIZE_LOG2,
+        );
+
+        let lockin = Lockin::new(
+            &iir_int::IIRState::default(), // TODO: lowpass, expose
+        );
+
         // Enable ADC/DAC events
         stabilizer.adcs.1.start();
         stabilizer.dacs.0.start();
@@ -56,6 +70,8 @@ const APP: () = {
         stabilizer.adc_dac_timer.start();
 
         init::LateResources {
+            lockin,
+            pll,
             afes: stabilizer.afes,
             adc1: stabilizer.adcs.1,
             dacs: stabilizer.dacs,
@@ -79,9 +95,11 @@ const APP: () = {
     ///
     /// 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=[adc1, dacs, iir_state, iir], priority=2)]
-    fn process(c: process::Context) {
-        let _adc_samples = c.resources.adc1.acquire_buffer();
+    ///
+    /// TODO: Document
+    #[task(binds=DMA1_STR4, resources=[adc1, dacs, iir_state, iir, lockin, pll], priority=2)]
+    fn process(mut c: process::Context) {
+        let adc_samples = c.resources.adc1.acquire_buffer();
         let dac_samples = [
             c.resources.dacs.0.acquire_buffer(),
             c.resources.dacs.1.acquire_buffer(),
@@ -97,19 +115,39 @@ const APP: () = {
             dac_samples[0][i] = y as u16 ^ 0x8000;
         }
 
-        // TODO: Introduce a "dummy" PLL here.
+        // TODO: Verify that the DAC code is always generated at T=0
+        let (pll_phase, pll_frequency) = c.resources.pll.update(Some(0));
 
-        // TODO: Demodulate the ADC0 input samples with the dummy PLL.
+        // Harmonic index of the LO: -1 to _de_modulate the fundamental
+        let harmonic: i32 = -1;
 
-        // TODO: Filter the demodulated ADC values
+        // Demodulation LO phase offset
+        let phase_offset: i32 = 0;
+        let sample_frequency = (pll_frequency as i32).wrapping_mul(harmonic);
+        let mut sample_phase = phase_offset
+            .wrapping_add((pll_phase as i32).wrapping_mul(harmonic));
 
-        // TODO: Compute phase of the last sample
+        let mut phase = 0f32;
 
-        // TODO: Place last sample phase into DAC1s output buffer.
-        let y = 0.0;
+        for sample in adc_samples.iter() {
+            // Convert to signed, MSB align the ADC sample.
+            let input = (*sample as i16 as i32) << 16;
+
+            // Obtain demodulated, filtered IQ sample.
+            let output = c.resources.lockin.update(input, sample_phase);
+
+            // Advance the sample phase.
+            sample_phase = sample_phase.wrapping_add(sample_frequency);
+
+            // Convert from IQ to phase.
+            phase = output.phase() as _;
+        }
+
+        // Filter phase through an IIR.
+        phase = c.resources.iir.update(&mut c.resources.iir_state, phase);
 
         for value in dac_samples[1].iter_mut() {
-            *value = y as u16 ^ 0x8000
+            *value = phase as u16 ^ 0x8000
         }
     }
 
diff --git a/src/lib.rs b/src/lib.rs
index 9a73582..254e626 100644
--- a/src/lib.rs
+++ b/src/lib.rs
@@ -1,6 +1,5 @@
 #![no_std]
 
-
 #[macro_use]
 extern crate log;
 

From 51085d175e07f8e55b8a0310076d5c17d7eb4b18 Mon Sep 17 00:00:00 2001
From: Ryan Summers <ryan.summers@vertigo-designs.com>
Date: Tue, 26 Jan 2021 12:23:17 +0100
Subject: [PATCH 09/71] Removing dead-code allowance

---
 src/hardware/adc.rs | 2 --
 1 file changed, 2 deletions(-)

diff --git a/src/hardware/adc.rs b/src/hardware/adc.rs
index d0d7d4f..188e436 100644
--- a/src/hardware/adc.rs
+++ b/src/hardware/adc.rs
@@ -328,7 +328,6 @@ macro_rules! adc_input {
                 }
 
                 /// Enable the ADC DMA transfer sequence.
-                #[allow(dead_code)]
                 pub fn start(&mut self) {
                     self.transfer.start(|spi| {
                         spi.enable_dma_rx();
@@ -346,7 +345,6 @@ macro_rules! adc_input {
                 ///
                 /// # Returns
                 /// A reference to the underlying buffer that has been filled with ADC samples.
-                #[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 no

From c030b97714b3027d0e035dc9463b3ca2ba1361a3 Mon Sep 17 00:00:00 2001
From: Ryan Summers <summers.ryan.m@gmail.com>
Date: Tue, 26 Jan 2021 12:49:45 +0100
Subject: [PATCH 10/71] Apply suggestions from code review
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

Co-authored-by: Robert Jördens <rj@quartiq.de>
---
 src/bin/lockin-internal-demo.rs | 11 ++---------
 1 file changed, 2 insertions(+), 9 deletions(-)

diff --git a/src/bin/lockin-internal-demo.rs b/src/bin/lockin-internal-demo.rs
index 113fafd..6acfc36 100644
--- a/src/bin/lockin-internal-demo.rs
+++ b/src/bin/lockin-internal-demo.rs
@@ -41,7 +41,6 @@ const APP: () = {
         #[init(iir::IIR { ba: [1., 0., 0., 0., 0.], y_offset: 0., y_min: -SCALE - 1., y_max: SCALE })]
         iir: iir::IIR,
 
-        pll: TimestampHandler,
         lockin: Lockin,
     }
 
@@ -50,13 +49,6 @@ const APP: () = {
         // Configure the microcontroller
         let (mut stabilizer, _pounder) = hardware::setup(c.core, c.device);
 
-        let pll = TimestampHandler::new(
-            4, // relative PLL frequency bandwidth: 2**-4, TODO: expose
-            3, // relative PLL phase bandwidth: 2**-3, TODO: expose
-            ADC_SAMPLE_TICKS_LOG2 as usize,
-            SAMPLE_BUFFER_SIZE_LOG2,
-        );
-
         let lockin = Lockin::new(
             &iir_int::IIRState::default(), // TODO: lowpass, expose
         );
@@ -116,7 +108,8 @@ const APP: () = {
         }
 
         // TODO: Verify that the DAC code is always generated at T=0
-        let (pll_phase, pll_frequency) = c.resources.pll.update(Some(0));
+        let pll_phase = 0i32;
+        let pll_frequency = 1i32 << (32 - 3); // 1/8 of the sample rate
 
         // Harmonic index of the LO: -1 to _de_modulate the fundamental
         let harmonic: i32 = -1;

From 45e7d6de3cb09d7b722bfcbc5b47d2cd42a3ca51 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Robert=20J=C3=B6rdens?= <rj@quartiq.de>
Date: Tue, 26 Jan 2021 22:30:46 +0100
Subject: [PATCH 11/71] rpll: auto-align counter

---
 dsp/src/rpll.rs   | 18 ++++++++++++++----
 src/bin/lockin.rs |  2 +-
 2 files changed, 15 insertions(+), 5 deletions(-)

diff --git a/dsp/src/rpll.rs b/dsp/src/rpll.rs
index 2bfd29c..292c523 100644
--- a/dsp/src/rpll.rs
+++ b/dsp/src/rpll.rs
@@ -18,14 +18,12 @@ impl RPLL {
     ///
     /// Args:
     /// * dt2: inverse update() rate. 1 << dt2 is the counter rate to update() rate ratio.
-    /// * t: Counter time. Counter value at the first update() call. Typically 0.
     ///
     /// Returns:
     /// Initialized RPLL instance.
-    pub fn new(dt2: u8, t: i32) -> RPLL {
+    pub fn new(dt2: u8) -> RPLL {
         RPLL {
             dt2,
-            t,
             ..Default::default()
         }
     }
@@ -69,7 +67,19 @@ impl RPLL {
             // Update frequency lock
             self.ff = self.ff.wrapping_add(p_ref.wrapping_sub(p_sig));
             // Time in counter cycles between timestamp and "now"
-            let dt = self.t.wrapping_sub(x);
+            let mut dt = self.t.wrapping_sub(x);
+
+            if dt < 0 {
+                // Timestamp is in the future
+                // Advance phase and time until we are just past the most recent
+                // timestamp.
+                let mut dt_ceil = dt >> self.dt2;
+                self.y = self.y.wrapping_sub(self.f.wrapping_mul(dt_ceil));
+                dt_ceil <<= self.dt2;
+                self.t = self.t.wrapping_sub(dt_ceil);
+                dt = dt.wrapping_sub(dt_ceil);
+            }
+
             // Reference phase estimate "now"
             let y_ref = (self.f >> self.dt2).wrapping_mul(dt);
             // Phase error
diff --git a/src/bin/lockin.rs b/src/bin/lockin.rs
index 7066ad8..1c2aa1f 100644
--- a/src/bin/lockin.rs
+++ b/src/bin/lockin.rs
@@ -53,7 +53,7 @@ const APP: () = {
         // Configure the microcontroller
         let (mut stabilizer, _pounder) = hardware::setup(c.core, c.device);
 
-        let pll = RPLL::new(ADC_SAMPLE_TICKS_LOG2 + SAMPLE_BUFFER_SIZE_LOG2, 0);
+        let pll = RPLL::new(ADC_SAMPLE_TICKS_LOG2 + SAMPLE_BUFFER_SIZE_LOG2);
 
         let lockin = Lockin::new(
             &iir_int::IIRState::lowpass(1e-3, 0.707, 2.), // TODO: expose

From 1749d48ca390a4df27dd770a998914fd5fd5fded Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Robert=20J=C3=B6rdens?= <rj@quartiq.de>
Date: Wed, 27 Jan 2021 09:00:48 +0100
Subject: [PATCH 12/71] Revert "rpll: auto-align counter"

This reverts commit dbacc5293e12f712fef7bd85848e1b0bd8fde823.
---
 dsp/src/rpll.rs   | 18 ++++--------------
 src/bin/lockin.rs |  2 +-
 2 files changed, 5 insertions(+), 15 deletions(-)

diff --git a/dsp/src/rpll.rs b/dsp/src/rpll.rs
index 292c523..2bfd29c 100644
--- a/dsp/src/rpll.rs
+++ b/dsp/src/rpll.rs
@@ -18,12 +18,14 @@ impl RPLL {
     ///
     /// Args:
     /// * dt2: inverse update() rate. 1 << dt2 is the counter rate to update() rate ratio.
+    /// * t: Counter time. Counter value at the first update() call. Typically 0.
     ///
     /// Returns:
     /// Initialized RPLL instance.
-    pub fn new(dt2: u8) -> RPLL {
+    pub fn new(dt2: u8, t: i32) -> RPLL {
         RPLL {
             dt2,
+            t,
             ..Default::default()
         }
     }
@@ -67,19 +69,7 @@ impl RPLL {
             // Update frequency lock
             self.ff = self.ff.wrapping_add(p_ref.wrapping_sub(p_sig));
             // Time in counter cycles between timestamp and "now"
-            let mut dt = self.t.wrapping_sub(x);
-
-            if dt < 0 {
-                // Timestamp is in the future
-                // Advance phase and time until we are just past the most recent
-                // timestamp.
-                let mut dt_ceil = dt >> self.dt2;
-                self.y = self.y.wrapping_sub(self.f.wrapping_mul(dt_ceil));
-                dt_ceil <<= self.dt2;
-                self.t = self.t.wrapping_sub(dt_ceil);
-                dt = dt.wrapping_sub(dt_ceil);
-            }
-
+            let dt = self.t.wrapping_sub(x);
             // Reference phase estimate "now"
             let y_ref = (self.f >> self.dt2).wrapping_mul(dt);
             // Phase error
diff --git a/src/bin/lockin.rs b/src/bin/lockin.rs
index 1c2aa1f..7066ad8 100644
--- a/src/bin/lockin.rs
+++ b/src/bin/lockin.rs
@@ -53,7 +53,7 @@ const APP: () = {
         // Configure the microcontroller
         let (mut stabilizer, _pounder) = hardware::setup(c.core, c.device);
 
-        let pll = RPLL::new(ADC_SAMPLE_TICKS_LOG2 + SAMPLE_BUFFER_SIZE_LOG2);
+        let pll = RPLL::new(ADC_SAMPLE_TICKS_LOG2 + SAMPLE_BUFFER_SIZE_LOG2, 0);
 
         let lockin = Lockin::new(
             &iir_int::IIRState::lowpass(1e-3, 0.707, 2.), // TODO: expose

From 36288225b3898ff0ab132077314a02f82fe3899a Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Robert=20J=C3=B6rdens?= <rj@quartiq.de>
Date: Thu, 28 Jan 2021 22:21:42 +0100
Subject: [PATCH 13/71] rpll: extend to above-nyquist frequencies

---
 dsp/src/rpll.rs   | 20 +++++++++++---------
 src/bin/lockin.rs |  5 +++--
 2 files changed, 14 insertions(+), 11 deletions(-)

diff --git a/dsp/src/rpll.rs b/dsp/src/rpll.rs
index 2bfd29c..e3b3894 100644
--- a/dsp/src/rpll.rs
+++ b/dsp/src/rpll.rs
@@ -8,8 +8,8 @@ pub struct RPLL {
     dt2: u8, // 1 << dt2 is the counter rate to update() rate ratio
     t: i32,  // current counter time
     x: i32,  // previous timestamp
-    ff: i32, // current frequency estimate from frequency loop
-    f: i32,  // current frequency estimate from both frequency and phase loop
+    ff: u32, // current frequency estimate from frequency loop
+    f: u32,  // current frequency estimate from both frequency and phase loop
     y: i32,  // current phase estimate
 }
 
@@ -49,33 +49,35 @@ impl RPLL {
         input: Option<i32>,
         shift_frequency: u8,
         shift_phase: u8,
-    ) -> (i32, i32) {
+    ) -> (i32, u32) {
         debug_assert!(shift_frequency > self.dt2);
         debug_assert!(shift_phase > self.dt2);
         // Advance phase
-        self.y = self.y.wrapping_add(self.f);
+        self.y = self.y.wrapping_add(self.f as i32);
         if let Some(x) = input {
             // Reference period in counter cycles
             let dx = x.wrapping_sub(self.x);
             // Store timestamp for next time.
             self.x = x;
             // Phase using the current frequency estimate
-            let p_sig_long = (self.ff as i64).wrapping_mul(dx as i64);
+            let p_sig_64 = (self.ff as u64).wrapping_mul(dx as u64);
             // Add half-up rounding bias and apply gain/attenuation
-            let p_sig = (p_sig_long.wrapping_add(1i64 << (shift_frequency - 1))
+            let p_sig = (p_sig_64.wrapping_add(1u64 << (shift_frequency - 1))
                 >> shift_frequency) as i32;
             // Reference phase (1 << dt2 full turns) with gain/attenuation applied
             let p_ref = 1i32 << (32 + self.dt2 - shift_frequency);
             // Update frequency lock
-            self.ff = self.ff.wrapping_add(p_ref.wrapping_sub(p_sig));
+            self.ff = self.ff.wrapping_add(p_ref.wrapping_sub(p_sig) as u32);
             // Time in counter cycles between timestamp and "now"
             let dt = self.t.wrapping_sub(x);
             // Reference phase estimate "now"
-            let y_ref = (self.f >> self.dt2).wrapping_mul(dt);
+            let y_ref = ((self.f >> self.dt2) as i32).wrapping_mul(dt);
             // Phase error
             let dy = y_ref.wrapping_sub(self.y);
             // Current frequency estimate from frequency lock and phase error
-            self.f = self.ff.wrapping_add(dy >> (shift_phase - self.dt2));
+            self.f = self
+                .ff
+                .wrapping_add((dy >> (shift_phase - self.dt2)) as u32);
         }
         // Advance time
         self.t = self.t.wrapping_add(1 << self.dt2);
diff --git a/src/bin/lockin.rs b/src/bin/lockin.rs
index 7066ad8..cbdda51 100644
--- a/src/bin/lockin.rs
+++ b/src/bin/lockin.rs
@@ -127,8 +127,9 @@ const APP: () = {
         let harmonic: i32 = -1;
         // Demodulation LO phase offset
         let phase_offset: i32 = 0;
-        let sample_frequency =
-            (pll_frequency >> SAMPLE_BUFFER_SIZE_LOG2).wrapping_mul(harmonic);
+        let sample_frequency = ((pll_frequency >> SAMPLE_BUFFER_SIZE_LOG2)
+            as i32)
+            .wrapping_mul(harmonic);  // TODO: maybe rounding bias
         let mut sample_phase =
             phase_offset.wrapping_add(pll_phase.wrapping_mul(harmonic));
 

From c34e33066376a9904130dcebfb3748d3bffcb82b Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Robert=20J=C3=B6rdens?= <rj@quartiq.de>
Date: Thu, 28 Jan 2021 23:00:55 +0100
Subject: [PATCH 14/71] lockin: fmt

---
 src/bin/lockin.rs | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/bin/lockin.rs b/src/bin/lockin.rs
index cbdda51..1519853 100644
--- a/src/bin/lockin.rs
+++ b/src/bin/lockin.rs
@@ -129,7 +129,7 @@ const APP: () = {
         let phase_offset: i32 = 0;
         let sample_frequency = ((pll_frequency >> SAMPLE_BUFFER_SIZE_LOG2)
             as i32)
-            .wrapping_mul(harmonic);  // TODO: maybe rounding bias
+            .wrapping_mul(harmonic); // TODO: maybe rounding bias
         let mut sample_phase =
             phase_offset.wrapping_add(pll_phase.wrapping_mul(harmonic));
 

From c628b8d57aa2bbcd383e1e14769e43a1438722a8 Mon Sep 17 00:00:00 2001
From: Ryan Summers <summers.ryan.m@gmail.com>
Date: Fri, 29 Jan 2021 09:55:23 +0100
Subject: [PATCH 15/71] Update src/bin/lockin-internal-demo.rs
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

Co-authored-by: Robert Jördens <rj@quartiq.de>
---
 src/bin/lockin-internal-demo.rs | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/bin/lockin-internal-demo.rs b/src/bin/lockin-internal-demo.rs
index 6acfc36..e36c676 100644
--- a/src/bin/lockin-internal-demo.rs
+++ b/src/bin/lockin-internal-demo.rs
@@ -50,7 +50,7 @@ const APP: () = {
         let (mut stabilizer, _pounder) = hardware::setup(c.core, c.device);
 
         let lockin = Lockin::new(
-            &iir_int::IIRState::default(), // TODO: lowpass, expose
+            &iir_int::IIRState::lowpass(1e-3, 0.707, 2.), // TODO: expose
         );
 
         // Enable ADC/DAC events

From 1ebbe0f6d77a01cd3aba478cc78a5248601bc27e Mon Sep 17 00:00:00 2001
From: Ryan Summers <ryan.summers@vertigo-designs.com>
Date: Fri, 29 Jan 2021 10:11:56 +0100
Subject: [PATCH 16/71] Cleaning up demo

---
 src/bin/lockin-internal-demo.rs | 133 ++------------------------------
 1 file changed, 8 insertions(+), 125 deletions(-)

diff --git a/src/bin/lockin-internal-demo.rs b/src/bin/lockin-internal-demo.rs
index e36c676..969f332 100644
--- a/src/bin/lockin-internal-demo.rs
+++ b/src/bin/lockin-internal-demo.rs
@@ -3,37 +3,22 @@
 #![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};
-
 // A constant sinusoid to send on the DAC output.
 const DAC_SEQUENCE: [f32; 8] =
     [0.0, 0.707, 1.0, 0.707, 0.0, -0.707, -1.0, -0.707];
 
-use dsp::{iir, iir_int, lockin::Lockin, reciprocal_pll::TimestampHandler};
+use dsp::{iir, iir_int, lockin::Lockin};
 use hardware::{Adc1Input, Dac0Output, Dac1Output, AFE0, AFE1};
-use stabilizer::{
-    hardware, server, ADC_SAMPLE_TICKS_LOG2, SAMPLE_BUFFER_SIZE_LOG2,
-};
+use stabilizer::hardware;
 
 const SCALE: f32 = ((1 << 15) - 1) as f32;
 
-const TCP_RX_BUFFER_SIZE: usize = 8192;
-const TCP_TX_BUFFER_SIZE: usize = 8192;
-
 #[rtic::app(device = stm32h7xx_hal::stm32, peripherals = true, monotonic = rtic::cyccnt::CYCCNT)]
 const APP: () = {
     struct Resources {
         afes: (AFE0, AFE1),
         adc1: Adc1Input,
         dacs: (Dac0Output, Dac1Output),
-        net_interface: hardware::Ethernet,
 
         #[init([0.; 5])]
         iir_state: iir::IIRState,
@@ -63,11 +48,9 @@ const APP: () = {
 
         init::LateResources {
             lockin,
-            pll,
             afes: stabilizer.afes,
             adc1: stabilizer.adcs.1,
             dacs: stabilizer.dacs,
-            net_interface: stabilizer.net.interface,
         }
     }
 
@@ -89,7 +72,7 @@ const APP: () = {
     /// the same time bounds, meeting one also means the other is also met.
     ///
     /// TODO: Document
-    #[task(binds=DMA1_STR4, resources=[adc1, dacs, iir_state, iir, lockin, pll], priority=2)]
+    #[task(binds=DMA1_STR4, resources=[adc1, dacs, iir_state, iir, lockin], priority=2)]
     fn process(mut c: process::Context) {
         let adc_samples = c.resources.adc1.acquire_buffer();
         let dac_samples = [
@@ -144,117 +127,17 @@ const APP: () = {
         }
     }
 
-    #[idle(resources=[net_interface, iir_state, iir, 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();
-
+    #[idle(resources=[iir_state, iir, afes])]
+    fn idle(_: idle::Context) -> ! {
         loop {
-            let tick = Instant::now() > next_ms;
-
-            if tick {
-                next_ms += 400_000.cycles();
-                time += 1;
-            }
-
-            {
-                let socket =
-                    &mut *sockets.get::<smoltcp::socket::TcpSocket>(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],
-                                            y0: iir_state[2],
-                                            x1: iir_state[0],
-                                            y1: iir_state[2],
-                                    });
-
-                                    Ok::<server::Status, ()>(state)
-                                }),
-                                "stabilizer/afe0/gain": (|| c.resources.afes.0.get_gain()),
-                                "stabilizer/afe1/gain": (|| c.resources.afes.1.get_gain())
-                            ],
-
-                            modifiable_attributes: [
-                                "stabilizer/iir/state": server::IirRequest, (|req: server::IirRequest| {
-                                    c.resources.iir.lock(|iir| {
-                                        if req.channel >= 1 {
-                                            return Err(());
-                                        }
-
-                                        *iir = req.iir;
-
-                                        Ok::<server::IirRequest, ()>(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();
-            }
+            // TODO: Implement network interface.
+            cortex_m::asm::wfi();
         }
     }
 
     #[task(binds = ETH, priority = 1)]
     fn eth(_: eth::Context) {
-        unsafe { hal::ethernet::interrupt_handler() }
+        unsafe { stm32h7xx_hal::ethernet::interrupt_handler() }
     }
 
     #[task(binds = SPI2, priority = 3)]

From ab7d725235708727f80005f29c8fb170c94a792a Mon Sep 17 00:00:00 2001
From: Ryan Summers <ryan.summers@vertigo-designs.com>
Date: Fri, 29 Jan 2021 11:01:21 +0100
Subject: [PATCH 17/71] Updating lockin demo after testing

---
 src/bin/lockin-internal-demo.rs | 22 +++++++++++++++++-----
 1 file changed, 17 insertions(+), 5 deletions(-)

diff --git a/src/bin/lockin-internal-demo.rs b/src/bin/lockin-internal-demo.rs
index 969f332..a8761d2 100644
--- a/src/bin/lockin-internal-demo.rs
+++ b/src/bin/lockin-internal-demo.rs
@@ -9,10 +9,12 @@ const DAC_SEQUENCE: [f32; 8] =
 
 use dsp::{iir, iir_int, lockin::Lockin};
 use hardware::{Adc1Input, Dac0Output, Dac1Output, AFE0, AFE1};
-use stabilizer::hardware;
+use stabilizer::{ADC_SAMPLE_TICKS, hardware};
 
 const SCALE: f32 = ((1 << 15) - 1) as f32;
 
+const PHASE_SCALING: f32 = 1e12;
+
 #[rtic::app(device = stm32h7xx_hal::stm32, peripherals = true, monotonic = rtic::cyccnt::CYCCNT)]
 const APP: () = {
     struct Resources {
@@ -34,8 +36,18 @@ const APP: () = {
         // Configure the microcontroller
         let (mut stabilizer, _pounder) = hardware::setup(c.core, c.device);
 
+        // The desired corner frequency is always
+        let desired_corner_frequency = 10e3;
+        let gain = 1000.0;
+
+        // Calculate the IIR corner freuqency parameter as a function of the sample rate.
+        let corner_frequency = {
+            let sample_rate = 1.0 / (10e-9 * ADC_SAMPLE_TICKS as f32);
+            desired_corner_frequency / sample_rate
+        };
+
         let lockin = Lockin::new(
-            &iir_int::IIRState::lowpass(1e-3, 0.707, 2.), // TODO: expose
+            &iir_int::IIRState::lowpass(corner_frequency, 0.707, gain), // TODO: expose
         );
 
         // Enable ADC/DAC events
@@ -82,7 +94,8 @@ const APP: () = {
 
         // DAC0 always generates a fixed sinusoidal output.
         for (i, value) in DAC_SEQUENCE.iter().enumerate() {
-            let y = value * i16::MAX as f32;
+            // Full-scale gives a +/- 12V amplitude waveform. Scale it down to give +/- 100mV.
+            let y = value * i16::MAX as f32 / 120.0;
             // Note(unsafe): The DAC_SEQUENCE values are guaranteed to be normalized.
             let y = unsafe { y.to_int_unchecked::<i16>() };
 
@@ -90,7 +103,6 @@ const APP: () = {
             dac_samples[0][i] = y as u16 ^ 0x8000;
         }
 
-        // TODO: Verify that the DAC code is always generated at T=0
         let pll_phase = 0i32;
         let pll_frequency = 1i32 << (32 - 3); // 1/8 of the sample rate
 
@@ -120,7 +132,7 @@ const APP: () = {
         }
 
         // Filter phase through an IIR.
-        phase = c.resources.iir.update(&mut c.resources.iir_state, phase);
+        phase = c.resources.iir.update(&mut c.resources.iir_state, phase) * PHASE_SCALING;
 
         for value in dac_samples[1].iter_mut() {
             *value = phase as u16 ^ 0x8000

From b152343aafd29b7da68caf7caee7243b7a5f5903 Mon Sep 17 00:00:00 2001
From: Ryan Summers <ryan.summers@vertigo-designs.com>
Date: Fri, 29 Jan 2021 11:05:46 +0100
Subject: [PATCH 18/71] Style

---
 src/bin/lockin-internal-demo.rs | 5 +++--
 1 file changed, 3 insertions(+), 2 deletions(-)

diff --git a/src/bin/lockin-internal-demo.rs b/src/bin/lockin-internal-demo.rs
index a8761d2..8611a2c 100644
--- a/src/bin/lockin-internal-demo.rs
+++ b/src/bin/lockin-internal-demo.rs
@@ -9,7 +9,7 @@ const DAC_SEQUENCE: [f32; 8] =
 
 use dsp::{iir, iir_int, lockin::Lockin};
 use hardware::{Adc1Input, Dac0Output, Dac1Output, AFE0, AFE1};
-use stabilizer::{ADC_SAMPLE_TICKS, hardware};
+use stabilizer::{hardware, ADC_SAMPLE_TICKS};
 
 const SCALE: f32 = ((1 << 15) - 1) as f32;
 
@@ -132,7 +132,8 @@ const APP: () = {
         }
 
         // Filter phase through an IIR.
-        phase = c.resources.iir.update(&mut c.resources.iir_state, phase) * PHASE_SCALING;
+        phase = c.resources.iir.update(&mut c.resources.iir_state, phase)
+            * PHASE_SCALING;
 
         for value in dac_samples[1].iter_mut() {
             *value = phase as u16 ^ 0x8000

From 8b46c3c768cb54e21810054154446fd7dafce503 Mon Sep 17 00:00:00 2001
From: Ryan Summers <ryan.summers@vertigo-designs.com>
Date: Fri, 29 Jan 2021 18:55:54 +0100
Subject: [PATCH 19/71] Updating internal lockin demo

---
 src/bin/lockin-internal-demo.rs | 47 +++++++++------------------------
 1 file changed, 12 insertions(+), 35 deletions(-)

diff --git a/src/bin/lockin-internal-demo.rs b/src/bin/lockin-internal-demo.rs
index 8611a2c..54a429f 100644
--- a/src/bin/lockin-internal-demo.rs
+++ b/src/bin/lockin-internal-demo.rs
@@ -7,13 +7,9 @@
 const DAC_SEQUENCE: [f32; 8] =
     [0.0, 0.707, 1.0, 0.707, 0.0, -0.707, -1.0, -0.707];
 
-use dsp::{iir, iir_int, lockin::Lockin};
+use dsp::{iir_int, lockin::Lockin};
 use hardware::{Adc1Input, Dac0Output, Dac1Output, AFE0, AFE1};
-use stabilizer::{hardware, ADC_SAMPLE_TICKS};
-
-const SCALE: f32 = ((1 << 15) - 1) as f32;
-
-const PHASE_SCALING: f32 = 1e12;
+use stabilizer::hardware;
 
 #[rtic::app(device = stm32h7xx_hal::stm32, peripherals = true, monotonic = rtic::cyccnt::CYCCNT)]
 const APP: () = {
@@ -22,12 +18,6 @@ const APP: () = {
         adc1: Adc1Input,
         dacs: (Dac0Output, Dac1Output),
 
-        #[init([0.; 5])]
-        iir_state: iir::IIRState,
-
-        #[init(iir::IIR { ba: [1., 0., 0., 0., 0.], y_offset: 0., y_min: -SCALE - 1., y_max: SCALE })]
-        iir: iir::IIR,
-
         lockin: Lockin,
     }
 
@@ -36,18 +26,8 @@ const APP: () = {
         // Configure the microcontroller
         let (mut stabilizer, _pounder) = hardware::setup(c.core, c.device);
 
-        // The desired corner frequency is always
-        let desired_corner_frequency = 10e3;
-        let gain = 1000.0;
-
-        // Calculate the IIR corner freuqency parameter as a function of the sample rate.
-        let corner_frequency = {
-            let sample_rate = 1.0 / (10e-9 * ADC_SAMPLE_TICKS as f32);
-            desired_corner_frequency / sample_rate
-        };
-
         let lockin = Lockin::new(
-            &iir_int::IIRState::lowpass(corner_frequency, 0.707, gain), // TODO: expose
+            &iir_int::IIRState::lowpass(1e-3, 0.707, 2.), // TODO: expose
         );
 
         // Enable ADC/DAC events
@@ -84,8 +64,8 @@ const APP: () = {
     /// the same time bounds, meeting one also means the other is also met.
     ///
     /// TODO: Document
-    #[task(binds=DMA1_STR4, resources=[adc1, dacs, iir_state, iir, lockin], priority=2)]
-    fn process(mut c: process::Context) {
+    #[task(binds=DMA1_STR4, resources=[adc1, dacs, lockin], priority=2)]
+    fn process(c: process::Context) {
         let adc_samples = c.resources.adc1.acquire_buffer();
         let dac_samples = [
             c.resources.dacs.0.acquire_buffer(),
@@ -103,19 +83,19 @@ const APP: () = {
             dac_samples[0][i] = y as u16 ^ 0x8000;
         }
 
-        let pll_phase = 0i32;
+        let pll_phase = 0;
         let pll_frequency = 1i32 << (32 - 3); // 1/8 of the sample rate
 
         // Harmonic index of the LO: -1 to _de_modulate the fundamental
         let harmonic: i32 = -1;
 
         // Demodulation LO phase offset
-        let phase_offset: i32 = 0;
+        let phase_offset: i32 = (0.7495 * i32::MAX as f32) as i32;
         let sample_frequency = (pll_frequency as i32).wrapping_mul(harmonic);
         let mut sample_phase = phase_offset
             .wrapping_add((pll_phase as i32).wrapping_mul(harmonic));
 
-        let mut phase = 0f32;
+        let mut phase = 0i16;
 
         for sample in adc_samples.iter() {
             // Convert to signed, MSB align the ADC sample.
@@ -127,20 +107,17 @@ const APP: () = {
             // Advance the sample phase.
             sample_phase = sample_phase.wrapping_add(sample_frequency);
 
-            // Convert from IQ to phase.
-            phase = output.phase() as _;
+            // Convert from IQ to phase. Scale the phase so that it fits in the DAC range. We do
+            // this by shifting it down into the 16-bit range.
+            phase = (output.phase() >> 16) as i16;
         }
 
-        // Filter phase through an IIR.
-        phase = c.resources.iir.update(&mut c.resources.iir_state, phase)
-            * PHASE_SCALING;
-
         for value in dac_samples[1].iter_mut() {
             *value = phase as u16 ^ 0x8000
         }
     }
 
-    #[idle(resources=[iir_state, iir, afes])]
+    #[idle(resources=[afes])]
     fn idle(_: idle::Context) -> ! {
         loop {
             // TODO: Implement network interface.

From 0d1b237202e67c9f2d27cf57806bbf2199e84c15 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Robert=20J=C3=B6rdens?= <rj@quartiq.de>
Date: Sat, 30 Jan 2021 18:05:54 +0100
Subject: [PATCH 20/71] complex: richer API

---
 dsp/src/complex.rs | 55 ++++++++++++++++++++++++++++++++++++++++------
 dsp/src/lockin.rs  | 21 ++++++++++++++++--
 src/bin/lockin.rs  | 37 +++++++++++++++++--------------
 3 files changed, 87 insertions(+), 26 deletions(-)

diff --git a/dsp/src/complex.rs b/dsp/src/complex.rs
index 4366b43..2bdb9ea 100644
--- a/dsp/src/complex.rs
+++ b/dsp/src/complex.rs
@@ -1,17 +1,58 @@
-use super::atan2;
+use super::{atan2, cossin};
 use serde::{Deserialize, Serialize};
 
-#[derive(Copy, Clone, Default, Deserialize, Serialize)]
+#[derive(Copy, Clone, Default, PartialEq, Debug, Deserialize, Serialize)]
 pub struct Complex<T>(pub T, pub T);
 
 impl Complex<i32> {
-    pub fn power(&self) -> i32 {
-        (((self.0 as i64) * (self.0 as i64)
-            + (self.1 as i64) * (self.1 as i64))
-            >> 32) as i32
+    /// Return a Complex on the unit circle given an angle.
+    ///
+    /// Example:
+    ///
+    /// ```
+    /// use dsp::Complex;
+    /// Complex::<i32>::from_angle(0);
+    /// Complex::<i32>::from_angle(1 << 30);  // pi/2
+    /// Complex::<i32>::from_angle(-1 << 30);  // -pi/2
+    /// ```
+    #[inline(always)]
+    pub fn from_angle(angle: i32) -> Complex<i32> {
+        cossin(angle)
     }
 
-    pub fn phase(&self) -> i32 {
+    /// Return the absolute square (the squared magnitude).
+    ///
+    /// Note: Normalization is `1 << 31`, i.e. Q0.31.
+    ///
+    /// Example:
+    ///
+    /// ```
+    /// use dsp::Complex;
+    /// assert_eq!(Complex(i32::MAX, 0).abs_sqr(), i32::MAX - 1);
+    /// assert_eq!(Complex(i32::MIN + 1, 0).abs_sqr(), i32::MAX - 1);
+    /// ```
+    pub fn abs_sqr(&self) -> i32 {
+        (((self.0 as i64) * (self.0 as i64)
+            + (self.1 as i64) * (self.1 as i64))
+            >> 31) as i32
+    }
+
+    /// Return the angle.
+    ///
+    /// Note: Normalization is `1 << 31 == pi`.
+    ///
+    /// Example:
+    ///
+    /// ```
+    /// use dsp::Complex;
+    /// assert_eq!(Complex(i32::MAX, 0).arg(), 0);
+    /// assert_eq!(Complex(-i32::MAX, 1).arg(), i32::MAX);
+    /// assert_eq!(Complex(-i32::MAX, -1).arg(), -i32::MAX);
+    /// assert_eq!(Complex(0, -i32::MAX).arg(), -i32::MAX >> 1);
+    /// assert_eq!(Complex(0, i32::MAX).arg(), (i32::MAX >> 1) + 1);
+    /// assert_eq!(Complex(i32::MAX, i32::MAX).arg(), (i32::MAX >> 2) + 1);
+    /// ```
+    pub fn arg(&self) -> i32 {
         atan2(self.1, self.0)
     }
 }
diff --git a/dsp/src/lockin.rs b/dsp/src/lockin.rs
index aa397bb..b254bea 100644
--- a/dsp/src/lockin.rs
+++ b/dsp/src/lockin.rs
@@ -1,4 +1,4 @@
-use super::{cossin, iir_int, Complex};
+use super::{iir_int, Complex};
 use serde::{Deserialize, Serialize};
 
 #[derive(Copy, Clone, Default, Deserialize, Serialize)]
@@ -19,7 +19,7 @@ impl Lockin {
 
     pub fn update(&mut self, signal: i32, phase: i32) -> Complex<i32> {
         // Get the LO signal for demodulation.
-        let m = cossin(phase);
+        let m = Complex::from_angle(phase);
 
         // Mix with the LO signal, filter with the IIR lowpass,
         // return IQ (in-phase and quadrature) data.
@@ -35,6 +35,23 @@ impl Lockin {
             ),
         )
     }
+
+    pub fn feed<I: IntoIterator<Item = i32>>(
+        &mut self,
+        signal: I,
+        phase: i32,
+        frequency: i32,
+    ) -> Option<Complex<i32>> {
+        let mut phase = phase;
+
+        signal
+            .into_iter()
+            .map(|s| {
+                phase = phase.wrapping_add(frequency);
+                self.update(s, phase)
+            })
+            .last()
+    }
 }
 
 #[cfg(test)]
diff --git a/src/bin/lockin.rs b/src/bin/lockin.rs
index 1519853..2b40dbd 100644
--- a/src/bin/lockin.rs
+++ b/src/bin/lockin.rs
@@ -123,27 +123,26 @@ const APP: () = {
             22, // relative PLL phase bandwidth: 2**-22, TODO: expose
         );
 
-        // Harmonic index of the LO: -1 to _de_modulate the fundamental
+        // Harmonic index of the LO: -1 to _de_modulate the fundamental (complex conjugate)
         let harmonic: i32 = -1;
         // Demodulation LO phase offset
         let phase_offset: i32 = 0;
         let sample_frequency = ((pll_frequency >> SAMPLE_BUFFER_SIZE_LOG2)
             as i32)
             .wrapping_mul(harmonic); // TODO: maybe rounding bias
-        let mut sample_phase =
+        let sample_phase =
             phase_offset.wrapping_add(pll_phase.wrapping_mul(harmonic));
 
-        for i in 0..adc_samples[0].len() {
-            // Convert to signed, MSB align the ADC sample.
-            let input = (adc_samples[0][i] as i16 as i32) << 16;
-            // Obtain demodulated, filtered IQ sample.
-            let output = lockin.update(input, sample_phase);
-            // Advance the sample phase.
-            sample_phase = sample_phase.wrapping_add(sample_frequency);
-
+        if let Some(output) = lockin.feed(
+            adc_samples[0].iter().map(|&i|
+                // Convert to signed, MSB align the ADC sample.
+                (i as i16 as i32) << 16),
+            sample_phase,
+            sample_frequency,
+        ) {
             // Convert from IQ to power and phase.
-            let mut power = output.power() as _;
-            let mut phase = output.phase() as _;
+            let mut power = output.abs_sqr() as _;
+            let mut phase = output.arg() as _;
 
             // Filter power and phase through IIR filters.
             // Note: Normalization to be done in filters. Phase will wrap happily.
@@ -152,13 +151,17 @@ const APP: () = {
                 phase = iir_ch[1][j].update(&mut iir_state[1][j], phase);
             }
 
+            // TODO: IIR filter DC gain needs to be 1/(1 << 16)
+
             // Note(unsafe): range clipping to i16 is ensured by IIR filters above.
             // Convert to DAC data.
-            unsafe {
-                dac_samples[0][i] =
-                    power.to_int_unchecked::<i16>() as u16 ^ 0x8000;
-                dac_samples[1][i] =
-                    phase.to_int_unchecked::<i16>() as u16 ^ 0x8000;
+            for i in 0..dac_samples[0].len() {
+                unsafe {
+                    dac_samples[0][i] =
+                        power.to_int_unchecked::<i16>() as u16 ^ 0x8000;
+                    dac_samples[1][i] =
+                        phase.to_int_unchecked::<i16>() as u16 ^ 0x8000;
+                }
             }
         }
     }

From be7aad1b81d9d55e922788fdfa38971874b9b129 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Robert=20J=C3=B6rdens?= <rj@quartiq.de>
Date: Sat, 30 Jan 2021 20:49:31 +0100
Subject: [PATCH 21/71] rpll: add unittest

---
 dsp/Cargo.toml    |   3 ++
 dsp/src/lockin.rs |   1 -
 dsp/src/rpll.rs   | 130 +++++++++++++++++++++++++++++++++++++++++++++-
 3 files changed, 132 insertions(+), 2 deletions(-)

diff --git a/dsp/Cargo.toml b/dsp/Cargo.toml
index 548e64f..16fd500 100644
--- a/dsp/Cargo.toml
+++ b/dsp/Cargo.toml
@@ -10,6 +10,9 @@ serde = { version = "1.0", features = ["derive"], default-features = false }
 
 [dev-dependencies]
 criterion = "0.3"
+rand = "0.8"
+ndarray = "0.14"
+ndarray-stats = "0.4"
 
 [[bench]]
 name = "trig"
diff --git a/dsp/src/lockin.rs b/dsp/src/lockin.rs
index b254bea..b75038e 100644
--- a/dsp/src/lockin.rs
+++ b/dsp/src/lockin.rs
@@ -57,7 +57,6 @@ impl Lockin {
 #[cfg(test)]
 mod test {
     use crate::{
-        atan2,
         iir_int::IIRState,
         lockin::Lockin,
         rpll::RPLL,
diff --git a/dsp/src/rpll.rs b/dsp/src/rpll.rs
index e3b3894..057abc8 100644
--- a/dsp/src/rpll.rs
+++ b/dsp/src/rpll.rs
@@ -51,7 +51,7 @@ impl RPLL {
         shift_phase: u8,
     ) -> (i32, u32) {
         debug_assert!(shift_frequency > self.dt2);
-        debug_assert!(shift_phase > self.dt2);
+        debug_assert!(shift_phase >= self.dt2);
         // Advance phase
         self.y = self.y.wrapping_add(self.f as i32);
         if let Some(x) = input {
@@ -84,3 +84,131 @@ impl RPLL {
         (self.y, self.f)
     }
 }
+
+#[cfg(test)]
+mod test {
+    use super::RPLL;
+    use ndarray::prelude::*;
+    use ndarray_stats::QuantileExt;
+    use rand::{prelude::*, rngs::StdRng};
+    use std::vec::Vec;
+
+    #[test]
+    fn make() {
+        let _ = RPLL::new(8, 0);
+    }
+
+    struct Harness {
+        rpll: RPLL,
+        dt2: u8,
+        shift_frequency: u8,
+        shift_phase: u8,
+        noise: i32,
+        period: i32,
+        next: i32,
+        time: i32,
+        rng: StdRng,
+    }
+
+    impl Harness {
+        fn default() -> Self {
+            Harness {
+                rpll: RPLL::new(8, 0),
+                dt2: 8,
+                shift_frequency: 9,
+                shift_phase: 8,
+                noise: 0,
+                period: 333,
+                next: 111,
+                time: 0,
+                rng: StdRng::seed_from_u64(42),
+            }
+        }
+
+        fn run(&mut self, n: i32) -> (Vec<i64>, Vec<i64>) {
+            let mut y = Vec::<i64>::new();
+            let mut f = Vec::<i64>::new();
+            for _ in 0..n {
+                let timestamp = if self.time - self.next >= 0 {
+                    let p_noise = self.rng.gen_range(-self.noise..=self.noise);
+                    let timestamp = self.next.wrapping_add(p_noise);
+                    self.next = self.next.wrapping_add(self.period);
+                    Some(timestamp)
+                } else {
+                    None
+                };
+                let (yi, fi) = self.rpll.update(
+                    timestamp,
+                    self.shift_frequency,
+                    self.shift_phase,
+                );
+                let y_ref = (self.time.wrapping_sub(self.next) as i64
+                    * (1i64 << 32)
+                    / self.period as i64) as i32;
+                y.push((yi as i64).wrapping_sub(y_ref as i64)); // phase error
+                let p_ref = 1 << 32 + self.dt2;
+                let p_sig = fi as i64 * self.period as i64;
+                f.push(p_sig.wrapping_sub(p_ref)); // relative frequency error
+                                                   // advance time
+                self.time = self.time.wrapping_add(1 << self.dt2);
+            }
+            (y, f)
+        }
+    }
+
+    #[test]
+    fn default() {
+        let mut h = Harness::default();
+        h.run(1 << 10); // settle
+
+        let (y, f) = h.run(1 << 10);
+        let y = Array::from(y);
+        let f = Array::from(f);
+
+        println!(
+            "f: {}+-{}",
+            f.mean().unwrap(),
+            *f.mapv(i64::abs).max().unwrap()
+        );
+        println!(
+            "y: {}+-{}",
+            y.mean().unwrap(),
+            *y.mapv(i64::abs).max().unwrap()
+        );
+
+        assert!(f.mean().unwrap().abs() < 20);
+        assert!(*f.mapv(i64::abs).max().unwrap() < 1 << 16);
+        assert!(y.mean().unwrap().abs() < 1 << 6);
+        assert!(*y.mapv(i64::abs).max().unwrap() < 1 << 8);
+    }
+
+    #[test]
+    fn noise() {
+        let mut h = Harness::default();
+        h.noise = 10;
+        h.shift_frequency = 23;
+        h.shift_phase = 22;
+
+        h.run(1 << 20); // settle
+
+        let (y, f) = h.run(1 << 10);
+        let y = Array::from(y);
+        let f = Array::from(f);
+
+        println!(
+            "f: {}+-{}",
+            f.mean().unwrap(),
+            *f.mapv(i64::abs).max().unwrap()
+        );
+        println!(
+            "y: {}+-{}",
+            y.mean().unwrap(),
+            *y.mapv(i64::abs).max().unwrap()
+        );
+
+        assert!(f.mean().unwrap().abs() < 1 << 14);
+        assert!(*f.mapv(i64::abs).max().unwrap() < 1 << 22);
+        assert!(y.mean().unwrap().abs() < 1 << 19);
+        assert!(*y.mapv(i64::abs).max().unwrap() < 1 << 19);
+    }
+}

From 183d84ad7af028b749951a1d10ecb9036f13e472 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Robert=20J=C3=B6rdens?= <rj@quartiq.de>
Date: Sat, 30 Jan 2021 20:57:44 +0100
Subject: [PATCH 22/71] cargo.lock: update

---
 Cargo.lock | 195 +++++++++++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 195 insertions(+)

diff --git a/Cargo.lock b/Cargo.lock
index c6083ae..4f6bca2 100644
--- a/Cargo.lock
+++ b/Cargo.lock
@@ -347,6 +347,9 @@ version = "0.1.0"
 dependencies = [
  "criterion",
  "libm",
+ "ndarray",
+ "ndarray-stats",
+ "rand 0.8.3",
  "serde",
 ]
 
@@ -423,6 +426,28 @@ dependencies = [
  "version_check",
 ]
 
+[[package]]
+name = "getrandom"
+version = "0.1.16"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "8fc3cb4d91f53b50155bdcfd23f6a4c39ae1969c2ae85982b135750cccaf5fce"
+dependencies = [
+ "cfg-if 1.0.0",
+ "libc",
+ "wasi 0.9.0+wasi-snapshot-preview1",
+]
+
+[[package]]
+name = "getrandom"
+version = "0.2.2"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "c9495705279e7140bf035dde1f6e750c162df8b625267cd52cc44e0b156732c8"
+dependencies = [
+ "cfg-if 1.0.0",
+ "libc",
+ "wasi 0.10.2+wasi-snapshot-preview1",
+]
+
 [[package]]
 name = "half"
 version = "1.6.0"
@@ -533,6 +558,15 @@ version = "0.7.2"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "c75de51135344a4f8ed3cfe2720dc27736f7711989703a0b43aadf3753c55577"
 
+[[package]]
+name = "matrixmultiply"
+version = "0.2.4"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "916806ba0031cd542105d916a97c8572e1fa6dd79c9c51e7eb43a09ec2dd84c1"
+dependencies = [
+ "rawpointer",
+]
+
 [[package]]
 name = "mcp23017"
 version = "0.1.1"
@@ -571,6 +605,62 @@ version = "1.0.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "546c37ac5d9e56f55e73b677106873d9d9f5190605e41a856503623648488cae"
 
+[[package]]
+name = "ndarray"
+version = "0.14.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "6c0d5c9540a691d153064dc47a4db2504587a75eae07bf1d73f7a596ebc73c04"
+dependencies = [
+ "matrixmultiply",
+ "num-complex",
+ "num-integer",
+ "num-traits",
+ "rawpointer",
+]
+
+[[package]]
+name = "ndarray-stats"
+version = "0.4.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "22c95a780960082c5746f6bf0ab22d4a3b8cee72bf580acfe9f1e10bc5ea8152"
+dependencies = [
+ "indexmap",
+ "itertools",
+ "ndarray",
+ "noisy_float",
+ "num-integer",
+ "num-traits",
+ "rand 0.7.3",
+]
+
+[[package]]
+name = "noisy_float"
+version = "0.1.13"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "a14c16cde392a1cd18084ffd8348cb8937525130e62f0478d72dcc683698809d"
+dependencies = [
+ "num-traits",
+]
+
+[[package]]
+name = "num-complex"
+version = "0.3.1"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "747d632c0c558b87dbabbe6a82f3b4ae03720d0646ac5b7b4dae89394be5f2c5"
+dependencies = [
+ "num-traits",
+]
+
+[[package]]
+name = "num-integer"
+version = "0.1.44"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "d2cc698a63b549a70bc047073d2949cce27cd1c7b0a4a862d08a8031bc2801db"
+dependencies = [
+ "autocfg",
+ "num-traits",
+]
+
 [[package]]
 name = "num-traits"
 version = "0.2.14"
@@ -630,6 +720,12 @@ dependencies = [
  "web-sys",
 ]
 
+[[package]]
+name = "ppv-lite86"
+version = "0.2.10"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "ac74c624d6b2d21f425f752262f42188365d7b8ff1aff74c82e45136510a4857"
+
 [[package]]
 name = "proc-macro2"
 version = "1.0.24"
@@ -654,6 +750,93 @@ version = "0.2.2"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "e2a38df5b15c8d5c7e8654189744d8e396bddc18ad48041a500ce52d6948941f"
 
+[[package]]
+name = "rand"
+version = "0.7.3"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "6a6b1679d49b24bbfe0c803429aa1874472f50d9b363131f0e89fc356b544d03"
+dependencies = [
+ "getrandom 0.1.16",
+ "libc",
+ "rand_chacha 0.2.2",
+ "rand_core 0.5.1",
+ "rand_hc 0.2.0",
+]
+
+[[package]]
+name = "rand"
+version = "0.8.3"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "0ef9e7e66b4468674bfcb0c81af8b7fa0bb154fa9f28eb840da5c447baeb8d7e"
+dependencies = [
+ "libc",
+ "rand_chacha 0.3.0",
+ "rand_core 0.6.1",
+ "rand_hc 0.3.0",
+]
+
+[[package]]
+name = "rand_chacha"
+version = "0.2.2"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "f4c8ed856279c9737206bf725bf36935d8666ead7aa69b52be55af369d193402"
+dependencies = [
+ "ppv-lite86",
+ "rand_core 0.5.1",
+]
+
+[[package]]
+name = "rand_chacha"
+version = "0.3.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "e12735cf05c9e10bf21534da50a147b924d555dc7a547c42e6bb2d5b6017ae0d"
+dependencies = [
+ "ppv-lite86",
+ "rand_core 0.6.1",
+]
+
+[[package]]
+name = "rand_core"
+version = "0.5.1"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "90bde5296fc891b0cef12a6d03ddccc162ce7b2aff54160af9338f8d40df6d19"
+dependencies = [
+ "getrandom 0.1.16",
+]
+
+[[package]]
+name = "rand_core"
+version = "0.6.1"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "c026d7df8b298d90ccbbc5190bd04d85e159eaf5576caeacf8741da93ccbd2e5"
+dependencies = [
+ "getrandom 0.2.2",
+]
+
+[[package]]
+name = "rand_hc"
+version = "0.2.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "ca3129af7b92a17112d59ad498c6f81eaf463253766b90396d39ea7a39d6613c"
+dependencies = [
+ "rand_core 0.5.1",
+]
+
+[[package]]
+name = "rand_hc"
+version = "0.3.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "3190ef7066a446f2e7f42e239d161e905420ccab01eb967c9eb27d21b2322a73"
+dependencies = [
+ "rand_core 0.6.1",
+]
+
+[[package]]
+name = "rawpointer"
+version = "0.2.1"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "60a357793950651c4ed0f3f52338f53b2f809f32d83a07f72909fa13e4c6c1e3"
+
 [[package]]
 name = "rayon"
 version = "1.5.0"
@@ -975,6 +1158,18 @@ dependencies = [
  "winapi-util",
 ]
 
+[[package]]
+name = "wasi"
+version = "0.9.0+wasi-snapshot-preview1"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "cccddf32554fecc6acb585f82a32a72e28b48f8c4c1883ddfeeeaa96f7d8e519"
+
+[[package]]
+name = "wasi"
+version = "0.10.2+wasi-snapshot-preview1"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "fd6fbd9a79829dd1ad0cc20627bf1ed606756a7f77edff7b66b7064f9cb327c6"
+
 [[package]]
 name = "wasm-bindgen"
 version = "0.2.69"

From 6b2d8169f068f1587e7305218cd66c235dec7bbf Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Robert=20J=C3=B6rdens?= <rj@quartiq.de>
Date: Sun, 31 Jan 2021 13:25:01 +0100
Subject: [PATCH 23/71] rpll: more/cleaner tests

---
 dsp/src/rpll.rs | 116 ++++++++++++++++++++++++++++--------------------
 1 file changed, 68 insertions(+), 48 deletions(-)

diff --git a/dsp/src/rpll.rs b/dsp/src/rpll.rs
index 057abc8..d17d42f 100644
--- a/dsp/src/rpll.rs
+++ b/dsp/src/rpll.rs
@@ -125,9 +125,9 @@ mod test {
             }
         }
 
-        fn run(&mut self, n: i32) -> (Vec<i64>, Vec<i64>) {
-            let mut y = Vec::<i64>::new();
-            let mut f = Vec::<i64>::new();
+        fn run(&mut self, n: usize) -> (Vec<f32>, Vec<f32>) {
+            let mut y = Vec::<f32>::new();
+            let mut f = Vec::<f32>::new();
             for _ in 0..n {
                 let timestamp = if self.time - self.next >= 0 {
                     let p_noise = self.rng.gen_range(-self.noise..=self.noise);
@@ -145,70 +145,90 @@ mod test {
                 let y_ref = (self.time.wrapping_sub(self.next) as i64
                     * (1i64 << 32)
                     / self.period as i64) as i32;
-                y.push((yi as i64).wrapping_sub(y_ref as i64)); // phase error
+                // phase error
+                y.push(yi.wrapping_sub(y_ref) as f32 / 2f32.powi(32));
                 let p_ref = 1 << 32 + self.dt2;
                 let p_sig = fi as i64 * self.period as i64;
-                f.push(p_sig.wrapping_sub(p_ref)); // relative frequency error
-                                                   // advance time
+                // relative frequency error
+                f.push(p_sig.wrapping_sub(p_ref) as f32 / 2f32.powi(32));
+                // advance time
                 self.time = self.time.wrapping_add(1 << self.dt2);
             }
             (y, f)
         }
+
+        fn measure(&mut self, n: usize) -> (f32, f32, f32, f32) {
+            let t_settle = (1 << self.shift_frequency - self.dt2 + 4)
+                + (1 << self.shift_phase - self.dt2 + 4);
+            self.run(t_settle);
+
+            let (y, f) = self.run(n);
+            let y = Array::from(y);
+            let f = Array::from(f);
+
+            let fm = f.mean().unwrap();
+            let fs = f.std_axis(Axis(0), 0.).into_scalar();
+            let ym = y.mean().unwrap();
+            let ys = y.std_axis(Axis(0), 0.).into_scalar();
+
+            println!("f: {:.2e}±{:.2e}; y: {:.2e}±{:.2e}", fm, fs, ym, ys);
+            (fm, fs, ym, ys)
+        }
     }
 
     #[test]
     fn default() {
         let mut h = Harness::default();
-        h.run(1 << 10); // settle
 
-        let (y, f) = h.run(1 << 10);
-        let y = Array::from(y);
-        let f = Array::from(f);
-
-        println!(
-            "f: {}+-{}",
-            f.mean().unwrap(),
-            *f.mapv(i64::abs).max().unwrap()
-        );
-        println!(
-            "y: {}+-{}",
-            y.mean().unwrap(),
-            *y.mapv(i64::abs).max().unwrap()
-        );
-
-        assert!(f.mean().unwrap().abs() < 20);
-        assert!(*f.mapv(i64::abs).max().unwrap() < 1 << 16);
-        assert!(y.mean().unwrap().abs() < 1 << 6);
-        assert!(*y.mapv(i64::abs).max().unwrap() < 1 << 8);
+        let (fm, fs, ym, ys) = h.measure(1 << 16);
+        assert!(fm.abs() < 1e-9);
+        assert!(fs.abs() < 8e-6);
+        assert!(ym.abs() < 2e-8);
+        assert!(ys.abs() < 2e-8);
     }
 
     #[test]
-    fn noise() {
+    fn noisy() {
         let mut h = Harness::default();
         h.noise = 10;
         h.shift_frequency = 23;
         h.shift_phase = 22;
 
-        h.run(1 << 20); // settle
-
-        let (y, f) = h.run(1 << 10);
-        let y = Array::from(y);
-        let f = Array::from(f);
-
-        println!(
-            "f: {}+-{}",
-            f.mean().unwrap(),
-            *f.mapv(i64::abs).max().unwrap()
-        );
-        println!(
-            "y: {}+-{}",
-            y.mean().unwrap(),
-            *y.mapv(i64::abs).max().unwrap()
-        );
-
-        assert!(f.mean().unwrap().abs() < 1 << 14);
-        assert!(*f.mapv(i64::abs).max().unwrap() < 1 << 22);
-        assert!(y.mean().unwrap().abs() < 1 << 19);
-        assert!(*y.mapv(i64::abs).max().unwrap() < 1 << 19);
+        let (fm, fs, ym, ys) = h.measure(1 << 16);
+        assert!(fm.abs() < 1e-6);
+        assert!(fs.abs() < 6e-4);
+        assert!(ym.abs() < 2e-4);
+        assert!(ys.abs() < 2e-4);
     }
+
+    #[test]
+    fn narrow_fast() {
+        let mut h = Harness::default();
+        h.period = 990;
+        h.noise = 5;
+        h.shift_frequency = 23;
+        h.shift_phase = 22;
+
+        let (fm, fs, ym, ys) = h.measure(1 << 16);
+        assert!(fm.abs() < 7e-6);
+        assert!(fs.abs() < 6e-4);
+        assert!(ym.abs() < 1e-3);
+        assert!(ys.abs() < 1e-4);
+    }
+
+    /*#[test]
+    fn narrow_slow() {
+        let mut h = Harness::default();
+        h.period = 1818181;
+        h.noise = 1800;
+        h.shift_frequency = 23;
+        h.shift_phase = 22;
+
+        let (fm, fs, ym, ys) = h.measure(1 << 16);
+        assert!(fm.abs() < 1e-8);
+        assert!(fs.abs() < 6e-4);
+        assert!(ym.abs() < 2e-4);
+        assert!(ys.abs() < 2e-4);
+    }
+    */
 }

From ab20d67a073fb703fa37dfd82a593a500ba32e74 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Robert=20J=C3=B6rdens?= <rj@quartiq.de>
Date: Sun, 31 Jan 2021 13:42:15 +0100
Subject: [PATCH 24/71] rpll: remove redundant time tracking

---
 dsp/src/rpll.rs   | 28 ++++++++++++++--------------
 src/bin/lockin.rs |  2 +-
 2 files changed, 15 insertions(+), 15 deletions(-)

diff --git a/dsp/src/rpll.rs b/dsp/src/rpll.rs
index d17d42f..4cd74b8 100644
--- a/dsp/src/rpll.rs
+++ b/dsp/src/rpll.rs
@@ -6,7 +6,6 @@
 #[derive(Copy, Clone, Default)]
 pub struct RPLL {
     dt2: u8, // 1 << dt2 is the counter rate to update() rate ratio
-    t: i32,  // current counter time
     x: i32,  // previous timestamp
     ff: u32, // current frequency estimate from frequency loop
     f: u32,  // current frequency estimate from both frequency and phase loop
@@ -18,14 +17,12 @@ impl RPLL {
     ///
     /// Args:
     /// * dt2: inverse update() rate. 1 << dt2 is the counter rate to update() rate ratio.
-    /// * t: Counter time. Counter value at the first update() call. Typically 0.
     ///
     /// Returns:
     /// Initialized RPLL instance.
-    pub fn new(dt2: u8, t: i32) -> RPLL {
+    pub fn new(dt2: u8) -> RPLL {
         RPLL {
             dt2,
-            t,
             ..Default::default()
         }
     }
@@ -69,7 +66,7 @@ impl RPLL {
             // Update frequency lock
             self.ff = self.ff.wrapping_add(p_ref.wrapping_sub(p_sig) as u32);
             // Time in counter cycles between timestamp and "now"
-            let dt = self.t.wrapping_sub(x);
+            let dt = x.wrapping_neg() & ((1 << self.dt2) - 1);
             // Reference phase estimate "now"
             let y_ref = ((self.f >> self.dt2) as i32).wrapping_mul(dt);
             // Phase error
@@ -79,8 +76,6 @@ impl RPLL {
                 .ff
                 .wrapping_add((dy >> (shift_phase - self.dt2)) as u32);
         }
-        // Advance time
-        self.t = self.t.wrapping_add(1 << self.dt2);
         (self.y, self.f)
     }
 }
@@ -95,7 +90,7 @@ mod test {
 
     #[test]
     fn make() {
-        let _ = RPLL::new(8, 0);
+        let _ = RPLL::new(8);
     }
 
     struct Harness {
@@ -106,6 +101,7 @@ mod test {
         noise: i32,
         period: i32,
         next: i32,
+        next_noisy: i32,
         time: i32,
         rng: StdRng,
     }
@@ -113,13 +109,14 @@ mod test {
     impl Harness {
         fn default() -> Self {
             Harness {
-                rpll: RPLL::new(8, 0),
+                rpll: RPLL::new(8),
                 dt2: 8,
                 shift_frequency: 9,
                 shift_phase: 8,
                 noise: 0,
                 period: 333,
                 next: 111,
+                next_noisy: 111,
                 time: 0,
                 rng: StdRng::seed_from_u64(42),
             }
@@ -129,10 +126,12 @@ mod test {
             let mut y = Vec::<f32>::new();
             let mut f = Vec::<f32>::new();
             for _ in 0..n {
-                let timestamp = if self.time - self.next >= 0 {
-                    let p_noise = self.rng.gen_range(-self.noise..=self.noise);
-                    let timestamp = self.next.wrapping_add(p_noise);
+                let timestamp = if self.time - self.next_noisy >= 0 {
+                    assert!(self.time - self.next_noisy < 1 << self.dt2);
+                    let timestamp = self.next_noisy;
                     self.next = self.next.wrapping_add(self.period);
+                    let p_noise = self.rng.gen_range(-self.noise..=self.noise);
+                    self.next_noisy = self.next.wrapping_add(p_noise);
                     Some(timestamp)
                 } else {
                     None
@@ -197,7 +196,7 @@ mod test {
         let (fm, fs, ym, ys) = h.measure(1 << 16);
         assert!(fm.abs() < 1e-6);
         assert!(fs.abs() < 6e-4);
-        assert!(ym.abs() < 2e-4);
+        assert!(ym.abs() < 4e-4);
         assert!(ys.abs() < 2e-4);
     }
 
@@ -216,7 +215,8 @@ mod test {
         assert!(ys.abs() < 1e-4);
     }
 
-    /*#[test]
+    /*
+    #[test]
     fn narrow_slow() {
         let mut h = Harness::default();
         h.period = 1818181;
diff --git a/src/bin/lockin.rs b/src/bin/lockin.rs
index 2b40dbd..f687c21 100644
--- a/src/bin/lockin.rs
+++ b/src/bin/lockin.rs
@@ -53,7 +53,7 @@ const APP: () = {
         // Configure the microcontroller
         let (mut stabilizer, _pounder) = hardware::setup(c.core, c.device);
 
-        let pll = RPLL::new(ADC_SAMPLE_TICKS_LOG2 + SAMPLE_BUFFER_SIZE_LOG2, 0);
+        let pll = RPLL::new(ADC_SAMPLE_TICKS_LOG2 + SAMPLE_BUFFER_SIZE_LOG2);
 
         let lockin = Lockin::new(
             &iir_int::IIRState::lowpass(1e-3, 0.707, 2.), // TODO: expose

From 82c8fa1a07abf5d41b70ff4e71ebd798f1c48f9b Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Robert=20J=C3=B6rdens?= <rj@quartiq.de>
Date: Sun, 31 Jan 2021 17:10:03 +0100
Subject: [PATCH 25/71] rpll: extend tests

---
 dsp/src/rpll.rs   | 93 +++++++++++++++++++++++++++++++++++------------
 src/bin/lockin.rs |  6 +--
 2 files changed, 72 insertions(+), 27 deletions(-)

diff --git a/dsp/src/rpll.rs b/dsp/src/rpll.rs
index 4cd74b8..4744093 100644
--- a/dsp/src/rpll.rs
+++ b/dsp/src/rpll.rs
@@ -57,24 +57,22 @@ impl RPLL {
             // Store timestamp for next time.
             self.x = x;
             // Phase using the current frequency estimate
-            let p_sig_64 = (self.ff as u64).wrapping_mul(dx as u64);
+            let p_sig_64 = self.ff as u64 * dx as u64;
             // Add half-up rounding bias and apply gain/attenuation
-            let p_sig = (p_sig_64.wrapping_add(1u64 << (shift_frequency - 1))
-                >> shift_frequency) as i32;
+            let p_sig = ((p_sig_64 + (1u32 << (shift_frequency - 1)) as u64)
+                >> shift_frequency) as u32;
             // Reference phase (1 << dt2 full turns) with gain/attenuation applied
-            let p_ref = 1i32 << (32 + self.dt2 - shift_frequency);
+            let p_ref = 1u32 << (32 + self.dt2 - shift_frequency);
             // Update frequency lock
             self.ff = self.ff.wrapping_add(p_ref.wrapping_sub(p_sig) as u32);
             // Time in counter cycles between timestamp and "now"
-            let dt = x.wrapping_neg() & ((1 << self.dt2) - 1);
+            let dt = (x.wrapping_neg() & ((1 << self.dt2) - 1)) as u32;
             // Reference phase estimate "now"
-            let y_ref = ((self.f >> self.dt2) as i32).wrapping_mul(dt);
-            // Phase error
-            let dy = y_ref.wrapping_sub(self.y);
+            let y_ref = (self.f >> self.dt2).wrapping_mul(dt) as i32;
+            // Phase error with gain
+            let dy = y_ref.wrapping_sub(self.y) >> (shift_phase - self.dt2);
             // Current frequency estimate from frequency lock and phase error
-            self.f = self
-                .ff
-                .wrapping_add((dy >> (shift_phase - self.dt2)) as u32);
+            self.f = self.ff.wrapping_add(dy as u32);
         }
         (self.y, self.f)
     }
@@ -128,8 +126,8 @@ mod test {
             for _ in 0..n {
                 let timestamp = if self.time - self.next_noisy >= 0 {
                     assert!(self.time - self.next_noisy < 1 << self.dt2);
-                    let timestamp = self.next_noisy;
                     self.next = self.next.wrapping_add(self.period);
+                    let timestamp = self.next_noisy;
                     let p_noise = self.rng.gen_range(-self.noise..=self.noise);
                     self.next_noisy = self.next.wrapping_add(p_noise);
                     Some(timestamp)
@@ -141,15 +139,21 @@ mod test {
                     self.shift_frequency,
                     self.shift_phase,
                 );
+
                 let y_ref = (self.time.wrapping_sub(self.next) as i64
                     * (1i64 << 32)
                     / self.period as i64) as i32;
                 // phase error
                 y.push(yi.wrapping_sub(y_ref) as f32 / 2f32.powi(32));
+
                 let p_ref = 1 << 32 + self.dt2;
-                let p_sig = fi as i64 * self.period as i64;
+                let p_sig = fi as u64 * self.period as u64;
                 // relative frequency error
-                f.push(p_sig.wrapping_sub(p_ref) as f32 / 2f32.powi(32));
+                f.push(
+                    p_sig.wrapping_sub(p_ref) as i64 as f32
+                        / 2f32.powi(32 + self.dt2 as i32),
+                );
+
                 // advance time
                 self.time = self.time.wrapping_add(1 << self.dt2);
             }
@@ -157,6 +161,10 @@ mod test {
         }
 
         fn measure(&mut self, n: usize) -> (f32, f32, f32, f32) {
+            assert!(self.period >= 1 << self.dt2);
+            assert!(self.dt2 <= self.shift_frequency);
+            assert!(self.period < 1 << self.shift_frequency);
+            assert!(self.period < 1 << self.shift_frequency + 1);
             let t_settle = (1 << self.shift_frequency - self.dt2 + 4)
                 + (1 << self.shift_phase - self.dt2 + 4);
             self.run(t_settle);
@@ -164,6 +172,7 @@ mod test {
             let (y, f) = self.run(n);
             let y = Array::from(y);
             let f = Array::from(f);
+            // println!("{:?} {:?}", f, y);
 
             let fm = f.mean().unwrap();
             let fs = f.std_axis(Axis(0), 0.).into_scalar();
@@ -180,8 +189,8 @@ mod test {
         let mut h = Harness::default();
 
         let (fm, fs, ym, ys) = h.measure(1 << 16);
-        assert!(fm.abs() < 1e-9);
-        assert!(fs.abs() < 8e-6);
+        assert!(fm.abs() < 1e-11);
+        assert!(fs.abs() < 4e-8);
         assert!(ym.abs() < 2e-8);
         assert!(ys.abs() < 2e-8);
     }
@@ -194,8 +203,8 @@ mod test {
         h.shift_phase = 22;
 
         let (fm, fs, ym, ys) = h.measure(1 << 16);
-        assert!(fm.abs() < 1e-6);
-        assert!(fs.abs() < 6e-4);
+        assert!(fm.abs() < 3e-9);
+        assert!(fs.abs() < 3e-6);
         assert!(ym.abs() < 4e-4);
         assert!(ys.abs() < 2e-4);
     }
@@ -204,31 +213,67 @@ mod test {
     fn narrow_fast() {
         let mut h = Harness::default();
         h.period = 990;
+        h.next = 351;
+        h.next_noisy = h.next;
         h.noise = 5;
         h.shift_frequency = 23;
         h.shift_phase = 22;
 
         let (fm, fs, ym, ys) = h.measure(1 << 16);
-        assert!(fm.abs() < 7e-6);
-        assert!(fs.abs() < 6e-4);
+        assert!(fm.abs() < 2e-9);
+        assert!(fs.abs() < 2e-6);
         assert!(ym.abs() < 1e-3);
         assert!(ys.abs() < 1e-4);
     }
 
-    /*
     #[test]
     fn narrow_slow() {
         let mut h = Harness::default();
         h.period = 1818181;
-        h.noise = 1800;
+        h.next = 35281;
+        h.next_noisy = h.next;
+        h.noise = 1000;
         h.shift_frequency = 23;
         h.shift_phase = 22;
 
         let (fm, fs, ym, ys) = h.measure(1 << 16);
-        assert!(fm.abs() < 1e-8);
+        assert!(fm.abs() < 2e-5);
         assert!(fs.abs() < 6e-4);
         assert!(ym.abs() < 2e-4);
         assert!(ys.abs() < 2e-4);
     }
-    */
+
+    #[test]
+    fn wide_fast() {
+        let mut h = Harness::default();
+        h.period = 990;
+        h.next = 351;
+        h.next_noisy = h.next;
+        h.noise = 5;
+        h.shift_frequency = 10;
+        h.shift_phase = 9;
+
+        let (fm, fs, ym, ys) = h.measure(1 << 16);
+        assert!(fm.abs() < 1e-3);
+        assert!(fs.abs() < 1e-1);
+        assert!(ym.abs() < 1e-4);
+        assert!(ys.abs() < 3e-2);
+    }
+
+    #[test]
+    fn wide_slow() {
+        let mut h = Harness::default();
+        h.period = 1818181;
+        h.next = 35281;
+        h.next_noisy = h.next;
+        h.noise = 1000;
+        h.shift_frequency = 21;
+        h.shift_phase = 20;
+
+        let (fm, fs, ym, ys) = h.measure(1 << 16);
+        assert!(fm.abs() < 2e-4);
+        assert!(fs.abs() < 6e-3);
+        assert!(ym.abs() < 2e-4);
+        assert!(ys.abs() < 2e-3);
+    }
 }
diff --git a/src/bin/lockin.rs b/src/bin/lockin.rs
index f687c21..48c717a 100644
--- a/src/bin/lockin.rs
+++ b/src/bin/lockin.rs
@@ -119,7 +119,7 @@ const APP: () = {
 
         let (pll_phase, pll_frequency) = c.resources.pll.update(
             c.resources.timestamper.latest_timestamp().map(|t| t as i32),
-            22, // relative PLL frequency bandwidth: 2**-22, TODO: expose
+            23, // relative PLL frequency bandwidth: 2**-23, TODO: expose
             22, // relative PLL phase bandwidth: 2**-22, TODO: expose
         );
 
@@ -128,8 +128,8 @@ const APP: () = {
         // Demodulation LO phase offset
         let phase_offset: i32 = 0;
         let sample_frequency = ((pll_frequency >> SAMPLE_BUFFER_SIZE_LOG2)
-            as i32)
-            .wrapping_mul(harmonic); // TODO: maybe rounding bias
+            as i32) // TODO: maybe rounding bias
+            .wrapping_mul(harmonic);
         let sample_phase =
             phase_offset.wrapping_add(pll_phase.wrapping_mul(harmonic));
 

From 80055076b8b6af141543f466b45c9e29c0bf3b1d Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Robert=20J=C3=B6rdens?= <rj@quartiq.de>
Date: Sun, 31 Jan 2021 17:41:20 +0100
Subject: [PATCH 26/71] lockin: scale output

---
 src/bin/lockin.rs | 6 ++----
 1 file changed, 2 insertions(+), 4 deletions(-)

diff --git a/src/bin/lockin.rs b/src/bin/lockin.rs
index 48c717a..f9e2f58 100644
--- a/src/bin/lockin.rs
+++ b/src/bin/lockin.rs
@@ -151,16 +151,14 @@ const APP: () = {
                 phase = iir_ch[1][j].update(&mut iir_state[1][j], phase);
             }
 
-            // TODO: IIR filter DC gain needs to be 1/(1 << 16)
-
             // Note(unsafe): range clipping to i16 is ensured by IIR filters above.
             // Convert to DAC data.
             for i in 0..dac_samples[0].len() {
                 unsafe {
                     dac_samples[0][i] =
-                        power.to_int_unchecked::<i16>() as u16 ^ 0x8000;
+                        (power.to_int_unchecked::<i32>() >> 16) as u16 ^ 0x8000;
                     dac_samples[1][i] =
-                        phase.to_int_unchecked::<i16>() as u16 ^ 0x8000;
+                        (phase.to_int_unchecked::<i32>() >> 16) as u16 ^ 0x8000;
                 }
             }
         }

From d281783f2e60870632174fb9c791736b7ec7e898 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Robert=20J=C3=B6rdens?= <rj@quartiq.de>
Date: Sun, 31 Jan 2021 18:10:13 +0100
Subject: [PATCH 27/71] rpll: reduce code

---
 dsp/src/rpll.rs | 55 +++++++++++++++++++++----------------------------
 1 file changed, 23 insertions(+), 32 deletions(-)

diff --git a/dsp/src/rpll.rs b/dsp/src/rpll.rs
index 4744093..9dee2a3 100644
--- a/dsp/src/rpll.rs
+++ b/dsp/src/rpll.rs
@@ -160,7 +160,7 @@ mod test {
             (y, f)
         }
 
-        fn measure(&mut self, n: usize) -> (f32, f32, f32, f32) {
+        fn measure(&mut self, n: usize, limits: [f32; 4]) {
             assert!(self.period >= 1 << self.dt2);
             assert!(self.dt2 <= self.shift_frequency);
             assert!(self.period < 1 << self.shift_frequency);
@@ -180,7 +180,22 @@ mod test {
             let ys = y.std_axis(Axis(0), 0.).into_scalar();
 
             println!("f: {:.2e}±{:.2e}; y: {:.2e}±{:.2e}", fm, fs, ym, ys);
-            (fm, fs, ym, ys)
+
+            let m = [fm, fs, ym, ys];
+
+            print!("relative: ");
+            for i in 0..m.len() {
+                let rel = m[i].abs() / limits[i].abs();
+                print!("{:.2e} ", rel);
+                assert!(
+                    rel <= 1.,
+                    "idx {}, have |{}| > want {}",
+                    i,
+                    m[i],
+                    limits[i]
+                );
+            }
+            println!("");
         }
     }
 
@@ -188,11 +203,7 @@ mod test {
     fn default() {
         let mut h = Harness::default();
 
-        let (fm, fs, ym, ys) = h.measure(1 << 16);
-        assert!(fm.abs() < 1e-11);
-        assert!(fs.abs() < 4e-8);
-        assert!(ym.abs() < 2e-8);
-        assert!(ys.abs() < 2e-8);
+        h.measure(1 << 16, [1e-11, 4e-8, 2e-8, 2e-8]);
     }
 
     #[test]
@@ -202,11 +213,7 @@ mod test {
         h.shift_frequency = 23;
         h.shift_phase = 22;
 
-        let (fm, fs, ym, ys) = h.measure(1 << 16);
-        assert!(fm.abs() < 3e-9);
-        assert!(fs.abs() < 3e-6);
-        assert!(ym.abs() < 4e-4);
-        assert!(ys.abs() < 2e-4);
+        h.measure(1 << 16, [3e-9, 3e-6, 4e-4, 2e-4]);
     }
 
     #[test]
@@ -219,11 +226,7 @@ mod test {
         h.shift_frequency = 23;
         h.shift_phase = 22;
 
-        let (fm, fs, ym, ys) = h.measure(1 << 16);
-        assert!(fm.abs() < 2e-9);
-        assert!(fs.abs() < 2e-6);
-        assert!(ym.abs() < 1e-3);
-        assert!(ys.abs() < 1e-4);
+        h.measure(1 << 16, [2e-9, 2e-6, 1e-3, 1e-4]);
     }
 
     #[test]
@@ -236,11 +239,7 @@ mod test {
         h.shift_frequency = 23;
         h.shift_phase = 22;
 
-        let (fm, fs, ym, ys) = h.measure(1 << 16);
-        assert!(fm.abs() < 2e-5);
-        assert!(fs.abs() < 6e-4);
-        assert!(ym.abs() < 2e-4);
-        assert!(ys.abs() < 2e-4);
+        h.measure(1 << 16, [2e-5, 6e-4, 2e-4, 2e-4]);
     }
 
     #[test]
@@ -253,11 +252,7 @@ mod test {
         h.shift_frequency = 10;
         h.shift_phase = 9;
 
-        let (fm, fs, ym, ys) = h.measure(1 << 16);
-        assert!(fm.abs() < 1e-3);
-        assert!(fs.abs() < 1e-1);
-        assert!(ym.abs() < 1e-4);
-        assert!(ys.abs() < 3e-2);
+        h.measure(1 << 16, [5e-7, 3e-2, 3e-2, 2e-2]);
     }
 
     #[test]
@@ -270,10 +265,6 @@ mod test {
         h.shift_frequency = 21;
         h.shift_phase = 20;
 
-        let (fm, fs, ym, ys) = h.measure(1 << 16);
-        assert!(fm.abs() < 2e-4);
-        assert!(fs.abs() < 6e-3);
-        assert!(ym.abs() < 2e-4);
-        assert!(ys.abs() < 2e-3);
+        h.measure(1 << 16, [2e-4, 6e-3, 2e-4, 2e-3]);
     }
 }

From 43342cef91eaaca5dbbac0b0e6ec4993af4ac8de Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Robert=20J=C3=B6rdens?= <rj@quartiq.de>
Date: Sun, 31 Jan 2021 18:21:47 +0100
Subject: [PATCH 28/71] rpll: docs

---
 dsp/src/rpll.rs | 6 ++++--
 1 file changed, 4 insertions(+), 2 deletions(-)

diff --git a/dsp/src/rpll.rs b/dsp/src/rpll.rs
index 9dee2a3..0006831 100644
--- a/dsp/src/rpll.rs
+++ b/dsp/src/rpll.rs
@@ -3,6 +3,8 @@
 /// Consumes noisy, quantized timestamps of a reference signal and reconstructs
 /// the phase and frequency of the update() invocations with respect to (and in units of
 /// 1 << 32 of) that reference.
+/// In other words, `update()` rate ralative to reference frequency,
+/// `u32::MAX` corresponding to both being equal.
 #[derive(Copy, Clone, Default)]
 pub struct RPLL {
     dt2: u8, // 1 << dt2 is the counter rate to update() rate ratio
@@ -40,7 +42,7 @@ impl RPLL {
     ///
     /// Returns:
     /// A tuple containing the current phase (wrapping at the i32 boundary, pi) and
-    /// frequency (wrapping at the i32 boundary, Nyquist) estimate.
+    /// frequency.
     pub fn update(
         &mut self,
         input: Option<i32>,
@@ -195,7 +197,7 @@ mod test {
                     limits[i]
                 );
             }
-            println!("");
+            println!();
         }
     }
 

From b6c10dbd8aec57203d50205b820a12d1b2534305 Mon Sep 17 00:00:00 2001
From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com>
Date: Sun, 31 Jan 2021 17:32:07 +0000
Subject: [PATCH 29/71] build(deps): bump criterion from 0.3.3 to 0.3.4

Bumps [criterion](https://github.com/bheisler/criterion.rs) from 0.3.3 to 0.3.4.
- [Release notes](https://github.com/bheisler/criterion.rs/releases)
- [Changelog](https://github.com/bheisler/criterion.rs/blob/master/CHANGELOG.md)
- [Commits](https://github.com/bheisler/criterion.rs/commits)

Signed-off-by: dependabot[bot] <support@github.com>
---
 Cargo.lock | 41 +++++++++++++++++++++++++++++++++--------
 1 file changed, 33 insertions(+), 8 deletions(-)

diff --git a/Cargo.lock b/Cargo.lock
index 4f6bca2..1380c7b 100644
--- a/Cargo.lock
+++ b/Cargo.lock
@@ -239,16 +239,16 @@ dependencies = [
 
 [[package]]
 name = "criterion"
-version = "0.3.3"
+version = "0.3.4"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "70daa7ceec6cf143990669a04c7df13391d55fb27bd4079d252fca774ba244d8"
+checksum = "ab327ed7354547cc2ef43cbe20ef68b988e70b4b593cbd66a2a61733123a3d23"
 dependencies = [
  "atty",
  "cast",
  "clap",
  "criterion-plot",
  "csv",
- "itertools",
+ "itertools 0.10.0",
  "lazy_static",
  "num-traits",
  "oorandom",
@@ -270,7 +270,7 @@ source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "e022feadec601fba1649cfa83586381a4ad31c6bf3a9ab7d408118b05dd9889d"
 dependencies = [
  "cast",
- "itertools",
+ "itertools 0.9.0",
 ]
 
 [[package]]
@@ -510,6 +510,15 @@ dependencies = [
  "either",
 ]
 
+[[package]]
+name = "itertools"
+version = "0.10.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "37d572918e350e82412fe766d24b15e6682fb2ed2bbe018280caa810397cb319"
+dependencies = [
+ "either",
+]
+
 [[package]]
 name = "itoa"
 version = "0.4.6"
@@ -625,7 +634,7 @@ source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "22c95a780960082c5746f6bf0ab22d4a3b8cee72bf580acfe9f1e10bc5ea8152"
 dependencies = [
  "indexmap",
- "itertools",
+ "itertools 0.9.0",
  "ndarray",
  "noisy_float",
  "num-integer",
@@ -710,16 +719,32 @@ checksum = "c5d65c4d95931acda4498f675e332fcbdc9a06705cd07086c510e9b6009cd1c1"
 
 [[package]]
 name = "plotters"
-version = "0.2.15"
+version = "0.3.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "0d1685fbe7beba33de0330629da9d955ac75bd54f33d7b79f9a895590124f6bb"
+checksum = "45ca0ae5f169d0917a7c7f5a9c1a3d3d9598f18f529dd2b8373ed988efea307a"
 dependencies = [
- "js-sys",
  "num-traits",
+ "plotters-backend",
+ "plotters-svg",
  "wasm-bindgen",
  "web-sys",
 ]
 
+[[package]]
+name = "plotters-backend"
+version = "0.3.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "b07fffcddc1cb3a1de753caa4e4df03b79922ba43cf882acc1bdd7e8df9f4590"
+
+[[package]]
+name = "plotters-svg"
+version = "0.3.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "b38a02e23bd9604b842a812063aec4ef702b57989c37b655254bb61c471ad211"
+dependencies = [
+ "plotters-backend",
+]
+
 [[package]]
 name = "ppv-lite86"
 version = "0.2.10"

From d679a9af61c988fdfa2ffcc263166611e53aee0c Mon Sep 17 00:00:00 2001
From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com>
Date: Sun, 31 Jan 2021 17:32:08 +0000
Subject: [PATCH 30/71] build(deps): bump serde from 1.0.120 to 1.0.123

Bumps [serde](https://github.com/serde-rs/serde) from 1.0.120 to 1.0.123.
- [Release notes](https://github.com/serde-rs/serde/releases)
- [Commits](https://github.com/serde-rs/serde/compare/v1.0.120...v1.0.123)

Signed-off-by: dependabot[bot] <support@github.com>
---
 Cargo.lock | 12 ++++++------
 1 file changed, 6 insertions(+), 6 deletions(-)

diff --git a/Cargo.lock b/Cargo.lock
index 4f6bca2..bd4f3db 100644
--- a/Cargo.lock
+++ b/Cargo.lock
@@ -950,9 +950,9 @@ checksum = "388a1df253eca08550bef6c72392cfe7c30914bf41df5269b68cbd6ff8f570a3"
 
 [[package]]
 name = "serde"
-version = "1.0.120"
+version = "1.0.123"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "166b2349061381baf54a58e4b13c89369feb0ef2eaa57198899e2312aac30aab"
+checksum = "92d5161132722baa40d802cc70b15262b98258453e85e5d1d365c757c73869ae"
 dependencies = [
  "serde_derive",
 ]
@@ -979,9 +979,9 @@ dependencies = [
 
 [[package]]
 name = "serde_derive"
-version = "1.0.120"
+version = "1.0.123"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "0ca2a8cb5805ce9e3b95435e3765b7b553cecc762d938d409434338386cb5775"
+checksum = "9391c295d64fc0abb2c556bad848f33cb8296276b1ad2677d1ae1ace4f258f31"
 dependencies = [
  "proc-macro2",
  "quote",
@@ -1074,9 +1074,9 @@ dependencies = [
 
 [[package]]
 name = "syn"
-version = "1.0.58"
+version = "1.0.60"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "cc60a3d73ea6594cd712d830cc1f0390fd71542d8c8cd24e70cc54cdfd5e05d5"
+checksum = "c700597eca8a5a762beb35753ef6b94df201c81cca676604f547495a0d7f0081"
 dependencies = [
  "proc-macro2",
  "quote",

From 8408bc58113508a571e33538502eb8c6f9ed7903 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Robert=20J=C3=B6rdens?= <rj@quartiq.de>
Date: Sun, 31 Jan 2021 18:54:09 +0100
Subject: [PATCH 31/71] dsp/bench: add pll/rpll micro benches

---
 dsp/benches/micro.rs | 51 ++++++++++++++++++++++++++++++++++++++++++++
 dsp/benches/trig.rs  | 28 ------------------------
 2 files changed, 51 insertions(+), 28 deletions(-)
 create mode 100644 dsp/benches/micro.rs
 delete mode 100644 dsp/benches/trig.rs

diff --git a/dsp/benches/micro.rs b/dsp/benches/micro.rs
new file mode 100644
index 0000000..b0f5016
--- /dev/null
+++ b/dsp/benches/micro.rs
@@ -0,0 +1,51 @@
+use core::f32::consts::PI;
+use criterion::{black_box, criterion_group, criterion_main, Criterion};
+use dsp::{atan2, cossin, pll::PLL, rpll::RPLL};
+
+fn atan2_bench(c: &mut Criterion) {
+    let xi = (10 << 16) as i32;
+    let xf = xi as f32 / i32::MAX as f32;
+
+    let yi = (-26_328 << 16) as i32;
+    let yf = yi as f32 / i32::MAX as f32;
+
+    c.bench_function("atan2(y, x)", |b| {
+        b.iter(|| atan2(black_box(yi), black_box(xi)))
+    });
+    c.bench_function("y.atan2(x)", |b| {
+        b.iter(|| black_box(yf).atan2(black_box(xf)))
+    });
+}
+
+fn cossin_bench(c: &mut Criterion) {
+    let zi = -0x7304_2531_i32;
+    let zf = zi as f32 / i32::MAX as f32 * PI;
+    c.bench_function("cossin(zi)", |b| b.iter(|| cossin(black_box(zi))));
+    c.bench_function("zf.sin_cos()", |b| b.iter(|| black_box(zf).sin_cos()));
+}
+
+fn rpll_bench(c: &mut Criterion) {
+    let mut dut = RPLL::new(8);
+    c.bench_function("RPLL::update(Some(t), 21, 20)", |b| {
+        b.iter(|| dut.update(black_box(Some(0x241)), 21, 20))
+    });
+    c.bench_function("RPLL::update(Some(t), sf, sp)", |b| {
+        b.iter(|| {
+            dut.update(black_box(Some(0x241)), black_box(21), black_box(20))
+        })
+    });
+}
+
+fn pll_bench(c: &mut Criterion) {
+    let mut dut = PLL::default();
+    c.bench_function("PLL::update(t, 12, 11)", |b| {
+        b.iter(|| dut.update(black_box(0x1234), 12, 1))
+    });
+    c.bench_function("PLL::update(t, sf, sp)", |b| {
+        b.iter(|| dut.update(black_box(0x241), black_box(21), black_box(20)))
+    });
+}
+
+criterion_group!(trig, atan2_bench, cossin_bench);
+criterion_group!(pll, rpll_bench, pll_bench);
+criterion_main!(trig, pll);
diff --git a/dsp/benches/trig.rs b/dsp/benches/trig.rs
deleted file mode 100644
index df17a65..0000000
--- a/dsp/benches/trig.rs
+++ /dev/null
@@ -1,28 +0,0 @@
-use core::f32::consts::PI;
-use criterion::{black_box, criterion_group, criterion_main, Criterion};
-use dsp::{atan2, cossin};
-
-fn atan2_bench(c: &mut Criterion) {
-    let xi = (10 << 16) as i32;
-    let xf = xi as f32 / i32::MAX as f32;
-
-    let yi = (-26_328 << 16) as i32;
-    let yf = yi as f32 / i32::MAX as f32;
-
-    c.bench_function("atan2(y, x)", |b| {
-        b.iter(|| atan2(black_box(yi), black_box(xi)))
-    });
-    c.bench_function("y.atan2(x)", |b| {
-        b.iter(|| black_box(yf).atan2(black_box(xf)))
-    });
-}
-
-fn cossin_bench(c: &mut Criterion) {
-    let zi = -0x7304_2531_i32;
-    let zf = zi as f32 / i32::MAX as f32 * PI;
-    c.bench_function("cossin(zi)", |b| b.iter(|| cossin(black_box(zi))));
-    c.bench_function("zf.sin_cos()", |b| b.iter(|| black_box(zf).sin_cos()));
-}
-
-criterion_group!(benches, atan2_bench, cossin_bench);
-criterion_main!(benches);

From b8fcf416cb3016ea99456e0fe14f7352835ac0bb Mon Sep 17 00:00:00 2001
From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com>
Date: Sun, 31 Jan 2021 18:00:41 +0000
Subject: [PATCH 32/71] build(deps): bump cortex-m-log from 0.6.2 to 0.7.0

Bumps [cortex-m-log](https://github.com/DoumanAsh/cortex-m-log) from 0.6.2 to 0.7.0.
- [Release notes](https://github.com/DoumanAsh/cortex-m-log/releases)
- [Commits](https://github.com/DoumanAsh/cortex-m-log/commits)

Signed-off-by: dependabot[bot] <support@github.com>
---
 Cargo.lock | 78 ++++++++++++++++++++++++++++++++++++++++--------------
 Cargo.toml |  2 +-
 2 files changed, 59 insertions(+), 21 deletions(-)

diff --git a/Cargo.lock b/Cargo.lock
index 4f6bca2..37fa4f1 100644
--- a/Cargo.lock
+++ b/Cargo.lock
@@ -36,7 +36,7 @@ source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "b9a69a963b70ddacfcd382524f72a4576f359af9334b3bf48a79566590bb8bfa"
 dependencies = [
  "bitrate",
- "cortex-m",
+ "cortex-m 0.7.1",
  "embedded-hal",
 ]
 
@@ -160,23 +160,36 @@ checksum = "cd51eab21ab4fd6a3bf889e2d0958c0a6e3a61ad04260325e919e652a2a62826"
 
 [[package]]
 name = "cortex-m"
-version = "0.6.4"
+version = "0.6.7"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "88cdafeafba636c00c467ded7f1587210725a1adfab0c24028a7844b87738263"
+checksum = "9075300b07c6a56263b9b582c214d0ff037b00d45ec9fde1cc711490c56f1bb9"
 dependencies = [
  "aligned",
  "bare-metal 0.2.5",
  "bitfield",
+ "cortex-m 0.7.1",
+ "volatile-register",
+]
+
+[[package]]
+name = "cortex-m"
+version = "0.7.1"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "a0b756a8bffc56025de45218a48ff9b801180440c0ee49a722b32d49dcebc771"
+dependencies = [
+ "bare-metal 0.2.5",
+ "bitfield",
+ "embedded-hal",
  "volatile-register",
 ]
 
 [[package]]
 name = "cortex-m-log"
-version = "0.6.2"
+version = "0.7.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "1d63959cb1e003dd97233fee6762351540253237eadf06fcdcb98cbfa3f9be4a"
+checksum = "0e202d2eac4e34adf7524a563e36623bae6f69cc0a73ef9bd22a4c93a5a806fa"
 dependencies = [
- "cortex-m",
+ "cortex-m 0.7.1",
  "cortex-m-semihosting",
  "log",
 ]
@@ -208,7 +221,7 @@ version = "0.5.5"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "b30efcb6b7920d9016182c485687f0012487032a14c415d2fce6e9862ef8260e"
 dependencies = [
- "cortex-m",
+ "cortex-m 0.6.7",
  "cortex-m-rt",
  "cortex-m-rtic-macros",
  "heapless",
@@ -234,21 +247,21 @@ version = "0.3.7"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "6bffa6c1454368a6aa4811ae60964c38e6996d397ff8095a8b9211b1c1f749bc"
 dependencies = [
- "cortex-m",
+ "cortex-m 0.7.1",
 ]
 
 [[package]]
 name = "criterion"
-version = "0.3.3"
+version = "0.3.4"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "70daa7ceec6cf143990669a04c7df13391d55fb27bd4079d252fca774ba244d8"
+checksum = "ab327ed7354547cc2ef43cbe20ef68b988e70b4b593cbd66a2a61733123a3d23"
 dependencies = [
  "atty",
  "cast",
  "clap",
  "criterion-plot",
  "csv",
- "itertools",
+ "itertools 0.10.0",
  "lazy_static",
  "num-traits",
  "oorandom",
@@ -270,7 +283,7 @@ source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "e022feadec601fba1649cfa83586381a4ad31c6bf3a9ab7d408118b05dd9889d"
 dependencies = [
  "cast",
- "itertools",
+ "itertools 0.9.0",
 ]
 
 [[package]]
@@ -510,6 +523,15 @@ dependencies = [
  "either",
 ]
 
+[[package]]
+name = "itertools"
+version = "0.10.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "37d572918e350e82412fe766d24b15e6682fb2ed2bbe018280caa810397cb319"
+dependencies = [
+ "either",
+]
+
 [[package]]
 name = "itoa"
 version = "0.4.6"
@@ -625,7 +647,7 @@ source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "22c95a780960082c5746f6bf0ab22d4a3b8cee72bf580acfe9f1e10bc5ea8152"
 dependencies = [
  "indexmap",
- "itertools",
+ "itertools 0.9.0",
  "ndarray",
  "noisy_float",
  "num-integer",
@@ -698,7 +720,7 @@ version = "0.5.6"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "c3d55dedd501dfd02514646e0af4d7016ce36bc12ae177ef52056989966a1eec"
 dependencies = [
- "cortex-m",
+ "cortex-m 0.7.1",
  "cortex-m-semihosting",
 ]
 
@@ -710,16 +732,32 @@ checksum = "c5d65c4d95931acda4498f675e332fcbdc9a06705cd07086c510e9b6009cd1c1"
 
 [[package]]
 name = "plotters"
-version = "0.2.15"
+version = "0.3.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "0d1685fbe7beba33de0330629da9d955ac75bd54f33d7b79f9a895590124f6bb"
+checksum = "45ca0ae5f169d0917a7c7f5a9c1a3d3d9598f18f529dd2b8373ed988efea307a"
 dependencies = [
- "js-sys",
  "num-traits",
+ "plotters-backend",
+ "plotters-svg",
  "wasm-bindgen",
  "web-sys",
 ]
 
+[[package]]
+name = "plotters-backend"
+version = "0.3.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "b07fffcddc1cb3a1de753caa4e4df03b79922ba43cf882acc1bdd7e8df9f4590"
+
+[[package]]
+name = "plotters-svg"
+version = "0.3.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "b38a02e23bd9604b842a812063aec4ef702b57989c37b655254bb61c471ad211"
+dependencies = [
+ "plotters-backend",
+]
+
 [[package]]
 name = "ppv-lite86"
 version = "0.2.10"
@@ -1016,7 +1054,7 @@ version = "0.4.1"
 dependencies = [
  "ad9959",
  "asm-delay",
- "cortex-m",
+ "cortex-m 0.6.7",
  "cortex-m-log",
  "cortex-m-rt",
  "cortex-m-rtic",
@@ -1049,7 +1087,7 @@ source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "7571f17d1ed7d67957d0004de6c52bd1ef5e736ed5ddc2bcecf001512269f77c"
 dependencies = [
  "bare-metal 0.2.5",
- "cortex-m",
+ "cortex-m 0.6.7",
  "cortex-m-rt",
  "vcell",
 ]
@@ -1061,7 +1099,7 @@ source = "git+https://github.com/stm32-rs/stm32h7xx-hal?branch=dma#3da22d4935c8f
 dependencies = [
  "bare-metal 1.0.0",
  "cast",
- "cortex-m",
+ "cortex-m 0.6.7",
  "cortex-m-rt",
  "embedded-dma",
  "embedded-hal",
diff --git a/Cargo.toml b/Cargo.toml
index f4f90fa..ec85a3c 100644
--- a/Cargo.toml
+++ b/Cargo.toml
@@ -30,7 +30,7 @@ members = ["ad9959", "dsp"]
 [dependencies]
 cortex-m = { version = "0.6", features = ["const-fn"] }
 cortex-m-rt = { version = "0.6", features = ["device"] }
-cortex-m-log = { version = "0.6", features = ["log-integration"] }
+cortex-m-log = { version = "0.7", features = ["log-integration"] }
 log = "0.4"
 panic-semihosting = { version = "0.5", optional = true }
 panic-halt = "0.2"

From 47089c267c368c597d18f1f1244066b2a1f3eda8 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Robert=20J=C3=B6rdens?= <rj@quartiq.de>
Date: Sun, 31 Jan 2021 19:12:24 +0100
Subject: [PATCH 33/71] dsp: align iir and iir_int, add iir micro benches

---
 dsp/Cargo.toml       |  2 +-
 dsp/benches/micro.rs | 23 +++++++++++++++++++++--
 dsp/src/iir.rs       | 21 +++++++++++----------
 src/bin/dual-iir.rs  | 20 ++++++++++----------
 src/bin/lockin.rs    | 20 ++++++++++----------
 5 files changed, 53 insertions(+), 33 deletions(-)

diff --git a/dsp/Cargo.toml b/dsp/Cargo.toml
index 16fd500..7e30348 100644
--- a/dsp/Cargo.toml
+++ b/dsp/Cargo.toml
@@ -15,7 +15,7 @@ ndarray = "0.14"
 ndarray-stats = "0.4"
 
 [[bench]]
-name = "trig"
+name = "micro"
 harness = false
 
 [features]
diff --git a/dsp/benches/micro.rs b/dsp/benches/micro.rs
index b0f5016..5a42640 100644
--- a/dsp/benches/micro.rs
+++ b/dsp/benches/micro.rs
@@ -1,6 +1,8 @@
 use core::f32::consts::PI;
 use criterion::{black_box, criterion_group, criterion_main, Criterion};
-use dsp::{atan2, cossin, pll::PLL, rpll::RPLL};
+use dsp::{atan2, cossin};
+use dsp::{iir, iir_int};
+use dsp::{pll::PLL, rpll::RPLL};
 
 fn atan2_bench(c: &mut Criterion) {
     let xi = (10 << 16) as i32;
@@ -46,6 +48,23 @@ fn pll_bench(c: &mut Criterion) {
     });
 }
 
+fn iir_int_bench(c: &mut Criterion) {
+    let dut = iir_int::IIR::default();
+    let mut xy = iir_int::IIRState::default();
+    c.bench_function("int_iir::IIR::update(s, x)", |b| {
+        b.iter(|| dut.update(&mut xy, black_box(0x2832)))
+    });
+}
+
+fn iir_bench(c: &mut Criterion) {
+    let dut = iir::IIR::default();
+    let mut xy = iir::IIRState::default();
+    c.bench_function("int::IIR::update(s, x)", |b| {
+        b.iter(|| dut.update(&mut xy, black_box(0.32241)))
+    });
+}
+
 criterion_group!(trig, atan2_bench, cossin_bench);
 criterion_group!(pll, rpll_bench, pll_bench);
-criterion_main!(trig, pll);
+criterion_group!(iir, iir_int_bench, iir_bench);
+criterion_main!(trig, pll, iir);
diff --git a/dsp/src/iir.rs b/dsp/src/iir.rs
index dbec8d8..50dd919 100644
--- a/dsp/src/iir.rs
+++ b/dsp/src/iir.rs
@@ -11,7 +11,8 @@ use core::f32;
 /// To represent the IIR coefficients, this contains the feed-forward
 /// coefficients (b0, b1, b2) followd by the negated feed-back coefficients
 /// (-a1, -a2), all five normalized such that a0 = 1.
-pub type IIRState = [f32; 5];
+#[derive(Copy, Clone, Default, Deserialize, Serialize)]
+pub struct IIRState(pub [f32; 5]);
 
 /// IIR configuration.
 ///
@@ -38,7 +39,7 @@ pub type IIRState = [f32; 5];
 ///   Therefore it can trivially implement bump-less transfer.
 /// * Cascading multiple IIR filters allows stable and robust
 ///   implementation of transfer functions beyond bequadratic terms.
-#[derive(Copy, Clone, Deserialize, Serialize)]
+#[derive(Copy, Clone, Default, Deserialize, Serialize)]
 pub struct IIR {
     pub ba: IIRState,
     pub y_offset: f32,
@@ -74,13 +75,13 @@ impl IIR {
             }
             (a1, b0, b1)
         };
-        self.ba.copy_from_slice(&[b0, b1, 0., a1, 0.]);
+        self.ba.0.copy_from_slice(&[b0, b1, 0., a1, 0.]);
         Ok(())
     }
 
     /// Compute the overall (DC feed-forward) gain.
     pub fn get_k(&self) -> f32 {
-        self.ba[..3].iter().sum()
+        self.ba.0[..3].iter().sum()
     }
 
     /// Compute input-referred (`x`) offset from output (`y`) offset.
@@ -108,21 +109,21 @@ impl IIR {
     /// * `xy` - Current filter state.
     /// * `x0` - New input.
     pub fn update(&self, xy: &mut IIRState, x0: f32) -> f32 {
-        let n = self.ba.len();
-        debug_assert!(xy.len() == n);
+        let n = self.ba.0.len();
+        debug_assert!(xy.0.len() == n);
         // `xy` contains       x0 x1 y0 y1 y2
         // Increment time      x1 x2 y1 y2 y3
         // Shift               x1 x1 x2 y1 y2
         // This unrolls better than xy.rotate_right(1)
-        xy.copy_within(0..n - 1, 1);
+        xy.0.copy_within(0..n - 1, 1);
         // Store x0            x0 x1 x2 y1 y2
-        xy[0] = x0;
+        xy.0[0] = x0;
         // Compute y0 by multiply-accumulate
-        let y0 = macc(self.y_offset, xy, &self.ba);
+        let y0 = macc(self.y_offset, &xy.0, &self.ba.0);
         // Limit y0
         let y0 = max(self.y_min, min(self.y_max, y0));
         // Store y0            x0 x1 y0 y1 y2
-        xy[n / 2] = y0;
+        xy.0[n / 2] = y0;
         y0
     }
 }
diff --git a/src/bin/dual-iir.rs b/src/bin/dual-iir.rs
index fd8f5e4..1757d7c 100644
--- a/src/bin/dual-iir.rs
+++ b/src/bin/dual-iir.rs
@@ -34,9 +34,9 @@ const APP: () = {
         net_interface: hardware::Ethernet,
 
         // Format: iir_state[ch][cascade-no][coeff]
-        #[init([[[0.; 5]; IIR_CASCADE_LENGTH]; 2])]
+        #[init([[iir::IIRState([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])]
+        #[init([[iir::IIR { ba: iir::IIRState([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],
     }
 
@@ -159,10 +159,10 @@ const APP: () = {
                                     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],
+                                            x0: iir_state[0][0].0[0],
+                                            y0: iir_state[0][0].0[2],
+                                            x1: iir_state[1][0].0[0],
+                                            y1: iir_state[1][0].0[2],
                                     });
 
                                     Ok::<server::Status, ()>(state)
@@ -171,10 +171,10 @@ const APP: () = {
                                 "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],
+                                            x0: iir_state[0][IIR_CASCADE_LENGTH-1].0[0],
+                                            y0: iir_state[0][IIR_CASCADE_LENGTH-1].0[2],
+                                            x1: iir_state[1][IIR_CASCADE_LENGTH-1].0[0],
+                                            y1: iir_state[1][IIR_CASCADE_LENGTH-1].0[2],
                                     });
 
                                     Ok::<server::Status, ()>(state)
diff --git a/src/bin/lockin.rs b/src/bin/lockin.rs
index f9e2f58..95c79eb 100644
--- a/src/bin/lockin.rs
+++ b/src/bin/lockin.rs
@@ -38,9 +38,9 @@ const APP: () = {
         net_interface: hardware::Ethernet,
 
         // Format: iir_state[ch][cascade-no][coeff]
-        #[init([[[0.; 5]; IIR_CASCADE_LENGTH]; 2])]
+        #[init([[iir::IIRState([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])]
+        #[init([[iir::IIR { ba: iir::IIRState([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],
 
         timestamper: InputStamper,
@@ -216,10 +216,10 @@ const APP: () = {
                                     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],
+                                            x0: iir_state[0][0].0[0],
+                                            y0: iir_state[0][0].0[2],
+                                            x1: iir_state[1][0].0[0],
+                                            y1: iir_state[1][0].0[2],
                                     });
 
                                     Ok::<server::Status, ()>(state)
@@ -228,10 +228,10 @@ const APP: () = {
                                 "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],
+                                            x0: iir_state[0][IIR_CASCADE_LENGTH-1].0[0],
+                                            y0: iir_state[0][IIR_CASCADE_LENGTH-1].0[2],
+                                            x1: iir_state[1][IIR_CASCADE_LENGTH-1].0[0],
+                                            y1: iir_state[1][IIR_CASCADE_LENGTH-1].0[2],
                                     });
 
                                     Ok::<server::Status, ()>(state)

From 46a7d67027ab2827dfcdc90705b3fbf4d2360b20 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Robert=20J=C3=B6rdens?= <rj@quartiq.de>
Date: Sun, 31 Jan 2021 19:26:07 +0100
Subject: [PATCH 34/71] lockin-internal: rename, adapt

---
 src/bin/{lockin.rs => lockin-external.rs}     |  0
 ...in-internal-demo.rs => lockin-internal.rs} | 40 +++++++++----------
 2 files changed, 18 insertions(+), 22 deletions(-)
 rename src/bin/{lockin.rs => lockin-external.rs} (100%)
 rename src/bin/{lockin-internal-demo.rs => lockin-internal.rs} (81%)

diff --git a/src/bin/lockin.rs b/src/bin/lockin-external.rs
similarity index 100%
rename from src/bin/lockin.rs
rename to src/bin/lockin-external.rs
diff --git a/src/bin/lockin-internal-demo.rs b/src/bin/lockin-internal.rs
similarity index 81%
rename from src/bin/lockin-internal-demo.rs
rename to src/bin/lockin-internal.rs
index 54a429f..8c41cfe 100644
--- a/src/bin/lockin-internal-demo.rs
+++ b/src/bin/lockin-internal.rs
@@ -74,8 +74,8 @@ const APP: () = {
 
         // DAC0 always generates a fixed sinusoidal output.
         for (i, value) in DAC_SEQUENCE.iter().enumerate() {
-            // Full-scale gives a +/- 12V amplitude waveform. Scale it down to give +/- 100mV.
-            let y = value * i16::MAX as f32 / 120.0;
+            // Full-scale gives a +/- 10V amplitude waveform. Scale it down to give +/- 1V.
+            let y = value * (0.1 * i16::MAX as f32);
             // Note(unsafe): The DAC_SEQUENCE values are guaranteed to be normalized.
             let y = unsafe { y.to_int_unchecked::<i16>() };
 
@@ -84,7 +84,8 @@ const APP: () = {
         }
 
         let pll_phase = 0;
-        let pll_frequency = 1i32 << (32 - 3); // 1/8 of the sample rate
+        // 1/8 of the sample rate: log2(DAC_SEQUENCE.len()) == 3
+        let pll_frequency = 1i32 << (32 - 3);
 
         // Harmonic index of the LO: -1 to _de_modulate the fundamental
         let harmonic: i32 = -1;
@@ -92,28 +93,23 @@ const APP: () = {
         // Demodulation LO phase offset
         let phase_offset: i32 = (0.7495 * i32::MAX as f32) as i32;
         let sample_frequency = (pll_frequency as i32).wrapping_mul(harmonic);
-        let mut sample_phase = phase_offset
+        let sample_phase = phase_offset
             .wrapping_add((pll_phase as i32).wrapping_mul(harmonic));
 
-        let mut phase = 0i16;
+        if let Some(output) = c.resources.lockin.feed(
+            adc_samples.iter().map(|&i|
+                // Convert to signed, MSB align the ADC sample.
+                (i as i16 as i32) << 16),
+            sample_phase,
+            sample_frequency,
+        ) {
+            // Convert from IQ to power and phase.
+            let _power = output.abs_sqr();
+            let phase = output.arg() >> 16;
 
-        for sample in adc_samples.iter() {
-            // Convert to signed, MSB align the ADC sample.
-            let input = (*sample as i16 as i32) << 16;
-
-            // Obtain demodulated, filtered IQ sample.
-            let output = c.resources.lockin.update(input, sample_phase);
-
-            // Advance the sample phase.
-            sample_phase = sample_phase.wrapping_add(sample_frequency);
-
-            // Convert from IQ to phase. Scale the phase so that it fits in the DAC range. We do
-            // this by shifting it down into the 16-bit range.
-            phase = (output.phase() >> 16) as i16;
-        }
-
-        for value in dac_samples[1].iter_mut() {
-            *value = phase as u16 ^ 0x8000
+            for value in dac_samples[1].iter_mut() {
+                *value = phase as u16 ^ 0x8000;
+            }
         }
     }
 

From 0a25abf9e70e3a91ec71d349e6d8002d8da15243 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Robert=20J=C3=B6rdens?= <rj@quartiq.de>
Date: Sun, 31 Jan 2021 19:31:09 +0100
Subject: [PATCH 35/71] ci: smaller matrix, fix/add lockins

---
 .github/workflows/ci.yml | 12 +++++++++---
 1 file changed, 9 insertions(+), 3 deletions(-)

diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml
index 1314418..6fa0e99 100644
--- a/.github/workflows/ci.yml
+++ b/.github/workflows/ci.yml
@@ -41,13 +41,19 @@ jobs:
       matrix:
         toolchain:
           - stable
-          - beta
         bin:
           - dual-iir
-          - lockin
+          - lockin-internal
+          - lockin-external
         features:
           - ''
-          - pounder_v1_1
+        include:
+          - toolchain: beta
+            bin: dual-iir
+            features: ''
+          - toolchain: stable
+            bin: lockin-internal
+            features: pounder_v1_1
     steps:
       - uses: actions/checkout@v2
       - name: Install Rust ${{ matrix.toolchain }}

From 2d43b8970b81f9655884e14d44747d43f3ef5cc5 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Robert=20J=C3=B6rdens?= <rj@quartiq.de>
Date: Sun, 31 Jan 2021 20:32:44 +0100
Subject: [PATCH 36/71] lockin: cleanup

---
 Cargo.lock        |  99 +++------------------------------
 dsp/Cargo.toml    |   1 -
 dsp/src/lockin.rs | 138 ++++------------------------------------------
 dsp/src/rpll.rs   |   3 +-
 4 files changed, 20 insertions(+), 221 deletions(-)

diff --git a/Cargo.lock b/Cargo.lock
index 1380c7b..d1e00e0 100644
--- a/Cargo.lock
+++ b/Cargo.lock
@@ -348,8 +348,7 @@ dependencies = [
  "criterion",
  "libm",
  "ndarray",
- "ndarray-stats",
- "rand 0.8.3",
+ "rand",
  "serde",
 ]
 
@@ -426,17 +425,6 @@ dependencies = [
  "version_check",
 ]
 
-[[package]]
-name = "getrandom"
-version = "0.1.16"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "8fc3cb4d91f53b50155bdcfd23f6a4c39ae1969c2ae85982b135750cccaf5fce"
-dependencies = [
- "cfg-if 1.0.0",
- "libc",
- "wasi 0.9.0+wasi-snapshot-preview1",
-]
-
 [[package]]
 name = "getrandom"
 version = "0.2.2"
@@ -445,7 +433,7 @@ checksum = "c9495705279e7140bf035dde1f6e750c162df8b625267cd52cc44e0b156732c8"
 dependencies = [
  "cfg-if 1.0.0",
  "libc",
- "wasi 0.10.2+wasi-snapshot-preview1",
+ "wasi",
 ]
 
 [[package]]
@@ -627,30 +615,6 @@ dependencies = [
  "rawpointer",
 ]
 
-[[package]]
-name = "ndarray-stats"
-version = "0.4.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "22c95a780960082c5746f6bf0ab22d4a3b8cee72bf580acfe9f1e10bc5ea8152"
-dependencies = [
- "indexmap",
- "itertools 0.9.0",
- "ndarray",
- "noisy_float",
- "num-integer",
- "num-traits",
- "rand 0.7.3",
-]
-
-[[package]]
-name = "noisy_float"
-version = "0.1.13"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "a14c16cde392a1cd18084ffd8348cb8937525130e62f0478d72dcc683698809d"
-dependencies = [
- "num-traits",
-]
-
 [[package]]
 name = "num-complex"
 version = "0.3.1"
@@ -775,19 +739,6 @@ version = "0.2.2"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "e2a38df5b15c8d5c7e8654189744d8e396bddc18ad48041a500ce52d6948941f"
 
-[[package]]
-name = "rand"
-version = "0.7.3"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "6a6b1679d49b24bbfe0c803429aa1874472f50d9b363131f0e89fc356b544d03"
-dependencies = [
- "getrandom 0.1.16",
- "libc",
- "rand_chacha 0.2.2",
- "rand_core 0.5.1",
- "rand_hc 0.2.0",
-]
-
 [[package]]
 name = "rand"
 version = "0.8.3"
@@ -795,19 +746,9 @@ source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "0ef9e7e66b4468674bfcb0c81af8b7fa0bb154fa9f28eb840da5c447baeb8d7e"
 dependencies = [
  "libc",
- "rand_chacha 0.3.0",
- "rand_core 0.6.1",
- "rand_hc 0.3.0",
-]
-
-[[package]]
-name = "rand_chacha"
-version = "0.2.2"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "f4c8ed856279c9737206bf725bf36935d8666ead7aa69b52be55af369d193402"
-dependencies = [
- "ppv-lite86",
- "rand_core 0.5.1",
+ "rand_chacha",
+ "rand_core",
+ "rand_hc",
 ]
 
 [[package]]
@@ -817,16 +758,7 @@ source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "e12735cf05c9e10bf21534da50a147b924d555dc7a547c42e6bb2d5b6017ae0d"
 dependencies = [
  "ppv-lite86",
- "rand_core 0.6.1",
-]
-
-[[package]]
-name = "rand_core"
-version = "0.5.1"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "90bde5296fc891b0cef12a6d03ddccc162ce7b2aff54160af9338f8d40df6d19"
-dependencies = [
- "getrandom 0.1.16",
+ "rand_core",
 ]
 
 [[package]]
@@ -835,16 +767,7 @@ version = "0.6.1"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "c026d7df8b298d90ccbbc5190bd04d85e159eaf5576caeacf8741da93ccbd2e5"
 dependencies = [
- "getrandom 0.2.2",
-]
-
-[[package]]
-name = "rand_hc"
-version = "0.2.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "ca3129af7b92a17112d59ad498c6f81eaf463253766b90396d39ea7a39d6613c"
-dependencies = [
- "rand_core 0.5.1",
+ "getrandom",
 ]
 
 [[package]]
@@ -853,7 +776,7 @@ version = "0.3.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "3190ef7066a446f2e7f42e239d161e905420ccab01eb967c9eb27d21b2322a73"
 dependencies = [
- "rand_core 0.6.1",
+ "rand_core",
 ]
 
 [[package]]
@@ -1183,12 +1106,6 @@ dependencies = [
  "winapi-util",
 ]
 
-[[package]]
-name = "wasi"
-version = "0.9.0+wasi-snapshot-preview1"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "cccddf32554fecc6acb585f82a32a72e28b48f8c4c1883ddfeeeaa96f7d8e519"
-
 [[package]]
 name = "wasi"
 version = "0.10.2+wasi-snapshot-preview1"
diff --git a/dsp/Cargo.toml b/dsp/Cargo.toml
index 7e30348..de96d81 100644
--- a/dsp/Cargo.toml
+++ b/dsp/Cargo.toml
@@ -12,7 +12,6 @@ serde = { version = "1.0", features = ["derive"], default-features = false }
 criterion = "0.3"
 rand = "0.8"
 ndarray = "0.14"
-ndarray-stats = "0.4"
 
 [[bench]]
 name = "micro"
diff --git a/dsp/src/lockin.rs b/dsp/src/lockin.rs
index b75038e..0f6b78f 100644
--- a/dsp/src/lockin.rs
+++ b/dsp/src/lockin.rs
@@ -8,6 +8,7 @@ pub struct Lockin {
 }
 
 impl Lockin {
+    /// Create a new Lockin with given IIR coefficients.
     pub fn new(ba: &iir_int::IIRState) -> Self {
         let mut iir = iir_int::IIR::default();
         iir.ba.0.copy_from_slice(&ba.0);
@@ -17,9 +18,10 @@ impl Lockin {
         }
     }
 
-    pub fn update(&mut self, signal: i32, phase: i32) -> Complex<i32> {
+    /// Update the lockin with a sample taken at a given phase.
+    pub fn update(&mut self, sample: i32, phase: i32) -> Complex<i32> {
         // Get the LO signal for demodulation.
-        let m = Complex::from_angle(phase);
+        let lo = Complex::from_angle(phase);
 
         // Mix with the LO signal, filter with the IIR lowpass,
         // return IQ (in-phase and quadrature) data.
@@ -27,15 +29,18 @@ impl Lockin {
         Complex(
             self.iir.update(
                 &mut self.iir_state[0],
-                ((signal as i64 * m.0 as i64) >> 32) as _,
+                ((sample as i64 * lo.0 as i64) >> 32) as _,
             ),
             self.iir.update(
                 &mut self.iir_state[1],
-                ((signal as i64 * m.1 as i64) >> 32) as _,
+                ((sample as i64 * lo.1 as i64) >> 32) as _,
             ),
         )
     }
 
+    /// Feed an iterator into the Lockin and return the latest I/Q data.
+    /// Initial stample phase and frequency (phase increment between samples)
+    /// are supplied.
     pub fn feed<I: IntoIterator<Item = i32>>(
         &mut self,
         signal: I,
@@ -46,131 +51,10 @@ impl Lockin {
 
         signal
             .into_iter()
-            .map(|s| {
+            .map(|sample| {
                 phase = phase.wrapping_add(frequency);
-                self.update(s, phase)
+                self.update(sample, phase)
             })
             .last()
     }
 }
-
-#[cfg(test)]
-mod test {
-    use crate::{
-        iir_int::IIRState,
-        lockin::Lockin,
-        rpll::RPLL,
-        testing::{isclose, max_error},
-        Complex,
-    };
-
-    use std::f64::consts::PI;
-    use std::vec::Vec;
-
-    /// ADC full scale in machine units (16 bit signed).
-    const ADC_SCALE: f64 = ((1 << 15) - 1) as _;
-
-    struct PllLockin {
-        harmonic: i32,
-        phase: i32,
-        lockin: Lockin,
-    }
-
-    impl PllLockin {
-        pub fn new(harmonic: i32, phase: i32, iir: &IIRState) -> Self {
-            PllLockin {
-                harmonic,
-                phase,
-                lockin: Lockin::new(iir),
-            }
-        }
-
-        pub fn update(
-            &mut self,
-            input: Vec<i16>,
-            phase: i32,
-            frequency: i32,
-        ) -> Complex<i32> {
-            let sample_frequency = frequency.wrapping_mul(self.harmonic);
-            let mut sample_phase =
-                self.phase.wrapping_add(phase.wrapping_mul(self.harmonic));
-            input
-                .iter()
-                .map(|&s| {
-                    let input = (s as i32) << 16;
-                    let signal =
-                        self.lockin.update(input, sample_phase.wrapping_neg());
-                    sample_phase = sample_phase.wrapping_add(sample_frequency);
-                    signal
-                })
-                .last()
-                .unwrap_or(Complex::default())
-        }
-    }
-
-    /// Single-frequency sinusoid.
-    #[derive(Copy, Clone)]
-    struct Tone {
-        // Frequency (in Hz).
-        frequency: f64,
-        // Phase offset (in radians).
-        phase: f64,
-        // Amplitude in dBFS (decibels relative to full-scale).
-        // A 16-bit ADC has a minimum dBFS for each sample of -90.
-        amplitude_dbfs: f64,
-    }
-
-    /// Convert dBFS to a linear ratio.
-    fn linear(dbfs: f64) -> f64 {
-        10f64.powf(dbfs / 20.)
-    }
-
-    impl Tone {
-        fn eval(&self, time: f64) -> f64 {
-            linear(self.amplitude_dbfs)
-                * (self.phase + self.frequency * time).cos()
-        }
-    }
-
-    /// Generate a full batch of samples with size `sample_buffer_size` starting at `time_offset`.
-    fn sample_tones(
-        tones: &Vec<Tone>,
-        time_offset: f64,
-        sample_buffer_size: u32,
-    ) -> Vec<i16> {
-        (0..sample_buffer_size)
-            .map(|i| {
-                let time = 2. * PI * (time_offset + i as f64);
-                let x: f64 = tones.iter().map(|t| t.eval(time)).sum();
-                assert!(-1. < x && x < 1.);
-                (x * ADC_SCALE) as i16
-            })
-            .collect()
-    }
-
-    /// Total maximum noise amplitude of the input signal after 2nd order lowpass filter.
-    /// Constructive interference is assumed.
-    ///
-    /// # Args
-    /// * `tones` - Noise sources at the ADC input.
-    /// * `frequency` - Frequency of the signal of interest.
-    /// * `corner` - Low-pass filter 3dB corner cutoff frequency.
-    ///
-    /// # Returns
-    /// Upper bound of the total amplitude of all noise sources in linear units full scale.
-    fn sampled_noise_amplitude(
-        tones: &Vec<Tone>,
-        frequency: f64,
-        corner: f64,
-    ) -> f64 {
-        tones
-            .iter()
-            .map(|t| {
-                let df = (t.frequency - frequency) / corner;
-                // Assuming a 2nd order lowpass filter: 40dB/decade.
-                linear(t.amplitude_dbfs - 40. * df.abs().max(1.).log10())
-            })
-            .sum::<f64>()
-            .max(1. / ADC_SCALE / 2.) // 1/2 LSB from quantization
-    }
-}
diff --git a/dsp/src/rpll.rs b/dsp/src/rpll.rs
index 0006831..9759cf9 100644
--- a/dsp/src/rpll.rs
+++ b/dsp/src/rpll.rs
@@ -84,7 +84,6 @@ impl RPLL {
 mod test {
     use super::RPLL;
     use ndarray::prelude::*;
-    use ndarray_stats::QuantileExt;
     use rand::{prelude::*, rngs::StdRng};
     use std::vec::Vec;
 
@@ -254,7 +253,7 @@ mod test {
         h.shift_frequency = 10;
         h.shift_phase = 9;
 
-        h.measure(1 << 16, [5e-7, 3e-2, 3e-2, 2e-2]);
+        h.measure(1 << 16, [5e-7, 3e-2, 2e-5, 2e-2]);
     }
 
     #[test]

From 8317185e07144f22a0e2ce0e5c38a8fb2360d342 Mon Sep 17 00:00:00 2001
From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com>
Date: Sun, 31 Jan 2021 22:28:48 +0000
Subject: [PATCH 37/71] build(deps): bump stm32h7xx-hal from `3da22d4` to
 `2b8a04c`

Bumps [stm32h7xx-hal](https://github.com/stm32-rs/stm32h7xx-hal) from `3da22d4` to `2b8a04c`.
- [Release notes](https://github.com/stm32-rs/stm32h7xx-hal/releases)
- [Commits](https://github.com/stm32-rs/stm32h7xx-hal/compare/3da22d4935c8f6e412b99e6662ec11da5265fb88...2b8a04caac566a8560f400ddd6503508f78bea77)

Signed-off-by: dependabot[bot] <support@github.com>
---
 Cargo.lock | 54 +++++++++++++++++++++++++++++++++++++++---------------
 1 file changed, 39 insertions(+), 15 deletions(-)

diff --git a/Cargo.lock b/Cargo.lock
index 1380c7b..965e08a 100644
--- a/Cargo.lock
+++ b/Cargo.lock
@@ -36,7 +36,7 @@ source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "b9a69a963b70ddacfcd382524f72a4576f359af9334b3bf48a79566590bb8bfa"
 dependencies = [
  "bitrate",
- "cortex-m",
+ "cortex-m 0.7.1",
  "embedded-hal",
 ]
 
@@ -160,23 +160,36 @@ checksum = "cd51eab21ab4fd6a3bf889e2d0958c0a6e3a61ad04260325e919e652a2a62826"
 
 [[package]]
 name = "cortex-m"
-version = "0.6.4"
+version = "0.6.7"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "88cdafeafba636c00c467ded7f1587210725a1adfab0c24028a7844b87738263"
+checksum = "9075300b07c6a56263b9b582c214d0ff037b00d45ec9fde1cc711490c56f1bb9"
 dependencies = [
  "aligned",
  "bare-metal 0.2.5",
  "bitfield",
+ "cortex-m 0.7.1",
+ "volatile-register",
+]
+
+[[package]]
+name = "cortex-m"
+version = "0.7.1"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "a0b756a8bffc56025de45218a48ff9b801180440c0ee49a722b32d49dcebc771"
+dependencies = [
+ "bare-metal 0.2.5",
+ "bitfield",
+ "embedded-hal",
  "volatile-register",
 ]
 
 [[package]]
 name = "cortex-m-log"
-version = "0.6.2"
+version = "0.7.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "1d63959cb1e003dd97233fee6762351540253237eadf06fcdcb98cbfa3f9be4a"
+checksum = "0e202d2eac4e34adf7524a563e36623bae6f69cc0a73ef9bd22a4c93a5a806fa"
 dependencies = [
- "cortex-m",
+ "cortex-m 0.7.1",
  "cortex-m-semihosting",
  "log",
 ]
@@ -208,7 +221,7 @@ version = "0.5.5"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "b30efcb6b7920d9016182c485687f0012487032a14c415d2fce6e9862ef8260e"
 dependencies = [
- "cortex-m",
+ "cortex-m 0.6.7",
  "cortex-m-rt",
  "cortex-m-rtic-macros",
  "heapless",
@@ -234,7 +247,7 @@ version = "0.3.7"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "6bffa6c1454368a6aa4811ae60964c38e6996d397ff8095a8b9211b1c1f749bc"
 dependencies = [
- "cortex-m",
+ "cortex-m 0.7.1",
 ]
 
 [[package]]
@@ -707,7 +720,7 @@ version = "0.5.6"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "c3d55dedd501dfd02514646e0af4d7016ce36bc12ae177ef52056989966a1eec"
 dependencies = [
- "cortex-m",
+ "cortex-m 0.7.1",
  "cortex-m-semihosting",
 ]
 
@@ -1035,13 +1048,24 @@ dependencies = [
  "managed",
 ]
 
+[[package]]
+name = "smoltcp"
+version = "0.7.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "ab527c390c7e107f687bd92a886a083fde61b8cdc700b37f3d7e4346ffd8fae1"
+dependencies = [
+ "bitflags",
+ "byteorder",
+ "managed",
+]
+
 [[package]]
 name = "stabilizer"
 version = "0.4.1"
 dependencies = [
  "ad9959",
  "asm-delay",
- "cortex-m",
+ "cortex-m 0.6.7",
  "cortex-m-log",
  "cortex-m-rt",
  "cortex-m-rtic",
@@ -1057,7 +1081,7 @@ dependencies = [
  "paste",
  "serde",
  "serde-json-core",
- "smoltcp",
+ "smoltcp 0.6.0",
  "stm32h7xx-hal",
 ]
 
@@ -1074,7 +1098,7 @@ source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "7571f17d1ed7d67957d0004de6c52bd1ef5e736ed5ddc2bcecf001512269f77c"
 dependencies = [
  "bare-metal 0.2.5",
- "cortex-m",
+ "cortex-m 0.6.7",
  "cortex-m-rt",
  "vcell",
 ]
@@ -1082,17 +1106,17 @@ dependencies = [
 [[package]]
 name = "stm32h7xx-hal"
 version = "0.8.0"
-source = "git+https://github.com/stm32-rs/stm32h7xx-hal?branch=dma#3da22d4935c8f6e412b99e6662ec11da5265fb88"
+source = "git+https://github.com/stm32-rs/stm32h7xx-hal?branch=dma#2b8a04caac566a8560f400ddd6503508f78bea77"
 dependencies = [
  "bare-metal 1.0.0",
  "cast",
- "cortex-m",
+ "cortex-m 0.6.7",
  "cortex-m-rt",
  "embedded-dma",
  "embedded-hal",
  "nb 1.0.0",
  "paste",
- "smoltcp",
+ "smoltcp 0.7.0",
  "stm32h7",
  "void",
 ]

From 7e37ac7b02aabaab50eb094f78c3dcf8554b5a67 Mon Sep 17 00:00:00 2001
From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com>
Date: Sun, 31 Jan 2021 22:28:52 +0000
Subject: [PATCH 38/71] build(deps): bump log from 0.4.13 to 0.4.14

Bumps [log](https://github.com/rust-lang/log) from 0.4.13 to 0.4.14.
- [Release notes](https://github.com/rust-lang/log/releases)
- [Changelog](https://github.com/rust-lang/log/blob/master/CHANGELOG.md)
- [Commits](https://github.com/rust-lang/log/compare/0.4.13...0.4.14)

Signed-off-by: dependabot[bot] <support@github.com>
---
 Cargo.lock | 63 ++++++++++++++++++++++++++++++------------------------
 1 file changed, 35 insertions(+), 28 deletions(-)

diff --git a/Cargo.lock b/Cargo.lock
index 1380c7b..c6ec7a4 100644
--- a/Cargo.lock
+++ b/Cargo.lock
@@ -36,7 +36,7 @@ source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "b9a69a963b70ddacfcd382524f72a4576f359af9334b3bf48a79566590bb8bfa"
 dependencies = [
  "bitrate",
- "cortex-m",
+ "cortex-m 0.7.1",
  "embedded-hal",
 ]
 
@@ -129,12 +129,6 @@ dependencies = [
  "rustc_version",
 ]
 
-[[package]]
-name = "cfg-if"
-version = "0.1.10"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "4785bdd1c96b2a846b2bd7cc02e86b6b3dbf14e7e53446c4f54c92a361040822"
-
 [[package]]
 name = "cfg-if"
 version = "1.0.0"
@@ -160,23 +154,36 @@ checksum = "cd51eab21ab4fd6a3bf889e2d0958c0a6e3a61ad04260325e919e652a2a62826"
 
 [[package]]
 name = "cortex-m"
-version = "0.6.4"
+version = "0.6.7"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "88cdafeafba636c00c467ded7f1587210725a1adfab0c24028a7844b87738263"
+checksum = "9075300b07c6a56263b9b582c214d0ff037b00d45ec9fde1cc711490c56f1bb9"
 dependencies = [
  "aligned",
  "bare-metal 0.2.5",
  "bitfield",
+ "cortex-m 0.7.1",
+ "volatile-register",
+]
+
+[[package]]
+name = "cortex-m"
+version = "0.7.1"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "a0b756a8bffc56025de45218a48ff9b801180440c0ee49a722b32d49dcebc771"
+dependencies = [
+ "bare-metal 0.2.5",
+ "bitfield",
+ "embedded-hal",
  "volatile-register",
 ]
 
 [[package]]
 name = "cortex-m-log"
-version = "0.6.2"
+version = "0.7.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "1d63959cb1e003dd97233fee6762351540253237eadf06fcdcb98cbfa3f9be4a"
+checksum = "0e202d2eac4e34adf7524a563e36623bae6f69cc0a73ef9bd22a4c93a5a806fa"
 dependencies = [
- "cortex-m",
+ "cortex-m 0.7.1",
  "cortex-m-semihosting",
  "log",
 ]
@@ -208,7 +215,7 @@ version = "0.5.5"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "b30efcb6b7920d9016182c485687f0012487032a14c415d2fce6e9862ef8260e"
 dependencies = [
- "cortex-m",
+ "cortex-m 0.6.7",
  "cortex-m-rt",
  "cortex-m-rtic-macros",
  "heapless",
@@ -234,7 +241,7 @@ version = "0.3.7"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "6bffa6c1454368a6aa4811ae60964c38e6996d397ff8095a8b9211b1c1f749bc"
 dependencies = [
- "cortex-m",
+ "cortex-m 0.7.1",
 ]
 
 [[package]]
@@ -279,7 +286,7 @@ version = "0.5.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "dca26ee1f8d361640700bde38b2c37d8c22b3ce2d360e1fc1c74ea4b0aa7d775"
 dependencies = [
- "cfg-if 1.0.0",
+ "cfg-if",
  "crossbeam-utils",
 ]
 
@@ -289,7 +296,7 @@ version = "0.8.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "94af6efb46fef72616855b036a624cf27ba656ffc9be1b9a3c931cfc7749a9a9"
 dependencies = [
- "cfg-if 1.0.0",
+ "cfg-if",
  "crossbeam-epoch",
  "crossbeam-utils",
 ]
@@ -300,7 +307,7 @@ version = "0.9.1"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "a1aaa739f95311c2c7887a76863f500026092fb1dce0161dab577e559ef3569d"
 dependencies = [
- "cfg-if 1.0.0",
+ "cfg-if",
  "const_fn",
  "crossbeam-utils",
  "lazy_static",
@@ -315,7 +322,7 @@ source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "02d96d1e189ef58269ebe5b97953da3274d83a93af647c2ddd6f9dab28cedb8d"
 dependencies = [
  "autocfg",
- "cfg-if 1.0.0",
+ "cfg-if",
  "lazy_static",
 ]
 
@@ -432,7 +439,7 @@ version = "0.1.16"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "8fc3cb4d91f53b50155bdcfd23f6a4c39ae1969c2ae85982b135750cccaf5fce"
 dependencies = [
- "cfg-if 1.0.0",
+ "cfg-if",
  "libc",
  "wasi 0.9.0+wasi-snapshot-preview1",
 ]
@@ -443,7 +450,7 @@ version = "0.2.2"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "c9495705279e7140bf035dde1f6e750c162df8b625267cd52cc44e0b156732c8"
 dependencies = [
- "cfg-if 1.0.0",
+ "cfg-if",
  "libc",
  "wasi 0.10.2+wasi-snapshot-preview1",
 ]
@@ -554,11 +561,11 @@ checksum = "c7d73b3f436185384286bd8098d17ec07c9a7d2388a6599f824d8502b529702a"
 
 [[package]]
 name = "log"
-version = "0.4.13"
+version = "0.4.14"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "fcf3805d4480bb5b86070dcfeb9e2cb2ebc148adb753c5cca5f884d1d65a42b2"
+checksum = "51b9bbe6c47d51fc3e1a9b945965946b4c44142ab8792c50835a980d362c2710"
 dependencies = [
- "cfg-if 0.1.10",
+ "cfg-if",
 ]
 
 [[package]]
@@ -707,7 +714,7 @@ version = "0.5.6"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "c3d55dedd501dfd02514646e0af4d7016ce36bc12ae177ef52056989966a1eec"
 dependencies = [
- "cortex-m",
+ "cortex-m 0.7.1",
  "cortex-m-semihosting",
 ]
 
@@ -1041,7 +1048,7 @@ version = "0.4.1"
 dependencies = [
  "ad9959",
  "asm-delay",
- "cortex-m",
+ "cortex-m 0.6.7",
  "cortex-m-log",
  "cortex-m-rt",
  "cortex-m-rtic",
@@ -1074,7 +1081,7 @@ source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "7571f17d1ed7d67957d0004de6c52bd1ef5e736ed5ddc2bcecf001512269f77c"
 dependencies = [
  "bare-metal 0.2.5",
- "cortex-m",
+ "cortex-m 0.6.7",
  "cortex-m-rt",
  "vcell",
 ]
@@ -1086,7 +1093,7 @@ source = "git+https://github.com/stm32-rs/stm32h7xx-hal?branch=dma#3da22d4935c8f
 dependencies = [
  "bare-metal 1.0.0",
  "cast",
- "cortex-m",
+ "cortex-m 0.6.7",
  "cortex-m-rt",
  "embedded-dma",
  "embedded-hal",
@@ -1201,7 +1208,7 @@ version = "0.2.69"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "3cd364751395ca0f68cafb17666eee36b63077fb5ecd972bbcd74c90c4bf736e"
 dependencies = [
- "cfg-if 1.0.0",
+ "cfg-if",
  "wasm-bindgen-macro",
 ]
 

From cc9110426af85b2e98178c8b01fd65c069de1949 Mon Sep 17 00:00:00 2001
From: Harry Ho <hh@m-labs.hk>
Date: Mon, 1 Feb 2021 17:47:32 +0800
Subject: [PATCH 39/71] update cargosha256

---
 cargosha256.nix | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/cargosha256.nix b/cargosha256.nix
index 92745cf..cb6b230 100644
--- a/cargosha256.nix
+++ b/cargosha256.nix
@@ -1 +1 @@
-"0mrnm74wd5c1cl3av8iqndg6xrm07vs862882m59pjnrgy3z2zqj"
+"1hz1s4hbdrxzhmxg3hvzv1n9xnz4h3l76411ch8c664csr9spv2r"

From 0fd4b167b4dfe0982f7b4321258114709a5db1c6 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Robert=20J=C3=B6rdens?= <rj@quartiq.de>
Date: Mon, 1 Feb 2021 12:07:03 +0100
Subject: [PATCH 40/71] complex/cossin: decouple modules

---
 dsp/src/complex.rs |  6 +++---
 dsp/src/cossin.rs  | 10 +++++-----
 2 files changed, 8 insertions(+), 8 deletions(-)

diff --git a/dsp/src/complex.rs b/dsp/src/complex.rs
index 2bdb9ea..bb928e0 100644
--- a/dsp/src/complex.rs
+++ b/dsp/src/complex.rs
@@ -1,7 +1,6 @@
 use super::{atan2, cossin};
-use serde::{Deserialize, Serialize};
 
-#[derive(Copy, Clone, Default, PartialEq, Debug, Deserialize, Serialize)]
+#[derive(Copy, Clone, Default, PartialEq, Debug)]
 pub struct Complex<T>(pub T, pub T);
 
 impl Complex<i32> {
@@ -17,7 +16,8 @@ impl Complex<i32> {
     /// ```
     #[inline(always)]
     pub fn from_angle(angle: i32) -> Complex<i32> {
-        cossin(angle)
+        let (c, s) = cossin(angle);
+        Complex(c, s)
     }
 
     /// Return the absolute square (the squared magnitude).
diff --git a/dsp/src/cossin.rs b/dsp/src/cossin.rs
index f9cc42e..e5746a3 100644
--- a/dsp/src/cossin.rs
+++ b/dsp/src/cossin.rs
@@ -1,4 +1,3 @@
-use super::Complex;
 use core::f64::consts::PI;
 
 include!(concat!(env!("OUT_DIR"), "/cossin_table.rs"));
@@ -11,10 +10,10 @@ include!(concat!(env!("OUT_DIR"), "/cossin_table.rs"));
 /// * `phase` - 32-bit phase.
 ///
 /// # Returns
-/// The cos and sin values of the provided phase as a `Complex<i32>`
-/// value. With a 7-bit deep LUT there is 1e-5 max and 6e-8 RMS error
+/// The cos and sin values of the provided phase as a `(i32, i32)`
+/// tuple. With a 7-bit deep LUT there is 1e-5 max and 6e-8 RMS error
 /// in each quadrature over 20 bit phase.
-pub fn cossin(phase: i32) -> Complex<i32> {
+pub fn cossin(phase: i32) -> (i32, i32) {
     // Phase bits excluding the three highes MSB
     const OCTANT_BITS: usize = 32 - 3;
 
@@ -69,12 +68,13 @@ pub fn cossin(phase: i32) -> Complex<i32> {
         sin *= -1;
     }
 
-    Complex(cos, sin)
+    (cos, sin)
 }
 
 #[cfg(test)]
 mod tests {
     use super::*;
+    use crate::Complex;
     use core::f64::consts::PI;
 
     #[test]

From 2c60103696e79ba983b9bc72257fbfa42cc1286c Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Robert=20J=C3=B6rdens?= <rj@quartiq.de>
Date: Mon, 1 Feb 2021 12:22:50 +0100
Subject: [PATCH 41/71] dsp: accu: add, iir: rename IIRState to Vec5

---
 dsp/src/accu.rs            | 21 +++++++++++++++++++++
 dsp/src/iir.rs             |  6 +++---
 dsp/src/iir_int.rs         | 16 ++++++++--------
 dsp/src/lib.rs             |  2 ++
 dsp/src/lockin.rs          | 26 +++++++++++---------------
 src/bin/dual-iir.rs        |  6 +++---
 src/bin/lockin-external.rs |  8 ++++----
 src/bin/lockin-internal.rs |  2 +-
 8 files changed, 53 insertions(+), 34 deletions(-)
 create mode 100644 dsp/src/accu.rs

diff --git a/dsp/src/accu.rs b/dsp/src/accu.rs
new file mode 100644
index 0000000..6560896
--- /dev/null
+++ b/dsp/src/accu.rs
@@ -0,0 +1,21 @@
+#[derive(Copy, Clone, Default, PartialEq, Debug)]
+pub struct Accu {
+    state: i32,
+    step: i32,
+}
+
+impl Accu {
+    pub fn new(state: i32, step: i32) -> Accu {
+        Accu { state, step }
+    }
+}
+
+impl Iterator for Accu {
+    type Item = i32;
+    #[inline]
+    fn next(&mut self) -> Option<i32> {
+        let s = self.state;
+        self.state = self.state.wrapping_add(self.step);
+        Some(s)
+    }
+}
diff --git a/dsp/src/iir.rs b/dsp/src/iir.rs
index 50dd919..8d0dbe4 100644
--- a/dsp/src/iir.rs
+++ b/dsp/src/iir.rs
@@ -12,7 +12,7 @@ use core::f32;
 /// coefficients (b0, b1, b2) followd by the negated feed-back coefficients
 /// (-a1, -a2), all five normalized such that a0 = 1.
 #[derive(Copy, Clone, Default, Deserialize, Serialize)]
-pub struct IIRState(pub [f32; 5]);
+pub struct Vec5(pub [f32; 5]);
 
 /// IIR configuration.
 ///
@@ -41,7 +41,7 @@ pub struct IIRState(pub [f32; 5]);
 ///   implementation of transfer functions beyond bequadratic terms.
 #[derive(Copy, Clone, Default, Deserialize, Serialize)]
 pub struct IIR {
-    pub ba: IIRState,
+    pub ba: Vec5,
     pub y_offset: f32,
     pub y_min: f32,
     pub y_max: f32,
@@ -108,7 +108,7 @@ impl IIR {
     /// # Arguments
     /// * `xy` - Current filter state.
     /// * `x0` - New input.
-    pub fn update(&self, xy: &mut IIRState, x0: f32) -> f32 {
+    pub fn update(&self, xy: &mut Vec5, x0: f32) -> f32 {
         let n = self.ba.0.len();
         debug_assert!(xy.0.len() == n);
         // `xy` contains       x0 x1 y0 y1 y2
diff --git a/dsp/src/iir_int.rs b/dsp/src/iir_int.rs
index 64ab175..2edca3a 100644
--- a/dsp/src/iir_int.rs
+++ b/dsp/src/iir_int.rs
@@ -5,9 +5,9 @@ use serde::{Deserialize, Serialize};
 /// This struct is used to hold the x/y input/output data vector or the b/a coefficient
 /// vector.
 #[derive(Copy, Clone, Default, Deserialize, Serialize)]
-pub struct IIRState(pub [i32; 5]);
+pub struct Vec5(pub [i32; 5]);
 
-impl IIRState {
+impl Vec5 {
     /// Lowpass biquad filter using cutoff and sampling frequencies.  Taken from:
     /// https://webaudio.github.io/Audio-EQ-Cookbook/audio-eq-cookbook.html
     ///
@@ -19,7 +19,7 @@ impl IIRState {
     ///
     /// # Returns
     /// 2nd-order IIR filter coefficients in the form [b0,b1,b2,a1,a2]. a0 is set to -1.
-    pub fn lowpass(f: f32, q: f32, k: f32) -> IIRState {
+    pub fn lowpass(f: f32, q: f32, k: f32) -> Vec5 {
         // 3rd order Taylor approximation of sin and cos.
         let f = f * 2. * PI;
         let fsin = f - f * f * f / 6.;
@@ -31,7 +31,7 @@ impl IIRState {
         let a1 = (2. * fcos / a0) as _;
         let a2 = ((alpha - 1.) / a0) as _;
 
-        IIRState([b0, 2 * b0, b0, a1, a2])
+        Vec5([b0, 2 * b0, b0, a1, a2])
     }
 }
 
@@ -53,7 +53,7 @@ fn macc(y0: i32, x: &[i32], a: &[i32], shift: u32) -> i32 {
 /// Coefficient scaling fixed and optimized.
 #[derive(Copy, Clone, Default, Deserialize, Serialize)]
 pub struct IIR {
-    pub ba: IIRState,
+    pub ba: Vec5,
     // pub y_offset: i32,
     // pub y_min: i32,
     // pub y_max: i32,
@@ -70,7 +70,7 @@ impl IIR {
     /// # Arguments
     /// * `xy` - Current filter state.
     /// * `x0` - New input.
-    pub fn update(&self, xy: &mut IIRState, x0: i32) -> i32 {
+    pub fn update(&self, xy: &mut Vec5, x0: i32) -> i32 {
         let n = self.ba.0.len();
         debug_assert!(xy.0.len() == n);
         // `xy` contains       x0 x1 y0 y1 y2
@@ -92,11 +92,11 @@ impl IIR {
 
 #[cfg(test)]
 mod test {
-    use super::IIRState;
+    use super::Vec5;
 
     #[test]
     fn lowpass_gen() {
-        let ba = IIRState::lowpass(1e-3, 1. / 2f32.sqrt(), 2.);
+        let ba = Vec5::lowpass(1e-3, 1. / 2f32.sqrt(), 2.);
         println!("{:?}", ba.0);
     }
 }
diff --git a/dsp/src/lib.rs b/dsp/src/lib.rs
index 191054a..74ebdbb 100644
--- a/dsp/src/lib.rs
+++ b/dsp/src/lib.rs
@@ -112,6 +112,7 @@ where
         .fold(y0, |y, xa| y + xa)
 }
 
+pub mod accu;
 mod atan2;
 mod complex;
 mod cossin;
@@ -122,6 +123,7 @@ pub mod pll;
 pub mod rpll;
 pub mod unwrap;
 
+pub use accu::Accu;
 pub use atan2::atan2;
 pub use complex::Complex;
 pub use cossin::cossin;
diff --git a/dsp/src/lockin.rs b/dsp/src/lockin.rs
index 0f6b78f..19f13e2 100644
--- a/dsp/src/lockin.rs
+++ b/dsp/src/lockin.rs
@@ -1,20 +1,20 @@
-use super::{iir_int, Complex};
+use super::{iir_int::{IIR, Vec5}, Accu, Complex};
 use serde::{Deserialize, Serialize};
 
 #[derive(Copy, Clone, Default, Deserialize, Serialize)]
 pub struct Lockin {
-    iir: iir_int::IIR,
-    iir_state: [iir_int::IIRState; 2],
+    iir: IIR,
+    state: [Vec5; 2],
 }
 
 impl Lockin {
     /// Create a new Lockin with given IIR coefficients.
-    pub fn new(ba: &iir_int::IIRState) -> Self {
-        let mut iir = iir_int::IIR::default();
-        iir.ba.0.copy_from_slice(&ba.0);
+    pub fn new(ba: Vec5) -> Self {
+        let mut iir = IIR::default();
+        iir.ba = ba;
         Lockin {
             iir,
-            iir_state: [iir_int::IIRState::default(); 2],
+            state: [Vec5::default(); 2],
         }
     }
 
@@ -28,11 +28,11 @@ impl Lockin {
         // Note: 32x32 -> 64 bit multiplications are pretty much free.
         Complex(
             self.iir.update(
-                &mut self.iir_state[0],
+                &mut self.state[0],
                 ((sample as i64 * lo.0 as i64) >> 32) as _,
             ),
             self.iir.update(
-                &mut self.iir_state[1],
+                &mut self.state[1],
                 ((sample as i64 * lo.1 as i64) >> 32) as _,
             ),
         )
@@ -47,14 +47,10 @@ impl Lockin {
         phase: i32,
         frequency: i32,
     ) -> Option<Complex<i32>> {
-        let mut phase = phase;
-
         signal
             .into_iter()
-            .map(|sample| {
-                phase = phase.wrapping_add(frequency);
-                self.update(sample, phase)
-            })
+            .zip(Accu::new(phase, frequency))
+            .map(|(sample, phase)| self.update(sample, phase))
             .last()
     }
 }
diff --git a/src/bin/dual-iir.rs b/src/bin/dual-iir.rs
index 1757d7c..ef79998 100644
--- a/src/bin/dual-iir.rs
+++ b/src/bin/dual-iir.rs
@@ -34,9 +34,9 @@ const APP: () = {
         net_interface: hardware::Ethernet,
 
         // Format: iir_state[ch][cascade-no][coeff]
-        #[init([[iir::IIRState([0.; 5]); IIR_CASCADE_LENGTH]; 2])]
-        iir_state: [[iir::IIRState; IIR_CASCADE_LENGTH]; 2],
-        #[init([[iir::IIR { ba: iir::IIRState([1., 0., 0., 0., 0.]), y_offset: 0., y_min: -SCALE - 1., y_max: SCALE }; IIR_CASCADE_LENGTH]; 2])]
+        #[init([[iir::Vec5([0.; 5]); IIR_CASCADE_LENGTH]; 2])]
+        iir_state: [[iir::Vec5; IIR_CASCADE_LENGTH]; 2],
+        #[init([[iir::IIR { ba: iir::Vec5([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],
     }
 
diff --git a/src/bin/lockin-external.rs b/src/bin/lockin-external.rs
index 95c79eb..d337662 100644
--- a/src/bin/lockin-external.rs
+++ b/src/bin/lockin-external.rs
@@ -38,9 +38,9 @@ const APP: () = {
         net_interface: hardware::Ethernet,
 
         // Format: iir_state[ch][cascade-no][coeff]
-        #[init([[iir::IIRState([0.; 5]); IIR_CASCADE_LENGTH]; 2])]
-        iir_state: [[iir::IIRState; IIR_CASCADE_LENGTH]; 2],
-        #[init([[iir::IIR { ba: iir::IIRState([1., 0., 0., 0., 0.]), y_offset: 0., y_min: -SCALE - 1., y_max: SCALE }; IIR_CASCADE_LENGTH]; 2])]
+        #[init([[iir::Vec5([0.; 5]); IIR_CASCADE_LENGTH]; 2])]
+        iir_state: [[iir::Vec5; IIR_CASCADE_LENGTH]; 2],
+        #[init([[iir::IIR { ba: iir::Vec5([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],
 
         timestamper: InputStamper,
@@ -56,7 +56,7 @@ const APP: () = {
         let pll = RPLL::new(ADC_SAMPLE_TICKS_LOG2 + SAMPLE_BUFFER_SIZE_LOG2);
 
         let lockin = Lockin::new(
-            &iir_int::IIRState::lowpass(1e-3, 0.707, 2.), // TODO: expose
+            iir_int::Vec5::lowpass(1e-3, 0.707, 2.), // TODO: expose
         );
 
         // Enable ADC/DAC events
diff --git a/src/bin/lockin-internal.rs b/src/bin/lockin-internal.rs
index 8c41cfe..f72c3ca 100644
--- a/src/bin/lockin-internal.rs
+++ b/src/bin/lockin-internal.rs
@@ -27,7 +27,7 @@ const APP: () = {
         let (mut stabilizer, _pounder) = hardware::setup(c.core, c.device);
 
         let lockin = Lockin::new(
-            &iir_int::IIRState::lowpass(1e-3, 0.707, 2.), // TODO: expose
+            iir_int::Vec5::lowpass(1e-3, 0.707, 2.), // TODO: expose
         );
 
         // Enable ADC/DAC events

From 7748d8eb543b390c5e204614909e7ac92161044a Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Robert=20J=C3=B6rdens?= <rj@quartiq.de>
Date: Mon, 1 Feb 2021 12:37:44 +0100
Subject: [PATCH 42/71] dsp: constructor style

---
 dsp/src/accu.rs    |  4 ++--
 dsp/src/complex.rs |  4 ++--
 dsp/src/iir_int.rs |  4 ++--
 dsp/src/lockin.rs  | 11 ++++++-----
 dsp/src/rpll.rs    |  6 +++---
 5 files changed, 15 insertions(+), 14 deletions(-)

diff --git a/dsp/src/accu.rs b/dsp/src/accu.rs
index 6560896..99369b2 100644
--- a/dsp/src/accu.rs
+++ b/dsp/src/accu.rs
@@ -5,8 +5,8 @@ pub struct Accu {
 }
 
 impl Accu {
-    pub fn new(state: i32, step: i32) -> Accu {
-        Accu { state, step }
+    pub fn new(state: i32, step: i32) -> Self {
+        Self { state, step }
     }
 }
 
diff --git a/dsp/src/complex.rs b/dsp/src/complex.rs
index bb928e0..9829312 100644
--- a/dsp/src/complex.rs
+++ b/dsp/src/complex.rs
@@ -15,9 +15,9 @@ impl Complex<i32> {
     /// Complex::<i32>::from_angle(-1 << 30);  // -pi/2
     /// ```
     #[inline(always)]
-    pub fn from_angle(angle: i32) -> Complex<i32> {
+    pub fn from_angle(angle: i32) -> Self {
         let (c, s) = cossin(angle);
-        Complex(c, s)
+        Self(c, s)
     }
 
     /// Return the absolute square (the squared magnitude).
diff --git a/dsp/src/iir_int.rs b/dsp/src/iir_int.rs
index 2edca3a..93626de 100644
--- a/dsp/src/iir_int.rs
+++ b/dsp/src/iir_int.rs
@@ -19,7 +19,7 @@ impl Vec5 {
     ///
     /// # Returns
     /// 2nd-order IIR filter coefficients in the form [b0,b1,b2,a1,a2]. a0 is set to -1.
-    pub fn lowpass(f: f32, q: f32, k: f32) -> Vec5 {
+    pub fn lowpass(f: f32, q: f32, k: f32) -> Self {
         // 3rd order Taylor approximation of sin and cos.
         let f = f * 2. * PI;
         let fsin = f - f * f * f / 6.;
@@ -31,7 +31,7 @@ impl Vec5 {
         let a1 = (2. * fcos / a0) as _;
         let a2 = ((alpha - 1.) / a0) as _;
 
-        Vec5([b0, 2 * b0, b0, a1, a2])
+        Self([b0, 2 * b0, b0, a1, a2])
     }
 }
 
diff --git a/dsp/src/lockin.rs b/dsp/src/lockin.rs
index 19f13e2..61627cf 100644
--- a/dsp/src/lockin.rs
+++ b/dsp/src/lockin.rs
@@ -1,4 +1,7 @@
-use super::{iir_int::{IIR, Vec5}, Accu, Complex};
+use super::{
+    iir_int::{Vec5, IIR},
+    Accu, Complex,
+};
 use serde::{Deserialize, Serialize};
 
 #[derive(Copy, Clone, Default, Deserialize, Serialize)]
@@ -10,10 +13,8 @@ pub struct Lockin {
 impl Lockin {
     /// Create a new Lockin with given IIR coefficients.
     pub fn new(ba: Vec5) -> Self {
-        let mut iir = IIR::default();
-        iir.ba = ba;
-        Lockin {
-            iir,
+        Self {
+            iir: IIR { ba, ..Default::default() },
             state: [Vec5::default(); 2],
         }
     }
diff --git a/dsp/src/rpll.rs b/dsp/src/rpll.rs
index 9759cf9..79a2b30 100644
--- a/dsp/src/rpll.rs
+++ b/dsp/src/rpll.rs
@@ -22,8 +22,8 @@ impl RPLL {
     ///
     /// Returns:
     /// Initialized RPLL instance.
-    pub fn new(dt2: u8) -> RPLL {
-        RPLL {
+    pub fn new(dt2: u8) -> Self {
+        Self {
             dt2,
             ..Default::default()
         }
@@ -107,7 +107,7 @@ mod test {
 
     impl Harness {
         fn default() -> Self {
-            Harness {
+            Self {
                 rpll: RPLL::new(8),
                 dt2: 8,
                 shift_frequency: 9,

From 965c6335e11a38251f30fa096b566614b94ca663 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Robert=20J=C3=B6rdens?= <rj@quartiq.de>
Date: Mon, 1 Feb 2021 12:40:12 +0100
Subject: [PATCH 43/71] dsp: fmt

---
 dsp/src/lockin.rs | 5 ++++-
 1 file changed, 4 insertions(+), 1 deletion(-)

diff --git a/dsp/src/lockin.rs b/dsp/src/lockin.rs
index 61627cf..775398d 100644
--- a/dsp/src/lockin.rs
+++ b/dsp/src/lockin.rs
@@ -14,7 +14,10 @@ impl Lockin {
     /// Create a new Lockin with given IIR coefficients.
     pub fn new(ba: Vec5) -> Self {
         Self {
-            iir: IIR { ba, ..Default::default() },
+            iir: IIR {
+                ba,
+                ..Default::default()
+            },
             state: [Vec5::default(); 2],
         }
     }

From 90bd4741ccc97ce87c0f6ccb7ed3464f76f693df Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Robert=20J=C3=B6rdens?= <rj@quartiq.de>
Date: Mon, 1 Feb 2021 13:27:49 +0100
Subject: [PATCH 44/71] dsp/benches: iir vec5

---
 dsp/benches/micro.rs | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/dsp/benches/micro.rs b/dsp/benches/micro.rs
index 5a42640..56b9149 100644
--- a/dsp/benches/micro.rs
+++ b/dsp/benches/micro.rs
@@ -50,7 +50,7 @@ fn pll_bench(c: &mut Criterion) {
 
 fn iir_int_bench(c: &mut Criterion) {
     let dut = iir_int::IIR::default();
-    let mut xy = iir_int::IIRState::default();
+    let mut xy = iir_int::Vec5::default();
     c.bench_function("int_iir::IIR::update(s, x)", |b| {
         b.iter(|| dut.update(&mut xy, black_box(0x2832)))
     });
@@ -58,7 +58,7 @@ fn iir_int_bench(c: &mut Criterion) {
 
 fn iir_bench(c: &mut Criterion) {
     let dut = iir::IIR::default();
-    let mut xy = iir::IIRState::default();
+    let mut xy = iir::Vec5::default();
     c.bench_function("int::IIR::update(s, x)", |b| {
         b.iter(|| dut.update(&mut xy, black_box(0.32241)))
     });

From 65a3f839a0930f319b60aa2775b7e1745c2aa804 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Robert=20J=C3=B6rdens?= <rj@quartiq.de>
Date: Mon, 1 Feb 2021 13:42:21 +0100
Subject: [PATCH 45/71] lockin: remove feed()

---
 dsp/src/lockin.rs          | 18 +-----------------
 src/bin/lockin-external.rs | 18 ++++++++++--------
 src/bin/lockin-internal.rs | 19 +++++++++++--------
 3 files changed, 22 insertions(+), 33 deletions(-)

diff --git a/dsp/src/lockin.rs b/dsp/src/lockin.rs
index 775398d..fcbb4c9 100644
--- a/dsp/src/lockin.rs
+++ b/dsp/src/lockin.rs
@@ -1,6 +1,6 @@
 use super::{
     iir_int::{Vec5, IIR},
-    Accu, Complex,
+    Complex,
 };
 use serde::{Deserialize, Serialize};
 
@@ -41,20 +41,4 @@ impl Lockin {
             ),
         )
     }
-
-    /// Feed an iterator into the Lockin and return the latest I/Q data.
-    /// Initial stample phase and frequency (phase increment between samples)
-    /// are supplied.
-    pub fn feed<I: IntoIterator<Item = i32>>(
-        &mut self,
-        signal: I,
-        phase: i32,
-        frequency: i32,
-    ) -> Option<Complex<i32>> {
-        signal
-            .into_iter()
-            .zip(Accu::new(phase, frequency))
-            .map(|(sample, phase)| self.update(sample, phase))
-            .last()
-    }
 }
diff --git a/src/bin/lockin-external.rs b/src/bin/lockin-external.rs
index d337662..3668b60 100644
--- a/src/bin/lockin-external.rs
+++ b/src/bin/lockin-external.rs
@@ -16,7 +16,7 @@ use stabilizer::{
     hardware, server, ADC_SAMPLE_TICKS_LOG2, SAMPLE_BUFFER_SIZE_LOG2,
 };
 
-use dsp::{iir, iir_int, lockin::Lockin, rpll::RPLL};
+use dsp::{iir, iir_int, lockin::Lockin, rpll::RPLL, Accu};
 use hardware::{
     Adc0Input, Adc1Input, Dac0Output, Dac1Output, InputStamper, AFE0, AFE1,
 };
@@ -133,13 +133,15 @@ const APP: () = {
         let sample_phase =
             phase_offset.wrapping_add(pll_phase.wrapping_mul(harmonic));
 
-        if let Some(output) = lockin.feed(
-            adc_samples[0].iter().map(|&i|
-                // Convert to signed, MSB align the ADC sample.
-                (i as i16 as i32) << 16),
-            sample_phase,
-            sample_frequency,
-        ) {
+        if let Some(output) = adc_samples[0]
+            .iter()
+            .zip(Accu::new(sample_phase, sample_frequency))
+            // Convert to signed, MSB align the ADC sample.
+            .map(|(&sample, phase)| {
+                lockin.update((sample as i16 as i32) << 16, phase)
+            })
+            .last()
+        {
             // Convert from IQ to power and phase.
             let mut power = output.abs_sqr() as _;
             let mut phase = output.arg() as _;
diff --git a/src/bin/lockin-internal.rs b/src/bin/lockin-internal.rs
index f72c3ca..e4a2521 100644
--- a/src/bin/lockin-internal.rs
+++ b/src/bin/lockin-internal.rs
@@ -7,7 +7,7 @@
 const DAC_SEQUENCE: [f32; 8] =
     [0.0, 0.707, 1.0, 0.707, 0.0, -0.707, -1.0, -0.707];
 
-use dsp::{iir_int, lockin::Lockin};
+use dsp::{iir_int, lockin::Lockin, Accu};
 use hardware::{Adc1Input, Dac0Output, Dac1Output, AFE0, AFE1};
 use stabilizer::hardware;
 
@@ -66,6 +66,7 @@ const APP: () = {
     /// TODO: Document
     #[task(binds=DMA1_STR4, resources=[adc1, dacs, lockin], priority=2)]
     fn process(c: process::Context) {
+        let lockin = c.resources.lockin;
         let adc_samples = c.resources.adc1.acquire_buffer();
         let dac_samples = [
             c.resources.dacs.0.acquire_buffer(),
@@ -96,13 +97,15 @@ const APP: () = {
         let sample_phase = phase_offset
             .wrapping_add((pll_phase as i32).wrapping_mul(harmonic));
 
-        if let Some(output) = c.resources.lockin.feed(
-            adc_samples.iter().map(|&i|
-                // Convert to signed, MSB align the ADC sample.
-                (i as i16 as i32) << 16),
-            sample_phase,
-            sample_frequency,
-        ) {
+        if let Some(output) = adc_samples
+            .iter()
+            .zip(Accu::new(sample_phase, sample_frequency))
+            // Convert to signed, MSB align the ADC sample.
+            .map(|(&sample, phase)| {
+                lockin.update((sample as i16 as i32) << 16, phase)
+            })
+            .last()
+        {
             // Convert from IQ to power and phase.
             let _power = output.abs_sqr();
             let phase = output.arg() >> 16;

From ab7e3d229bebcfae3dbeda4a5442a377eb6e1e52 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Robert=20J=C3=B6rdens?= <rj@quartiq.de>
Date: Mon, 1 Feb 2021 14:44:53 +0100
Subject: [PATCH 46/71] rpll: clean up asserts

---
 dsp/src/rpll.rs | 12 ++++++------
 1 file changed, 6 insertions(+), 6 deletions(-)

diff --git a/dsp/src/rpll.rs b/dsp/src/rpll.rs
index 79a2b30..108464e 100644
--- a/dsp/src/rpll.rs
+++ b/dsp/src/rpll.rs
@@ -49,7 +49,7 @@ impl RPLL {
         shift_frequency: u8,
         shift_phase: u8,
     ) -> (i32, u32) {
-        debug_assert!(shift_frequency > self.dt2);
+        debug_assert!(shift_frequency >= self.dt2);
         debug_assert!(shift_phase >= self.dt2);
         // Advance phase
         self.y = self.y.wrapping_add(self.f as i32);
@@ -122,6 +122,10 @@ mod test {
         }
 
         fn run(&mut self, n: usize) -> (Vec<f32>, Vec<f32>) {
+            assert!(self.period >= 1 << self.dt2);
+            assert!(self.period < 1 << self.shift_frequency);
+            assert!(self.period < 1 << self.shift_phase + 1);
+
             let mut y = Vec::<f32>::new();
             let mut f = Vec::<f32>::new();
             for _ in 0..n {
@@ -162,10 +166,6 @@ mod test {
         }
 
         fn measure(&mut self, n: usize, limits: [f32; 4]) {
-            assert!(self.period >= 1 << self.dt2);
-            assert!(self.dt2 <= self.shift_frequency);
-            assert!(self.period < 1 << self.shift_frequency);
-            assert!(self.period < 1 << self.shift_frequency + 1);
             let t_settle = (1 << self.shift_frequency - self.dt2 + 4)
                 + (1 << self.shift_phase - self.dt2 + 4);
             self.run(t_settle);
@@ -190,7 +190,7 @@ mod test {
                 print!("{:.2e} ", rel);
                 assert!(
                     rel <= 1.,
-                    "idx {}, have |{}| > want {}",
+                    "idx {}, have |{:.2e}| > limit {:.2e}",
                     i,
                     m[i],
                     limits[i]

From 656e3253ab6d31b7531366e7db8c23e68b2181ba Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Robert=20J=C3=B6rdens?= <rj@quartiq.de>
Date: Mon, 1 Feb 2021 15:59:11 +0100
Subject: [PATCH 47/71] lockin-internal: document, streamline sequence

---
 src/bin/dual-iir.rs        |  4 ++--
 src/bin/lockin-external.rs |  8 ++++----
 src/bin/lockin-internal.rs | 36 ++++++++++++++++++------------------
 3 files changed, 24 insertions(+), 24 deletions(-)

diff --git a/src/bin/dual-iir.rs b/src/bin/dual-iir.rs
index ef79998..d9ca445 100644
--- a/src/bin/dual-iir.rs
+++ b/src/bin/dual-iir.rs
@@ -17,7 +17,7 @@ use stabilizer::{hardware, server};
 use dsp::iir;
 use hardware::{Adc0Input, Adc1Input, Dac0Output, Dac1Output, AFE0, AFE1};
 
-const SCALE: f32 = ((1 << 15) - 1) as f32;
+const SCALE: f32 = i16::MAX as f32;
 
 const TCP_RX_BUFFER_SIZE: usize = 8192;
 const TCP_TX_BUFFER_SIZE: usize = 8192;
@@ -36,7 +36,7 @@ const APP: () = {
         // Format: iir_state[ch][cascade-no][coeff]
         #[init([[iir::Vec5([0.; 5]); IIR_CASCADE_LENGTH]; 2])]
         iir_state: [[iir::Vec5; IIR_CASCADE_LENGTH]; 2],
-        #[init([[iir::IIR { ba: iir::Vec5([1., 0., 0., 0., 0.]), y_offset: 0., y_min: -SCALE - 1., y_max: SCALE }; IIR_CASCADE_LENGTH]; 2])]
+        #[init([[iir::IIR { ba: iir::Vec5([1., 0., 0., 0., 0.]), y_offset: 0., y_min: -SCALE, y_max: SCALE }; IIR_CASCADE_LENGTH]; 2])]
         iir_ch: [[iir::IIR; IIR_CASCADE_LENGTH]; 2],
     }
 
diff --git a/src/bin/lockin-external.rs b/src/bin/lockin-external.rs
index 3668b60..72d3fc9 100644
--- a/src/bin/lockin-external.rs
+++ b/src/bin/lockin-external.rs
@@ -21,7 +21,7 @@ use hardware::{
     Adc0Input, Adc1Input, Dac0Output, Dac1Output, InputStamper, AFE0, AFE1,
 };
 
-const SCALE: f32 = ((1 << 15) - 1) as f32;
+const SCALE: f32 = i16::MAX as f32;
 
 const TCP_RX_BUFFER_SIZE: usize = 8192;
 const TCP_TX_BUFFER_SIZE: usize = 8192;
@@ -40,7 +40,7 @@ const APP: () = {
         // Format: iir_state[ch][cascade-no][coeff]
         #[init([[iir::Vec5([0.; 5]); IIR_CASCADE_LENGTH]; 2])]
         iir_state: [[iir::Vec5; IIR_CASCADE_LENGTH]; 2],
-        #[init([[iir::IIR { ba: iir::Vec5([1., 0., 0., 0., 0.]), y_offset: 0., y_min: -SCALE - 1., y_max: SCALE }; IIR_CASCADE_LENGTH]; 2])]
+        #[init([[iir::IIR { ba: iir::Vec5([1., 0., 0., 0., 0.]), y_offset: 0., y_min: -SCALE, y_max: SCALE }; IIR_CASCADE_LENGTH]; 2])]
         iir_ch: [[iir::IIR; IIR_CASCADE_LENGTH]; 2],
 
         timestamper: InputStamper,
@@ -158,9 +158,9 @@ const APP: () = {
             for i in 0..dac_samples[0].len() {
                 unsafe {
                     dac_samples[0][i] =
-                        (power.to_int_unchecked::<i32>() >> 16) as u16 ^ 0x8000;
+                        power.to_int_unchecked::<i16>() as u16 ^ 0x8000;
                     dac_samples[1][i] =
-                        (phase.to_int_unchecked::<i32>() >> 16) as u16 ^ 0x8000;
+                        phase.to_int_unchecked::<i16>() as u16 ^ 0x8000;
                 }
             }
         }
diff --git a/src/bin/lockin-internal.rs b/src/bin/lockin-internal.rs
index e4a2521..c8001e9 100644
--- a/src/bin/lockin-internal.rs
+++ b/src/bin/lockin-internal.rs
@@ -3,13 +3,16 @@
 #![no_main]
 #![cfg_attr(feature = "nightly", feature(core_intrinsics))]
 
-// A constant sinusoid to send on the DAC output.
-const DAC_SEQUENCE: [f32; 8] =
-    [0.0, 0.707, 1.0, 0.707, 0.0, -0.707, -1.0, -0.707];
-
 use dsp::{iir_int, lockin::Lockin, Accu};
 use hardware::{Adc1Input, Dac0Output, Dac1Output, AFE0, AFE1};
-use stabilizer::hardware;
+use stabilizer::{hardware, SAMPLE_BUFFER_SIZE, SAMPLE_BUFFER_SIZE_LOG2};
+
+// A constant sinusoid to send on the DAC output.
+// Full-scale gives a +/- 10V amplitude waveform. Scale it down to give +/- 1V.
+const ONE: i16 = (0.1 * u16::MAX as f32) as _;
+const SQRT2: i16 = (ONE as f32 * 0.707) as _;
+const DAC_SEQUENCE: [i16; SAMPLE_BUFFER_SIZE] =
+    [0, SQRT2, ONE, SQRT2, 0, -SQRT2, -ONE, -SQRT2]; // TODO: rotate by -2 samples to start with ONE
 
 #[rtic::app(device = stm32h7xx_hal::stm32, peripherals = true, monotonic = rtic::cyccnt::CYCCNT)]
 const APP: () = {
@@ -74,36 +77,33 @@ const APP: () = {
         ];
 
         // DAC0 always generates a fixed sinusoidal output.
-        for (i, value) in DAC_SEQUENCE.iter().enumerate() {
-            // Full-scale gives a +/- 10V amplitude waveform. Scale it down to give +/- 1V.
-            let y = value * (0.1 * i16::MAX as f32);
-            // Note(unsafe): The DAC_SEQUENCE values are guaranteed to be normalized.
-            let y = unsafe { y.to_int_unchecked::<i16>() };
-
-            // Convert to DAC code
-            dac_samples[0][i] = y as u16 ^ 0x8000;
-        }
+        dac_samples[0]
+            .iter_mut()
+            .zip(DAC_SEQUENCE.iter())
+            .for_each(|(d, s)| *d = *s as u16 ^ 0x8000);
 
+        // Reference phase and frequency are known.
         let pll_phase = 0;
-        // 1/8 of the sample rate: log2(DAC_SEQUENCE.len()) == 3
-        let pll_frequency = 1i32 << (32 - 3);
+        let pll_frequency = 1i32 << (32 - SAMPLE_BUFFER_SIZE_LOG2);
 
         // Harmonic index of the LO: -1 to _de_modulate the fundamental
         let harmonic: i32 = -1;
 
         // Demodulation LO phase offset
-        let phase_offset: i32 = (0.7495 * i32::MAX as f32) as i32;
+        let phase_offset: i32 = (0.7495 * i32::MAX as f32) as i32; // TODO: adapt to sequence rotation above
         let sample_frequency = (pll_frequency as i32).wrapping_mul(harmonic);
         let sample_phase = phase_offset
             .wrapping_add((pll_phase as i32).wrapping_mul(harmonic));
 
         if let Some(output) = adc_samples
             .iter()
+            // Zip in the LO phase.
             .zip(Accu::new(sample_phase, sample_frequency))
-            // Convert to signed, MSB align the ADC sample.
+            // Convert to signed, MSB align the ADC sample, update the Lockin (demodulate, filter)
             .map(|(&sample, phase)| {
                 lockin.update((sample as i16 as i32) << 16, phase)
             })
+            // Decimate
             .last()
         {
             // Convert from IQ to power and phase.

From b6e22b576bb9c39fb6104c853e14a20ca609a512 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Robert=20J=C3=B6rdens?= <rj@quartiq.de>
Date: Mon, 1 Feb 2021 17:18:10 +0100
Subject: [PATCH 48/71] iir: add const fn new()

---
 dsp/src/iir.rs             | 9 +++++++++
 src/bin/dual-iir.rs        | 4 ++--
 src/bin/lockin-external.rs | 4 ++--
 3 files changed, 13 insertions(+), 4 deletions(-)

diff --git a/dsp/src/iir.rs b/dsp/src/iir.rs
index 8d0dbe4..c8ef6ba 100644
--- a/dsp/src/iir.rs
+++ b/dsp/src/iir.rs
@@ -48,6 +48,15 @@ pub struct IIR {
 }
 
 impl IIR {
+    pub const fn new(gain: f32, y_min: f32, y_max: f32) -> Self {
+        Self {
+            ba: Vec5([gain, 0., 0., 0., 0.]),
+            y_offset: 0.,
+            y_min: y_min,
+            y_max: y_max,
+        }
+    }
+
     /// Configures IIR filter coefficients for proportional-integral behavior
     /// with gain limit.
     ///
diff --git a/src/bin/dual-iir.rs b/src/bin/dual-iir.rs
index d9ca445..e28465b 100644
--- a/src/bin/dual-iir.rs
+++ b/src/bin/dual-iir.rs
@@ -17,7 +17,7 @@ use stabilizer::{hardware, server};
 use dsp::iir;
 use hardware::{Adc0Input, Adc1Input, Dac0Output, Dac1Output, AFE0, AFE1};
 
-const SCALE: f32 = i16::MAX as f32;
+const SCALE: f32 = i16::MAX as _;
 
 const TCP_RX_BUFFER_SIZE: usize = 8192;
 const TCP_TX_BUFFER_SIZE: usize = 8192;
@@ -36,7 +36,7 @@ const APP: () = {
         // Format: iir_state[ch][cascade-no][coeff]
         #[init([[iir::Vec5([0.; 5]); IIR_CASCADE_LENGTH]; 2])]
         iir_state: [[iir::Vec5; IIR_CASCADE_LENGTH]; 2],
-        #[init([[iir::IIR { ba: iir::Vec5([1., 0., 0., 0., 0.]), y_offset: 0., y_min: -SCALE, y_max: SCALE }; IIR_CASCADE_LENGTH]; 2])]
+        #[init([[iir::IIR::new(1., -SCALE, SCALE); IIR_CASCADE_LENGTH]; 2])]
         iir_ch: [[iir::IIR; IIR_CASCADE_LENGTH]; 2],
     }
 
diff --git a/src/bin/lockin-external.rs b/src/bin/lockin-external.rs
index 72d3fc9..2e7f012 100644
--- a/src/bin/lockin-external.rs
+++ b/src/bin/lockin-external.rs
@@ -21,7 +21,7 @@ use hardware::{
     Adc0Input, Adc1Input, Dac0Output, Dac1Output, InputStamper, AFE0, AFE1,
 };
 
-const SCALE: f32 = i16::MAX as f32;
+const SCALE: f32 = i16::MAX as _;
 
 const TCP_RX_BUFFER_SIZE: usize = 8192;
 const TCP_TX_BUFFER_SIZE: usize = 8192;
@@ -40,7 +40,7 @@ const APP: () = {
         // Format: iir_state[ch][cascade-no][coeff]
         #[init([[iir::Vec5([0.; 5]); IIR_CASCADE_LENGTH]; 2])]
         iir_state: [[iir::Vec5; IIR_CASCADE_LENGTH]; 2],
-        #[init([[iir::IIR { ba: iir::Vec5([1., 0., 0., 0., 0.]), y_offset: 0., y_min: -SCALE, y_max: SCALE }; IIR_CASCADE_LENGTH]; 2])]
+        #[init([[iir::IIR::new(1./(1 << 16) as f32, -SCALE, SCALE); IIR_CASCADE_LENGTH]; 2])]
         iir_ch: [[iir::IIR; IIR_CASCADE_LENGTH]; 2],
 
         timestamper: InputStamper,

From f9b5d29450ad1bfebff395755e6a1ed6c1924565 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Robert=20J=C3=B6rdens?= <rj@quartiq.de>
Date: Mon, 1 Feb 2021 18:14:09 +0100
Subject: [PATCH 49/71] lockin: de-nest processing flow

---
 src/bin/lockin-external.rs | 60 ++++++++++++++++++++++----------------
 src/bin/lockin-internal.rs | 16 +++++-----
 2 files changed, 43 insertions(+), 33 deletions(-)

diff --git a/src/bin/lockin-external.rs b/src/bin/lockin-external.rs
index 2e7f012..f3717bb 100644
--- a/src/bin/lockin-external.rs
+++ b/src/bin/lockin-external.rs
@@ -119,21 +119,23 @@ const APP: () = {
 
         let (pll_phase, pll_frequency) = c.resources.pll.update(
             c.resources.timestamper.latest_timestamp().map(|t| t as i32),
-            23, // relative PLL frequency bandwidth: 2**-23, TODO: expose
-            22, // relative PLL phase bandwidth: 2**-22, TODO: expose
+            22, // frequency settling time (log2 counter cycles), TODO: expose
+            22, // phase settling time, TODO: expose
         );
 
         // Harmonic index of the LO: -1 to _de_modulate the fundamental (complex conjugate)
-        let harmonic: i32 = -1;
-        // Demodulation LO phase offset
-        let phase_offset: i32 = 0;
-        let sample_frequency = ((pll_frequency >> SAMPLE_BUFFER_SIZE_LOG2)
-            as i32) // TODO: maybe rounding bias
+        let harmonic: i32 = -1; // TODO: expose
+                                // Demodulation LO phase offset
+        let phase_offset: i32 = 0; // TODO: expose
+
+        let sample_frequency = ((pll_frequency
+            // .wrapping_add(1 << SAMPLE_BUFFER_SIZE_LOG2 - 1)  // half-up rounding bias
+            >> SAMPLE_BUFFER_SIZE_LOG2) as i32)
             .wrapping_mul(harmonic);
         let sample_phase =
             phase_offset.wrapping_add(pll_phase.wrapping_mul(harmonic));
 
-        if let Some(output) = adc_samples[0]
+        let output = adc_samples[0]
             .iter()
             .zip(Accu::new(sample_phase, sample_frequency))
             // Convert to signed, MSB align the ADC sample.
@@ -141,27 +143,35 @@ const APP: () = {
                 lockin.update((sample as i16 as i32) << 16, phase)
             })
             .last()
-        {
+            .unwrap();
+
+        // convert i/q to power/phase,
+        let power_phase = true; // TODO: expose
+
+        let mut output = if power_phase {
             // Convert from IQ to power and phase.
-            let mut power = output.abs_sqr() as _;
-            let mut phase = output.arg() as _;
+            [output.abs_sqr() as _, output.arg() as _]
+        } else {
+            [output.0 as _, output.1 as _]
+        };
 
-            // Filter power and phase through IIR filters.
-            // Note: Normalization to be done in filters. Phase will wrap happily.
-            for j in 0..iir_state[0].len() {
-                power = iir_ch[0][j].update(&mut iir_state[0][j], power);
-                phase = iir_ch[1][j].update(&mut iir_state[1][j], phase);
+        // Filter power and phase through IIR filters.
+        // Note: Normalization to be done in filters. Phase will wrap happily.
+        for j in 0..iir_state[0].len() {
+            for k in 0..output.len() {
+                output[k] =
+                    iir_ch[k][j].update(&mut iir_state[k][j], output[k]);
             }
+        }
 
-            // Note(unsafe): range clipping to i16 is ensured by IIR filters above.
-            // Convert to DAC data.
-            for i in 0..dac_samples[0].len() {
-                unsafe {
-                    dac_samples[0][i] =
-                        power.to_int_unchecked::<i16>() as u16 ^ 0x8000;
-                    dac_samples[1][i] =
-                        phase.to_int_unchecked::<i16>() as u16 ^ 0x8000;
-                }
+        // Note(unsafe): range clipping to i16 is ensured by IIR filters above.
+        // Convert to DAC data.
+        for i in 0..dac_samples[0].len() {
+            unsafe {
+                dac_samples[0][i] =
+                    output[0].to_int_unchecked::<i16>() as u16 ^ 0x8000;
+                dac_samples[1][i] =
+                    output[1].to_int_unchecked::<i16>() as u16 ^ 0x8000;
             }
         }
     }
diff --git a/src/bin/lockin-internal.rs b/src/bin/lockin-internal.rs
index c8001e9..0529220 100644
--- a/src/bin/lockin-internal.rs
+++ b/src/bin/lockin-internal.rs
@@ -95,7 +95,7 @@ const APP: () = {
         let sample_phase = phase_offset
             .wrapping_add((pll_phase as i32).wrapping_mul(harmonic));
 
-        if let Some(output) = adc_samples
+        let output = adc_samples
             .iter()
             // Zip in the LO phase.
             .zip(Accu::new(sample_phase, sample_frequency))
@@ -105,14 +105,14 @@ const APP: () = {
             })
             // Decimate
             .last()
-        {
-            // Convert from IQ to power and phase.
-            let _power = output.abs_sqr();
-            let phase = output.arg() >> 16;
+            .unwrap();
 
-            for value in dac_samples[1].iter_mut() {
-                *value = phase as u16 ^ 0x8000;
-            }
+        // Convert from IQ to power and phase.
+        let _power = output.abs_sqr();
+        let phase = output.arg() >> 16;
+
+        for value in dac_samples[1].iter_mut() {
+            *value = phase as u16 ^ 0x8000;
         }
     }
 

From 9ee60824d4d0bace4078dfd6b57f186685b10dfc Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Robert=20J=C3=B6rdens?= <rj@quartiq.de>
Date: Mon, 1 Feb 2021 18:15:51 +0100
Subject: [PATCH 50/71] lockin-internal: align processing with lockin-external

---
 src/bin/lockin-internal.rs | 14 ++++++++++----
 1 file changed, 10 insertions(+), 4 deletions(-)

diff --git a/src/bin/lockin-internal.rs b/src/bin/lockin-internal.rs
index 0529220..7a83a87 100644
--- a/src/bin/lockin-internal.rs
+++ b/src/bin/lockin-internal.rs
@@ -107,12 +107,18 @@ const APP: () = {
             .last()
             .unwrap();
 
-        // Convert from IQ to power and phase.
-        let _power = output.abs_sqr();
-        let phase = output.arg() >> 16;
+        // convert i/q to power/phase,
+        let power_phase = true; // TODO: expose
+
+        let output = if power_phase {
+            // Convert from IQ to power and phase.
+            [output.abs_sqr(), output.arg()]
+        } else {
+            [output.0, output.1]
+        };
 
         for value in dac_samples[1].iter_mut() {
-            *value = phase as u16 ^ 0x8000;
+            *value = (output[1] >> 16) as u16 ^ 0x8000;
         }
     }
 

From 5d7266abbcc68ddefe9190b7d7d2f95ff870ed95 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Robert=20J=C3=B6rdens?= <rj@quartiq.de>
Date: Mon, 1 Feb 2021 18:24:51 +0100
Subject: [PATCH 51/71] dsp: clippy

---
 dsp/src/iir.rs    | 4 ++--
 dsp/src/lockin.rs | 1 -
 2 files changed, 2 insertions(+), 3 deletions(-)

diff --git a/dsp/src/iir.rs b/dsp/src/iir.rs
index c8ef6ba..4206c2b 100644
--- a/dsp/src/iir.rs
+++ b/dsp/src/iir.rs
@@ -52,8 +52,8 @@ impl IIR {
         Self {
             ba: Vec5([gain, 0., 0., 0., 0.]),
             y_offset: 0.,
-            y_min: y_min,
-            y_max: y_max,
+            y_min,
+            y_max,
         }
     }
 
diff --git a/dsp/src/lockin.rs b/dsp/src/lockin.rs
index fcbb4c9..58670c7 100644
--- a/dsp/src/lockin.rs
+++ b/dsp/src/lockin.rs
@@ -16,7 +16,6 @@ impl Lockin {
         Self {
             iir: IIR {
                 ba,
-                ..Default::default()
             },
             state: [Vec5::default(); 2],
         }

From 2a84e3f299af7b6459b9b73cf51fd9f7aa987010 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Robert=20J=C3=B6rdens?= <rj@quartiq.de>
Date: Mon, 1 Feb 2021 18:37:05 +0100
Subject: [PATCH 52/71] dsp: remove unused code, let the compiler decide about
 inlining

---
 dsp/src/accu.rs    |  1 -
 dsp/src/complex.rs |  1 -
 dsp/src/lib.rs     | 32 --------------------------------
 3 files changed, 34 deletions(-)

diff --git a/dsp/src/accu.rs b/dsp/src/accu.rs
index 99369b2..954c35f 100644
--- a/dsp/src/accu.rs
+++ b/dsp/src/accu.rs
@@ -12,7 +12,6 @@ impl Accu {
 
 impl Iterator for Accu {
     type Item = i32;
-    #[inline]
     fn next(&mut self) -> Option<i32> {
         let s = self.state;
         self.state = self.state.wrapping_add(self.step);
diff --git a/dsp/src/complex.rs b/dsp/src/complex.rs
index 9829312..256cde2 100644
--- a/dsp/src/complex.rs
+++ b/dsp/src/complex.rs
@@ -14,7 +14,6 @@ impl Complex<i32> {
     /// Complex::<i32>::from_angle(1 << 30);  // pi/2
     /// Complex::<i32>::from_angle(-1 << 30);  // -pi/2
     /// ```
-    #[inline(always)]
     pub fn from_angle(angle: i32) -> Self {
         let (c, s) = cossin(angle);
         Self(c, s)
diff --git a/dsp/src/lib.rs b/dsp/src/lib.rs
index 74ebdbb..f42e7c7 100644
--- a/dsp/src/lib.rs
+++ b/dsp/src/lib.rs
@@ -3,38 +3,6 @@
 
 use core::ops::{Add, Mul, Neg};
 
-/// Bit shift, round up half.
-///
-/// # Arguments
-///
-/// `x` - Value to shift and round.
-/// `shift` - Number of bits to right shift `x`.
-///
-/// # Returns
-///
-/// Shifted and rounded value.
-#[inline(always)]
-pub fn shift_round(x: i32, shift: usize) -> i32 {
-    (x + (1 << (shift - 1))) >> shift
-}
-
-/// Integer division, round up half.
-///
-/// # Arguments
-///
-/// `dividend` - Value to divide.
-/// `divisor` - Value that divides the
-/// dividend. `dividend`+`divisor`-1 must be inside [i64::MIN,
-/// i64::MAX].
-///
-/// # Returns
-///
-/// Divided and rounded value.
-#[inline(always)]
-pub fn divide_round(dividend: i64, divisor: i64) -> i64 {
-    (dividend + (divisor - 1)) / divisor
-}
-
 fn abs<T>(x: T) -> T
 where
     T: PartialOrd + Default + Neg<Output = T>,

From f02d3cc95b5f1292d9887669025a8f8f6f38575a Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Robert=20J=C3=B6rdens?= <rj@quartiq.de>
Date: Mon, 1 Feb 2021 18:45:59 +0100
Subject: [PATCH 53/71] dsp: clippy

---
 dsp/src/lockin.rs | 4 +---
 dsp/src/rpll.rs   | 2 +-
 2 files changed, 2 insertions(+), 4 deletions(-)

diff --git a/dsp/src/lockin.rs b/dsp/src/lockin.rs
index 58670c7..ea80836 100644
--- a/dsp/src/lockin.rs
+++ b/dsp/src/lockin.rs
@@ -14,9 +14,7 @@ impl Lockin {
     /// Create a new Lockin with given IIR coefficients.
     pub fn new(ba: Vec5) -> Self {
         Self {
-            iir: IIR {
-                ba,
-            },
+            iir: IIR { ba },
             state: [Vec5::default(); 2],
         }
     }
diff --git a/dsp/src/rpll.rs b/dsp/src/rpll.rs
index 108464e..d4bd86f 100644
--- a/dsp/src/rpll.rs
+++ b/dsp/src/rpll.rs
@@ -66,7 +66,7 @@ impl RPLL {
             // Reference phase (1 << dt2 full turns) with gain/attenuation applied
             let p_ref = 1u32 << (32 + self.dt2 - shift_frequency);
             // Update frequency lock
-            self.ff = self.ff.wrapping_add(p_ref.wrapping_sub(p_sig) as u32);
+            self.ff = self.ff.wrapping_add(p_ref.wrapping_sub(p_sig));
             // Time in counter cycles between timestamp and "now"
             let dt = (x.wrapping_neg() & ((1 << self.dt2) - 1)) as u32;
             // Reference phase estimate "now"

From 24a4486847873d1a72bd331e5b1b4bc4188b33d1 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Robert=20J=C3=B6rdens?= <rj@quartiq.de>
Date: Mon, 1 Feb 2021 19:31:57 +0100
Subject: [PATCH 54/71] lockin-internal: rotate samples

---
 src/bin/lockin-internal.rs | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/bin/lockin-internal.rs b/src/bin/lockin-internal.rs
index 7a83a87..e14a4da 100644
--- a/src/bin/lockin-internal.rs
+++ b/src/bin/lockin-internal.rs
@@ -12,7 +12,7 @@ use stabilizer::{hardware, SAMPLE_BUFFER_SIZE, SAMPLE_BUFFER_SIZE_LOG2};
 const ONE: i16 = (0.1 * u16::MAX as f32) as _;
 const SQRT2: i16 = (ONE as f32 * 0.707) as _;
 const DAC_SEQUENCE: [i16; SAMPLE_BUFFER_SIZE] =
-    [0, SQRT2, ONE, SQRT2, 0, -SQRT2, -ONE, -SQRT2]; // TODO: rotate by -2 samples to start with ONE
+    [ONE, SQRT2, 0, -SQRT2, -ONE, -SQRT2, 0, SQRT2];
 
 #[rtic::app(device = stm32h7xx_hal::stm32, peripherals = true, monotonic = rtic::cyccnt::CYCCNT)]
 const APP: () = {
@@ -90,7 +90,7 @@ const APP: () = {
         let harmonic: i32 = -1;
 
         // Demodulation LO phase offset
-        let phase_offset: i32 = (0.7495 * i32::MAX as f32) as i32; // TODO: adapt to sequence rotation above
+        let phase_offset: i32 = (0.25 * i32::MAX as f32) as i32;
         let sample_frequency = (pll_frequency as i32).wrapping_mul(harmonic);
         let sample_phase = phase_offset
             .wrapping_add((pll_phase as i32).wrapping_mul(harmonic));

From 2144af5bcd1bff8f409ab8a6c5fa3d9015be4c92 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Robert=20J=C3=B6rdens?= <rj@quartiq.de>
Date: Mon, 1 Feb 2021 19:32:20 +0100
Subject: [PATCH 55/71] configuration: update to HITL ips

---
 src/hardware/configuration.rs | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/hardware/configuration.rs b/src/hardware/configuration.rs
index 18ca67f..3f01b27 100644
--- a/src/hardware/configuration.rs
+++ b/src/hardware/configuration.rs
@@ -510,11 +510,11 @@ pub fn setup(
         let store = unsafe { &mut NET_STORE };
 
         store.ip_addrs[0] = smoltcp::wire::IpCidr::new(
-            smoltcp::wire::IpAddress::v4(10, 0, 16, 99),
+            smoltcp::wire::IpAddress::v4(10, 34, 16, 103),
             24,
         );
 
-        let default_v4_gw = Ipv4Address::new(10, 0, 16, 1);
+        let default_v4_gw = Ipv4Address::new(10, 34, 16, 1);
         let mut routes = Routes::new(&mut store.routes_storage[..]);
         routes.add_default_ipv4_route(default_v4_gw).unwrap();
 

From e9a471bec9a7da3b4de46f3d61bf6f3afeb24937 Mon Sep 17 00:00:00 2001
From: Ilia Sergachev <ilia.sergachev@protonmail.ch>
Date: Mon, 1 Feb 2021 23:38:41 +0100
Subject: [PATCH 56/71] readme: fix source path and binary names

---
 README.md | 10 +++++-----
 1 file changed, 5 insertions(+), 5 deletions(-)

diff --git a/README.md b/README.md
index 8ee69f2..4244484 100644
--- a/README.md
+++ b/README.md
@@ -22,7 +22,7 @@
 ## Limitations/TODOs
 
 * Fixed AFE gains
-* The IP and MAC address are [hardcoded](src/main.rs)
+* The IP and MAC address are [hardcoded](src/hardware/configuration.rs)
 * Expose configurable limits
 * 100Base-T only
 * Digital IO, GPIO header, AFE header, EEM header are not handled
@@ -64,14 +64,14 @@ See https://github.com/sinara-hw/Stabilizer
 * Install the DFU USB tool (`dfu-util`)
 * Connect to the Micro USB connector below the RJ45
 * Short JC2/BOOT
-* `cargo objcopy --release --bin stabilizer -- -O binary stabilizer.bin` or `arm-none-eabi-objcopy -O binary target/thumbv7em-none-eabihf/release/stabilizer stabilizer.bin`
-* `dfu-util -a 0 -s 0x08000000:leave -D stabilizer.bin`
+* `cargo objcopy --release --bin dual-iir -- -O binary dual-iir.bin` or `arm-none-eabi-objcopy -O binary target/thumbv7em-none-eabihf/release/dual-iir dual-iir.bin`
+* `dfu-util -a 0 -s 0x08000000:leave -D dual-iir.bin`
 
 ### Using ST-Link virtual mass storage
 
-* `cargo objcopy --release --bin stabilizer -- -O binary stabilizer.bin` or `arm-none-eabi-objcopy -O binary target/thumbv7em-none-eabihf/release/stabilizer stabilizer.bin`
+* `cargo objcopy --release --bin dual-iir -- -O binary dual-iir.bin` or `arm-none-eabi-objcopy -O binary target/thumbv7em-none-eabihf/release/dual-iir dual-iir.bin`
 * Connect the ST-Link debugger
-* copy `stabilizer.bin` to the `NODE_H743ZI` USB disk
+* copy `dual-iir.bin` to the `NODE_H743ZI` USB disk
 
 ## Protocol
 

From 69755305a9e8c0006aef3094cd49ed4b02878e1a Mon Sep 17 00:00:00 2001
From: Harry Ho <hh@m-labs.hk>
Date: Tue, 2 Feb 2021 11:18:15 +0800
Subject: [PATCH 57/71] rename cargosha256.nix to differentiate the binaries *
 nixpkgs 20.09 imposes that cargoSha256 also depends on the package name,
 while
 https://git.m-labs.hk/M-Labs/nix-scripts/commit/726ee7aa82f812f40f631b7c4e7040d84a1b627a
 changes the package name from "stabilizer" to "stabilizer-dual-iir". Thus,
 the cargoSha256 value also needed to be updated.

---
 cargosha256-dual-iir.nix | 1 +
 cargosha256.nix          | 1 -
 2 files changed, 1 insertion(+), 1 deletion(-)
 create mode 100644 cargosha256-dual-iir.nix
 delete mode 100644 cargosha256.nix

diff --git a/cargosha256-dual-iir.nix b/cargosha256-dual-iir.nix
new file mode 100644
index 0000000..093bee6
--- /dev/null
+++ b/cargosha256-dual-iir.nix
@@ -0,0 +1 @@
+"1hzli0970p8w70mdzic0qgmpdnz2l8zchzf86fadnvpvr1y1yb4q"
diff --git a/cargosha256.nix b/cargosha256.nix
deleted file mode 100644
index cb6b230..0000000
--- a/cargosha256.nix
+++ /dev/null
@@ -1 +0,0 @@
-"1hz1s4hbdrxzhmxg3hvzv1n9xnz4h3l76411ch8c664csr9spv2r"

From 299b443e5f8cebbcff05d6b2e3540c6aba8d13d0 Mon Sep 17 00:00:00 2001
From: Harry Ho <hh@m-labs.hk>
Date: Tue, 2 Feb 2021 12:01:23 +0800
Subject: [PATCH 58/71] update cargosha256

---
 cargosha256-dual-iir.nix | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/cargosha256-dual-iir.nix b/cargosha256-dual-iir.nix
index 093bee6..8775167 100644
--- a/cargosha256-dual-iir.nix
+++ b/cargosha256-dual-iir.nix
@@ -1 +1 @@
-"1hzli0970p8w70mdzic0qgmpdnz2l8zchzf86fadnvpvr1y1yb4q"
+"0ysy8fg6kbblhmjyavq6pg77n21fcygwc0hvidmg2yywkhgdi348"

From 4475a2d04003c271e601051096c883308bb56197 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Robert=20J=C3=B6rdens?= <rj@quartiq.de>
Date: Tue, 2 Feb 2021 11:36:10 +0100
Subject: [PATCH 59/71] timestamping: full u32 range

The sampling timer and the timestamping timer have the same period.
The sampling interval and the batch size are powers of two.
If the timestamping timer wraps at a power of two larger than the
batch period, it will wrap in sync with the batch period.

Even if it didn't the RPLL would handle that. But it requires that the
timer wraps at the u32/i32 boundary (or be shifted left to wrap there).
---
 src/hardware/configuration.rs         |  3 +--
 src/hardware/digital_input_stamper.rs | 36 ---------------------------
 2 files changed, 1 insertion(+), 38 deletions(-)

diff --git a/src/hardware/configuration.rs b/src/hardware/configuration.rs
index 3f01b27..0b3e2fc 100644
--- a/src/hardware/configuration.rs
+++ b/src/hardware/configuration.rs
@@ -218,8 +218,7 @@ pub fn setup(
         // 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.set_period_ticks(u32::MAX);
 
         timer
     };
diff --git a/src/hardware/digital_input_stamper.rs b/src/hardware/digital_input_stamper.rs
index 4262800..9cda62b 100644
--- a/src/hardware/digital_input_stamper.rs
+++ b/src/hardware/digital_input_stamper.rs
@@ -25,42 +25,6 @@
 ///! 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};
-use crate::{ADC_SAMPLE_TICKS, SAMPLE_BUFFER_SIZE};
-
-/// Calculate the period of the digital input timestamp timer.
-///
-/// # Note
-/// The period returned will be 1 less than the required period in timer ticks. The value returned
-/// can be immediately programmed into a hardware timer period register.
-///
-/// The period is calculated to be some power-of-two multiple of the batch size, such that N batches
-/// will occur between each timestamp timer overflow.
-///
-/// # Returns
-/// A 32-bit value that can be programmed into a hardware timer period register.
-pub fn calculate_timestamp_timer_period() -> u32 {
-    // Calculate how long a single batch requires in timer ticks.
-    let batch_duration_ticks: u64 =
-        SAMPLE_BUFFER_SIZE as u64 * ADC_SAMPLE_TICKS as u64;
-
-    // Calculate the largest power-of-two that is less than or equal to
-    // `batches_per_overflow`.  This is completed by eliminating the least significant
-    // bits of the value until only the msb remains, which is always a power of two.
-    let batches_per_overflow: u64 =
-        (1u64 + u32::MAX as u64) / batch_duration_ticks;
-    let mut j = batches_per_overflow;
-    while (j & (j - 1)) != 0 {
-        j = j & (j - 1);
-    }
-
-    // Once the number of batches per timestamp overflow is calculated, we can figure out the final
-    // period of the timestamp timer. The period is always 1 larger than the value configured in the
-    // register.
-    let period: u64 = batch_duration_ticks * j - 1u64;
-    assert!(period <= u32::MAX as u64);
-
-    period as u32
-}
 
 /// The timestamper for DI0 reference clock inputs.
 pub struct InputStamper {

From 854ed29b1a19f0cca3c9bcce62361ba67e59f8f5 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Robert=20J=C3=B6rdens?= <rj@quartiq.de>
Date: Tue, 2 Feb 2021 12:34:07 +0100
Subject: [PATCH 60/71] timestamp: pass overflows to the top and ignore them
 there

---
 src/bin/lockin-external.rs            |  8 +++++++-
 src/hardware/digital_input_stamper.rs | 15 ++++++---------
 src/hardware/timers.rs                | 16 +++++++---------
 3 files changed, 20 insertions(+), 19 deletions(-)

diff --git a/src/bin/lockin-external.rs b/src/bin/lockin-external.rs
index f3717bb..1d3f11d 100644
--- a/src/bin/lockin-external.rs
+++ b/src/bin/lockin-external.rs
@@ -117,8 +117,14 @@ const APP: () = {
         let iir_state = c.resources.iir_state;
         let lockin = c.resources.lockin;
 
+        let t = c
+            .resources
+            .timestamper
+            .latest_timestamp()
+            .unwrap_or_else(|t| t) // Ignore timer capture overflows.
+            .map(|t| t as i32);
         let (pll_phase, pll_frequency) = c.resources.pll.update(
-            c.resources.timestamper.latest_timestamp().map(|t| t as i32),
+            t,
             22, // frequency settling time (log2 counter cycles), TODO: expose
             22, // phase settling time, TODO: expose
         );
diff --git a/src/hardware/digital_input_stamper.rs b/src/hardware/digital_input_stamper.rs
index 9cda62b..6bd8629 100644
--- a/src/hardware/digital_input_stamper.rs
+++ b/src/hardware/digital_input_stamper.rs
@@ -62,15 +62,12 @@ impl InputStamper {
     /// Get the latest timestamp that has occurred.
     ///
     /// # Note
-    /// This function must be called sufficiently often. If an over-capture event occurs, this
-    /// function will panic, as this indicates a timestamp was inadvertently dropped.
-    ///
-    /// 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.
+    /// This function must be called at least as often as timestamps arrive.
+    /// If an over-capture event occurs, this function will clear the overflow,
+    /// and return a new timestamp of unknown recency an `Err()`.
+    /// Note that this indicates at least one timestamp was inadvertently dropped.
     #[allow(dead_code)]
-    pub fn latest_timestamp(&mut self) -> Option<u32> {
-        self.capture_channel
-            .latest_capture()
-            .expect("DI0 timestamp overrun")
+    pub fn latest_timestamp(&mut self) -> Result<Option<u32>, Option<u32>> {
+        self.capture_channel.latest_capture()
     }
 }
diff --git a/src/hardware/timers.rs b/src/hardware/timers.rs
index 7199730..b686220 100644
--- a/src/hardware/timers.rs
+++ b/src/hardware/timers.rs
@@ -281,28 +281,26 @@ macro_rules! timer_channels {
             impl [< Channel $index InputCapture >] {
                 /// Get the latest capture from the channel.
                 #[allow(dead_code)]
-                pub fn latest_capture(&mut self) -> Result<Option<$size>, ()> {
+                pub fn latest_capture(&mut self) -> Result<Option<$size>, Option<$size>> {
                     // Note(unsafe): This channel owns all access to the specific timer channel.
                     // Only atomic operations on completed on the timer registers.
                     let regs = unsafe { &*<$TY>::ptr() };
-                    let sr = regs.sr.read();
 
-                    let result = if sr.[< cc $index if >]().bit_is_set() {
+                    let result = if regs.sr.read().[< cc $index if >]().bit_is_set() {
                         // Read the capture value. Reading the captured value clears the flag in the
                         // status register automatically.
-                        let ccx = regs.[< ccr $index >].read();
-                        Some(ccx.ccr().bits())
+                        Some(regs.[< ccr $index >].read().ccr().bits())
                     } else {
                         None
                     };
 
                     // Read SR again to check for a potential over-capture. If there is an
                     // overcapture, return an error.
-                    if regs.sr.read().[< cc $index of >]().bit_is_clear() {
-                        Ok(result)
-                    } else {
+                    if regs.sr.read().[< cc $index of >]().bit_is_set() {
                         regs.sr.modify(|_, w| w.[< cc $index of >]().clear_bit());
-                        Err(())
+                        Err(result)
+                    } else {
+                        Ok(result)
                     }
                 }
 

From e1c87c149f4ed3b76eba343703d5d3879cafb3ae Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Robert=20J=C3=B6rdens?= <rj@quartiq.de>
Date: Tue, 2 Feb 2021 13:25:45 +0100
Subject: [PATCH 61/71] timestamping_timer: also reset counter

---
 src/hardware/configuration.rs | 1 +
 1 file changed, 1 insertion(+)

diff --git a/src/hardware/configuration.rs b/src/hardware/configuration.rs
index 0b3e2fc..aa0b918 100644
--- a/src/hardware/configuration.rs
+++ b/src/hardware/configuration.rs
@@ -211,6 +211,7 @@ pub fn setup(
         // Configure the timer to count at the designed tick rate. We will manually set the
         // period below.
         timer5.pause();
+        timer5.reset_counter();
         timer5.set_tick_freq(design_parameters::TIMER_FREQUENCY);
 
         // The timestamp timer must run at exactly a multiple of the sample timer based on the

From ddbfa9d98831edd86ace42301a28481bc231045e Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Robert=20J=C3=B6rdens?= <rj@quartiq.de>
Date: Tue, 2 Feb 2021 14:34:48 +0100
Subject: [PATCH 62/71] timestamping: docs and naming

---
 src/bin/lockin-external.rs    | 4 ++--
 src/hardware/configuration.rs | 7 +++----
 2 files changed, 5 insertions(+), 6 deletions(-)

diff --git a/src/bin/lockin-external.rs b/src/bin/lockin-external.rs
index 1d3f11d..0933f17 100644
--- a/src/bin/lockin-external.rs
+++ b/src/bin/lockin-external.rs
@@ -117,14 +117,14 @@ const APP: () = {
         let iir_state = c.resources.iir_state;
         let lockin = c.resources.lockin;
 
-        let t = c
+        let timestamp = c
             .resources
             .timestamper
             .latest_timestamp()
             .unwrap_or_else(|t| t) // Ignore timer capture overflows.
             .map(|t| t as i32);
         let (pll_phase, pll_frequency) = c.resources.pll.update(
-            t,
+            timestamp,
             22, // frequency settling time (log2 counter cycles), TODO: expose
             22, // phase settling time, TODO: expose
         );
diff --git a/src/hardware/configuration.rs b/src/hardware/configuration.rs
index aa0b918..cbe4a1d 100644
--- a/src/hardware/configuration.rs
+++ b/src/hardware/configuration.rs
@@ -211,12 +211,11 @@ pub fn setup(
         // Configure the timer to count at the designed tick rate. We will manually set the
         // period below.
         timer5.pause();
-        timer5.reset_counter();
         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.
+        // The timestamp timer runs at the counter cycle period as the sampling timers.
+        // To accomodate this, we manually set the prescaler identical to the sample
+        // timer, but use maximum overflow period.
         let mut timer = timers::TimestampTimer::new(timer5);
 
         timer.set_period_ticks(u32::MAX);

From dcc71d5d11ab93acf9dad1ce8a3ed79d738dc295 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Robert=20J=C3=B6rdens?= <rj@quartiq.de>
Date: Tue, 2 Feb 2021 15:41:47 +0100
Subject: [PATCH 63/71] iir: tweak math a bit

---
 dsp/src/iir_int.rs | 5 +++--
 1 file changed, 3 insertions(+), 2 deletions(-)

diff --git a/dsp/src/iir_int.rs b/dsp/src/iir_int.rs
index 93626de..daff43c 100644
--- a/dsp/src/iir_int.rs
+++ b/dsp/src/iir_int.rs
@@ -22,8 +22,9 @@ impl Vec5 {
     pub fn lowpass(f: f32, q: f32, k: f32) -> Self {
         // 3rd order Taylor approximation of sin and cos.
         let f = f * 2. * PI;
-        let fsin = f - f * f * f / 6.;
-        let fcos = 1. - f * f / 2.;
+        let f2 = f * f * 0.5;
+        let fcos = 1. - f2;
+        let fsin = f * (1. - f2 / 3.);
         let alpha = fsin / (2. * q);
         // IIR uses Q2.30 fixed point
         let a0 = (1. + alpha) / (1 << IIR::SHIFT) as f32;

From 145b48074eed1aa979be6a013edd3feb69dd8b5e Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Robert=20J=C3=B6rdens?= <rj@quartiq.de>
Date: Tue, 2 Feb 2021 15:42:51 +0100
Subject: [PATCH 64/71] timers: remove spurious tim2 reset

---
 src/hardware/configuration.rs | 1 -
 1 file changed, 1 deletion(-)

diff --git a/src/hardware/configuration.rs b/src/hardware/configuration.rs
index cbe4a1d..acd98ae 100644
--- a/src/hardware/configuration.rs
+++ b/src/hardware/configuration.rs
@@ -154,7 +154,6 @@ pub fn setup(
         // 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);

From bd71136cdf2f096e460b12737ac68e2a2cf5d0a2 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Robert=20J=C3=B6rdens?= <rj@quartiq.de>
Date: Tue, 2 Feb 2021 15:46:50 +0100
Subject: [PATCH 65/71] hw/config: add TODO on synchronization

---
 src/hardware/configuration.rs | 3 +++
 1 file changed, 3 insertions(+)

diff --git a/src/hardware/configuration.rs b/src/hardware/configuration.rs
index acd98ae..e173dc7 100644
--- a/src/hardware/configuration.rs
+++ b/src/hardware/configuration.rs
@@ -217,6 +217,9 @@ pub fn setup(
         // timer, but use maximum overflow period.
         let mut timer = timers::TimestampTimer::new(timer5);
 
+        // TODO: Check hardware synchronization of timestamping and the sampling timers
+        // for phase shift determinism.
+
         timer.set_period_ticks(u32::MAX);
 
         timer

From e423eff0e2c290af51c9ed73d871a85bfb32453d Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Robert=20J=C3=B6rdens?= <rj@quartiq.de>
Date: Tue, 2 Feb 2021 15:50:31 +0100
Subject: [PATCH 66/71] lockin-external: add doc

---
 src/bin/lockin-external.rs | 21 +++++----------------
 1 file changed, 5 insertions(+), 16 deletions(-)

diff --git a/src/bin/lockin-external.rs b/src/bin/lockin-external.rs
index 0933f17..11d4958 100644
--- a/src/bin/lockin-external.rs
+++ b/src/bin/lockin-external.rs
@@ -83,24 +83,13 @@ const APP: () = {
         }
     }
 
-    /// Main DSP processing routine for Stabilizer.
+    /// Main DSP processing routine.
     ///
-    /// # Note
-    /// Processing time for the DSP application code is bounded by the following constraints:
+    /// See `dual-iir` for general notes on processing time and timing.
     ///
-    /// 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.
-    ///
-    /// TODO: document lockin
+    /// This is an implementation of a externally (DI0) referenced PLL lockin on the ADC0 signal.
+    /// It outputs either I/Q or power/phase on DAC0/DAC1. Data is normalized to full scale.
+    /// PLL bandwidth, filter bandwidth, slope, and x/y or power/phase post-filters are available.
     #[task(binds=DMA1_STR4, resources=[adcs, dacs, iir_state, iir_ch, lockin, timestamper, pll], priority=2)]
     fn process(c: process::Context) {
         let adc_samples = [

From e76707208983456896aebcb701758ec575cd950b Mon Sep 17 00:00:00 2001
From: Ryan Summers <ryan.summers@vertigo-designs.com>
Date: Tue, 2 Feb 2021 17:33:58 +0100
Subject: [PATCH 67/71] Adding support for cargo-embed

---
 README.md | 5 +++++
 1 file changed, 5 insertions(+)

diff --git a/README.md b/README.md
index 4244484..7f0967b 100644
--- a/README.md
+++ b/README.md
@@ -40,6 +40,11 @@ See https://github.com/sinara-hw/Stabilizer
 * `cargo build --release`
 * Do not try the debug (default) mode. It is guaranteed to panic.
 
+### Using Cargo-embed
+
+* Install `cargo-embed`: `cargo install cargo-embed`
+* Program the device: `cargo embed --bin dual-iir --release`
+
 ### Using GDB/OpenOCD
 
 * Get a recent openocd, a JTAG adapter ("st-link" or some clone) and

From fd54fe384e8d2b38e172cab17529dd9f2d2e38e9 Mon Sep 17 00:00:00 2001
From: Ryan Summers <ryan.summers@vertigo-designs.com>
Date: Tue, 2 Feb 2021 17:34:49 +0100
Subject: [PATCH 68/71] Adding Embed.toml file

---
 .gitignore | 1 +
 Embed.toml | 3 +++
 2 files changed, 4 insertions(+)
 create mode 100644 Embed.toml

diff --git a/.gitignore b/.gitignore
index c024ef6..5daea61 100644
--- a/.gitignore
+++ b/.gitignore
@@ -1,2 +1,3 @@
 /target
 openocd.gdb
+Embed.toml
diff --git a/Embed.toml b/Embed.toml
new file mode 100644
index 0000000..8a34951
--- /dev/null
+++ b/Embed.toml
@@ -0,0 +1,3 @@
+[default.general]
+chip = "STM32H743ZITx"
+connect_under_reset = true

From bf97b5972d251f17549212cd5332ba4870e2b36c Mon Sep 17 00:00:00 2001
From: Ryan Summers <summers.ryan.m@gmail.com>
Date: Tue, 2 Feb 2021 18:03:46 +0100
Subject: [PATCH 69/71] Update .gitignore
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

Co-authored-by: Robert Jördens <rj@quartiq.de>
---
 .gitignore | 2 --
 1 file changed, 2 deletions(-)

diff --git a/.gitignore b/.gitignore
index 5daea61..ea8c4bf 100644
--- a/.gitignore
+++ b/.gitignore
@@ -1,3 +1 @@
 /target
-openocd.gdb
-Embed.toml

From c5fde8563c36e7faeb1ce24d708bd02eb1661fe5 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Robert=20J=C3=B6rdens?= <rj@quartiq.de>
Date: Wed, 3 Feb 2021 13:16:22 +0100
Subject: [PATCH 70/71] deps: bump hal and smoltcp, adapt

---
 Cargo.lock          | 15 ++-------------
 src/hardware/adc.rs |  7 ++++++-
 src/hardware/dac.rs |  3 ++-
 src/hardware/mod.rs |  2 --
 4 files changed, 10 insertions(+), 17 deletions(-)

diff --git a/Cargo.lock b/Cargo.lock
index ee50a9a..ed15a58 100644
--- a/Cargo.lock
+++ b/Cargo.lock
@@ -954,17 +954,6 @@ dependencies = [
  "serde",
 ]
 
-[[package]]
-name = "smoltcp"
-version = "0.6.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "0fe46639fd2ec79eadf8fe719f237a7a0bd4dac5d957f1ca5bbdbc1c3c39e53a"
-dependencies = [
- "bitflags",
- "byteorder",
- "managed",
-]
-
 [[package]]
 name = "smoltcp"
 version = "0.7.0"
@@ -998,7 +987,7 @@ dependencies = [
  "paste",
  "serde",
  "serde-json-core",
- "smoltcp 0.7.0",
+ "smoltcp",
  "stm32h7xx-hal",
 ]
 
@@ -1033,7 +1022,7 @@ dependencies = [
  "embedded-hal",
  "nb 1.0.0",
  "paste",
- "smoltcp 0.7.0",
+ "smoltcp",
  "stm32h7",
  "void",
 ]
diff --git a/src/hardware/adc.rs b/src/hardware/adc.rs
index 188e436..1cb6c17 100644
--- a/src/hardware/adc.rs
+++ b/src/hardware/adc.rs
@@ -180,18 +180,21 @@ macro_rules! adc_input {
                     hal::spi::Spi<hal::stm32::$spi, hal::spi::Disabled, u16>,
                     PeripheralToMemory,
                     &'static mut [u16; SAMPLE_BUFFER_SIZE],
+                    hal::dma::DBTransfer,
                 >,
                 trigger_transfer: Transfer<
                     hal::dma::dma::$trigger_stream<hal::stm32::DMA1>,
                     [< $spi CR >],
                     MemoryToPeripheral,
                     &'static mut [u32; 1],
+                    hal::dma::DBTransfer,
                 >,
                 clear_transfer: Transfer<
                     hal::dma::dma::$clear_stream<hal::stm32::DMA1>,
                     [< $spi IFCR >],
                     MemoryToPeripheral,
                     &'static mut [u32; 1],
+                    hal::dma::DBTransfer,
                 >,
             }
 
@@ -239,6 +242,7 @@ macro_rules! adc_input {
                         _,
                         MemoryToPeripheral,
                         _,
+                        _,
                     > = Transfer::init(
                         clear_stream,
                         [< $spi IFCR >]::new(clear_channel),
@@ -276,6 +280,7 @@ macro_rules! adc_input {
                         _,
                         MemoryToPeripheral,
                         _,
+                        _,
                     > = Transfer::init(
                         trigger_stream,
                         [< $spi CR >]::new(trigger_channel),
@@ -306,7 +311,7 @@ macro_rules! adc_input {
 
                     // The data transfer is always a transfer of data from the peripheral to a RAM
                     // buffer.
-                    let data_transfer: Transfer<_, _, PeripheralToMemory, _> =
+                    let data_transfer: Transfer<_, _, PeripheralToMemory, _, _> =
                         Transfer::init(
                             data_stream,
                             spi,
diff --git a/src/hardware/dac.rs b/src/hardware/dac.rs
index 09d0497..5ca65fb 100644
--- a/src/hardware/dac.rs
+++ b/src/hardware/dac.rs
@@ -122,6 +122,7 @@ macro_rules! dac_output {
                 $spi,
                 MemoryToPeripheral,
                 &'static mut [u16; SAMPLE_BUFFER_SIZE],
+                hal::dma::DBTransfer,
             >,
         }
 
@@ -164,7 +165,7 @@ macro_rules! dac_output {
                 }
 
                 // Construct the trigger stream to write from memory to the peripheral.
-                let transfer: Transfer<_, _, MemoryToPeripheral, _> =
+                let transfer: Transfer<_, _, MemoryToPeripheral, _, _> =
                     Transfer::init(
                         stream,
                         $spi::new(trigger_channel, spi),
diff --git a/src/hardware/mod.rs b/src/hardware/mod.rs
index dc3aa25..f912057 100644
--- a/src/hardware/mod.rs
+++ b/src/hardware/mod.rs
@@ -37,8 +37,6 @@ pub type AFE1 = afe::ProgrammableGainAmplifier<
 
 // Type alias for the ethernet interface on Stabilizer.
 pub type Ethernet = smoltcp::iface::EthernetInterface<
-    'static,
-    'static,
     'static,
     hal::ethernet::EthernetDMA<'static>,
 >;

From 672ddfa3c3df5530150b388a4c370478c8fe6233 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Robert=20J=C3=B6rdens?= <rj@quartiq.de>
Date: Wed, 3 Feb 2021 13:25:00 +0100
Subject: [PATCH 71/71] pounder: also adapt to new hal

---
 src/hardware/pounder/timestamp.rs | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/src/hardware/pounder/timestamp.rs b/src/hardware/pounder/timestamp.rs
index 5ccf235..e97bc2c 100644
--- a/src/hardware/pounder/timestamp.rs
+++ b/src/hardware/pounder/timestamp.rs
@@ -43,6 +43,7 @@ pub struct Timestamper {
         timers::tim8::Channel1InputCapture,
         PeripheralToMemory,
         &'static mut [u16; SAMPLE_BUFFER_SIZE],
+        hal::dma::DBTransfer,
     >,
 }
 
@@ -89,7 +90,7 @@ impl Timestamper {
         input_capture.listen_dma();
 
         // The data transfer is always a transfer of data from the peripheral to a RAM buffer.
-        let data_transfer: Transfer<_, _, PeripheralToMemory, _> =
+        let data_transfer: Transfer<_, _, PeripheralToMemory, _, _> =
             Transfer::init(
                 stream,
                 input_capture,