Webcam/include/framework/picture/pict_yuv_tools.cpp

/* -----------------------------------------------------------------------------
	GSFramework
	Copyright 2001-2013 Emmanuel Julien. All Rights Reserved.
----------------------------------------------------------------------------- */


		#include "picture/pict.h"
	#if	__PLATFORM_WINDOWS__ && __MMX__
		#include "mmintrin.h"
	#endif

		using namespace GS;


//------------------------------------------------------------------------------
static		void mmx_yuv2rgb (uchar *py, uchar *pu, uchar *pv, uchar *image)
{
#if	__PLATFORM_WINDOWS__ && __MMX__
	static __m64	mmx_80w = {0x0080008000800080LL};
	static __m64	mmx_U_green = {0xf37df37df37df37dLL};
	static __m64	mmx_U_blue = {0x4093409340934093LL};
	static __m64	mmx_V_red = {0x3312331233123312LL};
	static __m64	mmx_V_green = {0xe5fce5fce5fce5fcLL};
	static __m64	mmx_10w = {0x1010101010101010LL};
	static __m64	mmx_00ffw = {0x00ff00ff00ff00ffLL};
	static __m64	mmx_Y_coeff = {0x253f253f253f253fLL};
	static __m64	mmx_A_unpack = {0xffffffffffffffffLL};

	__asm
	{
		push			esi
		pxor			mm4, mm4			; mm4 = 0

		mov				esi, pu
		movd			mm0, [esi]			; mm0 = 00 00 00 00 u3 u2 u1 u0
		mov				esi, pv
		movd			mm1, [esi]			; mm1 = 00 00 00 00 v3 v2 v1 v0
		mov				esi, py
		movq			mm6, [esi]			; mm6 = Y7 Y6 Y5 Y4 Y3 Y2 Y1 Y0

		; Multiply part of the conversion.
		punpcklbw		mm0, mm4			; mm0 = u3 u2 u1 u0
		punpcklbw		mm1, mm4			; mm1 = v3 v2 v1 v0
		psubsw			mm0, mmx_80w		; u -= 128
		psubsw			mm1, mmx_80w		; v -= 128
		psllw			mm0, 3				; promote precision
		psllw			mm1, 3				; promote precision
		movq			mm2, mm0			; mm2 = u3 u2 u1 u0
		movq			mm3, mm1			; mm3 = v3 v2 v1 v0
		pmulhw			mm2, mmx_U_green	; mm2 = u * u_green
		pmulhw			mm3, mmx_V_green	; mm3 = v * v_green
		pmulhw			mm0, mmx_U_blue		; mm0 = chroma_b
		pmulhw			mm1, mmx_V_red		; mm1 = chroma_r
		paddsw			mm2, mm3			; mm2 = chroma_g

		psubusb			mm6, mmx_10w		; Y -= 16
		movq			mm7, mm6			; mm7 = Y7 Y6 Y5 Y4 Y3 Y2 Y1 Y0
		pand			mm6, mmx_00ffw		; mm6 =    Y6    Y4    Y2    Y0
		psrlw			mm7, 8				; mm7 =    Y7    Y5    Y3    Y1
		psllw			mm6, 3				; promote precision
		psllw			mm7, 3				; promote precision
		pmulhw			mm6, mmx_Y_coeff	; mm6 = luma_rgb even
		pmulhw			mm7, mmx_Y_coeff	; mm7 = luma_rgb odd

		; Addition part of the conversion for even and odd pixels.
		movq			mm3, mm0			; mm3 = chroma_b
		movq			mm4, mm1			; mm4 = chroma_r
		movq			mm5, mm2			; mm5 = chroma_g
		paddsw			mm0, mm6			; mm0 = B6 B4 B2 B0
		paddsw			mm3, mm7			; mm3 = B7 B5 B3 B1
		paddsw			mm1, mm6			; mm1 = R6 R4 R2 R0
		paddsw			mm4, mm7			; mm4 = R7 R5 R3 R1
		paddsw			mm2, mm6			; mm2 = G6 G4 G2 G0
		paddsw			mm5, mm7			; mm5 = G7 G5 G3 G1
		packuswb		mm0, mm0			; saturate to 0-255
		packuswb		mm1, mm1			; saturate to 0-255
		packuswb		mm2, mm2			; saturate to 0-255
		packuswb		mm3, mm3			; saturate to 0-255
		packuswb		mm4, mm4			; saturate to 0-255
		packuswb		mm5, mm5			; saturate to 0-255
		punpcklbw		mm0, mm3			; mm0 = B7 B6 B5 B4 B3 B2 B1 B0
		punpcklbw		mm1, mm4			; mm1 = R7 R6 R5 R4 R3 R2 R1 R0
		punpcklbw		mm2, mm5			; mm2 = G7 G6 G5 G4 G3 G2 G1 G0

		mov				esi, image
		;pxor			mm3, mm3
		movq			mm3, mmx_A_unpack
		movq			mm6, mm0
		movq			mm7, mm1
		movq			mm4, mm0
		movq			mm5, mm1
		punpcklbw		mm6, mm2
		punpcklbw		mm7, mm3
		punpcklwd		mm6, mm7
		movq			[esi], mm6
		movq			mm6, mm0
		punpcklbw		mm6, mm2
		punpckhwd		mm6, mm7
		movq			[esi + 8], mm6
		punpckhbw		mm4, mm2
		punpckhbw		mm5, mm3
		punpcklwd		mm4, mm5
		movq			[esi + 16], mm4
		movq			mm4, mm0
		punpckhbw		mm4, mm2
		punpckhwd		mm4, mm5
		movq			[esi + 24], mm4
		pop				esi

		emms
	}
#endif
}
//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
void				Picture::YUV422toRGB32(uchar * const yuv_plane[3], uchar *rgb32, int width, int height, int dst_pitch, int dst_height)
{
	// Default pitch if none specified.
	if	(!dst_pitch)
		dst_pitch = width * 4;
	if	(!dst_height)
		dst_height = height;

	// Target resolution.
	int c_width = dst_pitch / 4;

	if	(width < c_width)
		c_width = width;
	if	(height < dst_height)
		dst_height = height;

	#pragma omp parallel for
	for (int y = 0; y < dst_height; ++y)
	{
		uchar	*p_out = rgb32 + dst_pitch * y,
				*p_yuv[3] =	{
								yuv_plane[0] + width * y,
								yuv_plane[1] + (width / 2) * (y / 2),
								yuv_plane[2] + (width / 2) * (y / 2)
							};

		// Convert scan line.
		for (int x = 0; x < c_width; x += 8)
		{
			mmx_yuv2rgb(p_yuv[0], p_yuv[1], p_yuv[2], p_out);

			p_yuv[0] += 8;
			p_yuv[1] += 4;
			p_yuv[2] += 4;
			p_out += 32;
		}
	}
}
void		Picture::UnpackYCbCr(uchar *y, uchar *cb, uchar *cr)
{
	if	(!y || !cb || !cr || (GetBpp() != 32))
		return;

	uchar		*pdata = GetData();
	for	(uint n = 0; n < (GetHeight() * GetWidth()); ++n)
	{
		float		r = (float)pdata[0], g = (float)pdata[1], b = (float)pdata[2];

		*y++  = (uchar)( 0.2990f * r + 0.5870f * g + 0.1140f * b + 0.5f);
		*cb++ = (uchar)(-0.1687f * r - 0.3313f * g + 0.5000f * b + 128.f + 0.5f);
		*cr++ = (uchar)( 0.5000f * r - 0.4187f * g - 0.0813f * b + 128.f + 0.5f);
		pdata += 4;
	}
}
void		Picture::PackYCbCr(uchar *y, uchar *cb, uchar *cr)
{
	if	(!y || !cb || !cr || (GetBpp() != 32))
		return;

	uchar *pdata = GetData();
	for	(uint n = 0; n < (GetHeight() * GetWidth()); ++n)
	{
		float _y = (float)*y++, _cb = ((float)*cb++) - 128.f, _cr = ((float)*cr++) - 128.f;

		float r = _y + 1.402f * _cr;
		float g = _y - 0.34414f * _cb -  0.71414f * _cr;
		float b = _y + 1.77200f * _cb;

				if	(r < 0)
				pdata[0] = 0;
		else	if	(r > 255.f)
				pdata[0] = 255;
		else	pdata[0] = (uchar)r;

				if	(g < 0)
				pdata[1] = 0;
		else	if	(g > 255.f)
				pdata[1] = 255;
		else	pdata[1] = (uchar)g;

				if	(b < 0)
				pdata[2] = 0;
		else	if	(b > 255.f)
				pdata[2] = 255;
		else	pdata[2] = (uchar)b;

		pdata += 4;
	}
}
//------------------------------------------------------------------------------