#![allow(dead_code)]
#![allow(unsafe_op_in_unsafe_fn)]
#![allow(clippy::too_many_arguments)]
use core::arch::x86_64::*;
#[target_feature(enable = "avx,sse3")]
#[inline]
unsafe fn hsum_avx(v: __m256) -> f32 {
let lo = _mm256_castps256_ps128(v);
let hi = _mm256_extractf128_ps(v, 1);
let s = _mm_add_ps(lo, hi);
let s2 = _mm_hadd_ps(s, s);
let s3 = _mm_hadd_ps(s2, s2);
_mm_cvtss_f32(s3)
}
#[target_feature(enable = "sse3")]
#[inline]
unsafe fn hsum_sse(v: __m128) -> f32 {
let s = _mm_hadd_ps(v, v);
let s2 = _mm_hadd_ps(s, s);
_mm_cvtss_f32(s2)
}
#[target_feature(enable = "avx2,fma")]
pub(crate) unsafe fn asymmetric_dot_pairs(
q_rot: &[f32],
amp_indices: &[u32],
phase_indices: &[u32],
sigma_per_pair: &[f32],
amp_levels_unit: &[f32],
phase_cos_lut: &[f32],
phase_sin_lut: &[f32],
) -> (f32, f32) {
let p = amp_indices.len();
let q_ptr = q_rot.as_ptr();
let mut k = 0usize;
let mut amp_buf = [0.0f32; 16];
let mut cos_buf = [0.0f32; 16];
let mut sin_buf = [0.0f32; 16];
let mut dot_lo = _mm256_setzero_ps();
let mut dot_hi = _mm256_setzero_ps();
let mut norm_lo = _mm256_setzero_ps();
let mut norm_hi = _mm256_setzero_ps();
while k + 16 <= p {
for j in 0..16 {
let ai = *amp_indices.get_unchecked(k + j) as usize;
let pi = *phase_indices.get_unchecked(k + j) as usize;
let sigma = *sigma_per_pair.get_unchecked(k + j);
amp_buf[j] = sigma * *amp_levels_unit.get_unchecked(ai);
cos_buf[j] = *phase_cos_lut.get_unchecked(pi);
sin_buf[j] = *phase_sin_lut.get_unchecked(pi);
}
let amp_lo_v = _mm256_loadu_ps(amp_buf.as_ptr());
let amp_hi_v = _mm256_loadu_ps(amp_buf.as_ptr().add(8));
let cos_lo_v = _mm256_loadu_ps(cos_buf.as_ptr());
let cos_hi_v = _mm256_loadu_ps(cos_buf.as_ptr().add(8));
let sin_lo_v = _mm256_loadu_ps(sin_buf.as_ptr());
let sin_hi_v = _mm256_loadu_ps(sin_buf.as_ptr().add(8));
let r0 = _mm256_loadu_ps(q_ptr.add(2 * k));
let r1 = _mm256_loadu_ps(q_ptr.add(2 * k + 8));
let r2 = _mm256_loadu_ps(q_ptr.add(2 * k + 16));
let r3 = _mm256_loadu_ps(q_ptr.add(2 * k + 24));
let (q_re_lo, q_im_lo) = deinterleave_pairs(r0, r1);
let (q_re_hi, q_im_hi) = deinterleave_pairs(r2, r3);
let tmp_lo = _mm256_fmadd_ps(q_im_lo, sin_lo_v, _mm256_mul_ps(q_re_lo, cos_lo_v));
let tmp_hi = _mm256_fmadd_ps(q_im_hi, sin_hi_v, _mm256_mul_ps(q_re_hi, cos_hi_v));
dot_lo = _mm256_fmadd_ps(tmp_lo, amp_lo_v, dot_lo);
dot_hi = _mm256_fmadd_ps(tmp_hi, amp_hi_v, dot_hi);
norm_lo = _mm256_fmadd_ps(amp_lo_v, amp_lo_v, norm_lo);
norm_hi = _mm256_fmadd_ps(amp_hi_v, amp_hi_v, norm_hi);
k += 16;
}
let mut dot_acc = _mm256_add_ps(dot_lo, dot_hi);
let mut norm_acc = _mm256_add_ps(norm_lo, norm_hi);
if k + 8 <= p {
for j in 0..8 {
let ai = *amp_indices.get_unchecked(k + j) as usize;
let pi = *phase_indices.get_unchecked(k + j) as usize;
let sigma = *sigma_per_pair.get_unchecked(k + j);
amp_buf[j] = sigma * *amp_levels_unit.get_unchecked(ai);
cos_buf[j] = *phase_cos_lut.get_unchecked(pi);
sin_buf[j] = *phase_sin_lut.get_unchecked(pi);
}
let amp = _mm256_loadu_ps(amp_buf.as_ptr());
let cos = _mm256_loadu_ps(cos_buf.as_ptr());
let sin = _mm256_loadu_ps(sin_buf.as_ptr());
let r0 = _mm256_loadu_ps(q_ptr.add(2 * k));
let r1 = _mm256_loadu_ps(q_ptr.add(2 * k + 8));
let (q_re, q_im) = deinterleave_pairs(r0, r1);
let tmp = _mm256_fmadd_ps(q_im, sin, _mm256_mul_ps(q_re, cos));
dot_acc = _mm256_fmadd_ps(tmp, amp, dot_acc);
norm_acc = _mm256_fmadd_ps(amp, amp, norm_acc);
k += 8;
}
let mut dot = hsum_avx(dot_acc);
let mut norm_sq = hsum_avx(norm_acc);
if k + 4 <= p {
let mut a4 = [0.0f32; 4];
let mut c4 = [0.0f32; 4];
let mut s4 = [0.0f32; 4];
for j in 0..4 {
let ai = *amp_indices.get_unchecked(k + j) as usize;
let pi = *phase_indices.get_unchecked(k + j) as usize;
let sigma = *sigma_per_pair.get_unchecked(k + j);
a4[j] = sigma * *amp_levels_unit.get_unchecked(ai);
c4[j] = *phase_cos_lut.get_unchecked(pi);
s4[j] = *phase_sin_lut.get_unchecked(pi);
}
let amp = _mm_loadu_ps(a4.as_ptr());
let cos = _mm_loadu_ps(c4.as_ptr());
let sin = _mm_loadu_ps(s4.as_ptr());
let r0 = _mm_loadu_ps(q_ptr.add(2 * k));
let r1 = _mm_loadu_ps(q_ptr.add(2 * k + 4));
let q_re = _mm_shuffle_ps(r0, r1, 0x88);
let q_im = _mm_shuffle_ps(r0, r1, 0xDD);
let tmp = _mm_fmadd_ps(q_im, sin, _mm_mul_ps(q_re, cos));
let dot4 = _mm_fmadd_ps(tmp, amp, _mm_setzero_ps());
let norm4 = _mm_mul_ps(amp, amp);
dot += hsum_sse(dot4);
norm_sq += hsum_sse(norm4);
k += 4;
}
while k < p {
let ai = amp_indices[k] as usize;
let pi = phase_indices[k] as usize;
let amp = sigma_per_pair[k] * amp_levels_unit[ai];
let c = phase_cos_lut[pi];
let s = phase_sin_lut[pi];
let q_re = q_rot[2 * k];
let q_im = q_rot[2 * k + 1];
dot += q_re * amp * c + q_im * amp * s;
norm_sq += amp * amp;
k += 1;
}
(dot, norm_sq)
}
#[target_feature(enable = "avx2")]
#[inline]
unsafe fn deinterleave_pairs(r0: __m256, r1: __m256) -> (__m256, __m256) {
let re_un = _mm256_shuffle_ps(r0, r1, 0x88);
let im_un = _mm256_shuffle_ps(r0, r1, 0xDD);
let re = _mm256_castpd_ps(_mm256_permute4x64_pd(_mm256_castps_pd(re_un), 0xD8));
let im = _mm256_castpd_ps(_mm256_permute4x64_pd(_mm256_castps_pd(im_un), 0xD8));
(re, im)
}
#[target_feature(enable = "avx2,fma")]
unsafe fn fwht_stages_h2_plus(bp: *mut f32, n: usize) {
debug_assert!(n >= 8);
let sign_h2 = _mm256_setr_ps(1.0, 1.0, -1.0, -1.0, 1.0, 1.0, -1.0, -1.0);
let mut i = 0usize;
while i + 8 <= n {
let v = _mm256_loadu_ps(bp.add(i));
let swap = _mm256_permute_ps::<0x4E>(v);
let v_signed = _mm256_mul_ps(v, sign_h2);
_mm256_storeu_ps(bp.add(i), _mm256_add_ps(v_signed, swap));
i += 8;
}
let sign_h4 = _mm256_setr_ps(1.0, 1.0, 1.0, 1.0, -1.0, -1.0, -1.0, -1.0);
let mut base = 0usize;
while base + 8 <= n {
let v = _mm256_loadu_ps(bp.add(base));
let swap = _mm256_permute2f128_ps::<0x01>(v, v);
let v_signed = _mm256_mul_ps(v, sign_h4);
_mm256_storeu_ps(bp.add(base), _mm256_add_ps(v_signed, swap));
base += 8;
}
let mut h = 8usize;
while h < n {
let mut base = 0usize;
while base < n {
let mut j = 0usize;
while j < h {
let a = _mm256_loadu_ps(bp.add(base + j));
let b = _mm256_loadu_ps(bp.add(base + j + h));
_mm256_storeu_ps(bp.add(base + j), _mm256_add_ps(a, b));
_mm256_storeu_ps(bp.add(base + j + h), _mm256_sub_ps(a, b));
j += 8;
}
base += 2 * h;
}
h *= 2;
}
}
#[target_feature(enable = "avx2,fma")]
pub(super) unsafe fn fwht_forward_fused(block: &mut [f32], signs: &[f32]) {
let n = block.len();
debug_assert!(n.is_power_of_two() && n >= 8);
debug_assert_eq!(signs.len(), n);
let bp = block.as_mut_ptr();
let sp = signs.as_ptr();
let lane_signs = _mm256_setr_ps(1.0, -1.0, 1.0, -1.0, 1.0, -1.0, 1.0, -1.0);
let mut i = 0usize;
while i + 8 <= n {
let raw = _mm256_loadu_ps(bp.add(i));
let s = _mm256_loadu_ps(sp.add(i));
let v = _mm256_mul_ps(raw, s);
let swap = _mm256_shuffle_ps::<0xB1>(v, v);
let v_signed = _mm256_mul_ps(v, lane_signs);
_mm256_storeu_ps(bp.add(i), _mm256_add_ps(v_signed, swap));
i += 8;
}
fwht_stages_h2_plus(bp, n);
let scale = (n as f32).sqrt().recip();
let scale_v = _mm256_set1_ps(scale);
let mut i = 0usize;
while i + 8 <= n {
let v = _mm256_loadu_ps(bp.add(i));
_mm256_storeu_ps(bp.add(i), _mm256_mul_ps(v, scale_v));
i += 8;
}
}
#[target_feature(enable = "avx2,fma")]
pub(super) unsafe fn fwht_inverse_fused(block: &mut [f32], signs: &[f32]) {
let n = block.len();
debug_assert!(n.is_power_of_two() && n >= 8);
debug_assert_eq!(signs.len(), n);
let bp = block.as_mut_ptr();
let sp = signs.as_ptr();
let lane_signs = _mm256_setr_ps(1.0, -1.0, 1.0, -1.0, 1.0, -1.0, 1.0, -1.0);
let mut i = 0usize;
while i + 8 <= n {
let v = _mm256_loadu_ps(bp.add(i));
let swap = _mm256_shuffle_ps::<0xB1>(v, v);
let v_signed = _mm256_mul_ps(v, lane_signs);
_mm256_storeu_ps(bp.add(i), _mm256_add_ps(v_signed, swap));
i += 8;
}
fwht_stages_h2_plus(bp, n);
let scale = (n as f32).sqrt().recip();
let scale_v = _mm256_set1_ps(scale);
let mut i = 0usize;
while i + 8 <= n {
let v = _mm256_loadu_ps(bp.add(i));
let s = _mm256_loadu_ps(sp.add(i));
let scaled = _mm256_mul_ps(v, scale_v);
_mm256_storeu_ps(bp.add(i), _mm256_mul_ps(scaled, s));
i += 8;
}
}
#[target_feature(enable = "avx2,fma")]
pub(crate) unsafe fn pair_magnitudes(pairs: &[f32], mags: &mut [f32]) {
let p = mags.len();
let pp = pairs.as_ptr();
let mp = mags.as_mut_ptr();
let mut k = 0usize;
while k + 8 <= p {
let r0 = _mm256_loadu_ps(pp.add(2 * k));
let r1 = _mm256_loadu_ps(pp.add(2 * k + 8));
let (re, im) = deinterleave_pairs(r0, r1);
let re_sq = _mm256_mul_ps(re, re);
let sum = _mm256_fmadd_ps(im, im, re_sq);
let m = _mm256_sqrt_ps(sum);
_mm256_storeu_ps(mp.add(k), m);
k += 8;
}
if k + 4 <= p {
let r0 = _mm_loadu_ps(pp.add(2 * k));
let r1 = _mm_loadu_ps(pp.add(2 * k + 4));
let re = _mm_shuffle_ps::<0x88>(r0, r1);
let im = _mm_shuffle_ps::<0xDD>(r0, r1);
let re_sq = _mm_mul_ps(re, re);
let sum = _mm_fmadd_ps(im, im, re_sq);
let m = _mm_sqrt_ps(sum);
_mm_storeu_ps(mp.add(k), m);
k += 4;
}
while k < p {
let re = pairs[2 * k];
let im = pairs[2 * k + 1];
mags[k] = (re * re + im * im).sqrt();
k += 1;
}
}