From d5b64d38ada48c5c3d5898b4d52f0e6e4e4136c5 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Robert=20J=C3=B6rdens?= <rj@quartiq.de>
Date: Wed, 17 Jan 2024 12:35:08 +0100
Subject: [PATCH] Dsm: add delta sigma modulator

---
 CHANGELOG.md |  6 ++++++
 README.md    |  4 ++++
 src/dsm.rs   | 55 ++++++++++++++++++++++++++++++++++++++++++++++++++++
 src/lib.rs   |  2 ++
 4 files changed, 67 insertions(+)
 create mode 100644 src/dsm.rs

diff --git a/CHANGELOG.md b/CHANGELOG.md
index 95c1445..9672ff7 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -5,6 +5,12 @@ All notable changes to this project will be documented in this file.
 The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/),
 and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0.html).
 
+## [Unreleased](https://github.com/quartiq/idsp/compare/v0.14.1..main) - 2024-01-15
+
+### Added
+
+* `Dsm`: Delta sigma modulator/noise shaper in MASH-(1)^K architecture.
+
 ## [0.14.1](https://github.com/quartiq/idsp/compare/v0.14.0..v0.14.1) - 2024-01-15
 
 * Fixed changelog
diff --git a/README.md b/README.md
index f8ec790..10aee67 100644
--- a/README.md
+++ b/README.md
@@ -90,3 +90,7 @@ bandpass, and notch filtering of a signal.
 Fast `f32` symmetric FIR filters, optimized half-band filters, half-band filter decimators and integators and cascades.
 These are used in [`stabilizer-stream`](https://github.com/quartiq/stabilizer-stream) for online PSD calculation on log
 frequency scale for arbitrarily large amounts of data.
+
+## Delta Sigma modulator/noise shaper
+
+[`Dsm`] is a delta sigma modulator/noise shaper in MASH-(1)^K architecture.
diff --git a/src/dsm.rs b/src/dsm.rs
new file mode 100644
index 0000000..7a9a4fd
--- /dev/null
+++ b/src/dsm.rs
@@ -0,0 +1,55 @@
+/// Delta-sigma modulator
+///
+/// * MASH-(1)^K architecture
+/// * `0 <= K <= 8` (`K=0` is valid but the output will be the constant quantized 0)
+/// * The output range is `1 - (1 << K - 1)..=(1 << K - 1)`.
+/// * Given constant input `x0`, the average output is `x0/(1 << 32)`.
+/// * The noise goes up as `K * 20 dB/decade`.
+///
+/// ```
+/// # use idsp::Dsm;
+/// let mut d = Dsm::<3>::default();
+/// let x = 0x87654321;
+/// let n = 1 << 20;
+/// let y = (0..n).map(|_| d.update(x) as f32).sum::<f32>() / n as f32;
+/// let m = x as f32 / (1u64 << 32) as f32;
+/// assert!((y / m - 1.0).abs() < (1.0 / n as f32).sqrt(), "{y} != {m}");
+/// ```
+#[derive(Copy, Clone, Debug, PartialEq, Eq, PartialOrd)]
+pub struct Dsm<const K: usize> {
+    a: [u32; K],
+    c: [i8; K],
+}
+
+impl<const K: usize> Default for Dsm<K> {
+    fn default() -> Self {
+        Self {
+            a: [0; K],
+            c: [0; K],
+        }
+    }
+}
+
+impl<const K: usize> Dsm<K> {
+    /// Ingest input sample, emit new output.
+    ///
+    /// # Arguments
+    /// * `x`: New input sample
+    ///
+    /// # Returns
+    /// New output
+    pub fn update(&mut self, x: u32) -> i8 {
+        let mut d = 0i8;
+        let mut c = false;
+        self.a.iter_mut().fold(x, |x, a| {
+            (*a, c) = a.overflowing_add(x);
+            d = (d << 1) | c as i8;
+            *a
+        });
+        self.c.iter_mut().take(K - 1).fold(d & 1, |mut y, c| {
+            d >>= 1;
+            (y, *c) = ((d & 1) + y - *c, y);
+            y
+        })
+    }
+}
diff --git a/src/lib.rs b/src/lib.rs
index 78a72a7..6fa71cd 100644
--- a/src/lib.rs
+++ b/src/lib.rs
@@ -29,7 +29,9 @@ pub use unwrap::*;
 pub mod hbf;
 mod num;
 pub use num::*;
+mod dsm;
 pub mod svf;
+pub use dsm::*;
 
 #[cfg(test)]
 pub mod testing;