From cc42c0c477c03293994692ff8f072c643b1f2f11 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Robert=20J=C3=B6rdens?= <rj@quartiq.de>
Date: Tue, 22 Dec 2020 16:49:12 +0100
Subject: [PATCH] iir_int: add optimized integer iir implementation

---
 dsp/src/iir_int.rs | 58 ++++++++++++++++++++++++++++++++++++++++++++++
 dsp/src/lib.rs     |  1 +
 2 files changed, 59 insertions(+)
 create mode 100644 dsp/src/iir_int.rs

diff --git a/dsp/src/iir_int.rs b/dsp/src/iir_int.rs
new file mode 100644
index 0000000..1a4a6a9
--- /dev/null
+++ b/dsp/src/iir_int.rs
@@ -0,0 +1,58 @@
+use serde::{Deserialize, Serialize};
+
+pub type IIRState = [i32; 5];
+
+fn macc(y0: i32, x: &[i32], a: &[i32], shift: u32) -> i32 {
+    // Rounding bias, half up
+    let y0 = ((y0 as i64) << shift) + (1 << (shift - 1));
+    let y = x
+        .iter()
+        .zip(a)
+        .map(|(x, a)| *x as i64 * *a as i64)
+        .fold(y0, |y, xa| y + xa);
+    (y >> shift) as i32
+}
+
+/// Integer biquad IIR
+///
+/// See `dsp::iir::IIR` for general implementation details.
+/// Offset and limiting disabled to suit lowpass applications.
+/// Coefficient scaling fixed and optimized.
+#[derive(Copy, Clone, Deserialize, Serialize)]
+pub struct IIR {
+    pub ba: IIRState,
+    // pub y_offset: i32,
+    // pub y_min: i32,
+    // pub y_max: i32,
+}
+
+impl IIR {
+    /// Coefficient fixed point: signed Q2.30.
+    /// Tailored to low-passes PI, II etc.
+    const SHIFT: u32 = 30;
+
+    /// Feed a new input value into the filter, update the filter state, and
+    /// return the new output. Only the state `xy` is modified.
+    ///
+    /// # Arguments
+    /// * `xy` - Current filter state.
+    /// * `x0` - New input.
+    pub fn update(&self, xy: &mut IIRState, x0: i32) -> i32 {
+        let n = self.ba.len();
+        debug_assert!(xy.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);
+        // Store x0            x0 x1 x2 y1 y2
+        xy[0] = x0;
+        // Compute y0 by multiply-accumulate
+        let y0 = macc(0, xy, &self.ba, IIR::SHIFT);
+        // Limit y0
+        // let y0 = y0.max(self.y_min).min(self.y_max);
+        // Store y0            x0 x1 y0 y1 y2
+        xy[n / 2] = y0;
+        y0
+    }
+}
diff --git a/dsp/src/lib.rs b/dsp/src/lib.rs
index 67b1882..fb189fa 100644
--- a/dsp/src/lib.rs
+++ b/dsp/src/lib.rs
@@ -98,6 +98,7 @@ where
 }
 
 pub mod iir;
+pub mod iir_int;
 pub mod lockin;
 pub mod pll;
 pub mod trig;