embedded_dsp/
resampling.rs

1//! Multi-rate digital signal processing routines: Cascaded Integrator-Comb (CIC) decimation/interpolation and linear fractional resampling.
2
3/// Cascaded Integrator-Comb (CIC) Decimator for downsampling signals in integer arithmetic.
4pub struct CicDecimator<const STAGES: usize> {
5    r: usize, // Decimation factor
6    integrator_state: [i32; STAGES],
7    comb_state: [i32; STAGES],
8    sample_counter: usize,
9}
10
11impl<const STAGES: usize> CicDecimator<STAGES> {
12    /// Initialise a new CIC decimator with decimation factor `r`.
13    pub fn new(r: usize) -> Self {
14        Self {
15            r,
16            integrator_state: [0; STAGES],
17            comb_state: [0; STAGES],
18            sample_counter: 0,
19        }
20    }
21
22    /// Theoretical maximum DC gain: `R^STAGES`.
23    pub fn gain(&self) -> u64 {
24        let mut g: u64 = 1;
25        for _ in 0..STAGES {
26            g = g.saturating_mul(self.r as u64);
27        }
28        g
29    }
30
31    /// Number of bits of bit-growth: `ceil(log2(R^STAGES))`.
32    pub fn gain_bits(&self) -> u32 {
33        let g = self.gain();
34        if g <= 1 {
35            0
36        } else {
37            64 - (g - 1).leading_zeros()
38        }
39    }
40
41    /// Process an input sample. Returns `Some(decimated_sample)` every `R` samples.
42    pub fn process_sample(&mut self, input: i32) -> Option<i32> {
43        // Integrator stages running at high sample rate
44        let mut val = input;
45        for i in 0..STAGES {
46            self.integrator_state[i] = self.integrator_state[i].wrapping_add(val);
47            val = self.integrator_state[i];
48        }
49
50        self.sample_counter += 1;
51        if self.sample_counter >= self.r {
52            self.sample_counter = 0;
53
54            // Comb stages running at low sample rate
55            for i in 0..STAGES {
56                let diff = val.wrapping_sub(self.comb_state[i]);
57                self.comb_state[i] = val;
58                val = diff;
59            }
60            Some(val)
61        } else {
62            None
63        }
64    }
65
66    /// Process an input sample and normalize output by bit-growth right-shift to prevent overflow.
67    pub fn process_sample_scaled(&mut self, input: i32) -> Option<i32> {
68        self.process_sample(input).map(|out| {
69            let shift = self.gain_bits();
70            if shift > 0 {
71                out >> shift
72            } else {
73                out
74            }
75        })
76    }
77}
78
79/// Cascaded Integrator-Comb (CIC) Interpolator for upsampling signals in integer arithmetic.
80pub struct CicInterpolator<const STAGES: usize> {
81    r: usize, // Interpolation factor
82    comb_state: [i32; STAGES],
83    integrator_state: [i32; STAGES],
84}
85
86impl<const STAGES: usize> CicInterpolator<STAGES> {
87    /// Initialise a new CIC interpolator with interpolation factor `r`.
88    pub fn new(r: usize) -> Self {
89        Self {
90            r,
91            comb_state: [0; STAGES],
92            integrator_state: [0; STAGES],
93        }
94    }
95
96    /// Theoretical maximum DC gain: `R^(STAGES - 1)`.
97    pub fn gain(&self) -> u64 {
98        if STAGES <= 1 {
99            return 1;
100        }
101        let mut g: u64 = 1;
102        for _ in 0..(STAGES - 1) {
103            g = g.saturating_mul(self.r as u64);
104        }
105        g
106    }
107
108    /// Number of bits of bit-growth: `ceil(log2(gain))`.
109    pub fn gain_bits(&self) -> u32 {
110        let g = self.gain();
111        if g <= 1 {
112            0
113        } else {
114            64 - (g - 1).leading_zeros()
115        }
116    }
117
118    /// Process a single input sample and populate `out_buf` with `R` interpolated output samples.
119    pub fn process_sample(&mut self, input: i32, out_buf: &mut [i32]) {
120        assert!(
121            out_buf.len() >= self.r,
122            "out_buf must hold at least R samples"
123        );
124
125        // Comb stages at low rate
126        let mut val = input;
127        for i in 0..STAGES {
128            let diff = val.wrapping_sub(self.comb_state[i]);
129            self.comb_state[i] = val;
130            val = diff;
131        }
132
133        // Zero stuffing and integrator stages at high rate
134        for step in 0..self.r {
135            let in_step = if step == 0 { val } else { 0 };
136            let mut stage_val = in_step;
137
138            for i in 0..STAGES {
139                self.integrator_state[i] = self.integrator_state[i].wrapping_add(stage_val);
140                stage_val = self.integrator_state[i];
141            }
142
143            out_buf[step] = stage_val;
144        }
145    }
146}
147
148// ─────────────────────────────────────────────────────────────────────────────
149// Polyphase & Linear Resampling in Q15
150// ─────────────────────────────────────────────────────────────────────────────
151
152use crate::types::q15;
153
154/// Polyphase FIR decimation by integer factor `M`.
155///
156/// Filters and downsamples `src` by factor `M` (`decimation_factor`).
157/// `coeffs` is the prototype FIR filter kernel (length typically multiple of `M`).
158/// Returns the number of output samples written to `dst`.
159pub fn polyphase_decimate_q15(
160    src: &[q15],
161    coeffs: &[q15],
162    decimation_factor: usize,
163    dst: &mut [q15],
164) -> usize {
165    if decimation_factor == 0 || coeffs.is_empty() || src.is_empty() {
166        return 0;
167    }
168    let num_taps = coeffs.len();
169    let out_len = dst.len().min(if src.len() >= num_taps { (src.len() - num_taps) / decimation_factor + 1 } else { 0 });
170
171    for i in 0..out_len {
172        let src_offset = i * decimation_factor;
173        let mut acc: i64 = 0;
174        for k in 0..num_taps {
175            acc += (src[src_offset + k].to_bits() as i64 * coeffs[k].to_bits() as i64) >> 15;
176        }
177        dst[i] = q15::from_bits(acc.clamp(i16::MIN as i64, i16::MAX as i64) as i16);
178    }
179
180    out_len
181}
182
183/// Polyphase FIR interpolation by integer factor `L`.
184///
185/// Upsamples `src` by factor `L` (`interpolation_factor`) using polyphase decomposition.
186/// `coeffs` length must be a multiple of `L`.
187/// Returns the number of output samples written to `dst`.
188pub fn polyphase_interpolate_q15(
189    src: &[q15],
190    coeffs: &[q15],
191    interpolation_factor: usize,
192    dst: &mut [q15],
193) -> usize {
194    let l = interpolation_factor;
195    if l == 0 || coeffs.is_empty() || src.is_empty() || coeffs.len() % l != 0 {
196        return 0;
197    }
198    let taps_per_phase = coeffs.len() / l;
199    let max_in_samples = if src.len() >= taps_per_phase { src.len() - taps_per_phase + 1 } else { 0 };
200    let out_len = dst.len().min(max_in_samples * l);
201
202    for in_idx in 0..max_in_samples {
203        for phase in 0..l {
204            let out_idx = in_idx * l + phase;
205            if out_idx >= dst.len() {
206                break;
207            }
208            let mut acc: i64 = 0;
209            for k in 0..taps_per_phase {
210                let coeff = coeffs[k * l + phase].to_bits() as i64;
211                let sample = src[in_idx + k].to_bits() as i64;
212                acc += (sample * coeff) >> 15;
213            }
214            dst[out_idx] =
215                q15::from_bits((acc * l as i64).clamp(i16::MIN as i64, i16::MAX as i64) as i16);
216        }
217    }
218
219    out_len
220}
221
222/// Linear fractional resampler in Q15.
223/// `ratio_q16` is `(src_sample_rate / dst_sample_rate)` in Q16.16 format.
224pub fn resample_linear_q15(src: &[q15], dst: &mut [q15], ratio_q16: i32) {
225    if src.is_empty() || dst.is_empty() || ratio_q16 <= 0 {
226        return;
227    }
228
229    let mut phase_acc: i64 = 0;
230    for i in 0..dst.len() {
231        let idx0 = (phase_acc >> 16) as usize;
232        let frac = (phase_acc & 0xFFFF) as i32; // [0, 65535]
233
234        if idx0 >= src.len() {
235            dst[i] = src[src.len() - 1];
236        } else {
237            let s0 = src[idx0].to_bits() as i32;
238            let s1 = if idx0 + 1 < src.len() {
239                src[idx0 + 1].to_bits() as i32
240            } else {
241                s0
242            };
243            let diff = s1 - s0;
244            let interp = s0 + ((diff * frac) >> 16);
245            dst[i] = q15::from_bits(interp.clamp(i16::MIN as i32, i16::MAX as i32) as i16);
246        }
247
248        phase_acc += ratio_q16 as i64;
249    }
250}
251
252/// Linear fractional resampler.
253/// Resamples `src` into `dst` according to `ratio` (`src_sample_rate / dst_sample_rate`).
254pub fn resample_linear_f32(src: &[f32], dst: &mut [f32], ratio: f32) {
255    if src.is_empty() || dst.is_empty() || ratio <= 0.0 {
256        return;
257    }
258
259    for i in 0..dst.len() {
260        let src_idx_float = i as f32 * ratio;
261        let idx0 = src_idx_float as usize;
262        let idx1 = (idx0 + 1).min(src.len() - 1);
263
264        if idx0 >= src.len() {
265            dst[i] = src[src.len() - 1];
266            continue;
267        }
268
269        let frac = src_idx_float - idx0 as f32;
270        dst[i] = src[idx0] * (1.0 - frac) + src[idx1] * frac;
271    }
272}
273
274#[cfg(feature = "transform")]
275use crate::transform::cfft_f32;
276#[cfg(feature = "transform")]
277use crate::types::Status;
278
279/// Spectral (Sinc) 2:1 Interpolator using frequency-domain zero-padding via FFT/IFFT.
280///
281/// `src` length must be a power of 2 (e.g. 16, 32, 64, 128, 256).
282/// `dst` must have length at least `2 * src.len()`.
283///
284/// Requires the `transform` feature (enabled by `full`).
285#[cfg(feature = "transform")]
286pub fn spectral_interpolate_2x_f32(src: &[f32], dst: &mut [f32]) -> Status {
287    let n = src.len();
288    if n < 4 || (n & (n - 1)) != 0 {
289        return Status::ArgumentError;
290    }
291    if dst.len() < 2 * n {
292        return Status::LengthError;
293    }
294    if 4 * n > 1024 {
295        return Status::LengthError; // Max scratch size limit (256-pt input -> 512-pt complex)
296    }
297
298    let mut c_buf = [0.0f32; 1024];
299
300    // Copy src into complex array (size 2 * 2n)
301    for i in 0..n {
302        c_buf[2 * i] = src[i];
303        c_buf[2 * i + 1] = 0.0;
304    }
305
306    // FFT of size n
307    cfft_f32(&mut c_buf[..2 * n], n, 0, 1);
308
309    // Half Nyquist component
310    let nyq_re = 0.5 * c_buf[n];
311    let nyq_im = 0.5 * c_buf[n + 1];
312    c_buf[n] = nyq_re;
313    c_buf[n + 1] = nyq_im;
314
315    // Shift negative frequencies to upper half and zero middle
316    let mut expanded = [0.0f32; 1024];
317    // Copy 0..=N/2
318    for i in 0..=(n / 2) {
319        expanded[2 * i] = c_buf[2 * i];
320        expanded[2 * i + 1] = c_buf[2 * i + 1];
321    }
322    // Nyquist conjugate mirror at 3N/2
323    expanded[2 * (3 * n / 2)] = nyq_re;
324    expanded[2 * (3 * n / 2) + 1] = nyq_im;
325
326    // Negative frequencies
327    for i in (n / 2 + 1)..n {
328        expanded[2 * (i + n)] = c_buf[2 * i];
329        expanded[2 * (i + n) + 1] = c_buf[2 * i + 1];
330    }
331
332    // IFFT of size 2n
333    cfft_f32(&mut expanded[..4 * n], 2 * n, 1, 1);
334
335    // Copy back scaled real part (factor of 2)
336    for i in 0..(2 * n) {
337        dst[i] = 2.0 * expanded[2 * i];
338    }
339
340    Status::Success
341}
embedded_dsp/resampling.rs

embedded_dsp/
resampling.rs
