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] 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))
     }
 }