/*

    SDL - Simple DirectMedia Layer

    Copyright (C) 1997-2006 Sam Lantinga

    This library is free software; you can redistribute it and/or

    modify it under the terms of the GNU Lesser General Public

    License as published by the Free Software Foundation; either

    version 2.1 of the License, or (at your option) any later version.

    This library is distributed in the hope that it will be useful,

    but WITHOUT ANY WARRANTY; without even the implied warranty of

    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU

    Lesser General Public License for more details.

    You should have received a copy of the GNU Lesser General Public

    License along with this library; if not, write to the Free Software

    Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA

    Sam Lantinga

    slouken@libsdl.org

*/

#include "SDL_config.h"

#include "SDL_video.h"

#include "SDL_blit.h"

/*

  In Visual C, VC6 has mmintrin.h in the "Processor Pack" add-on.

   Checking if _mm_free is #defined in malloc.h is is the only way to

   determine if the Processor Pack is installed, as far as I can tell.

*/

#if SDL_ASSEMBLY_ROUTINES

#  if defined(__GNUC__) && (defined(__i386__) || defined(__x86_64__))

#    define MMX_ASMBLIT 1

#    define GCC_ASMBLIT 1

#  elif defined(_MSC_VER) && defined(_M_IX86)

#    if (_MSC_VER <= 1200)  

#      include <malloc.h>   

#      if defined(_mm_free)

#          define HAVE_MMINTRIN_H 1

#      endif

#    else  /* Visual Studio > VC6 always has mmintrin.h */

#      define HAVE_MMINTRIN_H 1

#    endif

#    if HAVE_MMINTRIN_H

#      define MMX_ASMBLIT 1

#      define MSVC_ASMBLIT 1

#    endif

#  endif

#endif /* SDL_ASSEMBLY_ROUTINES */

/* Function to check the CPU flags */

#include "SDL_cpuinfo.h"

#if GCC_ASMBLIT

#include "mmx.h"

#elif MSVC_ASMBLIT

#include <mmintrin.h>

#include <mm3dnow.h>

#endif

/* Functions to perform alpha blended blitting */

/* N->1 blending with per-surface alpha */

static void BlitNto1SurfaceAlpha(SDL_BlitInfo *info)

{

	int width = info->d_width;

	int height = info->d_height;

	Uint8 *src = info->s_pixels;

	int srcskip = info->s_skip;

	Uint8 *dst = info->d_pixels;

	int dstskip = info->d_skip;

	Uint8 *palmap = info->table;

	SDL_PixelFormat *srcfmt = info->src;

	SDL_PixelFormat *dstfmt = info->dst;

	int srcbpp = srcfmt->BytesPerPixel;

	const unsigned A = srcfmt->alpha;

	while ( height-- ) {

	    DUFFS_LOOP4(

	    {

		Uint32 Pixel;

		unsigned sR;

		unsigned sG;

		unsigned sB;

		unsigned dR;

		unsigned dG;

		unsigned dB;

		DISEMBLE_RGB(src, srcbpp, srcfmt, Pixel, sR, sG, sB);

		dR = dstfmt->palette->colors[*dst].r;

		dG = dstfmt->palette->colors[*dst].g;

		dB = dstfmt->palette->colors[*dst].b;

		ALPHA_BLEND(sR, sG, sB, A, dR, dG, dB);

		dR &= 0xff;

		dG &= 0xff;

		dB &= 0xff;

		/* Pack RGB into 8bit pixel */

		if ( palmap == NULL ) {

		    *dst =((dR>>5)<<(3+2))|

			  ((dG>>5)<<(2))|

			  ((dB>>6)<<(0));

		} else {

		    *dst = palmap[((dR>>5)<<(3+2))|

				  ((dG>>5)<<(2))  |

				  ((dB>>6)<<(0))];

		}

		dst++;

		src += srcbpp;

	    },

	    width);

	    src += srcskip;

	    dst += dstskip;

	}

}

/* N->1 blending with pixel alpha */

static void BlitNto1PixelAlpha(SDL_BlitInfo *info)

{

	int width = info->d_width;

	int height = info->d_height;

	Uint8 *src = info->s_pixels;

	int srcskip = info->s_skip;

	Uint8 *dst = info->d_pixels;

	int dstskip = info->d_skip;

	Uint8 *palmap = info->table;

	SDL_PixelFormat *srcfmt = info->src;

	SDL_PixelFormat *dstfmt = info->dst;

	int srcbpp = srcfmt->BytesPerPixel;

	/* FIXME: fix alpha bit field expansion here too? */

	while ( height-- ) {

	    DUFFS_LOOP4(

	    {

		Uint32 Pixel;

		unsigned sR;

		unsigned sG;

		unsigned sB;

		unsigned sA;

		unsigned dR;

		unsigned dG;

		unsigned dB;

		DISEMBLE_RGBA(src,srcbpp,srcfmt,Pixel,sR,sG,sB,sA);

		dR = dstfmt->palette->colors[*dst].r;

		dG = dstfmt->palette->colors[*dst].g;

		dB = dstfmt->palette->colors[*dst].b;

		ALPHA_BLEND(sR, sG, sB, sA, dR, dG, dB);

		dR &= 0xff;

		dG &= 0xff;

		dB &= 0xff;

		/* Pack RGB into 8bit pixel */

		if ( palmap == NULL ) {

		    *dst =((dR>>5)<<(3+2))|

			  ((dG>>5)<<(2))|

			  ((dB>>6)<<(0));

		} else {

		    *dst = palmap[((dR>>5)<<(3+2))|

				  ((dG>>5)<<(2))  |

				  ((dB>>6)<<(0))  ];

		}

		dst++;

		src += srcbpp;

	    },

	    width);

	    src += srcskip;

	    dst += dstskip;

	}

}

/* colorkeyed N->1 blending with per-surface alpha */

static void BlitNto1SurfaceAlphaKey(SDL_BlitInfo *info)

{

	int width = info->d_width;

	int height = info->d_height;

	Uint8 *src = info->s_pixels;

	int srcskip = info->s_skip;

	Uint8 *dst = info->d_pixels;

	int dstskip = info->d_skip;

	Uint8 *palmap = info->table;

	SDL_PixelFormat *srcfmt = info->src;

	SDL_PixelFormat *dstfmt = info->dst;

	int srcbpp = srcfmt->BytesPerPixel;

	Uint32 ckey = srcfmt->colorkey;

	const int A = srcfmt->alpha;

	while ( height-- ) {

	    DUFFS_LOOP(

	    {

		Uint32 Pixel;

		unsigned sR;

		unsigned sG;

		unsigned sB;

		unsigned dR;

		unsigned dG;

		unsigned dB;

		DISEMBLE_RGB(src, srcbpp, srcfmt, Pixel, sR, sG, sB);

		if ( Pixel != ckey ) {

		    dR = dstfmt->palette->colors[*dst].r;

		    dG = dstfmt->palette->colors[*dst].g;

		    dB = dstfmt->palette->colors[*dst].b;

		    ALPHA_BLEND(sR, sG, sB, A, dR, dG, dB);

		    dR &= 0xff;

		    dG &= 0xff;

		    dB &= 0xff;

		    /* Pack RGB into 8bit pixel */

		    if ( palmap == NULL ) {

			*dst =((dR>>5)<<(3+2))|

			      ((dG>>5)<<(2)) |

			      ((dB>>6)<<(0));

		    } else {

			*dst = palmap[((dR>>5)<<(3+2))|

				      ((dG>>5)<<(2))  |

				      ((dB>>6)<<(0))  ];

		    }

		}

		dst++;

		src += srcbpp;

	    },

	    width);

	    src += srcskip;

	    dst += dstskip;

	}

}

#if GCC_ASMBLIT

/* fast RGB888->(A)RGB888 blending with surface alpha=128 special case */

static void BlitRGBtoRGBSurfaceAlpha128MMX(SDL_BlitInfo *info)

{

	int width = info->d_width;

	int height = info->d_height;

	Uint32 *srcp = (Uint32 *)info->s_pixels;

	int srcskip = info->s_skip >> 2;

	Uint32 *dstp = (Uint32 *)info->d_pixels;

	int dstskip = info->d_skip >> 2;

	Uint32 dalpha = info->dst->Amask;

	Uint8 load[8];

	*(Uint64 *)load = 0x00fefefe00fefefeULL;/* alpha128 mask */

	movq_m2r(*load, mm4); /* alpha128 mask -> mm4 */

	*(Uint64 *)load = 0x0001010100010101ULL;/* !alpha128 mask */

	movq_m2r(*load, mm3); /* !alpha128 mask -> mm3 */

	movd_m2r(dalpha, mm7); /* dst alpha mask */

	punpckldq_r2r(mm7, mm7); /* dst alpha mask | dst alpha mask -> mm7 */

	while(height--) {

		DUFFS_LOOP_DOUBLE2(

		{

			Uint32 s = *srcp++;

			Uint32 d = *dstp;

			*dstp++ = ((((s & 0x00fefefe) + (d & 0x00fefefe)) >> 1)

				   + (s & d & 0x00010101)) | dalpha;

		},{

			movq_m2r((*dstp), mm2);/* 2 x dst -> mm2(ARGBARGB) */

			movq_r2r(mm2, mm6); /* 2 x dst -> mm6(ARGBARGB) */

			movq_m2r((*srcp), mm1);/* 2 x src -> mm1(ARGBARGB) */

			movq_r2r(mm1, mm5); /* 2 x src -> mm5(ARGBARGB) */

			pand_r2r(mm4, mm6); /* dst & mask -> mm6 */

			pand_r2r(mm4, mm5); /* src & mask -> mm5 */

			paddd_r2r(mm6, mm5); /* mm6 + mm5 -> mm5 */

			pand_r2r(mm1, mm2); /* src & dst -> mm2 */

			psrld_i2r(1, mm5); /* mm5 >> 1 -> mm5 */

			pand_r2r(mm3, mm2); /* mm2 & !mask -> mm2 */

			paddd_r2r(mm5, mm2); /* mm5 + mm2 -> mm2 */

			por_r2r(mm7, mm2); /* mm7(full alpha) | mm2 -> mm2 */

			movq_r2m(mm2, (*dstp));/* mm2 -> 2 x dst pixels */

			dstp += 2;

			srcp += 2;

		}, width);

		srcp += srcskip;

		dstp += dstskip;

	}

	emms();

}

/* fast RGB888->(A)RGB888 blending with surface alpha */

static void BlitRGBtoRGBSurfaceAlphaMMX(SDL_BlitInfo *info)

{

	SDL_PixelFormat* df = info->dst;

	unsigned alpha = info->src->alpha;

	if (alpha == 128 && (df->Rmask | df->Gmask | df->Bmask) == 0x00FFFFFF) {

			/* only call a128 version when R,G,B occupy lower bits */

		BlitRGBtoRGBSurfaceAlpha128MMX(info);

	} else {

		int width = info->d_width;

		int height = info->d_height;

		Uint32 *srcp = (Uint32 *)info->s_pixels;

		int srcskip = info->s_skip >> 2;

		Uint32 *dstp = (Uint32 *)info->d_pixels;

		int dstskip = info->d_skip >> 2;

		pxor_r2r(mm5, mm5); /* 0 -> mm5 */

		/* form the alpha mult */

		movd_m2r(alpha, mm4); /* 0000000A -> mm4 */

		punpcklwd_r2r(mm4, mm4); /* 00000A0A -> mm4 */

		punpckldq_r2r(mm4, mm4); /* 0A0A0A0A -> mm4 */

		alpha = (0xff << df->Rshift) | (0xff << df->Gshift) | (0xff << df->Bshift);

		movd_m2r(alpha, mm0); /* 00000FFF -> mm0 */

		punpcklbw_r2r(mm0, mm0); /* 00FFFFFF -> mm0 */

		pand_r2r(mm0, mm4); /* 0A0A0A0A -> mm4, minus 1 chan */

			/* at this point mm4 can be 000A0A0A or 0A0A0A00 or another combo */

		movd_m2r(df->Amask, mm7); /* dst alpha mask */

		punpckldq_r2r(mm7, mm7); /* dst alpha mask | dst alpha mask -> mm7 */

		while(height--) {

			DUFFS_LOOP_DOUBLE2({

				/* One Pixel Blend */

				movd_m2r((*srcp), mm1);/* src(ARGB) -> mm1 (0000ARGB)*/

				movd_m2r((*dstp), mm2);/* dst(ARGB) -> mm2 (0000ARGB)*/

				punpcklbw_r2r(mm5, mm1); /* 0A0R0G0B -> mm1(src) */

				punpcklbw_r2r(mm5, mm2); /* 0A0R0G0B -> mm2(dst) */

				psubw_r2r(mm2, mm1);/* src - dst -> mm1 */

				pmullw_r2r(mm4, mm1); /* mm1 * alpha -> mm1 */

				psrlw_i2r(8, mm1); /* mm1 >> 8 -> mm1 */

				paddb_r2r(mm1, mm2); /* mm1 + mm2(dst) -> mm2 */

				packuswb_r2r(mm5, mm2);  /* ARGBARGB -> mm2 */

				por_r2r(mm7, mm2); /* mm7(full alpha) | mm2 -> mm2 */

				movd_r2m(mm2, *dstp);/* mm2 -> pixel */

				++srcp;

				++dstp;

			},{

				/* Two Pixels Blend */

				movq_m2r((*srcp), mm0);/* 2 x src -> mm0(ARGBARGB)*/

				movq_m2r((*dstp), mm2);/* 2 x dst -> mm2(ARGBARGB) */

				movq_r2r(mm0, mm1); /* 2 x src -> mm1(ARGBARGB) */

				movq_r2r(mm2, mm6); /* 2 x dst -> mm6(ARGBARGB) */

				punpcklbw_r2r(mm5, mm0); /* low - 0A0R0G0B -> mm0(src1) */

				punpckhbw_r2r(mm5, mm1); /* high - 0A0R0G0B -> mm1(src2) */

				punpcklbw_r2r(mm5, mm2); /* low - 0A0R0G0B -> mm2(dst1) */

				punpckhbw_r2r(mm5, mm6); /* high - 0A0R0G0B -> mm6(dst2) */

				psubw_r2r(mm2, mm0);/* src1 - dst1 -> mm0 */

				pmullw_r2r(mm4, mm0); /* mm0 * alpha -> mm0 */

				psrlw_i2r(8, mm0); /* mm0 >> 8 -> mm1 */

				paddb_r2r(mm0, mm2); /* mm0 + mm2(dst1) -> mm2 */

				psubw_r2r(mm6, mm1);/* src2 - dst2 -> mm1 */

				pmullw_r2r(mm4, mm1); /* mm1 * alpha -> mm1 */

				psrlw_i2r(8, mm1); /* mm1 >> 8 -> mm1 */

				paddb_r2r(mm1, mm6); /* mm1 + mm6(dst2) -> mm6 */

				packuswb_r2r(mm6, mm2);  /* ARGBARGB -> mm2 */

				por_r2r(mm7, mm2); /* mm7(dst alpha) | mm2 -> mm2 */

				movq_r2m(mm2, *dstp);/* mm2 -> 2 x pixel */

  				srcp += 2;

  				dstp += 2;

  			}, width);

			srcp += srcskip;

			dstp += dstskip;

		}

		emms();

	}

}

/* fast ARGB888->(A)RGB888 blending with pixel alpha */

static void BlitRGBtoRGBPixelAlphaMMX(SDL_BlitInfo *info)

{

	int width = info->d_width;

	int height = info->d_height;

	Uint32 *srcp = (Uint32 *)info->s_pixels;

	int srcskip = info->s_skip >> 2;

	Uint32 *dstp = (Uint32 *)info->d_pixels;

	int dstskip = info->d_skip >> 2;

	SDL_PixelFormat* sf = info->src;

	Uint32 amask = sf->Amask;

	pxor_r2r(mm6, mm6); /* 0 -> mm6 */

	/* form multiplication mask */

	movd_m2r(sf->Amask, mm7); /* 0000F000 -> mm7 */

	punpcklbw_r2r(mm7, mm7); /* FF000000 -> mm7 */

	pcmpeqb_r2r(mm0, mm0); /* FFFFFFFF -> mm0 */

	movq_r2r(mm0, mm3); /* FFFFFFFF -> mm3 (for later) */

	pxor_r2r(mm0, mm7); /* 00FFFFFF -> mm7 (mult mask) */

	/* form channel masks */

	movq_r2r(mm7, mm0); /* 00FFFFFF -> mm0 */

	packsswb_r2r(mm6, mm0); /* 00000FFF -> mm0 (channel mask) */

	packsswb_r2r(mm6, mm3); /* 0000FFFF -> mm3 */

	pxor_r2r(mm0, mm3); /* 0000F000 -> mm3 (~channel mask) */

	/* get alpha channel shift */

	__asm__ __volatile__ (

		"movd %0, %%mm5"

		: : "rm" ((Uint32) sf->Ashift) ); /* Ashift -> mm5 */

	while(height--) {

	    DUFFS_LOOP4({

		Uint32 alpha = *srcp & amask;

		/* FIXME: Here we special-case opaque alpha since the

			compositioning used (>>8 instead of /255) doesn't handle

			it correctly. Also special-case alpha=0 for speed?

			Benchmark this! */

		if(alpha == 0) {

			/* do nothing */

		} else if(alpha == amask) {

			/* opaque alpha -- copy RGB, keep dst alpha */

			/* using MMX here to free up regular registers for other things */

			movd_m2r((*srcp), mm1);/* src(ARGB) -> mm1 (0000ARGB)*/

			movd_m2r((*dstp), mm2);/* dst(ARGB) -> mm2 (0000ARGB)*/

			pand_r2r(mm0, mm1); /* src & chanmask -> mm1 */

			pand_r2r(mm3, mm2); /* dst & ~chanmask -> mm2 */

			por_r2r(mm1, mm2); /* src | dst -> mm2 */

			movd_r2m(mm2, (*dstp)); /* mm2 -> dst */

		} else {

			movd_m2r((*srcp), mm1);/* src(ARGB) -> mm1 (0000ARGB)*/

			punpcklbw_r2r(mm6, mm1); /* 0A0R0G0B -> mm1 */

			movd_m2r((*dstp), mm2);/* dst(ARGB) -> mm2 (0000ARGB)*/

			punpcklbw_r2r(mm6, mm2); /* 0A0R0G0B -> mm2 */

			__asm__ __volatile__ (

				"movd %0, %%mm4"

				: : "r" (alpha) ); /* 0000A000 -> mm4 */

			psrld_r2r(mm5, mm4); /* mm4 >> mm5 -> mm4 (0000000A) */

			punpcklwd_r2r(mm4, mm4); /* 00000A0A -> mm4 */

			punpcklwd_r2r(mm4, mm4); /* 0A0A0A0A -> mm4 */

			pand_r2r(mm7, mm4); /* 000A0A0A -> mm4, preserve dst alpha on add */

			/* blend */		    

			psubw_r2r(mm2, mm1);/* src - dst -> mm1 */

			pmullw_r2r(mm4, mm1); /* mm1 * alpha -> mm1 */

			psrlw_i2r(8, mm1); /* mm1 >> 8 -> mm1(000R0G0B) */

			paddb_r2r(mm1, mm2); /* mm1 + mm2(dst) -> mm2 */

			packuswb_r2r(mm6, mm2);  /* 0000ARGB -> mm2 */

			movd_r2m(mm2, *dstp);/* mm2 -> dst */

		}

		++srcp;

		++dstp;

	    }, width);

	    srcp += srcskip;

	    dstp += dstskip;

	}

	emms();

}

/* End GCC_ASMBLIT */

#elif MSVC_ASMBLIT

/* fast RGB888->(A)RGB888 blending with surface alpha=128 special case */

static void BlitRGBtoRGBSurfaceAlpha128MMX(SDL_BlitInfo *info)

{

	int width = info->d_width;

	int height = info->d_height;

	Uint32 *srcp = (Uint32 *)info->s_pixels;

	int srcskip = info->s_skip >> 2;

	Uint32 *dstp = (Uint32 *)info->d_pixels;

	int dstskip = info->d_skip >> 2;

	Uint32 dalpha = info->dst->Amask;

	__m64 src1, src2, dst1, dst2, lmask, hmask, dsta;

	hmask = _mm_set_pi32(0x00fefefe, 0x00fefefe); /* alpha128 mask -> hmask */

	lmask = _mm_set_pi32(0x00010101, 0x00010101); /* !alpha128 mask -> lmask */

	dsta = _mm_set_pi32(dalpha, dalpha); /* dst alpha mask -> dsta */

	while (height--) {

		int n = width;

		if ( n & 1 ) {

			Uint32 s = *srcp++;

			Uint32 d = *dstp;

			*dstp++ = ((((s & 0x00fefefe) + (d & 0x00fefefe)) >> 1)

				   + (s & d & 0x00010101)) | dalpha;

			n--;

		}

		for (n >>= 1; n > 0; --n) {

			dst1 = *(__m64*)dstp; /* 2 x dst -> dst1(ARGBARGB) */

			dst2 = dst1;   /* 2 x dst -> dst2(ARGBARGB) */

			src1 = *(__m64*)srcp; /* 2 x src -> src1(ARGBARGB) */

			src2 = src1; /* 2 x src -> src2(ARGBARGB) */

			dst2 = _mm_and_si64(dst2, hmask); /* dst & mask -> dst2 */

			src2 = _mm_and_si64(src2, hmask); /* src & mask -> src2 */

			src2 = _mm_add_pi32(src2, dst2); /* dst2 + src2 -> src2 */

			src2 = _mm_srli_pi32(src2, 1); /* src2 >> 1 -> src2 */

			dst1 = _mm_and_si64(dst1, src1); /* src & dst -> dst1 */

			dst1 = _mm_and_si64(dst1, lmask); /* dst1 & !mask -> dst1 */

			dst1 = _mm_add_pi32(dst1, src2); /* src2 + dst1 -> dst1 */

			dst1 = _mm_or_si64(dst1, dsta); /* dsta(full alpha) | dst1 -> dst1 */

			*(__m64*)dstp = dst1; /* dst1 -> 2 x dst pixels */

			dstp += 2;

			srcp += 2;

		}

		srcp += srcskip;

		dstp += dstskip;

	}

	_mm_empty();

}

/* fast RGB888->(A)RGB888 blending with surface alpha */

static void BlitRGBtoRGBSurfaceAlphaMMX(SDL_BlitInfo *info)

{

	SDL_PixelFormat* df = info->dst;

	Uint32 chanmask = df->Rmask | df->Gmask | df->Bmask;

	unsigned alpha = info->src->alpha;

	if (alpha == 128 && (df->Rmask | df->Gmask | df->Bmask) == 0x00FFFFFF) {

			/* only call a128 version when R,G,B occupy lower bits */

		BlitRGBtoRGBSurfaceAlpha128MMX(info);

	} else {

		int width = info->d_width;

		int height = info->d_height;

		Uint32 *srcp = (Uint32 *)info->s_pixels;

		int srcskip = info->s_skip >> 2;

		Uint32 *dstp = (Uint32 *)info->d_pixels;

		int dstskip = info->d_skip >> 2;

		Uint32 dalpha = df->Amask;

		Uint32 amult;

		__m64 src1, src2, dst1, dst2, mm_alpha, mm_zero, dsta;

		mm_zero = _mm_setzero_si64(); /* 0 -> mm_zero */

		/* form the alpha mult */

		amult = alpha | (alpha << 8);

		amult = amult | (amult << 16);

		chanmask = (0xff << df->Rshift) | (0xff << df->Gshift) | (0xff << df->Bshift);

		mm_alpha = _mm_set_pi32(0, amult & chanmask); /* 0000AAAA -> mm_alpha, minus 1 chan */

		mm_alpha = _mm_unpacklo_pi8(mm_alpha, mm_zero); /* 0A0A0A0A -> mm_alpha, minus 1 chan */

			/* at this point mm_alpha can be 000A0A0A or 0A0A0A00 or another combo */