From a97829baf54e2ddd6fc182f114bd5abc3a4b8e79 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Robert=20J=C3=B6rdens?= <rj@quartiq.de>
Date: Thu, 18 Feb 2021 22:14:21 +0100
Subject: [PATCH] add saturating_scale

---
 dsp/src/lib.rs | 17 +++++++++++++++++
 1 file changed, 17 insertions(+)

diff --git a/dsp/src/lib.rs b/dsp/src/lib.rs
index 359eee1..f3af4d4 100644
--- a/dsp/src/lib.rs
+++ b/dsp/src/lib.rs
@@ -80,6 +80,23 @@ where
         .fold(y0, |y, xa| y + xa)
 }
 
+/// 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 {
+        i32::MAX
+    } else {
+        (lo >> shift) + (hi << shift_hi)
+    }
+}
+
 mod atan2;
 pub use atan2::*;
 mod accu;