/**

 * FreeRDP: A Remote Desktop Protocol Implementation

 * Optimized YUV/RGB conversion operations

 *

 * Copyright 2014 Thomas Erbesdobler

 * Copyright 2016-2017 Armin Novak <armin.novak@thincast.com>

 * Copyright 2016-2017 Norbert Federa <norbert.federa@thincast.com>

 * Copyright 2016-2017 Thincast Technologies GmbH

 *

 * Licensed under the Apache License, Version 2.0 (the "License");

 * you may not use this file except in compliance with the License.

 * You may obtain a copy of the License at

 *

 *     http://www.apache.org/licenses/LICENSE-2.0

 *

 * Unless required by applicable law or agreed to in writing, software

 * distributed under the License is distributed on an "AS IS" BASIS,

 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.

 * See the License for the specific language governing permissions and

 * limitations under the License.

 */

#ifdef HAVE_CONFIG_H

#include "config.h"

#endif

#include <winpr/sysinfo.h>

#include <winpr/crt.h>

#include <freerdp/types.h>

#include <freerdp/primitives.h>

#include "prim_internal.h"

#include <emmintrin.h>

#include <tmmintrin.h>

#if !defined(WITH_SSE2)

#error "This file needs WITH_SSE2 enabled!"

#endif

static primitives_t* generic = NULL;

/****************************************************************************/

/* SSSE3 YUV420 -> RGB conversion                                           */

/****************************************************************************/

static __m128i* ssse3_YUV444Pixel(__m128i* dst, __m128i Yraw, __m128i Uraw, __m128i Vraw, UINT8 pos)

{

	/* Visual Studio 2010 doesn't like _mm_set_epi32 in array initializer list */

	/* Note: This also applies to Visual Studio 2013 before Update 4 */

#if !defined(_MSC_VER) || (_MSC_VER > 1600)

	const __m128i mapY[] =

	{

		_mm_set_epi32(0x80800380, 0x80800280, 0x80800180, 0x80800080),

		_mm_set_epi32(0x80800780, 0x80800680, 0x80800580, 0x80800480),

		_mm_set_epi32(0x80800B80, 0x80800A80, 0x80800980, 0x80800880),

		_mm_set_epi32(0x80800F80, 0x80800E80, 0x80800D80, 0x80800C80)

	};

	const __m128i mapUV[] =

	{

		_mm_set_epi32(0x80038002, 0x80018000, 0x80808080, 0x80808080),

		_mm_set_epi32(0x80078006, 0x80058004, 0x80808080, 0x80808080),

		_mm_set_epi32(0x800B800A, 0x80098008, 0x80808080, 0x80808080),

		_mm_set_epi32(0x800F800E, 0x800D800C, 0x80808080, 0x80808080)

	};

	const __m128i mask[] =

	{

		_mm_set_epi32(0x80038080, 0x80028080, 0x80018080, 0x80008080),

		_mm_set_epi32(0x80800380, 0x80800280, 0x80800180, 0x80800080),

		_mm_set_epi32(0x80808003, 0x80808002, 0x80808001, 0x80808000)

	};

#else

	/* Note: must be in little-endian format ! */

	const __m128i mapY[] =

	{

		{ 0x80, 0x00, 0x80, 0x80, 0x80, 0x01, 0x80, 0x80, 0x80, 0x02, 0x80, 0x80, 0x80, 0x03, 0x80, 0x80 },

		{ 0x80, 0x04, 0x80, 0x80, 0x80, 0x05, 0x80, 0x80, 0x80, 0x06, 0x80, 0x80, 0x80, 0x07, 0x80, 0x80 },

		{ 0x80, 0x08, 0x80, 0x80, 0x80, 0x09, 0x80, 0x80, 0x80, 0x0a, 0x80, 0x80, 0x80, 0x0b, 0x80, 0x80 },

		{ 0x80, 0x0c, 0x80, 0x80, 0x80, 0x0d, 0x80, 0x80, 0x80, 0x0e, 0x80, 0x80, 0x80, 0x0f, 0x80, 0x80 }

	};

	const __m128i mapUV[] =

	{

		{ 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x00, 0x80, 0x01, 0x80, 0x02, 0x80, 0x03, 0x80 },

		{ 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x04, 0x80, 0x05, 0x80, 0x06, 0x80, 0x07, 0x80 },

		{ 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x08, 0x80, 0x09, 0x80, 0x0a, 0x80, 0x0b, 0x80 },

		{ 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x0c, 0x80, 0x0d, 0x80, 0x0e, 0x80, 0x0f, 0x80 }

	};

	const __m128i mask[] =

	{

		{ 0x80, 0x80, 0x00, 0x80, 0x80, 0x80, 0x01, 0x80, 0x80, 0x80, 0x02, 0x80, 0x80, 0x80, 0x03, 0x80 },

		{ 0x80, 0x00, 0x80, 0x80, 0x80, 0x01, 0x80, 0x80, 0x80, 0x02, 0x80, 0x80, 0x80, 0x03, 0x80, 0x80 },

		{ 0x00, 0x80, 0x80, 0x80, 0x01, 0x80, 0x80, 0x80, 0x02, 0x80, 0x80, 0x80, 0x03, 0x80, 0x80, 0x80 }

	};

#endif

	const __m128i c128 = _mm_set1_epi16(128);

	__m128i BGRX = _mm_set_epi32(0xFF000000, 0xFF000000, 0xFF000000, 0xFF000000);

	{

		__m128i C, D, E;

		/* Load Y values and expand to 32 bit */

		{

			C = _mm_shuffle_epi8(Yraw, mapY[pos]); /* Reorder and multiply by 256 */

		}

		/* Load U values and expand to 32 bit */

		{

			const __m128i U = _mm_shuffle_epi8(Uraw, mapUV[pos]); /* Reorder dcba */

			D = _mm_sub_epi16(U, c128); /* D = U - 128 */

		}

		/* Load V values and expand to 32 bit */

		{

			const __m128i V = _mm_shuffle_epi8(Vraw, mapUV[pos]); /* Reorder dcba */

			E = _mm_sub_epi16(V, c128); /* E = V - 128 */

		}

		/* Get the R value */

		{

			const __m128i c403 = _mm_set1_epi16(403);

			const __m128i e403 = _mm_unpackhi_epi16(_mm_mullo_epi16(E, c403), _mm_mulhi_epi16(E, c403));

			const __m128i Rs = _mm_add_epi32(C, e403);

			const __m128i R32 = _mm_srai_epi32(Rs, 8);

			const __m128i R16 = _mm_packs_epi32(R32, _mm_setzero_si128());

			const __m128i R = _mm_packus_epi16(R16, _mm_setzero_si128());

			const __m128i packed = _mm_shuffle_epi8(R, mask[0]);

			BGRX = _mm_or_si128(BGRX, packed);

		}

		/* Get the G value */

		{

			const __m128i c48 = _mm_set1_epi16(48);

			const __m128i d48 = _mm_unpackhi_epi16(_mm_mullo_epi16(D, c48), _mm_mulhi_epi16(D, c48));

			const __m128i c120 = _mm_set1_epi16(120);

			const __m128i e120 = _mm_unpackhi_epi16(_mm_mullo_epi16(E, c120), _mm_mulhi_epi16(E, c120));

			const __m128i de = _mm_add_epi32(d48, e120);

			const __m128i Gs = _mm_sub_epi32(C, de);

			const __m128i G32 = _mm_srai_epi32(Gs, 8);

			const __m128i G16 = _mm_packs_epi32(G32, _mm_setzero_si128());

			const __m128i G = _mm_packus_epi16(G16, _mm_setzero_si128());

			const __m128i packed = _mm_shuffle_epi8(G, mask[1]);

			BGRX = _mm_or_si128(BGRX, packed);

		}

		/* Get the B value */

		{

			const __m128i c475 = _mm_set1_epi16(475);

			const __m128i d475 = _mm_unpackhi_epi16(_mm_mullo_epi16(D, c475), _mm_mulhi_epi16(D, c475));

			const __m128i Bs = _mm_add_epi32(C, d475);

			const __m128i B32 = _mm_srai_epi32(Bs, 8);

			const __m128i B16 = _mm_packs_epi32(B32, _mm_setzero_si128());

			const __m128i B = _mm_packus_epi16(B16, _mm_setzero_si128());

			const __m128i packed = _mm_shuffle_epi8(B, mask[2]);

			BGRX = _mm_or_si128(BGRX, packed);

		}

	}

	_mm_storeu_si128(dst++, BGRX);

	return dst;

}

static pstatus_t ssse3_YUV420ToRGB_BGRX(

    const BYTE** pSrc, const UINT32* srcStep,

    BYTE* pDst, UINT32 dstStep,

    const prim_size_t* roi)

{

	const UINT32 nWidth = roi->width;

	const UINT32 nHeight = roi->height;

	const UINT32 pad = roi->width % 16;

	const __m128i duplicate = _mm_set_epi8(7, 7, 6, 6, 5, 5, 4, 4, 3, 3, 2, 2, 1, 1, 0, 0);

	UINT32 y;

	for (y = 0; y < nHeight; y++)

	{

		UINT32 x;

		__m128i* dst = (__m128i*)(pDst + dstStep * y);

		const BYTE* YData = pSrc[0] + y * srcStep[0];

		const BYTE* UData = pSrc[1] + (y / 2) * srcStep[1];

		const BYTE* VData = pSrc[2] + (y / 2) * srcStep[2];

		for (x = 0; x < nWidth - pad; x += 16)

		{

			const __m128i Y = _mm_loadu_si128((__m128i*)YData);

			const __m128i uRaw = _mm_loadu_si128((__m128i*)UData);

			const __m128i vRaw = _mm_loadu_si128((__m128i*)VData);

			const __m128i U = _mm_shuffle_epi8(uRaw, duplicate);

			const __m128i V = _mm_shuffle_epi8(vRaw, duplicate);

			YData += 16;

			UData += 8;

			VData += 8;

			dst = ssse3_YUV444Pixel(dst, Y, U, V, 0);

			dst = ssse3_YUV444Pixel(dst, Y, U, V, 1);

			dst = ssse3_YUV444Pixel(dst, Y, U, V, 2);

			dst = ssse3_YUV444Pixel(dst, Y, U, V, 3);

		}

		for (x = 0; x < pad; x++)

		{

			const BYTE Y = *YData++;

			const BYTE U = *UData;

			const BYTE V = *VData;

			const BYTE r = YUV2R(Y, U, V);

			const BYTE g = YUV2G(Y, U, V);

			const BYTE b = YUV2B(Y, U, V);

			dst = (__m128i*)writePixelBGRX((BYTE*)dst, 4, PIXEL_FORMAT_BGRX32, r, g, b, 0xFF);

			if (x % 2)

			{

				UData++;

				VData++;

			}

		}

	}

	return PRIMITIVES_SUCCESS;

}

static pstatus_t ssse3_YUV420ToRGB(

    const BYTE** pSrc, const UINT32* srcStep,

    BYTE* pDst, UINT32 dstStep, UINT32 DstFormat,

    const prim_size_t* roi)

{

	switch (DstFormat)

	{

		case PIXEL_FORMAT_BGRX32:

		case PIXEL_FORMAT_BGRA32:

			return ssse3_YUV420ToRGB_BGRX(pSrc, srcStep, pDst, dstStep, roi);

		default:

			return generic->YUV420ToRGB_8u_P3AC4R(pSrc, srcStep, pDst, dstStep, DstFormat, roi);

	}

}

static pstatus_t ssse3_YUV444ToRGB_8u_P3AC4R_BGRX(

    const BYTE** pSrc, const UINT32* srcStep,

    BYTE* pDst, UINT32 dstStep,

    const prim_size_t* roi)

{

	const UINT32 nWidth = roi->width;

	const UINT32 nHeight = roi->height;

	const UINT32 pad = roi->width % 16;

	UINT32 y;

	for (y = 0; y < nHeight; y++)

	{

		UINT32 x;

		__m128i* dst = (__m128i*)(pDst + dstStep * y);

		const BYTE* YData = pSrc[0] + y * srcStep[0];

		const BYTE* UData = pSrc[1] + y * srcStep[1];

		const BYTE* VData = pSrc[2] + y * srcStep[2];

		for (x = 0; x < nWidth - pad; x += 16)

		{

			__m128i Y = _mm_load_si128((__m128i*)YData);

			__m128i U = _mm_load_si128((__m128i*)UData);

			__m128i V = _mm_load_si128((__m128i*)VData);

			YData += 16;

			UData += 16;

			VData += 16;

			dst = ssse3_YUV444Pixel(dst, Y, U, V, 0);

			dst = ssse3_YUV444Pixel(dst, Y, U, V, 1);

			dst = ssse3_YUV444Pixel(dst, Y, U, V, 2);

			dst = ssse3_YUV444Pixel(dst, Y, U, V, 3);

		}

		for (x = 0; x < pad; x++)

		{

			const BYTE Y = *YData++;

			const BYTE U = *UData++;

			const BYTE V = *VData++;

			const BYTE r = YUV2R(Y, U, V);

			const BYTE g = YUV2G(Y, U, V);

			const BYTE b = YUV2B(Y, U, V);

			dst = (__m128i*)writePixelBGRX((BYTE*)dst, 4, PIXEL_FORMAT_BGRX32, r, g, b, 0xFF);

		}

	}

	return PRIMITIVES_SUCCESS;

}

static pstatus_t ssse3_YUV444ToRGB_8u_P3AC4R(const BYTE** pSrc, const UINT32* srcStep,

        BYTE* pDst, UINT32 dstStep, UINT32 DstFormat,

        const prim_size_t* roi)

{

	if ((unsigned long)pSrc[0] % 16 || (unsigned long)pSrc[1] % 16 || (unsigned long)pSrc[2] % 16 ||

	    srcStep[0] % 16 || srcStep[1] % 16 || srcStep[2] % 16)

		return generic->YUV444ToRGB_8u_P3AC4R(pSrc, srcStep, pDst, dstStep, DstFormat, roi);

	switch (DstFormat)

	{

		case PIXEL_FORMAT_BGRX32:

		case PIXEL_FORMAT_BGRA32:

			return ssse3_YUV444ToRGB_8u_P3AC4R_BGRX(pSrc, srcStep, pDst, dstStep, roi);

		default:

			return generic->YUV444ToRGB_8u_P3AC4R(pSrc, srcStep, pDst, dstStep, DstFormat, roi);

	}

}

/****************************************************************************/

/* SSSE3 RGB -> YUV420 conversion                                          **/

/****************************************************************************/

/**

 * Note (nfedera):

 * The used forward transformation factors from RGB to YUV are based on the

 * values specified in [Rec. ITU-R BT.709-6] Section 3:

 * http://www.itu.int/rec/R-REC-BT.709-6-201506-I/en

 *

 * Y =  0.21260 * R + 0.71520 * G + 0.07220 * B +   0;

 * U = -0.11457 * R - 0.38543 * G + 0.50000 * B + 128;

 * V =  0.50000 * R - 0.45415 * G - 0.04585 * B + 128;

 *

 * The most accurate integer arithmetic approximation when using 8-bit signed

 * integer factors with 16-bit signed integer intermediate results is:

 *

 * Y = ( ( 27 * R + 92 * G +  9 * B) >> 7 );

 * U = ( (-29 * R - 99 * G + 128 * B) >> 8 ) + 128;

 * V = ( ( 128 * R - 116 * G -  12 * B) >> 8 ) + 128;

 *

 * Due to signed 8bit range being [-128,127] the U and V constants of 128 are

 * rounded to 127

 */

#define BGRX_Y_FACTORS _mm_set_epi8(0, 27, 92, 9, 0, 27, 92, 9, 0, 27, 92, 9, 0, 27, 92, 9)

#define BGRX_U_FACTORS _mm_set_epi8(0, -29, -99, 127, 0, -29, -99, 127, 0, -29, -99, 127, 0, -29, -99, 127)

#define BGRX_V_FACTORS _mm_set_epi8(0, 127, -116, -12, 0, 127, -116, -12, 0, 127, -116, -12, 0, 127, -116, -12)

#define CONST128_FACTORS _mm_set1_epi8(-128)

#define Y_SHIFT 7

#define U_SHIFT 8

#define V_SHIFT 8

/*

TODO:

RGB[AX] can simply be supported using the following factors. And instead of loading the

globals directly the functions below could be passed pointers to the correct vectors

depending on the source picture format.

PRIM_ALIGN_128 static const BYTE rgbx_y_factors[] = {

      27,  92,   9,   0,  27,  92,   9,   0,  27,  92,   9,   0,  27,  92,   9,   0

};

PRIM_ALIGN_128 static const BYTE rgbx_u_factors[] = {

     -15, -49,  64,   0, -15, -49,  64,   0, -15, -49,  64,   0, -15, -49,  64,   0

};

PRIM_ALIGN_128 static const BYTE rgbx_v_factors[] = {

      64, -58,  -6,   0,  64, -58,  -6,   0,  64, -58,  -6,   0,  64, -58,  -6,   0

};

*/

/* compute the luma (Y) component from a single rgb source line */

static INLINE void ssse3_RGBToYUV420_BGRX_Y(

    const BYTE* src, BYTE* dst, UINT32 width)

{

	UINT32 x;

	__m128i x0, x1, x2, x3;

	const __m128i y_factors = BGRX_Y_FACTORS;

	const __m128i* argb = (const __m128i*) src;

	__m128i* ydst = (__m128i*) dst;

	for (x = 0; x < width; x += 16)

	{

		/* store 16 rgba pixels in 4 128 bit registers */

		x0 = _mm_load_si128(argb++); // 1st 4 pixels

		x1 = _mm_load_si128(argb++); // 2nd 4 pixels

		x2 = _mm_load_si128(argb++); // 3rd 4 pixels

		x3 = _mm_load_si128(argb++); // 4th 4 pixels

		/* multiplications and subtotals */

		x0 = _mm_maddubs_epi16(x0, y_factors);

		x1 = _mm_maddubs_epi16(x1, y_factors);

		x2 = _mm_maddubs_epi16(x2, y_factors);

		x3 = _mm_maddubs_epi16(x3, y_factors);

		/* the total sums */

		x0 = _mm_hadd_epi16(x0, x1);

		x2 = _mm_hadd_epi16(x2, x3);

		/* shift the results */

		x0 = _mm_srli_epi16(x0, Y_SHIFT);

		x2 = _mm_srli_epi16(x2, Y_SHIFT);

		/* pack the 16 words into bytes */

		x0 = _mm_packus_epi16(x0, x2);

		/* save to y plane */

		_mm_storeu_si128(ydst++, x0);

	}

}

/* compute the chrominance (UV) components from two rgb source lines */

static INLINE void ssse3_RGBToYUV420_BGRX_UV(

    const BYTE* src1, const BYTE* src2,

    BYTE* dst1, BYTE* dst2, UINT32 width)

{

	UINT32 x;

	const __m128i u_factors = BGRX_U_FACTORS;

	const __m128i v_factors = BGRX_V_FACTORS;

	const __m128i vector128 = CONST128_FACTORS;

	__m128i x0, x1, x2, x3, x4, x5;

	const __m128i* rgb1 = (const __m128i*)src1;

	const __m128i* rgb2 = (const __m128i*)src2;

	__m64* udst = (__m64*)dst1;

	__m64* vdst = (__m64*)dst2;

	for (x = 0; x < width; x += 16)

	{

		/* subsample 16x2 pixels into 16x1 pixels */

		x0 = _mm_load_si128(rgb1++);

		x4 = _mm_load_si128(rgb2++);

		x0 = _mm_avg_epu8(x0, x4);

		x1 = _mm_load_si128(rgb1++);

		x4 = _mm_load_si128(rgb2++);

		x1 = _mm_avg_epu8(x1, x4);

		x2 = _mm_load_si128(rgb1++);

		x4 = _mm_load_si128(rgb2++);

		x2 = _mm_avg_epu8(x2, x4);

		x3 = _mm_load_si128(rgb1++);

		x4 = _mm_load_si128(rgb2++);

		x3 = _mm_avg_epu8(x3, x4);

		/* subsample these 16x1 pixels into 8x1 pixels */

		/**

		 * shuffle controls

		 * c = a[0],a[2],b[0],b[2] == 10 00 10 00 = 0x88

		 * c = a[1],a[3],b[1],b[3] == 11 01 11 01 = 0xdd

		 */

		x4 = _mm_castps_si128(_mm_shuffle_ps(_mm_castsi128_ps(x0), _mm_castsi128_ps(x1), 0x88));

		x0 = _mm_castps_si128(_mm_shuffle_ps(_mm_castsi128_ps(x0), _mm_castsi128_ps(x1), 0xdd));

		x0 = _mm_avg_epu8(x0, x4);

		x4 = _mm_castps_si128(_mm_shuffle_ps(_mm_castsi128_ps(x2), _mm_castsi128_ps(x3), 0x88));

		x1 = _mm_castps_si128(_mm_shuffle_ps(_mm_castsi128_ps(x2), _mm_castsi128_ps(x3), 0xdd));

		x1 = _mm_avg_epu8(x1, x4);

		/* multiplications and subtotals */

		x2 = _mm_maddubs_epi16(x0, u_factors);

		x3 = _mm_maddubs_epi16(x1, u_factors);

		x4 = _mm_maddubs_epi16(x0, v_factors);

		x5 = _mm_maddubs_epi16(x1, v_factors);

		/* the total sums */

		x0 = _mm_hadd_epi16(x2, x3);

		x1 = _mm_hadd_epi16(x4, x5);

		/* shift the results */

		x0 = _mm_srai_epi16(x0, U_SHIFT);

		x1 = _mm_srai_epi16(x1, V_SHIFT);

		/* pack the 16 words into bytes */

		x0 = _mm_packs_epi16(x0, x1);

		/* add 128 */

		x0 = _mm_sub_epi8(x0, vector128);

		/* the lower 8 bytes go to the u plane */

		_mm_storel_pi(udst++, _mm_castsi128_ps(x0));

		/* the upper 8 bytes go to the v plane */

		_mm_storeh_pi(vdst++, _mm_castsi128_ps(x0));

	}

}

static pstatus_t ssse3_RGBToYUV420_BGRX(

    const BYTE* pSrc, UINT32 srcFormat, UINT32 srcStep,

    BYTE* pDst[3], UINT32 dstStep[3],

    const prim_size_t* roi)

{

	UINT32 y;

	const BYTE* argb = pSrc;

	BYTE* ydst = pDst[0];

	BYTE* udst = pDst[1];

	BYTE* vdst = pDst[2];

	if (roi->height < 1 || roi->width < 1)

	{

		return !PRIMITIVES_SUCCESS;

	}

	if (roi->width % 16 || (unsigned long)pSrc % 16 || srcStep % 16)

	{

		return generic->RGBToYUV420_8u_P3AC4R(pSrc, srcFormat, srcStep, pDst, dstStep, roi);

	}

	for (y = 0; y < roi->height - 1; y += 2)

	{

		const BYTE* line1 = argb;

		const BYTE* line2 = argb + srcStep;

		ssse3_RGBToYUV420_BGRX_UV(line1, line2, udst, vdst, roi->width);

		ssse3_RGBToYUV420_BGRX_Y(line1, ydst, roi->width);

		ssse3_RGBToYUV420_BGRX_Y(line2, ydst + dstStep[0], roi->width);

		argb += 2 * srcStep;

		ydst += 2 * dstStep[0];

		udst += 1 * dstStep[1];

		vdst += 1 * dstStep[2];

	}

	if (roi->height & 1)

	{

		/* pass the same last line of an odd height twice for UV */

		ssse3_RGBToYUV420_BGRX_UV(argb, argb, udst, vdst, roi->width);

		ssse3_RGBToYUV420_BGRX_Y(argb, ydst, roi->width);

	}

	return PRIMITIVES_SUCCESS;

}

static pstatus_t ssse3_RGBToYUV420(

    const BYTE* pSrc, UINT32 srcFormat, UINT32 srcStep,

    BYTE* pDst[3], UINT32 dstStep[3],

    const prim_size_t* roi)

{

	switch (srcFormat)

	{

		case PIXEL_FORMAT_BGRX32:

		case PIXEL_FORMAT_BGRA32:

			return ssse3_RGBToYUV420_BGRX(pSrc, srcFormat, srcStep, pDst, dstStep, roi);

		default:

			return generic->RGBToYUV420_8u_P3AC4R(pSrc, srcFormat, srcStep, pDst, dstStep, roi);

	}

}

/****************************************************************************/

/* SSSE3 RGB -> AVC444-YUV conversion                                      **/

/****************************************************************************/

static INLINE void ssse3_RGBToAVC444YUV_BGRX_DOUBLE_ROW(

    const BYTE* srcEven, const BYTE* srcOdd, BYTE* b1Even, BYTE* b1Odd, BYTE* b2,

    BYTE* b3, BYTE* b4, BYTE* b5, BYTE* b6, BYTE* b7, UINT32 width)

{

	UINT32 x;

	const __m128i* argbEven = (const __m128i*) srcEven;

	const __m128i* argbOdd = (const __m128i*) srcOdd;

	const __m128i y_factors = BGRX_Y_FACTORS;

	const __m128i u_factors = BGRX_U_FACTORS;

	const __m128i v_factors = BGRX_V_FACTORS;

	const __m128i vector128 = CONST128_FACTORS;

	for (x = 0; x < width; x += 16)

	{

		/* store 16 rgba pixels in 4 128 bit registers */

		const __m128i xe1 = _mm_load_si128(argbEven++); // 1st 4 pixels

		const __m128i xe2 = _mm_load_si128(argbEven++); // 2nd 4 pixels

		const __m128i xe3 = _mm_load_si128(argbEven++); // 3rd 4 pixels

		const __m128i xe4 = _mm_load_si128(argbEven++); // 4th 4 pixels

		const __m128i xo1 = _mm_load_si128(argbOdd++); // 1st 4 pixels

		const __m128i xo2 = _mm_load_si128(argbOdd++); // 2nd 4 pixels

		const __m128i xo3 = _mm_load_si128(argbOdd++); // 3rd 4 pixels

		const __m128i xo4 = _mm_load_si128(argbOdd++); // 4th 4 pixels

		{

			/* Y: multiplications with subtotals and horizontal sums */

			const __m128i ye1 = _mm_srli_epi16(_mm_hadd_epi16(_mm_maddubs_epi16(xe1, y_factors),

			                                   _mm_maddubs_epi16(xe2, y_factors)), Y_SHIFT);

			const __m128i ye2 = _mm_srli_epi16(_mm_hadd_epi16(_mm_maddubs_epi16(xe3, y_factors),

			                                   _mm_maddubs_epi16(xe4, y_factors)), Y_SHIFT);

			const __m128i ye = _mm_packus_epi16(ye1, ye2);

			const __m128i yo1 = _mm_srli_epi16(_mm_hadd_epi16(_mm_maddubs_epi16(xo1, y_factors),

			                                   _mm_maddubs_epi16(xo2, y_factors)), Y_SHIFT);

			const __m128i yo2 = _mm_srli_epi16(_mm_hadd_epi16(_mm_maddubs_epi16(xo3, y_factors),

			                                   _mm_maddubs_epi16(xo4, y_factors)), Y_SHIFT);

			const __m128i yo = _mm_packus_epi16(yo1, yo2);

			/* store y [b1] */

			_mm_storeu_si128((__m128i*)b1Even, ye);

			b1Even += 16;

			if (b1Odd)

			{

				_mm_storeu_si128((__m128i*)b1Odd, yo);

				b1Odd += 16;

			}

		}

		{

			/* We have now

			   * 16 even U values in ue

			   * 16 odd U values in uo

			   *

			   * We need to split these according to

			   * 3.3.8.3.2 YUV420p Stream Combination for YUV444 mode */

			__m128i ue, uo;

			{

				const __m128i ue1 = _mm_srai_epi16(_mm_hadd_epi16(_mm_maddubs_epi16(xe1, u_factors),

				                                   _mm_maddubs_epi16(xe2, u_factors)), U_SHIFT);

				const __m128i ue2 = _mm_srai_epi16(_mm_hadd_epi16(_mm_maddubs_epi16(xe3, u_factors),

				                                   _mm_maddubs_epi16(xe4, u_factors)), U_SHIFT);

				ue = _mm_sub_epi8(_mm_packs_epi16(ue1, ue2), vector128);

			}

			if (b1Odd)

			{

				const __m128i uo1 = _mm_srai_epi16(_mm_hadd_epi16(_mm_maddubs_epi16(xo1, u_factors),

				                                   _mm_maddubs_epi16(xo2, u_factors)), U_SHIFT);

				const __m128i uo2 = _mm_srai_epi16(_mm_hadd_epi16(_mm_maddubs_epi16(xo3, u_factors),

				                                   _mm_maddubs_epi16(xo4, u_factors)), U_SHIFT);

				uo = _mm_sub_epi8(_mm_packs_epi16(uo1, uo2), vector128);

			}

			/* Now we need the following storage distribution:

			 * 2x   2y    -> b2

			 * x    2y+1  -> b4

			 * 2x+1 2y    -> b6 */

			if (b1Odd) /* b2 */

			{

				const __m128i ueh = _mm_unpackhi_epi8(ue, _mm_setzero_si128());

				const __m128i uoh = _mm_unpackhi_epi8(uo, _mm_setzero_si128());

				const __m128i hi = _mm_add_epi16(ueh, uoh);

				const __m128i uel = _mm_unpacklo_epi8(ue, _mm_setzero_si128());

				const __m128i uol = _mm_unpacklo_epi8(uo, _mm_setzero_si128());

				const __m128i lo = _mm_add_epi16(uel, uol);

				const __m128i added = _mm_hadd_epi16(lo, hi);

				const __m128i avg16 = _mm_srai_epi16(added, 2);

				const __m128i avg = _mm_packus_epi16(avg16, avg16);

				_mm_storel_epi64((__m128i*)b2, avg);

			}

			else

			{

				const __m128i mask = _mm_set_epi8(0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80,

				                                  14, 12, 10, 8, 6, 4, 2, 0);

				const __m128i ud = _mm_shuffle_epi8(ue, mask);

				_mm_storel_epi64((__m128i*)b2, ud);

			}

			b2 += 8;

			if (b1Odd) /* b4 */

			{

				_mm_store_si128((__m128i*)b4, uo);

				b4 += 16;

			}

			{

				/* b6 */

				const __m128i mask = _mm_set_epi8(0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80,

				                                  15, 13, 11, 9, 7, 5, 3, 1);

				const __m128i ude = _mm_shuffle_epi8(ue, mask);

				_mm_storel_epi64((__m128i*)b6, ude);

				b6 += 8;

			}

		}

		{

			/* We have now

			   * 16 even V values in ue

			   * 16 odd V values in uo

			   *

			   * We need to split these according to

			   * 3.3.8.3.2 YUV420p Stream Combination for YUV444 mode */

			__m128i ve, vo;

			{

				const __m128i ve1 = _mm_srai_epi16(_mm_hadd_epi16(_mm_maddubs_epi16(xe1, v_factors),

				                                   _mm_maddubs_epi16(xe2, v_factors)), V_SHIFT);

				const __m128i ve2 = _mm_srai_epi16(_mm_hadd_epi16(_mm_maddubs_epi16(xe3, v_factors),

				                                   _mm_maddubs_epi16(xe4, v_factors)), V_SHIFT);

				ve = _mm_sub_epi8(_mm_packs_epi16(ve1, ve2), vector128);

			}

			if (b1Odd)

			{

				const __m128i vo1 = _mm_srai_epi16(_mm_hadd_epi16(_mm_maddubs_epi16(xo1, v_factors),

				                                   _mm_maddubs_epi16(xo2, v_factors)), V_SHIFT);

				const __m128i vo2 = _mm_srai_epi16(_mm_hadd_epi16(_mm_maddubs_epi16(xo3, v_factors),

				                                   _mm_maddubs_epi16(xo4, v_factors)), V_SHIFT);

				vo = _mm_sub_epi8(_mm_packs_epi16(vo1, vo2), vector128);

			}

			/* Now we need the following storage distribution:

			 * 2x   2y    -> b3

			 * x    2y+1  -> b5

			 * 2x+1 2y    -> b7 */

			if (b1Odd) /* b3 */

			{

				const __m128i veh = _mm_unpackhi_epi8(ve, _mm_setzero_si128());

				const __m128i voh = _mm_unpackhi_epi8(vo, _mm_setzero_si128());

				const __m128i hi = _mm_add_epi16(veh, voh);

				const __m128i vel = _mm_unpacklo_epi8(ve, _mm_setzero_si128());

				const __m128i vol = _mm_unpacklo_epi8(vo, _mm_setzero_si128());

				const __m128i lo = _mm_add_epi16(vel, vol);

				const __m128i added = _mm_hadd_epi16(lo, hi);

				const __m128i avg16 = _mm_srai_epi16(added, 2);

				const __m128i avg = _mm_packus_epi16(avg16, avg16);

				_mm_storel_epi64((__m128i*)b3, avg);

			}

			else

			{

				const __m128i mask = _mm_set_epi8(0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80,

				                                  14, 12, 10, 8, 6, 4, 2, 0);

				const __m128i vd = _mm_shuffle_epi8(ve, mask);

				_mm_storel_epi64((__m128i*)b3, vd);

			}

			b3 += 8;

			if (b1Odd) /* b5 */

			{

				_mm_store_si128((__m128i*)b5, vo);

				b5 += 16;

			}

			{

				/* b7 */

				const __m128i mask = _mm_set_epi8(0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80,

				                                  15, 13, 11, 9, 7, 5, 3, 1);

				const __m128i vde = _mm_shuffle_epi8(ve, mask);

				_mm_storel_epi64((__m128i*)b7, vde);

				b7 += 8;

			}

		}

	}

}

static pstatus_t ssse3_RGBToAVC444YUV_BGRX(

    const BYTE* pSrc, UINT32 srcFormat, UINT32 srcStep,

    BYTE* pDst1[3], const UINT32 dst1Step[3],

    BYTE* pDst2[3], const UINT32 dst2Step[3],

    const prim_size_t* roi)

{

	UINT32 y;

	const BYTE* pMaxSrc = pSrc + (roi->height - 1) * srcStep;

	if (roi->height < 1 || roi->width < 1)

		return !PRIMITIVES_SUCCESS;

	if (roi->width % 16 || (unsigned long)pSrc % 16 || srcStep % 16)

		return generic->RGBToAVC444YUV(pSrc, srcFormat, srcStep, pDst1, dst1Step, pDst2, dst2Step, roi);

	for (y = 0; y < roi->height; y += 2)

	{

		const BOOL last = (y >= (roi->height - 1));

		const BYTE* srcEven = y < roi->height ? pSrc + y * srcStep : pMaxSrc;

		const BYTE* srcOdd = !last ? pSrc + (y + 1) * srcStep : pMaxSrc;

		const UINT32 i = y >> 1;

		const UINT32 n = (i & ~7) + i;

		BYTE* b1Even = pDst1[0] + y * dst1Step[0];

		BYTE* b1Odd = !last ? (b1Even + dst1Step[0]) : NULL;

		BYTE* b2 = pDst1[1] + (y / 2) * dst1Step[1];

		BYTE* b3 = pDst1[2] + (y / 2) * dst1Step[2];

		BYTE* b4 = pDst2[0] + dst2Step[0] * n;

		BYTE* b5 = b4 + 8 * dst2Step[0];

		BYTE* b6 = pDst2[1] + (y / 2) * dst2Step[1];

		BYTE* b7 = pDst2[2] + (y / 2) * dst2Step[2];

		ssse3_RGBToAVC444YUV_BGRX_DOUBLE_ROW(srcEven, srcOdd, b1Even, b1Odd, b2, b3, b4, b5, b6, b7,

		                                     roi->width);

	}

	return PRIMITIVES_SUCCESS;

}

static pstatus_t ssse3_RGBToAVC444YUV(

    const BYTE* pSrc, UINT32 srcFormat, UINT32 srcStep,

    BYTE* pDst1[3], const UINT32 dst1Step[3],

    BYTE* pDst2[3], const UINT32 dst2Step[3],

    const prim_size_t* roi)

{

	switch (srcFormat)

	{

		case PIXEL_FORMAT_BGRX32:

		case PIXEL_FORMAT_BGRA32:

			return ssse3_RGBToAVC444YUV_BGRX(pSrc, srcFormat, srcStep, pDst1, dst1Step, pDst2, dst2Step, roi);

		default:

			return generic->RGBToAVC444YUV(pSrc, srcFormat, srcStep, pDst1, dst1Step, pDst2, dst2Step, roi);

	}

}

/* Mapping of arguments:

 *

 * b1 [even lines] -> yLumaDstEven

 * b1 [odd lines]  -> yLumaDstOdd

 * b2              -> uLumaDst

 * b3              -> vLumaDst

 * b4              -> yChromaDst1

 * b5              -> yChromaDst2

 * b6              -> uChromaDst1

 * b7              -> uChromaDst2

 * b8              -> vChromaDst1

 * b9              -> vChromaDst2

 */

static INLINE void ssse3_RGBToAVC444YUVv2_BGRX_DOUBLE_ROW(

    const BYTE* srcEven, const BYTE* srcOdd,

    BYTE* yLumaDstEven, BYTE* yLumaDstOdd,

    BYTE* uLumaDst, BYTE* vLumaDst,

    BYTE* yEvenChromaDst1, BYTE* yEvenChromaDst2,

    BYTE* yOddChromaDst1, BYTE* yOddChromaDst2,

    BYTE* uChromaDst1, BYTE* uChromaDst2,

    BYTE* vChromaDst1, BYTE* vChromaDst2,

    UINT32 width)

{

	UINT32 x;

	const __m128i vector128 = CONST128_FACTORS;

	const __m128i* argbEven = (const __m128i*) srcEven;

	const __m128i* argbOdd = (const __m128i*) srcOdd;

	for (x = 0; x < width; x += 16)

	{

		/* store 16 rgba pixels in 4 128 bit registers

		 * for even and odd rows.

		 */

		const __m128i xe1 = _mm_load_si128(argbEven++); /* 1st 4 pixels */

		const __m128i xe2 = _mm_load_si128(argbEven++); /* 2nd 4 pixels */

		const __m128i xe3 = _mm_load_si128(argbEven++); /* 3rd 4 pixels */

		const __m128i xe4 = _mm_load_si128(argbEven++); /* 4th 4 pixels */

		const __m128i xo1 = _mm_load_si128(argbOdd++); /* 1st 4 pixels */

		const __m128i xo2 = _mm_load_si128(argbOdd++); /* 2nd 4 pixels */

		const __m128i xo3 = _mm_load_si128(argbOdd++); /* 3rd 4 pixels */

		const __m128i xo4 = _mm_load_si128(argbOdd++); /* 4th 4 pixels */

		{

			/* Y: multiplications with subtotals and horizontal sums */

			const __m128i y_factors = BGRX_Y_FACTORS;

			const __m128i ye1 = _mm_srli_epi16(_mm_hadd_epi16(_mm_maddubs_epi16(xe1, y_factors),

			                                   _mm_maddubs_epi16(xe2, y_factors)), Y_SHIFT);

			const __m128i ye2 = _mm_srli_epi16(_mm_hadd_epi16(_mm_maddubs_epi16(xe3, y_factors),

			                                   _mm_maddubs_epi16(xe4, y_factors)), Y_SHIFT);

			const __m128i ye = _mm_packus_epi16(ye1, ye2);

			/* store y [b1] */

			_mm_storeu_si128((__m128i*)yLumaDstEven, ye);

			yLumaDstEven += 16;

		}

		if (yLumaDstOdd)

		{

			const __m128i y_factors = BGRX_Y_FACTORS;

			const __m128i yo1 = _mm_srli_epi16(_mm_hadd_epi16(_mm_maddubs_epi16(xo1, y_factors),

			                                   _mm_maddubs_epi16(xo2, y_factors)), Y_SHIFT);

			const __m128i yo2 = _mm_srli_epi16(_mm_hadd_epi16(_mm_maddubs_epi16(xo3, y_factors),

			                                   _mm_maddubs_epi16(xo4, y_factors)), Y_SHIFT);

			const __m128i yo = _mm_packus_epi16(yo1, yo2);

			_mm_storeu_si128((__m128i*)yLumaDstOdd, yo);

			yLumaDstOdd += 16;

		}

		{

			/* We have now

			   * 16 even U values in ue

			   * 16 odd U values in uo

			   *

			   * We need to split these according to

			   * 3.3.8.3.3 YUV420p Stream Combination for YUV444v2 mode */

			/* U: multiplications with subtotals and horizontal sums */

			__m128i ue, uo, uavg;

			{

				const __m128i u_factors = BGRX_U_FACTORS;

				const __m128i ue1 = _mm_srai_epi16(_mm_hadd_epi16(_mm_maddubs_epi16(xe1, u_factors),

				                                   _mm_maddubs_epi16(xe2, u_factors)), U_SHIFT);

				const __m128i ue2 = _mm_srai_epi16(_mm_hadd_epi16(_mm_maddubs_epi16(xe3, u_factors),

				                                   _mm_maddubs_epi16(xe4, u_factors)), U_SHIFT);

				const __m128i ueavg = _mm_hadd_epi16(ue1, ue2);

				ue = _mm_sub_epi8(_mm_packs_epi16(ue1, ue2), vector128);

				uavg = ueavg;

			}

			{

				const __m128i u_factors = BGRX_U_FACTORS;

				const __m128i uo1 = _mm_srai_epi16(_mm_hadd_epi16(_mm_maddubs_epi16(xo1, u_factors),

				                                   _mm_maddubs_epi16(xo2, u_factors)), U_SHIFT);

				const __m128i uo2 = _mm_srai_epi16(_mm_hadd_epi16(_mm_maddubs_epi16(xo3, u_factors),

				                                   _mm_maddubs_epi16(xo4, u_factors)), U_SHIFT);

				const __m128i uoavg = _mm_hadd_epi16(uo1, uo2);

				uo = _mm_sub_epi8(_mm_packs_epi16(uo1, uo2), vector128);

				uavg = _mm_add_epi16(uavg, uoavg);

				uavg = _mm_srai_epi16(uavg, 2);

				uavg = _mm_packs_epi16(uavg, uoavg);

				uavg = _mm_sub_epi8(uavg, vector128);

			}

			/* Now we need the following storage distribution:

			 * 2x   2y    -> uLumaDst

			 * 2x+1  y    -> yChromaDst1

			 * 4x   2y+1  -> uChromaDst1

			 * 4x+2 2y+1  -> vChromaDst1 */

			{

				const __m128i mask = _mm_set_epi8(0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80,

				                                  15, 13, 11, 9, 7, 5, 3, 1);

				const __m128i ude = _mm_shuffle_epi8(ue, mask);

				_mm_storel_epi64((__m128i*)yEvenChromaDst1, ude);

				yEvenChromaDst1 += 8;

			}

			if (yLumaDstOdd)

			{

				const __m128i mask = _mm_set_epi8(0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80,

				                                  15, 13, 11, 9, 7, 5, 3, 1);

				const __m128i udo = _mm_shuffle_epi8(uo, mask);

				_mm_storel_epi64((__m128i*)yOddChromaDst1, udo);

				yOddChromaDst1 += 8;

			}

			if (yLumaDstOdd)

			{

				const __m128i mask = _mm_set_epi8(0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80,

				                                  14, 10, 6, 2, 12, 8, 4, 0);

				const __m128i ud = _mm_shuffle_epi8(uo, mask);

				int* uDst1 = (int*)uChromaDst1;

				int* vDst1 = (int*)vChromaDst1;

				const int* src = (const int*)&ud;

				_mm_stream_si32(uDst1, src[0]);

				_mm_stream_si32(vDst1, src[1]);

				uChromaDst1 += 4;

				vChromaDst1 += 4;

			}