Skip to main content

threers_codec/
swr.rs

1//! What the ffmpeg n7.1 CLI does to decoded float audio for
2//! `-f s16le -c:a pcm_s16le -ar RATE -ac 2`: the auto-inserted `aresample`
3//! filter's libswresample context (default options, aarch64 NEON build).
4//!
5//! - **Rematrix** mono -> stereo: both outputs `in * (float)M_SQRT1_2`
6//!   (`swr_build_matrix2`, no normalisation below unity gain), before the
7//!   resampler or after it as `resample_first` decides
8//!   (`out_ch/in_ch - 1 < out_rate/in_rate - 1`, integer left side).
9//! - **Resample** (only when the rates differ): `resample_init` with
10//!   `filter_size 32`, `phase_shift 10`, `cutoff 0.97`, Kaiser beta 9,
11//!   `exact_rational 1`; float filter bank from `build_filter`; the NEON
12//!   `resample_common` (8-wide fused dot product, pairwise lane sum, then
13//!   an unfused C tail); the start mirrored by `invert_initial_buffer` and the end by
14//!   `resample_flush`.
15//! - **s16 conversion** per `swr_convert` call: the first `len & !15`
16//!   samples by NEON (`fcvtzs #31` then `>> 16`: truncation), the rest by C
17//!   `av_clip_int16(lrintf(x * 32768))`. One call per decoded frame, so the
18//!   split follows the frame sizes; the flush is one more call.
19//!
20//! Rate pairs that would need `resample_linear` (a non-zero
21//! `dst_incr_mod`) are refused.
22
23#![allow(clippy::needless_range_loop, clippy::type_complexity)]
24
25use crate::aac::dsp::bessel_i0;
26
27/// Planar float input as the decoder delivered it.
28pub struct DecodedFrames<'a> {
29    /// One `Vec` per channel.
30    pub planes: &'a [Vec<f32>],
31    /// Samples per second.
32    pub sample_rate: u32,
33    /// Samples per delivered frame (after priming was dropped), in order.
34    pub frame_lens: &'a [usize],
35}
36
37struct Resampler {
38    phase_count: i64,
39    filter_length: usize,
40    filter_alloc: usize,
41    bank: Vec<f32>,
42    src_incr: i64,
43    dst_incr_div: i64,
44    dst_incr_mod: i64,
45}
46
47fn gcd(a: i64, b: i64) -> i64 {
48    if b == 0 {
49        a
50    } else {
51        gcd(b, a % b)
52    }
53}
54
55impl Resampler {
56    fn new(out_rate: i64, in_rate: i64) -> Result<Self, String> {
57        Self::with_filter_size(out_rate, in_rate, 32)
58    }
59
60    fn with_filter_size(out_rate: i64, in_rate: i64, filter_size: usize) -> Result<Self, String> {
61        let cutoff = 0.97f64;
62        let factor = (out_rate as f64 * cutoff / in_rate as f64).min(1.0);
63        let mut phase_count: i64 = 1 << 10;
64        let mut filter_length = ((filter_size as f64 / factor).ceil() as i64).max(1) as usize;
65        if filter_length > 1 {
66            filter_length = (filter_length + 1) & !1;
67        }
68        let g = gcd(out_rate, in_rate);
69        let exact = out_rate / g;
70        if exact <= phase_count {
71            phase_count = exact;
72        }
73        let filter_alloc = (filter_length + 7) & !7;
74        let mut bank = vec![0f32; filter_alloc * (phase_count as usize + 1)];
75        build_filter(
76            &mut bank,
77            factor,
78            filter_length,
79            filter_alloc,
80            phase_count as usize,
81            9.0,
82        );
83        let pc = phase_count as usize;
84        let (head, tail) = bank.split_at_mut(filter_alloc * pc + 1);
85        tail[..filter_alloc - 1].copy_from_slice(&head[..filter_alloc - 1]);
86        bank[filter_alloc * pc] = bank[filter_alloc - 1];
87        // av_reduce(out_rate, in_rate * phase_count) then double up to 2^20.
88        let (mut src_incr, mut dst_incr) = (out_rate, in_rate * phase_count);
89        let g = gcd(src_incr, dst_incr);
90        src_incr /= g;
91        dst_incr /= g;
92        if src_incr > i32::MAX as i64 / 2 || dst_incr > i32::MAX as i64 / 2 {
93            return Err("resampling ratio needs av_reduce approximation".into());
94        }
95        while dst_incr < (1 << 20) && src_incr < (1 << 20) {
96            dst_incr *= 2;
97            src_incr *= 2;
98        }
99        let r = Self {
100            phase_count,
101            filter_length,
102            filter_alloc,
103            bank,
104            src_incr,
105            dst_incr_div: dst_incr / src_incr,
106            dst_incr_mod: dst_incr % src_incr,
107        };
108        if r.dst_incr_mod != 0 {
109            return Err(format!("{in_rate} -> {out_rate} Hz needs resample_linear"));
110        }
111        Ok(r)
112    }
113
114    /// `ff_resample_common_float_neon` for one output: four fused lanes over
115    /// blocks of 8 (or 4 when shorter), the pairwise lane sum, then an
116    /// unfused C tail.
117    #[inline]
118    fn apply(&self, src: &[f32], index: usize) -> f32 {
119        let l = self.filter_length;
120        let base = self.filter_alloc * index;
121        let f = &self.bank[base..base + l];
122        let src = &src[..l];
123        let x8 = l & !7;
124        let x4 = l & !3;
125        let mut a = [0f32; 4];
126        let (mut val, mut i);
127        if x8 >= 8 {
128            for (s8, f8) in src[..x8]
129                .as_chunks::<8>()
130                .0
131                .iter()
132                .zip(f[..x8].as_chunks::<8>().0)
133            {
134                for k in 0..4 {
135                    a[k] = s8[k].mul_add(f8[k], a[k]);
136                }
137                for k in 0..4 {
138                    a[k] = s8[4 + k].mul_add(f8[4 + k], a[k]);
139                }
140            }
141            val = (a[0] + a[1]) + (a[2] + a[3]);
142            i = x8;
143        } else if x4 >= 4 {
144            for (s4, f4) in src[..x4]
145                .as_chunks::<4>()
146                .0
147                .iter()
148                .zip(f[..x4].as_chunks::<4>().0)
149            {
150                for k in 0..4 {
151                    a[k] = s4[k].mul_add(f4[k], a[k]);
152                }
153            }
154            val = (a[0] + a[1]) + (a[2] + a[3]);
155            i = x4;
156        } else {
157            val = 0.0;
158            i = 0;
159        }
160        // The C tail is not contracted (measured: fused leaves 0.2% of
161        // 48 -> 44.1 kHz outputs 1 ulp off, with a filter length of 36).
162        while i < l {
163            val += src[i] * f[i];
164            i += 1;
165        }
166        val
167    }
168}
169
170/// `build_filter` for `AV_SAMPLE_FMT_FLTP`, Kaiser window.
171fn build_filter(
172    bank: &mut [f32],
173    factor: f64,
174    tap_count: usize,
175    alloc: usize,
176    phase_count: usize,
177    kaiser_beta: f64,
178) {
179    use std::f64::consts::PI;
180    let ph_nb = if phase_count % 2 == 1 {
181        phase_count
182    } else {
183        phase_count / 2 + 1
184    };
185    let center = (tap_count as i64 - 1) / 2;
186    let mut tab = vec![0f64; tap_count + 1];
187    let mut sin_lut = vec![0f64; ph_nb];
188    let factor = factor.min(1.0);
189    if factor == 1.0 {
190        for (ph, s) in sin_lut.iter_mut().enumerate() {
191            *s = (PI * ph as f64 / phase_count as f64).sin()
192                * if center & 1 == 1 { 1.0 } else { -1.0 };
193        }
194    }
195    let mut norm = 0f64;
196    for ph in 0..ph_nb {
197        let mut s = sin_lut[ph];
198        for i in 0..tap_count {
199            let x = PI * ((i as i64 - center) as f64 - ph as f64 / phase_count as f64) * factor;
200            let mut y = if x == 0.0 {
201                1.0
202            } else if factor == 1.0 {
203                s / x
204            } else {
205                x.sin() / x
206            };
207            let w = 2.0 * x / (factor * tap_count as f64 * PI);
208            y *= bessel_i0(kaiser_beta * (1.0 - w * w).max(0.0).sqrt());
209            tab[i] = y;
210            s = -s;
211            if ph == 0 {
212                norm += y;
213            }
214        }
215        for i in 0..tap_count {
216            bank[ph * alloc + i] = (tab[i] * 1.0 / norm) as f32;
217        }
218        if phase_count % 2 == 1 {
219            continue;
220        }
221        for i in 0..tap_count {
222            bank[(phase_count - ph) * alloc + tap_count - 1 - i] = bank[ph * alloc + i];
223        }
224    }
225}
226
227/// The s16 conversion of one `swr_convert` call: the first `len & !15`
228/// samples by NEON, the rest by C `av_clip_int16(lrintf(x * 32768))`.
229/// Stereo (`conv_fltp_to_s16_2ch_neon`) narrows `fcvtzs #31` with `sri #16`
230/// (truncation); mono (`conv_flt_to_s16_neon`, ffmpeg maps 1-channel
231/// formats to planar) with `sqrshrn #16` (rounding).
232fn to_s16(out: &mut Vec<i16>, planes: &[&[f32]], len: usize) {
233    let off = len & !15;
234    let mono = planes.len() == 1;
235    for i in 0..len {
236        for p in planes {
237            let x = p[i];
238            out.push(if i < off {
239                let t = (x as f64 * 2147483648.0)
240                    .trunc()
241                    .clamp(i32::MIN as f64, i32::MAX as f64);
242                let t = if t.is_nan() { 0 } else { t as i32 as i64 };
243                if mono {
244                    ((t + 0x8000) >> 16).clamp(-32768, 32767) as i16
245                } else {
246                    (t >> 16) as i16
247                }
248            } else {
249                // lrintf: round half to even in float.
250                let v = (x * 32768.0).round_ties_even();
251                v.clamp(-32768.0, 32767.0) as i16
252            });
253        }
254    }
255}
256
257/// Interleaved s16 at `out_rate`, stereo, as the ffmpeg 7.1 CLI writes it.
258pub fn ffmpeg_cli_s16_stereo(input: &DecodedFrames, out_rate: u32) -> Result<Vec<i16>, String> {
259    ffmpeg_cli_s16(input, out_rate, 2)
260}
261
262/// Interleaved s16 at `out_rate` with `out_ch` (1 or 2) channels, as
263/// `ffmpeg -i IN -f s16le -ar RATE -ac CH` writes it. The rematrix is
264/// `swr_build_matrix2`'s: mono -> stereo `x * (float)M_SQRT1_2` per side;
265/// stereo -> mono `0.5f*L + 0.5f*R` (`M_SQRT1_2` each, normalised by their
266/// sum because s16 output caps the gain at 1).
267pub fn ffmpeg_cli_s16(
268    input: &DecodedFrames,
269    out_rate: u32,
270    out_ch: usize,
271) -> Result<Vec<i16>, String> {
272    let in_ch = input.planes.len();
273    let n: usize = input.frame_lens.iter().sum();
274    if input.planes.iter().any(|p| p.len() != n) {
275        return Err("frame lengths do not add up to the planes".into());
276    }
277    if !(1..=2).contains(&in_ch) || !(1..=2).contains(&out_ch) {
278        return Err(format!("{in_ch} -> {out_ch} channels"));
279    }
280    let up = std::f64::consts::FRAC_1_SQRT_2 as f32;
281    let down = (std::f64::consts::FRAC_1_SQRT_2
282        / (std::f64::consts::FRAC_1_SQRT_2 + std::f64::consts::FRAC_1_SQRT_2))
283        as f32;
284    // swri_rematrix on planar float chunks (C, uncontracted).
285    let rematrix = |p: &[&[f32]]| -> Vec<Vec<f32>> {
286        match (in_ch, out_ch) {
287            (1, 2) => {
288                let m: Vec<f32> = p[0].iter().map(|&x| up * x).collect();
289                vec![m.clone(), m]
290            }
291            (2, 1) => vec![p[0]
292                .iter()
293                .zip(p[1])
294                .map(|(&l, &r)| down * l + down * r)
295                .collect()],
296            _ => p.iter().map(|c| c.to_vec()).collect(),
297        }
298    };
299    let resample = input.sample_rate != out_rate;
300    // resample_first: `out_ch/in_ch - 1` (integer) < `out_rate/(float)in_rate - 1.0`.
301    let resample_first = resample
302        && (((out_ch / in_ch) as i64 - 1) as f64)
303            < (out_rate as f32 / input.sample_rate as f32) as f64 - 1.0;
304    let mut out = Vec::new();
305    if !resample {
306        let mut at = 0;
307        for &len in input.frame_lens {
308            let chunk: Vec<&[f32]> = input.planes.iter().map(|p| &p[at..at + len]).collect();
309            let m = rematrix(&chunk);
310            let refs: Vec<&[f32]> = m.iter().map(|v| v.as_slice()).collect();
311            to_s16(&mut out, &refs, len);
312            at += len;
313        }
314        return Ok(out);
315    }
316    // The resampler runs on the channels it sees: the input's when it goes
317    // first, else the rematrixed ones.
318    let pre: Vec<Vec<f32>> = if resample_first {
319        input.planes.to_vec()
320    } else {
321        let refs: Vec<&[f32]> = input.planes.iter().map(|p| p.as_slice()).collect();
322        rematrix(&refs)
323    };
324    let post = |ys: &[&[f32]]| -> Vec<Vec<f32>> {
325        if resample_first {
326            rematrix(ys)
327        } else {
328            ys.iter().map(|c| c.to_vec()).collect()
329        }
330    };
331    let r = Resampler::new(out_rate as i64, input.sample_rate as i64)?;
332    let l = r.filter_length;
333    let half = (l - 1) / 2;
334    if input.frame_lens.first().copied().unwrap_or(0) < l + 1 {
335        return Err("first frame shorter than the resampler's filter".into());
336    }
337    // Extended signal: x[-half..0) mirrored (x[-k] = x[k]), then x, then
338    // the flush reflection, sized once it is known.
339    let mut ext: Vec<Vec<f32>> = pre
340        .iter()
341        .map(|p| {
342            let mut e: Vec<f32> = (1..=half).rev().map(|k| p[k]).collect();
343            e.extend_from_slice(p);
344            e
345        })
346        .collect();
347    let (mut s, mut index, mut frac) = (0usize, 0i64, 0i64);
348    let nch = pre.len();
349    let mut ys: Vec<Vec<f32>> = vec![Vec::new(); nch];
350    let emit = |ext: &Vec<Vec<f32>>,
351                avail: usize,
352                ys: &mut Vec<Vec<f32>>,
353                s: &mut usize,
354                index: &mut i64,
355                frac: &mut i64,
356                limit: usize| {
357        let mut produced = 0;
358        while *s + l <= avail && produced < limit {
359            for c in 0..nch {
360                ys[c].push(r.apply(&ext[c][*s..], *index as usize));
361            }
362            produced += 1;
363            *frac += r.dst_incr_mod;
364            *index += r.dst_incr_div;
365            if *frac >= r.src_incr {
366                *frac -= r.src_incr;
367                *index += 1;
368            }
369            while *index >= r.phase_count {
370                *s += 1;
371                *index -= r.phase_count;
372            }
373        }
374        produced
375    };
376    let mut avail = half;
377    for &len in input.frame_lens {
378        avail += len;
379        let k = emit(
380            &ext,
381            avail,
382            &mut ys,
383            &mut s,
384            &mut index,
385            &mut frac,
386            usize::MAX,
387        );
388        let start = ys[0].len() - k;
389        let chunk: Vec<&[f32]> = ys.iter().map(|y| &y[start..]).collect();
390        let m = post(&chunk);
391        let refs: Vec<&[f32]> = m.iter().map(|v| v.as_slice()).collect();
392        to_s16(&mut out, &refs, k);
393    }
394    // resample_flush: reflect min(buffered, L) samples, half-sample symmetric.
395    let buffered = avail - s;
396    let reflection = buffered.min(l).div_ceil(2);
397    for e in &mut ext {
398        let end = e.len();
399        for j in 0..reflection {
400            let v = e[end - 1 - j];
401            e.push(v);
402        }
403    }
404    avail += reflection;
405    loop {
406        // flush_frame: swr_convert(NULL) into 4096-sample frames.
407        let k = emit(&ext, avail, &mut ys, &mut s, &mut index, &mut frac, 4096);
408        if k == 0 {
409            break;
410        }
411        let start = ys[0].len() - k;
412        let chunk: Vec<&[f32]> = ys.iter().map(|y| &y[start..]).collect();
413        let m = post(&chunk);
414        let refs: Vec<&[f32]> = m.iter().map(|v| v.as_slice()).collect();
415        to_s16(&mut out, &refs, k);
416    }
417    Ok(out)
418}
419
420/// A planar-float libswresample context driven call by call, as a decoder
421/// drives its own `SwrContext` (Opus's SILK resampler: `filter_size 16`, no
422/// rematrix, fltp in and out). `convert` is `swr_convert(out, limit, in,
423/// n)`: the input is buffered, at most `limit` outputs are produced; `flush`
424/// is `swr_convert(out, limit, NULL, 0)` after `resample_flush`'s
425/// reflection. The start is mirrored as `invert_initial_buffer` does.
426pub struct SwrStream {
427    r: Resampler,
428    half: usize,
429    /// Per channel: `half` mirrored samples (filled once known), then input.
430    ext: Vec<Vec<f32>>,
431    nin: usize,
432    mirrored: bool,
433    s: usize,
434    index: i64,
435    frac: i64,
436    flushed: bool,
437    /// Input sample rate.
438    pub in_rate: u32,
439}
440
441impl SwrStream {
442    /// A resampler from `in_rate` to `out_rate` with ffmpeg's `filter_size` taps
443    /// per phase (32 is swresample's default).
444    pub fn new(
445        in_rate: u32,
446        out_rate: u32,
447        channels: usize,
448        filter_size: usize,
449    ) -> Result<Self, String> {
450        let r = Resampler::with_filter_size(out_rate as i64, in_rate as i64, filter_size)?;
451        let half = (r.filter_length - 1) / 2;
452        Ok(Self {
453            r,
454            half,
455            ext: vec![vec![0.0; half]; channels],
456            nin: 0,
457            mirrored: false,
458            s: 0,
459            index: 0,
460            frac: 0,
461            flushed: false,
462            in_rate,
463        })
464    }
465
466    /// Append one block of planar input, a slice per channel.
467    pub fn push(&mut self, input: &[&[f32]]) {
468        for (e, x) in self.ext.iter_mut().zip(input) {
469            e.extend_from_slice(x);
470        }
471        self.nin += input.first().map_or(0, |x| x.len());
472        if !self.mirrored && self.nin > self.half {
473            for e in self.ext.iter_mut() {
474                for k in 1..=self.half {
475                    e[self.half - k] = e[self.half + k];
476                }
477            }
478            self.mirrored = true;
479        }
480    }
481
482    fn emit(&mut self, out: &mut [Vec<f32>], limit: usize) -> usize {
483        let l = self.r.filter_length;
484        let avail = self.ext[0].len();
485        let mut produced = 0;
486        // invert_initial_buffer waits for filter_length + 1 input samples.
487        if !self.flushed && self.nin < l + 1 {
488            return 0;
489        }
490        while self.s + l <= avail && produced < limit {
491            for (c, o) in out.iter_mut().enumerate() {
492                o.push(self.r.apply(&self.ext[c][self.s..], self.index as usize));
493            }
494            produced += 1;
495            self.frac += self.r.dst_incr_mod;
496            self.index += self.r.dst_incr_div;
497            if self.frac >= self.r.src_incr {
498                self.frac -= self.r.src_incr;
499                self.index += 1;
500            }
501            while self.index >= self.r.phase_count {
502                self.s += 1;
503                self.index -= self.r.phase_count;
504            }
505        }
506        produced
507    }
508
509    /// Buffer `input`, then append up to `limit` outputs per channel to `out`.
510    pub fn convert(&mut self, out: &mut [Vec<f32>], limit: usize, input: &[&[f32]]) -> usize {
511        self.push(input);
512        self.emit(out, limit)
513    }
514
515    /// `swr_convert(out, limit, NULL, 0)`.
516    pub fn flush(&mut self, out: &mut [Vec<f32>], limit: usize) -> usize {
517        if !self.flushed {
518            let buffered = self.ext[0].len() - self.s;
519            let reflection = buffered.min(self.r.filter_length).div_ceil(2);
520            for e in &mut self.ext {
521                let end = e.len();
522                for j in 0..reflection {
523                    let v = e[end - 1 - j];
524                    e.push(v);
525                }
526            }
527            self.flushed = true;
528        }
529        self.emit(out, limit)
530    }
531}