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