#![allow(unsafe_op_in_unsafe_fn)]
#[target_feature(enable = "neon")]
pub(super) 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) {
use std::arch::aarch64::{
float32x4_t, vaddq_f32, vaddvq_f32, vdupq_n_f32, vfmaq_f32, vld1q_f32, vld2q_f32, vmulq_f32,
};
let p = amp_indices.len();
let q_ptr = q_rot.as_ptr();
let mut k = 0usize;
let mut amp_buf = [0.0f32; 8];
let mut cos_buf = [0.0f32; 8];
let mut sin_buf = [0.0f32; 8];
let mut dot0: float32x4_t = vdupq_n_f32(0.0);
let mut dot1: float32x4_t = vdupq_n_f32(0.0);
let mut norm0: float32x4_t = vdupq_n_f32(0.0);
let mut norm1: float32x4_t = vdupq_n_f32(0.0);
while 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_lo = vld1q_f32(amp_buf.as_ptr());
let amp_hi = vld1q_f32(amp_buf.as_ptr().add(4));
let cos_lo = vld1q_f32(cos_buf.as_ptr());
let cos_hi = vld1q_f32(cos_buf.as_ptr().add(4));
let sin_lo = vld1q_f32(sin_buf.as_ptr());
let sin_hi = vld1q_f32(sin_buf.as_ptr().add(4));
let q_lo = vld2q_f32(q_ptr.add(2 * k));
let q_hi = vld2q_f32(q_ptr.add(2 * k + 8));
let tmp_lo = vfmaq_f32(vmulq_f32(q_lo.0, cos_lo), q_lo.1, sin_lo);
let tmp_hi = vfmaq_f32(vmulq_f32(q_hi.0, cos_hi), q_hi.1, sin_hi);
dot0 = vfmaq_f32(dot0, tmp_lo, amp_lo);
dot1 = vfmaq_f32(dot1, tmp_hi, amp_hi);
norm0 = vfmaq_f32(norm0, amp_lo, amp_lo);
norm1 = vfmaq_f32(norm1, amp_hi, amp_hi);
k += 8;
}
let mut dot_acc = vaddq_f32(dot0, dot1);
let mut norm_acc = vaddq_f32(norm0, norm1);
while k + 4 <= p {
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);
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 amp4 = vld1q_f32(amp_buf.as_ptr());
let cos4 = vld1q_f32(cos_buf.as_ptr());
let sin4 = vld1q_f32(sin_buf.as_ptr());
let q = vld2q_f32(q_ptr.add(2 * k));
let tmp = vfmaq_f32(vmulq_f32(q.0, cos4), q.1, sin4);
dot_acc = vfmaq_f32(dot_acc, tmp, amp4);
norm_acc = vfmaq_f32(norm_acc, amp4, amp4);
k += 4;
}
let mut dot = vaddvq_f32(dot_acc);
let mut norm_sq = vaddvq_f32(norm_acc);
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)
}
#[cfg(any(test, feature = "bench"))]
#[target_feature(enable = "neon")]
#[allow(clippy::too_many_arguments)]
pub(super) unsafe fn symmetric_dot_pairs(
amp_indices_a: &[u32],
phase_indices_a: &[u32],
amp_indices_b: &[u32],
phase_indices_b: &[u32],
sigma_per_pair: &[f32],
amp_levels_unit: &[f32],
phase_cos_lut: &[f32],
phase_sin_lut: &[f32],
) -> (f32, f32, f32) {
use std::arch::aarch64::{
float32x4_t, vaddvq_f32, vdupq_n_f32, vfmaq_f32, vld1q_f32, vmulq_f32,
};
let p = amp_indices_a.len();
let mut dot_acc: float32x4_t = vdupq_n_f32(0.0);
let mut na_acc: float32x4_t = vdupq_n_f32(0.0);
let mut nb_acc: float32x4_t = vdupq_n_f32(0.0);
let mut amp_a_buf = [0.0f32; 4];
let mut amp_b_buf = [0.0f32; 4];
let mut cos_a_buf = [0.0f32; 4];
let mut sin_a_buf = [0.0f32; 4];
let mut cos_b_buf = [0.0f32; 4];
let mut sin_b_buf = [0.0f32; 4];
let mut k = 0usize;
while k + 4 <= p {
for j in 0..4 {
let ai_a = *amp_indices_a.get_unchecked(k + j) as usize;
let pi_a = *phase_indices_a.get_unchecked(k + j) as usize;
let ai_b = *amp_indices_b.get_unchecked(k + j) as usize;
let pi_b = *phase_indices_b.get_unchecked(k + j) as usize;
let sigma = *sigma_per_pair.get_unchecked(k + j);
amp_a_buf[j] = sigma * *amp_levels_unit.get_unchecked(ai_a);
amp_b_buf[j] = sigma * *amp_levels_unit.get_unchecked(ai_b);
cos_a_buf[j] = *phase_cos_lut.get_unchecked(pi_a);
sin_a_buf[j] = *phase_sin_lut.get_unchecked(pi_a);
cos_b_buf[j] = *phase_cos_lut.get_unchecked(pi_b);
sin_b_buf[j] = *phase_sin_lut.get_unchecked(pi_b);
}
let amp_a4 = vld1q_f32(amp_a_buf.as_ptr());
let amp_b4 = vld1q_f32(amp_b_buf.as_ptr());
let cos_a4 = vld1q_f32(cos_a_buf.as_ptr());
let sin_a4 = vld1q_f32(sin_a_buf.as_ptr());
let cos_b4 = vld1q_f32(cos_b_buf.as_ptr());
let sin_b4 = vld1q_f32(sin_b_buf.as_ptr());
let cos_diff = vfmaq_f32(vmulq_f32(sin_a4, sin_b4), cos_a4, cos_b4);
let amp_prod = vmulq_f32(amp_a4, amp_b4);
dot_acc = vfmaq_f32(dot_acc, amp_prod, cos_diff);
na_acc = vfmaq_f32(na_acc, amp_a4, amp_a4);
nb_acc = vfmaq_f32(nb_acc, amp_b4, amp_b4);
k += 4;
}
let mut dot = vaddvq_f32(dot_acc);
let mut na_sq = vaddvq_f32(na_acc);
let mut nb_sq = vaddvq_f32(nb_acc);
while k < p {
let ai_a = amp_indices_a[k] as usize;
let pi_a = phase_indices_a[k] as usize;
let ai_b = amp_indices_b[k] as usize;
let pi_b = phase_indices_b[k] as usize;
let sigma = sigma_per_pair[k];
let amp_a = sigma * amp_levels_unit[ai_a];
let amp_b = sigma * amp_levels_unit[ai_b];
let cos_diff =
phase_cos_lut[pi_a] * phase_cos_lut[pi_b] + phase_sin_lut[pi_a] * phase_sin_lut[pi_b];
dot += amp_a * amp_b * cos_diff;
na_sq += amp_a * amp_a;
nb_sq += amp_b * amp_b;
k += 1;
}
(dot, na_sq, nb_sq)
}
#[target_feature(enable = "neon")]
unsafe fn fwht_stages_h2_plus(bp: *mut f32, n: usize) {
use std::arch::aarch64::{
vadd_f32, vaddq_f32, vcombine_f32, vget_high_f32, vget_low_f32, vld1q_f32, vst1q_f32,
vsub_f32, vsubq_f32,
};
let mut i = 0usize;
while i + 4 <= n {
let v = vld1q_f32(bp.add(i));
let lo = vget_low_f32(v);
let hi = vget_high_f32(v);
let new_lo = vadd_f32(lo, hi);
let new_hi = vsub_f32(lo, hi);
vst1q_f32(bp.add(i), vcombine_f32(new_lo, new_hi));
i += 4;
}
let mut h = 4usize;
while h < n {
let mut base = 0usize;
while base < n {
let mut j = 0usize;
while j < h {
let a = vld1q_f32(bp.add(base + j));
let b = vld1q_f32(bp.add(base + j + h));
vst1q_f32(bp.add(base + j), vaddq_f32(a, b));
vst1q_f32(bp.add(base + j + h), vsubq_f32(a, b));
j += 4;
}
base += 2 * h;
}
h *= 2;
}
}
#[target_feature(enable = "neon")]
pub(super) unsafe fn fwht_forward_fused(block: &mut [f32], signs: &[f32]) {
use std::arch::aarch64::{
vaddq_f32, vld1q_f32, vmulq_f32, vmulq_n_f32, vrev64q_f32, vst1q_f32,
};
let n = block.len();
debug_assert!(n.is_power_of_two() && n >= 4);
debug_assert_eq!(signs.len(), n);
let bp = block.as_mut_ptr();
let sp = signs.as_ptr();
let lane_signs = vld1q_f32([1.0_f32, -1.0, 1.0, -1.0].as_ptr());
let mut i = 0usize;
while i + 4 <= n {
let raw = vld1q_f32(bp.add(i));
let s = vld1q_f32(sp.add(i));
let v = vmulq_f32(raw, s);
let swap = vrev64q_f32(v);
let v_signed = vmulq_f32(v, lane_signs);
vst1q_f32(bp.add(i), vaddq_f32(v_signed, swap));
i += 4;
}
fwht_stages_h2_plus(bp, n);
let scale = (n as f32).sqrt().recip();
let mut i = 0usize;
while i + 4 <= n {
let v = vld1q_f32(bp.add(i));
vst1q_f32(bp.add(i), vmulq_n_f32(v, scale));
i += 4;
}
}
#[target_feature(enable = "neon")]
pub(super) unsafe fn fwht_inverse_fused(block: &mut [f32], signs: &[f32]) {
use std::arch::aarch64::{
vaddq_f32, vld1q_f32, vmulq_f32, vmulq_n_f32, vrev64q_f32, vst1q_f32,
};
let n = block.len();
debug_assert!(n.is_power_of_two() && n >= 4);
debug_assert_eq!(signs.len(), n);
let bp = block.as_mut_ptr();
let sp = signs.as_ptr();
let lane_signs = vld1q_f32([1.0_f32, -1.0, 1.0, -1.0].as_ptr());
let mut i = 0usize;
while i + 4 <= n {
let v = vld1q_f32(bp.add(i));
let swap = vrev64q_f32(v);
let v_signed = vmulq_f32(v, lane_signs);
vst1q_f32(bp.add(i), vaddq_f32(v_signed, swap));
i += 4;
}
fwht_stages_h2_plus(bp, n);
let scale = (n as f32).sqrt().recip();
let mut i = 0usize;
while i + 4 <= n {
let v = vld1q_f32(bp.add(i));
let s = vld1q_f32(sp.add(i));
let scaled = vmulq_n_f32(v, scale);
vst1q_f32(bp.add(i), vmulq_f32(scaled, s));
i += 4;
}
}
#[target_feature(enable = "neon")]
pub(super) unsafe fn pair_magnitudes(pairs: &[f32], mags: &mut [f32]) {
use std::arch::aarch64::{vfmaq_f32, vld2q_f32, vmulq_f32, vsqrtq_f32, vst1q_f32};
let p = mags.len();
let pp = pairs.as_ptr();
let mp = mags.as_mut_ptr();
let mut k = 0usize;
while k + 4 <= p {
let q = vld2q_f32(pp.add(2 * k));
let re = q.0;
let im = q.1;
let re_sq = vmulq_f32(re, re);
let sum = vfmaq_f32(re_sq, im, im);
let m = vsqrtq_f32(sum);
vst1q_f32(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;
}
}