use crate::dc_ops::{B_DC_Y_COMPENSATION, BdcCompensation};
#[cfg(target_arch = "x86")]
use std::arch::x86::*;
#[cfg(target_arch = "x86_64")]
use std::arch::x86_64::*;
#[derive(Clone, Copy)]
struct CompensationVectors {
negative_scale: __m128,
y_step: __m128,
weight: __m128,
}
#[inline]
#[target_feature(enable = "sse4.1")]
unsafe fn load_f32(ptr: *const f32) -> __m128 {
unsafe { _mm_loadu_ps(ptr) }
}
#[inline]
#[target_feature(enable = "sse4.1")]
unsafe fn load_i16(ptr: *const i16) -> __m128 {
let value = unsafe { _mm_loadl_epi64(ptr.cast()) };
_mm_cvtepi32_ps(_mm_cvtepi16_epi32(value))
}
#[inline]
#[target_feature(enable = "sse4.1")]
unsafe fn store_f32(ptr: *mut f32, value: __m128) {
unsafe { _mm_storeu_ps(ptr, value) };
}
#[inline]
#[target_feature(enable = "sse4.1")]
unsafe fn store_rounded_i16(ptr: *mut i16, value: __m128) {
let value = super::quant::round_ties_away_i32x4(value);
let packed = _mm_packs_epi32(value, value);
unsafe { _mm_storel_epi64(ptr.cast(), packed) };
}
#[inline]
#[target_feature(enable = "sse4.1")]
fn adjust_value(target: __m128, source_y: __m128, yq: __m128, p: CompensationVectors) -> __m128 {
let correction = _mm_mul_ps(source_y, p.negative_scale);
let error = _mm_add_ps(_mm_mul_ps(yq, p.y_step), correction);
_mm_add_ps(_mm_mul_ps(p.weight, error), target)
}
#[inline]
#[target_feature(enable = "sse4.1")]
unsafe fn compensate_block(
b: *const f32,
y: *const f32,
yq: *const i16,
out: *mut i16,
scale: __m128,
negative_cfl: __m128,
p: CompensationVectors,
) {
unsafe {
let b = load_f32(b);
let y = load_f32(y);
let yq = load_i16(yq);
let correction = _mm_mul_ps(yq, negative_cfl);
let target = _mm_add_ps(_mm_mul_ps(b, scale), correction);
store_rounded_i16(out, adjust_value(target, y, yq, p));
}
}
#[inline]
#[target_feature(enable = "sse4.1")]
unsafe fn adjust_block(t: *mut f32, y: *const f32, yq: *const i16, p: CompensationVectors) {
unsafe {
let target = load_f32(t);
let y = load_f32(y);
let yq = load_i16(yq);
store_f32(t, adjust_value(target, y, yq, p));
}
}
#[inline]
#[target_feature(enable = "sse4.1")]
unsafe fn reconstruct_block(
x: *const i16,
y: *const i16,
b: *const i16,
rx: *mut f32,
ry: *mut f32,
rb: *mut f32,
cfl: [__m128; 2],
steps: [__m128; 3],
) {
unsafe {
let x = load_i16(x);
let y = load_i16(y);
let b = load_i16(b);
store_f32(
rx,
_mm_mul_ps(_mm_add_ps(_mm_mul_ps(y, cfl[0]), x), steps[0]),
);
store_f32(ry, _mm_mul_ps(y, steps[1]));
store_f32(
rb,
_mm_mul_ps(_mm_add_ps(_mm_mul_ps(y, cfl[1]), b), steps[2]),
);
}
}
#[target_feature(enable = "sse4.1")]
pub(crate) fn compensate_b_dc_sse41(
source_b: &[f32],
source_y: &[f32],
y_quant: &[i16],
compensation: BdcCompensation,
cfl_b: f32,
output: &mut [i16],
) {
let n = output.len();
assert_eq!(source_b.len(), n);
assert_eq!(source_y.len(), n);
assert_eq!(y_quant.len(), n);
let scale = _mm_set1_ps(compensation.scale_b);
let negative_cfl = _mm_set1_ps(-cfl_b);
let p = CompensationVectors {
negative_scale: _mm_set1_ps(-compensation.scale_b),
y_step: _mm_set1_ps(compensation.y_step_b),
weight: _mm_set1_ps(B_DC_Y_COMPENSATION),
};
let mut i = 0;
unsafe {
while n - i >= 16 {
compensate_block(
source_b.as_ptr().add(i),
source_y.as_ptr().add(i),
y_quant.as_ptr().add(i),
output.as_mut_ptr().add(i),
scale,
negative_cfl,
p,
);
compensate_block(
source_b.as_ptr().add(i + 4),
source_y.as_ptr().add(i + 4),
y_quant.as_ptr().add(i + 4),
output.as_mut_ptr().add(i + 4),
scale,
negative_cfl,
p,
);
compensate_block(
source_b.as_ptr().add(i + 8),
source_y.as_ptr().add(i + 8),
y_quant.as_ptr().add(i + 8),
output.as_mut_ptr().add(i + 8),
scale,
negative_cfl,
p,
);
compensate_block(
source_b.as_ptr().add(i + 12),
source_y.as_ptr().add(i + 12),
y_quant.as_ptr().add(i + 12),
output.as_mut_ptr().add(i + 12),
scale,
negative_cfl,
p,
);
i += 16;
}
while n - i >= 4 {
compensate_block(
source_b.as_ptr().add(i),
source_y.as_ptr().add(i),
y_quant.as_ptr().add(i),
output.as_mut_ptr().add(i),
scale,
negative_cfl,
p,
);
i += 4;
}
}
if i < n {
let tail = n - i;
let mut b = [0.0; 4];
let mut y = [0.0; 4];
let mut yq = [0i16; 4];
let mut out = [0i16; 4];
b[..tail].copy_from_slice(&source_b[i..]);
y[..tail].copy_from_slice(&source_y[i..]);
yq[..tail].copy_from_slice(&y_quant[i..]);
unsafe {
compensate_block(
b.as_ptr(),
y.as_ptr(),
yq.as_ptr(),
out.as_mut_ptr(),
scale,
negative_cfl,
p,
);
}
output[i..].copy_from_slice(&out[..tail]);
}
}
#[target_feature(enable = "sse4.1")]
pub(crate) fn adjust_b_dc_sse41(
targets: &mut [f32],
source_y: &[f32],
y_quant: &[i16],
compensation: BdcCompensation,
) {
let n = targets.len();
assert_eq!(source_y.len(), n);
assert_eq!(y_quant.len(), n);
let p = CompensationVectors {
negative_scale: _mm_set1_ps(-compensation.scale_b),
y_step: _mm_set1_ps(compensation.y_step_b),
weight: _mm_set1_ps(B_DC_Y_COMPENSATION),
};
let mut i = 0;
unsafe {
while n - i >= 16 {
adjust_block(
targets.as_mut_ptr().add(i),
source_y.as_ptr().add(i),
y_quant.as_ptr().add(i),
p,
);
adjust_block(
targets.as_mut_ptr().add(i + 4),
source_y.as_ptr().add(i + 4),
y_quant.as_ptr().add(i + 4),
p,
);
adjust_block(
targets.as_mut_ptr().add(i + 8),
source_y.as_ptr().add(i + 8),
y_quant.as_ptr().add(i + 8),
p,
);
adjust_block(
targets.as_mut_ptr().add(i + 12),
source_y.as_ptr().add(i + 12),
y_quant.as_ptr().add(i + 12),
p,
);
i += 16;
}
while n - i >= 4 {
adjust_block(
targets.as_mut_ptr().add(i),
source_y.as_ptr().add(i),
y_quant.as_ptr().add(i),
p,
);
i += 4;
}
}
if i < n {
let tail = n - i;
let mut t = [0.0; 4];
let mut y = [0.0; 4];
let mut yq = [0i16; 4];
t[..tail].copy_from_slice(&targets[i..]);
y[..tail].copy_from_slice(&source_y[i..]);
yq[..tail].copy_from_slice(&y_quant[i..]);
unsafe {
adjust_block(t.as_mut_ptr(), y.as_ptr(), yq.as_ptr(), p);
}
targets[i..].copy_from_slice(&t[..tail]);
}
}
#[target_feature(enable = "sse4.1")]
pub(crate) fn reconstruct_dc_sse41(
input: [&[i16]; 3],
cfl: [f32; 2],
steps: [f32; 3],
output: [&mut [f32]; 3],
) {
let [x, y, b] = input;
let [rx, ry, rb] = output;
let n = y.len();
for len in [x.len(), b.len(), rx.len(), ry.len(), rb.len()] {
assert_eq!(len, n);
}
let cfl = cfl.map(|v| _mm_set1_ps(v));
let steps = steps.map(|v| _mm_set1_ps(v));
let mut i = 0;
unsafe {
while n - i >= 16 {
reconstruct_block(
x.as_ptr().add(i),
y.as_ptr().add(i),
b.as_ptr().add(i),
rx.as_mut_ptr().add(i),
ry.as_mut_ptr().add(i),
rb.as_mut_ptr().add(i),
cfl,
steps,
);
reconstruct_block(
x.as_ptr().add(i + 4),
y.as_ptr().add(i + 4),
b.as_ptr().add(i + 4),
rx.as_mut_ptr().add(i + 4),
ry.as_mut_ptr().add(i + 4),
rb.as_mut_ptr().add(i + 4),
cfl,
steps,
);
reconstruct_block(
x.as_ptr().add(i + 8),
y.as_ptr().add(i + 8),
b.as_ptr().add(i + 8),
rx.as_mut_ptr().add(i + 8),
ry.as_mut_ptr().add(i + 8),
rb.as_mut_ptr().add(i + 8),
cfl,
steps,
);
reconstruct_block(
x.as_ptr().add(i + 12),
y.as_ptr().add(i + 12),
b.as_ptr().add(i + 12),
rx.as_mut_ptr().add(i + 12),
ry.as_mut_ptr().add(i + 12),
rb.as_mut_ptr().add(i + 12),
cfl,
steps,
);
i += 16;
}
while n - i >= 4 {
reconstruct_block(
x.as_ptr().add(i),
y.as_ptr().add(i),
b.as_ptr().add(i),
rx.as_mut_ptr().add(i),
ry.as_mut_ptr().add(i),
rb.as_mut_ptr().add(i),
cfl,
steps,
);
i += 4;
}
}
if i < n {
let tail = n - i;
let mut tx = [0i16; 4];
let mut ty = [0i16; 4];
let mut tb = [0i16; 4];
let mut ox = [0.0; 4];
let mut oy = [0.0; 4];
let mut ob = [0.0; 4];
tx[..tail].copy_from_slice(&x[i..]);
ty[..tail].copy_from_slice(&y[i..]);
tb[..tail].copy_from_slice(&b[i..]);
unsafe {
reconstruct_block(
tx.as_ptr(),
ty.as_ptr(),
tb.as_ptr(),
ox.as_mut_ptr(),
oy.as_mut_ptr(),
ob.as_mut_ptr(),
cfl,
steps,
);
}
rx[i..].copy_from_slice(&ox[..tail]);
ry[i..].copy_from_slice(&oy[..tail]);
rb[i..].copy_from_slice(&ob[..tail]);
}
}