use core::arch::x86_64::*;
use super::{endian::load_endian_u32x8, *};
use crate::{ColorMatrix, row::scalar};
#[inline]
#[target_feature(enable = "avx2")]
unsafe fn unpack_v210_2words_avx2<const BE: bool>(ptr: *const u8) -> (__m256i, __m256i, __m256i) {
unsafe {
let words = load_endian_u32x8::<BE>(ptr);
let mask10 = _mm256_set1_epi32(0x3FF);
let low10 = _mm256_and_si256(words, mask10);
let mid10 = _mm256_and_si256(_mm256_srli_epi32::<10>(words), mask10);
let high10 = _mm256_and_si256(_mm256_srli_epi32::<20>(words), mask10);
let y_idx_mid = _mm256_setr_epi8(
0, 1, -1, -1, -1, -1, 8, 9, -1, -1, -1, -1, -1, -1, -1, -1, 0, 1, -1, -1, -1, -1, 8, 9, -1, -1, -1, -1, -1, -1, -1, -1, );
let y_idx_low = _mm256_setr_epi8(
-1, -1, 4, 5, -1, -1, -1, -1, 12, 13, -1, -1, -1, -1, -1, -1, -1, -1, 4, 5, -1, -1, -1, -1, 12, 13, -1, -1, -1, -1, -1, -1, );
let y_idx_high = _mm256_setr_epi8(
-1, -1, -1, -1, 4, 5, -1, -1, -1, -1, 12, 13, -1, -1, -1, -1, -1, -1, -1, -1, 4, 5, -1, -1, -1, -1, 12, 13, -1, -1, -1, -1, );
let y_from_mid = _mm256_shuffle_epi8(mid10, y_idx_mid);
let y_from_low = _mm256_shuffle_epi8(low10, y_idx_low);
let y_from_high = _mm256_shuffle_epi8(high10, y_idx_high);
let y_per_lane = _mm256_or_si256(_mm256_or_si256(y_from_mid, y_from_low), y_from_high);
let y_gather_idx = _mm256_setr_epi32(0, 1, 2, 4, 5, 6, 7, 7);
let y_vec = _mm256_permutevar8x32_epi32(y_per_lane, y_gather_idx);
let u_idx_low = _mm256_setr_epi8(
0, 1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1,
-1, -1, -1, -1, -1, -1, 0, 1, -1, -1, -1, -1, -1, -1, -1, -1,
);
let u_idx_mid = _mm256_setr_epi8(
-1, -1, 4, 5, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1,
-1, -1, -1, -1, -1, -1, -1, -1, 4, 5, -1, -1, -1, -1, -1, -1,
);
let u_idx_high = _mm256_setr_epi8(
-1, -1, -1, -1, 8, 9, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1,
-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, 8, 9, -1, -1, -1, -1,
);
let u_from_low = _mm256_shuffle_epi8(low10, u_idx_low);
let u_from_mid = _mm256_shuffle_epi8(mid10, u_idx_mid);
let u_from_high = _mm256_shuffle_epi8(high10, u_idx_high);
let u_per_lane = _mm256_or_si256(_mm256_or_si256(u_from_low, u_from_mid), u_from_high);
let u_lo128 = _mm256_castsi256_si128(u_per_lane);
let u_hi128 = _mm256_extracti128_si256::<1>(u_per_lane);
let u_low_combined = _mm_or_si128(u_lo128, u_hi128);
let u_vec = _mm256_castsi128_si256(u_low_combined);
let v_idx_high = _mm256_setr_epi8(
0, 1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1,
-1, -1, -1, -1, -1, -1, 0, 1, -1, -1, -1, -1, -1, -1, -1, -1,
);
let v_idx_low = _mm256_setr_epi8(
-1, -1, 8, 9, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1,
-1, -1, -1, -1, -1, -1, -1, -1, 8, 9, -1, -1, -1, -1, -1, -1,
);
let v_idx_mid = _mm256_setr_epi8(
-1, -1, -1, -1, 12, 13, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1,
-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, 12, 13, -1, -1, -1, -1,
);
let v_from_high = _mm256_shuffle_epi8(high10, v_idx_high);
let v_from_low = _mm256_shuffle_epi8(low10, v_idx_low);
let v_from_mid = _mm256_shuffle_epi8(mid10, v_idx_mid);
let v_per_lane = _mm256_or_si256(_mm256_or_si256(v_from_high, v_from_low), v_from_mid);
let v_lo128 = _mm256_castsi256_si128(v_per_lane);
let v_hi128 = _mm256_extracti128_si256::<1>(v_per_lane);
let v_low_combined = _mm_or_si128(v_lo128, v_hi128);
let v_vec = _mm256_castsi128_si256(v_low_combined);
(y_vec, u_vec, v_vec)
}
}
#[inline]
#[target_feature(enable = "avx2")]
pub(crate) unsafe fn v210_to_rgb_or_rgba_row<const ALPHA: bool, const BE: bool>(
packed: &[u8],
out: &mut [u8],
width: usize,
matrix: ColorMatrix,
full_range: bool,
) {
debug_assert!(width.is_multiple_of(2), "v210 requires even width");
let total_words = width.div_ceil(6);
let words = width / 6;
let bpp: usize = if ALPHA { 4 } else { 3 };
debug_assert!(packed.len() >= total_words * 16);
debug_assert!(out.len() >= width * bpp);
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 = _mm256_set1_epi32(RND);
let y_off_v = _mm256_set1_epi16(y_off as i16);
let y_scale_v = _mm256_set1_epi32(y_scale);
let c_scale_v = _mm256_set1_epi32(c_scale);
let bias_v = _mm256_set1_epi16(bias as i16);
let cru = _mm256_set1_epi32(coeffs.r_u());
let crv = _mm256_set1_epi32(coeffs.r_v());
let cgu = _mm256_set1_epi32(coeffs.g_u());
let cgv = _mm256_set1_epi32(coeffs.g_v());
let cbu = _mm256_set1_epi32(coeffs.b_u());
let cbv = _mm256_set1_epi32(coeffs.b_v());
let pairs = words / 2;
for p in 0..pairs {
let (y_vec, u_vec, v_vec) = unpack_v210_2words_avx2::<BE>(packed.as_ptr().add(p * 32));
let y_i16 = y_vec;
let u_i16 = _mm256_sub_epi16(u_vec, bias_v);
let v_i16 = _mm256_sub_epi16(v_vec, bias_v);
let u_lo_i32 = _mm256_cvtepi16_epi32(_mm256_castsi256_si128(u_i16));
let u_hi_i32 = _mm256_cvtepi16_epi32(_mm256_extracti128_si256::<1>(u_i16));
let v_lo_i32 = _mm256_cvtepi16_epi32(_mm256_castsi256_si128(v_i16));
let v_hi_i32 = _mm256_cvtepi16_epi32(_mm256_extracti128_si256::<1>(v_i16));
let u_d_lo = q15_shift(_mm256_add_epi32(
_mm256_mullo_epi32(u_lo_i32, c_scale_v),
rnd_v,
));
let u_d_hi = q15_shift(_mm256_add_epi32(
_mm256_mullo_epi32(u_hi_i32, c_scale_v),
rnd_v,
));
let v_d_lo = q15_shift(_mm256_add_epi32(
_mm256_mullo_epi32(v_lo_i32, c_scale_v),
rnd_v,
));
let v_d_hi = q15_shift(_mm256_add_epi32(
_mm256_mullo_epi32(v_hi_i32, c_scale_v),
rnd_v,
));
let r_chroma = chroma_i16x16(cru, crv, u_d_lo, v_d_lo, u_d_hi, v_d_hi, rnd_v);
let g_chroma = chroma_i16x16(cgu, cgv, u_d_lo, v_d_lo, u_d_hi, v_d_hi, rnd_v);
let b_chroma = chroma_i16x16(cbu, cbv, u_d_lo, v_d_lo, u_d_hi, v_d_hi, rnd_v);
let (r_dup_lo, _r_dup_hi) = chroma_dup(r_chroma);
let (g_dup_lo, _g_dup_hi) = chroma_dup(g_chroma);
let (b_dup_lo, _b_dup_hi) = chroma_dup(b_chroma);
let y_scaled = scale_y(y_i16, y_off_v, y_scale_v, rnd_v);
let r_sum = _mm256_adds_epi16(y_scaled, r_dup_lo);
let g_sum = _mm256_adds_epi16(y_scaled, g_dup_lo);
let b_sum = _mm256_adds_epi16(y_scaled, b_dup_lo);
let zero = _mm256_setzero_si256();
let r_u8 = narrow_u8x32(r_sum, zero);
let g_u8 = narrow_u8x32(g_sum, zero);
let b_u8 = narrow_u8x32(b_sum, zero);
let mut r_tmp = [0u8; 32];
let mut g_tmp = [0u8; 32];
let mut b_tmp = [0u8; 32];
_mm256_storeu_si256(r_tmp.as_mut_ptr().cast(), r_u8);
_mm256_storeu_si256(g_tmp.as_mut_ptr().cast(), g_u8);
_mm256_storeu_si256(b_tmp.as_mut_ptr().cast(), b_u8);
let base_px = p * 12;
if ALPHA {
let dst = &mut out[base_px * 4..base_px * 4 + 12 * 4];
for i in 0..12 {
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[base_px * 3..base_px * 3 + 12 * 3];
for i in 0..12 {
dst[i * 3] = r_tmp[i];
dst[i * 3 + 1] = g_tmp[i];
dst[i * 3 + 2] = b_tmp[i];
}
}
}
if pairs * 12 < width {
let tail_start_px = pairs * 12;
let tail_packed = &packed[pairs * 32..total_words * 16];
let tail_out = &mut out[tail_start_px * bpp..width * bpp];
let tail_w = width - tail_start_px;
scalar::v210_to_rgb_or_rgba_row::<ALPHA, BE>(
tail_packed,
tail_out,
tail_w,
matrix,
full_range,
);
}
}
}
#[inline]
#[target_feature(enable = "avx2")]
pub(crate) unsafe fn v210_to_rgb_u16_or_rgba_u16_row<const ALPHA: bool, const BE: bool>(
packed: &[u8],
out: &mut [u16],
width: usize,
matrix: ColorMatrix,
full_range: bool,
) {
debug_assert!(width.is_multiple_of(2), "v210 requires even width");
let total_words = width.div_ceil(6);
let words = width / 6;
let bpp: usize = if ALPHA { 4 } else { 3 };
debug_assert!(packed.len() >= total_words * 16);
debug_assert!(out.len() >= width * bpp);
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 = _mm256_set1_epi32(RND);
let y_off_v = _mm256_set1_epi16(y_off as i16);
let y_scale_v = _mm256_set1_epi32(y_scale);
let c_scale_v = _mm256_set1_epi32(c_scale);
let bias_v = _mm256_set1_epi16(bias as i16);
let max_v = _mm256_set1_epi16(out_max);
let zero_v = _mm256_set1_epi16(0);
let cru = _mm256_set1_epi32(coeffs.r_u());
let crv = _mm256_set1_epi32(coeffs.r_v());
let cgu = _mm256_set1_epi32(coeffs.g_u());
let cgv = _mm256_set1_epi32(coeffs.g_v());
let cbu = _mm256_set1_epi32(coeffs.b_u());
let cbv = _mm256_set1_epi32(coeffs.b_v());
let pairs = words / 2;
for p in 0..pairs {
let (y_vec, u_vec, v_vec) = unpack_v210_2words_avx2::<BE>(packed.as_ptr().add(p * 32));
let y_i16 = y_vec;
let u_i16 = _mm256_sub_epi16(u_vec, bias_v);
let v_i16 = _mm256_sub_epi16(v_vec, bias_v);
let u_lo_i32 = _mm256_cvtepi16_epi32(_mm256_castsi256_si128(u_i16));
let u_hi_i32 = _mm256_cvtepi16_epi32(_mm256_extracti128_si256::<1>(u_i16));
let v_lo_i32 = _mm256_cvtepi16_epi32(_mm256_castsi256_si128(v_i16));
let v_hi_i32 = _mm256_cvtepi16_epi32(_mm256_extracti128_si256::<1>(v_i16));
let u_d_lo = q15_shift(_mm256_add_epi32(
_mm256_mullo_epi32(u_lo_i32, c_scale_v),
rnd_v,
));
let u_d_hi = q15_shift(_mm256_add_epi32(
_mm256_mullo_epi32(u_hi_i32, c_scale_v),
rnd_v,
));
let v_d_lo = q15_shift(_mm256_add_epi32(
_mm256_mullo_epi32(v_lo_i32, c_scale_v),
rnd_v,
));
let v_d_hi = q15_shift(_mm256_add_epi32(
_mm256_mullo_epi32(v_hi_i32, c_scale_v),
rnd_v,
));
let r_chroma = chroma_i16x16(cru, crv, u_d_lo, v_d_lo, u_d_hi, v_d_hi, rnd_v);
let g_chroma = chroma_i16x16(cgu, cgv, u_d_lo, v_d_lo, u_d_hi, v_d_hi, rnd_v);
let b_chroma = chroma_i16x16(cbu, cbv, u_d_lo, v_d_lo, u_d_hi, v_d_hi, rnd_v);
let (r_dup_lo, _r_dup_hi) = chroma_dup(r_chroma);
let (g_dup_lo, _g_dup_hi) = chroma_dup(g_chroma);
let (b_dup_lo, _b_dup_hi) = chroma_dup(b_chroma);
let y_scaled = scale_y(y_i16, y_off_v, y_scale_v, rnd_v);
let r = clamp_u16_max_x16(_mm256_adds_epi16(y_scaled, r_dup_lo), zero_v, max_v);
let g = clamp_u16_max_x16(_mm256_adds_epi16(y_scaled, g_dup_lo), zero_v, max_v);
let b = clamp_u16_max_x16(_mm256_adds_epi16(y_scaled, b_dup_lo), zero_v, max_v);
let mut r_tmp = [0u16; 16];
let mut g_tmp = [0u16; 16];
let mut b_tmp = [0u16; 16];
_mm256_storeu_si256(r_tmp.as_mut_ptr().cast(), r);
_mm256_storeu_si256(g_tmp.as_mut_ptr().cast(), g);
_mm256_storeu_si256(b_tmp.as_mut_ptr().cast(), b);
let base_px = p * 12;
if ALPHA {
let dst = &mut out[base_px * 4..base_px * 4 + 12 * 4];
let alpha = out_max as u16;
for i in 0..12 {
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[base_px * 3..base_px * 3 + 12 * 3];
for i in 0..12 {
dst[i * 3] = r_tmp[i];
dst[i * 3 + 1] = g_tmp[i];
dst[i * 3 + 2] = b_tmp[i];
}
}
}
if pairs * 12 < width {
let tail_start_px = pairs * 12;
let tail_packed = &packed[pairs * 32..total_words * 16];
let tail_out = &mut out[tail_start_px * bpp..width * bpp];
let tail_w = width - tail_start_px;
scalar::v210_to_rgb_u16_or_rgba_u16_row::<ALPHA, BE>(
tail_packed,
tail_out,
tail_w,
matrix,
full_range,
);
}
}
}
#[inline]
#[target_feature(enable = "avx2")]
pub(crate) unsafe fn v210_to_luma_row<const BE: bool>(
packed: &[u8],
luma_out: &mut [u8],
width: usize,
) {
debug_assert!(width.is_multiple_of(2), "v210 requires even width");
let total_words = width.div_ceil(6);
let words = width / 6;
debug_assert!(packed.len() >= total_words * 16);
debug_assert!(luma_out.len() >= width);
unsafe {
let pairs = words / 2;
for p in 0..pairs {
let (y_vec, _, _) = unpack_v210_2words_avx2::<BE>(packed.as_ptr().add(p * 32));
let y_shr = _mm256_srli_epi16::<2>(y_vec);
let y_u8 = narrow_u8x32(y_shr, _mm256_setzero_si256());
let mut tmp = [0u8; 32];
_mm256_storeu_si256(tmp.as_mut_ptr().cast(), y_u8);
luma_out[p * 12..p * 12 + 12].copy_from_slice(&tmp[..12]);
}
if pairs * 12 < width {
let tail_start_px = pairs * 12;
let tail_packed = &packed[pairs * 32..total_words * 16];
let tail_out = &mut luma_out[tail_start_px..width];
let tail_w = width - tail_start_px;
scalar::v210_to_luma_row::<BE>(tail_packed, tail_out, tail_w);
}
}
}
#[inline]
#[target_feature(enable = "avx2")]
pub(crate) unsafe fn v210_to_luma_u16_row<const BE: bool>(
packed: &[u8],
luma_out: &mut [u16],
width: usize,
) {
debug_assert!(width.is_multiple_of(2), "v210 requires even width");
let total_words = width.div_ceil(6);
let words = width / 6;
debug_assert!(packed.len() >= total_words * 16);
debug_assert!(luma_out.len() >= width);
unsafe {
let pairs = words / 2;
for p in 0..pairs {
let (y_vec, _, _) = unpack_v210_2words_avx2::<BE>(packed.as_ptr().add(p * 32));
let mut tmp = [0u16; 16];
_mm256_storeu_si256(tmp.as_mut_ptr().cast(), y_vec);
luma_out[p * 12..p * 12 + 12].copy_from_slice(&tmp[..12]);
}
if pairs * 12 < width {
let tail_start_px = pairs * 12;
let tail_packed = &packed[pairs * 32..total_words * 16];
let tail_out = &mut luma_out[tail_start_px..width];
let tail_w = width - tail_start_px;
scalar::v210_to_luma_u16_row::<BE>(tail_packed, tail_out, tail_w);
}
}
}