use crate::celt_lpc::{autocorr, lpc};
#[cfg(not(feature = "std"))]
use crate::compat::Math;
use crate::fixedvec::FixedVec;
use crate::mathops::mac;
use core::mem::MaybeUninit;
const PITCH_BUF_MAX: usize = 1024;
#[allow(dead_code)] fn xcorr_by_sample_scalar(x: &[f32], y: &[f32], out: &mut [f32], len: usize) {
const L: usize = 8;
let lags = out.len();
if lags == 0 {
return;
}
let x = &x[..len];
let y = &y[..len + lags - 1];
if lags < L {
for (k, o) in out.iter_mut().enumerate() {
let mut acc = 0.0f32;
for (&xj, &yj) in x.iter().zip(&y[k..]) {
acc = mac(acc, xj, yj);
}
*o = acc;
}
return;
}
let mut pass = |k: usize| {
let mut acc = [0.0f32; L];
for (&xj, yw) in x.iter().zip(y[k..].windows(L)) {
for l in 0..L {
acc[l] = mac(acc[l], xj, yw[l]);
}
}
out[k..k + L].copy_from_slice(&acc);
};
let mut k = 0;
while k + L <= lags {
pass(k);
k += L;
}
if k < lags {
pass(lags - L);
}
}
#[inline(always)]
pub(crate) fn celt_inner_prod(x: &[f32], y: &[f32]) -> f32 {
let n = x.len();
let y = &y[..n];
#[cfg(target_arch = "aarch64")]
{
use core::arch::aarch64::*;
unsafe {
let xp = x.as_ptr();
let yp = y.as_ptr();
let mut acc = vdupq_n_f32(0.0);
let mut i = 0;
while i + 8 <= n {
acc = vfmaq_f32(acc, vld1q_f32(xp.add(i)), vld1q_f32(yp.add(i)));
acc = vfmaq_f32(acc, vld1q_f32(xp.add(i + 4)), vld1q_f32(yp.add(i + 4)));
i += 8;
}
if n - i >= 4 {
acc = vfmaq_f32(acc, vld1q_f32(xp.add(i)), vld1q_f32(yp.add(i)));
i += 4;
}
let mut xy = lanes_sum(acc);
while i < n {
xy = mac(xy, *xp.add(i), *yp.add(i));
i += 1;
}
xy
}
}
#[cfg(all(target_arch = "x86_64", target_feature = "sse2"))]
{
use core::arch::x86_64::*;
unsafe {
let xp = x.as_ptr();
let yp = y.as_ptr();
let mut sum = _mm_setzero_ps();
let mut i = 0;
while i + 4 <= n {
sum = _mm_add_ps(sum, _mm_mul_ps(_mm_loadu_ps(xp.add(i)), _mm_loadu_ps(yp.add(i))));
i += 4;
}
let sum = _mm_add_ps(sum, _mm_movehl_ps(sum, sum));
let sum = _mm_add_ss(sum, _mm_shuffle_ps::<0x55>(sum, sum));
let mut xy = _mm_cvtss_f32(sum);
while i < n {
xy = mac(xy, *xp.add(i), *yp.add(i));
i += 1;
}
xy
}
}
#[cfg(not(any(target_arch = "aarch64", all(target_arch = "x86_64", target_feature = "sse2"))))]
{
let mut xy = 0.0f32;
for (&a, &b) in x.iter().zip(y) {
xy = mac(xy, a, b);
}
xy
}
}
#[cfg(target_arch = "aarch64")]
#[target_feature(enable = "neon")]
#[inline]
fn lanes_sum(v: core::arch::aarch64::float32x4_t) -> f32 {
use core::arch::aarch64::*;
let half = vadd_f32(vget_low_f32(v), vget_high_f32(v));
vget_lane_f32::<0>(vpadd_f32(half, half))
}
pub fn inner_prod(x: &[f32], y: &[f32], n: usize) -> f32 {
let (x, y) = (&x[..n], &y[..n]);
#[cfg(any(target_arch = "x86", target_arch = "x86_64"))]
if crate::compat::x86_has_avx_fma() {
return unsafe { inner_prod_avx(x, y, n) };
}
#[cfg(all(target_arch = "x86_64", target_feature = "sse"))]
unsafe {
inner_prod_sse(x, y, n)
}
#[cfg(not(all(target_arch = "x86_64", target_feature = "sse")))]
celt_inner_prod(x, y)
}
pub fn dual_inner_prod(x: &[f32], y1: &[f32], y2: &[f32], n: usize) -> (f32, f32) {
let (x, y1, y2) = (&x[..n], &y1[..n], &y2[..n]);
#[cfg(any(target_arch = "x86", target_arch = "x86_64"))]
if crate::compat::x86_has_avx_fma() {
return unsafe { dual_inner_prod_avx(x, y1, y2, n) };
}
#[cfg(all(target_arch = "x86_64", target_feature = "sse"))]
unsafe {
dual_inner_prod_sse(x, y1, y2, n)
}
#[cfg(target_arch = "aarch64")]
{
let [a, b] = inner_prod_k_neon::<2>(x, &[y1, y2]);
(a, b)
}
#[cfg(not(any(
target_arch = "aarch64",
all(target_arch = "x86_64", target_feature = "sse")
)))]
(celt_inner_prod(x, y1), celt_inner_prod(x, y2))
}
pub fn pitch_xcorr(x: &[f32], y: &[f32], xcorr: &mut [f32], len: usize, max_pitch: usize) {
let max_pitch = if len > 0 {
max_pitch.min((y.len() + 1).saturating_sub(len))
} else {
0
};
if max_pitch == 0 {
return;
}
let x = &x[..len];
#[cfg(any(target_arch = "x86", target_arch = "x86_64"))]
if crate::compat::x86_has_avx_fma() {
return unsafe { pitch_xcorr_avx(x, y, xcorr, len, max_pitch) };
}
#[cfg(target_arch = "aarch64")]
pitch_xcorr_neon(x, y, &mut xcorr[..max_pitch]);
#[cfg(all(target_arch = "x86_64", target_feature = "sse"))]
unsafe {
pitch_xcorr_sse(x, y, xcorr, len, max_pitch)
}
#[cfg(not(any(
target_arch = "aarch64",
all(target_arch = "x86_64", target_feature = "sse")
)))]
xcorr_by_sample_scalar(x, y, &mut xcorr[..max_pitch], len);
}
#[cfg(target_arch = "aarch64")]
const MANY_PER_PASS: usize = 16;
fn inner_prod_many(x: &[f32], y: &[f32], offsets: &[usize], n: usize, out: &mut [f32]) {
inner_prod_many_side(x, y, offsets, n, out, &mut || {});
}
fn inner_prod_many_side<F: FnMut()>(
x: &[f32],
y: &[f32],
offsets: &[usize],
n: usize,
out: &mut [f32],
side: &mut F,
) {
assert_eq!(offsets.len(), out.len());
#[cfg(target_arch = "aarch64")]
{
let x = &x[..n];
for (offs, out) in offsets
.chunks(MANY_PER_PASS)
.zip(out.chunks_mut(MANY_PER_PASS))
{
let ys: [&[f32]; MANY_PER_PASS] = core::array::from_fn(|k| {
let off = offs[if k < offs.len() { k } else { 0 }];
&y[off..off + n]
});
match offs.len().div_ceil(4) {
1 => inner_prod_pass_neon::<4, F>(x, &ys, out, side),
2 => inner_prod_pass_neon::<8, F>(x, &ys, out, side),
3 => inner_prod_pass_neon::<12, F>(x, &ys, out, side),
_ => inner_prod_pass_neon::<16, F>(x, &ys, out, side),
}
}
}
#[cfg(not(target_arch = "aarch64"))]
{
let _ = side;
for (o, &off) in out.iter_mut().zip(offsets) {
*o = inner_prod(x, &y[off..], n);
}
}
}
#[cfg(target_arch = "aarch64")]
#[inline(always)]
fn inner_prod_pass_neon<const K: usize, F: FnMut()>(
x: &[f32],
ys: &[&[f32]; MANY_PER_PASS],
out: &mut [f32],
side: &mut F,
) {
let ys_k: [&[f32]; K] = core::array::from_fn(|k| ys[k]);
let sums = inner_prod_k_side_neon::<K, F>(x, &ys_k, side);
out.copy_from_slice(&sums[..out.len()]);
}
#[cfg(target_arch = "aarch64")]
#[inline(always)]
fn inner_prod_k_neon<const K: usize>(x: &[f32], ys: &[&[f32]; K]) -> [f32; K] {
inner_prod_k_side_neon::<K, _>(x, ys, &mut || {})
}
#[cfg(target_arch = "aarch64")]
#[inline(always)]
fn inner_prod_k_side_neon<const K: usize, F: FnMut()>(
x: &[f32],
ys: &[&[f32]; K],
side: &mut F,
) -> [f32; K] {
use core::arch::aarch64::*;
let n = x.len();
assert!(ys.iter().all(|y| y.len() >= n));
let x = x.as_ptr();
let ys: [*const f32; K] = core::array::from_fn(|k| ys[k].as_ptr());
unsafe {
let mut acc = [vdupq_n_f32(0.0); K];
let mut i = 0;
while i + 4 <= n {
side();
let xv = vld1q_f32(x.add(i));
for (a, &y) in acc.iter_mut().zip(&ys) {
*a = vfmaq_f32(*a, xv, vld1q_f32(y.add(i)));
}
i += 4;
}
let mut sums = [0.0f32; K];
for ((sum, a), &y) in sums.iter_mut().zip(&acc).zip(&ys) {
let mut s = lanes_sum(*a);
for j in i..n {
s = mac(s, *x.add(j), *y.add(j));
}
*sum = s;
}
sums
}
}
#[cfg(target_arch = "aarch64")]
#[inline(always)]
fn xcorr_by_sample_neon<const G: usize>(x: &[f32], y: &[f32], out: &mut [f32]) {
xcorr_by_sample_side_neon::<G, _>(x, y, out, &mut || {});
}
#[cfg(target_arch = "aarch64")]
#[inline(always)]
fn xcorr_by_sample_side_neon<const G: usize, F: FnMut()>(
x: &[f32],
y: &[f32],
out: &mut [f32],
side: &mut F,
) {
use core::arch::aarch64::*;
let len = x.len();
assert!(y.len() >= len + 4 * G - 1 && out.len() >= 4 * G);
let (x, y, out) = (x.as_ptr(), y.as_ptr(), out.as_mut_ptr());
unsafe {
let mut acc = [vdupq_n_f32(0.0); G];
let mut j = 0;
while j + 8 <= len {
side();
let x0 = vld1q_f32(x.add(j));
let x1 = vld1q_f32(x.add(j + 4));
let mut yp = y.add(j);
let mut w = [
vld1q_f32(yp),
vld1q_f32(yp.add(1)),
vld1q_f32(yp.add(2)),
vld1q_f32(yp.add(3)),
];
for a in acc.iter_mut() {
let q = yp.add(4);
let next = [
vld1q_f32(q),
vld1q_f32(q.add(1)),
vld1q_f32(q.add(2)),
vld1q_f32(q.add(3)),
];
let mut s = *a;
s = vfmaq_laneq_f32::<0>(s, w[0], x0);
s = vfmaq_laneq_f32::<1>(s, w[1], x0);
s = vfmaq_laneq_f32::<2>(s, w[2], x0);
s = vfmaq_laneq_f32::<3>(s, w[3], x0);
s = vfmaq_laneq_f32::<0>(s, next[0], x1);
s = vfmaq_laneq_f32::<1>(s, next[1], x1);
s = vfmaq_laneq_f32::<2>(s, next[2], x1);
s = vfmaq_laneq_f32::<3>(s, next[3], x1);
*a = s;
w = next;
yp = q;
}
j += 8;
}
if j + 4 <= len {
let xv = vld1q_f32(x.add(j));
for (g, a) in acc.iter_mut().enumerate() {
let yp = y.add(j + 4 * g);
let mut s = *a;
s = vfmaq_laneq_f32::<0>(s, vld1q_f32(yp), xv);
s = vfmaq_laneq_f32::<1>(s, vld1q_f32(yp.add(1)), xv);
s = vfmaq_laneq_f32::<2>(s, vld1q_f32(yp.add(2)), xv);
s = vfmaq_laneq_f32::<3>(s, vld1q_f32(yp.add(3)), xv);
*a = s;
}
j += 4;
}
while j < len {
let xv = vld1q_dup_f32(x.add(j));
for (g, a) in acc.iter_mut().enumerate() {
*a = vfmaq_f32(*a, vld1q_f32(y.add(j + 4 * g)), xv);
}
j += 1;
}
for (g, a) in acc.iter().enumerate() {
vst1q_f32(out.add(4 * g), *a);
}
}
}
#[cfg(target_arch = "aarch64")]
#[inline(always)]
fn xcorr_by_sample_passes_neon<const G: usize>(x: &[f32], y: &[f32], out: &mut [f32]) {
xcorr_by_sample_passes_side_neon::<G, _>(x, y, out, &mut || {});
}
#[cfg(target_arch = "aarch64")]
#[inline(always)]
fn xcorr_by_sample_passes_side_neon<const G: usize, F: FnMut()>(
x: &[f32],
y: &[f32],
out: &mut [f32],
side: &mut F,
) {
let per_pass = 4 * G;
let lags = out.len();
assert!(lags >= per_pass);
let mut i = 0;
while i < lags {
let first = i.min(lags - per_pass);
xcorr_by_sample_side_neon::<G, F>(x, &y[first..], &mut out[first..], side);
i += per_pass;
}
}
#[cfg(target_arch = "aarch64")]
const SKEW_PASS: usize = 64;
#[cfg(target_arch = "aarch64")]
const SKEW_MIN_BLOCKS: usize = 8;
#[cfg(target_arch = "aarch64")]
const USE_SKEW: bool = !cfg!(target_vendor = "apple");
#[cfg(target_arch = "aarch64")]
const SKEW_PAIRS_PER_CALL: usize = 2;
#[cfg(target_arch = "aarch64")]
type SkewSums = [core::arch::aarch64::float32x4_t; 16];
#[cfg(target_arch = "aarch64")]
#[rustfmt::skip]
macro_rules! skew_fmas {
($a:literal, $b:literal, $x:literal, $wa:literal, $wb:literal, $lane:literal) => {
concat!(
"fmla v", $a, ".4s, v", $wa, ".4s, v", $x, ".s[", $lane, "]\n",
"fmla v", $b, ".4s, v", $wb, ".4s, v", $x, ".s[", $lane, "]\n",
)
};
}
#[cfg(target_arch = "aarch64")]
#[rustfmt::skip]
macro_rules! skew_ext {
($dst:literal, $cur:literal, $next:literal, $shift:literal) => {
concat!("ext v", $dst, ".16b, v", $cur, ".16b, v", $next, ".16b, #", $shift, "\n")
};
}
#[cfg(target_arch = "aarch64")]
macro_rules! skew_step {
($wa:literal, $wb:literal, $lane:literal, [$($mid:tt)*]) => {
concat!(
skew_fmas!(0, 8, 24, $wa, $wb, $lane),
skew_fmas!(1, 9, 25, $wa, $wb, $lane),
skew_fmas!(2, 10, 26, $wa, $wb, $lane),
skew_fmas!(3, 11, 27, $wa, $wb, $lane),
$($mid)*
skew_fmas!(4, 12, 28, $wa, $wb, $lane),
skew_fmas!(5, 13, 29, $wa, $wb, $lane),
skew_fmas!(6, 14, 30, $wa, $wb, $lane),
skew_fmas!(7, 15, 31, $wa, $wb, $lane),
)
};
($wa:literal, $wb:literal, $lane:literal, [$($mid:tt)*],
$o0:literal, $o1:literal, $o2:literal, $o3:literal,
$o4:literal, $o5:literal, $o6:literal, $o7:literal) => {
concat!(
skew_fmas!(0, 8, 24, $wa, $wb, $lane),
"ldr d24, [{xm}, #", $o0, "]\n",
skew_fmas!(1, 9, 25, $wa, $wb, $lane),
"ldr d25, [{xm}, #", $o1, "]\n",
skew_fmas!(2, 10, 26, $wa, $wb, $lane),
"ldr d26, [{xm}, #", $o2, "]\n",
skew_fmas!(3, 11, 27, $wa, $wb, $lane),
"ldr d27, [{xm}, #", $o3, "]\n",
$($mid)*
skew_fmas!(4, 12, 28, $wa, $wb, $lane),
"ldr d28, [{xm}, #", $o4, "]\n",
skew_fmas!(5, 13, 29, $wa, $wb, $lane),
"ldr d29, [{xm}, #", $o5, "]\n",
skew_fmas!(6, 14, 30, $wa, $wb, $lane),
"ldr d30, [{xm}, #", $o6, "]\n",
skew_fmas!(7, 15, 31, $wa, $wb, $lane),
"ldr d31, [{xm}, #", $o7, "]\n",
)
};
}
#[cfg(target_arch = "aarch64")]
#[rustfmt::skip]
macro_rules! skew_block {
($cy:literal, $cz:literal, $ny:literal, $nz:literal) => {
concat!(
"ldr q", $ny, ", [{ym}, #16]\n",
"ldr q", $nz, ", [{ym}, #144]\n",
skew_step!($cy, $cz, 0,
[skew_ext!(20, $cy, $ny, 4), skew_ext!(21, $cz, $nz, 4),]),
skew_step!(20, 21, 1,
[skew_ext!(22, $cy, $ny, 8), skew_ext!(23, $cz, $nz, 8),],
8, -8, -24, -40, -56, -72, -88, -104),
skew_step!(22, 23, 0,
[skew_ext!(20, $cy, $ny, 12), skew_ext!(21, $cz, $nz, 12),]),
skew_step!(20, 21, 1, [], 16, 0, -16, -32, -48, -64, -80, -96),
"add {xm}, {xm}, #16\n",
"add {ym}, {ym}, #16\n",
)
};
}
#[cfg(target_arch = "aarch64")]
#[inline(always)]
unsafe fn skew_change_step<const I: usize>(
acc: &mut SkewSums,
xp: *const f32,
yp: *const f32,
m_full: usize,
old: usize,
new: usize,
) {
use core::arch::aarch64::*;
unsafe {
let bn = yp.add(new + 4 * I);
let bo = yp.add(old + 4 * (m_full + I));
let xq: [*const f32; 8] = core::array::from_fn(|g| {
if g <= I {
xp.add(4 * (I - g))
} else {
xp.add(4 * (m_full + I - g))
}
});
let lo: [float32x2_t; 8] = core::array::from_fn(|g| vld1_f32(xq[g]));
let hi: [float32x2_t; 8] = core::array::from_fn(|g| vld1_f32(xq[g].add(2)));
macro_rules! step {
($t:literal, $x:ident, $lane:literal) => {{
let (wn, vn) = (vld1q_f32(bn.add($t)), vld1q_f32(bn.add(32 + $t)));
let (wo, vo) = (vld1q_f32(bo.add($t)), vld1q_f32(bo.add(32 + $t)));
for g in 0..8 {
let (w, v) = if g <= I { (wn, vn) } else { (wo, vo) };
acc[g] = vfmaq_lane_f32::<$lane>(acc[g], w, $x[g]);
acc[8 + g] = vfmaq_lane_f32::<$lane>(acc[8 + g], v, $x[g]);
}
}};
}
step!(0, lo, 0);
step!(1, lo, 1);
step!(2, hi, 0);
step!(3, hi, 1);
}
}
#[cfg(target_arch = "aarch64")]
#[inline(always)]
#[allow(clippy::too_many_arguments)]
unsafe fn skew_finish_group<const G: usize>(
acc: &mut SkewSums,
xp: *const f32,
yp: *const f32,
len: usize,
o: usize,
out: *mut f32,
store: bool,
) {
use core::arch::aarch64::*;
unsafe {
if store {
for j in len & !3..len {
let xv = vdupq_n_f32(*xp.add(j));
acc[G] = vfmaq_f32(acc[G], vld1q_f32(yp.add(o + 4 * G + j)), xv);
acc[8 + G] = vfmaq_f32(acc[8 + G], vld1q_f32(yp.add(o + 32 + 4 * G + j)), xv);
}
vst1q_f32(out.add(o + 4 * G), acc[G]);
vst1q_f32(out.add(o + 32 + 4 * G), acc[8 + G]);
}
acc[G] = vdupq_n_f32(0.0);
acc[8 + G] = vdupq_n_f32(0.0);
}
}
#[cfg(target_arch = "aarch64")]
#[inline(never)]
#[allow(clippy::too_many_arguments)]
unsafe fn skew_change(
acc: &mut SkewSums,
xp: *const f32,
yp: *const f32,
len: usize,
old: usize,
new: usize,
out: *mut f32,
store: bool,
) {
let m_full = len / 4;
unsafe {
skew_finish_group::<0>(acc, xp, yp, len, old, out, store);
skew_change_step::<0>(acc, xp, yp, m_full, old, new);
skew_finish_group::<1>(acc, xp, yp, len, old, out, store);
skew_change_step::<1>(acc, xp, yp, m_full, old, new);
skew_finish_group::<2>(acc, xp, yp, len, old, out, store);
skew_change_step::<2>(acc, xp, yp, m_full, old, new);
skew_finish_group::<3>(acc, xp, yp, len, old, out, store);
skew_change_step::<3>(acc, xp, yp, m_full, old, new);
skew_finish_group::<4>(acc, xp, yp, len, old, out, store);
skew_change_step::<4>(acc, xp, yp, m_full, old, new);
skew_finish_group::<5>(acc, xp, yp, len, old, out, store);
skew_change_step::<5>(acc, xp, yp, m_full, old, new);
skew_finish_group::<6>(acc, xp, yp, len, old, out, store);
skew_change_step::<6>(acc, xp, yp, m_full, old, new);
skew_finish_group::<7>(acc, xp, yp, len, old, out, store);
}
}
#[cfg(target_arch = "aarch64")]
#[inline(always)]
unsafe fn skew_block(acc: &mut SkewSums, xp: *const f32, yp: *const f32, o: usize, m: usize) {
use core::arch::aarch64::*;
unsafe {
let b = yp.add(o + 4 * m);
let lo: [float32x2_t; 8] = core::array::from_fn(|g| vld1_f32(xp.add(4 * (m - g))));
let hi: [float32x2_t; 8] = core::array::from_fn(|g| vld1_f32(xp.add(4 * (m - g) + 2)));
macro_rules! step {
($t:literal, $x:ident, $lane:literal) => {{
let (w, v) = (vld1q_f32(b.add($t)), vld1q_f32(b.add(32 + $t)));
for g in 0..8 {
acc[g] = vfmaq_lane_f32::<$lane>(acc[g], w, $x[g]);
acc[8 + g] = vfmaq_lane_f32::<$lane>(acc[8 + g], v, $x[g]);
}
}};
}
step!(0, lo, 0);
step!(1, lo, 1);
step!(2, hi, 0);
step!(3, hi, 1);
}
}
#[cfg(target_arch = "aarch64")]
#[inline(never)]
unsafe fn skew_steady<F: FnMut()>(
acc: &mut SkewSums,
xp: *const f32,
yp: *const f32,
m_full: usize,
o: usize,
side: &mut F,
) {
use core::arch::aarch64::*;
unsafe {
let mut m = 7;
let mut pairs = (m_full - SKEW_MIN_BLOCKS) / 2;
while pairs > 0 {
let count = pairs.min(SKEW_PAIRS_PER_CALL);
let mut xm = xp.add(4 * m);
let mut ym = yp.add(o + 4 * m);
let mut n = count;
let lo: [float32x2_t; 8] = core::array::from_fn(|g| vld1_f32(xp.add(4 * (m - g))));
let ycur = vld1q_f32(ym);
let zcur = vld1q_f32(ym.add(32));
core::arch::asm!(
"2:",
skew_block!(16, 17, 18, 19),
skew_block!(18, 19, 16, 17),
"subs {n}, {n}, #1",
"b.ne 2b",
xm = inout(reg) xm,
ym = inout(reg) ym,
n = inout(reg) n,
inout("v0") acc[0],
inout("v1") acc[1],
inout("v2") acc[2],
inout("v3") acc[3],
inout("v4") acc[4],
inout("v5") acc[5],
inout("v6") acc[6],
inout("v7") acc[7],
inout("v8") acc[8],
inout("v9") acc[9],
inout("v10") acc[10],
inout("v11") acc[11],
inout("v12") acc[12],
inout("v13") acc[13],
inout("v14") acc[14],
inout("v15") acc[15],
inout("v16") ycur => _,
inout("v17") zcur => _,
out("v18") _,
out("v19") _,
out("v20") _,
out("v21") _,
out("v22") _,
out("v23") _,
inout("v24") lo[0] => _,
inout("v25") lo[1] => _,
inout("v26") lo[2] => _,
inout("v27") lo[3] => _,
inout("v28") lo[4] => _,
inout("v29") lo[5] => _,
inout("v30") lo[6] => _,
inout("v31") lo[7] => _,
options(nostack, readonly),
);
let _ = (xm, ym, n);
m += 2 * count;
pairs -= count;
side();
}
while m < m_full {
skew_block(acc, xp, yp, o, m);
m += 1;
}
}
}
#[cfg(target_arch = "aarch64")]
fn skew_side_calls(len: usize, lags: usize) -> usize {
let pairs = (len / 4).saturating_sub(SKEW_MIN_BLOCKS) / 2;
lags.div_ceil(SKEW_PASS) * pairs.div_ceil(SKEW_PAIRS_PER_CALL)
}
#[cfg(target_arch = "aarch64")]
fn xcorr_skewed_neon<F: FnMut()>(x: &[f32], y: &[f32], out: &mut [f32], side: &mut F) {
use core::arch::aarch64::*;
let len = x.len();
let lags = out.len();
let m_full = len / 4;
assert!(
lags.is_multiple_of(4)
&& lags >= SKEW_PASS
&& m_full >= SKEW_MIN_BLOCKS
&& y.len() >= len + lags - 1
);
let passes = lags.div_ceil(SKEW_PASS);
let start = |p: usize| (SKEW_PASS * p).min(lags - SKEW_PASS);
unsafe {
let (xp, yp, op) = (x.as_ptr(), y.as_ptr(), out.as_mut_ptr());
let mut acc: SkewSums = [vdupq_n_f32(0.0); 16];
for p in 0..passes {
let old = start(p.saturating_sub(1));
skew_change(&mut acc, xp, yp, len, old, start(p), op, p > 0);
skew_steady(&mut acc, xp, yp, m_full, start(p), side);
}
let last = start(passes - 1);
skew_change(&mut acc, xp, yp, len, last, last, op, true);
}
}
#[cfg(target_arch = "aarch64")]
fn pitch_xcorr_neon(x: &[f32], y: &[f32], xcorr: &mut [f32]) {
let len = x.len();
let lags = xcorr.len();
if lags == 0 {
return;
}
assert!(len > 0 && y.len() >= len + lags - 1);
if lags == 5 {
unsafe { xcorr5_neon(x, y, xcorr) };
return;
}
let by_sample = lags & !3;
let (grouped, rest) = xcorr.split_at_mut(by_sample);
if USE_SKEW && by_sample >= SKEW_PASS && len >= 4 * SKEW_MIN_BLOCKS {
xcorr_skewed_neon(x, y, grouped, &mut || {});
} else {
xcorr_by_sample_groups_neon(x, y, grouped);
}
let mut offsets = [0usize; 3];
for (k, off) in offsets[..rest.len()].iter_mut().enumerate() {
*off = by_sample + k;
}
inner_prod_many(x, y, &offsets[..rest.len()], len, rest);
}
#[cfg(target_arch = "aarch64")]
fn xcorr_by_sample_groups_neon(x: &[f32], y: &[f32], grouped: &mut [f32]) {
match grouped.len() / 4 {
0 => {}
1 => xcorr_by_sample_neon::<1>(x, y, grouped),
2 => xcorr_by_sample_neon::<2>(x, y, grouped),
3 => xcorr_by_sample_neon::<3>(x, y, grouped),
4 => xcorr_by_sample_neon::<4>(x, y, grouped),
5 => xcorr_by_sample_neon::<5>(x, y, grouped),
6 => xcorr_by_sample_neon::<6>(x, y, grouped),
7 => xcorr_by_sample_neon::<7>(x, y, grouped),
8..16 => xcorr_by_sample_passes_neon::<8>(x, y, grouped),
_ => xcorr_by_sample_passes_neon::<16>(x, y, grouped),
}
}
#[cfg(target_arch = "aarch64")]
#[target_feature(enable = "neon")]
fn xcorr5_neon(x: &[f32], y: &[f32], xcorr: &mut [f32]) {
use crate::simd_mem::neon::{load_f32x4, store_f32x4};
use crate::simd_mem::{array_at, array_at_mut};
use core::arch::aarch64::*;
let len = x.len();
let y = &y[..len + 4];
let xcorr = &mut xcorr[..5];
let (x4, x_tail) = x.as_chunks::<4>();
let w = |t: usize| y[t..].as_chunks::<4>().0[..x4.len()].iter();
let mut by_sample = vdupq_n_f32(0.0);
let mut lag4 = vdupq_n_f32(0.0);
let groups = x4.iter().zip(w(0)).zip(w(1)).zip(w(2)).zip(w(3)).zip(w(4));
for (((((xa, w0), w1), w2), w3), w4) in groups {
let xv = load_f32x4(xa);
let mut s = by_sample;
s = vfmaq_laneq_f32::<0>(s, load_f32x4(w0), xv);
s = vfmaq_laneq_f32::<1>(s, load_f32x4(w1), xv);
s = vfmaq_laneq_f32::<2>(s, load_f32x4(w2), xv);
s = vfmaq_laneq_f32::<3>(s, load_f32x4(w3), xv);
by_sample = s;
lag4 = vfmaq_f32(lag4, xv, load_f32x4(w4));
}
let mut lag4 = lanes_sum(lag4);
let done = len - x_tail.len();
for (&xj, w) in x_tail.iter().zip(y[done..].windows(5)) {
by_sample = vfmaq_f32(by_sample, load_f32x4(array_at(w, 0)), vdupq_n_f32(xj));
lag4 = mac(lag4, xj, w[4]);
}
store_f32x4(array_at_mut(xcorr, 0), by_sample);
xcorr[4] = lag4;
}
#[cfg(all(target_arch = "x86_64", target_feature = "sse"))]
#[target_feature(enable = "sse")]
#[inline]
fn inner_prod_sse(x: &[f32], y: &[f32], n: usize) -> f32 {
#[cfg(target_arch = "x86")]
use core::arch::x86::*;
#[cfg(target_arch = "x86_64")]
use core::arch::x86_64::*;
use crate::simd_mem::x86::load_f32x4;
assert!(x.len() >= n && y.len() >= n);
let (x4, x_tail) = x[..n].as_chunks::<4>();
let (y4, y_tail) = y[..n].as_chunks::<4>();
let (x8, x4) = x4.as_chunks::<2>();
let (y8, y4) = y4.as_chunks::<2>();
let mut sum0 = _mm_setzero_ps();
let mut sum1 = _mm_setzero_ps();
let (mut xs, mut ys) = (x8, &y8[..x8.len()]);
while let ([xa, x_rest @ ..], [ya, y_rest @ ..]) = (xs, ys) {
let x0 = load_f32x4(&xa[0]);
let y0 = load_f32x4(&ya[0]);
sum0 = _mm_add_ps(sum0, _mm_mul_ps(x0, y0));
let x1 = load_f32x4(&xa[1]);
let y1 = load_f32x4(&ya[1]);
sum1 = _mm_add_ps(sum1, _mm_mul_ps(x1, y1));
(xs, ys) = (x_rest, y_rest);
}
if let ([xa], [ya]) = (x4, y4) {
let x0 = load_f32x4(xa);
let y0 = load_f32x4(ya);
sum0 = _mm_add_ps(sum0, _mm_mul_ps(x0, y0));
}
let sum = _mm_add_ps(sum0, sum1);
let tmp = _mm_movehl_ps(sum, sum);
let sum = _mm_add_ps(sum, tmp);
let tmp2 = _mm_shuffle_ps(sum, sum, 0x55);
let sum = _mm_add_ss(sum, tmp2);
let mut result = _mm_cvtss_f32(sum);
for (&a, &b) in x_tail.iter().zip(y_tail) {
result += a * b;
}
result
}
#[cfg(all(target_arch = "x86_64", target_feature = "sse"))]
#[target_feature(enable = "sse")]
#[inline]
fn dual_inner_prod_sse(x: &[f32], y1: &[f32], y2: &[f32], n: usize) -> (f32, f32) {
#[cfg(target_arch = "x86")]
use core::arch::x86::*;
#[cfg(target_arch = "x86_64")]
use core::arch::x86_64::*;
assert!(x.len() >= n && y1.len() >= n && y2.len() >= n);
let mut xy1 = _mm_setzero_ps();
let mut xy2 = _mm_setzero_ps();
let mut i = 0;
unsafe {
while i + 4 <= n {
let xi = _mm_loadu_ps(x.as_ptr().add(i));
let y1i = _mm_loadu_ps(y1.as_ptr().add(i));
let y2i = _mm_loadu_ps(y2.as_ptr().add(i));
xy1 = _mm_add_ps(xy1, _mm_mul_ps(xi, y1i));
xy2 = _mm_add_ps(xy2, _mm_mul_ps(xi, y2i));
i += 4;
}
}
let tmp = _mm_movehl_ps(xy1, xy1);
let xy1 = _mm_add_ps(xy1, tmp);
let tmp2 = _mm_shuffle_ps(xy1, xy1, 0x55);
let xy1 = _mm_add_ss(xy1, tmp2);
let mut s1 = _mm_cvtss_f32(xy1);
let tmp = _mm_movehl_ps(xy2, xy2);
let xy2 = _mm_add_ps(xy2, tmp);
let tmp2 = _mm_shuffle_ps(xy2, xy2, 0x55);
let xy2 = _mm_add_ss(xy2, tmp2);
let mut s2 = _mm_cvtss_f32(xy2);
while i < n {
s1 += x[i] * y1[i];
s2 += x[i] * y2[i];
i += 1;
}
(s1, s2)
}
#[cfg(all(target_arch = "x86_64", target_feature = "sse"))]
#[target_feature(enable = "sse")]
#[inline]
fn xcorr_kernel_sse(x: &[f32], y: &[f32], sum: &mut [f32; 4], len: usize) {
#[cfg(target_arch = "x86")]
use core::arch::x86::*;
#[cfg(target_arch = "x86_64")]
use core::arch::x86_64::*;
use crate::simd_mem::array_at;
use crate::simd_mem::x86::{load_f32x4, store_f32x4};
assert!(x.len() >= len && y.len() >= len + 3);
let (x4, x_tail) = x[..len].as_chunks::<4>();
let y0 = y[..len].as_chunks::<4>().0;
let y3 = y[3..len + 3].as_chunks::<4>().0;
let mut xsum1 = load_f32x4(sum);
let mut xsum2 = _mm_setzero_ps();
let (mut xs, mut y0s, mut y3s) = (x4, &y0[..x4.len()], &y3[..x4.len()]);
while let ([xa, x_rest @ ..], [ya, y0_rest @ ..], [yb, y3_rest @ ..]) = (xs, y0s, y3s) {
(xs, y0s, y3s) = (x_rest, y0_rest, y3_rest);
let x0 = load_f32x4(xa);
let yj = load_f32x4(ya);
let y3 = load_f32x4(yb);
xsum1 = _mm_add_ps(xsum1, _mm_mul_ps(_mm_shuffle_ps(x0, x0, 0x00), yj));
xsum2 = _mm_add_ps(
xsum2,
_mm_mul_ps(_mm_shuffle_ps(x0, x0, 0x55), _mm_shuffle_ps(yj, y3, 0x49)),
);
xsum1 = _mm_add_ps(
xsum1,
_mm_mul_ps(_mm_shuffle_ps(x0, x0, 0xaa), _mm_shuffle_ps(yj, y3, 0x9e)),
);
xsum2 = _mm_add_ps(xsum2, _mm_mul_ps(_mm_shuffle_ps(x0, x0, 0xff), y3));
}
let done = len - x_tail.len();
for (k, (&xj, w)) in x_tail.iter().zip(y[done..].windows(4)).enumerate() {
let v = _mm_mul_ps(_mm_set1_ps(xj), load_f32x4(array_at(w, 0)));
if k % 2 == 0 {
xsum1 = _mm_add_ps(xsum1, v);
} else {
xsum2 = _mm_add_ps(xsum2, v);
}
}
store_f32x4(sum, _mm_add_ps(xsum1, xsum2));
}
#[cfg(all(target_arch = "x86_64", target_feature = "sse"))]
#[target_feature(enable = "sse")]
#[inline]
fn pitch_xcorr_sse(x: &[f32], y: &[f32], xcorr: &mut [f32], len: usize, max_pitch: usize) {
assert!(x.len() >= len && y.len() + 1 >= len + max_pitch);
let mut i = 0;
while i + 4 <= max_pitch {
let mut sum = [0.0f32; 4];
xcorr_kernel_sse(x, &y[i..], &mut sum, len);
xcorr[i] = sum[0];
xcorr[i + 1] = sum[1];
xcorr[i + 2] = sum[2];
xcorr[i + 3] = sum[3];
i += 4;
}
for j in i..max_pitch {
xcorr[j] = inner_prod_sse(x, &y[j..], len);
}
}
#[cfg(any(target_arch = "x86", target_arch = "x86_64"))]
#[target_feature(enable = "avx,fma")]
fn inner_prod_avx(x: &[f32], y: &[f32], n: usize) -> f32 {
#[cfg(target_arch = "x86")]
use core::arch::x86::*;
#[cfg(target_arch = "x86_64")]
use core::arch::x86_64::*;
assert!(x.len() >= n && y.len() >= n);
let mut acc0 = _mm256_setzero_ps();
let mut acc1 = _mm256_setzero_ps();
let mut i = 0usize;
unsafe {
while i + 16 <= n {
let x0 = _mm256_loadu_ps(x.as_ptr().add(i));
let y0 = _mm256_loadu_ps(y.as_ptr().add(i));
acc0 = _mm256_fmadd_ps(x0, y0, acc0);
let x1 = _mm256_loadu_ps(x.as_ptr().add(i + 8));
let y1 = _mm256_loadu_ps(y.as_ptr().add(i + 8));
acc1 = _mm256_fmadd_ps(x1, y1, acc1);
i += 16;
}
while i + 8 <= n {
let x0 = _mm256_loadu_ps(x.as_ptr().add(i));
let y0 = _mm256_loadu_ps(y.as_ptr().add(i));
acc0 = _mm256_fmadd_ps(x0, y0, acc0);
i += 8;
}
}
let acc = _mm256_add_ps(acc0, acc1);
let hi = _mm256_extractf128_ps(acc, 1);
let lo = _mm256_castps256_ps128(acc);
let sum4 = _mm_add_ps(lo, hi);
let tmp = _mm_movehl_ps(sum4, sum4);
let sum2 = _mm_add_ps(sum4, tmp);
let tmp2 = _mm_shuffle_ps(sum2, sum2, 0x55);
let mut result = _mm_cvtss_f32(_mm_add_ss(sum2, tmp2));
while i < n {
result += x[i] * y[i];
i += 1;
}
result
}
#[cfg(any(target_arch = "x86", target_arch = "x86_64"))]
#[target_feature(enable = "avx,fma")]
fn dual_inner_prod_avx(x: &[f32], y1: &[f32], y2: &[f32], n: usize) -> (f32, f32) {
#[cfg(target_arch = "x86")]
use core::arch::x86::*;
#[cfg(target_arch = "x86_64")]
use core::arch::x86_64::*;
use crate::simd_mem::x86::load_f32x8;
assert!(x.len() >= n && y1.len() >= n && y2.len() >= n);
let (x8, x_tail) = x[..n].as_chunks::<8>();
let (y18, y1_tail) = y1[..n].as_chunks::<8>();
let (y28, y2_tail) = y2[..n].as_chunks::<8>();
let (x16, x8) = x8.as_chunks::<2>();
let (y116, y18) = y18.as_chunks::<2>();
let (y216, y28) = y28.as_chunks::<2>();
let mut acc1 = _mm256_setzero_ps();
let mut acc2 = _mm256_setzero_ps();
let mut acc1b = _mm256_setzero_ps();
let mut acc2b = _mm256_setzero_ps();
let (mut xs, mut y1s, mut y2s) = (x16, &y116[..x16.len()], &y216[..x16.len()]);
while let ([xa, x_rest @ ..], [y1a, y1_rest @ ..], [y2a, y2_rest @ ..]) = (xs, y1s, y2s) {
(xs, y1s, y2s) = (x_rest, y1_rest, y2_rest);
let xv0 = load_f32x8(&xa[0]);
let xv1 = load_f32x8(&xa[1]);
let y1v0 = load_f32x8(&y1a[0]);
let y2v0 = load_f32x8(&y2a[0]);
let y1v1 = load_f32x8(&y1a[1]);
let y2v1 = load_f32x8(&y2a[1]);
acc1 = _mm256_fmadd_ps(xv0, y1v0, acc1);
acc2 = _mm256_fmadd_ps(xv0, y2v0, acc2);
acc1b = _mm256_fmadd_ps(xv1, y1v1, acc1b);
acc2b = _mm256_fmadd_ps(xv1, y2v1, acc2b);
}
if let ([xa], [y1a], [y2a]) = (x8, y18, y28) {
let xv = load_f32x8(xa);
let y1v = load_f32x8(y1a);
let y2v = load_f32x8(y2a);
acc1 = _mm256_fmadd_ps(xv, y1v, acc1);
acc2 = _mm256_fmadd_ps(xv, y2v, acc2);
}
let acc1 = _mm256_add_ps(acc1, acc1b);
let acc2 = _mm256_add_ps(acc2, acc2b);
let hi1 = _mm256_extractf128_ps(acc1, 1);
let lo1 = _mm256_castps256_ps128(acc1);
let sum41 = _mm_add_ps(lo1, hi1);
let t11 = _mm_movehl_ps(sum41, sum41);
let s21 = _mm_add_ps(sum41, t11);
let t12 = _mm_shuffle_ps(s21, s21, 0x55);
let mut s1 = _mm_cvtss_f32(_mm_add_ss(s21, t12));
let hi2 = _mm256_extractf128_ps(acc2, 1);
let lo2 = _mm256_castps256_ps128(acc2);
let sum42 = _mm_add_ps(lo2, hi2);
let t21 = _mm_movehl_ps(sum42, sum42);
let s22 = _mm_add_ps(sum42, t21);
let t22 = _mm_shuffle_ps(s22, s22, 0x55);
let mut s2 = _mm_cvtss_f32(_mm_add_ss(s22, t22));
for ((&a, &b1), &b2) in x_tail.iter().zip(y1_tail).zip(y2_tail) {
s1 += a * b1;
s2 += a * b2;
}
(s1, s2)
}
#[cfg(any(target_arch = "x86", target_arch = "x86_64"))]
#[target_feature(enable = "avx,fma")]
fn pitch_xcorr_avx(x: &[f32], y: &[f32], xcorr: &mut [f32], len: usize, max_pitch: usize) {
assert!(x.len() >= len && y.len() + 1 >= len + max_pitch);
let mut i = 0;
while i + 4 <= max_pitch {
let mut sum = [0.0f32; 4];
xcorr_kernel_avx(x, &y[i..], &mut sum, len);
xcorr[i] = sum[0];
xcorr[i + 1] = sum[1];
xcorr[i + 2] = sum[2];
xcorr[i + 3] = sum[3];
i += 4;
}
for j in i..max_pitch {
xcorr[j] = inner_prod_avx(x, &y[j..], len);
}
}
#[cfg(any(target_arch = "x86", target_arch = "x86_64"))]
#[target_feature(enable = "avx,fma")]
fn xcorr_kernel_avx(x: &[f32], y: &[f32], sum: &mut [f32; 4], len: usize) {
#[cfg(target_arch = "x86")]
use core::arch::x86::*;
#[cfg(target_arch = "x86_64")]
use core::arch::x86_64::*;
use crate::simd_mem::array_at;
use crate::simd_mem::x86::{load_f32x4, store_f32x4};
assert!(x.len() >= len && y.len() >= len + 3);
let (x4, x_tail) = x[..len].as_chunks::<4>();
let y0 = y[..len].as_chunks::<4>().0;
let y3 = y[3..len + 3].as_chunks::<4>().0;
let (x8, x4) = x4.as_chunks::<2>();
let (y08, y0) = y0.as_chunks::<2>();
let (y38, y3) = y3.as_chunks::<2>();
let mut xsum1 = load_f32x4(sum);
let mut xsum2 = _mm_setzero_ps();
let (mut xs, mut y0s, mut y3s) = (x8, &y08[..x8.len()], &y38[..x8.len()]);
while let ([xa, x_rest @ ..], [ya, y0_rest @ ..], [yb, y3_rest @ ..]) = (xs, y0s, y3s) {
(xs, y0s, y3s) = (x_rest, y0_rest, y3_rest);
let x0 = load_f32x4(&xa[0]);
let yj = load_f32x4(&ya[0]);
let y3 = load_f32x4(&yb[0]);
xsum1 = _mm_fmadd_ps(_mm_shuffle_ps(x0, x0, 0x00), yj, xsum1);
xsum2 = _mm_fmadd_ps(
_mm_shuffle_ps(x0, x0, 0x55),
_mm_shuffle_ps(yj, y3, 0x49),
xsum2,
);
xsum1 = _mm_fmadd_ps(
_mm_shuffle_ps(x0, x0, 0xaa),
_mm_shuffle_ps(yj, y3, 0x9e),
xsum1,
);
xsum2 = _mm_fmadd_ps(_mm_shuffle_ps(x0, x0, 0xff), y3, xsum2);
let x1 = load_f32x4(&xa[1]);
let yj4 = load_f32x4(&ya[1]);
let y7 = load_f32x4(&yb[1]);
xsum1 = _mm_fmadd_ps(_mm_shuffle_ps(x1, x1, 0x00), yj4, xsum1);
xsum2 = _mm_fmadd_ps(
_mm_shuffle_ps(x1, x1, 0x55),
_mm_shuffle_ps(yj4, y7, 0x49),
xsum2,
);
xsum1 = _mm_fmadd_ps(
_mm_shuffle_ps(x1, x1, 0xaa),
_mm_shuffle_ps(yj4, y7, 0x9e),
xsum1,
);
xsum2 = _mm_fmadd_ps(_mm_shuffle_ps(x1, x1, 0xff), y7, xsum2);
}
if let ([xa], [ya], [yb]) = (x4, y0, y3) {
let x0 = load_f32x4(xa);
let yj = load_f32x4(ya);
let y3 = load_f32x4(yb);
xsum1 = _mm_fmadd_ps(_mm_shuffle_ps(x0, x0, 0x00), yj, xsum1);
xsum2 = _mm_fmadd_ps(
_mm_shuffle_ps(x0, x0, 0x55),
_mm_shuffle_ps(yj, y3, 0x49),
xsum2,
);
xsum1 = _mm_fmadd_ps(
_mm_shuffle_ps(x0, x0, 0xaa),
_mm_shuffle_ps(yj, y3, 0x9e),
xsum1,
);
xsum2 = _mm_fmadd_ps(_mm_shuffle_ps(x0, x0, 0xff), y3, xsum2);
}
let done = len - x_tail.len();
for (k, (&xj, w)) in x_tail.iter().zip(y[done..].windows(4)).enumerate() {
let yv = load_f32x4(array_at(w, 0));
if k % 2 == 0 {
xsum1 = _mm_fmadd_ps(_mm_set1_ps(xj), yv, xsum1);
} else {
xsum2 = _mm_fmadd_ps(_mm_set1_ps(xj), yv, xsum2);
}
}
store_f32x4(sum, _mm_add_ps(xsum1, xsum2));
}
pub fn pitch_downsample(x: &[&[f32]], x_lp: &mut [f32], len: usize, c: usize, factor: usize) {
let offset = factor / 2;
if x_lp.len() < len || len == 0 {
return;
}
#[cfg(target_arch = "aarch64")]
if factor == 2
&& (c == 1 || c == 2)
&& len >= 8
&& x[..c].iter().all(|x_k| x_k.len() >= 2 * len)
{
let mut ac = [0.0f32; 5];
unsafe {
if c == 1 {
decimate2_autocorr5_neon([x[0]], &mut x_lp[..len], &mut ac);
} else {
decimate2_autocorr5_neon([x[0], x[1]], &mut x_lp[..len], &mut ac);
}
}
flatten_with(&mut x_lp[..len], ac);
return;
}
let mut full = 1;
if factor == 2 && (c == 1 || c == 2) {
let shortest = x[..c].iter().map(|x_k| x_k.len()).min().unwrap_or(0);
let avail = len.min(shortest / 2);
if avail >= 2 {
let out = &mut x_lp[..avail];
if c == 1 {
decimate2([x[0]], out);
} else {
decimate2([x[0], x[1]], out);
}
full = avail;
}
}
for i in full..len {
let mut val = 0.0f32;
for (k, x_k) in x[..c].iter().enumerate() {
let idx_p = factor * i + offset;
if idx_p < x_k.len() {
let v = lowpass3(x_k[factor * i - offset], x_k[factor * i], x_k[idx_p]);
val = if k == 0 { v } else { val + v };
}
}
x_lp[i] = val;
}
let mut val = 0.0f32;
for (k, x_k) in x[..c].iter().enumerate() {
if offset < x_k.len() {
let v = mac(0.5 * x_k[0], x_k[offset], 0.25);
val = if k == 0 { v } else { val + v };
}
}
x_lp[0] = val;
flatten_spectrum(&mut x_lp[..len]);
}
#[inline(always)]
fn lowpass3(before: f32, centre: f32, after: f32) -> f32 {
mac(mac(0.25 * after, before, 0.25), centre, 0.5)
}
fn flatten_spectrum(x_lp: &mut [f32]) {
let len = x_lp.len();
if len < 5 {
return;
}
let mut ac = [0.0f32; 5];
autocorr(x_lp, &mut ac, None, 0, 4, len);
flatten_with(x_lp, ac);
}
fn flatten_with(x_lp: &mut [f32], mut ac: [f32; 5]) {
ac[0] *= 1.0001;
for (i, a) in ac.iter_mut().enumerate().skip(1) {
let f = 0.008 * i as f32;
*a = mac(*a, -(*a * f), f);
}
let mut coef = [0.0f32; 4];
lpc(&mut coef, &ac, 4);
let mut tmp = 1.0f32;
for v in coef.iter_mut() {
tmp *= 0.9;
*v *= tmp;
}
let c1 = 0.8f32;
let lpc2 = [
coef[0] + 0.8,
mac(coef[1], c1, coef[0]),
mac(coef[2], c1, coef[1]),
mac(coef[3], c1, coef[2]),
c1 * coef[3],
];
celt_fir5(x_lp, &lpc2);
}
fn celt_fir5(x: &mut [f32], num: &[f32; 5]) {
#[cfg(target_arch = "aarch64")]
unsafe {
celt_fir5_neon(x, num);
}
#[cfg(not(target_arch = "aarch64"))]
celt_fir5_scalar(x, num, [0.0; 5]);
}
#[inline(always)]
fn celt_fir5_scalar(x: &mut [f32], num: &[f32; 5], mut mem: [f32; 5]) {
for v in x.iter_mut() {
let mut sum = *v;
for (&n, &m) in num.iter().zip(&mem) {
sum = mac(sum, n, m);
}
mem = [*v, mem[0], mem[1], mem[2], mem[3]];
*v = sum;
}
}
#[cfg(target_arch = "aarch64")]
#[target_feature(enable = "neon")]
fn celt_fir5_neon(x: &mut [f32], num: &[f32; 5]) {
use crate::simd_mem::neon::{load_f32x4, store_f32x4};
use core::arch::aarch64::*;
let (blocks, x_tail) = x.as_chunks_mut::<4>();
let (steps, blocks) = blocks.as_chunks_mut::<4>();
let taps = [
vdupq_n_f32(num[0]),
vdupq_n_f32(num[1]),
vdupq_n_f32(num[2]),
vdupq_n_f32(num[3]),
vdupq_n_f32(num[4]),
];
let delayed = |prev2: float32x4_t, prev: float32x4_t, cur: float32x4_t| {
[
vextq_f32::<3>(prev, cur),
vextq_f32::<2>(prev, cur),
vextq_f32::<1>(prev, cur),
prev,
vextq_f32::<3>(prev2, prev),
]
};
let mut prev2 = vdupq_n_f32(0.0);
let mut prev = vdupq_n_f32(0.0);
for step in steps.iter_mut() {
let cur: [float32x4_t; 4] = core::array::from_fn(|k| load_f32x4(&step[k]));
let m = [
delayed(prev2, prev, cur[0]),
delayed(prev, cur[0], cur[1]),
delayed(cur[0], cur[1], cur[2]),
delayed(cur[1], cur[2], cur[3]),
];
let mut sum = cur;
for (tap, &t) in taps.iter().enumerate() {
for k in 0..4 {
sum[k] = vfmaq_f32(sum[k], t, m[k][tap]);
}
}
for (block, s) in step.iter_mut().zip(&sum) {
store_f32x4(block, *s);
}
prev2 = cur[2];
prev = cur[3];
}
for block in blocks.iter_mut() {
let cur = load_f32x4(block);
let m = delayed(prev2, prev, cur);
let mut sum = cur;
for (&t, &d) in taps.iter().zip(&m) {
sum = vfmaq_f32(sum, t, d);
}
store_f32x4(block, sum);
prev2 = prev;
prev = cur;
}
let mem = [
vgetq_lane_f32::<3>(prev),
vgetq_lane_f32::<2>(prev),
vgetq_lane_f32::<1>(prev),
vgetq_lane_f32::<0>(prev),
vgetq_lane_f32::<3>(prev2),
];
celt_fir5_scalar(x_tail, num, mem);
}
#[inline(always)]
fn decimate2<const C: usize>(x: [&[f32]; C], out: &mut [f32]) {
#[cfg(target_arch = "aarch64")]
unsafe {
decimate2_neon(x, out);
}
#[cfg(not(target_arch = "aarch64"))]
decimate2_scalar(x, out);
}
#[inline(always)]
fn decimate2_scalar<const C: usize>(x: [&[f32]; C], out: &mut [f32]) {
let n = out.len();
if n < 2 {
return;
}
for (k, x_k) in x.iter().enumerate() {
let (before, _) = x_k[1..2 * n - 1].as_chunks::<2>();
let (after, _) = x_k[2..2 * n].as_chunks::<2>();
for ((o, b), a) in out[1..].iter_mut().zip(before).zip(after) {
let val = lowpass3(b[0], a[0], a[1]);
*o = if k == 0 { val } else { *o + val };
}
}
}
#[cfg(target_arch = "aarch64")]
#[target_feature(enable = "neon")]
#[inline]
fn decimate2_neon<const C: usize>(x: [&[f32]; C], out: &mut [f32]) {
use crate::simd_mem::neon::{load2_f32x4, load_dup_f32x4, store_f32x4};
use core::arch::aarch64::*;
let n = out.len();
assert!(x.iter().all(|x_k| x_k.len() >= 2 * n));
let mut i = 1;
if n >= 5 {
let quarter = vdupq_n_f32(0.25);
let half = vdupq_n_f32(0.5);
let (passes, _) = out[1..].as_chunks_mut::<4>();
let x8: [&[[f32; 8]]; C] = x.map(|x_k| &x_k[2..].as_chunks::<8>().0[..passes.len()]);
let mut odd_prev = [vdupq_n_f32(0.0); C];
for (o, x_k) in odd_prev.iter_mut().zip(&x) {
*o = load_dup_f32x4(&x_k[1]);
}
for p in 0..passes.len() {
let mut val = vdupq_n_f32(0.0);
for (k, (o, x_k)) in odd_prev.iter_mut().zip(&x8).enumerate() {
let pair = load2_f32x4(&x_k[p]);
let before = vextq_f32::<3>(*o, pair.1);
let v = vfmaq_f32(
vfmaq_f32(vmulq_f32(pair.1, quarter), before, quarter),
pair.0,
half,
);
val = if k == 0 { v } else { vaddq_f32(val, v) };
*o = pair.1;
}
store_f32x4(&mut passes[p], val);
}
i += 4 * passes.len();
}
decimate2_scalar(x.map(|x_k| &x_k[2 * (i - 1)..]), &mut out[i - 1..]);
}
#[cfg(target_arch = "aarch64")]
#[target_feature(enable = "neon")]
fn decimate2_autocorr5_neon<const C: usize>(x: [&[f32]; C], x_lp: &mut [f32], ac: &mut [f32; 5]) {
use crate::simd_mem::neon::{load2_f32x4, load_f32x4, store_f32x4};
use crate::simd_mem::{array_at, array_at_mut};
use core::arch::aarch64::*;
let n = x_lp.len();
assert!(n >= 8 && x.iter().all(|x_k| x_k.len() >= 2 * n));
let (out, x_lp_tail) = x_lp.as_chunks_mut::<4>();
let blocks = out.len();
let x8: [&[[f32; 8]]; C] = x.map(|x_k| &x_k.as_chunks::<8>().0[..blocks]);
let quarter = vdupq_n_f32(0.25);
let half = vdupq_n_f32(0.5);
let mut odd_prev = [vdupq_n_f32(0.0); C];
let block = |m: usize, odd_prev: &mut [float32x4_t; C]| {
let mut val = vdupq_n_f32(0.0);
for (k, (o, x_k)) in odd_prev.iter_mut().zip(&x8).enumerate() {
let pair = load2_f32x4(&x_k[m]);
let before = vextq_f32::<3>(*o, pair.1);
let v = vfmaq_f32(
vfmaq_f32(vmulq_f32(pair.1, quarter), before, quarter),
pair.0,
half,
);
val = if k == 0 { v } else { vaddq_f32(val, v) };
*o = pair.1;
}
val
};
let mut first = 0.0f32;
for (k, x_k) in x.iter().enumerate() {
let v = mac(0.5 * x_k[0], x_k[1], 0.25);
first = if k == 0 { v } else { first + v };
}
let mut cur = vsetq_lane_f32::<0>(first, block(0, &mut odd_prev));
store_f32x4(&mut out[0], cur);
let mut by_sample = vdupq_n_f32(0.0);
let mut lag4 = vdupq_n_f32(0.0);
let step = |by_sample: &mut float32x4_t,
lag4: &mut float32x4_t,
cur: float32x4_t,
next: float32x4_t| {
let mut s = *by_sample;
s = vfmaq_laneq_f32::<0>(s, cur, cur);
s = vfmaq_laneq_f32::<1>(s, vextq_f32::<1>(cur, next), cur);
s = vfmaq_laneq_f32::<2>(s, vextq_f32::<2>(cur, next), cur);
s = vfmaq_laneq_f32::<3>(s, vextq_f32::<3>(cur, next), cur);
*by_sample = s;
*lag4 = vfmaq_f32(*lag4, cur, next);
};
let mut next = block(1, &mut odd_prev);
store_f32x4(&mut out[1], next);
for m1 in 2..blocks {
let after = block(m1, &mut odd_prev);
step(&mut by_sample, &mut lag4, cur, next);
store_f32x4(&mut out[m1], after);
cur = next;
next = after;
}
step(&mut by_sample, &mut lag4, cur, next);
for (i, o) in (4 * blocks..).zip(x_lp_tail) {
let mut val = 0.0f32;
for (k, x_k) in x.iter().enumerate() {
let v = lowpass3(x_k[2 * i - 1], x_k[2 * i], x_k[2 * i + 1]);
val = if k == 0 { v } else { val + v };
}
*o = val;
}
let mut lag4 = lanes_sum(lag4);
for w in x_lp[4 * (blocks - 1)..].windows(5) {
let xj = w[0];
by_sample = vfmaq_f32(by_sample, load_f32x4(array_at(w, 0)), vdupq_n_f32(xj));
lag4 = mac(lag4, xj, w[4]);
}
store_f32x4(array_at_mut(ac, 0), by_sample);
ac[4] = lag4;
let fast_n = n - 4;
for (k, out) in ac.iter_mut().enumerate() {
let mut d = 0.0f32;
for (&a, &b) in x_lp[k + fast_n..].iter().zip(&x_lp[fast_n..]) {
d = mac(d, a, b);
}
*out += d;
}
}
struct BestPitch {
num: [f32; 2],
den: [f32; 2],
pitch: [usize; 2],
}
impl BestPitch {
fn new() -> Self {
Self {
num: [-1.0, -1.0],
den: [0.0, 0.0],
pitch: [0, 1],
}
}
#[inline(always)]
fn take(&mut self, i: usize, num: f32, syy: f32) {
if num * self.den[0] > self.num[0] * syy {
self.num[1] = self.num[0];
self.den[1] = self.den[0];
self.pitch[1] = self.pitch[0];
self.num[0] = num;
self.den[0] = syy;
self.pitch[0] = i;
} else {
self.num[1] = num;
self.den[1] = syy;
self.pitch[1] = i;
}
}
#[inline(always)]
fn consider(&mut self, i: usize, xc: f32, syy: f32) {
if xc > 0.0 {
let xc16 = xc * 1e-12;
let num = xc16 * xc16;
if num * self.den[1] > self.num[1] * syy {
self.take(i, num, syy);
}
}
}
}
fn best_pitch_from(xcorr: &[f32], syy: &[f32], best_pitch: &mut [usize; 2]) {
let n = xcorr.len();
let syy = &syy[..n];
let mut best = BestPitch::new();
#[cfg(target_arch = "aarch64")]
let i = unsafe { best_pitch_neon(xcorr, syy, &mut best) };
#[cfg(not(target_arch = "aarch64"))]
let i = 0;
for k in i..n {
best.consider(k, xcorr[k], syy[k]);
}
*best_pitch = best.pitch;
}
#[cfg(target_arch = "aarch64")]
#[target_feature(enable = "neon")]
#[inline]
fn best_pitch_neon(xcorr: &[f32], syy: &[f32], best: &mut BestPitch) -> usize {
use crate::simd_mem::neon::{load_f32x4, load_u32x4, store_f32x4};
use core::arch::aarch64::*;
let (xc8, _) = xcorr.as_chunks::<8>();
let syy8 = &syy.as_chunks::<8>().0[..xc8.len()];
let zero = vdupq_n_f32(0.0);
let bits = [load_u32x4(&[1, 2, 4, 8]), load_u32x4(&[16, 32, 64, 128])];
for (g, (xa, sa)) in xc8.iter().zip(syy8).enumerate() {
let i = 8 * g;
let (xa, sa) = (xa.as_chunks::<4>().0, sa.as_chunks::<4>().0);
let xc = [load_f32x4(&xa[0]), load_f32x4(&xa[1])];
let s = [load_f32x4(&sa[0]), load_f32x4(&sa[1])];
let num = xc.map(|v| {
let xc16 = vmulq_n_f32(v, 1e-12);
vmulq_f32(xc16, xc16)
});
let positive = [vcgtq_f32(xc[0], zero), vcgtq_f32(xc[1], zero)];
let mut from = 0;
loop {
let mut mask = 0u32;
for h in 0..2 {
let beats = vcgtq_f32(
vmulq_n_f32(num[h], best.den[1]),
vmulq_n_f32(s[h], best.num[1]),
);
mask |= vaddvq_u32(vandq_u32(vandq_u32(beats, positive[h]), bits[h]));
}
mask &= !0u32 << from;
if mask == 0 {
break;
}
let lane = mask.trailing_zeros() as usize;
let (h, l) = (lane / 4, lane % 4);
let mut nums = [0.0f32; 4];
store_f32x4(&mut nums, num[h]);
best.take(i + lane, nums[l], syy[i + lane]);
from = lane + 1;
}
}
8 * xc8.len()
}
struct WindowEnergies<'a> {
leaving: &'a [f32],
entering: &'a [f32],
syy: f32,
next: usize,
table: &'a mut [MaybeUninit<f32>],
}
impl<'a> WindowEnergies<'a> {
fn new(y: &'a [f32], len: usize, first: f32, table: &'a mut [MaybeUninit<f32>]) -> Self {
let n = table.len();
Self {
leaving: &y[..n],
entering: &y[len..len + n],
syy: first,
next: 0,
table,
}
}
fn fill_exact(&mut self, end: usize) {
for i in self.next..end {
self.table[i].write(self.syy);
let (old, new) = (self.leaving[i], self.entering[i]);
self.syy += mac(-(old * old), new, new);
if self.syy < 1.0 {
self.syy = 1.0;
}
}
self.next = end;
}
fn increments(&mut self, end: usize) {
let range = self.next..end;
let (old, new) = (&self.leaving[range.clone()], &self.entering[range.clone()]);
let out = &mut self.table[range];
#[cfg(target_arch = "aarch64")]
let done = unsafe { increments_neon(old, new, out) };
#[cfg(not(target_arch = "aarch64"))]
let done = 0;
for ((d, &o), &w) in out[done..].iter_mut().zip(&old[done..]).zip(&new[done..]) {
d.write(mac(-(o * o), w, w));
}
}
fn filled(&self) -> &[f32] {
unsafe { assume_written(&self.table[..self.next]) }
}
fn fill_to<B: Beside>(&mut self, end: usize, beside: &mut B) {
if end <= self.next {
return;
}
let (start, first) = (self.next, self.syy);
self.increments(end);
let table = unsafe { assume_written_mut(&mut self.table[start..end]) };
let mut s = first;
for part in table.chunks_mut(B::EVERY) {
for t in part {
let d = *t;
*t = s;
s += d;
}
beside.fill(1);
}
self.syy = s;
self.next = end;
if below_one(table) || s < 1.0 {
core::hint::cold_path();
(self.next, self.syy) = (start, first);
self.fill_exact(end);
}
}
fn fill_both<B: Beside>(a: &mut Self, b: &mut Self, count: usize, beside: &mut B) {
let (ia, ib) = (a.next, b.next);
let (fa, fb) = (a.syy, b.syy);
a.increments(ia + count);
b.increments(ib + count);
let ta = unsafe { assume_written_mut(&mut a.table[ia..ia + count]) };
let tb = unsafe { assume_written_mut(&mut b.table[ib..ib + count]) };
let (mut sa, mut sb) = (fa, fb);
for (pa, pb) in ta.chunks_mut(B::EVERY).zip(tb.chunks_mut(B::EVERY)) {
for (ta, tb) in pa.iter_mut().zip(pb) {
let (da, db) = (*ta, *tb);
*ta = sa;
*tb = sb;
sa += da;
sb += db;
}
beside.fill(1);
}
(a.syy, b.syy) = (sa, sb);
a.next += count;
b.next += count;
let (redo_a, redo_b) = (below_one(ta) || sa < 1.0, below_one(tb) || sb < 1.0);
if redo_a {
core::hint::cold_path();
(a.next, a.syy) = (ia, fa);
a.fill_exact(ia + count);
}
if redo_b {
core::hint::cold_path();
(b.next, b.syy) = (ib, fb);
b.fill_exact(ib + count);
}
}
}
#[cfg(target_arch = "aarch64")]
#[target_feature(enable = "neon")]
#[inline]
fn increments_neon(old: &[f32], new: &[f32], out: &mut [MaybeUninit<f32>]) -> usize {
use crate::simd_mem::neon::{load_f32x4, store_f32x4_uninit};
use core::arch::aarch64::*;
let (out4, _) = out.as_chunks_mut::<4>();
let old4 = &old.as_chunks::<4>().0[..out4.len()];
let new4 = &new.as_chunks::<4>().0[..out4.len()];
for ((d, oa), wa) in out4.iter_mut().zip(old4).zip(new4) {
let o = load_f32x4(oa);
let w = load_f32x4(wa);
let d4 = vfmaq_f32(vnegq_f32(vmulq_f32(o, o)), w, w);
store_f32x4_uninit(d, d4);
}
4 * out4.len()
}
#[inline(always)]
unsafe fn assume_written(v: &[MaybeUninit<f32>]) -> &[f32] {
unsafe { core::slice::from_raw_parts(v.as_ptr().cast(), v.len()) }
}
#[inline(always)]
unsafe fn assume_written_mut(v: &mut [MaybeUninit<f32>]) -> &mut [f32] {
unsafe { core::slice::from_raw_parts_mut(v.as_mut_ptr().cast(), v.len()) }
}
fn below_one(v: &[f32]) -> bool {
#[allow(unused_mut)]
let mut done = 0;
#[cfg(target_arch = "aarch64")]
{
use core::arch::aarch64::*;
unsafe {
let one = vdupq_n_f32(1.0);
let mut any = vdupq_n_u32(0);
while done + 4 <= v.len() {
any = vorrq_u32(any, vcltq_f32(vld1q_f32(v.as_ptr().add(done)), one));
done += 4;
}
if vmaxvq_u32(any) != 0 {
return true;
}
}
}
v[done..].iter().any(|&x| x < 1.0)
}
fn fine_xcorr<B: Beside>(
x_lp: &[f32],
y: &[f32],
len: usize,
best_pitch: &[usize; 2],
max_pitch: usize,
beside: &mut B,
) -> FineLags {
let near = |i: usize, coarse: usize| (i as i32 - 2 * coarse as i32).abs() <= 2;
let mut lags = [0usize; 10];
let mut count = 0;
for (c, &coarse) in best_pitch.iter().enumerate() {
let first = (2 * coarse).saturating_sub(2);
let end = (2 * coarse + 3).min(max_pitch);
for i in first..end {
if c == 1 && near(i, best_pitch[0]) {
continue;
}
lags[count] = i;
count += 1;
}
}
let mut sums = [0.0f32; 10];
if B::ACTIVE {
inner_prod_many_side(
x_lp,
y,
&lags[..count],
len,
&mut sums[..count],
&mut || beside.fill(1),
);
} else {
inner_prod_many(x_lp, y, &lags[..count], len, &mut sums[..count]);
}
let mut fine = FineLags {
lags,
xcorr: [0.0; 10],
count,
end: 0,
};
for (k, (&i, &sum)) in lags[..count].iter().zip(&sums).enumerate() {
fine.xcorr[k] = if -1.0 > sum { -1.0 } else { sum };
if sum > 0.0 {
fine.end = fine.end.max(i + 1);
}
}
for i in 1..count {
let mut j = i;
while j > 0 && fine.lags[j - 1] > fine.lags[j] {
fine.lags.swap(j - 1, j);
fine.xcorr.swap(j - 1, j);
j -= 1;
}
}
fine
}
struct FineLags {
lags: [usize; 10],
xcorr: [f32; 10],
count: usize,
end: usize,
}
impl FineLags {
fn at(&self, i: usize) -> f32 {
self.lags[..self.count]
.iter()
.position(|&lag| lag == i)
.map_or(0.0, |k| self.xcorr[k])
}
}
fn coarse_xcorr<B: Beside>(
x4: &[f32],
y4: &[f32],
y: &[f32],
xcorr: &mut [f32],
beside: &mut B,
) -> (f32, f32) {
let len4 = x4.len();
#[allow(unused_mut)]
let (mut coarse, mut fine) = (Energy::new(&y4[..len4]), Energy::new(y));
#[cfg(not(target_arch = "aarch64"))]
let _ = beside;
#[cfg(target_arch = "aarch64")]
if xcorr.len() >= 64 && len4 >= 8 {
let lags = xcorr.len();
assert!(y4.len() >= len4 + lags - 1);
let by_sample = lags & !3;
let (grouped, rest) = xcorr.split_at_mut(by_sample);
if USE_SKEW && by_sample >= SKEW_PASS && len4 >= 4 * SKEW_MIN_BLOCKS {
let calls = skew_side_calls(len4, by_sample).max(1);
let per_call = y.len().max(len4).div_ceil(calls);
let mut side = || {
Energy::step_both(&mut fine, &mut coarse, per_call);
beside.fill(BESIDE_PER_SKEW_CALL);
};
xcorr_skewed_neon(x4, y4, grouped, &mut side);
} else {
let mut side = || {
fine.step(4);
coarse.step(2);
beside.fill(BESIDE_PER_STEP);
};
xcorr_by_sample_passes_side_neon::<16, _>(x4, y4, grouped, &mut side);
}
let mut offsets = [0usize; 3];
for (k, off) in offsets[..rest.len()].iter_mut().enumerate() {
*off = by_sample + k;
}
inner_prod_many(x4, y4, &offsets[..rest.len()], len4, rest);
return (coarse.finish(), fine.finish());
}
pitch_xcorr(x4, y4, xcorr, len4, xcorr.len());
(coarse.finish(), fine.finish())
}
struct Energy<'a> {
y: &'a [f32],
pos: usize,
sum: f32,
}
impl<'a> Energy<'a> {
fn new(y: &'a [f32]) -> Self {
Self {
y,
pos: 0,
sum: 1.0,
}
}
#[inline(always)]
fn step(&mut self, count: usize) {
let end = (self.pos + count).min(self.y.len());
for &v in &self.y[self.pos..end] {
self.sum = mac(self.sum, v, v);
}
self.pos = end;
}
fn finish(mut self) -> f32 {
self.step(self.y.len());
self.sum
}
#[cfg_attr(not(target_arch = "aarch64"), allow(dead_code))]
#[inline(always)]
fn step_both(a: &mut Self, b: &mut Self, count: usize) {
let na = count.min(a.y.len() - a.pos);
let nb = count.min(b.y.len() - b.pos);
let both = na.min(nb);
let (mut sa, mut sb) = (a.sum, b.sum);
for (&va, &vb) in a.y[a.pos..a.pos + both]
.iter()
.zip(&b.y[b.pos..b.pos + both])
{
sa = mac(sa, va, va);
sb = mac(sb, vb, vb);
}
(a.sum, b.sum) = (sa, sb);
a.pos += both;
b.pos += both;
a.step(na - both);
b.step(nb - both);
}
}
#[inline(always)]
fn take_even<'b>(src: &[f32], dst: &'b mut [MaybeUninit<f32>]) -> &'b [f32] {
let n = dst.len();
if n == 0 {
return &[];
}
let (pairs, last) = src[..2 * n - 1].as_chunks::<2>();
assert!(pairs.len() == n - 1);
for (d, pair) in dst.iter_mut().zip(pairs) {
d.write(pair[0]);
}
dst[n - 1].write(last[0]);
unsafe { assume_written(dst) }
}
pub fn pitch_search(x_lp: &[f32], y: &[f32], len: usize, max_pitch: usize) -> usize {
pitch_search_beside(x_lp, y, len, max_pitch, &mut ())
}
trait Beside {
const ACTIVE: bool;
const EVERY: usize;
fn fill(&mut self, count: usize);
fn coarse_found(&mut self, best_pitch: &[usize; 2]);
}
impl Beside for () {
const ACTIVE: bool = false;
const EVERY: usize = usize::MAX;
#[inline(always)]
fn fill(&mut self, _count: usize) {}
#[inline(always)]
fn coarse_found(&mut self, _best_pitch: &[usize; 2]) {}
}
#[cfg(target_arch = "aarch64")]
const BESIDE_PER_SKEW_CALL: usize = 4;
#[cfg(target_arch = "aarch64")]
const BESIDE_PER_STEP: usize = 3;
fn pitch_search_beside<B: Beside>(
x_lp: &[f32],
y: &[f32],
len: usize,
max_pitch: usize,
beside: &mut B,
) -> usize {
let mut best_pitch = [0, 0];
let max_pitch = max_pitch >> 1;
let len = len >> 1;
let lag = len + max_pitch;
let coarse_lags = max_pitch >> 1;
let mut x4_buf = [const { MaybeUninit::<f32>::uninit() }; PITCH_BUF_MAX];
let mut y4_buf = [const { MaybeUninit::<f32>::uninit() }; PITCH_BUF_MAX];
let x_lp4 = take_even(x_lp, &mut x4_buf[..len >> 1]);
let y_lp4 = take_even(y, &mut y4_buf[..lag >> 1]);
let mut xcorr: FixedVec<f32, PITCH_BUF_MAX> = FixedVec::new();
xcorr.resize(coarse_lags, 0.0);
let (syy_coarse, syy_fine) = coarse_xcorr(x_lp4, y_lp4, &y[..len], &mut xcorr, beside);
let mut tables = [const { MaybeUninit::<f32>::uninit() }; PITCH_BUF_MAX + PITCH_BUF_MAX / 2];
let (coarse_table, fine_table) = tables[..coarse_lags + max_pitch].split_at_mut(coarse_lags);
let mut coarse = WindowEnergies::new(y_lp4, len >> 1, syy_coarse, coarse_table);
let mut fine = WindowEnergies::new(y, len, syy_fine, fine_table);
WindowEnergies::fill_both(&mut coarse, &mut fine, coarse_lags.min(max_pitch), beside);
coarse.fill_to(coarse_lags, beside);
best_pitch_from(&xcorr, coarse.filled(), &mut best_pitch);
beside.coarse_found(&best_pitch);
let found = fine_xcorr(x_lp, y, len, &best_pitch, max_pitch, beside);
fine.fill_to(found.end, beside);
let syy = fine.filled();
let mut best = BestPitch::new();
for (&i, &xc) in found.lags[..found.count].iter().zip(&found.xcorr) {
if i < found.end {
best.consider(i, xc, syy[i]);
}
}
best_pitch = best.pitch;
let mut offset = 0;
if best_pitch[0] > 0 && best_pitch[0] < max_pitch - 1 {
let a = found.at(best_pitch[0] - 1);
let b = found.at(best_pitch[0]);
let c = found.at(best_pitch[0] + 1);
if (c - a) > 0.7 * (b - a) {
offset = 1;
} else if (a - c) > 0.7 * (b - c) {
offset = -1;
}
}
((2 * best_pitch[0]) as isize).wrapping_sub(offset as isize) as usize
}
fn compute_pitch_gain(xy: f32, xx: f32, yy: f32) -> f32 {
xy / mac(1.0, xx, yy).sqrt()
}
static SECOND_CHECK: [usize; 16] = [0, 0, 3, 2, 3, 2, 5, 2, 3, 2, 3, 2, 5, 2, 3, 2];
const MAX_DOUBLING_CANDS: usize = 14;
const MAX_YY_SIZE: usize = 1024;
#[allow(clippy::too_many_arguments)]
fn doubling_products<'a>(
x: &'a [f32],
max_period: usize,
n: usize,
t0: usize,
cands: &[(usize, usize)],
pair_xy: &mut [(f32, f32); MAX_DOUBLING_CANDS],
end: usize,
started: Option<Energies<'a>>,
own_table: &'a mut [MaybeUninit<f32>; MAX_YY_SIZE],
around_t0: &mut Option<[f32; 3]>,
) -> (f32, f32, Energies<'a>) {
let x_target = &x[max_period..];
#[cfg(target_arch = "aarch64")]
{
#[cfg(not(target_vendor = "apple"))]
let started: Option<Energies> = {
let _ = started;
None
};
const TOTAL: usize = 4 + 2 * MAX_DOUBLING_CANDS;
let mut offsets = [0usize; TOTAL];
offsets[..4].copy_from_slice(&[
max_period,
max_period - t0,
max_period - (t0 - 1),
max_period - (t0 + 1),
]);
for (o, &(t1, t1b)) in offsets[4..].as_chunks_mut::<2>().0.iter_mut().zip(cands) {
*o = [max_period - t1, max_period - t1b];
}
let count = 4 + 2 * cands.len();
let mut sums = [0.0f32; TOTAL];
let (mut energies, first) = match started {
Some(energies) => {
sums[0] = energies.filled()[0];
(energies, 1)
}
None => {
inner_prod_many(x_target, x, &offsets[..4], n, &mut sums[..4]);
let table = &mut own_table[..end];
(Energies::new(x, max_period, n, sums[0], table), 4)
}
};
energies.set_end(end);
let steps = n / 4;
for (offs, out) in offsets[first..count]
.chunks(MANY_PER_PASS)
.zip(sums[first..count].chunks_mut(MANY_PER_PASS))
{
if energies.left() >= 2 * steps {
inner_prod_many_side(x_target, x, offs, n, out, &mut || unsafe {
energies.step2_unchecked()
});
} else {
let per_call = energies.left().div_ceil(steps.max(1));
inner_prod_many_side(x_target, x, offs, n, out, &mut || {
#[cfg(target_vendor = "apple")]
energies.step_runs(per_call);
#[cfg(not(target_vendor = "apple"))]
energies.step(per_call);
});
}
}
energies.finish();
for (p, s) in pair_xy.iter_mut().zip(sums[4..count].as_chunks::<2>().0) {
*p = (s[0], s[1]);
}
*around_t0 = Some([sums[2], sums[1], sums[3]]);
(sums[0], sums[1], energies)
}
#[cfg(not(target_arch = "aarch64"))]
{
let _ = (around_t0, started);
let (xx, xy) = dual_inner_prod(x_target, x_target, &x[max_period - t0..], n);
for (p, &(t1, t1b)) in pair_xy.iter_mut().zip(cands) {
*p = dual_inner_prod(x_target, &x[max_period - t1..], &x[max_period - t1b..], n);
}
let mut energies = Energies::new(x, max_period, n, xx, &mut own_table[..end]);
energies.finish();
(xx, xy, energies)
}
}
struct Energies<'a> {
x: &'a [f32],
max_period: usize,
n: usize,
yy: f32,
next: usize,
end: usize,
table: &'a mut [MaybeUninit<f32>],
}
impl<'a> Energies<'a> {
fn new(
x: &'a [f32],
max_period: usize,
n: usize,
xx: f32,
table: &'a mut [MaybeUninit<f32>],
) -> Self {
assert!(table.len() <= max_period + 1 && max_period + n <= x.len());
table[0].write(xx);
Self {
x,
max_period,
n,
yy: xx,
next: 1,
end: table.len(),
table,
}
}
#[cfg(target_arch = "aarch64")]
fn set_end(&mut self, end: usize) {
self.end = end.min(self.table.len());
}
#[inline(always)]
fn step(&mut self, count: usize) {
let end = (self.next + count).min(self.end);
for i in self.next..end {
let new = self.x[self.max_period - i];
let old = self.x[self.max_period + self.n - i];
self.yy = mac(mac(self.yy, new, new), -old, old);
self.table[i].write(if 0.0 > self.yy { 0.0 } else { self.yy });
}
self.next = self.next.max(end);
}
#[cfg(all(target_arch = "aarch64", target_vendor = "apple"))]
#[inline(always)]
fn step_runs(&mut self, count: usize) {
let (start, end) = (self.next, (self.next + count).min(self.end));
if end <= start {
return;
}
let (at, n) = (self.max_period, self.n);
let entering = &self.x[at + 1 - end..=at - start];
let leaving = &self.x[at + n + 1 - end..=at + n - start];
let mut yy = self.yy;
for ((t, &new), &old) in self.table[start..end]
.iter_mut()
.zip(entering.iter().rev())
.zip(leaving.iter().rev())
{
yy = mac(mac(yy, new, new), -old, old);
t.write(if 0.0 > yy { 0.0 } else { yy });
}
self.yy = yy;
self.next = end;
}
fn filled(&self) -> &[f32] {
unsafe { assume_written(&self.table[..self.next]) }
}
fn finish(&mut self) {
self.step(self.left());
}
fn left(&self) -> usize {
self.end.saturating_sub(self.next)
}
#[cfg(target_arch = "aarch64")]
#[inline(always)]
unsafe fn step2_unchecked(&mut self) {
let i = self.next;
debug_assert!(i + 2 <= self.end && self.end <= self.table.len());
unsafe {
let at = self.x.as_ptr().add(self.max_period - i);
let (new0, new1) = (*at, *at.sub(1));
let (old0, old1) = (*at.add(self.n), *at.add(self.n - 1));
let yy0 = mac(mac(self.yy, new0, new0), -old0, old0);
let yy1 = mac(mac(yy0, new1, new1), -old1, old1);
let table = self.table.as_mut_ptr().add(i).cast::<f32>();
table.write(if 0.0 > yy0 { 0.0 } else { yy0 });
table.add(1).write(if 0.0 > yy1 { 0.0 } else { yy1 });
self.yy = yy1;
}
self.next = i + 2;
}
}
fn doubling_candidates(
t0: usize,
min_period: usize,
max_period: usize,
) -> ([(usize, usize); MAX_DOUBLING_CANDS], usize) {
let mut cands = [(0usize, 0usize); MAX_DOUBLING_CANDS];
let mut n_cands = 0;
for k in 2..=15 {
let t1 = (2 * t0 + k) / (2 * k);
if t1 < min_period {
break;
}
let t1b = if k == 2 {
if t1 + t0 > max_period { t0 } else { t0 + t1 }
} else {
(2 * SECOND_CHECK[k] * t0 + k) / (2 * k)
};
cands[n_cands] = (t1, t1b);
n_cands += 1;
}
(cands, n_cands)
}
pub fn remove_doubling(
x: &[f32],
max_period: usize,
min_period: usize,
n: usize,
t0_ptr: &mut usize,
prev_period: usize,
prev_gain: f32,
) -> f32 {
remove_doubling_from(
x,
max_period,
min_period,
n,
t0_ptr,
prev_period,
prev_gain,
None,
)
}
#[allow(clippy::too_many_arguments)]
fn remove_doubling_from<'a>(
x: &'a [f32],
mut max_period: usize,
mut min_period: usize,
mut n: usize,
t0_ptr: &mut usize,
mut prev_period: usize,
prev_gain: f32,
started: Option<Energies<'a>>,
) -> f32 {
let min_period0 = min_period;
max_period /= 2;
min_period /= 2;
*t0_ptr /= 2;
prev_period /= 2;
n /= 2;
let x_target = &x[max_period..];
if *t0_ptr >= max_period {
*t0_ptr = max_period - 1;
}
let mut t = *t0_ptr;
let t0 = *t0_ptr;
let (cands, n_cands) = doubling_candidates(t0, min_period, max_period);
let cands = &cands[..n_cands];
let last = cands.iter().fold(t0, |m, &(t1, t1b)| m.max(t1).max(t1b));
let mut own_table = [const { MaybeUninit::<f32>::uninit() }; MAX_YY_SIZE];
let mut pair_xy = [(0.0f32, 0.0f32); MAX_DOUBLING_CANDS];
let mut around_t0 = None;
let (xx, xy, energies) = doubling_products(
x,
max_period,
n,
t0,
cands,
&mut pair_xy,
last + 1,
started,
&mut own_table,
&mut around_t0,
);
let yy_lookup = &energies.filled()[..last + 1];
let mut best_xy = xy;
let mut best_yy = yy_lookup[t0];
let mut g = compute_pitch_gain(best_xy, xx, best_yy);
let g0 = g;
for (i, (&(t1, t1b), &(xy_a, xy_b))) in cands.iter().zip(&pair_xy).enumerate() {
let k = i + 2;
let xy_new = 0.5 * (xy_a + xy_b);
let yy_new = 0.5 * (yy_lookup[t1] + yy_lookup[t1b]);
let g1 = compute_pitch_gain(xy_new, xx, yy_new);
let mut cont = 0.0f32;
if (t1 as i32 - prev_period as i32).abs() <= 1 {
cont = prev_gain;
} else if (t1 as i32 - prev_period as i32).abs() <= 2 && 5 * k * k < t0 {
cont = 0.5 * prev_gain;
}
let thresh = if t1 < 3 * min_period {
max_first(0.4, mac(-cont, 0.85, g0))
} else {
max_first(0.3, mac(-cont, 0.7, g0))
};
if g1 > thresh {
best_xy = xy_new;
best_yy = yy_new;
t = t1;
g = g1;
}
}
best_xy = max_first(0.0, best_xy);
let pg = if best_yy <= best_xy {
1.0f32
} else {
best_xy / (best_yy + 1.0)
};
let xcorr_res = match around_t0 {
Some(products) if t == t0 => products,
_ => {
let offsets: [usize; 3] = core::array::from_fn(|k_idx| {
let lag = (t as i32 + k_idx as i32 - 1) as usize;
max_period - lag
});
let mut products = [0.0f32; 3];
inner_prod_many(x_target, x, &offsets, n, &mut products);
products
}
};
let mut offset = 0;
if (xcorr_res[2] - xcorr_res[0]) > 0.7 * (xcorr_res[1] - xcorr_res[0]) {
offset = 1;
} else if (xcorr_res[0] - xcorr_res[2]) > 0.7 * (xcorr_res[1] - xcorr_res[2]) {
offset = -1;
}
let pg = if pg > g { g } else { pg };
*t0_ptr = (2 * t as i32 + offset) as usize;
if *t0_ptr < min_period0 {
*t0_ptr = min_period0;
}
pg
}
pub(crate) fn prefilter_pitch(
x: &[f32],
max_period: usize,
min_period: usize,
n: usize,
prev_period: usize,
prev_gain: f32,
) -> (usize, f32) {
let search_max = max_period - 3 * min_period;
let period = |lag: usize| (max_period - lag).min(max_period - 2);
let half = max_period >> 1;
#[cfg(all(target_arch = "aarch64", target_vendor = "apple"))]
{
let target = &x[half..half + n / 2];
let xx = celt_inner_prod(target, target);
let mut table = [const { MaybeUninit::<f32>::uninit() }; MAX_YY_SIZE];
let mut energies = Energies::new(x, half, n / 2, xx, &mut table[..half + 1]);
energies.set_end(DOUBLING_EARLY);
let mut chain = DoublingChain {
energies,
max_period,
min_period,
};
let mut t0 = period(pitch_search_beside(
&x[half..],
x,
n,
search_max,
&mut chain,
));
let gain = remove_doubling_from(
x,
max_period,
min_period,
n,
&mut t0,
prev_period,
prev_gain,
Some(chain.energies),
);
(t0, gain)
}
#[cfg(not(all(target_arch = "aarch64", target_vendor = "apple")))]
{
let mut t0 = period(pitch_search(&x[half..], x, n, search_max));
let gain = remove_doubling(
x,
max_period,
min_period,
n,
&mut t0,
prev_period,
prev_gain,
);
(t0, gain)
}
}
#[cfg(all(target_arch = "aarch64", target_vendor = "apple"))]
const DOUBLING_EARLY: usize = 512;
#[cfg(all(target_arch = "aarch64", target_vendor = "apple"))]
struct DoublingChain<'a> {
energies: Energies<'a>,
max_period: usize,
min_period: usize,
}
#[cfg(all(target_arch = "aarch64", target_vendor = "apple"))]
impl Beside for DoublingChain<'_> {
const ACTIVE: bool = true;
const EVERY: usize = 3;
#[inline(always)]
fn fill(&mut self, count: usize) {
self.energies.step_runs(count);
}
fn coarse_found(&mut self, best_pitch: &[usize; 2]) {
let half = self.max_period / 2;
let min_half = self.min_period / 2;
let mut end = 0;
for &c in best_pitch {
for lag in (4 * c).saturating_sub(5)..=4 * c + 5 {
let index = self.max_period.saturating_sub(lag).min(self.max_period - 2);
let t0 = (index / 2).min(half - 1);
let t1 = (2 * t0 + 2) / 4;
let last = if t1 >= min_half && t1 + t0 <= half {
t0 + t1
} else {
t0
};
end = end.max(last + 1);
}
}
self.energies.set_end(end);
}
}
#[inline(always)]
fn max_first(a: f32, b: f32) -> f32 {
if a > b { a } else { b }
}
#[cfg(test)]
pub(crate) mod tests {
use super::*;
#[cfg(not(feature = "std"))]
use std::vec::Vec;
fn as_table(v: &mut [f32]) -> &mut [MaybeUninit<f32>] {
unsafe { core::slice::from_raw_parts_mut(v.as_mut_ptr().cast(), v.len()) }
}
pub(crate) struct Rng(u64);
impl Rng {
pub(crate) fn new(seed: u64) -> Self {
Rng(seed.wrapping_mul(0x9E37_79B9_7F4A_7C15) | 1)
}
pub(crate) fn next_u32(&mut self) -> u32 {
self.0 = self
.0
.wrapping_mul(6364136223846793005)
.wrapping_add(1442695040888963407);
(self.0 >> 33) as u32
}
pub(crate) fn unit(&mut self) -> f32 {
self.next_u32() as f32 / (1u32 << 30) as f32 - 1.0
}
}
#[derive(Clone, Copy, Debug)]
pub(crate) enum Kind {
Audio,
WideRange,
Tiny,
Huge,
NonFinite,
}
pub(crate) const KINDS: [Kind; 5] = [
Kind::Audio,
Kind::WideRange,
Kind::Tiny,
Kind::Huge,
Kind::NonFinite,
];
pub(crate) fn samples(rng: &mut Rng, n: usize, kind: Kind) -> Vec<f32> {
(0..n)
.map(|_| match kind {
Kind::Audio => 32768.0 * rng.unit(),
Kind::WideRange => {
let exp = (rng.next_u32() % 46) as i32 - 30;
rng.unit() * 10f32.powi(exp)
}
Kind::Tiny => match rng.next_u32() % 5 {
0 => 0.0,
1 => -0.0,
2 => f32::MIN_POSITIVE,
3 => -f32::from_bits(1 + rng.next_u32() % 1000),
_ => f32::from_bits(1 + rng.next_u32() % 0x0080_0000),
},
Kind::Huge => match rng.next_u32() % 4 {
0 => f32::MAX,
1 => f32::MIN,
_ => 1e30 * rng.unit(),
},
Kind::NonFinite => match rng.next_u32() % 16 {
0 => f32::INFINITY,
1 => f32::NEG_INFINITY,
2 => f32::NAN,
_ => 100.0 * rng.unit(),
},
})
.collect()
}
pub(crate) fn same_bits(a: f32, b: f32) -> bool {
a.to_bits() == b.to_bits() || (a.is_nan() && b.is_nan())
}
#[track_caller]
pub(crate) fn assert_same(a: &[f32], b: &[f32], what: &str) {
assert_eq!(a.len(), b.len(), "{what}: lengths differ");
for (i, (&p, &q)) in a.iter().zip(b).enumerate() {
assert!(
same_bits(p, q),
"{what}: element {i} differs: {p:e} ({:#010x}) and {q:e} ({:#010x})",
p.to_bits(),
q.to_bits()
);
}
}
fn lag_by_sample(x: &[f32], y: &[f32], len: usize) -> f32 {
let mut acc = 0.0f32;
for j in 0..len {
acc = mac(acc, x[j], y[j]);
}
acc
}
fn inner_prod_model(x: &[f32], y: &[f32]) -> f32 {
let n = x.len();
if cfg!(any(target_arch = "aarch64", all(target_arch = "x86_64", target_feature = "sse2"))) {
let whole = n / 4 * 4;
let mut s = [0.0f32; 4];
for j in 0..whole {
s[j % 4] = mac(s[j % 4], x[j], y[j]);
}
let mut sum = (s[0] + s[2]) + (s[1] + s[3]);
for j in whole..n {
sum = mac(sum, x[j], y[j]);
}
sum
} else {
lag_by_sample(x, y, n)
}
}
#[test]
fn celt_inner_prod_matches_its_model() {
let mut rng = Rng::new(3);
for kind in KINDS {
for n in (0..=40).chain([63, 64, 65, 240, 481]) {
let x = samples(&mut rng, n, kind);
let y = samples(&mut rng, n + 3, kind);
let got = celt_inner_prod(&x, &y);
let want = inner_prod_model(&x, &y[..n]);
assert_same(&[got], &[want], &format!("{kind:?} n {n}"));
}
}
}
#[cfg(all(feature = "std", target_arch = "x86_64"))]
#[test]
fn celt_inner_prod_matches_libopus_sse() {
use audiopus_sys as _;
unsafe extern "C" {
#[link_name = "celt_inner_prod_sse"]
fn c_celt_inner_prod_sse(x: *const f32, y: *const f32, n: i32) -> f32;
}
let mut rng = Rng::new(31);
for kind in KINDS {
for n in (0..=40).chain([63, 64, 65, 240, 481]) {
let x = samples(&mut rng, n, kind);
let y = samples(&mut rng, n, kind);
let want = unsafe { c_celt_inner_prod_sse(x.as_ptr(), y.as_ptr(), n as i32) };
let got = celt_inner_prod(&x, &y);
assert_same(&[got], &[want], &format!("{kind:?} n {n}"));
}
}
}
#[test]
fn xcorr_by_sample_scalar_matches_one_lag_at_a_time() {
let mut rng = Rng::new(1);
for kind in KINDS {
for len in (0..=20).chain([31, 32, 33, 61, 240]) {
for lags in (0..=20).chain([31, 32, 33, 40, 61]) {
let x = samples(&mut rng, len, kind);
let y = samples(&mut rng, (len + lags).saturating_sub(1), kind);
let mut got = vec![f32::NAN; lags];
xcorr_by_sample_scalar(&x, &y, &mut got, len);
let want: Vec<f32> =
(0..lags).map(|k| lag_by_sample(&x, &y[k..], len)).collect();
assert_same(&got, &want, &format!("{kind:?} len {len} lags {lags}"));
}
}
}
}
#[test]
fn energy_in_parts_is_the_energy_in_one_loop() {
let mut rng = Rng::new(4);
for kind in KINDS {
for n in [0usize, 1, 7, 60, 480] {
let y = samples(&mut rng, n, kind);
let mut want = 1.0f32;
for &v in &y {
want = mac(want, v, v);
}
for part in [1usize, 2, 3, 4, 8, 1000] {
let mut e = Energy::new(&y);
let mut calls = 0;
while e.pos < n && calls < 3 {
e.step(part);
calls += 1;
}
assert_same(
&[e.finish()],
&[want],
&format!("{kind:?} n {n} part {part}"),
);
let half = &y[..n / 2];
let mut want_half = 1.0f32;
for &v in half {
want_half = mac(want_half, v, v);
}
let (mut a, mut b) = (Energy::new(&y), Energy::new(half));
for _ in 0..2 {
Energy::step_both(&mut a, &mut b, part);
}
assert_same(
&[a.finish(), b.finish()],
&[want, want_half],
&format!("{kind:?} n {n} part {part} side by side"),
);
}
}
}
}
#[test]
fn celt_fir5_matches_the_filter_written_out() {
let mut rng = Rng::new(5);
let num = [0.5f32, -0.25, 0.125, 0.75, -0.0625];
for kind in KINDS {
for n in (0..=20).chain([572, 991, 992]) {
let x = samples(&mut rng, n, kind);
let mut want = x.clone();
let mut past = [0.0f32; 5];
for v in want.iter_mut() {
let mut sum = *v;
for j in 0..5 {
sum = mac(sum, num[j], past[j]);
}
past.copy_within(0..4, 1);
past[0] = *v;
*v = sum;
}
let mut got = x.clone();
celt_fir5(&mut got, &num);
assert_same(&got, &want, &format!("{kind:?} n {n}"));
}
}
}
fn find_best_pitch_model(xcorr: &[f32], y: &[f32], len: usize) -> [usize; 2] {
let mut syy = 1.0f32;
for &v in &y[..len] {
syy = mac(syy, v, v);
}
let mut best_num = [-1.0f32, -1.0];
let mut best_den = [0.0f32, 0.0];
let mut best_pitch = [0, 1];
for (i, &xc) in xcorr.iter().enumerate() {
if xc > 0.0 {
let xc16 = xc * 1e-12;
let num = xc16 * xc16;
if num * best_den[1] > best_num[1] * syy {
if num * best_den[0] > best_num[0] * syy {
best_num[1] = best_num[0];
best_den[1] = best_den[0];
best_pitch[1] = best_pitch[0];
best_num[0] = num;
best_den[0] = syy;
best_pitch[0] = i;
} else {
best_num[1] = num;
best_den[1] = syy;
best_pitch[1] = i;
}
}
}
syy += mac(-(y[i] * y[i]), y[i + len], y[i + len]);
if syy < 1.0 {
syy = 1.0;
}
}
best_pitch
}
#[test]
fn best_pitch_from_matches_the_plain_loop() {
let mut rng = Rng::new(13);
for kind in KINDS {
for (len, lags) in [
(1usize, 1usize),
(5, 7),
(30, 8),
(30, 244),
(60, 489),
(240, 244),
] {
let y = samples(&mut rng, len + lags, kind);
let xcorr: Vec<f32> = (0..lags)
.map(|i| {
let r = rng.unit();
if r > 0.6 { r * (1 + i) as f32 * 1e9 } else { r }
})
.collect();
let what = format!("{kind:?} len {len} lags {lags}");
let want = find_best_pitch_model(&xcorr, &y, len);
let first = Energy::new(&y[..len]).finish();
let mut exact = vec![f32::NAN; lags];
WindowEnergies::new(&y, len, first, as_table(&mut exact)).fill_exact(lags);
for part in [1usize, 3, 8, 1000] {
let mut table = vec![f32::NAN; lags];
let mut e = WindowEnergies::new(&y, len, first, as_table(&mut table));
while e.next < lags {
let end = (e.next + part).min(lags);
e.fill_to(end, &mut ());
}
assert_same(&table, &exact, &format!("{what} part {part}"));
let mut got = [7, 7];
best_pitch_from(&xcorr, &table, &mut got);
assert_eq!(got, want, "{what} part {part}");
}
let mut table = vec![f32::NAN; lags];
let mut other = vec![f32::NAN; lags];
let mut a = WindowEnergies::new(&y, len, first, as_table(&mut table));
let mut b = WindowEnergies::new(&y, len, 2.5, as_table(&mut other));
b.fill_to(1, &mut ());
WindowEnergies::fill_both(&mut a, &mut b, lags / 2, &mut ());
a.fill_to(lags, &mut ());
b.fill_to(lags, &mut ());
assert_same(&table, &exact, &format!("{what} side by side"));
let mut alone = vec![f32::NAN; lags];
WindowEnergies::new(&y, len, 2.5, as_table(&mut alone)).fill_exact(lags);
assert_same(&other, &alone, &format!("{what} second chain"));
}
}
}
#[test]
fn window_energies_reach_the_lower_limit_as_libopus() {
let mut rng = Rng::new(15);
let mut limited = 0;
for trial in 0..60 {
let (len, lags) = ([4usize, 7, 60][trial % 3], [1usize, 5, 30, 244][trial % 4]);
{
let mut y = vec![0.0f32; len + lags];
for (i, v) in y.iter_mut().enumerate() {
*v = if i < len {
1e4 * rng.unit()
} else {
1e-3 * rng.unit()
};
}
let first = Energy::new(&y[..len]).finish();
let mut want = vec![f32::NAN; lags];
WindowEnergies::new(&y, len, first, as_table(&mut want)).fill_exact(lags);
let mut got = vec![f32::NAN; lags];
WindowEnergies::new(&y, len, first, as_table(&mut got)).fill_to(lags, &mut ());
assert_same(&got, &want, &format!("len {len} lags {lags}"));
let mut other = vec![f32::NAN; lags];
let mut a = WindowEnergies::new(&y, len, first, as_table(&mut got));
let mut b = WindowEnergies::new(&y, len, first, as_table(&mut other));
WindowEnergies::fill_both(&mut a, &mut b, lags, &mut ());
assert_same(&got, &want, &format!("len {len} lags {lags} first"));
assert_same(&other, &want, &format!("len {len} lags {lags} second"));
if want.contains(&1.0) {
limited += 1;
}
}
}
assert!(limited > 5, "{limited}");
}
#[test]
fn prefilter_pitch_is_pitch_search_then_remove_doubling() {
let mut rng = Rng::new(21);
for frame in [120usize, 240, 480, 960] {
let buf_len = (1024 + frame) >> 1;
for trial in 0..60 {
let kind = KINDS[trial % KINDS.len()];
let buf: Vec<f32> = if trial % 3 == 0 {
samples(&mut rng, buf_len, kind)
} else {
let period = 8.0 + 512.0 * (0.5 + 0.5 * rng.unit());
let noise = samples(&mut rng, buf_len, kind);
(0..buf_len)
.map(|i| {
let phase = core::f32::consts::TAU * i as f32 / period;
1e4 * (phase.sin() + 0.3 * (2.0 * phase).sin()) + 0.1 * noise[i]
})
.collect()
};
let prev_period = 15 + (rng.next_u32() % 1000) as usize;
let prev_gain = 0.5 + 0.5 * rng.unit();
let found = pitch_search(&buf[512..], &buf, frame, 979);
let mut want = (1024 - found).min(1022);
let want_gain =
remove_doubling(&buf, 1024, 15, frame, &mut want, prev_period, prev_gain);
let (got, gain) = prefilter_pitch(&buf, 1024, 15, frame, prev_period, prev_gain);
let what = format!("frame {frame} trial {trial} {kind:?}");
assert_eq!(got, want, "{what}");
assert_same(&[gain], &[want_gain], &what);
}
let silence = vec![0.0f32; buf_len];
let found = pitch_search(&silence[512..], &silence, frame, 979);
let mut want = (1024 - found).min(1022);
let want_gain = remove_doubling(&silence, 1024, 15, frame, &mut want, 100, 0.5);
let (got, gain) = prefilter_pitch(&silence, 1024, 15, frame, 100, 0.5);
assert_eq!(got, want, "silence, frame {frame}");
assert_same(&[gain], &[want_gain], &format!("silence, frame {frame}"));
}
}
fn prefilter_search(
pitch_buf: &[f32],
frame: usize,
prev_period: usize,
prev_gain: f32,
) -> (usize, f32) {
prefilter_pitch(pitch_buf, 1024, 15, frame, prev_period, prev_gain)
}
#[test]
#[ignore]
fn prefilter_timing() {
use std::time::Instant;
const MAX_PERIOD: usize = 1024;
#[cfg(target_os = "macos")]
{
unsafe extern "C" {
fn pthread_set_qos_class_self_np(qos_class: u32, relative_priority: i32) -> i32;
}
unsafe { pthread_set_qos_class_self_np(0x21, 0) };
}
let wav = std::env::var("PITCH_TIMING_WAV").ok();
let mut input: Vec<Vec<f32>> = match &wav {
Some(path) => {
let bytes = std::fs::read(path).expect("read PITCH_TIMING_WAV");
let (mut at, mut channels, mut data) = (12, 0usize, &bytes[..0]);
while at + 8 <= bytes.len() {
let size = u32::from_le_bytes(bytes[at + 4..at + 8].try_into().unwrap());
let body = &bytes[at + 8..(at + 8 + size as usize).min(bytes.len())];
match &bytes[at..at + 4] {
b"fmt " => channels = u16::from_le_bytes([body[2], body[3]]) as usize,
b"data" => data = body,
_ => {}
}
at += 8 + size as usize + (size as usize & 1);
}
assert!(channels == 1 || channels == 2, "{channels} channels");
let samples: Vec<f32> = data
.as_chunks::<2>()
.0
.iter()
.map(|b| i16::from_le_bytes(*b) as f32)
.collect();
(0..channels)
.map(|c| samples.iter().skip(c).step_by(channels).copied().collect())
.collect()
}
None => {
let mut rng = Rng::new(5);
(0..2)
.map(|c| {
(0..5 * 48000)
.map(|i| {
let note = [110.0f32, 147.0, 196.0, 131.0][(i / 12000) % 4];
let f0 = note * (1.0 + 0.5 * c as f32);
let t = i as f32 / 48000.0;
let mut s = 0.0;
for h in 1..8 {
s += (core::f32::consts::TAU * f0 * h as f32 * t).sin()
/ h as f32;
}
6000.0 * s + 200.0 * rng.unit()
})
.collect()
})
.collect()
}
};
for ch in &mut input {
let mut mem = 0.0f32;
for v in ch.iter_mut() {
let x = *v;
*v = x - mem;
mem = 0.850_006_1 * x;
}
}
let reps: usize = std::env::var("PITCH_TIMING_REPS")
.ok()
.and_then(|r| r.parse().ok())
.unwrap_or(15);
let len = input[0].len();
println!(
"input {} ({} ch, {:.1} s), {reps} reps",
wav.as_deref().unwrap_or("synthetic"),
input.len(),
len as f64 / 48000.0
);
println!(
"frame ch frames | downsample search doubling search+doubling (us/frame) | hash"
);
let fastest = |f: &mut dyn FnMut()| {
(0..reps)
.map(|_| {
let t = Instant::now();
f();
t.elapsed().as_secs_f64()
})
.fold(f64::INFINITY, f64::min)
};
for frame in [120usize, 240, 480, 960] {
for channels in 1..=input.len().min(2) {
let frames = ((len - MAX_PERIOD) / frame).min(400);
let buf_len = (MAX_PERIOD + frame) >> 1;
let mut bufs = vec![0.0f32; frames * buf_len];
let t_down = fastest(&mut || {
for (f, buf) in bufs.chunks_exact_mut(buf_len).enumerate() {
let at = f * frame;
let pre: Vec<&[f32]> = input[..channels]
.iter()
.map(|ch| &ch[at..at + MAX_PERIOD + frame])
.collect();
pitch_downsample(&pre, buf, buf_len, channels, 2);
}
});
let mut found = vec![0usize; frames];
let t_search = fastest(&mut || {
for (buf, out) in bufs.chunks_exact(buf_len).zip(found.iter_mut()) {
let lag = pitch_search(&buf[MAX_PERIOD >> 1..], buf, frame, 979);
*out = (MAX_PERIOD - lag).min(MAX_PERIOD - 2);
}
});
let t_doubling = fastest(&mut || {
let (mut period, mut gain) = (15usize, 0.0f32);
for (buf, &t0) in bufs.chunks_exact(buf_len).zip(&found) {
let mut index = t0;
let g =
remove_doubling(buf, MAX_PERIOD, 15, frame, &mut index, period, gain);
(period, gain) = (index, 0.7 * g);
}
});
let mut hash = 0u64;
let t_both = fastest(&mut || {
let (mut period, mut gain) = (15usize, 0.0f32);
hash = 0;
for buf in bufs.chunks_exact(buf_len) {
let (index, g) = prefilter_search(buf, frame, period, gain);
(period, gain) = (index, 0.7 * g);
hash = hash.wrapping_mul(0x100_0000_01b3)
^ (index as u64)
^ ((g.to_bits() as u64) << 16);
}
});
let us = |t: f64| t * 1e6 / frames as f64;
println!(
"{frame:5} {channels:2} {frames:6} | {:10.2} {:7.2} {:9.2} {:16.2} | {hash:016x}",
us(t_down),
us(t_search),
us(t_doubling),
us(t_both)
);
}
}
}
#[test]
fn pitch_xcorr_handles_short_and_empty_inputs() {
let x = [1.0f32; 8];
let y = [1.0f32; 4];
let mut out = [7.0f32; 4];
pitch_xcorr(&x, &y, &mut out, 8, 4);
assert_eq!(out, [7.0; 4]);
pitch_xcorr(&x, &y, &mut out, 0, 4);
assert_eq!(out, [7.0; 4]);
let y = [1.0f32; 9];
pitch_xcorr(&x, &y, &mut out, 8, 4);
assert_eq!(out, [8.0, 8.0, 7.0, 7.0]);
}
#[cfg(target_arch = "aarch64")]
mod neon {
use super::*;
#[test]
fn xcorr_by_sample_neon_matches_scalar_and_calls_the_side_work() {
let mut rng = Rng::new(2);
for kind in KINDS {
for len in (1..=40).chain([60, 61, 120, 240, 241]) {
let x = samples(&mut rng, len, kind);
let y = samples(&mut rng, len + 4 * 16 - 1, kind);
let what = format!("{kind:?} len {len}");
macro_rules! check {
($g:literal) => {{
let mut got = vec![f32::NAN; 4 * $g];
let mut calls = 0;
xcorr_by_sample_side_neon::<$g, _>(&x, &y, &mut got, &mut || {
calls += 1
});
assert_eq!(calls, len / 8, "{what} G {}", $g);
let mut want = vec![f32::NAN; 4 * $g];
xcorr_by_sample_scalar(&x, &y, &mut want, len);
assert_same(&got, &want, &format!("{what} G {}", $g));
}};
}
check!(1);
check!(2);
check!(3);
check!(5);
check!(7);
check!(8);
check!(16);
}
}
}
#[test]
fn xcorr_skewed_neon_matches_scalar_and_calls_the_side_work() {
let mut rng = Rng::new(14);
for kind in KINDS {
for len in (32..=72).chain([240, 241, 242, 243, 332, 664]) {
for lags in [64usize, 68, 124, 128, 132, 244] {
let x = samples(&mut rng, len, kind);
let y = samples(&mut rng, len + lags - 1, kind);
let what = format!("{kind:?} len {len} lags {lags}");
let mut got = vec![f32::NAN; lags];
let mut calls = 0;
xcorr_skewed_neon(&x, &y, &mut got, &mut || calls += 1);
assert_eq!(calls, skew_side_calls(len, lags), "{what}");
let mut want = vec![f32::NAN; lags];
xcorr_by_sample_scalar(&x, &y, &mut want, len);
assert_same(&got, &want, &what);
}
}
}
}
#[test]
fn pitch_xcorr_neon_follows_libopus_for_every_count_of_lags() {
let mut rng = Rng::new(6);
for kind in KINDS {
for len in [1usize, 3, 4, 9, 30, 60, 240] {
for lags in (1..=40).chain([63, 64, 65, 66, 67, 155, 244, 245]) {
let x = samples(&mut rng, len, kind);
let y = samples(&mut rng, len + lags - 1, kind);
let mut got = vec![f32::NAN; lags];
pitch_xcorr_neon(&x, &y, &mut got);
let by_sample = lags & !3;
let want: Vec<f32> = (0..lags)
.map(|k| {
if k < by_sample {
lag_by_sample(&x, &y[k..], len)
} else {
inner_prod_model(&x, &y[k..k + len])
}
})
.collect();
assert_same(&got, &want, &format!("{kind:?} len {len} lags {lags}"));
}
}
}
}
#[test]
fn inner_prod_many_matches_one_call_per_window() {
let mut rng = Rng::new(7);
for kind in KINDS {
for n in [0usize, 1, 5, 8, 61, 480] {
for count in [0usize, 1, 3, 4, 9, 16, 17, 30] {
let x = samples(&mut rng, n, kind);
let y = samples(&mut rng, n + 40, kind);
let offsets: Vec<usize> = (0..count).map(|k| (7 * k) % 41).collect();
let mut got = vec![f32::NAN; count];
let mut calls = 0;
inner_prod_many_side(&x, &y, &offsets, n, &mut got, &mut || calls += 1);
assert_eq!(calls, count.div_ceil(MANY_PER_PASS) * (n / 4));
let want: Vec<f32> = offsets
.iter()
.map(|&o| inner_prod_model(&x, &y[o..o + n]))
.collect();
assert_same(&got, &want, &format!("{kind:?} n {n} count {count}"));
let (a, b) = dual_inner_prod(&x, &y, &y[3..], n);
let want = [
inner_prod_model(&x, &y[..n]),
inner_prod_model(&x, &y[3..3 + n]),
];
assert_same(&[a, b], &want, &format!("{kind:?} n {n} dual"));
}
}
}
}
#[test]
fn decimate2_neon_matches_scalar() {
let mut rng = Rng::new(10);
for kind in KINDS {
for n in (0..=40).chain([572, 632, 752, 991, 992]) {
let x0 = samples(&mut rng, 2 * n, kind);
let x1 = samples(&mut rng, 2 * n, kind);
let mut got = vec![f32::NAN; n];
unsafe { decimate2_neon([&x0[..]], &mut got) };
let mut want = vec![f32::NAN; n];
decimate2_scalar([&x0[..]], &mut want);
assert_same(&got, &want, &format!("{kind:?} n {n} mono"));
let mut got = vec![f32::NAN; n];
unsafe { decimate2_neon([&x0[..], &x1[..]], &mut got) };
let mut want = vec![f32::NAN; n];
decimate2_scalar([&x0[..], &x1[..]], &mut want);
assert_same(&got, &want, &format!("{kind:?} n {n} stereo"));
}
}
}
#[test]
fn decimate2_autocorr5_neon_matches_the_separate_steps() {
let mut rng = Rng::new(11);
for kind in KINDS {
for n in (8..=40).chain([572, 632, 752, 991, 992, 1024]) {
let x0 = samples(&mut rng, 2 * n, kind);
let x1 = samples(&mut rng, 2 * n, kind);
for stereo in [false, true] {
let mut got = vec![f32::NAN; n];
let mut got_ac = [f32::NAN; 5];
let mut want = vec![f32::NAN; n];
if stereo {
unsafe {
decimate2_autocorr5_neon([&x0[..], &x1[..]], &mut got, &mut got_ac)
};
decimate2_scalar([&x0[..], &x1[..]], &mut want);
want[0] = mac(0.5 * x0[0], x0[1], 0.25) + mac(0.5 * x1[0], x1[1], 0.25);
} else {
unsafe { decimate2_autocorr5_neon([&x0[..]], &mut got, &mut got_ac) };
decimate2_scalar([&x0[..]], &mut want);
want[0] = mac(0.5 * x0[0], x0[1], 0.25);
}
let what = format!("{kind:?} n {n} stereo {stereo}");
assert_same(&got, &want, &what);
let mut want_ac = [f32::NAN; 5];
autocorr(&want, &mut want_ac, None, 0, 4, n);
assert_same(&got_ac, &want_ac, &what);
}
}
}
}
#[test]
fn xcorr5_neon_matches_the_general_kernels() {
let mut rng = Rng::new(12);
for kind in KINDS {
for len in (1..=20).chain([988, 1020]) {
let x = samples(&mut rng, len, kind);
let y = samples(&mut rng, len + 4, kind);
let mut got = [f32::NAN; 5];
unsafe { xcorr5_neon(&x, &y, &mut got) };
let mut want = [f32::NAN; 5];
xcorr_by_sample_scalar(&x, &y, &mut want[..4], len);
want[4] = inner_prod_model(&x, &y[4..4 + len]);
assert_same(&got, &want, &format!("{kind:?} len {len}"));
}
}
}
}
}