From 21ab98823985127d97bdda25c377880ee1e0a051 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Robert=20J=C3=B6rdens?= <rj@quartiq.de>
Date: Thu, 6 May 2021 10:10:59 +0200
Subject: [PATCH 1/3] atan2: tweak for speed

---
 dsp/src/atan2.rs | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/dsp/src/atan2.rs b/dsp/src/atan2.rs
index 6d6e476..bfe33db 100644
--- a/dsp/src/atan2.rs
+++ b/dsp/src/atan2.rs
@@ -77,6 +77,8 @@ pub fn atan2(y: i32, x: i32) -> i32 {
 
     if sign.1 {
         angle = angle.wrapping_neg();
+        // Negation ends up in slightly faster assembly
+        // angle = !angle;
     }
 
     angle

From 2ba49258f98c6bde25d20020ea5f3ebfb768dffb Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Robert=20J=C3=B6rdens?= <rj@quartiq.de>
Date: Sat, 29 May 2021 22:50:37 +0200
Subject: [PATCH 2/3] dsp/staurating_scale: fix math

---
 dsp/src/tools.rs | 14 +++++---------
 1 file changed, 5 insertions(+), 9 deletions(-)

diff --git a/dsp/src/tools.rs b/dsp/src/tools.rs
index 6fc2b12..2337159 100644
--- a/dsp/src/tools.rs
+++ b/dsp/src/tools.rs
@@ -91,16 +91,12 @@ pub fn macc_i32(y0: i32, x: &[i32], a: &[i32], shift: u32) -> i32 {
 /// Combine high and low i32 into a single downscaled i32, saturating the type.
 pub fn saturating_scale(lo: i32, hi: i32, shift: u32) -> i32 {
     debug_assert!(shift & 31 == shift);
-
-    let shift_hi = 31 - shift;
-    debug_assert!(shift_hi & 31 == shift_hi);
-
-    let over = hi >> shift;
-    if over < -1 {
-        i32::MIN
-    } else if over > 0 {
+    let scale = -1 << shift;
+    if hi <= scale {
+        -i32::MAX
+    } else if -hi <= scale {
         i32::MAX
     } else {
-        (lo >> shift) + (hi << shift_hi)
+        (lo >> shift) + (hi << (31 - shift))
     }
 }

From fb4ed888ee83a50ead941374c8a78466f5f8a559 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Robert=20J=C3=B6rdens?= <rj@quartiq.de>
Date: Sun, 30 May 2021 20:12:02 +0200
Subject: [PATCH 3/3] dsp/saturating_scale: fix range

---
 dsp/src/tools.rs  | 13 -------------
 dsp/src/unwrap.rs | 21 +++++++++++++++++++++
 2 files changed, 21 insertions(+), 13 deletions(-)

diff --git a/dsp/src/tools.rs b/dsp/src/tools.rs
index 2337159..9fba214 100644
--- a/dsp/src/tools.rs
+++ b/dsp/src/tools.rs
@@ -87,16 +87,3 @@ pub fn macc_i32(y0: i32, x: &[i32], a: &[i32], shift: u32) -> i32 {
         .fold(y0, |y, xa| y + xa);
     (y >> shift) as i32
 }
-
-/// Combine high and low i32 into a single downscaled i32, saturating the type.
-pub fn saturating_scale(lo: i32, hi: i32, shift: u32) -> i32 {
-    debug_assert!(shift & 31 == shift);
-    let scale = -1 << shift;
-    if hi <= scale {
-        -i32::MAX
-    } else if -hi <= scale {
-        i32::MAX
-    } else {
-        (lo >> shift) + (hi << (31 - shift))
-    }
-}
diff --git a/dsp/src/unwrap.rs b/dsp/src/unwrap.rs
index 870629e..68ddd0c 100644
--- a/dsp/src/unwrap.rs
+++ b/dsp/src/unwrap.rs
@@ -16,6 +16,27 @@ pub fn overflowing_sub(y: i32, x: i32) -> (i32, i8) {
     (delta, wrap)
 }
 
+/// Combine high and low i32 into a single downscaled i32, saturating monotonically.
+///
+/// Args:
+/// `lo`: LSB i32 to scale down by `shift` and range-extend with `hi`
+/// `hi`: MSB i32 to scale up and extend `lo` with. Output will be clipped if
+///     `hi` exceeds the output i32 range.
+/// `shift`: Downscale `lo` by that many bits. Values from 1 to 32 inclusive
+///     are valid.
+pub fn saturating_scale(lo: i32, hi: i32, shift: u32) -> i32 {
+    debug_assert!(shift > 0);
+    debug_assert!(shift <= 32);
+    let hi_range = -1 << (shift - 1);
+    if hi <= hi_range {
+        i32::MIN - hi_range
+    } else if -hi <= hi_range {
+        hi_range - i32::MIN
+    } else {
+        (lo >> shift) + (hi << (32 - shift))
+    }
+}
+
 /// Overflow unwrapper.
 ///
 /// This is unwrapping as in the phase and overflow unwrapping context, not