use std::arch::aarch64::*;
#[allow(clippy::too_many_arguments)]
#[target_feature(enable = "neon")]
pub(crate) fn sse_and_rate_neon<const BIASED: bool>(
coeff: &[f32],
inv_matrix: &[f32],
q_scaled: f32,
bias: crate::group::DequantBias,
width: usize,
height: usize,
half: usize,
cx: usize,
cy: usize,
_rate_log2_lut: &crate::inflated_cost::RateLog2Lut,
thr: &[f32; 4],
scan_pos: &[u32],
) -> (f32, usize, f32, u32) {
sse_and_rate_neon_impl::<BIASED, false>(
coeff,
inv_matrix,
q_scaled,
bias,
width,
height,
half,
cx,
cy,
_rate_log2_lut,
thr,
scan_pos,
&mut [],
)
}
#[allow(clippy::too_many_arguments)]
#[target_feature(enable = "neon")]
pub(crate) fn sse_and_rate_neon_impl<const BIASED: bool, const SAVE_LEVELS: bool>(
coeff: &[f32],
inv_matrix: &[f32],
q_scaled: f32,
bias: crate::group::DequantBias,
width: usize,
height: usize,
half: usize,
cx: usize,
cy: usize,
_rate_log2_lut: &crate::inflated_cost::RateLog2Lut,
thr: &[f32; 4],
scan_pos: &[u32],
levels: &mut [i32],
) -> (f32, usize, f32, u32) {
let n = width * height;
assert!(coeff.len() >= n && inv_matrix.len() >= n);
assert!(if SAVE_LEVELS {
levels.len() >= n
} else {
scan_pos.len() >= n
});
debug_assert!(width.is_multiple_of(4) && half.is_multiple_of(4));
let qs = vdupq_n_f32(q_scaled);
let mut sse_acc = vdupq_n_f32(0.0);
let mut nz_acc = vdupq_n_u32(0);
let mut mag_acc = vdupq_n_f32(0.0);
let mut scan_acc = vdupq_n_u32(0);
let zero = vdupq_n_f32(0.0);
let all_active = vdupq_n_u32(u32::MAX);
let lane_ids = unsafe { vld1q_u32([0u32, 1, 2, 3].as_ptr()) };
for (y, (coeffs, inv_matrix)) in coeff
.chunks_exact(width)
.zip(inv_matrix.chunks_exact(width))
.take(height)
.enumerate()
{
let yfix = if y >= height / 2 { 2 } else { 0 };
let thr_lo = thr[yfix];
let thr_hi = thr[yfix + 1];
for (x0, (coeff, inv_matrix)) in coeffs
.as_chunks::<4>()
.0
.iter()
.zip(inv_matrix.as_chunks::<4>().0.iter())
.enumerate()
{
let x = x0 * 4;
let threshold = if x >= half { thr_hi } else { thr_lo };
let thrv = vdupq_n_f32(threshold);
let cv = unsafe { vld1q_f32(coeff.as_ptr()) };
let mv = unsafe { vld1q_f32(inv_matrix.as_ptr()) };
let a = vmulq_f32(vmulq_f32(mv, qs), cv);
let absa = vabsq_f32(a);
let keep = vcgeq_f32(absa, thrv);
let rounded = vrndaq_f32(a);
let q = vreinterpretq_f32_u32(vandq_u32(vreinterpretq_u32_f32(rounded), keep));
let absq = vabsq_f32(q);
let nz = vcgtq_f32(absq, zero);
let d = if BIASED && vmaxvq_u32(nz) != 0 {
vsubq_f32(a, neon_dequantized_level_f32(q, bias))
} else if BIASED {
vsubq_f32(a, zero)
} else {
vsubq_f32(a, q)
};
let d2 = vmulq_f32(d, d);
let active = if y < cy && x < cx {
let lane_x = vaddq_u32(vdupq_n_u32(x as u32), lane_ids);
vcgeq_u32(lane_x, vdupq_n_u32(cx as u32))
} else {
all_active
};
let d2 = vbslq_f32(active, d2, zero);
sse_acc = vaddq_f32(sse_acc, d2);
let rate_mask = vandq_u32(nz, active);
nz_acc = vaddq_u32(nz_acc, vshrq_n_u32::<31>(rate_mask));
if SAVE_LEVELS {
let quantized = vandq_s32(
vcvtaq_s32_f32(a),
vreinterpretq_s32_u32(vandq_u32(keep, active)),
);
unsafe { vst1q_s32(levels.as_mut_ptr().add(y * width + x), quantized) };
}
if !SAVE_LEVELS && vmaxvq_u32(rate_mask) != 0 {
let ratev = neon_log2p1_f32(absq);
mag_acc = vaddq_f32(mag_acc, vbslq_f32(rate_mask, ratev, zero));
let sv = unsafe { vld1q_u32(scan_pos.as_ptr().add(y * width + x)) };
scan_acc = vmaxq_u32(scan_acc, vandq_u32(sv, rate_mask));
}
}
}
(
vaddvq_f32(sse_acc),
vaddvq_u32(nz_acc) as usize,
vaddvq_f32(mag_acc),
vmaxvq_u32(scan_acc),
)
}
#[inline]
#[target_feature(enable = "neon")]
pub(crate) fn neon_dequantized_level_f32(
q: float32x4_t,
bias: crate::group::DequantBias,
) -> float32x4_t {
let absq = vabsq_f32(q);
let big = vsubq_f32(q, vdivq_f32(vdupq_n_f32(bias.numerator), q));
let sign = vandq_u32(vreinterpretq_u32_f32(q), vdupq_n_u32(0x8000_0000));
let one = vreinterpretq_f32_u32(vorrq_u32(
sign,
vreinterpretq_u32_f32(vdupq_n_f32(bias.one)),
));
let use_big = vcgeq_f32(absq, vdupq_n_f32(1.125));
let dq = vbslq_f32(use_big, big, one);
let nz = vcgtq_f32(absq, vdupq_n_f32(0.0));
vreinterpretq_f32_u32(vandq_u32(vreinterpretq_u32_f32(dq), nz))
}
#[inline]
#[target_feature(enable = "neon")]
pub(crate) fn neon_log2p1_f32(x: float32x4_t) -> float32x4_t {
let y = vaddq_f32(x, vdupq_n_f32(1.0));
let bits = vreinterpretq_u32_f32(y);
let exp_u = vshrq_n_u32::<23>(bits);
let exp_i = vsubq_s32(vreinterpretq_s32_u32(exp_u), vdupq_n_s32(127));
let e = vcvtq_f32_s32(exp_i);
let mant_bits = vorrq_u32(
vandq_u32(bits, vdupq_n_u32(0x007f_ffff)),
vdupq_n_u32(0x3f80_0000),
);
let m = vreinterpretq_f32_u32(mant_bits);
let t = vsubq_f32(m, vdupq_n_f32(1.0));
let c0 = vdupq_n_f32(1.4426934719085693359375);
let c1 = vdupq_n_f32(-0.721179187297821044921875);
let c2 = vdupq_n_f32(0.477900087833404541015625);
let c3 = vdupq_n_f32(-0.340080082416534423828125);
let c4 = vdupq_n_f32(0.21719777584075927734375);
let c5 = vdupq_n_f32(-9.749893844127655029296875e-2);
let c6 = vdupq_n_f32(2.096841670572757720947265625e-2);
let mut p = c6;
p = vfmaq_f32(c5, t, p);
p = vfmaq_f32(c4, t, p);
p = vfmaq_f32(c3, t, p);
p = vfmaq_f32(c2, t, p);
p = vfmaq_f32(c1, t, p);
p = vfmaq_f32(c0, t, p);
vfmaq_f32(e, t, p)
}
#[cfg(test)]
mod tests {
use super::{neon_log2p1_f32, sse_and_rate_neon};
use std::arch::aarch64::{vdupq_n_f32, vgetq_lane_f32};
#[allow(clippy::too_many_arguments)]
fn reference(
coeff: &[f32],
inv: &[f32],
qs: f32,
bias: crate::group::DequantBias,
w: usize,
h: usize,
half: usize,
cx: usize,
cy: usize,
thr: &[f32; 4],
scan_pos: &[u32],
biased: bool,
) -> (f32, usize, f32, u32) {
let (mut sse, mut nzeros, mut mag_bits) = (0.0f32, 0usize, 0.0f32);
let mut max_scan = 0u32;
for y in 0..h {
let yfix = if y >= h / 2 { 2 } else { 0 };
for x in 0..w {
if x < cx && y < cy {
continue;
}
let i = y * w + x;
let threshold = if x >= half { thr[yfix + 1] } else { thr[yfix] };
let a = inv[i] * qs * coeff[i];
let q = if a.abs() >= threshold { a.round() } else { 0.0 };
let d = if biased { a - bias.level_f32(q) } else { a - q };
sse += d * d;
if q != 0.0 {
nzeros += 1;
mag_bits += (1.0 + q.abs()).log2();
max_scan = max_scan.max(scan_pos[i]);
}
}
}
(sse, nzeros, mag_bits, max_scan)
}
#[test]
fn test_sse_and_rate_neon_vs_reference() {
let mut state = 0x5e5e_a11d_0f00_d00du64;
let mut random = || {
state = state
.wrapping_mul(6364136223846793005)
.wrapping_add(1442695040888963407);
(state >> 32) as u32 as f32 / u32::MAX as f32
};
for &(w, h, half, cx, cy) in &[
(8usize, 8usize, 4usize, 1usize, 1usize),
(16, 8, 8, 2, 1),
(8, 16, 4, 1, 2),
(16, 16, 8, 2, 2),
(32, 32, 16, 4, 4),
] {
for case in 0..100 {
let n = w * h;
let coeff_scale = if case % 2 == 0 { 0.2 } else { 200.0 };
let coeff: Vec<f32> = (0..n).map(|_| (random() - 0.5) * coeff_scale).collect();
let inv: Vec<f32> = (0..n).map(|_| 0.001 + random() * 0.5).collect();
let qs = 0.5 + random() * 3.0;
let bias = crate::group::DequantBias::DEFAULT[case % 3];
let thr = [
random() * 0.6,
random() * 0.6,
random() * 0.6,
random() * 0.6,
];
let scan_pos = crate::coeff_order::scan_pos_lut(w, h);
for biased in [false, true] {
let expected = reference(
&coeff, &inv, qs, bias, w, h, half, cx, cy, &thr, scan_pos, biased,
);
let kernel = if biased {
sse_and_rate_neon::<true>
} else {
sse_and_rate_neon::<false>
};
let actual = unsafe {
kernel(
&coeff,
&inv,
qs,
bias,
w,
h,
half,
cx,
cy,
crate::inflated_cost::rate_log2_lut(),
&thr,
scan_pos,
)
};
assert_eq!(actual.1, expected.1, "nzeros mismatch for {w}x{h}");
assert_eq!(actual.3, expected.3, "max-scan mismatch for {w}x{h}");
let sse_rel = (actual.0 - expected.0).abs() / expected.0.abs().max(1.0);
let mag_rel = (actual.2 - expected.2).abs() / expected.2.abs().max(1.0);
assert!(
sse_rel < 1e-4,
"SSE relative error {sse_rel} for {w}x{h} biased={biased}"
);
assert!(
mag_rel < 1e-5,
"magnitude-rate relative error {mag_rel} for {w}x{h} biased={biased}"
);
}
}
}
}
#[test]
fn test_log2p1() {
unsafe {
for i in 0..5000 {
let q1 = ((i as f32 / 5000.) + 1.).log2();
let q2 = vgetq_lane_f32::<0>(neon_log2p1_f32(vdupq_n_f32(i as f32 / 5000.)));
assert!((q1 - q2).abs() < 1e-5, "q1 {} q2 {}", q1, q2);
}
}
}
}