FreeRDP
Loading...
Searching...
No Matches
prim_YUV_sse4.1.c
1
23#include <winpr/wtypes.h>
24#include <freerdp/config.h>
25
26#include <winpr/sysinfo.h>
27#include <winpr/crt.h>
28#include <freerdp/types.h>
29#include <freerdp/primitives.h>
30
31#include "prim_internal.h"
32#include "prim_avxsse.h"
33#include "prim_YUV.h"
34
35#if defined(SSE_AVX_INTRINSICS_ENABLED)
36#include <emmintrin.h>
37#include <tmmintrin.h>
38#include <smmintrin.h>
39
40static primitives_t* generic = nullptr;
41
42/****************************************************************************/
43/* sse41 YUV420 -> RGB conversion */
44/****************************************************************************/
45static inline __m128i* sse41_YUV444Pixel(__m128i* WINPR_RESTRICT dst, __m128i Yraw, __m128i Uraw,
46 __m128i Vraw, UINT8 pos)
47{
48 const __m128i mapY[] = { mm_set_epu32(0x80800380, 0x80800280, 0x80800180, 0x80800080),
49 mm_set_epu32(0x80800780, 0x80800680, 0x80800580, 0x80800480),
50 mm_set_epu32(0x80800B80, 0x80800A80, 0x80800980, 0x80800880),
51 mm_set_epu32(0x80800F80, 0x80800E80, 0x80800D80, 0x80800C80) };
52 const __m128i mapUV[] = { mm_set_epu32(0x80038002, 0x80018000, 0x80808080, 0x80808080),
53 mm_set_epu32(0x80078006, 0x80058004, 0x80808080, 0x80808080),
54 mm_set_epu32(0x800B800A, 0x80098008, 0x80808080, 0x80808080),
55 mm_set_epu32(0x800F800E, 0x800D800C, 0x80808080, 0x80808080) };
56 const __m128i mask[] = { mm_set_epu32(0x80038080, 0x80028080, 0x80018080, 0x80008080),
57 mm_set_epu32(0x80800380, 0x80800280, 0x80800180, 0x80800080),
58 mm_set_epu32(0x80808003, 0x80808002, 0x80808001, 0x80808000) };
59 const __m128i c128 = _mm_set1_epi16(128);
60 __m128i BGRX = _mm_and_si128(LOAD_SI128(dst),
61 mm_set_epu32(0xFF000000, 0xFF000000, 0xFF000000, 0xFF000000));
62 {
63 __m128i C;
64 __m128i D;
65 __m128i E;
66 /* Load Y values and expand to 32 bit */
67 {
68 C = _mm_shuffle_epi8(Yraw, mapY[pos]); /* Reorder and multiply by 256 */
69 }
70 /* Load U values and expand to 32 bit */
71 {
72 const __m128i U = _mm_shuffle_epi8(Uraw, mapUV[pos]); /* Reorder dcba */
73 D = _mm_sub_epi16(U, c128); /* D = U - 128 */
74 }
75 /* Load V values and expand to 32 bit */
76 {
77 const __m128i V = _mm_shuffle_epi8(Vraw, mapUV[pos]); /* Reorder dcba */
78 E = _mm_sub_epi16(V, c128); /* E = V - 128 */
79 }
80 /* Get the R value */
81 {
82 const __m128i c403 = _mm_set1_epi16(403);
83 const __m128i e403 =
84 _mm_unpackhi_epi16(_mm_mullo_epi16(E, c403), _mm_mulhi_epi16(E, c403));
85 const __m128i Rs = _mm_add_epi32(C, e403);
86 const __m128i R32 = _mm_srai_epi32(Rs, 8);
87 const __m128i R16 = _mm_packs_epi32(R32, _mm_setzero_si128());
88 const __m128i R = _mm_packus_epi16(R16, _mm_setzero_si128());
89 const __m128i packed = _mm_shuffle_epi8(R, mask[0]);
90 BGRX = _mm_or_si128(BGRX, packed);
91 }
92 /* Get the G value */
93 {
94 const __m128i c48 = _mm_set1_epi16(48);
95 const __m128i d48 =
96 _mm_unpackhi_epi16(_mm_mullo_epi16(D, c48), _mm_mulhi_epi16(D, c48));
97 const __m128i c120 = _mm_set1_epi16(120);
98 const __m128i e120 =
99 _mm_unpackhi_epi16(_mm_mullo_epi16(E, c120), _mm_mulhi_epi16(E, c120));
100 const __m128i de = _mm_add_epi32(d48, e120);
101 const __m128i Gs = _mm_sub_epi32(C, de);
102 const __m128i G32 = _mm_srai_epi32(Gs, 8);
103 const __m128i G16 = _mm_packs_epi32(G32, _mm_setzero_si128());
104 const __m128i G = _mm_packus_epi16(G16, _mm_setzero_si128());
105 const __m128i packed = _mm_shuffle_epi8(G, mask[1]);
106 BGRX = _mm_or_si128(BGRX, packed);
107 }
108 /* Get the B value */
109 {
110 const __m128i c475 = _mm_set1_epi16(475);
111 const __m128i d475 =
112 _mm_unpackhi_epi16(_mm_mullo_epi16(D, c475), _mm_mulhi_epi16(D, c475));
113 const __m128i Bs = _mm_add_epi32(C, d475);
114 const __m128i B32 = _mm_srai_epi32(Bs, 8);
115 const __m128i B16 = _mm_packs_epi32(B32, _mm_setzero_si128());
116 const __m128i B = _mm_packus_epi16(B16, _mm_setzero_si128());
117 const __m128i packed = _mm_shuffle_epi8(B, mask[2]);
118 BGRX = _mm_or_si128(BGRX, packed);
119 }
120 }
121 STORE_SI128(dst++, BGRX);
122 return dst;
123}
124
125static inline pstatus_t sse41_YUV420ToRGB_BGRX(const BYTE* WINPR_RESTRICT pSrc[],
126 const UINT32* WINPR_RESTRICT srcStep,
127 BYTE* WINPR_RESTRICT pDst, UINT32 dstStep,
128 const prim_size_t* WINPR_RESTRICT roi)
129{
130 const UINT32 nWidth = roi->width;
131 const UINT32 nHeight = roi->height;
132 const UINT32 pad = roi->width % 16;
133 const __m128i duplicate = _mm_set_epi8(7, 7, 6, 6, 5, 5, 4, 4, 3, 3, 2, 2, 1, 1, 0, 0);
134
135 for (size_t y = 0; y < nHeight; y++)
136 {
137 __m128i* dst = WINPR_PACKED_ALIGN_CAST(__m128i*, (pDst + dstStep * y));
138 const BYTE* YData = pSrc[0] + y * srcStep[0];
139 const BYTE* UData = pSrc[1] + (y / 2) * srcStep[1];
140 const BYTE* VData = pSrc[2] + (y / 2) * srcStep[2];
141
142 for (UINT32 x = 0; x < nWidth - pad; x += 16)
143 {
144 const __m128i Y = LOAD_SI128(YData);
145 const __m128i uRaw = LOAD_SI128(UData);
146 const __m128i vRaw = LOAD_SI128(VData);
147 const __m128i U = _mm_shuffle_epi8(uRaw, duplicate);
148 const __m128i V = _mm_shuffle_epi8(vRaw, duplicate);
149 YData += 16;
150 UData += 8;
151 VData += 8;
152 dst = sse41_YUV444Pixel(dst, Y, U, V, 0);
153 dst = sse41_YUV444Pixel(dst, Y, U, V, 1);
154 dst = sse41_YUV444Pixel(dst, Y, U, V, 2);
155 dst = sse41_YUV444Pixel(dst, Y, U, V, 3);
156 }
157
158 for (UINT32 x = 0; x < pad; x++)
159 {
160 const BYTE Y = *YData++;
161 const BYTE U = *UData;
162 const BYTE V = *VData;
163 dst = WINPR_PACKED_ALIGN_CAST(
164 __m128i*, writeYUVPixel((BYTE*)dst, PIXEL_FORMAT_BGRX32, Y, U, V, writePixelBGRX));
165
166 if (x % 2)
167 {
168 UData++;
169 VData++;
170 }
171 }
172 }
173
174 return PRIMITIVES_SUCCESS;
175}
176
177static pstatus_t sse41_YUV420ToRGB(const BYTE* WINPR_RESTRICT pSrc[3], const UINT32 srcStep[3],
178 BYTE* WINPR_RESTRICT pDst, UINT32 dstStep, UINT32 DstFormat,
179 const prim_size_t* WINPR_RESTRICT roi)
180{
181 switch (DstFormat)
182 {
183 case PIXEL_FORMAT_BGRX32:
184 case PIXEL_FORMAT_BGRA32:
185 return sse41_YUV420ToRGB_BGRX(pSrc, srcStep, pDst, dstStep, roi);
186
187 default:
188 return generic->YUV420ToRGB_8u_P3AC4R(pSrc, srcStep, pDst, dstStep, DstFormat, roi);
189 }
190}
191
192static inline void BGRX_fillRGB(size_t offset, BYTE* WINPR_RESTRICT pRGB[2],
193 const BYTE* WINPR_RESTRICT pY[2], const BYTE* WINPR_RESTRICT pU[2],
194 const BYTE* WINPR_RESTRICT pV[2], BOOL filter)
195{
196 WINPR_ASSERT(pRGB);
197 WINPR_ASSERT(pY);
198 WINPR_ASSERT(pU);
199 WINPR_ASSERT(pV);
200
201 const UINT32 DstFormat = PIXEL_FORMAT_BGRX32;
202 const UINT32 bpp = 4;
203
204 for (size_t i = 0; i < 2; i++)
205 {
206 for (size_t j = 0; j < 2; j++)
207 {
208 const BYTE Y = pY[i][offset + j];
209 BYTE U = pU[i][offset + j];
210 BYTE V = pV[i][offset + j];
211 if ((i == 0) && (j == 0) && filter)
212 {
213 const INT32 avgU =
214 4 * pU[0][offset] - pU[0][offset + 1] - pU[1][offset] - pU[1][offset + 1];
215 const INT32 avgV =
216 4 * pV[0][offset] - pV[0][offset + 1] - pV[1][offset] - pV[1][offset + 1];
217
218 U = CONDITIONAL_CLIP(avgU, pU[0][offset]);
219 V = CONDITIONAL_CLIP(avgV, pV[0][offset]);
220 }
221
222 writeYUVPixel(&pRGB[i][(j + offset) * bpp], DstFormat, Y, U, V, writePixelBGRX);
223 }
224 }
225}
226
227/* input are uint16_t vectors */
228static inline __m128i sse41_yuv2x_single(const __m128i Y, __m128i U, __m128i V, const short iMulU,
229 const short iMulV)
230{
231 const __m128i zero = _mm_set1_epi8(0);
232
233 __m128i Ylo = _mm_unpacklo_epi16(Y, zero);
234 __m128i Yhi = _mm_unpackhi_epi16(Y, zero);
235 if (iMulU != 0)
236 {
237 const __m128i addX = _mm_set1_epi16(128);
238 const __m128i D = _mm_sub_epi16(U, addX);
239 const __m128i mulU = _mm_set1_epi16(iMulU);
240 const __m128i mulDlo = _mm_mullo_epi16(D, mulU);
241 const __m128i mulDhi = _mm_mulhi_epi16(D, mulU);
242 const __m128i Dlo = _mm_unpacklo_epi16(mulDlo, mulDhi);
243 Ylo = _mm_add_epi32(Ylo, Dlo);
244
245 const __m128i Dhi = _mm_unpackhi_epi16(mulDlo, mulDhi);
246 Yhi = _mm_add_epi32(Yhi, Dhi);
247 }
248 if (iMulV != 0)
249 {
250 const __m128i addX = _mm_set1_epi16(128);
251 const __m128i E = _mm_sub_epi16(V, addX);
252 const __m128i mul = _mm_set1_epi16(iMulV);
253 const __m128i mulElo = _mm_mullo_epi16(E, mul);
254 const __m128i mulEhi = _mm_mulhi_epi16(E, mul);
255 const __m128i Elo = _mm_unpacklo_epi16(mulElo, mulEhi);
256 const __m128i esumlo = _mm_add_epi32(Ylo, Elo);
257
258 const __m128i Ehi = _mm_unpackhi_epi16(mulElo, mulEhi);
259 const __m128i esumhi = _mm_add_epi32(Yhi, Ehi);
260 Ylo = esumlo;
261 Yhi = esumhi;
262 }
263
264 const __m128i rYlo = _mm_srai_epi32(Ylo, 8);
265 const __m128i rYhi = _mm_srai_epi32(Yhi, 8);
266 const __m128i rY = _mm_packs_epi32(rYlo, rYhi);
267 return rY;
268}
269
270/* Input are uint8_t vectors */
271static inline __m128i sse41_yuv2x(const __m128i Y, __m128i U, __m128i V, const short iMulU,
272 const short iMulV)
273{
274 const __m128i zero = _mm_set1_epi8(0);
275
276 /* Ylo = Y * 256
277 * Ulo = uint8_t -> uint16_t
278 * Vlo = uint8_t -> uint16_t
279 */
280 const __m128i Ylo = _mm_unpacklo_epi8(zero, Y);
281 const __m128i Ulo = _mm_unpacklo_epi8(U, zero);
282 const __m128i Vlo = _mm_unpacklo_epi8(V, zero);
283 const __m128i preslo = sse41_yuv2x_single(Ylo, Ulo, Vlo, iMulU, iMulV);
284
285 const __m128i Yhi = _mm_unpackhi_epi8(zero, Y);
286 const __m128i Uhi = _mm_unpackhi_epi8(U, zero);
287 const __m128i Vhi = _mm_unpackhi_epi8(V, zero);
288 const __m128i preshi = sse41_yuv2x_single(Yhi, Uhi, Vhi, iMulU, iMulV);
289 const __m128i res = _mm_packus_epi16(preslo, preshi);
290
291 return res;
292}
293
294/* const INT32 r = ((256L * C(Y) + 0L * D(U) + 403L * E(V))) >> 8; */
295static inline __m128i sse41_yuv2r(const __m128i Y, __m128i U, __m128i V)
296{
297 return sse41_yuv2x(Y, U, V, 0, 403);
298}
299
300/* const INT32 g = ((256L * C(Y) - 48L * D(U) - 120L * E(V))) >> 8; */
301static inline __m128i sse41_yuv2g(const __m128i Y, __m128i U, __m128i V)
302{
303 return sse41_yuv2x(Y, U, V, -48, -120);
304}
305
306/* const INT32 b = ((256L * C(Y) + 475L * D(U) + 0L * E(V))) >> 8; */
307static inline __m128i sse41_yuv2b(const __m128i Y, __m128i U, __m128i V)
308{
309 return sse41_yuv2x(Y, U, V, 475, 0);
310}
311
312static inline void sse41_BGRX_fillRGB_pixel(BYTE* WINPR_RESTRICT pRGB, __m128i Y, __m128i U,
313 __m128i V)
314{
315 const __m128i zero = _mm_set1_epi8(0);
316 /* Y * 256 */
317 const __m128i r = sse41_yuv2r(Y, U, V);
318 const __m128i rx[2] = { _mm_unpackhi_epi8(r, zero), _mm_unpacklo_epi8(r, zero) };
319
320 const __m128i g = sse41_yuv2g(Y, U, V);
321 const __m128i b = sse41_yuv2b(Y, U, V);
322
323 const __m128i bg[2] = { _mm_unpackhi_epi8(b, g), _mm_unpacklo_epi8(b, g) };
324
325 const __m128i mask = mm_set_epu8(0x00, 0xFF, 0xFF, 0xFF, 0x00, 0xFF, 0xFF, 0xFF, 0x00, 0xFF,
326 0xFF, 0xFF, 0x00, 0xFF, 0xFF, 0xFF);
327
328 __m128i* rgb = WINPR_PACKED_ALIGN_CAST(__m128i*, pRGB);
329 const __m128i bgrx0 = _mm_unpacklo_epi16(bg[1], rx[1]);
330 _mm_maskmoveu_si128(bgrx0, mask, (char*)&rgb[0]);
331 const __m128i bgrx1 = _mm_unpackhi_epi16(bg[1], rx[1]);
332 _mm_maskmoveu_si128(bgrx1, mask, (char*)&rgb[1]);
333 const __m128i bgrx2 = _mm_unpacklo_epi16(bg[0], rx[0]);
334 _mm_maskmoveu_si128(bgrx2, mask, (char*)&rgb[2]);
335 const __m128i bgrx3 = _mm_unpackhi_epi16(bg[0], rx[0]);
336 _mm_maskmoveu_si128(bgrx3, mask, (char*)&rgb[3]);
337}
338
339static inline __m128i odd1sum(__m128i u1)
340{
341 const __m128i zero = _mm_set1_epi8(0);
342 const __m128i u1hi = _mm_unpackhi_epi8(u1, zero);
343 const __m128i u1lo = _mm_unpacklo_epi8(u1, zero);
344 return _mm_hadds_epi16(u1lo, u1hi);
345}
346
347static inline __m128i odd0sum(__m128i u0, __m128i u1sum)
348{
349 /* Mask out even bytes, extend uint8_t to uint16_t by filling in zero bytes,
350 * horizontally add the values */
351 const __m128i mask = mm_set_epu8(0x80, 0x0F, 0x80, 0x0D, 0x80, 0x0B, 0x80, 0x09, 0x80, 0x07,
352 0x80, 0x05, 0x80, 0x03, 0x80, 0x01);
353 const __m128i u0odd = _mm_shuffle_epi8(u0, mask);
354 return _mm_adds_epi16(u1sum, u0odd);
355}
356
357static inline __m128i calcavg(__m128i u0even, __m128i sum)
358{
359 const __m128i u4zero = _mm_slli_epi16(u0even, 2);
360 const __m128i uavg = _mm_sub_epi16(u4zero, sum);
361 const __m128i zero = _mm_set1_epi8(0);
362 const __m128i savg = _mm_packus_epi16(uavg, zero);
363 const __m128i smask = mm_set_epu8(0x80, 0x07, 0x80, 0x06, 0x80, 0x05, 0x80, 0x04, 0x80, 0x03,
364 0x80, 0x02, 0x80, 0x01, 0x80, 0x00);
365 return _mm_shuffle_epi8(savg, smask);
366}
367
368static inline __m128i diffmask(__m128i avg, __m128i u0even)
369{
370 /* Check for values >= 30 to apply the avg value to
371 * use int16 for calculations to avoid issues with signed 8bit integers
372 */
373 const __m128i diff = _mm_subs_epi16(u0even, avg);
374 const __m128i absdiff = _mm_abs_epi16(diff);
375 const __m128i val30 = _mm_set1_epi16(30);
376 return _mm_cmplt_epi16(absdiff, val30);
377}
378
379static inline void sse41_filter(__m128i pU[2])
380{
381 const __m128i u1sum = odd1sum(pU[1]);
382 const __m128i sum = odd0sum(pU[0], u1sum);
383
384 /* Mask out the odd bytes. We don´t need to do anything to make the uint8_t to uint16_t */
385 const __m128i emask = mm_set_epu8(0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0xff, 0x00, 0xff,
386 0x00, 0xff, 0x00, 0xff, 0x00, 0xff);
387 const __m128i u0even = _mm_and_si128(pU[0], emask);
388 const __m128i avg = calcavg(u0even, sum);
389 const __m128i umask = diffmask(avg, u0even);
390
391 const __m128i u0orig = _mm_and_si128(u0even, umask);
392 const __m128i u0avg = _mm_andnot_si128(umask, avg);
393 const __m128i evenresult = _mm_or_si128(u0orig, u0avg);
394 const __m128i omask = mm_set_epu8(0xFF, 0x00, 0xFF, 0x00, 0xFF, 0x00, 0xFF, 0x00, 0xFF, 0x00,
395 0xFF, 0x00, 0xFF, 0x00, 0xFF, 0x00);
396 const __m128i u0odd = _mm_and_si128(pU[0], omask);
397 const __m128i result = _mm_or_si128(evenresult, u0odd);
398 pU[0] = result;
399}
400
401static inline void sse41_BGRX_fillRGB(BYTE* WINPR_RESTRICT pRGB[2], const __m128i pY[2],
402 __m128i pU[2], __m128i pV[2])
403{
404 WINPR_ASSERT(pRGB);
405 WINPR_ASSERT(pY);
406 WINPR_ASSERT(pU);
407 WINPR_ASSERT(pV);
408
409 sse41_filter(pU);
410 sse41_filter(pV);
411
412 for (size_t i = 0; i < 2; i++)
413 {
414 sse41_BGRX_fillRGB_pixel(pRGB[i], pY[i], pU[i], pV[i]);
415 }
416}
417
418static inline pstatus_t sse41_YUV444ToRGB_8u_P3AC4R_BGRX_DOUBLE_ROW(
419 BYTE* WINPR_RESTRICT pDst[2], const BYTE* WINPR_RESTRICT YData[2],
420 const BYTE* WINPR_RESTRICT UData[2], const BYTE* WINPR_RESTRICT VData[2], UINT32 nWidth)
421{
422 const UINT32 pad = nWidth % 16;
423
424 size_t x = 0;
425 for (; x < nWidth - pad; x += 16)
426 {
427 const __m128i Y[] = { LOAD_SI128(&YData[0][x]), LOAD_SI128(&YData[1][x]) };
428 __m128i U[] = { LOAD_SI128(&UData[0][x]), LOAD_SI128(&UData[1][x]) };
429 __m128i V[] = { LOAD_SI128(&VData[0][x]), LOAD_SI128(&VData[1][x]) };
430
431 BYTE* dstp[] = { &pDst[0][x * 4], &pDst[1][x * 4] };
432 sse41_BGRX_fillRGB(dstp, Y, U, V);
433 }
434
435 for (; x < nWidth; x += 2)
436 {
437 BGRX_fillRGB(x, pDst, YData, UData, VData, TRUE);
438 }
439
440 return PRIMITIVES_SUCCESS;
441}
442
443static inline void BGRX_fillRGB_single(size_t offset, BYTE* WINPR_RESTRICT pRGB,
444 const BYTE* WINPR_RESTRICT pY, const BYTE* WINPR_RESTRICT pU,
445 const BYTE* WINPR_RESTRICT pV, WINPR_ATTR_UNUSED BOOL filter)
446{
447 WINPR_ASSERT(pRGB);
448 WINPR_ASSERT(pY);
449 WINPR_ASSERT(pU);
450 WINPR_ASSERT(pV);
451
452 const UINT32 bpp = 4;
453
454 for (size_t j = 0; j < 2; j++)
455 {
456 const BYTE Y = pY[offset + j];
457 BYTE U = pU[offset + j];
458 BYTE V = pV[offset + j];
459
460 writeYUVPixel(&pRGB[(j + offset) * bpp], PIXEL_FORMAT_BGRX32, Y, U, V, writePixelBGRX);
461 }
462}
463
464static inline pstatus_t sse41_YUV444ToRGB_8u_P3AC4R_BGRX_SINGLE_ROW(
465 BYTE* WINPR_RESTRICT pDst, const BYTE* WINPR_RESTRICT YData, const BYTE* WINPR_RESTRICT UData,
466 const BYTE* WINPR_RESTRICT VData, UINT32 nWidth)
467{
468 for (size_t x = 0; x < nWidth; x += 2)
469 {
470 BGRX_fillRGB_single(x, pDst, YData, UData, VData, TRUE);
471 }
472
473 return PRIMITIVES_SUCCESS;
474}
475
476static inline pstatus_t sse41_YUV444ToRGB_8u_P3AC4R_BGRX(const BYTE* WINPR_RESTRICT pSrc[],
477 const UINT32 srcStep[],
478 BYTE* WINPR_RESTRICT pDst, UINT32 dstStep,
479 const prim_size_t* WINPR_RESTRICT roi)
480{
481 const UINT32 nWidth = roi->width;
482 const UINT32 nHeight = roi->height;
483
484 size_t y = 0;
485 for (; y < nHeight - nHeight % 2; y += 2)
486 {
487 BYTE* dst[] = { (pDst + dstStep * y), (pDst + dstStep * (y + 1)) };
488 const BYTE* YData[] = { pSrc[0] + y * srcStep[0], pSrc[0] + (y + 1) * srcStep[0] };
489 const BYTE* UData[] = { pSrc[1] + y * srcStep[1], pSrc[1] + (y + 1) * srcStep[1] };
490 const BYTE* VData[] = { pSrc[2] + y * srcStep[2], pSrc[2] + (y + 1) * srcStep[2] };
491
492 const pstatus_t rc =
493 sse41_YUV444ToRGB_8u_P3AC4R_BGRX_DOUBLE_ROW(dst, YData, UData, VData, nWidth);
494 if (rc != PRIMITIVES_SUCCESS)
495 return rc;
496 }
497 for (; y < nHeight; y++)
498 {
499 BYTE* dst = (pDst + dstStep * y);
500 const BYTE* YData = pSrc[0] + y * srcStep[0];
501 const BYTE* UData = pSrc[1] + y * srcStep[1];
502 const BYTE* VData = pSrc[2] + y * srcStep[2];
503 const pstatus_t rc =
504 sse41_YUV444ToRGB_8u_P3AC4R_BGRX_SINGLE_ROW(dst, YData, UData, VData, nWidth);
505 if (rc != PRIMITIVES_SUCCESS)
506 return rc;
507 }
508
509 return PRIMITIVES_SUCCESS;
510}
511
512static pstatus_t sse41_YUV444ToRGB_8u_P3AC4R(const BYTE* WINPR_RESTRICT pSrc[],
513 const UINT32 srcStep[], BYTE* WINPR_RESTRICT pDst,
514 UINT32 dstStep, UINT32 DstFormat,
515 const prim_size_t* WINPR_RESTRICT roi)
516{
517 switch (DstFormat)
518 {
519 case PIXEL_FORMAT_BGRX32:
520 case PIXEL_FORMAT_BGRA32:
521 return sse41_YUV444ToRGB_8u_P3AC4R_BGRX(pSrc, srcStep, pDst, dstStep, roi);
522
523 default:
524 return generic->YUV444ToRGB_8u_P3AC4R(pSrc, srcStep, pDst, dstStep, DstFormat, roi);
525 }
526}
527
528static inline void sse41_I444ToRGB_BGRX_ROW(BYTE* WINPR_RESTRICT pDst,
529 const BYTE* WINPR_RESTRICT YData,
530 const BYTE* WINPR_RESTRICT UData,
531 const BYTE* WINPR_RESTRICT VData, UINT32 nWidth)
532{
533 const UINT32 pad = nWidth % 16;
534
535 size_t x = 0;
536 for (; x < nWidth - pad; x += 16)
537 {
538 const __m128i Y = LOAD_SI128(&YData[x]);
539 const __m128i U = LOAD_SI128(&UData[x]);
540 const __m128i V = LOAD_SI128(&VData[x]);
541
542 sse41_BGRX_fillRGB_pixel(&pDst[x * 4], Y, U, V);
543 }
544
545 for (; x < nWidth; x++)
546 {
547 writeYUVPixel(&pDst[x * 4], PIXEL_FORMAT_BGRX32, YData[x], UData[x], VData[x],
548 writePixelBGRX);
549 }
550}
551
552static pstatus_t sse41_I444ToRGB_8u(const BYTE* WINPR_RESTRICT pSrc[], const UINT32 srcStep[],
553 BYTE* WINPR_RESTRICT pDst, UINT32 dstStep, UINT32 DstFormat,
554 const prim_size_t* WINPR_RESTRICT roi)
555{
556 switch (DstFormat)
557 {
558 case PIXEL_FORMAT_BGRX32:
559 case PIXEL_FORMAT_BGRA32:
560 for (size_t y = 0; y < roi->height; y++)
561 {
562 BYTE* dst = pDst + dstStep * y;
563 const BYTE* YData = pSrc[0] + y * srcStep[0];
564 const BYTE* UData = pSrc[1] + y * srcStep[1];
565 const BYTE* VData = pSrc[2] + y * srcStep[2];
566
567 sse41_I444ToRGB_BGRX_ROW(dst, YData, UData, VData, roi->width);
568 }
569 return PRIMITIVES_SUCCESS;
570
571 default:
572 return generic->I444ToRGB_8u(pSrc, srcStep, pDst, dstStep, DstFormat, roi);
573 }
574}
575
576/****************************************************************************/
577/* sse41 RGB -> YUV420 conversion **/
578/****************************************************************************/
579
601#define BGRX_Y_FACTORS _mm_set_epi8(0, 27, 92, 9, 0, 27, 92, 9, 0, 27, 92, 9, 0, 27, 92, 9)
602#define BGRX_U_FACTORS \
603 _mm_set_epi8(0, -29, -99, 127, 0, -29, -99, 127, 0, -29, -99, 127, 0, -29, -99, 127)
604#define BGRX_V_FACTORS \
605 _mm_set_epi8(0, 127, -116, -12, 0, 127, -116, -12, 0, 127, -116, -12, 0, 127, -116, -12)
606#define CONST128_FACTORS _mm_set1_epi8(-128)
607
608#define Y_SHIFT 7
609#define U_SHIFT 8
610#define V_SHIFT 8
611
612/*
613TODO:
614RGB[AX] can simply be supported using the following factors. And instead of loading the
615globals directly the functions below could be passed pointers to the correct vectors
616depending on the source picture format.
617
618PRIM_ALIGN_128 static const BYTE rgbx_y_factors[] = {
619 27, 92, 9, 0, 27, 92, 9, 0, 27, 92, 9, 0, 27, 92, 9, 0
620};
621PRIM_ALIGN_128 static const BYTE rgbx_u_factors[] = {
622 -15, -49, 64, 0, -15, -49, 64, 0, -15, -49, 64, 0, -15, -49, 64, 0
623};
624PRIM_ALIGN_128 static const BYTE rgbx_v_factors[] = {
625 64, -58, -6, 0, 64, -58, -6, 0, 64, -58, -6, 0, 64, -58, -6, 0
626};
627*/
628
629static inline void sse41_BGRX_TO_YUV(const BYTE* WINPR_RESTRICT pLine1, BYTE* WINPR_RESTRICT pYLine,
630 BYTE* WINPR_RESTRICT pULine, BYTE* WINPR_RESTRICT pVLine)
631{
632 const BYTE r1 = pLine1[2];
633 const BYTE g1 = pLine1[1];
634 const BYTE b1 = pLine1[0];
635
636 if (pYLine)
637 pYLine[0] = RGB2Y(r1, g1, b1);
638 if (pULine)
639 pULine[0] = RGB2U(r1, g1, b1);
640 if (pVLine)
641 pVLine[0] = RGB2V(r1, g1, b1);
642}
643
644/* compute the luma (Y) component from a single rgb source line */
645
646static inline void sse41_RGBToYUV420_BGRX_Y(const BYTE* WINPR_RESTRICT src, BYTE* dst, UINT32 width)
647{
648 const __m128i y_factors = BGRX_Y_FACTORS;
649 const __m128i* argb = WINPR_PACKED_ALIGN_CAST(const __m128i*, src);
650 __m128i* ydst = WINPR_PACKED_ALIGN_CAST(__m128i*, dst);
651
652 UINT32 x = 0;
653
654 for (; x < width - width % 16; x += 16)
655 {
656 /* store 16 rgba pixels in 4 128 bit registers */
657 __m128i x0 = LOAD_SI128(argb++); // 1st 4 pixels
658 {
659 x0 = _mm_maddubs_epi16(x0, y_factors);
660
661 __m128i x1 = LOAD_SI128(argb++); // 2nd 4 pixels
662 x1 = _mm_maddubs_epi16(x1, y_factors);
663 x0 = _mm_hadds_epi16(x0, x1);
664 x0 = _mm_srli_epi16(x0, Y_SHIFT);
665 }
666
667 __m128i x2 = LOAD_SI128(argb++); // 3rd 4 pixels
668 {
669 x2 = _mm_maddubs_epi16(x2, y_factors);
670
671 __m128i x3 = LOAD_SI128(argb++); // 4th 4 pixels
672 x3 = _mm_maddubs_epi16(x3, y_factors);
673 x2 = _mm_hadds_epi16(x2, x3);
674 x2 = _mm_srli_epi16(x2, Y_SHIFT);
675 }
676
677 x0 = _mm_packus_epi16(x0, x2);
678 /* save to y plane */
679 STORE_SI128(ydst++, x0);
680 }
681
682 for (; x < width; x++)
683 {
684 sse41_BGRX_TO_YUV(&src[4ULL * x], &dst[x], nullptr, nullptr);
685 }
686}
687
688/* compute the chrominance (UV) components from two rgb source lines */
689
690static inline void sse41_RGBToYUV420_BGRX_UV(const BYTE* WINPR_RESTRICT src1,
691 const BYTE* WINPR_RESTRICT src2,
692 BYTE* WINPR_RESTRICT dst1, BYTE* WINPR_RESTRICT dst2,
693 UINT32 width)
694{
695 const __m128i u_factors = BGRX_U_FACTORS;
696 const __m128i v_factors = BGRX_V_FACTORS;
697 const __m128i vector128 = CONST128_FACTORS;
698
699 size_t x = 0;
700
701 for (; x < width - width % 16; x += 16)
702 {
703 const __m128i* rgb1 = WINPR_PACKED_ALIGN_CAST(const __m128i*, &src1[4ULL * x]);
704 const __m128i* rgb2 = WINPR_PACKED_ALIGN_CAST(const __m128i*, &src2[4ULL * x]);
705 __m64* udst = WINPR_PACKED_ALIGN_CAST(__m64*, &dst1[x / 2]);
706 __m64* vdst = WINPR_PACKED_ALIGN_CAST(__m64*, &dst2[x / 2]);
707
708 /* subsample 16x2 pixels into 16x1 pixels */
709 __m128i x0 = LOAD_SI128(&rgb1[0]);
710 __m128i x4 = LOAD_SI128(&rgb2[0]);
711 x0 = _mm_avg_epu8(x0, x4);
712
713 __m128i x1 = LOAD_SI128(&rgb1[1]);
714 x4 = LOAD_SI128(&rgb2[1]);
715 x1 = _mm_avg_epu8(x1, x4);
716
717 __m128i x2 = LOAD_SI128(&rgb1[2]);
718 x4 = LOAD_SI128(&rgb2[2]);
719 x2 = _mm_avg_epu8(x2, x4);
720
721 __m128i x3 = LOAD_SI128(&rgb1[3]);
722 x4 = LOAD_SI128(&rgb2[3]);
723 x3 = _mm_avg_epu8(x3, x4);
724
725 /* subsample these 16x1 pixels into 8x1 pixels */
731 x4 = _mm_castps_si128(_mm_shuffle_ps(_mm_castsi128_ps(x0), _mm_castsi128_ps(x1), 0x88));
732 x0 = _mm_castps_si128(_mm_shuffle_ps(_mm_castsi128_ps(x0), _mm_castsi128_ps(x1), 0xdd));
733 x0 = _mm_avg_epu8(x0, x4);
734 x4 = _mm_castps_si128(_mm_shuffle_ps(_mm_castsi128_ps(x2), _mm_castsi128_ps(x3), 0x88));
735 x1 = _mm_castps_si128(_mm_shuffle_ps(_mm_castsi128_ps(x2), _mm_castsi128_ps(x3), 0xdd));
736 x1 = _mm_avg_epu8(x1, x4);
737 /* multiplications and subtotals */
738 x2 = _mm_maddubs_epi16(x0, u_factors);
739 x3 = _mm_maddubs_epi16(x1, u_factors);
740 x4 = _mm_maddubs_epi16(x0, v_factors);
741 __m128i x5 = _mm_maddubs_epi16(x1, v_factors);
742 /* the total sums */
743 x0 = _mm_hadd_epi16(x2, x3);
744 x1 = _mm_hadd_epi16(x4, x5);
745 /* shift the results */
746 x0 = _mm_srai_epi16(x0, U_SHIFT);
747 x1 = _mm_srai_epi16(x1, V_SHIFT);
748 /* pack the 16 words into bytes */
749 x0 = _mm_packs_epi16(x0, x1);
750 /* add 128 */
751 x0 = _mm_sub_epi8(x0, vector128);
752 /* the lower 8 bytes go to the u plane */
753 _mm_storel_pi(udst, _mm_castsi128_ps(x0));
754 /* the upper 8 bytes go to the v plane */
755 _mm_storeh_pi(vdst, _mm_castsi128_ps(x0));
756 }
757
758 for (; x < width - width % 2; x += 2)
759 {
760 BYTE u[4] = WINPR_C_ARRAY_INIT;
761 BYTE v[4] = WINPR_C_ARRAY_INIT;
762 sse41_BGRX_TO_YUV(&src1[4ULL * x], nullptr, &u[0], &v[0]);
763 sse41_BGRX_TO_YUV(&src1[4ULL * (1ULL + x)], nullptr, &u[1], &v[1]);
764 sse41_BGRX_TO_YUV(&src2[4ULL * x], nullptr, &u[2], &v[2]);
765 sse41_BGRX_TO_YUV(&src2[4ULL * (1ULL + x)], nullptr, &u[3], &v[3]);
766 const INT16 u4 = WINPR_ASSERTING_INT_CAST(INT16, (INT16)u[0] + u[1] + u[2] + u[3]);
767 const INT16 uu = WINPR_ASSERTING_INT_CAST(INT16, u4 / 4);
768 const BYTE u8 = CLIP(uu);
769 dst1[x / 2] = u8;
770
771 const INT16 v4 = WINPR_ASSERTING_INT_CAST(INT16, (INT16)v[0] + v[1] + v[2] + v[3]);
772 const INT16 vu = WINPR_ASSERTING_INT_CAST(INT16, v4 / 4);
773 const BYTE v8 = CLIP(vu);
774 dst2[x / 2] = v8;
775 }
776}
777
778static pstatus_t sse41_RGBToYUV420_BGRX(const BYTE* WINPR_RESTRICT pSrc, UINT32 srcStep,
779 BYTE* WINPR_RESTRICT pDst[], const UINT32 dstStep[],
780 const prim_size_t* WINPR_RESTRICT roi)
781{
782 if (roi->height < 1 || roi->width < 1)
783 {
784 return !PRIMITIVES_SUCCESS;
785 }
786
787 size_t y = 0;
788 for (; y < roi->height - roi->height % 2; y += 2)
789 {
790 const BYTE* line1 = &pSrc[y * srcStep];
791 const BYTE* line2 = &pSrc[(1ULL + y) * srcStep];
792 BYTE* ydst1 = &pDst[0][y * dstStep[0]];
793 BYTE* ydst2 = &pDst[0][(1ULL + y) * dstStep[0]];
794 BYTE* udst = &pDst[1][y / 2 * dstStep[1]];
795 BYTE* vdst = &pDst[2][y / 2 * dstStep[2]];
796
797 sse41_RGBToYUV420_BGRX_UV(line1, line2, udst, vdst, roi->width);
798 sse41_RGBToYUV420_BGRX_Y(line1, ydst1, roi->width);
799 sse41_RGBToYUV420_BGRX_Y(line2, ydst2, roi->width);
800 }
801
802 for (; y < roi->height; y++)
803 {
804 const BYTE* line = &pSrc[y * srcStep];
805 BYTE* ydst = &pDst[0][1ULL * y * dstStep[0]];
806 sse41_RGBToYUV420_BGRX_Y(line, ydst, roi->width);
807 }
808
809 return PRIMITIVES_SUCCESS;
810}
811
812static pstatus_t sse41_RGBToYUV420(const BYTE* WINPR_RESTRICT pSrc, UINT32 srcFormat,
813 UINT32 srcStep, BYTE* WINPR_RESTRICT pDst[],
814 const UINT32 dstStep[], const prim_size_t* WINPR_RESTRICT roi)
815{
816 switch (srcFormat)
817 {
818 case PIXEL_FORMAT_BGRX32:
819 case PIXEL_FORMAT_BGRA32:
820 return sse41_RGBToYUV420_BGRX(pSrc, srcStep, pDst, dstStep, roi);
821
822 default:
823 return generic->RGBToYUV420_8u_P3AC4R(pSrc, srcFormat, srcStep, pDst, dstStep, roi);
824 }
825}
826
827/****************************************************************************/
828/* sse41 RGB -> AVC444-YUV conversion **/
829/****************************************************************************/
830
831static inline void sse41_RGBToAVC444YUV_BGRX_DOUBLE_ROW(
832 const BYTE* WINPR_RESTRICT srcEven, const BYTE* WINPR_RESTRICT srcOdd,
833 BYTE* WINPR_RESTRICT b1Even, BYTE* WINPR_RESTRICT b1Odd, BYTE* WINPR_RESTRICT b2,
834 BYTE* WINPR_RESTRICT b3, BYTE* WINPR_RESTRICT b4, BYTE* WINPR_RESTRICT b5,
835 BYTE* WINPR_RESTRICT b6, BYTE* WINPR_RESTRICT b7, UINT32 width)
836{
837 const __m128i* argbEven = WINPR_PACKED_ALIGN_CAST(const __m128i*, srcEven);
838 const __m128i* argbOdd = WINPR_PACKED_ALIGN_CAST(const __m128i*, srcOdd);
839 const __m128i y_factors = BGRX_Y_FACTORS;
840 const __m128i u_factors = BGRX_U_FACTORS;
841 const __m128i v_factors = BGRX_V_FACTORS;
842 const __m128i vector128 = CONST128_FACTORS;
843
844 UINT32 x = 0;
845 for (; x < width - width % 16; x += 16)
846 {
847 /* store 16 rgba pixels in 4 128 bit registers */
848 const __m128i xe1 = LOAD_SI128(argbEven++); // 1st 4 pixels
849 const __m128i xe2 = LOAD_SI128(argbEven++); // 2nd 4 pixels
850 const __m128i xe3 = LOAD_SI128(argbEven++); // 3rd 4 pixels
851 const __m128i xe4 = LOAD_SI128(argbEven++); // 4th 4 pixels
852 const __m128i xo1 = LOAD_SI128(argbOdd++); // 1st 4 pixels
853 const __m128i xo2 = LOAD_SI128(argbOdd++); // 2nd 4 pixels
854 const __m128i xo3 = LOAD_SI128(argbOdd++); // 3rd 4 pixels
855 const __m128i xo4 = LOAD_SI128(argbOdd++); // 4th 4 pixels
856 {
857 /* Y: multiplications with subtotals and horizontal sums */
858 const __m128i ye1 = _mm_srli_epi16(_mm_hadd_epi16(_mm_maddubs_epi16(xe1, y_factors),
859 _mm_maddubs_epi16(xe2, y_factors)),
860 Y_SHIFT);
861 const __m128i ye2 = _mm_srli_epi16(_mm_hadd_epi16(_mm_maddubs_epi16(xe3, y_factors),
862 _mm_maddubs_epi16(xe4, y_factors)),
863 Y_SHIFT);
864 const __m128i ye = _mm_packus_epi16(ye1, ye2);
865 const __m128i yo1 = _mm_srli_epi16(_mm_hadd_epi16(_mm_maddubs_epi16(xo1, y_factors),
866 _mm_maddubs_epi16(xo2, y_factors)),
867 Y_SHIFT);
868 const __m128i yo2 = _mm_srli_epi16(_mm_hadd_epi16(_mm_maddubs_epi16(xo3, y_factors),
869 _mm_maddubs_epi16(xo4, y_factors)),
870 Y_SHIFT);
871 const __m128i yo = _mm_packus_epi16(yo1, yo2);
872 /* store y [b1] */
873 STORE_SI128(b1Even, ye);
874 b1Even += 16;
875
876 if (b1Odd)
877 {
878 STORE_SI128(b1Odd, yo);
879 b1Odd += 16;
880 }
881 }
882 {
883 /* We have now
884 * 16 even U values in ue
885 * 16 odd U values in uo
886 *
887 * We need to split these according to
888 * 3.3.8.3.2 YUV420p Stream Combination for YUV444 mode */
889 __m128i ue;
890 __m128i uo = WINPR_C_ARRAY_INIT;
891 {
892 const __m128i ue1 =
893 _mm_srai_epi16(_mm_hadd_epi16(_mm_maddubs_epi16(xe1, u_factors),
894 _mm_maddubs_epi16(xe2, u_factors)),
895 U_SHIFT);
896 const __m128i ue2 =
897 _mm_srai_epi16(_mm_hadd_epi16(_mm_maddubs_epi16(xe3, u_factors),
898 _mm_maddubs_epi16(xe4, u_factors)),
899 U_SHIFT);
900 ue = _mm_sub_epi8(_mm_packs_epi16(ue1, ue2), vector128);
901 }
902
903 if (b1Odd)
904 {
905 const __m128i uo1 =
906 _mm_srai_epi16(_mm_hadd_epi16(_mm_maddubs_epi16(xo1, u_factors),
907 _mm_maddubs_epi16(xo2, u_factors)),
908 U_SHIFT);
909 const __m128i uo2 =
910 _mm_srai_epi16(_mm_hadd_epi16(_mm_maddubs_epi16(xo3, u_factors),
911 _mm_maddubs_epi16(xo4, u_factors)),
912 U_SHIFT);
913 uo = _mm_sub_epi8(_mm_packs_epi16(uo1, uo2), vector128);
914 }
915
916 /* Now we need the following storage distribution:
917 * 2x 2y -> b2
918 * x 2y+1 -> b4
919 * 2x+1 2y -> b6 */
920 if (b1Odd) /* b2 */
921 {
922 const __m128i ueh = _mm_unpackhi_epi8(ue, _mm_setzero_si128());
923 const __m128i uoh = _mm_unpackhi_epi8(uo, _mm_setzero_si128());
924 const __m128i hi = _mm_add_epi16(ueh, uoh);
925 const __m128i uel = _mm_unpacklo_epi8(ue, _mm_setzero_si128());
926 const __m128i uol = _mm_unpacklo_epi8(uo, _mm_setzero_si128());
927 const __m128i lo = _mm_add_epi16(uel, uol);
928 const __m128i added = _mm_hadd_epi16(lo, hi);
929 const __m128i avg16 = _mm_srai_epi16(added, 2);
930 const __m128i avg = _mm_packus_epi16(avg16, avg16);
931 _mm_storel_epi64(WINPR_PACKED_ALIGN_CAST(__m128i*, b2), avg);
932 }
933 else
934 {
935 const __m128i mask =
936 _mm_set_epi8((char)0x80, (char)0x80, (char)0x80, (char)0x80, (char)0x80,
937 (char)0x80, (char)0x80, (char)0x80, 14, 12, 10, 8, 6, 4, 2, 0);
938 const __m128i ud = _mm_shuffle_epi8(ue, mask);
939 _mm_storel_epi64(WINPR_PACKED_ALIGN_CAST(__m128i*, b2), ud);
940 }
941
942 b2 += 8;
943
944 if (b1Odd) /* b4 */
945 {
946 STORE_SI128(b4, uo);
947 b4 += 16;
948 }
949
950 {
951 /* b6 */
952 const __m128i mask =
953 _mm_set_epi8((char)0x80, (char)0x80, (char)0x80, (char)0x80, (char)0x80,
954 (char)0x80, (char)0x80, (char)0x80, 15, 13, 11, 9, 7, 5, 3, 1);
955 const __m128i ude = _mm_shuffle_epi8(ue, mask);
956 _mm_storel_epi64(WINPR_PACKED_ALIGN_CAST(__m128i*, b6), ude);
957 b6 += 8;
958 }
959 }
960 {
961 /* We have now
962 * 16 even V values in ue
963 * 16 odd V values in uo
964 *
965 * We need to split these according to
966 * 3.3.8.3.2 YUV420p Stream Combination for YUV444 mode */
967 __m128i ve;
968 __m128i vo = WINPR_C_ARRAY_INIT;
969 {
970 const __m128i ve1 =
971 _mm_srai_epi16(_mm_hadd_epi16(_mm_maddubs_epi16(xe1, v_factors),
972 _mm_maddubs_epi16(xe2, v_factors)),
973 V_SHIFT);
974 const __m128i ve2 =
975 _mm_srai_epi16(_mm_hadd_epi16(_mm_maddubs_epi16(xe3, v_factors),
976 _mm_maddubs_epi16(xe4, v_factors)),
977 V_SHIFT);
978 ve = _mm_sub_epi8(_mm_packs_epi16(ve1, ve2), vector128);
979 }
980
981 if (b1Odd)
982 {
983 const __m128i vo1 =
984 _mm_srai_epi16(_mm_hadd_epi16(_mm_maddubs_epi16(xo1, v_factors),
985 _mm_maddubs_epi16(xo2, v_factors)),
986 V_SHIFT);
987 const __m128i vo2 =
988 _mm_srai_epi16(_mm_hadd_epi16(_mm_maddubs_epi16(xo3, v_factors),
989 _mm_maddubs_epi16(xo4, v_factors)),
990 V_SHIFT);
991 vo = _mm_sub_epi8(_mm_packs_epi16(vo1, vo2), vector128);
992 }
993
994 /* Now we need the following storage distribution:
995 * 2x 2y -> b3
996 * x 2y+1 -> b5
997 * 2x+1 2y -> b7 */
998 if (b1Odd) /* b3 */
999 {
1000 const __m128i veh = _mm_unpackhi_epi8(ve, _mm_setzero_si128());
1001 const __m128i voh = _mm_unpackhi_epi8(vo, _mm_setzero_si128());
1002 const __m128i hi = _mm_add_epi16(veh, voh);
1003 const __m128i vel = _mm_unpacklo_epi8(ve, _mm_setzero_si128());
1004 const __m128i vol = _mm_unpacklo_epi8(vo, _mm_setzero_si128());
1005 const __m128i lo = _mm_add_epi16(vel, vol);
1006 const __m128i added = _mm_hadd_epi16(lo, hi);
1007 const __m128i avg16 = _mm_srai_epi16(added, 2);
1008 const __m128i avg = _mm_packus_epi16(avg16, avg16);
1009 _mm_storel_epi64(WINPR_PACKED_ALIGN_CAST(__m128i*, b3), avg);
1010 }
1011 else
1012 {
1013 const __m128i mask =
1014 _mm_set_epi8((char)0x80, (char)0x80, (char)0x80, (char)0x80, (char)0x80,
1015 (char)0x80, (char)0x80, (char)0x80, 14, 12, 10, 8, 6, 4, 2, 0);
1016 const __m128i vd = _mm_shuffle_epi8(ve, mask);
1017 _mm_storel_epi64(WINPR_PACKED_ALIGN_CAST(__m128i*, b3), vd);
1018 }
1019
1020 b3 += 8;
1021
1022 if (b1Odd) /* b5 */
1023 {
1024 STORE_SI128(b5, vo);
1025 b5 += 16;
1026 }
1027
1028 {
1029 /* b7 */
1030 const __m128i mask =
1031 _mm_set_epi8((char)0x80, (char)0x80, (char)0x80, (char)0x80, (char)0x80,
1032 (char)0x80, (char)0x80, (char)0x80, 15, 13, 11, 9, 7, 5, 3, 1);
1033 const __m128i vde = _mm_shuffle_epi8(ve, mask);
1034 _mm_storel_epi64(WINPR_PACKED_ALIGN_CAST(__m128i*, b7), vde);
1035 b7 += 8;
1036 }
1037 }
1038 }
1039
1040 general_RGBToAVC444YUV_BGRX_DOUBLE_ROW(x, srcEven, srcOdd, b1Even, b1Odd, b2, b3, b4, b5, b6,
1041 b7, width);
1042}
1043
1044static pstatus_t sse41_RGBToAVC444YUV_BGRX(const BYTE* WINPR_RESTRICT pSrc,
1045 WINPR_ATTR_UNUSED UINT32 srcFormat, UINT32 srcStep,
1046 BYTE* WINPR_RESTRICT pDst1[], const UINT32 dst1Step[],
1047 BYTE* WINPR_RESTRICT pDst2[], const UINT32 dst2Step[],
1048 const prim_size_t* WINPR_RESTRICT roi)
1049{
1050 if (roi->height < 1 || roi->width < 1)
1051 return !PRIMITIVES_SUCCESS;
1052
1053 size_t y = 0;
1054 for (; y < roi->height - roi->height % 2; y += 2)
1055 {
1056 const BYTE* srcEven = pSrc + y * srcStep;
1057 const BYTE* srcOdd = pSrc + (y + 1) * srcStep;
1058 const size_t i = y >> 1;
1059 const size_t n = (i & (size_t)~7) + i;
1060 BYTE* b1Even = pDst1[0] + y * dst1Step[0];
1061 BYTE* b1Odd = (b1Even + dst1Step[0]);
1062 BYTE* b2 = pDst1[1] + (y / 2) * dst1Step[1];
1063 BYTE* b3 = pDst1[2] + (y / 2) * dst1Step[2];
1064 BYTE* b4 = pDst2[0] + 1ULL * dst2Step[0] * n;
1065 BYTE* b5 = b4 + 8ULL * dst2Step[0];
1066 BYTE* b6 = pDst2[1] + (y / 2) * dst2Step[1];
1067 BYTE* b7 = pDst2[2] + (y / 2) * dst2Step[2];
1068 sse41_RGBToAVC444YUV_BGRX_DOUBLE_ROW(srcEven, srcOdd, b1Even, b1Odd, b2, b3, b4, b5, b6, b7,
1069 roi->width);
1070 }
1071
1072 for (; y < roi->height; y++)
1073 {
1074 const BYTE* srcEven = pSrc + y * srcStep;
1075 BYTE* b1Even = pDst1[0] + y * dst1Step[0];
1076 BYTE* b2 = pDst1[1] + (y / 2) * dst1Step[1];
1077 BYTE* b3 = pDst1[2] + (y / 2) * dst1Step[2];
1078 BYTE* b6 = pDst2[1] + (y / 2) * dst2Step[1];
1079 BYTE* b7 = pDst2[2] + (y / 2) * dst2Step[2];
1080 general_RGBToAVC444YUV_BGRX_DOUBLE_ROW(0, srcEven, nullptr, b1Even, nullptr, b2, b3,
1081 nullptr, nullptr, b6, b7, roi->width);
1082 }
1083
1084 return PRIMITIVES_SUCCESS;
1085}
1086
1087static pstatus_t sse41_RGBToAVC444YUV(const BYTE* WINPR_RESTRICT pSrc, UINT32 srcFormat,
1088 UINT32 srcStep, BYTE* WINPR_RESTRICT pDst1[],
1089 const UINT32 dst1Step[], BYTE* WINPR_RESTRICT pDst2[],
1090 const UINT32 dst2Step[],
1091 const prim_size_t* WINPR_RESTRICT roi)
1092{
1093 switch (srcFormat)
1094 {
1095 case PIXEL_FORMAT_BGRX32:
1096 case PIXEL_FORMAT_BGRA32:
1097 return sse41_RGBToAVC444YUV_BGRX(pSrc, srcFormat, srcStep, pDst1, dst1Step, pDst2,
1098 dst2Step, roi);
1099
1100 default:
1101 return generic->RGBToAVC444YUV(pSrc, srcFormat, srcStep, pDst1, dst1Step, pDst2,
1102 dst2Step, roi);
1103 }
1104}
1105
1106/* Mapping of arguments:
1107 *
1108 * b1 [even lines] -> yLumaDstEven
1109 * b1 [odd lines] -> yLumaDstOdd
1110 * b2 -> uLumaDst
1111 * b3 -> vLumaDst
1112 * b4 -> yChromaDst1
1113 * b5 -> yChromaDst2
1114 * b6 -> uChromaDst1
1115 * b7 -> uChromaDst2
1116 * b8 -> vChromaDst1
1117 * b9 -> vChromaDst2
1118 */
1119static inline void sse41_RGBToAVC444YUVv2_BGRX_DOUBLE_ROW(
1120 const BYTE* WINPR_RESTRICT srcEven, const BYTE* WINPR_RESTRICT srcOdd,
1121 BYTE* WINPR_RESTRICT yLumaDstEven, BYTE* WINPR_RESTRICT yLumaDstOdd,
1122 BYTE* WINPR_RESTRICT uLumaDst, BYTE* WINPR_RESTRICT vLumaDst,
1123 BYTE* WINPR_RESTRICT yEvenChromaDst1, BYTE* WINPR_RESTRICT yEvenChromaDst2,
1124 BYTE* WINPR_RESTRICT yOddChromaDst1, BYTE* WINPR_RESTRICT yOddChromaDst2,
1125 BYTE* WINPR_RESTRICT uChromaDst1, BYTE* WINPR_RESTRICT uChromaDst2,
1126 BYTE* WINPR_RESTRICT vChromaDst1, BYTE* WINPR_RESTRICT vChromaDst2, UINT32 width)
1127{
1128 const __m128i vector128 = CONST128_FACTORS;
1129 const __m128i* argbEven = WINPR_PACKED_ALIGN_CAST(const __m128i*, srcEven);
1130 const __m128i* argbOdd = WINPR_PACKED_ALIGN_CAST(const __m128i*, srcOdd);
1131
1132 UINT32 x = 0;
1133 for (; x < width - width % 16; x += 16)
1134 {
1135 /* store 16 rgba pixels in 4 128 bit registers
1136 * for even and odd rows.
1137 */
1138 const __m128i xe1 = LOAD_SI128(argbEven++); /* 1st 4 pixels */
1139 const __m128i xe2 = LOAD_SI128(argbEven++); /* 2nd 4 pixels */
1140 const __m128i xe3 = LOAD_SI128(argbEven++); /* 3rd 4 pixels */
1141 const __m128i xe4 = LOAD_SI128(argbEven++); /* 4th 4 pixels */
1142 const __m128i xo1 = LOAD_SI128(argbOdd++); /* 1st 4 pixels */
1143 const __m128i xo2 = LOAD_SI128(argbOdd++); /* 2nd 4 pixels */
1144 const __m128i xo3 = LOAD_SI128(argbOdd++); /* 3rd 4 pixels */
1145 const __m128i xo4 = LOAD_SI128(argbOdd++); /* 4th 4 pixels */
1146 {
1147 /* Y: multiplications with subtotals and horizontal sums */
1148 const __m128i y_factors = BGRX_Y_FACTORS;
1149 const __m128i ye1 = _mm_srli_epi16(_mm_hadd_epi16(_mm_maddubs_epi16(xe1, y_factors),
1150 _mm_maddubs_epi16(xe2, y_factors)),
1151 Y_SHIFT);
1152 const __m128i ye2 = _mm_srli_epi16(_mm_hadd_epi16(_mm_maddubs_epi16(xe3, y_factors),
1153 _mm_maddubs_epi16(xe4, y_factors)),
1154 Y_SHIFT);
1155 const __m128i ye = _mm_packus_epi16(ye1, ye2);
1156 /* store y [b1] */
1157 STORE_SI128(yLumaDstEven, ye);
1158 yLumaDstEven += 16;
1159 }
1160
1161 if (yLumaDstOdd)
1162 {
1163 const __m128i y_factors = BGRX_Y_FACTORS;
1164 const __m128i yo1 = _mm_srli_epi16(_mm_hadd_epi16(_mm_maddubs_epi16(xo1, y_factors),
1165 _mm_maddubs_epi16(xo2, y_factors)),
1166 Y_SHIFT);
1167 const __m128i yo2 = _mm_srli_epi16(_mm_hadd_epi16(_mm_maddubs_epi16(xo3, y_factors),
1168 _mm_maddubs_epi16(xo4, y_factors)),
1169 Y_SHIFT);
1170 const __m128i yo = _mm_packus_epi16(yo1, yo2);
1171 STORE_SI128(yLumaDstOdd, yo);
1172 yLumaDstOdd += 16;
1173 }
1174
1175 {
1176 /* We have now
1177 * 16 even U values in ue
1178 * 16 odd U values in uo
1179 *
1180 * We need to split these according to
1181 * 3.3.8.3.3 YUV420p Stream Combination for YUV444v2 mode */
1182 /* U: multiplications with subtotals and horizontal sums */
1183 __m128i ue;
1184 __m128i uo;
1185 __m128i uavg;
1186 {
1187 const __m128i u_factors = BGRX_U_FACTORS;
1188 const __m128i ue1 =
1189 _mm_srai_epi16(_mm_hadd_epi16(_mm_maddubs_epi16(xe1, u_factors),
1190 _mm_maddubs_epi16(xe2, u_factors)),
1191 U_SHIFT);
1192 const __m128i ue2 =
1193 _mm_srai_epi16(_mm_hadd_epi16(_mm_maddubs_epi16(xe3, u_factors),
1194 _mm_maddubs_epi16(xe4, u_factors)),
1195 U_SHIFT);
1196 const __m128i ueavg = _mm_hadd_epi16(ue1, ue2);
1197 ue = _mm_sub_epi8(_mm_packs_epi16(ue1, ue2), vector128);
1198 uavg = ueavg;
1199 }
1200 {
1201 const __m128i u_factors = BGRX_U_FACTORS;
1202 const __m128i uo1 =
1203 _mm_srai_epi16(_mm_hadd_epi16(_mm_maddubs_epi16(xo1, u_factors),
1204 _mm_maddubs_epi16(xo2, u_factors)),
1205 U_SHIFT);
1206 const __m128i uo2 =
1207 _mm_srai_epi16(_mm_hadd_epi16(_mm_maddubs_epi16(xo3, u_factors),
1208 _mm_maddubs_epi16(xo4, u_factors)),
1209 U_SHIFT);
1210 const __m128i uoavg = _mm_hadd_epi16(uo1, uo2);
1211 uo = _mm_sub_epi8(_mm_packs_epi16(uo1, uo2), vector128);
1212 uavg = _mm_add_epi16(uavg, uoavg);
1213 uavg = _mm_srai_epi16(uavg, 2);
1214 uavg = _mm_packs_epi16(uavg, uoavg);
1215 uavg = _mm_sub_epi8(uavg, vector128);
1216 }
1217 /* Now we need the following storage distribution:
1218 * 2x 2y -> uLumaDst
1219 * 2x+1 y -> yChromaDst1
1220 * 4x 2y+1 -> uChromaDst1
1221 * 4x+2 2y+1 -> vChromaDst1 */
1222 {
1223 const __m128i mask =
1224 _mm_set_epi8((char)0x80, (char)0x80, (char)0x80, (char)0x80, (char)0x80,
1225 (char)0x80, (char)0x80, (char)0x80, 15, 13, 11, 9, 7, 5, 3, 1);
1226 const __m128i ude = _mm_shuffle_epi8(ue, mask);
1227 _mm_storel_epi64(WINPR_PACKED_ALIGN_CAST(__m128i*, yEvenChromaDst1), ude);
1228 yEvenChromaDst1 += 8;
1229 }
1230
1231 if (yLumaDstOdd)
1232 {
1233 const __m128i mask =
1234 _mm_set_epi8((char)0x80, (char)0x80, (char)0x80, (char)0x80, (char)0x80,
1235 (char)0x80, (char)0x80, (char)0x80, 15, 13, 11, 9, 7, 5, 3, 1);
1236 const __m128i udo /* codespell:ignore udo */ = _mm_shuffle_epi8(uo, mask);
1237 _mm_storel_epi64(WINPR_PACKED_ALIGN_CAST(__m128i*, yOddChromaDst1),
1238 udo); // codespell:ignore udo
1239 yOddChromaDst1 += 8;
1240 }
1241
1242 if (yLumaDstOdd)
1243 {
1244 const __m128i mask =
1245 _mm_set_epi8((char)0x80, (char)0x80, (char)0x80, (char)0x80, (char)0x80,
1246 (char)0x80, (char)0x80, (char)0x80, 14, 10, 6, 2, 12, 8, 4, 0);
1247 const __m128i ud = _mm_shuffle_epi8(uo, mask);
1248 int* uDst1 = WINPR_PACKED_ALIGN_CAST(int*, uChromaDst1);
1249 int* vDst1 = WINPR_PACKED_ALIGN_CAST(int*, vChromaDst1);
1250 const int* src = (const int*)&ud;
1251 _mm_stream_si32(uDst1, src[0]);
1252 _mm_stream_si32(vDst1, src[1]);
1253 uChromaDst1 += 4;
1254 vChromaDst1 += 4;
1255 }
1256
1257 if (yLumaDstOdd)
1258 {
1259 _mm_storel_epi64(WINPR_PACKED_ALIGN_CAST(__m128i*, uLumaDst), uavg);
1260 uLumaDst += 8;
1261 }
1262 else
1263 {
1264 const __m128i mask =
1265 _mm_set_epi8((char)0x80, (char)0x80, (char)0x80, (char)0x80, (char)0x80,
1266 (char)0x80, (char)0x80, (char)0x80, 14, 12, 10, 8, 6, 4, 2, 0);
1267 const __m128i ud = _mm_shuffle_epi8(ue, mask);
1268 _mm_storel_epi64(WINPR_PACKED_ALIGN_CAST(__m128i*, uLumaDst), ud);
1269 uLumaDst += 8;
1270 }
1271 }
1272
1273 {
1274 /* V: multiplications with subtotals and horizontal sums */
1275 __m128i ve;
1276 __m128i vo;
1277 __m128i vavg;
1278 {
1279 const __m128i v_factors = BGRX_V_FACTORS;
1280 const __m128i ve1 =
1281 _mm_srai_epi16(_mm_hadd_epi16(_mm_maddubs_epi16(xe1, v_factors),
1282 _mm_maddubs_epi16(xe2, v_factors)),
1283 V_SHIFT);
1284 const __m128i ve2 =
1285 _mm_srai_epi16(_mm_hadd_epi16(_mm_maddubs_epi16(xe3, v_factors),
1286 _mm_maddubs_epi16(xe4, v_factors)),
1287 V_SHIFT);
1288 const __m128i veavg = _mm_hadd_epi16(ve1, ve2);
1289 ve = _mm_sub_epi8(_mm_packs_epi16(ve1, ve2), vector128);
1290 vavg = veavg;
1291 }
1292 {
1293 const __m128i v_factors = BGRX_V_FACTORS;
1294 const __m128i vo1 =
1295 _mm_srai_epi16(_mm_hadd_epi16(_mm_maddubs_epi16(xo1, v_factors),
1296 _mm_maddubs_epi16(xo2, v_factors)),
1297 V_SHIFT);
1298 const __m128i vo2 =
1299 _mm_srai_epi16(_mm_hadd_epi16(_mm_maddubs_epi16(xo3, v_factors),
1300 _mm_maddubs_epi16(xo4, v_factors)),
1301 V_SHIFT);
1302 const __m128i voavg = _mm_hadd_epi16(vo1, vo2);
1303 vo = _mm_sub_epi8(_mm_packs_epi16(vo1, vo2), vector128);
1304 vavg = _mm_add_epi16(vavg, voavg);
1305 vavg = _mm_srai_epi16(vavg, 2);
1306 vavg = _mm_packs_epi16(vavg, voavg);
1307 vavg = _mm_sub_epi8(vavg, vector128);
1308 }
1309 /* Now we need the following storage distribution:
1310 * 2x 2y -> vLumaDst
1311 * 2x+1 y -> yChromaDst2
1312 * 4x 2y+1 -> uChromaDst2
1313 * 4x+2 2y+1 -> vChromaDst2 */
1314 {
1315 const __m128i mask =
1316 _mm_set_epi8((char)0x80, (char)0x80, (char)0x80, (char)0x80, (char)0x80,
1317 (char)0x80, (char)0x80, (char)0x80, 15, 13, 11, 9, 7, 5, 3, 1);
1318 __m128i vde = _mm_shuffle_epi8(ve, mask);
1319 _mm_storel_epi64(WINPR_PACKED_ALIGN_CAST(__m128i*, yEvenChromaDst2), vde);
1320 yEvenChromaDst2 += 8;
1321 }
1322
1323 if (yLumaDstOdd)
1324 {
1325 const __m128i mask =
1326 _mm_set_epi8((char)0x80, (char)0x80, (char)0x80, (char)0x80, (char)0x80,
1327 (char)0x80, (char)0x80, (char)0x80, 15, 13, 11, 9, 7, 5, 3, 1);
1328 __m128i vdo = _mm_shuffle_epi8(vo, mask);
1329 _mm_storel_epi64(WINPR_PACKED_ALIGN_CAST(__m128i*, yOddChromaDst2), vdo);
1330 yOddChromaDst2 += 8;
1331 }
1332
1333 if (yLumaDstOdd)
1334 {
1335 const __m128i mask =
1336 _mm_set_epi8((char)0x80, (char)0x80, (char)0x80, (char)0x80, (char)0x80,
1337 (char)0x80, (char)0x80, (char)0x80, 14, 10, 6, 2, 12, 8, 4, 0);
1338 const __m128i vd = _mm_shuffle_epi8(vo, mask);
1339 int* uDst2 = WINPR_PACKED_ALIGN_CAST(int*, uChromaDst2);
1340 int* vDst2 = WINPR_PACKED_ALIGN_CAST(int*, vChromaDst2);
1341 const int* src = (const int*)&vd;
1342 _mm_stream_si32(uDst2, src[0]);
1343 _mm_stream_si32(vDst2, src[1]);
1344 uChromaDst2 += 4;
1345 vChromaDst2 += 4;
1346 }
1347
1348 if (yLumaDstOdd)
1349 {
1350 _mm_storel_epi64(WINPR_PACKED_ALIGN_CAST(__m128i*, vLumaDst), vavg);
1351 vLumaDst += 8;
1352 }
1353 else
1354 {
1355 const __m128i mask =
1356 _mm_set_epi8((char)0x80, (char)0x80, (char)0x80, (char)0x80, (char)0x80,
1357 (char)0x80, (char)0x80, (char)0x80, 14, 12, 10, 8, 6, 4, 2, 0);
1358 __m128i vd = _mm_shuffle_epi8(ve, mask);
1359 _mm_storel_epi64(WINPR_PACKED_ALIGN_CAST(__m128i*, vLumaDst), vd);
1360 vLumaDst += 8;
1361 }
1362 }
1363 }
1364
1365 general_RGBToAVC444YUVv2_BGRX_DOUBLE_ROW(x, srcEven, srcOdd, yLumaDstEven, yLumaDstOdd,
1366 uLumaDst, vLumaDst, yEvenChromaDst1, yEvenChromaDst2,
1367 yOddChromaDst1, yOddChromaDst2, uChromaDst1,
1368 uChromaDst2, vChromaDst1, vChromaDst2, width);
1369}
1370
1371static pstatus_t sse41_RGBToAVC444YUVv2_BGRX(const BYTE* WINPR_RESTRICT pSrc,
1372 WINPR_ATTR_UNUSED UINT32 srcFormat, UINT32 srcStep,
1373 BYTE* WINPR_RESTRICT pDst1[], const UINT32 dst1Step[],
1374 BYTE* WINPR_RESTRICT pDst2[], const UINT32 dst2Step[],
1375 const prim_size_t* WINPR_RESTRICT roi)
1376{
1377 if (roi->height < 1 || roi->width < 1)
1378 return !PRIMITIVES_SUCCESS;
1379
1380 size_t y = 0;
1381 for (; y < roi->height - roi->height % 2; y += 2)
1382 {
1383 const BYTE* srcEven = (pSrc + y * srcStep);
1384 const BYTE* srcOdd = (srcEven + srcStep);
1385 BYTE* dstLumaYEven = (pDst1[0] + y * dst1Step[0]);
1386 BYTE* dstLumaYOdd = (dstLumaYEven + dst1Step[0]);
1387 BYTE* dstLumaU = (pDst1[1] + (y / 2) * dst1Step[1]);
1388 BYTE* dstLumaV = (pDst1[2] + (y / 2) * dst1Step[2]);
1389 BYTE* dstEvenChromaY1 = (pDst2[0] + y * dst2Step[0]);
1390 BYTE* dstEvenChromaY2 = dstEvenChromaY1 + roi->width / 2;
1391 BYTE* dstOddChromaY1 = dstEvenChromaY1 + dst2Step[0];
1392 BYTE* dstOddChromaY2 = dstEvenChromaY2 + dst2Step[0];
1393 BYTE* dstChromaU1 = (pDst2[1] + (y / 2) * dst2Step[1]);
1394 BYTE* dstChromaV1 = (pDst2[2] + (y / 2) * dst2Step[2]);
1395 BYTE* dstChromaU2 = dstChromaU1 + roi->width / 4;
1396 BYTE* dstChromaV2 = dstChromaV1 + roi->width / 4;
1397 sse41_RGBToAVC444YUVv2_BGRX_DOUBLE_ROW(srcEven, srcOdd, dstLumaYEven, dstLumaYOdd, dstLumaU,
1398 dstLumaV, dstEvenChromaY1, dstEvenChromaY2,
1399 dstOddChromaY1, dstOddChromaY2, dstChromaU1,
1400 dstChromaU2, dstChromaV1, dstChromaV2, roi->width);
1401 }
1402
1403 for (; y < roi->height; y++)
1404 {
1405 const BYTE* srcEven = (pSrc + y * srcStep);
1406 BYTE* dstLumaYEven = (pDst1[0] + y * dst1Step[0]);
1407 BYTE* dstLumaU = (pDst1[1] + (y / 2) * dst1Step[1]);
1408 BYTE* dstLumaV = (pDst1[2] + (y / 2) * dst1Step[2]);
1409 BYTE* dstEvenChromaY1 = (pDst2[0] + y * dst2Step[0]);
1410 BYTE* dstEvenChromaY2 = dstEvenChromaY1 + roi->width / 2;
1411 BYTE* dstChromaU1 = (pDst2[1] + (y / 2) * dst2Step[1]);
1412 BYTE* dstChromaV1 = (pDst2[2] + (y / 2) * dst2Step[2]);
1413 BYTE* dstChromaU2 = dstChromaU1 + roi->width / 4;
1414 BYTE* dstChromaV2 = dstChromaV1 + roi->width / 4;
1415 general_RGBToAVC444YUVv2_BGRX_DOUBLE_ROW(0, srcEven, nullptr, dstLumaYEven, nullptr,
1416 dstLumaU, dstLumaV, dstEvenChromaY1,
1417 dstEvenChromaY2, nullptr, nullptr, dstChromaU1,
1418 dstChromaU2, dstChromaV1, dstChromaV2, roi->width);
1419 }
1420
1421 return PRIMITIVES_SUCCESS;
1422}
1423
1424static pstatus_t sse41_RGBToAVC444YUVv2(const BYTE* WINPR_RESTRICT pSrc, UINT32 srcFormat,
1425 UINT32 srcStep, BYTE* WINPR_RESTRICT pDst1[],
1426 const UINT32 dst1Step[], BYTE* WINPR_RESTRICT pDst2[],
1427 const UINT32 dst2Step[],
1428 const prim_size_t* WINPR_RESTRICT roi)
1429{
1430 switch (srcFormat)
1431 {
1432 case PIXEL_FORMAT_BGRX32:
1433 case PIXEL_FORMAT_BGRA32:
1434 return sse41_RGBToAVC444YUVv2_BGRX(pSrc, srcFormat, srcStep, pDst1, dst1Step, pDst2,
1435 dst2Step, roi);
1436
1437 default:
1438 return generic->RGBToAVC444YUVv2(pSrc, srcFormat, srcStep, pDst1, dst1Step, pDst2,
1439 dst2Step, roi);
1440 }
1441}
1442
1443static pstatus_t sse41_LumaToYUV444(const BYTE* WINPR_RESTRICT pSrcRaw[], const UINT32 srcStep[],
1444 BYTE* WINPR_RESTRICT pDstRaw[], const UINT32 dstStep[],
1445 const RECTANGLE_16* WINPR_RESTRICT roi)
1446{
1447 const UINT32 nWidth = roi->right - roi->left;
1448 const UINT32 nHeight = roi->bottom - roi->top;
1449 const UINT32 halfWidth = (nWidth + 1) / 2;
1450 const UINT32 halfPad = halfWidth % 16;
1451 const UINT32 halfHeight = (nHeight + 1) / 2;
1452 const UINT32 oddY = 1;
1453 const UINT32 evenY = 0;
1454 const UINT32 oddX = 1;
1455 const UINT32 evenX = 0;
1456 const BYTE* pSrc[3] = { pSrcRaw[0] + 1ULL * roi->top * srcStep[0] + roi->left,
1457 pSrcRaw[1] + 1ULL * roi->top / 2 * srcStep[1] + roi->left / 2,
1458 pSrcRaw[2] + 1ULL * roi->top / 2 * srcStep[2] + roi->left / 2 };
1459 BYTE* pDst[3] = { pDstRaw[0] + 1ULL * roi->top * dstStep[0] + roi->left,
1460 pDstRaw[1] + 1ULL * roi->top * dstStep[1] + roi->left,
1461 pDstRaw[2] + 1ULL * roi->top * dstStep[2] + roi->left };
1462
1463 /* Y data is already here... */
1464 /* B1 */
1465 for (size_t y = 0; y < nHeight; y++)
1466 {
1467 const BYTE* Ym = pSrc[0] + y * srcStep[0];
1468 BYTE* pY = pDst[0] + y * dstStep[0];
1469 memcpy(pY, Ym, nWidth);
1470 }
1471
1472 /* The first half of U, V are already here part of this frame. */
1473 /* B2 and B3 */
1474 for (size_t y = 0; y < halfHeight; y++)
1475 {
1476 const size_t val2y = (2 * y + evenY);
1477 const size_t val2y1 = val2y + oddY;
1478 const BYTE* Um = pSrc[1] + 1ULL * srcStep[1] * y;
1479 const BYTE* Vm = pSrc[2] + 1ULL * srcStep[2] * y;
1480 BYTE* pU = pDst[1] + 1ULL * dstStep[1] * val2y;
1481 BYTE* pV = pDst[2] + 1ULL * dstStep[2] * val2y;
1482 BYTE* pU1 = pDst[1] + 1ULL * dstStep[1] * val2y1;
1483 BYTE* pV1 = pDst[2] + 1ULL * dstStep[2] * val2y1;
1484
1485 size_t x = 0;
1486 for (; x < halfWidth - halfPad; x += 16)
1487 {
1488 const __m128i unpackHigh = _mm_set_epi8(7, 7, 6, 6, 5, 5, 4, 4, 3, 3, 2, 2, 1, 1, 0, 0);
1489 const __m128i unpackLow =
1490 _mm_set_epi8(15, 15, 14, 14, 13, 13, 12, 12, 11, 11, 10, 10, 9, 9, 8, 8);
1491 {
1492 const __m128i u = LOAD_SI128(&Um[x]);
1493 const __m128i uHigh = _mm_shuffle_epi8(u, unpackHigh);
1494 const __m128i uLow = _mm_shuffle_epi8(u, unpackLow);
1495 STORE_SI128(&pU[2ULL * x], uHigh);
1496 STORE_SI128(&pU[2ULL * x + 16], uLow);
1497 STORE_SI128(&pU1[2ULL * x], uHigh);
1498 STORE_SI128(&pU1[2ULL * x + 16], uLow);
1499 }
1500 {
1501 const __m128i u = LOAD_SI128(&Vm[x]);
1502 const __m128i uHigh = _mm_shuffle_epi8(u, unpackHigh);
1503 const __m128i uLow = _mm_shuffle_epi8(u, unpackLow);
1504 STORE_SI128(&pV[2 * x], uHigh);
1505 STORE_SI128(&pV[2 * x + 16], uLow);
1506 STORE_SI128(&pV1[2 * x], uHigh);
1507 STORE_SI128(&pV1[2 * x + 16], uLow);
1508 }
1509 }
1510
1511 for (; x < halfWidth; x++)
1512 {
1513 const size_t val2x = 2 * x + evenX;
1514 const size_t val2x1 = val2x + oddX;
1515 pU[val2x] = Um[x];
1516 pV[val2x] = Vm[x];
1517 pU[val2x1] = Um[x];
1518 pV[val2x1] = Vm[x];
1519 pU1[val2x] = Um[x];
1520 pV1[val2x] = Vm[x];
1521 pU1[val2x1] = Um[x];
1522 pV1[val2x1] = Vm[x];
1523 }
1524 }
1525
1526 return PRIMITIVES_SUCCESS;
1527}
1528
1529static pstatus_t sse41_ChromaV1ToYUV444(const BYTE* WINPR_RESTRICT pSrcRaw[3],
1530 const UINT32 srcStep[3], BYTE* WINPR_RESTRICT pDstRaw[3],
1531 const UINT32 dstStep[3],
1532 const RECTANGLE_16* WINPR_RESTRICT roi)
1533{
1534 const UINT32 mod = 16;
1535 UINT32 uY = 0;
1536 UINT32 vY = 0;
1537 const UINT32 nWidth = roi->right - roi->left;
1538 const UINT32 nHeight = roi->bottom - roi->top;
1539 const UINT32 halfWidth = (nWidth + 1) / 2;
1540 const UINT32 halfPad = halfWidth % 16;
1541 const UINT32 halfHeight = (nHeight + 1) / 2;
1542 const UINT32 oddY = 1;
1543 const UINT32 evenY = 0;
1544 const UINT32 oddX = 1;
1545 /* The auxiliary frame is aligned to multiples of 16x16.
1546 * We need the padded height for B4 and B5 conversion. */
1547 const UINT32 padHeight = nHeight + 16 - nHeight % 16;
1548 const BYTE* pSrc[3] = { pSrcRaw[0] + 1ULL * roi->top * srcStep[0] + roi->left,
1549 pSrcRaw[1] + 1ULL * roi->top / 2 * srcStep[1] + roi->left / 2,
1550 pSrcRaw[2] + 1ULL * roi->top / 2 * srcStep[2] + roi->left / 2 };
1551 BYTE* pDst[3] = { pDstRaw[0] + 1ULL * roi->top * dstStep[0] + roi->left,
1552 pDstRaw[1] + 1ULL * roi->top * dstStep[1] + roi->left,
1553 pDstRaw[2] + 1ULL * roi->top * dstStep[2] + roi->left };
1554 const __m128i zero = _mm_setzero_si128();
1555 const __m128i mask = _mm_set_epi8(0, (char)0x80, 0, (char)0x80, 0, (char)0x80, 0, (char)0x80, 0,
1556 (char)0x80, 0, (char)0x80, 0, (char)0x80, 0, (char)0x80);
1557
1558 /* The second half of U and V is a bit more tricky... */
1559 /* B4 and B5 */
1560 for (size_t y = 0; y < padHeight; y++)
1561 {
1562 const BYTE* Ya = pSrc[0] + 1ULL * srcStep[0] * y;
1563 BYTE* pX = nullptr;
1564
1565 if ((y) % mod < (mod + 1) / 2)
1566 {
1567 const UINT32 pos = (2 * uY++ + oddY);
1568
1569 if (pos >= nHeight)
1570 continue;
1571
1572 pX = pDst[1] + 1ULL * dstStep[1] * pos;
1573 }
1574 else
1575 {
1576 const UINT32 pos = (2 * vY++ + oddY);
1577
1578 if (pos >= nHeight)
1579 continue;
1580
1581 pX = pDst[2] + 1ULL * dstStep[2] * pos;
1582 }
1583
1584 if (y < nHeight)
1585 memcpy(pX, Ya, nWidth);
1586 }
1587
1588 /* B6 and B7 */
1589 for (size_t y = 0; y < halfHeight; y++)
1590 {
1591 const size_t val2y = (y * 2 + evenY);
1592 const BYTE* Ua = pSrc[1] + srcStep[1] * y;
1593 const BYTE* Va = pSrc[2] + srcStep[2] * y;
1594 BYTE* pU = pDst[1] + dstStep[1] * val2y;
1595 BYTE* pV = pDst[2] + dstStep[2] * val2y;
1596
1597 size_t x = 0;
1598 for (; x < halfWidth - halfPad; x += 16)
1599 {
1600 {
1601 const __m128i u = LOAD_SI128(&Ua[x]);
1602 const __m128i u2 = _mm_unpackhi_epi8(u, zero);
1603 const __m128i u1 = _mm_unpacklo_epi8(u, zero);
1604 /* the mask selects the even bytes of u1/u2, which carry the
1605 * samples; B6 goes to the odd columns 2x + 1 */
1606 _mm_maskmoveu_si128(u1, mask, (char*)&pU[2 * x + 1]);
1607 _mm_maskmoveu_si128(u2, mask, (char*)&pU[2 * x + 17]);
1608 }
1609 {
1610 const __m128i u = LOAD_SI128(&Va[x]);
1611 const __m128i u2 = _mm_unpackhi_epi8(u, zero);
1612 const __m128i u1 = _mm_unpacklo_epi8(u, zero);
1613 /* B7 likewise to the odd columns */
1614 _mm_maskmoveu_si128(u1, mask, (char*)&pV[2 * x + 1]);
1615 _mm_maskmoveu_si128(u2, mask, (char*)&pV[2 * x + 17]);
1616 }
1617 }
1618
1619 for (; x < halfWidth; x++)
1620 {
1621 const size_t val2x1 = (x * 2ULL + oddX);
1622 pU[val2x1] = Ua[x];
1623 pV[val2x1] = Va[x];
1624 }
1625 }
1626
1627 return PRIMITIVES_SUCCESS;
1628}
1629
1630static pstatus_t sse41_ChromaV2ToYUV444(const BYTE* WINPR_RESTRICT pSrc[3], const UINT32 srcStep[3],
1631 UINT32 nTotalWidth, WINPR_ATTR_UNUSED UINT32 nTotalHeight,
1632 BYTE* WINPR_RESTRICT pDst[3], const UINT32 dstStep[3],
1633 const RECTANGLE_16* WINPR_RESTRICT roi)
1634{
1635 const UINT32 nWidth = roi->right - roi->left;
1636 const UINT32 nHeight = roi->bottom - roi->top;
1637 const UINT32 halfWidth = (nWidth + 1) / 2;
1638 const UINT32 halfPad = halfWidth % 16;
1639 const UINT32 halfHeight = (nHeight + 1) / 2;
1640 const UINT32 quaterWidth = (nWidth + 3) / 4;
1641 const UINT32 quaterPad = quaterWidth % 16;
1642 const __m128i zero = _mm_setzero_si128();
1643 const __m128i mask = _mm_set_epi8((char)0x80, 0, (char)0x80, 0, (char)0x80, 0, (char)0x80, 0,
1644 (char)0x80, 0, (char)0x80, 0, (char)0x80, 0, (char)0x80, 0);
1645 const __m128i mask2 = _mm_set_epi8(0, (char)0x80, 0, (char)0x80, 0, (char)0x80, 0, (char)0x80,
1646 0, (char)0x80, 0, (char)0x80, 0, (char)0x80, 0, (char)0x80);
1647 const __m128i shuffle1 =
1648 _mm_set_epi8((char)0x80, 15, (char)0x80, 14, (char)0x80, 13, (char)0x80, 12, (char)0x80, 11,
1649 (char)0x80, 10, (char)0x80, 9, (char)0x80, 8);
1650 const __m128i shuffle2 =
1651 _mm_set_epi8((char)0x80, 7, (char)0x80, 6, (char)0x80, 5, (char)0x80, 4, (char)0x80, 3,
1652 (char)0x80, 2, (char)0x80, 1, (char)0x80, 0);
1653
1654 /* B4 and B5: odd UV values for width/2, height */
1655 for (size_t y = 0; y < nHeight; y++)
1656 {
1657 const size_t yTop = y + roi->top;
1658 const BYTE* pYaU = pSrc[0] + srcStep[0] * yTop + roi->left / 2;
1659 const BYTE* pYaV = pYaU + nTotalWidth / 2;
1660 BYTE* pU = pDst[1] + 1ULL * dstStep[1] * yTop + roi->left;
1661 BYTE* pV = pDst[2] + 1ULL * dstStep[2] * yTop + roi->left;
1662
1663 size_t x = 0;
1664 for (; x < halfWidth - halfPad; x += 16)
1665 {
1666 {
1667 const __m128i u = LOAD_SI128(&pYaU[x]);
1668 const __m128i u2 = _mm_unpackhi_epi8(zero, u);
1669 const __m128i u1 = _mm_unpacklo_epi8(zero, u);
1670 _mm_maskmoveu_si128(u1, mask, (char*)&pU[2 * x]);
1671 _mm_maskmoveu_si128(u2, mask, (char*)&pU[2 * x + 16]);
1672 }
1673 {
1674 const __m128i v = LOAD_SI128(&pYaV[x]);
1675 const __m128i v2 = _mm_unpackhi_epi8(zero, v);
1676 const __m128i v1 = _mm_unpacklo_epi8(zero, v);
1677 _mm_maskmoveu_si128(v1, mask, (char*)&pV[2 * x]);
1678 _mm_maskmoveu_si128(v2, mask, (char*)&pV[2 * x + 16]);
1679 }
1680 }
1681
1682 for (; x < halfWidth; x++)
1683 {
1684 const size_t odd = 2ULL * x + 1;
1685 pU[odd] = pYaU[x];
1686 pV[odd] = pYaV[x];
1687 }
1688 }
1689
1690 /* B6 - B9 */
1691 for (size_t y = 0; y < halfHeight; y++)
1692 {
1693 const BYTE* pUaU = pSrc[1] + srcStep[1] * (y + roi->top / 2) + roi->left / 4;
1694 const BYTE* pUaV = pUaU + nTotalWidth / 4;
1695 const BYTE* pVaU = pSrc[2] + srcStep[2] * (y + roi->top / 2) + roi->left / 4;
1696 const BYTE* pVaV = pVaU + nTotalWidth / 4;
1697 BYTE* pU = pDst[1] + dstStep[1] * (2 * y + 1 + roi->top) + roi->left;
1698 BYTE* pV = pDst[2] + dstStep[2] * (2 * y + 1 + roi->top) + roi->left;
1699
1700 UINT32 x = 0;
1701 for (; x < quaterWidth - quaterPad; x += 16)
1702 {
1703 {
1704 const __m128i uU = LOAD_SI128(&pUaU[x]);
1705 const __m128i uV = LOAD_SI128(&pVaU[x]);
1706 const __m128i uHigh = _mm_unpackhi_epi8(uU, uV);
1707 const __m128i uLow = _mm_unpacklo_epi8(uU, uV);
1708 const __m128i u1 = _mm_shuffle_epi8(uLow, shuffle2);
1709 const __m128i u2 = _mm_shuffle_epi8(uLow, shuffle1);
1710 const __m128i u3 = _mm_shuffle_epi8(uHigh, shuffle2);
1711 const __m128i u4 = _mm_shuffle_epi8(uHigh, shuffle1);
1712 _mm_maskmoveu_si128(u1, mask2, (char*)&pU[4 * x + 0]);
1713 _mm_maskmoveu_si128(u2, mask2, (char*)&pU[4 * x + 16]);
1714 _mm_maskmoveu_si128(u3, mask2, (char*)&pU[4 * x + 32]);
1715 _mm_maskmoveu_si128(u4, mask2, (char*)&pU[4 * x + 48]);
1716 }
1717 {
1718 const __m128i vU = LOAD_SI128(&pUaV[x]);
1719 const __m128i vV = LOAD_SI128(&pVaV[x]);
1720 const __m128i vHigh = _mm_unpackhi_epi8(vU, vV);
1721 const __m128i vLow = _mm_unpacklo_epi8(vU, vV);
1722 const __m128i v1 = _mm_shuffle_epi8(vLow, shuffle2);
1723 const __m128i v2 = _mm_shuffle_epi8(vLow, shuffle1);
1724 const __m128i v3 = _mm_shuffle_epi8(vHigh, shuffle2);
1725 const __m128i v4 = _mm_shuffle_epi8(vHigh, shuffle1);
1726 _mm_maskmoveu_si128(v1, mask2, (char*)&pV[4 * x + 0]);
1727 _mm_maskmoveu_si128(v2, mask2, (char*)&pV[4 * x + 16]);
1728 _mm_maskmoveu_si128(v3, mask2, (char*)&pV[4 * x + 32]);
1729 _mm_maskmoveu_si128(v4, mask2, (char*)&pV[4 * x + 48]);
1730 }
1731 }
1732
1733 for (; x < quaterWidth; x++)
1734 {
1735 pU[4 * x + 0] = pUaU[x];
1736 pV[4 * x + 0] = pUaV[x];
1737 pU[4 * x + 2] = pVaU[x];
1738 pV[4 * x + 2] = pVaV[x];
1739 }
1740 }
1741
1742 return PRIMITIVES_SUCCESS;
1743}
1744
1745static pstatus_t sse41_YUV420CombineToYUV444(avc444_frame_type type,
1746 const BYTE* WINPR_RESTRICT pSrc[3],
1747 const UINT32 srcStep[3], UINT32 nWidth, UINT32 nHeight,
1748 BYTE* WINPR_RESTRICT pDst[3], const UINT32 dstStep[3],
1749 const RECTANGLE_16* WINPR_RESTRICT roi)
1750{
1751 if (!pSrc || !pSrc[0] || !pSrc[1] || !pSrc[2])
1752 return -1;
1753
1754 if (!pDst || !pDst[0] || !pDst[1] || !pDst[2])
1755 return -1;
1756
1757 if (!roi)
1758 return -1;
1759
1760 switch (type)
1761 {
1762 case AVC444_LUMA:
1763 return sse41_LumaToYUV444(pSrc, srcStep, pDst, dstStep, roi);
1764
1765 case AVC444_CHROMAv1:
1766 return sse41_ChromaV1ToYUV444(pSrc, srcStep, pDst, dstStep, roi);
1767
1768 case AVC444_CHROMAv2:
1769 return sse41_ChromaV2ToYUV444(pSrc, srcStep, nWidth, nHeight, pDst, dstStep, roi);
1770
1771 default:
1772 return -1;
1773 }
1774}
1775#endif
1776
1777void primitives_init_YUV_sse41_int(primitives_t* WINPR_RESTRICT prims)
1778{
1779#if defined(SSE_AVX_INTRINSICS_ENABLED)
1780 generic = primitives_get_generic();
1781
1782 WLog_VRB(PRIM_TAG, "SSE3/sse41 optimizations");
1783 prims->RGBToYUV420_8u_P3AC4R = sse41_RGBToYUV420;
1784 prims->RGBToAVC444YUV = sse41_RGBToAVC444YUV;
1785 prims->RGBToAVC444YUVv2 = sse41_RGBToAVC444YUVv2;
1786 prims->YUV420ToRGB_8u_P3AC4R = sse41_YUV420ToRGB;
1787 prims->YUV444ToRGB_8u_P3AC4R = sse41_YUV444ToRGB_8u_P3AC4R;
1788 prims->I444ToRGB_8u = sse41_I444ToRGB_8u;
1789 prims->YUV420CombineToYUV444 = sse41_YUV420CombineToYUV444;
1790#else
1791 WLog_VRB(PRIM_TAG, "undefined WITH_SIMD or sse41 intrinsics not available");
1792 WINPR_UNUSED(prims);
1793#endif
1794}
RECTANGLE_16
Definition types.h:109
prim_size_t
Definition primitives.h:74
primitives_t
Definition primitives.h:255