use crate::silk::define::*;
use crate::silk::macros::*;
#[inline(always)]
pub(crate) const fn smulwb_operand(b: i16) -> i32 {
(b as i32) << 15
}
#[inline]
pub fn silk_k2a_q16(a_q24: &mut [i32], rc_q16: &[i32], order: usize) {
for k in 0..order {
let rc = rc_q16[k];
for n in 0..((k + 1) >> 1) {
let tmp1 = a_q24[n];
let tmp2 = a_q24[k - n - 1];
a_q24[n] = silk_smlaww(tmp1, tmp2, rc);
a_q24[k - n - 1] = silk_smlaww(tmp2, tmp1, rc);
}
a_q24[k] = -(rc << 8);
}
}
pub fn silk_schur64(rc_q16: &mut [i32], c: &[i32], order: usize) -> i32 {
if c[0] <= 0 {
for v in rc_q16.iter_mut().take(order) {
*v = 0;
}
return 0;
}
let mut upper = [0i32; MAX_SHAPE_LPC_ORDER + 1];
let mut lower = [0i32; MAX_SHAPE_LPC_ORDER + 1];
upper[..=order].copy_from_slice(&c[..=order]);
lower[..=order].copy_from_slice(&c[..=order]);
let rc_q16 = &mut rc_q16[..order];
let mut num = upper[1];
let mut den = lower[0];
let mut k = 0;
while k < order {
if num.wrapping_abs() >= den {
rc_q16[k] = if num > 0 { -64880 } else { 64880 };
k += 1;
break;
}
let rc_tmp_q31 = silk_div32_varq(num.wrapping_neg(), den, 31);
rc_q16[k] = silk_rshift_round(rc_tmp_q31, 15);
den = den.wrapping_add(silk_smmul(num << 1, rc_tmp_q31));
let upper_split = upper[k + 2..=order].split_first_mut();
let lower_split = lower[1..order - k].split_first_mut();
if let (Some((u1, upper_rest)), Some((l1, lower_rest))) = (upper_split, lower_split) {
let (c_tmp1_q30, c_tmp2_q30) = (*u1, *l1);
num = c_tmp1_q30.wrapping_add(silk_smmul(c_tmp2_q30 << 1, rc_tmp_q31));
*l1 = c_tmp2_q30.wrapping_add(silk_smmul(c_tmp1_q30 << 1, rc_tmp_q31));
for (u, l) in upper_rest.iter_mut().zip(lower_rest) {
let (c_tmp1_q30, c_tmp2_q30) = (*u, *l);
*u = c_tmp1_q30.wrapping_add(silk_smmul(c_tmp2_q30 << 1, rc_tmp_q31));
*l = c_tmp2_q30.wrapping_add(silk_smmul(c_tmp1_q30 << 1, rc_tmp_q31));
}
}
k += 1;
}
rc_q16[k..].fill(0);
den.max(1)
}
pub fn silk_biquad_alt_stride1(
input_output: &mut [i16],
b_q28: &[i32],
a_q28: &[i32],
s: &mut [i32],
len: usize,
) {
let b_q28: &[i32; 3] = b_q28[..3].try_into().unwrap();
let a_q28: &[i32; 2] = a_q28[..2].try_into().unwrap();
let s: &mut [i32; 2] = (&mut s[..2]).try_into().unwrap();
let a0_l_q28 = (-a_q28[0]) & 0x00003FFF;
let a0_u_q28 = -a_q28[0] >> 14;
let a1_l_q28 = (-a_q28[1]) & 0x00003FFF;
let a1_u_q28 = -a_q28[1] >> 14;
for item in input_output.iter_mut().take(len) {
let inval = *item as i32;
let out32_q14 = silk_smlawb(s[0], b_q28[0], inval) << 2;
s[0] = s[1] + silk_rshift_round(silk_smulwb(out32_q14, a0_l_q28), 14);
s[0] = silk_smlawb(s[0], out32_q14, a0_u_q28);
s[0] = silk_smlawb(s[0], b_q28[1], inval);
s[1] = silk_rshift_round(silk_smulwb(out32_q14, a1_l_q28), 14);
s[1] = silk_smlawb(s[1], out32_q14, a1_u_q28);
s[1] = silk_smlawb(s[1], b_q28[2], inval);
*item = silk_sat16(silk_rshift(out32_q14 + (1 << 14) - 1, 14)) as i16;
}
}
pub fn silk_biquad_alt_stride2(
input_output: &mut [i16],
b_q28: &[i32],
a_q28: &[i32],
s: &mut [i32],
len: usize,
) {
let input_output = &mut input_output[..2 * len];
let b_q28: &[i32; 3] = b_q28[..3].try_into().unwrap();
let a_q28: &[i32; 2] = a_q28[..2].try_into().unwrap();
let s: &mut [i32; 4] = (&mut s[..4]).try_into().unwrap();
#[cfg(target_arch = "aarch64")]
unsafe {
silk_biquad_alt_stride2_neon(input_output, b_q28, a_q28, s);
}
#[cfg(not(target_arch = "aarch64"))]
silk_biquad_alt_stride2_scalar(input_output, b_q28, a_q28, s);
}
#[cfg_attr(target_arch = "aarch64", allow(dead_code))]
fn silk_biquad_alt_stride2_scalar(
input_output: &mut [i16],
b_q28: &[i32; 3],
a_q28: &[i32; 2],
s: &mut [i32; 4],
) {
let a0_l_q28 = (-a_q28[0]) & 0x00003FFF;
let a0_u_q28 = -a_q28[0] >> 14;
let a1_l_q28 = (-a_q28[1]) & 0x00003FFF;
let a1_u_q28 = -a_q28[1] >> 14;
let (pairs, _) = input_output.as_chunks_mut::<2>();
for pair in pairs {
let (in0, in1) = (pair[0] as i32, pair[1] as i32);
let out32_q14_0 = silk_smlawb(s[0], b_q28[0], in0) << 2;
let out32_q14_1 = silk_smlawb(s[2], b_q28[0], in1) << 2;
s[0] = s[1].wrapping_add(silk_rshift_round(silk_smulwb(out32_q14_0, a0_l_q28), 14));
s[2] = s[3].wrapping_add(silk_rshift_round(silk_smulwb(out32_q14_1, a0_l_q28), 14));
s[0] = silk_smlawb(s[0], out32_q14_0, a0_u_q28);
s[2] = silk_smlawb(s[2], out32_q14_1, a0_u_q28);
s[0] = silk_smlawb(s[0], b_q28[1], in0);
s[2] = silk_smlawb(s[2], b_q28[1], in1);
s[1] = silk_rshift_round(silk_smulwb(out32_q14_0, a1_l_q28), 14);
s[3] = silk_rshift_round(silk_smulwb(out32_q14_1, a1_l_q28), 14);
s[1] = silk_smlawb(s[1], out32_q14_0, a1_u_q28);
s[3] = silk_smlawb(s[3], out32_q14_1, a1_u_q28);
s[1] = silk_smlawb(s[1], b_q28[2], in0);
s[3] = silk_smlawb(s[3], b_q28[2], in1);
pair[0] = silk_sat16(silk_rshift(out32_q14_0.wrapping_add((1 << 14) - 1), 14)) as i16;
pair[1] = silk_sat16(silk_rshift(out32_q14_1.wrapping_add((1 << 14) - 1), 14)) as i16;
}
}
#[inline(always)]
fn inner_prod16_wrapping(a: &[i16], b: &[i16]) -> i32 {
debug_assert_eq!(a.len(), b.len());
a.iter()
.zip(b)
.fold(0i32, |sum, (&a, &b)| mac16_16(sum, a, b))
}
#[inline(always)]
fn xcorr_kernel(x: &[i16], y: &[i16], sum: &mut [i32; 4], len: usize) {
#[cfg(target_arch = "x86_64")]
if x.len() >= len && y.len() > len + 3 && crate::compat::x86_has_avx2() {
unsafe { xcorr_kernel_avx2(x, y, sum, len) };
return;
}
let x = &x[..len];
let y = &y[..len + 3];
#[cfg(all(target_arch = "aarch64", target_feature = "neon"))]
{
unsafe { xcorr_kernel_neon(x, y, sum) }
}
#[cfg(not(all(target_arch = "aarch64", target_feature = "neon")))]
xcorr_kernel_scalar(x, y, sum);
}
#[cfg_attr(
all(target_arch = "aarch64", target_feature = "neon"),
allow(dead_code)
)]
#[inline(always)]
fn xcorr_kernel_scalar(x: &[i16], y: &[i16], sum: &mut [i32; 4]) {
debug_assert_eq!(y.len(), x.len() + 3);
for (k, s) in sum.iter_mut().enumerate() {
*s = s.wrapping_add(inner_prod16_wrapping(x, &y[k..k + x.len()]));
}
}
#[cfg(all(target_arch = "aarch64", target_feature = "neon"))]
#[target_feature(enable = "neon")]
#[inline]
fn xcorr_kernel_neon(x: &[i16], y: &[i16], sum: &mut [i32; 4]) {
use crate::simd_mem::{array_at, neon::load_i16x8};
use core::arch::aarch64::*;
#[target_feature(enable = "neon")]
#[inline]
fn load(v: &[i16], at: usize) -> int16x8_t {
load_i16x8(array_at(v, at))
}
#[target_feature(enable = "neon")]
#[inline]
fn mac(acc: int32x4_t, a: int16x8_t, b: int16x8_t) -> int32x4_t {
vmlal_high_s16(vmlal_s16(acc, vget_low_s16(a), vget_low_s16(b)), a, b)
}
const LAST_LANES: [i16; 16] = [0, 0, 0, 0, 0, 0, 0, 0, -1, -1, -1, -1, -1, -1, -1, -1];
assert!(y.len() == x.len() + 3);
if x.len() < 8 {
for (j, &xj) in x.iter().enumerate() {
for k in 0..4 {
sum[k] = mac16_16(sum[k], xj, y[j + k]);
}
}
return;
}
let mut acc_a = [vdupq_n_s32(0); 4];
let mut acc_b = [vdupq_n_s32(0); 4];
let (blocks, rest) = x.as_chunks::<16>();
for (i, xb) in blocks.iter().enumerate() {
let yb = &y[16 * i..16 * i + 19];
let x0 = load(xb, 0);
let x1 = load(xb, 8);
for k in 0..4 {
acc_a[k] = mac(acc_a[k], x0, load(yb, k));
acc_b[k] = mac(acc_b[k], x1, load(yb, k + 8));
}
}
let mut left = rest.len();
if left >= 8 {
let at = x.len() - left;
let yb = &y[at..at + 11];
let x0 = load(x, at);
for k in 0..4 {
acc_a[k] = mac(acc_a[k], x0, load(yb, k));
}
left -= 8;
}
if left > 0 {
let at = x.len() - 8;
let yb = &y[at..at + 11];
let x_last = vandq_s16(load(x, at), load(&LAST_LANES, left));
for k in 0..4 {
acc_b[k] = mac(acc_b[k], x_last, load(yb, k));
}
}
for k in 0..4 {
sum[k] = sum[k].wrapping_add(vaddvq_s32(vaddq_s32(acc_a[k], acc_b[k])));
}
}
#[inline(always)]
fn mac16_16(a: i32, b: i16, c: i16) -> i32 {
a.wrapping_add((b as i32).wrapping_mul(c as i32))
}
#[inline]
fn autocorr_sums(results: &mut [i32], x: &[i16]) {
#[cfg(target_arch = "x86_64")]
if crate::compat::x86_has_avx2() {
unsafe { autocorr_sums_avx2(results, x) };
return;
}
autocorr_sums_portable(results, x);
}
#[cfg(target_arch = "x86_64")]
#[target_feature(enable = "avx2")]
fn autocorr_sums_avx2(results: &mut [i32], x: &[i16]) {
autocorr_sums_portable(results, x);
}
#[inline(always)]
fn autocorr_sums_portable(results: &mut [i32], x: &[i16]) {
let n = x.len();
let count = results.len();
let mut k = 0;
while k + 4 <= count {
let len = n - (k + 3);
let mut sum = [0i32; 4];
xcorr_kernel(x, &x[k..], &mut sum, len);
for m in 0..3 {
for j in len..n - (k + m) {
sum[m] = mac16_16(sum[m], x[j], x[j + k + m]);
}
}
results[k..k + 4].copy_from_slice(&sum);
k += 4;
}
while k < count {
results[k] = silk_inner_prod_aligned(x, &x[k..], n - k);
k += 1;
}
}
pub fn silk_autocorr(
results: &mut [i32],
scale: &mut i32,
input_data: &[i16],
input_data_size: usize,
correlation_count: usize,
) {
#[inline]
fn ec_ilog(x: u32) -> i32 {
if x == 0 {
0
} else {
32 - x.leading_zeros() as i32
}
}
let n = input_data_size;
assert!(correlation_count <= n + 1);
let input = &input_data[..n];
let results = &mut results[..correlation_count];
let ac0_shift = ec_ilog((n + (n >> 4)) as u32) - 1;
let mut ac0: i32 = 1 + ((n as i32) << 7);
for &sample in input {
ac0 += ((sample as i32) * (sample as i32)) >> ac0_shift;
}
ac0 += ac0 >> 7;
let ac0_log2 = ec_ilog(ac0 as u32) - 1;
let mut shift = (ac0_log2 - 30 + ac0_shift + 1) / 2;
if shift > 0 {
let mut scaled = [0i16; PE_MAX_FRAME_LENGTH];
for (dst, &sample) in scaled[..n].iter_mut().zip(input) {
*dst = silk_rshift_round(sample as i32, shift) as i16;
}
autocorr_sums(results, &scaled[..n]);
} else {
shift = 0;
autocorr_sums(results, input);
}
shift *= 2;
if shift <= 0 {
let add_shift = (-shift).min(30);
results[0] += 1i32 << add_shift;
}
if results[0] > 0 && results[0] < 268435456 {
let shift2 = 29 - ec_ilog(results[0] as u32);
for v in results.iter_mut() {
*v <<= shift2;
}
shift -= shift2;
} else if results[0] >= 536870912 {
let mut shift2 = 1;
if results[0] >= 1073741824 {
shift2 += 1;
}
for v in results.iter_mut() {
*v >>= shift2;
}
shift += shift2;
}
*scale = shift;
}
pub fn silk_sum_sqr_shift(energy: &mut i32, shift: &mut i32, x: &[i16], len: usize) {
let x = &x[..len];
let mut shft = 31 - silk_clz32(len as i32);
let nrg = sum_sqr_pairs_shifted(x, len as i32, shft);
shft = (shft + 3 - silk_clz32(nrg)).max(0);
#[cfg(target_arch = "x86_64")]
let nrg = if crate::compat::x86_has_avx2() {
unsafe { silk_sum_sqr_shift_avx2(x, len, shft) }
} else {
sum_sqr_pairs_shifted(x, 0, shft)
};
#[cfg(not(target_arch = "x86_64"))]
let nrg = sum_sqr_pairs_shifted(x, 0, shft);
*shift = shft;
*energy = nrg;
}
#[inline(always)]
fn sum_sqr_pairs_shifted(x: &[i16], init: i32, shft: i32) -> i32 {
let (pairs, last) = x.as_chunks::<2>();
let mut nrg = init;
for &[a, b] in pairs {
let nrg_tmp = (silk_smulbb(a as i32, a as i32) as u32)
.wrapping_add(silk_smulbb(b as i32, b as i32) as u32);
nrg = nrg.wrapping_add((nrg_tmp >> shft) as i32);
}
if let [a] = last {
let nrg_tmp = silk_smulbb(*a as i32, *a as i32) as u32;
nrg = nrg.wrapping_add((nrg_tmp >> shft) as i32);
}
nrg
}
pub fn silk_inner_prod_aligned_scale(
in_vec1: &[i16],
in_vec2: &[i16],
scale: i32,
len: usize,
) -> i32 {
let mut sum: i32 = 0;
for (&a, &b) in in_vec1[..len].iter().zip(&in_vec2[..len]) {
sum = sum.wrapping_add(silk_smulbb(a as i32, b as i32) >> scale);
}
sum
}
#[inline(always)]
pub fn silk_inner_prod_aligned(ptr1: &[i16], ptr2: &[i16], len: usize) -> i32 {
let ptr1 = &ptr1[..len];
let ptr2 = &ptr2[..len];
#[cfg(target_arch = "aarch64")]
{
silk_inner_prod_aligned_neon(ptr1, ptr2, len)
}
#[cfg(target_arch = "x86_64")]
if crate::compat::x86_has_avx2() {
return unsafe { silk_inner_prod_aligned_avx2(ptr1, ptr2, len) };
}
#[cfg(not(target_arch = "aarch64"))]
silk_inner_prod_aligned_scalar(ptr1, ptr2, len)
}
#[cfg_attr(target_arch = "aarch64", allow(dead_code))]
#[inline(always)]
fn silk_inner_prod_aligned_scalar(ptr1: &[i16], ptr2: &[i16], len: usize) -> i32 {
let ptr1 = &ptr1[..len];
let ptr2 = &ptr2[..len];
let mut i = 0;
let mut sum0 = 0i32;
let mut sum1 = 0i32;
let mut sum2 = 0i32;
let mut sum3 = 0i32;
let len4 = (len / 4) * 4;
while i < len4 {
sum0 = sum0.wrapping_add((ptr1[i] as i32).wrapping_mul(ptr2[i] as i32));
sum1 = sum1.wrapping_add((ptr1[i + 1] as i32).wrapping_mul(ptr2[i + 1] as i32));
sum2 = sum2.wrapping_add((ptr1[i + 2] as i32).wrapping_mul(ptr2[i + 2] as i32));
sum3 = sum3.wrapping_add((ptr1[i + 3] as i32).wrapping_mul(ptr2[i + 3] as i32));
i += 4;
}
while i < len {
sum0 = sum0.wrapping_add((ptr1[i] as i32).wrapping_mul(ptr2[i] as i32));
i += 1;
}
sum0.wrapping_add(sum1)
.wrapping_add(sum2)
.wrapping_add(sum3)
}
#[cfg(target_arch = "aarch64")]
#[inline(always)]
fn silk_inner_prod_aligned_neon(ptr1: &[i16], ptr2: &[i16], len: usize) -> i32 {
use core::arch::aarch64::*;
let ptr1 = &ptr1[..len];
let ptr2 = &ptr2[..len];
let (mut result, mut i) = unsafe {
let mut acc0 = vdupq_n_s32(0i32);
let mut acc1 = vdupq_n_s32(0i32);
let mut acc2 = vdupq_n_s32(0i32);
let mut acc3 = vdupq_n_s32(0i32);
let mut i = 0;
while i + 32 <= len {
let a0 = vld1q_s16(ptr1.as_ptr().add(i));
let b0 = vld1q_s16(ptr2.as_ptr().add(i));
let a1 = vld1q_s16(ptr1.as_ptr().add(i + 8));
let b1 = vld1q_s16(ptr2.as_ptr().add(i + 8));
let a2 = vld1q_s16(ptr1.as_ptr().add(i + 16));
let b2 = vld1q_s16(ptr2.as_ptr().add(i + 16));
let a3 = vld1q_s16(ptr1.as_ptr().add(i + 24));
let b3 = vld1q_s16(ptr2.as_ptr().add(i + 24));
acc0 = vmlal_s16(acc0, vget_low_s16(a0), vget_low_s16(b0));
acc0 = vmlal_high_s16(acc0, a0, b0);
acc1 = vmlal_s16(acc1, vget_low_s16(a1), vget_low_s16(b1));
acc1 = vmlal_high_s16(acc1, a1, b1);
acc2 = vmlal_s16(acc2, vget_low_s16(a2), vget_low_s16(b2));
acc2 = vmlal_high_s16(acc2, a2, b2);
acc3 = vmlal_s16(acc3, vget_low_s16(a3), vget_low_s16(b3));
acc3 = vmlal_high_s16(acc3, a3, b3);
i += 32;
}
while i + 8 <= len {
let a0 = vld1q_s16(ptr1.as_ptr().add(i));
let b0 = vld1q_s16(ptr2.as_ptr().add(i));
acc0 = vmlal_s16(acc0, vget_low_s16(a0), vget_low_s16(b0));
acc0 = vmlal_high_s16(acc0, a0, b0);
i += 8;
}
let sum01 = vpaddq_s32(acc0, acc1);
let sum23 = vpaddq_s32(acc2, acc3);
let sum = vpaddq_s32(sum01, sum23);
let wide = vpaddlq_s32(sum);
(vaddvq_s64(wide), i)
};
while i < len {
result += (ptr1[i] as i64) * (ptr2[i] as i64);
i += 1;
}
result as i32
}
pub fn silk_corr_vector_fix(
x: &[i16],
t: &[i16],
l: usize,
order: usize,
xt: &mut [i32],
rshifts: i32,
) {
#[cfg(target_arch = "x86_64")]
if crate::compat::x86_has_avx2() {
unsafe { corr_vector_avx2(x, t, l, order, xt, rshifts) };
return;
}
corr_vector_portable(x, t, l, order, xt, rshifts);
}
#[cfg(target_arch = "x86_64")]
#[target_feature(enable = "avx2")]
fn corr_vector_avx2(
x: &[i16],
t: &[i16],
l: usize,
order: usize,
xt: &mut [i32],
rshifts: i32,
) {
corr_vector_portable(x, t, l, order, xt, rshifts);
}
#[inline(always)]
fn corr_vector_portable(
x: &[i16],
t: &[i16],
l: usize,
order: usize,
xt: &mut [i32],
rshifts: i32,
) {
let mut ptr1_idx = order - 1;
if rshifts > 0 {
for xt_val in xt[..order].iter_mut() {
let mut inner_prod: i32 = 0;
for i in 0..l {
inner_prod = silk_add_rshift32(
inner_prod,
silk_smulbb(x[ptr1_idx + i] as i32, t[i] as i32),
rshifts,
);
}
*xt_val = inner_prod;
ptr1_idx = ptr1_idx.saturating_sub(1);
}
} else {
for xt_val in xt[..order].iter_mut() {
*xt_val = silk_inner_prod_aligned(&x[ptr1_idx..], t, l);
ptr1_idx = ptr1_idx.saturating_sub(1);
}
}
}
pub fn silk_corr_matrix_fix(
x: &[i16],
l: usize,
order: usize,
xx: &mut [i32],
nrg: &mut i32,
rshifts: &mut i32,
) {
#[cfg(target_arch = "x86_64")]
if crate::compat::x86_has_avx2() {
unsafe { corr_matrix_avx2(x, l, order, xx, nrg, rshifts) };
return;
}
corr_matrix_portable(x, l, order, xx, nrg, rshifts);
}
#[cfg(target_arch = "x86_64")]
#[target_feature(enable = "avx2")]
fn corr_matrix_avx2(
x: &[i16],
l: usize,
order: usize,
xx: &mut [i32],
nrg: &mut i32,
rshifts: &mut i32,
) {
corr_matrix_portable(x, l, order, xx, nrg, rshifts);
}
#[inline(always)]
fn corr_matrix_portable(
x: &[i16],
l: usize,
order: usize,
xx: &mut [i32],
nrg: &mut i32,
rshifts: &mut i32,
) {
silk_sum_sqr_shift(nrg, rshifts, x, l + order - 1);
let mut energy = *nrg;
for xi in x.iter().take(order - 1) {
energy -= silk_rshift32(silk_smulbb(*xi as i32, *xi as i32), *rshifts);
}
xx[0] = energy;
let ptr1_start_idx = order - 1;
for j in 1..order {
energy = silk_sub32(
energy,
silk_rshift32(
silk_smulbb(
x[ptr1_start_idx + l - j] as i32,
x[ptr1_start_idx + l - j] as i32,
),
*rshifts,
),
);
energy = silk_add32(
energy,
silk_rshift32(
silk_smulbb(x[ptr1_start_idx - j] as i32, x[ptr1_start_idx - j] as i32),
*rshifts,
),
);
xx[j * order + j] = energy;
}
for lag in 1..order {
let ptr1_idx = ptr1_start_idx;
let ptr2_idx = ptr1_start_idx - lag;
let mut inner_prod: i32 = 0;
if *rshifts > 0 {
for i in 0..l {
inner_prod = silk_add_rshift32(
inner_prod,
silk_smulbb(x[ptr1_idx + i] as i32, x[ptr2_idx + i] as i32),
*rshifts,
);
}
} else {
inner_prod = silk_inner_prod_aligned(&x[ptr1_idx..], &x[ptr2_idx..], l);
}
xx[lag] = inner_prod;
xx[lag * order] = inner_prod;
for j in 1..(order - lag) {
inner_prod = silk_sub32(
inner_prod,
silk_rshift32(
silk_smulbb(x[ptr1_idx + l - j] as i32, x[ptr2_idx + l - j] as i32),
*rshifts,
),
);
inner_prod = silk_add32(
inner_prod,
silk_rshift32(
silk_smulbb(x[ptr1_idx - j] as i32, x[ptr2_idx - j] as i32),
*rshifts,
),
);
xx[j * order + (lag + j)] = inner_prod;
xx[(lag + j) * order + j] = inner_prod;
}
}
}
const FREQ_TABLE_Q16: [i16; 27] = [
12111, 9804, 8235, 7100, 6239, 5565, 5022, 4575, 4202, 3885, 3612, 3375, 3167, 2984, 2820,
2674, 2542, 2422, 2313, 2214, 2123, 2038, 1961, 1889, 1822, 1760, 1702,
];
const fn smulwb_const(a: i32, b: i32) -> i32 {
(((a as i64) * (b as i16 as i64)) >> 16) as i32
}
const fn sine_window_start(win_type: i32, length: usize) -> (i32, i32, i32) {
let f_q16 = FREQ_TABLE_Q16[(length >> 2) - 4] as i32;
let c_q16 = smulwb_const(f_q16, -f_q16);
if win_type == 1 {
(0, f_q16 + (length as i32 >> 3), c_q16)
} else {
(
1 << 16,
(1 << 16) + (c_q16 >> 1) + (length as i32 >> 4),
c_q16,
)
}
}
const fn sine_window_step(s0_q16: i32, s1_q16: i32, c_q16: i32) -> ([i32; 4], i32, i32) {
let limit = 1 << 16;
let f0 = (s0_q16 + s1_q16) >> 1;
let f1 = s1_q16;
let mut s0_q16 = smulwb_const(s1_q16, c_q16) + (s1_q16 << 1) - s0_q16 + 1;
if s0_q16 > limit {
s0_q16 = limit;
}
let f2 = (s0_q16 + s1_q16) >> 1;
let f3 = s0_q16;
let mut s1_q16 = smulwb_const(s0_q16, c_q16) + (s0_q16 << 1) - s1_q16;
if s1_q16 > limit {
s1_q16 = limit;
}
([f0, f1, f2, f3], s0_q16, s1_q16)
}
const fn sine_window_factors<const N: usize>() -> [[i32; N]; 2] {
let mut tables = [[0i32; N]; 2];
let mut t = 0;
while t < 2 {
let (mut s0_q16, mut s1_q16, c_q16) = sine_window_start(t as i32 + 1, N);
let mut k = 0;
while k < N {
let (factors, next0, next1) = sine_window_step(s0_q16, s1_q16, c_q16);
let mut i = 0;
while i < 4 {
assert!(factors[i] >= 0 && factors[i] <= 1 << 16);
tables[t][k + i] = factors[i];
i += 1;
}
s0_q16 = next0;
s1_q16 = next1;
k += 4;
}
t += 1;
}
tables
}
static SINE_WINDOW_16: [[i32; 16]; 2] = sine_window_factors();
static SINE_WINDOW_24: [[i32; 24]; 2] = sine_window_factors();
static SINE_WINDOW_32: [[i32; 32]; 2] = sine_window_factors();
static SINE_WINDOW_48: [[i32; 48]; 2] = sine_window_factors();
static SINE_WINDOW_64: [[i32; 64]; 2] = sine_window_factors();
static SINE_WINDOW_72: [[i32; 72]; 2] = sine_window_factors();
static SINE_WINDOW_96: [[i32; 96]; 2] = sine_window_factors();
#[inline]
fn sine_window_table(win_type: i32, length: usize) -> Option<&'static [i32]> {
let t = if win_type == 1 { 0 } else { 1 };
Some(match length {
16 => &SINE_WINDOW_16[t],
24 => &SINE_WINDOW_24[t],
32 => &SINE_WINDOW_32[t],
48 => &SINE_WINDOW_48[t],
64 => &SINE_WINDOW_64[t],
72 => &SINE_WINDOW_72[t],
96 => &SINE_WINDOW_96[t],
_ => return None,
})
}
pub fn silk_apply_sine_window(px_win: &mut [i16], px: &[i16], win_type: i32, length: usize) {
if let Some(factors) = sine_window_table(win_type, length) {
let px_win = &mut px_win[..length];
let px = &px[..length];
for ((win, &x), &factor_q16) in px_win.iter_mut().zip(px).zip(factors) {
*win = (factor_q16.wrapping_mul(x as i32) >> 16) as i16;
}
return;
}
let (mut s0_q16, mut s1_q16, c_q16) = sine_window_start(win_type, length);
let groups = length.div_ceil(4);
let px_win = &mut px_win[..4 * groups];
let px = &px[..4 * groups];
let (px_win, _) = px_win.as_chunks_mut::<4>();
let (px, _) = px.as_chunks::<4>();
for (win, x) in px_win.iter_mut().zip(px) {
let (factors, next0, next1) = sine_window_step(s0_q16, s1_q16, c_q16);
for i in 0..4 {
win[i] = silk_smulwb(factors[i], x[i] as i32) as i16;
}
s0_q16 = next0;
s1_q16 = next1;
}
}
#[inline(always)]
pub fn silk_pitch_xcorr(x: &[i16], y: &[i16], xcorr: &mut [i32], len: usize, max_pitch: usize) {
debug_assert!(max_pitch > 0);
debug_assert!(x.len() >= len);
debug_assert!(xcorr.len() >= max_pitch);
let y_len = y.len();
let mut i = 0;
while i + 3 < max_pitch && y_len >= i + len + 3 {
let mut sum = [0i32; 4];
xcorr_kernel(x, &y[i..], &mut sum, len);
xcorr[i..i + 4].copy_from_slice(&sum);
i += 4;
}
while i < max_pitch {
let avail = len.min(y_len.saturating_sub(i));
xcorr[i] = if avail > 0 {
silk_inner_prod_aligned(&x[..avail], &y[i..], avail)
} else {
0
};
i += 1;
}
}
const WARPED_QC: i32 = 10;
const WARPED_QS: i32 = 13;
pub fn silk_warped_autocorrelation_fix(
corr: &mut [i32],
scale: &mut i32,
input: &[i16],
warping_q16: i32,
length: usize,
order: usize,
) {
debug_assert!((order & 1) == 0);
#[cfg(all(target_arch = "aarch64", target_feature = "neon"))]
if (order & 1) == 0
&& order <= MAX_SHAPE_LPC_ORDER
&& length <= input.len()
&& warping_q16.unsigned_abs() <= 65535
{
let input = &input[..length];
unsafe {
match order.div_ceil(4) {
0..=4 => warped_autocorr_neon::<4>(corr, scale, input, warping_q16, order),
5 => warped_autocorr_neon::<5>(corr, scale, input, warping_q16, order),
_ => warped_autocorr_neon::<6>(corr, scale, input, warping_q16, order),
}
}
return;
}
#[cfg(target_arch = "x86_64")]
if (order & 1) == 0
&& order <= MAX_SHAPE_LPC_ORDER
&& length <= input.len()
&& (0..=i16::MAX as i32).contains(&warping_q16)
{
let input = &input[..length];
if crate::compat::x86_has_avx2() {
unsafe {
match order.div_ceil(8) {
0..=2 => warped_autocorr_avx2::<2>(corr, scale, input, warping_q16, order),
_ => warped_autocorr_avx2::<3>(corr, scale, input, warping_q16, order),
}
}
return;
}
if crate::compat::x86_has_sse41() {
unsafe {
match order.div_ceil(4) {
0..=4 => warped_autocorr_sse41::<4>(corr, scale, input, warping_q16, order),
5 => warped_autocorr_sse41::<5>(corr, scale, input, warping_q16, order),
_ => warped_autocorr_sse41::<6>(corr, scale, input, warping_q16, order),
}
}
return;
}
}
silk_warped_autocorrelation_scalar(corr, scale, input, warping_q16, length, order);
}
#[cfg(all(target_arch = "aarch64", target_feature = "neon"))]
#[target_feature(enable = "neon")]
#[inline(never)]
fn warped_autocorr_neon<const K: usize>(
corr: &mut [i32],
scale: &mut i32,
input: &[i16],
warping_q16: i32,
order: usize,
) {
use crate::simd_mem::neon::{load_i32x4, store_i32x4};
use core::arch::aarch64::*;
const RING: usize = 8;
assert!(K >= 1 && K < RING && 4 * K <= MAX_SHAPE_LPC_ORDER && order <= 4 * K);
debug_assert!(warping_q16.unsigned_abs() <= 65535);
let warping = vdupq_n_s32(warping_q16 << 15);
let zero = vdupq_n_s32(0);
let mut cur = [zero; K];
let mut below_before = [zero; K];
let mut sum_lo = [vdupq_n_s64(0); K];
let mut sum_hi = [vdupq_n_s64(0); K];
let mut samples = [[0i32; 4]; RING];
let mut step = |m: usize, newest: int32x4_t| {
let oldest = m.wrapping_sub(K);
let below_last = vextq_s32::<1>(cur[0], newest);
let x_new = vextq_s32::<1>(load_i32x4(&samples[oldest % RING]), newest);
for r in 0..K {
let below = if r + 1 < K { cur[r + 1] } else { below_last };
let x = load_i32x4(&samples[oldest.wrapping_add(r) % RING]);
sum_lo[r] =
vsraq_n_s64::<16>(sum_lo[r], vmull_s32(vget_low_s32(cur[r]), vget_low_s32(x)));
sum_hi[r] = vsraq_n_s64::<16>(sum_hi[r], vmull_high_s32(cur[r], x));
cur[r] = vaddq_s32(
below_before[r],
vqdmulhq_s32(vsubq_s32(cur[r], below), warping),
);
below_before[r] = below;
}
store_i32x4(&mut samples[m % RING], x_new);
};
for (m, &sample) in input.iter().enumerate() {
step(m, vdupq_n_s32((sample as i32) << WARPED_QS));
}
for m in input.len()..input.len() + order {
step(m, zero);
}
let mut sums = [[0i64; 4]; K];
for r in 0..K {
sums[r] = [
vgetq_lane_s64::<0>(sum_lo[r]),
vgetq_lane_s64::<1>(sum_lo[r]),
vgetq_lane_s64::<0>(sum_hi[r]),
vgetq_lane_s64::<1>(sum_hi[r]),
];
}
warped_autocorr_finish(corr, scale, input, order, &sums, 0);
}
#[cfg(any(all(target_arch = "aarch64", target_feature = "neon"), target_arch = "x86_64"))]
#[inline]
fn warped_autocorr_finish<const L: usize, const K: usize>(
corr: &mut [i32],
scale: &mut i32,
input: &[i16],
order: usize,
sums: &[[i64; L]; K],
excess_per_step: i64,
) {
let steps = (input.len() + order) as i64;
let excess = steps.wrapping_mul(excess_per_step);
let mut energy = 0i64;
for &sample in input {
energy += (sample as i32 * sample as i32) as i64;
}
let mut corr_qc = [0i64; MAX_SHAPE_LPC_ORDER + 1];
corr_qc[0] = energy << WARPED_QC;
for i in 1..=order {
let d = L * K - i;
corr_qc[i] = sums[d % K][d / K].wrapping_sub(excess);
}
warped_autocorr_output(corr, scale, &corr_qc[..=order]);
}
#[cfg(target_arch = "x86_64")]
const WARPED_RING: usize = 8;
#[cfg(target_arch = "x86_64")]
#[target_feature(enable = "sse4.1")]
#[inline]
fn warped_mulw_sse41(d: __m128i, w_low: __m128i, w_high: __m128i) -> __m128i {
_mm_add_epi32(_mm_madd_epi16(d, w_high), _mm_mulhi_epu16(d, w_low))
}
#[cfg(target_arch = "x86_64")]
#[target_feature(enable = "avx2")]
#[inline]
fn warped_mulw_avx2(d: __m256i, w_low: __m256i, w_high: __m256i) -> __m256i {
_mm256_add_epi32(_mm256_madd_epi16(d, w_high), _mm256_mulhi_epu16(d, w_low))
}
#[cfg(target_arch = "x86_64")]
#[target_feature(enable = "sse4.1")]
#[inline(never)]
fn warped_autocorr_sse41<const K: usize>(
corr: &mut [i32],
scale: &mut i32,
input: &[i16],
warping_q16: i32,
order: usize,
) {
assert!(K >= 1 && K < WARPED_RING && 4 * K <= MAX_SHAPE_LPC_ORDER && order <= 4 * K);
assert!((0..=i16::MAX as i32).contains(&warping_q16));
let w_low = _mm_set1_epi32(warping_q16);
let w_high = _mm_set1_epi32(warping_q16 << 16);
let bias = _mm_set1_epi64x(i64::MIN);
let zero = _mm_setzero_si128();
let mut cur = [zero; K];
let mut below_before = [zero; K];
let mut sum_even = [zero; K];
let mut sum_odd = [zero; K];
let mut samples = [[zero; 2]; WARPED_RING];
let mut step = |m: usize, newest: __m128i| {
let oldest = m.wrapping_sub(K);
let w_low = core::hint::black_box(w_low);
let below_last = _mm_alignr_epi8::<4>(newest, cur[0]);
let x_new = _mm_alignr_epi8::<4>(newest, samples[oldest % WARPED_RING][0]);
for r in 0..K {
let below = if r + 1 < K { cur[r + 1] } else { below_last };
let [x, x_odd] = samples[oldest.wrapping_add(r) % WARPED_RING];
let even = _mm_mul_epi32(cur[r], x);
let odd = _mm_mul_epi32(_mm_shuffle_epi32::<0xF5>(cur[r]), x_odd);
sum_even[r] =
_mm_add_epi64(sum_even[r], _mm_srli_epi64::<16>(_mm_xor_si128(even, bias)));
sum_odd[r] = _mm_add_epi64(sum_odd[r], _mm_srli_epi64::<16>(_mm_xor_si128(odd, bias)));
let d = _mm_sub_epi32(cur[r], below);
cur[r] = _mm_add_epi32(below_before[r], warped_mulw_sse41(d, w_low, w_high));
below_before[r] = below;
}
samples[m % WARPED_RING] = [x_new, _mm_shuffle_epi32::<0xF5>(x_new)];
};
for (m, &sample) in input.iter().enumerate() {
step(m, _mm_cvtsi32_si128((sample as i32) << WARPED_QS));
}
for m in input.len()..input.len() + order {
step(m, zero);
}
let mut sums = [[0i64; 4]; K];
for r in 0..K {
sums[r] = [
_mm_cvtsi128_si64(sum_even[r]),
_mm_cvtsi128_si64(sum_odd[r]),
_mm_extract_epi64::<1>(sum_even[r]),
_mm_extract_epi64::<1>(sum_odd[r]),
];
}
warped_autocorr_finish(corr, scale, input, order, &sums, 1 << 47);
}
#[cfg(target_arch = "x86_64")]
#[target_feature(enable = "avx2")]
#[inline(never)]
fn warped_autocorr_avx2<const K: usize>(
corr: &mut [i32],
scale: &mut i32,
input: &[i16],
warping_q16: i32,
order: usize,
) {
assert!(K >= 1 && K < WARPED_RING && 8 * K <= MAX_SHAPE_LPC_ORDER && order <= 8 * K);
assert!((0..=i16::MAX as i32).contains(&warping_q16));
#[target_feature(enable = "avx2")]
#[inline]
fn shift_in(v: __m256i, newest: __m256i) -> __m256i {
let upper = _mm256_permute2x128_si256::<0x21>(v, newest);
_mm256_alignr_epi8::<4>(upper, v)
}
let w_low = _mm256_set1_epi32(warping_q16);
let w_high = _mm256_set1_epi32(warping_q16 << 16);
let bias = _mm256_set1_epi64x(i64::MIN);
let zero = _mm256_setzero_si256();
let mut cur = [zero; K];
let mut below_before = [zero; K];
let mut sum_even = [zero; K];
let mut sum_odd = [zero; K];
let mut samples = [[zero; 2]; WARPED_RING];
let mut step = |m: usize, newest: __m256i| {
let oldest = m.wrapping_sub(K);
let w_low = core::hint::black_box(w_low);
let below_last = shift_in(cur[0], newest);
let x_new = shift_in(samples[oldest % WARPED_RING][0], newest);
for r in 0..K {
let below = if r + 1 < K { cur[r + 1] } else { below_last };
let [x, x_odd] = samples[oldest.wrapping_add(r) % WARPED_RING];
let even = _mm256_mul_epi32(cur[r], x);
let odd = _mm256_mul_epi32(_mm256_shuffle_epi32::<0xF5>(cur[r]), x_odd);
sum_even[r] = _mm256_add_epi64(
sum_even[r],
_mm256_srli_epi64::<16>(_mm256_xor_si256(even, bias)),
);
sum_odd[r] =
_mm256_add_epi64(sum_odd[r], _mm256_srli_epi64::<16>(_mm256_xor_si256(odd, bias)));
let d = _mm256_sub_epi32(cur[r], below);
cur[r] = _mm256_add_epi32(below_before[r], warped_mulw_avx2(d, w_low, w_high));
below_before[r] = below;
}
samples[m % WARPED_RING] = [x_new, _mm256_shuffle_epi32::<0xF5>(x_new)];
};
for (m, &sample) in input.iter().enumerate() {
step(m, _mm256_castsi128_si256(_mm_cvtsi32_si128((sample as i32) << WARPED_QS)));
}
for m in input.len()..input.len() + order {
step(m, zero);
}
let mut sums = [[0i64; 8]; K];
for r in 0..K {
sums[r] = [
_mm256_extract_epi64::<0>(sum_even[r]),
_mm256_extract_epi64::<0>(sum_odd[r]),
_mm256_extract_epi64::<1>(sum_even[r]),
_mm256_extract_epi64::<1>(sum_odd[r]),
_mm256_extract_epi64::<2>(sum_even[r]),
_mm256_extract_epi64::<2>(sum_odd[r]),
_mm256_extract_epi64::<3>(sum_even[r]),
_mm256_extract_epi64::<3>(sum_odd[r]),
];
}
warped_autocorr_finish(corr, scale, input, order, &sums, 1 << 47);
}
fn silk_warped_autocorrelation_scalar(
corr: &mut [i32],
scale: &mut i32,
input: &[i16],
warping_q16: i32,
length: usize,
order: usize,
) {
let mut state_qs = [0i32; MAX_SHAPE_LPC_ORDER + 1];
let mut corr_qc = [0i64; MAX_SHAPE_LPC_ORDER + 1];
let state_qs = &mut state_qs[..=order];
let corr_qc = &mut corr_qc[..=order];
let input = &input[..length.min(input.len())];
let (blocks, rest) = input.as_chunks::<4>();
for x in blocks {
let x_qs = x.map(|sample| (sample as i32) << WARPED_QS);
warped_autocorr_pass(state_qs, corr_qc, x_qs, warping_q16);
}
for &x in rest {
warped_autocorr_pass(state_qs, corr_qc, [(x as i32) << WARPED_QS], warping_q16);
}
warped_autocorr_output(corr, scale, corr_qc);
}
#[inline(always)]
fn warped_autocorr_pass<const N: usize>(
state_qs: &mut [i32],
corr_qc: &mut [i64],
x_qs: [i32; N],
warping_q16: i32,
) {
let order = state_qs.len() - 1;
let corr_qc = &mut corr_qc[..=order];
let mut cur = x_qs;
let mut before = state_qs[0];
for i in 0..order {
let before_next = state_qs[i + 1];
let mut next = [0i32; N];
next[0] = silk_smlaww(before, before_next.wrapping_sub(cur[0]), warping_q16);
for k in 1..N {
next[k] = silk_smlaww(cur[k - 1], next[k - 1].wrapping_sub(cur[k]), warping_q16);
}
let mut sum = corr_qc[i];
for k in 0..N {
sum += silk_rshift64(silk_smull(cur[k], x_qs[k]), 2 * WARPED_QS - WARPED_QC);
}
corr_qc[i] = sum;
state_qs[i] = cur[N - 1];
cur = next;
before = before_next;
}
let mut sum = corr_qc[order];
for k in 0..N {
sum += silk_rshift64(silk_smull(cur[k], x_qs[k]), 2 * WARPED_QS - WARPED_QC);
}
corr_qc[order] = sum;
state_qs[order] = cur[N - 1];
}
#[inline(always)]
fn warped_autocorr_output(corr: &mut [i32], scale: &mut i32, corr_qc: &[i64]) {
let corr = &mut corr[..corr_qc.len()];
let mut lsh = silk_clz64(corr_qc[0]) - 35;
lsh = silk_limit_32(lsh, -12 - WARPED_QC, 30 - WARPED_QC);
*scale = -(WARPED_QC + lsh);
if lsh >= 0 {
for (c, &c_qc) in corr.iter_mut().zip(corr_qc) {
*c = (c_qc << lsh) as i32;
}
} else {
for (c, &c_qc) in corr.iter_mut().zip(corr_qc) {
*c = (c_qc >> (-lsh)) as i32;
}
}
}
pub fn silk_schur(rc_q15: &mut [i16], c: &[i32], order: usize) -> i32 {
let mut c_inner = [[0i32; 2]; MAX_LPC_ORDER + 1];
let mut ctmp1: i32;
let mut ctmp2: i32;
let mut rc_tmp_q15: i32;
assert!(order <= MAX_LPC_ORDER);
let lz = c[0].leading_zeros() as i32;
if lz < 2 {
for i in 0..=order {
c_inner[i][0] = c[i] >> 1;
c_inner[i][1] = c[i] >> 1;
}
} else if lz > 2 {
let lz_adj = lz - 2;
for i in 0..=order {
c_inner[i][0] = c[i] << lz_adj;
c_inner[i][1] = c[i] << lz_adj;
}
} else {
for i in 0..=order {
c_inner[i][0] = c[i];
c_inner[i][1] = c[i];
}
}
for k in 0..order {
if c_inner[k + 1][0].abs() >= c_inner[0][1] {
if c_inner[k + 1][0] > 0 {
rc_q15[k] = -32440;
} else {
rc_q15[k] = 32440;
}
return c_inner[0][1];
}
rc_tmp_q15 = -silk_div32_16(c_inner[k + 1][0], (c_inner[0][1] >> 15).max(1));
rc_tmp_q15 = silk_sat16(rc_tmp_q15);
rc_q15[k] = rc_tmp_q15 as i16;
for n in 0..order - k {
ctmp1 = c_inner[n + k + 1][0];
ctmp2 = c_inner[n][1];
c_inner[n + k + 1][0] = silk_smlawb(ctmp1, ctmp2 << 1, rc_tmp_q15);
c_inner[n][1] = silk_smlawb(ctmp2, ctmp1 << 1, rc_tmp_q15);
}
}
c_inner[0][1]
}
pub fn silk_k2a(a_q24: &mut [i32], rc_q15: &[i16], order: usize) {
for k in 0..order {
let rc = rc_q15[k] as i32;
for n in 0..(k + 1) >> 1 {
let tmp1 = a_q24[n];
let tmp2 = a_q24[k - n - 1];
a_q24[n] = silk_smlawb(tmp1, tmp2 << 1, rc);
a_q24[k - n - 1] = silk_smlawb(tmp2, tmp1 << 1, rc);
}
a_q24[k] = -(rc << 9);
}
}
pub fn silk_bwexpander(ar: &mut [i16], d: usize, mut chirp_q16: i32) {
let chirp_minus_one_q16 = chirp_q16 - 65536;
for ar_val in ar[..d - 1].iter_mut() {
*ar_val = silk_rshift_round((*ar_val as i32).wrapping_mul(chirp_q16), 16) as i16;
chirp_q16 += silk_rshift_round(chirp_q16.wrapping_mul(chirp_minus_one_q16), 16);
}
ar[d - 1] = silk_rshift_round((ar[d - 1] as i32).wrapping_mul(chirp_q16), 16) as i16;
}
pub fn silk_lpc_analysis_filter(
out: &mut [i16],
input: &[i16],
b: &[i16],
len: usize,
d: usize,
_arch: i32,
) {
assert!(d >= 6);
assert!((d & 1) == 0);
assert!(d <= len);
let out = &mut out[..len];
let input = &input[..len];
#[cfg(target_arch = "x86_64")]
if d <= 16 && crate::compat::x86_has_avx2() {
unsafe {
silk_lpc_analysis_filter_avx2(out, input, b, len, d);
}
return;
}
match d {
6 => lpc_analysis_filter_taps(out, input, &b[..6]),
8 => lpc_analysis_filter_taps(out, input, &b[..8]),
10 => lpc_analysis_filter_taps(out, input, &b[..10]),
12 => lpc_analysis_filter_taps(out, input, &b[..12]),
14 => lpc_analysis_filter_taps(out, input, &b[..14]),
16 => lpc_analysis_filter_taps(out, input, &b[..16]),
_ => lpc_analysis_filter_taps(out, input, &b[..d]),
}
}
#[inline(always)]
fn lpc_analysis_filter_taps(out: &mut [i16], input: &[i16], b: &[i16]) {
let d = b.len();
debug_assert_eq!(out.len(), input.len());
let (zeros, out) = out.split_at_mut(d);
zeros.fill(0);
for (o, window) in out.iter_mut().zip(input.windows(d + 1)) {
let (history, x) = window.split_at(d);
let mut sum = 0i32;
for (&sample, &coef) in history.iter().rev().zip(b) {
sum = sum.wrapping_add(silk_smulbb(sample as i32, coef as i32));
}
let out32_q12 = ((x[0] as i32) << 12).wrapping_sub(sum);
*o = silk_sat16(silk_rshift_round(out32_q12, 12)) as i16;
}
}
pub fn silk_scale_copy_vector16(
data_out: &mut [i16],
data_in: &[i16],
gain_q16: i32,
data_size: usize,
) {
for i in 0..data_size {
let tmp32 = silk_smulwb(gain_q16, data_in[i] as i32);
data_out[i] = silk_sat16(tmp32) as i16;
}
}
#[cfg(target_arch = "x86_64")]
use crate::simd_mem::{
array_at,
x86::{load_i16x4, load_i16x8, load_i16x16, store_i32x4},
};
#[cfg(target_arch = "x86_64")]
use core::arch::x86_64::*;
#[cfg(target_arch = "x86_64")]
#[target_feature(enable = "avx2")]
#[inline]
fn silk_inner_prod_aligned_avx2(ptr1: &[i16], ptr2: &[i16], len: usize) -> i32 {
assert!(ptr1.len() >= len && ptr2.len() >= len);
let mut acc0 = _mm256_setzero_si256();
let mut acc1 = _mm256_setzero_si256();
let mut i = 0;
while i + 32 <= len {
let (a, b) = unsafe { (ptr1.get_unchecked(i..i + 32), ptr2.get_unchecked(i..i + 32)) };
let a0 = load_i16x16(array_at(a, 0));
let b0 = load_i16x16(array_at(b, 0));
let a1 = load_i16x16(array_at(a, 16));
let b1 = load_i16x16(array_at(b, 16));
acc0 = _mm256_add_epi32(acc0, _mm256_madd_epi16(a0, b0));
acc1 = _mm256_add_epi32(acc1, _mm256_madd_epi16(a1, b1));
i += 32;
}
while i + 16 <= len {
let (a, b) = unsafe { (ptr1.get_unchecked(i..i + 16), ptr2.get_unchecked(i..i + 16)) };
let a = load_i16x16(array_at(a, 0));
let b = load_i16x16(array_at(b, 0));
acc0 = _mm256_add_epi32(acc0, _mm256_madd_epi16(a, b));
i += 16;
}
let acc = _mm256_add_epi32(acc0, acc1);
let hi = _mm256_extracti128_si256(acc, 1);
let lo = _mm256_castsi256_si128(acc);
let sum4 = _mm_add_epi32(lo, hi);
let sum2 = _mm_add_epi32(sum4, _mm_srli_si128(sum4, 8));
let sum1 = _mm_add_epi32(sum2, _mm_srli_si128(sum2, 4));
let mut result = _mm_cvtsi128_si32(sum1);
if i + 8 <= len {
let (a, b) = unsafe { (ptr1.get_unchecked(i..i + 8), ptr2.get_unchecked(i..i + 8)) };
let a = load_i16x8(array_at(a, 0));
let b = load_i16x8(array_at(b, 0));
let p = _mm_madd_epi16(a, b);
let p2 = _mm_add_epi32(p, _mm_srli_si128(p, 8));
let p1 = _mm_add_epi32(p2, _mm_srli_si128(p2, 4));
result = result.wrapping_add(_mm_cvtsi128_si32(p1));
i += 8;
}
while i < len {
result = result.wrapping_add((ptr1[i] as i32).wrapping_mul(ptr2[i] as i32));
i += 1;
}
result
}
#[cfg(target_arch = "x86_64")]
#[target_feature(enable = "avx2")]
#[inline]
fn xcorr_kernel_avx2(x: &[i16], y: &[i16], sum: &mut [i32; 4], len: usize) {
assert!(x.len() >= len && y.len() >= len + 4);
let mut acc = _mm_setzero_si128();
let mut i = 0;
while i + 4 <= len {
let (x4, y8) = unsafe { (x.get_unchecked(i..i + 4), y.get_unchecked(i..i + 8)) };
let x4 = load_i16x4(array_at(x4, 0));
let xdup = _mm_unpacklo_epi64(x4, x4);
let y8 = load_i16x8(array_at(y8, 0));
let y1 = _mm_bsrli_si128(y8, 2);
let y2 = _mm_bsrli_si128(y8, 4);
let y3 = _mm_bsrli_si128(y8, 6);
let y4_01 = _mm_unpacklo_epi64(y8, y1);
let y4_23 = _mm_unpacklo_epi64(y2, y3);
let p01 = _mm_madd_epi16(xdup, y4_01);
let p23 = _mm_madd_epi16(xdup, y4_23);
acc = _mm_add_epi32(acc, _mm_hadd_epi32(p01, p23));
i += 4;
}
let mut tmp = [0i32; 4];
store_i32x4(&mut tmp, acc);
sum[0] = sum[0].wrapping_add(tmp[0]);
sum[1] = sum[1].wrapping_add(tmp[1]);
sum[2] = sum[2].wrapping_add(tmp[2]);
sum[3] = sum[3].wrapping_add(tmp[3]);
let mut y_0 = y[i];
let mut y_1 = y[i + 1];
let mut y_2 = y[i + 2];
for j in i..len {
let y_3 = y[j + 3];
let xv = x[j] as i32;
sum[0] = sum[0].wrapping_add(xv.wrapping_mul(y_0 as i32));
sum[1] = sum[1].wrapping_add(xv.wrapping_mul(y_1 as i32));
sum[2] = sum[2].wrapping_add(xv.wrapping_mul(y_2 as i32));
sum[3] = sum[3].wrapping_add(xv.wrapping_mul(y_3 as i32));
y_0 = y_1;
y_1 = y_2;
y_2 = y_3;
}
}
#[cfg(target_arch = "x86_64")]
#[target_feature(enable = "avx2")]
#[inline]
fn silk_sum_sqr_shift_avx2(x: &[i16], len: usize, shft: i32) -> i32 {
assert!(x.len() >= len);
let mut acc = _mm256_setzero_si256();
let mut i = 0;
while i + 16 <= len {
let v = load_i16x16(array_at(unsafe { x.get_unchecked(i..i + 16) }, 0));
let sq = _mm256_madd_epi16(v, v);
let vshift = _mm256_set1_epi32(shft);
let shifted = if shft > 0 {
_mm256_srlv_epi32(sq, vshift)
} else {
sq
};
acc = _mm256_add_epi32(acc, shifted);
i += 16;
}
let hi = _mm256_extracti128_si256(acc, 1);
let lo = _mm256_castsi256_si128(acc);
let sum4 = _mm_add_epi32(lo, hi);
let sum2 = _mm_add_epi32(sum4, _mm_srli_si128(sum4, 8));
let sum1 = _mm_add_epi32(sum2, _mm_srli_si128(sum2, 4));
let nrg = _mm_cvtsi128_si32(sum1);
sum_sqr_pairs_shifted(&x[i..len], nrg, shft)
}
#[cfg(target_arch = "x86_64")]
#[target_feature(enable = "avx2")]
#[inline]
fn silk_lpc_analysis_filter_avx2(
out: &mut [i16],
input: &[i16],
b: &[i16],
len: usize,
d: usize,
) {
debug_assert!(d >= 6 && (d & 1) == 0 && d <= 16);
assert!(input.len() >= len && out.len() >= len);
let mut b_rev_pad = [0i16; 16];
for k in 0..d {
b_rev_pad[15 - k] = b[k];
}
for v in out[..d].iter_mut() {
*v = 0;
}
let avx_start = 16usize.max(d);
for ix in d..avx_start.min(len) {
let mut s: i32 = 0;
for k in 0..d {
s = s.wrapping_add((input[ix - 1 - k] as i32).wrapping_mul(b[k] as i32));
}
let out32_q12 = ((input[ix] as i32) << 12).wrapping_sub(s);
out[ix] = silk_sat16(silk_rshift_round(out32_q12, 12)) as i16;
}
if avx_start >= len {
return;
}
let b_lo = _mm256_cvtepi16_epi32(load_i16x8(array_at(&b_rev_pad, 0)));
let b_hi = _mm256_cvtepi16_epi32(load_i16x8(array_at(&b_rev_pad, 8)));
for ix in avx_start..len {
let history: &[i16; 16] = array_at(input, ix - 16);
let inp_lo = _mm256_cvtepi16_epi32(load_i16x8(array_at(history, 0)));
let inp_hi = _mm256_cvtepi16_epi32(load_i16x8(array_at(history, 8)));
let prod_lo = _mm256_mullo_epi32(inp_lo, b_lo);
let prod_hi = _mm256_mullo_epi32(inp_hi, b_hi);
let sum256 = _mm256_add_epi32(prod_lo, prod_hi);
let hi128 = _mm256_extracti128_si256(sum256, 1);
let lo128 = _mm256_castsi256_si128(sum256);
let sum128 = _mm_add_epi32(lo128, hi128);
let sum64 = _mm_add_epi32(sum128, _mm_srli_si128(sum128, 8));
let sum32 = _mm_add_epi32(sum64, _mm_srli_si128(sum64, 4));
let s = _mm_cvtsi128_si32(sum32);
let out32_q12 = ((input[ix] as i32) << 12).wrapping_sub(s);
out[ix] = silk_sat16(silk_rshift_round(out32_q12, 12)) as i16;
}
}
#[cfg(target_arch = "aarch64")]
#[target_feature(enable = "neon")]
#[inline]
fn silk_biquad_alt_stride2_neon(
input_output: &mut [i16],
b_q28: &[i32; 3],
a_q28: &[i32; 2],
s: &mut [i32; 4],
) {
use crate::simd_mem::{
array_at,
neon::{load_i16x4, load_i32x2, load_i32x4, store_i16x4, store_lane_i32x4},
};
use core::arch::aarch64::*;
debug_assert!(input_output.len().is_multiple_of(2));
let offset_s32x2 = vdup_n_s32((1 << 14) - 1);
let offset_s32x4 = vcombine_s32(offset_s32x2, offset_s32x2);
let a_q28_s32x2 = vneg_s32(load_i32x2(a_q28));
let a_l_s32x2 = vreinterpret_s32_u32(vshr_n_u32(
vreinterpret_u32_s32(vshl_n_s32(a_q28_s32x2, 18)),
3,
));
let mut a_u_s32x2 = vshr_n_s32(a_q28_s32x2, 14);
a_u_s32x2 = vshr_n_s32(vshl_n_s32(a_u_s32x2, 16), 1);
let b_q28_s32x2 = load_i32x2(array_at(b_q28, 0));
let t_s32x2 = load_i32x2(array_at(b_q28, 1));
let t0_s32x2x2 = vzip_s32(a_l_s32x2, a_l_s32x2);
let t1_s32x2x2 = vzip_s32(a_u_s32x2, a_u_s32x2);
let t2_s32x2x2 = vzip_s32(t_s32x2, t_s32x2);
let a_l_s32x4 = vcombine_s32(t0_s32x2x2.0, t0_s32x2x2.1);
let a_u_s32x4 = vcombine_s32(t1_s32x2x2.0, t1_s32x2x2.1);
let b_q28_s32x4 = vcombine_s32(t2_s32x2x2.0, t2_s32x2x2.1);
let mut s_s32x4 = load_i32x4(s);
let s_s32x2x2 = vtrn_s32(vget_low_s32(s_s32x4), vget_high_s32(s_s32x4));
s_s32x4 = vcombine_s32(s_s32x2x2.0, s_s32x2x2.1);
let (quads, rest) = input_output.as_chunks_mut::<4>();
for quad in quads.iter_mut() {
let in_s16x4 = load_i16x4(quad);
let in_s32x4 = vshll_n_s16(in_s16x4, 15);
let t_s32x4 = vqdmulhq_lane_s32(in_s32x4, b_q28_s32x2, 0);
let in_low = vcombine_s32(vget_low_s32(in_s32x4), vget_low_s32(in_s32x4));
let t_low = vget_low_s32(t_s32x4);
let mut out32_q14_s32x2 = vadd_s32(vget_low_s32(s_s32x4), t_low);
out32_q14_s32x2 = vshl_n_s32(out32_q14_s32x2, 2);
s_s32x4 = vcombine_s32(vget_high_s32(s_s32x4), vdup_n_s32(0));
let out32_q14_s32x4 = vcombine_s32(out32_q14_s32x2, out32_q14_s32x2);
let tmp_s32x4 = vqdmulhq_s32(out32_q14_s32x4, a_l_s32x4);
s_s32x4 = vrsraq_n_s32(s_s32x4, tmp_s32x4, 14);
let tmp_s32x4 = vqdmulhq_s32(out32_q14_s32x4, a_u_s32x4);
s_s32x4 = vaddq_s32(s_s32x4, tmp_s32x4);
let tmp_s32x4 = vqdmulhq_s32(in_low, b_q28_s32x4);
s_s32x4 = vaddq_s32(s_s32x4, tmp_s32x4);
let in_high = vcombine_s32(vget_high_s32(in_s32x4), vget_high_s32(in_s32x4));
let t_high = vget_high_s32(t_s32x4);
let mut out32_q14_1_s32x2 = vadd_s32(vget_low_s32(s_s32x4), t_high);
out32_q14_1_s32x2 = vshl_n_s32(out32_q14_1_s32x2, 2);
s_s32x4 = vcombine_s32(vget_high_s32(s_s32x4), vdup_n_s32(0));
let out32_q14_1_s32x4 = vcombine_s32(out32_q14_1_s32x2, out32_q14_1_s32x2);
let tmp_s32x4 = vqdmulhq_s32(out32_q14_1_s32x4, a_l_s32x4);
s_s32x4 = vrsraq_n_s32(s_s32x4, tmp_s32x4, 14);
let tmp_s32x4 = vqdmulhq_s32(out32_q14_1_s32x4, a_u_s32x4);
s_s32x4 = vaddq_s32(s_s32x4, tmp_s32x4);
let tmp_s32x4 = vqdmulhq_s32(in_high, b_q28_s32x4);
s_s32x4 = vaddq_s32(s_s32x4, tmp_s32x4);
let out32_q14_combined = vcombine_s32(out32_q14_s32x2, out32_q14_1_s32x2);
let out_s32x4 = vaddq_s32(out32_q14_combined, offset_s32x4);
let out_s16x4 = vqshrn_n_s32(out_s32x4, 14);
store_i16x4(quad, out_s16x4);
}
if let [io0, io1, ..] = rest {
let in0 = *io0 as i32;
let in1 = *io1 as i32;
let out32_q14_0 = silk_smlawb(vgetq_lane_s32(s_s32x4, 0), b_q28[0], in0) << 2;
let out32_q14_1 = silk_smlawb(vgetq_lane_s32(s_s32x4, 1), b_q28[0], in1) << 2;
let a0_l = (-a_q28[0]) & 0x00003FFF;
let a0_u = -a_q28[0] >> 14;
let a1_l = (-a_q28[1]) & 0x00003FFF;
let a1_u = -a_q28[1] >> 14;
let s0_new = vgetq_lane_s32(s_s32x4, 2)
.wrapping_add(silk_rshift_round(silk_smulwb(out32_q14_0, a0_l), 14));
let s2_new = vgetq_lane_s32(s_s32x4, 3)
.wrapping_add(silk_rshift_round(silk_smulwb(out32_q14_1, a0_l), 14));
let s0_new = silk_smlawb(s0_new, out32_q14_0, a0_u);
let s2_new = silk_smlawb(s2_new, out32_q14_1, a0_u);
let s0_new = silk_smlawb(s0_new, b_q28[1], in0);
let s2_new = silk_smlawb(s2_new, b_q28[1], in1);
let s1_new = silk_rshift_round(silk_smulwb(out32_q14_0, a1_l), 14);
let s3_new = silk_rshift_round(silk_smulwb(out32_q14_1, a1_l), 14);
let s1_new = silk_smlawb(s1_new, out32_q14_0, a1_u);
let s3_new = silk_smlawb(s3_new, out32_q14_1, a1_u);
let s1_new = silk_smlawb(s1_new, b_q28[2], in0);
let s3_new = silk_smlawb(s3_new, b_q28[2], in1);
*io0 = silk_sat16(silk_rshift(out32_q14_0.wrapping_add((1 << 14) - 1), 14)) as i16;
*io1 = silk_sat16(silk_rshift(out32_q14_1.wrapping_add((1 << 14) - 1), 14)) as i16;
s[0] = s0_new;
s[1] = s1_new;
s[2] = s2_new;
s[3] = s3_new;
} else {
store_lane_i32x4::<0>(&mut s[0], s_s32x4);
store_lane_i32x4::<2>(&mut s[1], s_s32x4);
store_lane_i32x4::<1>(&mut s[2], s_s32x4);
store_lane_i32x4::<3>(&mut s[3], s_s32x4);
}
}
#[cfg(all(test, feature = "std"))]
mod tests {
use super::*;
fn lpc_analysis_filter_ref(out: &mut [i16], input: &[i16], b: &[i16], len: usize, d: usize) {
out[..d].fill(0);
for ix in d..len {
let mut s = 0i32;
for k in 0..d {
s = s.wrapping_add((input[ix - 1 - k] as i32).wrapping_mul(b[k] as i32));
}
let out32_q12 = ((input[ix] as i32) << 12).wrapping_sub(s);
out[ix] = silk_sat16(silk_rshift_round(out32_q12, 12)) as i16;
}
}
struct TestRng(u64);
impl TestRng {
fn next_u32(&mut self) -> u32 {
self.0 ^= self.0 << 13;
self.0 ^= self.0 >> 7;
self.0 ^= self.0 << 17;
(self.0 >> 16) as u32
}
fn below(&mut self, n: u32) -> u32 {
self.next_u32() % n
}
fn i16(&mut self) -> i16 {
self.next_u32() as i16
}
}
fn test_signal(rng: &mut TestRng, len: usize, kind: u32) -> Vec<i16> {
let shift = rng.below(12);
(0..len)
.map(|i| match kind {
0 => rng.i16(),
1 => rng.i16() >> shift,
2 => {
if rng.below(2) == 0 {
i16::MAX
} else {
i16::MIN
}
}
3 => i16::MIN,
4 => i16::MAX,
_ => {
if i & 1 == 0 {
i16::MIN
} else {
i16::MAX
}
}
})
.collect()
}
fn warped_autocorrelation_ref(
corr: &mut [i32],
scale: &mut i32,
input: &[i16],
warping_q16: i32,
length: usize,
order: usize,
) {
const QC: i32 = 10;
const QS: i32 = 13;
let mut state_qs = [0i32; MAX_SHAPE_LPC_ORDER + 1];
let mut corr_qc = [0i64; MAX_SHAPE_LPC_ORDER + 1];
for &input_n in &input[..length] {
let mut tmp1_qs = (input_n as i32) << QS;
let mut i = 0;
while i < order {
let tmp2_qs = silk_smlaww(
state_qs[i],
state_qs[i + 1].wrapping_sub(tmp1_qs),
warping_q16,
);
state_qs[i] = tmp1_qs;
corr_qc[i] += silk_rshift64(silk_smull(tmp1_qs, state_qs[0]), 2 * QS - QC);
tmp1_qs = silk_smlaww(
state_qs[i + 1],
state_qs[i + 2].wrapping_sub(tmp2_qs),
warping_q16,
);
state_qs[i + 1] = tmp2_qs;
corr_qc[i + 1] += silk_rshift64(silk_smull(tmp2_qs, state_qs[0]), 2 * QS - QC);
i += 2;
}
state_qs[order] = tmp1_qs;
corr_qc[order] += silk_rshift64(silk_smull(tmp1_qs, state_qs[0]), 2 * QS - QC);
}
let lsh = silk_limit_32(silk_clz64(corr_qc[0]) - 35, -12 - QC, 30 - QC);
*scale = -(QC + lsh);
for i in 0..=order {
corr[i] = if lsh >= 0 {
(corr_qc[i] << lsh) as i32
} else {
(corr_qc[i] >> (-lsh)) as i32
};
}
}
type WarpedFn = fn(&mut [i32], &mut i32, &[i16], i32, usize, usize);
fn check_warped(name: &str, f: WarpedFn, input: &[i16], warping_q16: i32, order: usize) {
let length = input.len();
let mut corr = [0i32; MAX_SHAPE_LPC_ORDER + 1];
let mut corr_ref = [0i32; MAX_SHAPE_LPC_ORDER + 1];
let (mut scale, mut scale_ref) = (0i32, 0i32);
f(&mut corr, &mut scale, input, warping_q16, length, order);
warped_autocorrelation_ref(
&mut corr_ref,
&mut scale_ref,
input,
warping_q16,
length,
order,
);
assert_eq!(
(&corr[..=order], scale),
(&corr_ref[..=order], scale_ref),
"{name}: order={order} length={length} warping_q16={warping_q16}"
);
}
fn test_warping(rng: &mut TestRng, case: u32) -> i32 {
match case % 8 {
0 => 15729,
1 => 7864,
2 => 0,
3 => 65535,
4 => -65535,
5 => rng.below(16385) as i32,
6 => rng.below(65536) as i32,
_ => -(rng.below(65536) as i32),
}
}
fn check_warped_all(name: &str, f: WarpedFn) {
let mut rng = TestRng(0x5eed_1234_9abc_def1);
for order in (0..=MAX_SHAPE_LPC_ORDER).step_by(2) {
for length in 0..=40 {
let kind = (order + length) as u32 % 6;
let input = test_signal(&mut rng, length, kind);
let warping_q16 = test_warping(&mut rng, (order / 2 + length) as u32);
check_warped(name, f, &input, warping_q16, order);
}
}
for case in 0..3000u32 {
let order = 2 * rng.below(MAX_SHAPE_LPC_ORDER as u32 / 2 + 1) as usize;
let length = match case % 4 {
0 => 240,
1 => 120,
_ => rng.below(241) as usize,
};
let input = test_signal(&mut rng, length, case % 6);
let warping_q16 = test_warping(&mut rng, case / 6);
check_warped(name, f, &input, warping_q16, order);
}
}
#[test]
fn warped_autocorrelation_scalar_matches_reference() {
check_warped_all("scalar", silk_warped_autocorrelation_scalar);
}
#[test]
fn warped_autocorrelation_matches_reference() {
check_warped_all("dispatch", silk_warped_autocorrelation_fix);
}
#[cfg(target_arch = "x86_64")]
type WarpedKernel = dyn Fn(&mut [i32], &mut i32, &[i16], i32, usize);
#[cfg(target_arch = "x86_64")]
fn check_warped_x86(name: &str, max_order: usize, run: &WarpedKernel) {
let check = |input: &[i16], warping_q16: i32, order: usize| {
let mut corr = [0i32; MAX_SHAPE_LPC_ORDER + 1];
let mut corr_ref = [0i32; MAX_SHAPE_LPC_ORDER + 1];
let (mut scale, mut scale_ref) = (0i32, 0i32);
run(&mut corr, &mut scale, input, warping_q16, order);
silk_warped_autocorrelation_scalar(
&mut corr_ref,
&mut scale_ref,
input,
warping_q16,
input.len(),
order,
);
assert_eq!(
(&corr[..=order], scale),
(&corr_ref[..=order], scale_ref),
"{name}: order={order} length={} warping_q16={warping_q16}",
input.len()
);
};
let warping = |rng: &mut TestRng, case: usize| match case % 6 {
0 => 15728,
1 => 16383,
2 => 0,
3 => i16::MAX as i32,
4 => 1,
_ => rng.below(1 << 15) as i32,
};
let mut rng = TestRng(0x3c6e_f372_fe94_f82b);
for order in (0..=max_order).step_by(2) {
for length in (0..=40).chain([120, 180, 240]) {
let input = test_signal(&mut rng, length, (order + length) as u32 % 6);
check(&input, warping(&mut rng, order / 2 + length), order);
}
}
for case in 0..2000 {
let order = 2 * rng.below(max_order as u32 / 2 + 1) as usize;
let length = rng.below(481) as usize;
let input = test_signal(&mut rng, length, case as u32 % 6);
check(&input, warping(&mut rng, case / 6), order);
}
}
#[cfg(target_arch = "x86_64")]
#[test]
fn warped_autocorrelation_sse41_matches_scalar() {
if !crate::compat::x86_has_sse41() {
return;
}
check_warped_x86("sse4.1 K=4", 16, &|corr, scale, input, warping_q16, order| {
unsafe { warped_autocorr_sse41::<4>(corr, scale, input, warping_q16, order) }
});
check_warped_x86("sse4.1 K=5", 20, &|corr, scale, input, warping_q16, order| {
unsafe { warped_autocorr_sse41::<5>(corr, scale, input, warping_q16, order) }
});
check_warped_x86("sse4.1 K=6", 24, &|corr, scale, input, warping_q16, order| {
unsafe { warped_autocorr_sse41::<6>(corr, scale, input, warping_q16, order) }
});
}
#[cfg(target_arch = "x86_64")]
#[test]
fn warped_autocorrelation_avx2_matches_scalar() {
if !crate::compat::x86_has_avx2() {
return;
}
check_warped_x86("avx2 K=2", 16, &|corr, scale, input, warping_q16, order| {
unsafe { warped_autocorr_avx2::<2>(corr, scale, input, warping_q16, order) }
});
check_warped_x86("avx2 K=3", 24, &|corr, scale, input, warping_q16, order| {
unsafe { warped_autocorr_avx2::<3>(corr, scale, input, warping_q16, order) }
});
}
fn apply_sine_window_ref(px_win: &mut [i16], px: &[i16], win_type: i32, length: usize) {
let f_q16 = FREQ_TABLE_Q16[(length >> 2) - 4] as i32;
let c_q16 = silk_smulwb(f_q16, -f_q16);
let (mut s0_q16, mut s1_q16) = if win_type == 1 {
(0, f_q16 + (length as i32 >> 3))
} else {
(1 << 16, (1 << 16) + (c_q16 >> 1) + (length as i32 >> 4))
};
for k in (0..length).step_by(4) {
px_win[k] = silk_smulwb((s0_q16 + s1_q16) >> 1, px[k] as i32) as i16;
px_win[k + 1] = silk_smulwb(s1_q16, px[k + 1] as i32) as i16;
s0_q16 = silk_smulwb(s1_q16, c_q16) + (s1_q16 << 1) - s0_q16 + 1;
s0_q16 = s0_q16.min(1 << 16);
px_win[k + 2] = silk_smulwb((s0_q16 + s1_q16) >> 1, px[k + 2] as i32) as i16;
px_win[k + 3] = silk_smulwb(s0_q16, px[k + 3] as i32) as i16;
s1_q16 = silk_smulwb(s0_q16, c_q16) + (s0_q16 << 1) - s1_q16;
s1_q16 = s1_q16.min(1 << 16);
}
}
#[test]
fn apply_sine_window_matches_reference() {
let mut rng = TestRng(0x0f1e_2d3c_4b5a_6978);
for length in (16..=120usize).step_by(4) {
for win_type in [1, 2] {
for kind in 0..6 {
let px = test_signal(&mut rng, length, kind);
let mut win = vec![0x1234i16; length];
let mut win_ref = vec![0x1234i16; length];
silk_apply_sine_window(&mut win, &px, win_type, length);
apply_sine_window_ref(&mut win_ref, &px, win_type, length);
assert_eq!(
win, win_ref,
"length={length} win_type={win_type} kind={kind}"
);
}
}
}
}
fn autocorr_ref(results: &mut [i32], scale: &mut i32, input: &[i16], n: usize, count: usize) {
let ilog = |x: u32| 32 - x.leading_zeros() as i32;
let ac0_shift = ilog((n + (n >> 4)) as u32) - 1;
let mut ac0: i32 = 1 + ((n as i32) << 7);
for &sample in &input[..n] {
ac0 += (sample as i32 * sample as i32) >> ac0_shift;
}
ac0 += ac0 >> 7;
let mut shift = (ilog(ac0 as u32) - 1 - 30 + ac0_shift + 1) / 2;
let x: Vec<i16> = if shift > 0 {
input[..n]
.iter()
.map(|&sample| silk_rshift_round(sample as i32, shift) as i16)
.collect()
} else {
shift = 0;
input[..n].to_vec()
};
for k in 0..count {
let mut sum = 0i32;
for j in 0..n - k {
sum = sum.wrapping_add((x[j] as i32).wrapping_mul(x[j + k] as i32));
}
results[k] = sum;
}
shift *= 2;
if shift <= 0 {
results[0] += 1i32 << (-shift).min(30);
}
if results[0] > 0 && results[0] < 1 << 28 {
let shift2 = 29 - ilog(results[0] as u32);
results[..count].iter_mut().for_each(|v| *v <<= shift2);
shift -= shift2;
} else if results[0] >= 1 << 29 {
let shift2 = if results[0] >= 1 << 30 { 2 } else { 1 };
results[..count].iter_mut().for_each(|v| *v >>= shift2);
shift += shift2;
}
*scale = shift;
}
#[test]
fn autocorr_matches_reference() {
let mut rng = TestRng(0x7777_1234_8888_4321);
let sizes = [
0usize, 1, 2, 3, 4, 7, 8, 16, 17, 31, 100, 120, 192, 240, 384, 640,
];
for &n in &sizes {
for count in 1..=25.min(n + 1) {
for kind in 0..7 {
let mut input = if kind == 6 {
vec![0i16; n]
} else {
test_signal(&mut rng, n, kind)
};
input.extend_from_slice(&[i16::MAX; 5]);
let mut results = [0x0bad_beefi32; 26];
let mut results_ref = [0x0bad_beefi32; 26];
let (mut scale, mut scale_ref) = (0, 0);
silk_autocorr(&mut results, &mut scale, &input, n, count);
autocorr_ref(&mut results_ref, &mut scale_ref, &input, n, count);
assert_eq!(
(results, scale),
(results_ref, scale_ref),
"n={n} count={count} kind={kind}"
);
}
}
}
}
#[cfg(target_arch = "x86_64")]
#[test]
fn avx2_copies_match_portable() {
if !crate::compat::x86_has_avx2() {
return;
}
let mut rng = TestRng(0x6a09_e667_f3bc_c908);
let lengths: Vec<usize> = (0..=40)
.chain([80, 120, 160, 240, 640])
.chain((0..60).map(|_| rng.below(700) as usize))
.collect();
for (case, &n) in lengths.iter().enumerate() {
for kind in 0..6 {
let x = test_signal(&mut rng, n + 16, kind);
let t = test_signal(&mut rng, n, (kind + 1) % 6);
let count = (rng.below(25) as usize + 1).min(n + 1);
let mut want = [0x0bad_beefi32; 25];
let mut got = [0x0bad_beefi32; 25];
autocorr_sums_portable(&mut want[..count], &x[..n]);
unsafe { autocorr_sums_avx2(&mut got[..count], &x[..n]) };
assert_eq!(got, want, "autocorr_sums n={n} count={count} kind={kind}");
if n == 0 {
continue;
}
let order = 1 + (case + kind as usize) % 16;
let rshifts = rng.below(7) as i32;
let mut want = [0x0bad_beefi32; 16];
let mut got = [0x0bad_beefi32; 16];
corr_vector_portable(&x, &t, n, order, &mut want, rshifts);
unsafe { corr_vector_avx2(&x, &t, n, order, &mut got, rshifts) };
assert_eq!(got, want, "corr_vector n={n} order={order} rshifts={rshifts}");
let mut want = ([0x0bad_beefi32; 256], 0i32, 0i32);
let mut got = ([0x0bad_beefi32; 256], 0i32, 0i32);
corr_matrix_portable(&x, n, order, &mut want.0, &mut want.1, &mut want.2);
unsafe { corr_matrix_avx2(&x, n, order, &mut got.0, &mut got.1, &mut got.2) };
assert_eq!(got, want, "corr_matrix n={n} order={order} kind={kind}");
}
}
}
fn schur64_ref(rc_q16: &mut [i32], c: &[i32], order: usize) -> i32 {
let mut c_matrix = [[0i32; 2]; MAX_SHAPE_LPC_ORDER + 1];
if c[0] <= 0 {
rc_q16[..order].fill(0);
return 0;
}
for k in 0..=order {
c_matrix[k] = [c[k], c[k]];
}
let mut k = 0;
while k < order {
if c_matrix[k + 1][0].wrapping_abs() >= c_matrix[0][1] {
rc_q16[k] = if c_matrix[k + 1][0] > 0 {
-64880
} else {
64880
};
k += 1;
break;
}
let rc_tmp_q31 = silk_div32_varq(c_matrix[k + 1][0].wrapping_neg(), c_matrix[0][1], 31);
rc_q16[k] = silk_rshift_round(rc_tmp_q31, 15);
for n in 0..(order - k) {
let c_tmp1_q30 = c_matrix[n + k + 1][0];
let c_tmp2_q30 = c_matrix[n][1];
c_matrix[n + k + 1][0] =
c_tmp1_q30.wrapping_add(silk_smmul(c_tmp2_q30 << 1, rc_tmp_q31));
c_matrix[n][1] = c_tmp2_q30.wrapping_add(silk_smmul(c_tmp1_q30 << 1, rc_tmp_q31));
}
k += 1;
}
rc_q16[k..order].fill(0);
c_matrix[0][1].max(1)
}
#[test]
fn schur64_matches_reference() {
let mut rng = TestRng(0x9e37_79b9_7f4a_7c15);
let check = |c: &[i32], order: usize, what: &str| {
let mut rc = [0x1111_1111i32; MAX_SHAPE_LPC_ORDER];
let mut rc_ref = [0x1111_1111i32; MAX_SHAPE_LPC_ORDER];
let nrg = silk_schur64(&mut rc, c, order);
let nrg_ref = schur64_ref(&mut rc_ref, c, order);
assert_eq!(
(nrg, rc),
(nrg_ref, rc_ref),
"{what}: order={order} c={c:?}"
);
};
for order in 0..=MAX_SHAPE_LPC_ORDER {
for case in 0..60u32 {
let mut signal = test_signal(&mut rng, 240, case % 2);
if case % 3 == 0 {
for i in 1..signal.len() {
signal[i] = ((signal[i] as i32 + 7 * signal[i - 1] as i32) / 8) as i16;
}
}
let mut corr = [0i32; MAX_SHAPE_LPC_ORDER + 1];
let mut scale = 0;
silk_autocorr(&mut corr, &mut scale, &signal, 240, order + 1);
if case % 4 != 0 {
corr[0] = corr[0].wrapping_add(silk_smulwb(corr[0] >> 4, 31).max(1));
}
check(&corr, order, "autocorrelation");
let mut c = [0i32; MAX_SHAPE_LPC_ORDER + 1];
for v in c.iter_mut() {
*v = (rng.next_u32() as i32) >> rng.below(8);
}
c[0] = match case % 5 {
0 => i32::MAX,
1 => (rng.next_u32() >> 1) as i32,
2 => 1,
3 => 0,
_ => -((rng.next_u32() >> 1) as i32),
};
check(&c, order, "arbitrary");
if case % 7 == 0 {
c[1 + rng.below(MAX_SHAPE_LPC_ORDER as u32) as usize] = i32::MIN;
check(&c, order, "with i32::MIN");
}
}
}
}
fn sum_sqr_shift_ref(x: &[i16]) -> (i32, i32) {
let len = x.len();
let pass = |init: i32, shft: i32| {
let mut nrg = init;
let mut i = 0;
while i < len {
let a = (x[i] as i32 * x[i] as i32) as u32;
if i + 1 < len {
let b = (x[i + 1] as i32 * x[i + 1] as i32) as u32;
nrg = nrg.wrapping_add((a.wrapping_add(b) >> shft) as i32);
} else {
nrg = nrg.wrapping_add((a >> shft) as i32);
}
i += 2;
}
nrg
};
let shft = 31 - (len as i32).leading_zeros() as i32;
let first = if len == 0 { 0 } else { pass(len as i32, shft) };
let shft = (shft + 3 - first.leading_zeros() as i32).max(0);
(shft, pass(0, shft))
}
#[test]
fn sum_sqr_shift_matches_reference() {
let mut rng = TestRng(0x4d59_5df4_d0f3_3173);
for len in (0..=70usize).chain([100, 160, 320, 656]) {
for kind in 0..7 {
let mut x = if kind == 6 {
vec![0i16; len]
} else {
test_signal(&mut rng, len, kind)
};
let (shft, nrg) = sum_sqr_shift_ref(&x);
x.extend_from_slice(&[i16::MIN; 3]);
let (mut energy, mut shift) = (-1, -1);
silk_sum_sqr_shift(&mut energy, &mut shift, &x, len);
assert_eq!((energy, shift), (nrg, shft), "len={len} kind={kind}");
}
}
}
fn xcorr_one_lag_ref(x: &[i16], y: &[i16], len: usize, lag: usize) -> i32 {
let mut sum = 0i32;
for j in 0..len {
if lag + j < y.len() {
sum = sum.wrapping_add((x[j] as i32).wrapping_mul(y[lag + j] as i32));
}
}
sum
}
type XcorrKernelFn = fn(&[i16], &[i16], &mut [i32; 4], usize);
fn check_xcorr_kernel(name: &str, kernel: XcorrKernelFn) {
let mut rng = TestRng(0x0bad_cafe_1357_9bdf);
for len in 0..=70usize {
for kind in 0..6 {
let x = test_signal(&mut rng, len, kind);
let y = test_signal(&mut rng, len + 3, (kind + len as u32) % 6);
let start = [rng.next_u32() as i32, 0, i32::MAX, i32::MIN];
let mut sum = start;
kernel(&x, &y, &mut sum, len);
for k in 0..4 {
let expected = start[k].wrapping_add(xcorr_one_lag_ref(&x, &y, len, k));
assert_eq!(sum[k], expected, "{name}: len={len} kind={kind} lag={k}");
}
}
}
}
#[test]
fn xcorr_kernel_scalar_matches_reference() {
check_xcorr_kernel("scalar", |x, y, sum, len| {
xcorr_kernel_scalar(&x[..len], &y[..len + 3], sum)
});
}
#[test]
fn xcorr_kernel_matches_reference() {
check_xcorr_kernel("dispatch", xcorr_kernel);
}
#[test]
#[should_panic]
fn pitch_xcorr_short_x_panics_in_single_lag() {
let (x, y) = ([1i16; 15], [1i16; 16]);
let mut xcorr = [0i32; 1];
silk_pitch_xcorr(&x, &y, &mut xcorr, 16, 1);
}
#[test]
#[should_panic]
fn pitch_xcorr_short_x_panics_in_group() {
let (x, y) = ([1i16; 15], [1i16; 19]);
let mut xcorr = [0i32; 4];
silk_pitch_xcorr(&x, &y, &mut xcorr, 16, 4);
}
#[test]
fn pitch_xcorr_matches_reference() {
let mut rng = TestRng(0x2468_ace0_1122_3344);
let lens = [0usize, 1, 2, 7, 8, 9, 15, 16, 17, 31, 40, 41, 80, 147, 320];
for &len in &lens {
for max_pitch in 1..=21usize {
for kind in 0..6 {
let x = test_signal(&mut rng, len, kind);
let full = len + max_pitch - 1;
for y_len in [full, full.saturating_sub(1), full / 2] {
let y = test_signal(&mut rng, y_len, (kind + 1) % 6);
let mut xcorr = [0x5a5a_5a5ai32; 21];
silk_pitch_xcorr(&x, &y, &mut xcorr[..max_pitch], len, max_pitch);
for lag in 0..max_pitch {
assert_eq!(
xcorr[lag],
xcorr_one_lag_ref(&x, &y, len, lag),
"len={len} max_pitch={max_pitch} y_len={y_len} lag={lag}"
);
}
}
}
}
}
}
fn inner_prod_ref(a: &[i16], b: &[i16], len: usize) -> i32 {
let mut sum = 0i32;
for i in 0..len {
sum = sum.wrapping_add((a[i] as i32).wrapping_mul(b[i] as i32));
}
sum
}
#[test]
fn inner_prod_aligned_matches_reference() {
let mut rng = TestRng(0x1bad_f00d_5eed_0001);
for len in (0..=100usize).chain([160, 320, 641]) {
for kind in 0..6 {
let mut a = test_signal(&mut rng, len, kind);
let mut b = test_signal(&mut rng, len, (kind + len as u32) % 6);
let expected = inner_prod_ref(&a, &b, len);
assert_eq!(
silk_inner_prod_aligned_scalar(&a, &b, len),
expected,
"scalar: len={len} kind={kind}"
);
a.extend_from_slice(&[i16::MAX; 9]);
b.extend_from_slice(&[i16::MIN; 9]);
assert_eq!(
silk_inner_prod_aligned(&a, &b, len),
expected,
"dispatch: len={len} kind={kind}"
);
}
}
}
#[test]
#[should_panic]
fn inner_prod_aligned_short_first_slice_panics() {
silk_inner_prod_aligned(&[1i16; 39], &[1i16; 40], 40);
}
#[test]
#[should_panic]
fn inner_prod_aligned_short_second_slice_panics() {
silk_inner_prod_aligned(&[1i16; 40], &[1i16; 39], 40);
}
#[test]
fn biquad_alt_stride2_matches_scalar() {
let mut rng = TestRng(0x0123_4567_89ab_cdef);
for case in 0..600u32 {
let len = match case % 12 {
10 => 160,
11 => 481,
n => n as usize,
};
let fc_q19 = 1 + rng.below(80_000) as i64;
let r_q28 = (1i64 << 28) - 471 * fc_q19 / 2;
let r_q22 = r_q28 >> 6;
let fc2_q28 = (fc_q19 * fc_q19) >> 10;
let b = [r_q28 as i32, (-2 * r_q28) as i32, r_q28 as i32];
let a = [
((r_q22 * (fc2_q28 - (2i64 << 28))) >> 22) as i32,
((r_q22 * r_q22) >> 16) as i32,
];
let kind = case % 6;
let input = test_signal(&mut rng, 2 * len, kind);
let start: [i32; 4] = if case % 5 == 0 {
[0; 4]
} else {
core::array::from_fn(|_| (rng.next_u32() as i32) >> (6 + rng.below(8)))
};
let mut expected = input.clone();
let mut state_expected = start;
silk_biquad_alt_stride2_scalar(&mut expected, &b, &a, &mut state_expected);
let mut got = input.clone();
got.extend_from_slice(&[0x55aai16; 5]);
let mut b_long = b.to_vec();
b_long.push(i32::MIN);
let mut a_long = a.to_vec();
a_long.push(i32::MIN);
let mut state = [0x0bad_beefi32; 6];
state[..4].copy_from_slice(&start);
silk_biquad_alt_stride2(&mut got, &b_long, &a_long, &mut state, len);
assert_eq!(&got[..2 * len], &expected[..], "len={len} kind={kind}");
assert_eq!(&got[2 * len..], &[0x55aai16; 5], "len={len} kind={kind}");
assert_eq!(&state[..4], &state_expected, "len={len} kind={kind}");
assert_eq!(&state[4..], &[0x0bad_beefi32; 2], "len={len} kind={kind}");
}
}
#[test]
#[should_panic]
fn biquad_alt_stride2_short_input_panics() {
let mut samples = [1i16; 7];
let mut state = [0i32; 4];
silk_biquad_alt_stride2(&mut samples, &[1 << 28, -2 << 28, 1 << 28], &[0, 0], &mut state, 4);
}
#[test]
fn test_silk_lpc_analysis_filter_matches_reference() {
let mut rng = TestRng(0x1357_2468_aaaa_5555);
for d in (6usize..=24).step_by(2) {
let lens = (d..=d + 40).chain([160, 320, 672]);
for (case, len) in lens.enumerate() {
let kind = case as u32 % 6;
let input = test_signal(&mut rng, len, kind);
let b = test_signal(&mut rng, d, if case % 2 == 0 { 0 } else { 1 });
let mut out_opt = vec![0x55aai16; len + 3];
let mut out_ref = vec![0x55aai16; len + 3];
let mut input_long = input.clone();
input_long.extend_from_slice(&[i16::MAX; 3]);
let mut b_long = b.clone();
b_long.push(i16::MIN);
silk_lpc_analysis_filter(&mut out_opt, &input_long, &b_long, len, d, 0);
lpc_analysis_filter_ref(&mut out_ref, &input, &b, len, d);
assert_eq!(
out_opt, out_ref,
"mismatch for d={d}, len={len}, kind={kind}"
);
}
}
}
}