use super::{endian, *};
use crate::{KernelMatrix, row::scalar};
#[inline]
#[target_feature(enable = "sse4.1")]
pub(crate) unsafe fn v410_to_rgb_or_rgba_row<const ALPHA: bool, const BE: bool>(
packed: &[u32],
out: &mut [u8],
width: usize,
matrix: KernelMatrix,
full_range: bool,
) {
debug_assert!(packed.len() >= width, "packed row too short");
let bpp: usize = if ALPHA { 4 } else { 3 };
debug_assert!(out.len() >= width * bpp, "out row too short");
let coeffs = scalar::Coefficients::for_matrix(matrix);
let (y_off, y_scale, c_scale) = scalar::range_params_n::<10, 8>(full_range);
let bias = scalar::chroma_bias::<10>();
const RND: i32 = 1 << 14;
unsafe {
let rnd_v = _mm_set1_epi32(RND);
let y_off_v = _mm_set1_epi16(y_off as i16);
let y_scale_v = _mm_set1_epi32(y_scale);
let c_scale_v = _mm_set1_epi32(c_scale);
let bias_v = _mm_set1_epi16(bias as i16);
let cru = _mm_set1_epi32(coeffs.r_u());
let crv = _mm_set1_epi32(coeffs.r_v());
let cgu = _mm_set1_epi32(coeffs.g_u());
let cgv = _mm_set1_epi32(coeffs.g_v());
let cbu = _mm_set1_epi32(coeffs.b_u());
let cbv = _mm_set1_epi32(coeffs.b_v());
let mask = _mm_set1_epi32(0x3FF);
let mut x = 0usize;
while x + 8 <= width {
let words_lo = endian::load_endian_u32x4::<BE>(packed.as_ptr().add(x) as *const u8);
let words_hi = endian::load_endian_u32x4::<BE>(packed.as_ptr().add(x + 4) as *const u8);
let u_lo_i32 = _mm_and_si128(words_lo, mask);
let y_lo_i32 = _mm_and_si128(_mm_srli_epi32::<10>(words_lo), mask);
let v_lo_i32 = _mm_and_si128(_mm_srli_epi32::<20>(words_lo), mask);
let u_hi_i32 = _mm_and_si128(words_hi, mask);
let y_hi_i32 = _mm_and_si128(_mm_srli_epi32::<10>(words_hi), mask);
let v_hi_i32 = _mm_and_si128(_mm_srli_epi32::<20>(words_hi), mask);
let u_i16 = _mm_packs_epi32(u_lo_i32, u_hi_i32);
let y_i16 = _mm_packs_epi32(y_lo_i32, y_hi_i32);
let v_i16 = _mm_packs_epi32(v_lo_i32, v_hi_i32);
let u_sub = _mm_sub_epi16(u_i16, bias_v);
let v_sub = _mm_sub_epi16(v_i16, bias_v);
let u_d_lo_i32 = _mm_cvtepi16_epi32(u_sub);
let u_d_hi_i32 = _mm_cvtepi16_epi32(_mm_srli_si128::<8>(u_sub));
let v_d_lo_i32 = _mm_cvtepi16_epi32(v_sub);
let v_d_hi_i32 = _mm_cvtepi16_epi32(_mm_srli_si128::<8>(v_sub));
let u_d_lo = q15_shift(_mm_add_epi32(_mm_mullo_epi32(u_d_lo_i32, c_scale_v), rnd_v));
let u_d_hi = q15_shift(_mm_add_epi32(_mm_mullo_epi32(u_d_hi_i32, c_scale_v), rnd_v));
let v_d_lo = q15_shift(_mm_add_epi32(_mm_mullo_epi32(v_d_lo_i32, c_scale_v), rnd_v));
let v_d_hi = q15_shift(_mm_add_epi32(_mm_mullo_epi32(v_d_hi_i32, c_scale_v), rnd_v));
let r_chroma = chroma_i16x8(cru, crv, u_d_lo, v_d_lo, u_d_hi, v_d_hi, rnd_v);
let g_chroma = chroma_i16x8(cgu, cgv, u_d_lo, v_d_lo, u_d_hi, v_d_hi, rnd_v);
let b_chroma = chroma_i16x8(cbu, cbv, u_d_lo, v_d_lo, u_d_hi, v_d_hi, rnd_v);
let y_scaled = scale_y(y_i16, y_off_v, y_scale_v, rnd_v);
let zero = _mm_setzero_si128();
let r_u8 = _mm_packus_epi16(_mm_adds_epi16(y_scaled, r_chroma), zero);
let g_u8 = _mm_packus_epi16(_mm_adds_epi16(y_scaled, g_chroma), zero);
let b_u8 = _mm_packus_epi16(_mm_adds_epi16(y_scaled, b_chroma), zero);
let mut r_tmp = [0u8; 16];
let mut g_tmp = [0u8; 16];
let mut b_tmp = [0u8; 16];
_mm_storeu_si128(r_tmp.as_mut_ptr().cast(), r_u8);
_mm_storeu_si128(g_tmp.as_mut_ptr().cast(), g_u8);
_mm_storeu_si128(b_tmp.as_mut_ptr().cast(), b_u8);
if ALPHA {
let dst = &mut out[x * 4..x * 4 + 8 * 4];
for i in 0..8 {
dst[i * 4] = r_tmp[i];
dst[i * 4 + 1] = g_tmp[i];
dst[i * 4 + 2] = b_tmp[i];
dst[i * 4 + 3] = 0xFF;
}
} else {
let dst = &mut out[x * 3..x * 3 + 8 * 3];
for i in 0..8 {
dst[i * 3] = r_tmp[i];
dst[i * 3 + 1] = g_tmp[i];
dst[i * 3 + 2] = b_tmp[i];
}
}
x += 8;
}
if x < width {
let tail_packed = &packed[x..width];
let tail_out = &mut out[x * bpp..width * bpp];
let tail_w = width - x;
scalar::v410_to_rgb_or_rgba_row::<ALPHA, BE>(
tail_packed,
tail_out,
tail_w,
matrix,
full_range,
);
}
}
}
const HSV_CHUNK: usize = 64;
#[inline]
unsafe fn v410_hsv_via_rgb_chunks(
h_out: &mut [u8],
s_out: &mut [u8],
v_out: &mut [u8],
width: usize,
mut fill_rgb: impl FnMut(usize, usize, &mut [u8]),
) {
let mut scratch = [0u8; HSV_CHUNK * 3];
let mut offset = 0;
while offset < width {
let n = (width - offset).min(HSV_CHUNK);
fill_rgb(offset, n, &mut scratch[..n * 3]);
unsafe {
rgb_to_hsv_row(
&scratch[..n * 3],
&mut h_out[offset..offset + n],
&mut s_out[offset..offset + n],
&mut v_out[offset..offset + n],
n,
);
}
offset += n;
}
}
#[inline]
#[target_feature(enable = "sse4.1")]
#[allow(clippy::too_many_arguments)]
pub(crate) unsafe fn v410_to_hsv_row<const BE: bool>(
packed: &[u32],
h_out: &mut [u8],
s_out: &mut [u8],
v_out: &mut [u8],
width: usize,
matrix: KernelMatrix,
full_range: bool,
) {
debug_assert!(packed.len() >= width, "packed row too short");
debug_assert!(h_out.len() >= width, "h_out row too short");
debug_assert!(s_out.len() >= width, "s_out row too short");
debug_assert!(v_out.len() >= width, "v_out row too short");
unsafe {
v410_hsv_via_rgb_chunks(h_out, s_out, v_out, width, |offset, n, rgb| {
v410_to_rgb_or_rgba_row::<false, BE>(&packed[offset..], rgb, n, matrix, full_range);
});
}
}
#[inline]
#[target_feature(enable = "sse4.1")]
pub(crate) unsafe fn v410_to_rgb_u16_or_rgba_u16_row<const ALPHA: bool, const BE: bool>(
packed: &[u32],
out: &mut [u16],
width: usize,
matrix: KernelMatrix,
full_range: bool,
) {
debug_assert!(packed.len() >= width, "packed row too short");
let bpp: usize = if ALPHA { 4 } else { 3 };
debug_assert!(out.len() >= width * bpp, "out row too short");
let coeffs = scalar::Coefficients::for_matrix(matrix);
let (y_off, y_scale, c_scale) = scalar::range_params_n::<10, 10>(full_range);
let bias = scalar::chroma_bias::<10>();
const RND: i32 = 1 << 14;
let out_max: i16 = ((1i32 << 10) - 1) as i16;
unsafe {
let rnd_v = _mm_set1_epi32(RND);
let y_off_v = _mm_set1_epi16(y_off as i16);
let y_scale_v = _mm_set1_epi32(y_scale);
let c_scale_v = _mm_set1_epi32(c_scale);
let bias_v = _mm_set1_epi16(bias as i16);
let max_v = _mm_set1_epi16(out_max);
let zero_v = _mm_set1_epi16(0);
let cru = _mm_set1_epi32(coeffs.r_u());
let crv = _mm_set1_epi32(coeffs.r_v());
let cgu = _mm_set1_epi32(coeffs.g_u());
let cgv = _mm_set1_epi32(coeffs.g_v());
let cbu = _mm_set1_epi32(coeffs.b_u());
let cbv = _mm_set1_epi32(coeffs.b_v());
let mask = _mm_set1_epi32(0x3FF);
let mut x = 0usize;
while x + 8 <= width {
let words_lo = endian::load_endian_u32x4::<BE>(packed.as_ptr().add(x) as *const u8);
let words_hi = endian::load_endian_u32x4::<BE>(packed.as_ptr().add(x + 4) as *const u8);
let u_lo_i32 = _mm_and_si128(words_lo, mask);
let y_lo_i32 = _mm_and_si128(_mm_srli_epi32::<10>(words_lo), mask);
let v_lo_i32 = _mm_and_si128(_mm_srli_epi32::<20>(words_lo), mask);
let u_hi_i32 = _mm_and_si128(words_hi, mask);
let y_hi_i32 = _mm_and_si128(_mm_srli_epi32::<10>(words_hi), mask);
let v_hi_i32 = _mm_and_si128(_mm_srli_epi32::<20>(words_hi), mask);
let u_i16 = _mm_packs_epi32(u_lo_i32, u_hi_i32);
let y_i16 = _mm_packs_epi32(y_lo_i32, y_hi_i32);
let v_i16 = _mm_packs_epi32(v_lo_i32, v_hi_i32);
let u_sub = _mm_sub_epi16(u_i16, bias_v);
let v_sub = _mm_sub_epi16(v_i16, bias_v);
let u_d_lo_i32 = _mm_cvtepi16_epi32(u_sub);
let u_d_hi_i32 = _mm_cvtepi16_epi32(_mm_srli_si128::<8>(u_sub));
let v_d_lo_i32 = _mm_cvtepi16_epi32(v_sub);
let v_d_hi_i32 = _mm_cvtepi16_epi32(_mm_srli_si128::<8>(v_sub));
let u_d_lo = q15_shift(_mm_add_epi32(_mm_mullo_epi32(u_d_lo_i32, c_scale_v), rnd_v));
let u_d_hi = q15_shift(_mm_add_epi32(_mm_mullo_epi32(u_d_hi_i32, c_scale_v), rnd_v));
let v_d_lo = q15_shift(_mm_add_epi32(_mm_mullo_epi32(v_d_lo_i32, c_scale_v), rnd_v));
let v_d_hi = q15_shift(_mm_add_epi32(_mm_mullo_epi32(v_d_hi_i32, c_scale_v), rnd_v));
let r_chroma = chroma_i16x8(cru, crv, u_d_lo, v_d_lo, u_d_hi, v_d_hi, rnd_v);
let g_chroma = chroma_i16x8(cgu, cgv, u_d_lo, v_d_lo, u_d_hi, v_d_hi, rnd_v);
let b_chroma = chroma_i16x8(cbu, cbv, u_d_lo, v_d_lo, u_d_hi, v_d_hi, rnd_v);
let y_scaled = scale_y(y_i16, y_off_v, y_scale_v, rnd_v);
let r = clamp_u16_max(_mm_adds_epi16(y_scaled, r_chroma), zero_v, max_v);
let g = clamp_u16_max(_mm_adds_epi16(y_scaled, g_chroma), zero_v, max_v);
let b = clamp_u16_max(_mm_adds_epi16(y_scaled, b_chroma), zero_v, max_v);
let mut r_tmp = [0u16; 8];
let mut g_tmp = [0u16; 8];
let mut b_tmp = [0u16; 8];
_mm_storeu_si128(r_tmp.as_mut_ptr().cast(), r);
_mm_storeu_si128(g_tmp.as_mut_ptr().cast(), g);
_mm_storeu_si128(b_tmp.as_mut_ptr().cast(), b);
if ALPHA {
let dst = &mut out[x * 4..x * 4 + 8 * 4];
let alpha = out_max as u16; for i in 0..8 {
dst[i * 4] = r_tmp[i];
dst[i * 4 + 1] = g_tmp[i];
dst[i * 4 + 2] = b_tmp[i];
dst[i * 4 + 3] = alpha;
}
} else {
let dst = &mut out[x * 3..x * 3 + 8 * 3];
for i in 0..8 {
dst[i * 3] = r_tmp[i];
dst[i * 3 + 1] = g_tmp[i];
dst[i * 3 + 2] = b_tmp[i];
}
}
x += 8;
}
if x < width {
let tail_packed = &packed[x..width];
let tail_out = &mut out[x * bpp..width * bpp];
let tail_w = width - x;
scalar::v410_to_rgb_u16_or_rgba_u16_row::<ALPHA, BE>(
tail_packed,
tail_out,
tail_w,
matrix,
full_range,
);
}
}
}
#[inline]
#[target_feature(enable = "sse4.1")]
pub(crate) unsafe fn v410_to_luma_row<const BE: bool>(
packed: &[u32],
out: &mut [u8],
width: usize,
) {
debug_assert!(packed.len() >= width);
debug_assert!(out.len() >= width);
unsafe {
let mask = _mm_set1_epi32(0x3FF);
let mut x = 0usize;
while x + 8 <= width {
let words_lo = endian::load_endian_u32x4::<BE>(packed.as_ptr().add(x) as *const u8);
let words_hi = endian::load_endian_u32x4::<BE>(packed.as_ptr().add(x + 4) as *const u8);
let y_lo_i32 = _mm_and_si128(_mm_srli_epi32::<10>(words_lo), mask);
let y_hi_i32 = _mm_and_si128(_mm_srli_epi32::<10>(words_hi), mask);
let y_i16 = _mm_packs_epi32(y_lo_i32, y_hi_i32);
let y_shr = _mm_srli_epi16::<2>(y_i16);
let y_u8 = _mm_packus_epi16(y_shr, _mm_setzero_si128());
let mut tmp = [0u8; 16];
_mm_storeu_si128(tmp.as_mut_ptr().cast(), y_u8);
out[x..x + 8].copy_from_slice(&tmp[..8]);
x += 8;
}
if x < width {
scalar::v410_to_luma_row::<BE>(&packed[x..width], &mut out[x..width], width - x);
}
}
}
#[inline]
#[target_feature(enable = "sse4.1")]
pub(crate) unsafe fn v410_to_luma_u16_row<const BE: bool>(
packed: &[u32],
out: &mut [u16],
width: usize,
) {
debug_assert!(packed.len() >= width);
debug_assert!(out.len() >= width);
unsafe {
let mask = _mm_set1_epi32(0x3FF);
let mut x = 0usize;
while x + 8 <= width {
let words_lo = endian::load_endian_u32x4::<BE>(packed.as_ptr().add(x) as *const u8);
let words_hi = endian::load_endian_u32x4::<BE>(packed.as_ptr().add(x + 4) as *const u8);
let y_lo_i32 = _mm_and_si128(_mm_srli_epi32::<10>(words_lo), mask);
let y_hi_i32 = _mm_and_si128(_mm_srli_epi32::<10>(words_hi), mask);
let y_i16 = _mm_packs_epi32(y_lo_i32, y_hi_i32);
_mm_storeu_si128(out.as_mut_ptr().add(x).cast(), y_i16);
x += 8;
}
if x < width {
scalar::v410_to_luma_u16_row::<BE>(&packed[x..width], &mut out[x..width], width - x);
}
}
}