pixman/pixman-sse2.c

   1 /*
   2  * Copyright © 2008 Rodrigo Kumpera
   3  * Copyright © 2008 André Tupinambá
   4  *
   5  * Permission to use, copy, modify, distribute, and sell this software and its
   6  * documentation for any purpose is hereby granted without fee, provided that
   7  * the above copyright notice appear in all copies and that both that
   8  * copyright notice and this permission notice appear in supporting
   9  * documentation, and that the name of Red Hat not be used in advertising or
  10  * publicity pertaining to distribution of the software without specific,
  11  * written prior permission.  Red Hat makes no representations about the
  12  * suitability of this software for any purpose.  It is provided "as is"
  13  * without express or implied warranty.
  14  *
  15  * THE COPYRIGHT HOLDERS DISCLAIM ALL WARRANTIES WITH REGARD TO THIS
  16  * SOFTWARE, INCLUDING ALL IMPLIED WARRANTIES OF MERCHANTABILITY AND
  17  * FITNESS, IN NO EVENT SHALL THE COPYRIGHT HOLDERS BE LIABLE FOR ANY
  18  * SPECIAL, INDIRECT OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
  19  * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN
  20  * AN ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING
  21  * OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS
  22  * SOFTWARE.
  23  *
  24  * Author:  Rodrigo Kumpera (kumpera@gmail.com)
  25  *          André Tupinambá (andrelrt@gmail.com)
  26  *
  27  * Based on work by Owen Taylor and Søren Sandmann
  28  */
  29 #ifdef HAVE_CONFIG_H
  30 #include <config.h>
  31 #endif
  32
  33 #include <mmintrin.h>
  34 #include <xmmintrin.h> /* for _mm_shuffle_pi16 and _MM_SHUFFLE */
  35 #include <emmintrin.h> /* for SSE2 intrinsics */
  36
  37 #include "pixman-sse2.h"
  38
  39 #ifdef USE_SSE2
  40
  41 #ifdef _MSC_VER
  42 #undef inline
  43 #define inline __forceinline
  44 #endif
  45
  46 #ifdef __GNUC__
  47 #    define inline __inline__ __attribute__ ((__always_inline__))
  48 #endif
  49
  50 /* -------------------------------------------------------------------------------------------------
  51  * Locals
  52  */
  53
  54 static __m64 xMask0080;
  55 static __m64 xMask00ff;
  56 static __m64 xMask0101;
  57 static __m64 xMaskAlpha;
  58
  59 static __m64 xMask565rgb;
  60 static __m64 xMask565Unpack;
  61
  62 static __m128i Mask0080;
  63 static __m128i Mask00ff;
  64 static __m128i Mask0101;
  65 static __m128i Maskffff;
  66 static __m128i Maskff000000;
  67 static __m128i MaskAlpha;
  68
  69 static __m128i Mask565r;
  70 static __m128i Mask565g1, Mask565g2;
  71 static __m128i Mask565b;
  72 static __m128i MaskRed;
  73 static __m128i MaskGreen;
  74 static __m128i MaskBlue;
  75
  76 static __m128i Mask565FixRB;
  77 static __m128i Mask565FixG;
  78
  79 /* -------------------------------------------------------------------------------------------------
  80  * SSE2 Inlines
  81  */
  82 static inline __m128i
  83 unpack_32_1x128 (uint32_t data)
  84 {
  85     return _mm_unpacklo_epi8 (_mm_cvtsi32_si128 (data), _mm_setzero_si128());
  86 }
  87
  88 static inline void
  89 unpack_128_2x128 (__m128i data, __m128i* dataLo, __m128i* dataHi)
  90 {
  91     *dataLo = _mm_unpacklo_epi8 (data, _mm_setzero_si128 ());
  92     *dataHi = _mm_unpackhi_epi8 (data, _mm_setzero_si128 ());
  93 }
  94
  95 static inline __m128i
  96 unpack565to8888 (__m128i lo)
  97 {
  98     __m128i r, g, b, rb, t;
  99
 100     r = _mm_and_si128 (_mm_slli_epi32 (lo, 8), MaskRed);
 101     g = _mm_and_si128 (_mm_slli_epi32 (lo, 5), MaskGreen);
 102     b = _mm_and_si128 (_mm_slli_epi32 (lo, 3), MaskBlue);
 103
 104     rb = _mm_or_si128 (r, b);
 105     t  = _mm_and_si128 (rb, Mask565FixRB);
 106     t  = _mm_srli_epi32 (t, 5);
 107     rb = _mm_or_si128 (rb, t);
 108
 109     t  = _mm_and_si128 (g, Mask565FixG);
 110     t  = _mm_srli_epi32 (t, 6);
 111     g  = _mm_or_si128 (g, t);
 112
 113     return _mm_or_si128 (rb, g);
 114 }
 115
 116 static inline void
 117 unpack565_128_4x128 (__m128i data, __m128i* data0, __m128i* data1, __m128i* data2, __m128i* data3)
 118 {
 119     __m128i lo, hi;
 120
 121     lo = _mm_unpacklo_epi16 (data, _mm_setzero_si128 ());
 122     hi = _mm_unpackhi_epi16 (data, _mm_setzero_si128 ());
 123
 124     lo = unpack565to8888 (lo);
 125     hi = unpack565to8888 (hi);
 126
 127     unpack_128_2x128 (lo, data0, data1);
 128     unpack_128_2x128 (hi, data2, data3);
 129 }
 130
 131 static inline uint16_t
 132 pack565_32_16 (uint32_t pixel)
 133 {
 134     return (uint16_t) (((pixel>>8) & 0xf800) | ((pixel>>5) & 0x07e0) | ((pixel>>3) & 0x001f));
 135 }
 136
 137 static inline __m128i
 138 pack_2x128_128 (__m128i lo, __m128i hi)
 139 {
 140     return _mm_packus_epi16 (lo, hi);
 141 }
 142
 143 static inline __m128i
 144 pack565_2x128_128 (__m128i lo, __m128i hi)
 145 {
 146     __m128i data;
 147     __m128i r, g1, g2, b;
 148
 149     data = pack_2x128_128 ( lo, hi );
 150
 151     r  = _mm_and_si128 (data , Mask565r);
 152     g1 = _mm_and_si128 (_mm_slli_epi32 (data , 3), Mask565g1);
 153     g2 = _mm_and_si128 (_mm_srli_epi32 (data , 5), Mask565g2);
 154     b  = _mm_and_si128 (_mm_srli_epi32 (data , 3), Mask565b);
 155
 156     return _mm_or_si128 (_mm_or_si128 (_mm_or_si128 (r, g1), g2), b);
 157 }
 158
 159 static inline __m128i
 160 pack565_4x128_128 (__m128i* xmm0, __m128i* xmm1, __m128i* xmm2, __m128i* xmm3)
 161 {
 162     return _mm_packus_epi16 (pack565_2x128_128 (*xmm0, *xmm1), pack565_2x128_128 (*xmm2, *xmm3));
 163 }
 164
 165 static inline uint32_t
 166 packAlpha (__m128i x)
 167 {
 168     return _mm_cvtsi128_si32 (_mm_packus_epi16 (_mm_packus_epi16 (_mm_srli_epi32 (x, 24),
 169                                                                   _mm_setzero_si128 ()),
 170                                                 _mm_setzero_si128 ()));
 171 }
 172
 173 static inline __m128i
 174 expandPixel_32_1x128 (uint32_t data)
 175 {
 176     return _mm_shuffle_epi32 (unpack_32_1x128 (data), _MM_SHUFFLE(1, 0, 1, 0));
 177 }
 178
 179 static inline __m128i
 180 expandAlpha_1x128 (__m128i data)
 181 {
 182     return _mm_shufflehi_epi16 (_mm_shufflelo_epi16 (data, _MM_SHUFFLE(3, 3, 3, 3)), _MM_SHUFFLE(3, 3, 3, 3));
 183 }
 184
 185 static inline void
 186 expandAlpha_2x128 (__m128i dataLo, __m128i dataHi, __m128i* alphaLo, __m128i* alphaHi)
 187 {
 188     __m128i lo, hi;
 189
 190     lo = _mm_shufflelo_epi16 (dataLo, _MM_SHUFFLE(3, 3, 3, 3));
 191     hi = _mm_shufflelo_epi16 (dataHi, _MM_SHUFFLE(3, 3, 3, 3));
 192     *alphaLo = _mm_shufflehi_epi16 (lo, _MM_SHUFFLE(3, 3, 3, 3));
 193     *alphaHi = _mm_shufflehi_epi16 (hi, _MM_SHUFFLE(3, 3, 3, 3));
 194 }
 195
 196 static inline void
 197 expandAlphaRev_2x128 (__m128i dataLo, __m128i dataHi, __m128i* alphaLo, __m128i* alphaHi)
 198 {
 199     __m128i lo, hi;
 200
 201     lo = _mm_shufflelo_epi16 (dataLo, _MM_SHUFFLE(0, 0, 0, 0));
 202     hi = _mm_shufflelo_epi16 (dataHi, _MM_SHUFFLE(0, 0, 0, 0));
 203     *alphaLo = _mm_shufflehi_epi16 (lo, _MM_SHUFFLE(0, 0, 0, 0));
 204     *alphaHi = _mm_shufflehi_epi16 (hi, _MM_SHUFFLE(0, 0, 0, 0));
 205 }
 206
 207 static inline void
 208 pixMultiply_2x128 (__m128i* dataLo, __m128i* dataHi, __m128i* alphaLo, __m128i* alphaHi, __m128i* retLo, __m128i* retHi)
 209 {
 210     __m128i lo, hi;
 211
 212     lo = _mm_mullo_epi16 (*dataLo, *alphaLo);
 213     hi = _mm_mullo_epi16 (*dataHi, *alphaHi);
 214     lo = _mm_adds_epu16 (lo, Mask0080);
 215     hi = _mm_adds_epu16 (hi, Mask0080);
 216     *retLo = _mm_mulhi_epu16 (lo, Mask0101);
 217     *retHi = _mm_mulhi_epu16 (hi, Mask0101);
 218 }
 219
 220 static inline void
 221 pixAddMultiply_2x128 (__m128i* srcLo, __m128i* srcHi, __m128i* alphaDstLo, __m128i* alphaDstHi,
 222                       __m128i* dstLo, __m128i* dstHi, __m128i* alphaSrcLo, __m128i* alphaSrcHi,
 223                       __m128i* retLo, __m128i* retHi)
 224 {
 225     __m128i lo, hi;
 226     __m128i mulLo, mulHi;
 227
 228     lo = _mm_mullo_epi16 (*srcLo, *alphaDstLo);
 229     hi = _mm_mullo_epi16 (*srcHi, *alphaDstHi);
 230     mulLo = _mm_mullo_epi16 (*dstLo, *alphaSrcLo);
 231     mulHi = _mm_mullo_epi16 (*dstHi, *alphaSrcHi);
 232     lo = _mm_adds_epu16 (lo, Mask0080);
 233     hi = _mm_adds_epu16 (hi, Mask0080);
 234     lo = _mm_adds_epu16 (lo, mulLo);
 235     hi = _mm_adds_epu16 (hi, mulHi);
 236     *retLo = _mm_mulhi_epu16 (lo, Mask0101);
 237     *retHi = _mm_mulhi_epu16 (hi, Mask0101);
 238 }
 239
 240 static inline void
 241 negate_2x128 (__m128i dataLo, __m128i dataHi, __m128i* negLo, __m128i* negHi)
 242 {
 243     *negLo = _mm_xor_si128 (dataLo, Mask00ff);
 244     *negHi = _mm_xor_si128 (dataHi, Mask00ff);
 245 }
 246
 247 static inline void
 248 invertColors_2x128 (__m128i dataLo, __m128i dataHi, __m128i* invLo, __m128i* invHi)
 249 {
 250     __m128i lo, hi;
 251
 252     lo = _mm_shufflelo_epi16 (dataLo, _MM_SHUFFLE(3, 0, 1, 2));
 253     hi = _mm_shufflelo_epi16 (dataHi, _MM_SHUFFLE(3, 0, 1, 2));
 254     *invLo = _mm_shufflehi_epi16 (lo, _MM_SHUFFLE(3, 0, 1, 2));
 255     *invHi = _mm_shufflehi_epi16 (hi, _MM_SHUFFLE(3, 0, 1, 2));
 256 }
 257
 258 static inline void
 259 over_2x128 (__m128i* srcLo, __m128i* srcHi, __m128i* alphaLo, __m128i* alphaHi, __m128i* dstLo, __m128i* dstHi)
 260 {
 261     __m128i t1, t2;
 262
 263     negate_2x128 (*alphaLo, *alphaHi, &t1, &t2);
 264
 265     pixMultiply_2x128 (dstLo, dstHi, &t1, &t2, dstLo, dstHi);
 266
 267     *dstLo = _mm_adds_epu8 (*srcLo, *dstLo);
 268     *dstHi = _mm_adds_epu8 (*srcHi, *dstHi);
 269 }
 270
 271 static inline void
 272 overRevNonPre_2x128 (__m128i srcLo, __m128i srcHi, __m128i* dstLo, __m128i* dstHi)
 273 {
 274     __m128i lo, hi;
 275     __m128i alphaLo, alphaHi;
 276
 277     expandAlpha_2x128 (srcLo, srcHi, &alphaLo, &alphaHi);
 278
 279     lo = _mm_or_si128 (alphaLo, MaskAlpha);
 280     hi = _mm_or_si128 (alphaHi, MaskAlpha);
 281
 282     invertColors_2x128 (srcLo, srcHi, &srcLo, &srcHi);
 283
 284     pixMultiply_2x128 (&srcLo, &srcHi, &lo, &hi, &lo, &hi);
 285
 286     over_2x128 (&lo, &hi, &alphaLo, &alphaHi, dstLo, dstHi);
 287 }
 288
 289 static inline void
 290 inOver_2x128 (__m128i* srcLo,  __m128i* srcHi,  __m128i*  alphaLo, __m128i*  alphaHi,
 291               __m128i* maskLo, __m128i* maskHi, __m128i* dstLo,   __m128i* dstHi)
 292 {
 293     __m128i sLo, sHi;
 294     __m128i aLo, aHi;
 295
 296     pixMultiply_2x128 (  srcLo,   srcHi, maskLo, maskHi, &sLo, &sHi);
 297     pixMultiply_2x128 (alphaLo, alphaHi, maskLo, maskHi, &aLo, &aHi);
 298
 299     over_2x128 (&sLo, &sHi, &aLo, &aHi, dstLo, dstHi);
 300 }
 301
 302 static inline void
 303 cachePrefetch (__m128i* addr)
 304 {
 305     _mm_prefetch (addr, _MM_HINT_T0);
 306 }
 307
 308 static inline void
 309 cachePrefetchNext (__m128i* addr)
 310 {
 311     _mm_prefetch (addr + 4, _MM_HINT_T0); // 64 bytes ahead
 312 }
 313
 314 /* load 4 pixels from a 16-byte boundary aligned address */
 315 static inline __m128i
 316 load128Aligned (__m128i* src)
 317 {
 318     return _mm_load_si128 (src);
 319 }
 320
 321 /* load 4 pixels from a unaligned address */
 322 static inline __m128i
 323 load128Unaligned (__m128i* src)
 324 {
 325     return _mm_loadu_si128 (src);
 326 }
 327
 328 /* save 4 pixels using Write Combining memory on a 16-byte boundary aligned address */
 329 static inline void
 330 save128WriteCombining (__m128i* dst, __m128i data)
 331 {
 332     _mm_stream_si128 (dst, data);
 333 }
 334
 335 /* save 4 pixels on a 16-byte boundary aligned address */
 336 static inline void
 337 save128Aligned (__m128i* dst, __m128i data)
 338 {
 339     _mm_store_si128 (dst, data);
 340 }
 341
 342 /* save 4 pixels on a unaligned address */
 343 static inline void
 344 save128Unaligned (__m128i* dst, __m128i data)
 345 {
 346     _mm_storeu_si128 (dst, data);
 347 }
 348
 349 /* -------------------------------------------------------------------------------------------------
 350  * MMX inlines
 351  */
 352
 353 static inline __m64
 354 unpack_32_1x64 (uint32_t data)
 355 {
 356     return _mm_unpacklo_pi8 (_mm_cvtsi32_si64 (data), _mm_setzero_si64());
 357 }
 358
 359 static inline __m64
 360 expandAlpha_1x64 (__m64 data)
 361 {
 362     return _mm_shuffle_pi16 (data, _MM_SHUFFLE(3, 3, 3, 3));
 363 }
 364
 365 static inline __m64
 366 expandAlphaRev_1x64 (__m64 data)
 367 {
 368     return _mm_shuffle_pi16 (data, _MM_SHUFFLE(0, 0, 0, 0));
 369 }
 370
 371 static inline __m64
 372 expandPixel_8_1x64 (uint8_t data)
 373 {
 374     return _mm_shuffle_pi16 (unpack_32_1x64 ((uint32_t)data), _MM_SHUFFLE(0, 0, 0, 0));
 375 }
 376
 377 static inline __m64
 378 pixMultiply_1x64 (__m64 data, __m64 alpha)
 379 {
 380     return _mm_mulhi_pu16 (_mm_adds_pu16 (_mm_mullo_pi16 (data, alpha),
 381                                           xMask0080),
 382                            xMask0101);
 383 }
 384
 385 static inline __m64
 386 pixAddMultiply_1x64 (__m64* src, __m64* alphaDst, __m64* dst, __m64* alphaSrc)
 387 {
 388     return _mm_mulhi_pu16 (_mm_adds_pu16 (_mm_adds_pu16 (_mm_mullo_pi16 (*src, *alphaDst),
 389                                                          xMask0080),
 390                                           _mm_mullo_pi16 (*dst, *alphaSrc)),
 391                            xMask0101);
 392 }
 393
 394 static inline __m64
 395 negate_1x64 (__m64 data)
 396 {
 397     return _mm_xor_si64 (data, xMask00ff);
 398 }
 399
 400 static inline __m64
 401 invertColors_1x64 (__m64 data)
 402 {
 403     return _mm_shuffle_pi16 (data, _MM_SHUFFLE(3, 0, 1, 2));
 404 }
 405
 406 static inline __m64
 407 over_1x64 (__m64 src, __m64 alpha, __m64 dst)
 408 {
 409     return _mm_adds_pu8 (src, pixMultiply_1x64 (dst, negate_1x64 (alpha)));
 410 }
 411
 412 static inline __m64
 413 inOver_1x64 (__m64* src, __m64* alpha, __m64* mask, __m64* dst)
 414 {
 415     return over_1x64 (pixMultiply_1x64 (*src, *mask),
 416                       pixMultiply_1x64 (*alpha, *mask),
 417                       *dst);
 418 }
 419
 420 static inline __m64
 421 overRevNonPre_1x64 (__m64 src, __m64 dst)
 422 {
 423     __m64 alpha = expandAlpha_1x64 (src);
 424
 425     return over_1x64 (pixMultiply_1x64 (invertColors_1x64 (src),
 426                                         _mm_or_si64 (alpha, xMaskAlpha)),
 427                       alpha,
 428                       dst);
 429 }
 430
 431 static inline uint32_t
 432 pack_1x64_32( __m64 data )
 433 {
 434     return _mm_cvtsi64_si32 (_mm_packs_pu16 (data, _mm_setzero_si64()));
 435 }
 436
 437 /* Expand 16 bits positioned at @pos (0-3) of a mmx register into
 438  *
 439  *    00RR00GG00BB
 440  *
 441  * --- Expanding 565 in the low word ---
 442  *
 443  * m = (m << (32 - 3)) | (m << (16 - 5)) | m;
 444  * m = m & (01f0003f001f);
 445  * m = m * (008404100840);
 446  * m = m >> 8;
 447  *
 448  * Note the trick here - the top word is shifted by another nibble to
 449  * avoid it bumping into the middle word
 450  */
 451 static inline __m64
 452 expand565_16_1x64 (uint16_t pixel)
 453 {
 454     __m64 p;
 455     __m64 t1, t2;
 456
 457     p = _mm_cvtsi32_si64 ((uint32_t) pixel);
 458
 459     t1 = _mm_slli_si64 (p, 36 - 11);
 460     t2 = _mm_slli_si64 (p, 16 - 5);
 461
 462     p = _mm_or_si64 (t1, p);
 463     p = _mm_or_si64 (t2, p);
 464     p = _mm_and_si64 (p, xMask565rgb);
 465     p = _mm_mullo_pi16 (p, xMask565Unpack);
 466
 467     return _mm_srli_pi16 (p, 8);
 468 }
 469
 470 /* -------------------------------------------------------------------------------------------------
 471  * Compose Core transformations
 472  */
 473 static inline uint32_t
 474 coreCombineOverUPixelsse2 (uint32_t src, uint32_t dst)
 475 {
 476     uint8_t     a;
 477     __m64       ms;
 478
 479     a = src >> 24;
 480
 481     if (a == 0xff)
 482     {
 483         return src;
 484     }
 485     else if (a)
 486     {
 487         ms = unpack_32_1x64 (src);
 488         return pack_1x64_32 (over_1x64 (ms, expandAlpha_1x64 (ms), unpack_32_1x64 (dst)));
 489     }
 490
 491     return dst;
 492 }
 493
 494 static inline void
 495 coreCombineOverUsse2 (uint32_t* pd, const uint32_t* ps, int w)
 496 {
 497     uint32_t pa;
 498     uint32_t s, d;
 499
 500     __m128i xmmDstLo, xmmDstHi;
 501     __m128i xmmSrcLo, xmmSrcHi;
 502     __m128i xmmAlphaLo, xmmAlphaHi;
 503
 504     /* call prefetch hint to optimize cache load*/
 505     cachePrefetch ((__m128i*)ps);
 506     cachePrefetch ((__m128i*)pd);
 507
 508     /* Align dst on a 16-byte boundary */
 509     while (w &&
 510            ((unsigned long)pd & 15))
 511     {
 512         d = *pd;
 513         s = *ps++;
 514
 515         *pd++ = coreCombineOverUPixelsse2 (s, d);
 516         w--;
 517     }
 518
 519     /* call prefetch hint to optimize cache load*/
 520     cachePrefetch ((__m128i*)ps);
 521     cachePrefetch ((__m128i*)pd);
 522
 523     while (w >= 4)
 524     {
 525         /* fill cache line with next memory */
 526         cachePrefetchNext ((__m128i*)ps);
 527         cachePrefetchNext ((__m128i*)pd);
 528
 529         /* I'm loading unaligned because I'm not sure about the address alignment. */
 530         xmmSrcHi = load128Unaligned ((__m128i*) ps);
 531
 532         /* Check the alpha channel */
 533         pa = packAlpha (xmmSrcHi);
 534
 535         if (pa == 0xffffffff)
 536         {
 537             save128Aligned ((__m128i*)pd, xmmSrcHi);
 538         }
 539         else if (pa)
 540         {
 541             xmmDstHi = load128Aligned ((__m128i*) pd);
 542
 543             unpack_128_2x128 (xmmSrcHi, &xmmSrcLo, &xmmSrcHi);
 544             unpack_128_2x128 (xmmDstHi, &xmmDstLo, &xmmDstHi);
 545
 546             expandAlpha_2x128 (xmmSrcLo, xmmSrcHi, &xmmAlphaLo, &xmmAlphaHi);
 547
 548             over_2x128 (&xmmSrcLo, &xmmSrcHi, &xmmAlphaLo, &xmmAlphaHi, &xmmDstLo, &xmmDstHi);
 549
 550             /* rebuid the 4 pixel data and save*/
 551             save128Aligned ((__m128i*)pd, pack_2x128_128 (xmmDstLo, xmmDstHi));
 552         }
 553
 554         w -= 4;
 555         ps += 4;
 556         pd += 4;
 557     }
 558
 559     while (w)
 560     {
 561         d = *pd;
 562         s = *ps++;
 563
 564         *pd++ = coreCombineOverUPixelsse2 (s, d);
 565         w--;
 566     }
 567 }
 568
 569 static inline void
 570 coreCombineOverReverseUsse2 (uint32_t* pd, const uint32_t* ps, int w)
 571 {
 572     uint32_t s, d;
 573
 574     __m128i xmmDstLo, xmmDstHi;
 575     __m128i xmmSrcLo, xmmSrcHi;
 576     __m128i xmmAlphaLo, xmmAlphaHi;
 577
 578     /* call prefetch hint to optimize cache load*/
 579     cachePrefetch ((__m128i*)ps);
 580     cachePrefetch ((__m128i*)pd);
 581
 582     /* Align dst on a 16-byte boundary */
 583     while (w &&
 584            ((unsigned long)pd & 15))
 585     {
 586         d = *pd;
 587         s = *ps++;
 588
 589         *pd++ = coreCombineOverUPixelsse2 (d, s);
 590         w--;
 591     }
 592
 593     /* call prefetch hint to optimize cache load*/
 594     cachePrefetch ((__m128i*)ps);
 595     cachePrefetch ((__m128i*)pd);
 596
 597     while (w >= 4)
 598     {
 599         /* fill cache line with next memory */
 600         cachePrefetchNext ((__m128i*)ps);
 601         cachePrefetchNext ((__m128i*)pd);
 602
 603         /* I'm loading unaligned because I'm not sure about the address alignment. */
 604         xmmSrcHi = load128Unaligned ((__m128i*) ps);
 605         xmmDstHi = load128Aligned ((__m128i*) pd);
 606
 607         unpack_128_2x128 (xmmSrcHi, &xmmSrcLo, &xmmSrcHi);
 608         unpack_128_2x128 (xmmDstHi, &xmmDstLo, &xmmDstHi);
 609
 610         expandAlpha_2x128 (xmmDstLo, xmmDstHi, &xmmAlphaLo, &xmmAlphaHi);
 611
 612         over_2x128 (&xmmDstLo, &xmmDstHi, &xmmAlphaLo, &xmmAlphaHi, &xmmSrcLo, &xmmSrcHi);
 613
 614         /* rebuid the 4 pixel data and save*/
 615         save128Aligned ((__m128i*)pd, pack_2x128_128 (xmmSrcLo, xmmSrcHi));
 616
 617         w -= 4;
 618         ps += 4;
 619         pd += 4;
 620     }
 621
 622     while (w)
 623     {
 624         d = *pd;
 625         s = *ps++;
 626
 627         *pd++ = coreCombineOverUPixelsse2 (d, s);
 628         w--;
 629     }
 630 }
 631
 632 static inline uint32_t
 633 coreCombineInUPixelsse2 (uint32_t src, uint32_t dst)
 634 {
 635     uint32_t maska = src >> 24;
 636
 637     if (maska == 0)
 638     {
 639         return 0;
 640     }
 641     else if (maska != 0xff)
 642     {
 643         return pack_1x64_32(pixMultiply_1x64 (unpack_32_1x64 (dst), expandAlpha_1x64 (unpack_32_1x64 (src))));
 644     }
 645
 646     return dst;
 647 }
 648
 649 static inline void
 650 coreCombineInUsse2 (uint32_t* pd, const uint32_t* ps, int w)
 651 {
 652     uint32_t s, d;
 653
 654     __m128i xmmSrcLo, xmmSrcHi;
 655     __m128i xmmDstLo, xmmDstHi;
 656
 657     /* call prefetch hint to optimize cache load*/
 658     cachePrefetch ((__m128i*)ps);
 659     cachePrefetch ((__m128i*)pd);
 660
 661     while (w && ((unsigned long) pd & 15))
 662     {
 663         s = *ps++;
 664         d = *pd;
 665
 666         *pd++ = coreCombineInUPixelsse2 (d, s);
 667         w--;
 668     }
 669
 670     /* call prefetch hint to optimize cache load*/
 671     cachePrefetch ((__m128i*)ps);
 672     cachePrefetch ((__m128i*)pd);
 673
 674     while (w >= 4)
 675     {
 676         /* fill cache line with next memory */
 677         cachePrefetchNext ((__m128i*)ps);
 678         cachePrefetchNext ((__m128i*)pd);
 679
 680         xmmDstHi = load128Aligned ((__m128i*) pd);
 681         xmmSrcHi = load128Unaligned ((__m128i*) ps);
 682
 683         unpack_128_2x128 (xmmDstHi, &xmmDstLo, &xmmDstHi);
 684         expandAlpha_2x128 (xmmDstLo, xmmDstHi, &xmmDstLo, &xmmDstHi);
 685
 686         unpack_128_2x128 (xmmSrcHi, &xmmSrcLo, &xmmSrcHi);
 687         pixMultiply_2x128 (&xmmSrcLo, &xmmSrcHi, &xmmDstLo, &xmmDstHi, &xmmDstLo, &xmmDstHi);
 688
 689         save128Aligned ((__m128i*)pd, pack_2x128_128 (xmmDstLo, xmmDstHi));
 690
 691         ps += 4;
 692         pd += 4;
 693         w -= 4;
 694     }
 695
 696     while (w)
 697     {
 698         s = *ps++;
 699         d = *pd;
 700
 701         *pd++ = coreCombineInUPixelsse2 (d, s);
 702         w--;
 703     }
 704 }
 705
 706 static inline void
 707 coreCombineReverseInUsse2 (uint32_t* pd, const uint32_t* ps, int w)
 708 {
 709     uint32_t s, d;
 710
 711     __m128i xmmSrcLo, xmmSrcHi;
 712     __m128i xmmDstLo, xmmDstHi;
 713
 714     /* call prefetch hint to optimize cache load*/
 715     cachePrefetch ((__m128i*)ps);
 716     cachePrefetch ((__m128i*)pd);
 717
 718     while (w && ((unsigned long) pd & 15))
 719     {
 720         s = *ps++;
 721         d = *pd;
 722
 723         *pd++ = coreCombineInUPixelsse2 (s, d);
 724         w--;
 725     }
 726
 727     /* call prefetch hint to optimize cache load*/
 728     cachePrefetch ((__m128i*)ps);
 729     cachePrefetch ((__m128i*)pd);
 730
 731     while (w >= 4)
 732     {
 733         /* fill cache line with next memory */
 734         cachePrefetchNext ((__m128i*)ps);
 735         cachePrefetchNext ((__m128i*)pd);
 736
 737         xmmDstHi = load128Aligned ((__m128i*) pd);
 738         xmmSrcHi = load128Unaligned ((__m128i*) ps);
 739
 740         unpack_128_2x128 (xmmSrcHi, &xmmSrcLo, &xmmSrcHi);
 741         expandAlpha_2x128 (xmmSrcLo, xmmSrcHi, &xmmSrcLo, &xmmSrcHi);
 742
 743         unpack_128_2x128 (xmmDstHi, &xmmDstLo, &xmmDstHi);
 744         pixMultiply_2x128 (&xmmDstLo, &xmmDstHi, &xmmSrcLo, &xmmSrcHi, &xmmDstLo, &xmmDstHi);
 745
 746         save128Aligned ((__m128i*)pd, pack_2x128_128 (xmmDstLo, xmmDstHi));
 747
 748         ps += 4;
 749         pd += 4;
 750         w -= 4;
 751     }
 752
 753     while (w)
 754     {
 755         s = *ps++;
 756         d = *pd;
 757
 758         *pd++ = coreCombineInUPixelsse2 (s, d);
 759         w--;
 760     }
 761 }
 762
 763 static inline void
 764 coreCombineReverseOutUsse2 (uint32_t* pd, const uint32_t* ps, int w)
 765 {
 766     /* call prefetch hint to optimize cache load*/
 767     cachePrefetch ((__m128i*)ps);
 768     cachePrefetch ((__m128i*)pd);
 769
 770     while (w && ((unsigned long) pd & 15))
 771     {
 772         uint32_t s = *ps++;
 773         uint32_t d = *pd;
 774
 775         *pd++ = pack_1x64_32 (pixMultiply_1x64 (unpack_32_1x64 (d), negate_1x64 (expandAlpha_1x64 (unpack_32_1x64 (s)))));
 776         w--;
 777     }
 778
 779     /* call prefetch hint to optimize cache load*/
 780     cachePrefetch ((__m128i*)ps);
 781     cachePrefetch ((__m128i*)pd);
 782
 783     while (w >= 4)
 784     {
 785         __m128i xmmSrcLo, xmmSrcHi;
 786         __m128i xmmDstLo, xmmDstHi;
 787
 788         /* fill cache line with next memory */
 789         cachePrefetchNext ((__m128i*)ps);
 790         cachePrefetchNext ((__m128i*)pd);
 791
 792         xmmSrcHi = load128Unaligned ((__m128i*) ps);
 793         xmmDstHi = load128Aligned ((__m128i*) pd);
 794
 795         unpack_128_2x128 (xmmSrcHi, &xmmSrcLo, &xmmSrcHi);
 796         unpack_128_2x128 (xmmDstHi, &xmmDstLo, &xmmDstHi);
 797
 798         expandAlpha_2x128 (xmmSrcLo, xmmSrcHi, &xmmSrcLo, &xmmSrcHi);
 799         negate_2x128      (xmmSrcLo, xmmSrcHi, &xmmSrcLo, &xmmSrcHi);
 800
 801         pixMultiply_2x128 (&xmmDstLo, &xmmDstHi, &xmmSrcLo, &xmmSrcHi, &xmmDstLo, &xmmDstHi);
 802
 803         save128Aligned ((__m128i*)pd, pack_2x128_128 (xmmDstLo, xmmDstHi));
 804
 805         ps += 4;
 806         pd += 4;
 807         w -= 4;
 808     }
 809
 810     while (w)
 811     {
 812         uint32_t s = *ps++;
 813         uint32_t d = *pd;
 814
 815         *pd++ = pack_1x64_32 (pixMultiply_1x64 (unpack_32_1x64 (d), negate_1x64 (expandAlpha_1x64 (unpack_32_1x64 (s)))));
 816         w--;
 817     }
 818 }
 819
 820 static inline void
 821 coreCombineOutUsse2 (uint32_t* pd, const uint32_t* ps, int w)
 822 {
 823     /* call prefetch hint to optimize cache load*/
 824     cachePrefetch ((__m128i*)ps);
 825     cachePrefetch ((__m128i*)pd);
 826
 827     while (w && ((unsigned long) pd & 15))
 828     {
 829         uint32_t s = *ps++;
 830         uint32_t d = *pd;
 831
 832         *pd++ = pack_1x64_32 (pixMultiply_1x64 (unpack_32_1x64 (s), negate_1x64 (expandAlpha_1x64 (unpack_32_1x64 (d)))));
 833         w--;
 834     }
 835
 836     /* call prefetch hint to optimize cache load*/
 837     cachePrefetch ((__m128i*)ps);
 838     cachePrefetch ((__m128i*)pd);
 839
 840     while (w >= 4)
 841     {
 842         __m128i xmmSrcLo, xmmSrcHi;
 843         __m128i xmmDstLo, xmmDstHi;
 844
 845         /* fill cache line with next memory */
 846         cachePrefetchNext ((__m128i*)ps);
 847         cachePrefetchNext ((__m128i*)pd);
 848
 849         xmmSrcHi = load128Unaligned ((__m128i*) ps);
 850         xmmDstHi = load128Aligned ((__m128i*) pd);
 851
 852         unpack_128_2x128 (xmmSrcHi, &xmmSrcLo, &xmmSrcHi);
 853         unpack_128_2x128 (xmmDstHi, &xmmDstLo, &xmmDstHi);
 854
 855         expandAlpha_2x128 (xmmDstLo, xmmDstHi, &xmmDstLo, &xmmDstHi);
 856         negate_2x128      (xmmDstLo, xmmDstHi, &xmmDstLo, &xmmDstHi);
 857
 858         pixMultiply_2x128 (&xmmSrcLo, &xmmSrcHi, &xmmDstLo, &xmmDstHi, &xmmDstLo, &xmmDstHi);
 859
 860         save128Aligned ((__m128i*)pd, pack_2x128_128 (xmmDstLo, xmmDstHi));
 861
 862         ps += 4;
 863         pd += 4;
 864         w -= 4;
 865     }
 866
 867     while (w)
 868     {
 869         uint32_t s = *ps++;
 870         uint32_t d = *pd;
 871
 872         *pd++ = pack_1x64_32 (pixMultiply_1x64 (unpack_32_1x64 (s), negate_1x64 (expandAlpha_1x64 (unpack_32_1x64 (d)))));
 873         w--;
 874     }
 875 }
 876
 877 static inline uint32_t
 878 coreCombineAtopUPixelsse2 (uint32_t src, uint32_t dst)
 879 {
 880     __m64 s = unpack_32_1x64 (src);
 881     __m64 d = unpack_32_1x64 (dst);
 882
 883     __m64 sa = negate_1x64 (expandAlpha_1x64 (s));
 884     __m64 da = expandAlpha_1x64 (d);
 885
 886     return pack_1x64_32 (pixAddMultiply_1x64 (&s, &da, &d, &sa));
 887 }
 888
 889 static inline void
 890 coreCombineAtopUsse2 (uint32_t* pd, const uint32_t* ps, int w)
 891 {
 892     uint32_t s, d;
 893
 894     __m128i xmmSrcLo, xmmSrcHi;
 895     __m128i xmmDstLo, xmmDstHi;
 896     __m128i xmmAlphaSrcLo, xmmAlphaSrcHi;
 897     __m128i xmmAlphaDstLo, xmmAlphaDstHi;
 898
 899     /* call prefetch hint to optimize cache load*/
 900     cachePrefetch ((__m128i*)ps);
 901     cachePrefetch ((__m128i*)pd);
 902
 903     while (w && ((unsigned long) pd & 15))
 904     {
 905         s = *ps++;
 906         d = *pd;
 907
 908         *pd++ = coreCombineAtopUPixelsse2 (s, d);
 909         w--;
 910     }
 911
 912     /* call prefetch hint to optimize cache load*/
 913     cachePrefetch ((__m128i*)ps);
 914     cachePrefetch ((__m128i*)pd);
 915
 916     while (w >= 4)
 917     {
 918         /* fill cache line with next memory */
 919         cachePrefetchNext ((__m128i*)ps);
 920         cachePrefetchNext ((__m128i*)pd);
 921
 922         xmmSrcHi = load128Unaligned ((__m128i*) ps);
 923         xmmDstHi = load128Aligned ((__m128i*) pd);
 924
 925         unpack_128_2x128 (xmmSrcHi, &xmmSrcLo, &xmmSrcHi);
 926         unpack_128_2x128 (xmmDstHi, &xmmDstLo, &xmmDstHi);
 927
 928         expandAlpha_2x128 (xmmSrcLo, xmmSrcHi, &xmmAlphaSrcLo, &xmmAlphaSrcHi);
 929         expandAlpha_2x128 (xmmDstLo, xmmDstHi, &xmmAlphaDstLo, &xmmAlphaDstHi);
 930
 931         negate_2x128 (xmmAlphaSrcLo, xmmAlphaSrcHi, &xmmAlphaSrcLo, &xmmAlphaSrcHi);
 932
 933         pixAddMultiply_2x128 ( &xmmSrcLo, &xmmSrcHi, &xmmAlphaDstLo, &xmmAlphaDstHi,
 934                                &xmmDstLo, &xmmDstHi, &xmmAlphaSrcLo, &xmmAlphaSrcHi,
 935                                &xmmDstLo, &xmmDstHi );
 936
 937         save128Aligned ((__m128i*)pd, pack_2x128_128 (xmmDstLo, xmmDstHi));
 938
 939         ps += 4;
 940         pd += 4;
 941         w -= 4;
 942     }
 943
 944     while (w)
 945     {
 946         s = *ps++;
 947         d = *pd;
 948
 949         *pd++ = coreCombineAtopUPixelsse2 (s, d);
 950         w--;
 951     }
 952 }
 953
 954 static inline uint32_t
 955 coreCombineReverseAtopUPixelsse2 (uint32_t src, uint32_t dst)
 956 {
 957     __m64 s = unpack_32_1x64 (src);
 958     __m64 d = unpack_32_1x64 (dst);
 959
 960     __m64 sa = expandAlpha_1x64 (s);
 961     __m64 da = negate_1x64 (expandAlpha_1x64 (d));
 962
 963     return pack_1x64_32 (pixAddMultiply_1x64 (&s, &da, &d, &sa));
 964 }
 965
 966 static inline void
 967 coreCombineReverseAtopUsse2 (uint32_t* pd, const uint32_t* ps, int w)
 968 {
 969     uint32_t s, d;
 970
 971     __m128i xmmSrcLo, xmmSrcHi;
 972     __m128i xmmDstLo, xmmDstHi;
 973     __m128i xmmAlphaSrcLo, xmmAlphaSrcHi;
 974     __m128i xmmAlphaDstLo, xmmAlphaDstHi;
 975
 976     /* call prefetch hint to optimize cache load*/
 977     cachePrefetch ((__m128i*)ps);
 978     cachePrefetch ((__m128i*)pd);
 979
 980     while (w && ((unsigned long) pd & 15))
 981     {
 982         s = *ps++;
 983         d = *pd;
 984
 985         *pd++ = coreCombineReverseAtopUPixelsse2 (s, d);
 986         w--;
 987     }
 988
 989     /* call prefetch hint to optimize cache load*/
 990     cachePrefetch ((__m128i*)ps);
 991     cachePrefetch ((__m128i*)pd);
 992
 993     while (w >= 4)
 994     {
 995         /* fill cache line with next memory */
 996         cachePrefetchNext ((__m128i*)ps);
 997         cachePrefetchNext ((__m128i*)pd);
 998
 999         xmmSrcHi = load128Unaligned ((__m128i*) ps);
1000         xmmDstHi = load128Aligned ((__m128i*) pd);
1001
1002         unpack_128_2x128 (xmmSrcHi, &xmmSrcLo, &xmmSrcHi);
1003         unpack_128_2x128 (xmmDstHi, &xmmDstLo, &xmmDstHi);
1004
1005         expandAlpha_2x128 (xmmSrcLo, xmmSrcHi, &xmmAlphaSrcLo, &xmmAlphaSrcHi);
1006         expandAlpha_2x128 (xmmDstLo, xmmDstHi, &xmmAlphaDstLo, &xmmAlphaDstHi);
1007
1008         negate_2x128 (xmmAlphaDstLo, xmmAlphaDstHi, &xmmAlphaDstLo, &xmmAlphaDstHi);
1009
1010         pixAddMultiply_2x128 ( &xmmSrcLo, &xmmSrcHi, &xmmAlphaDstLo, &xmmAlphaDstHi,
1011                                &xmmDstLo, &xmmDstHi, &xmmAlphaSrcLo, &xmmAlphaSrcHi,
1012                                &xmmDstLo, &xmmDstHi );
1013
1014         save128Aligned ((__m128i*)pd, pack_2x128_128 (xmmDstLo, xmmDstHi));
1015
1016         ps += 4;
1017         pd += 4;
1018         w -= 4;
1019     }
1020
1021     while (w)
1022     {
1023         s = *ps++;
1024         d = *pd;
1025
1026         *pd++ = coreCombineReverseAtopUPixelsse2 (s, d);
1027         w--;
1028     }
1029 }
1030
1031 static inline uint32_t
1032 coreCombineXorUPixelsse2 (uint32_t src, uint32_t dst)
1033 {
1034     __m64 s = unpack_32_1x64 (src);
1035     __m64 d = unpack_32_1x64 (dst);
1036
1037     __m64 negD = negate_1x64 (expandAlpha_1x64 (d));
1038     __m64 negS = negate_1x64 (expandAlpha_1x64 (s));
1039
1040     return pack_1x64_32 (pixAddMultiply_1x64 (&s, &negD, &d, &negS));
1041 }
1042
1043 static inline void
1044 coreCombineXorUsse2 (uint32_t* dst, const uint32_t* src, int width)
1045 {
1046     int w = width;
1047     uint32_t s, d;
1048     uint32_t* pd = dst;
1049     const uint32_t* ps = src;
1050
1051     __m128i xmmSrc, xmmSrcLo, xmmSrcHi;
1052     __m128i xmmDst, xmmDstLo, xmmDstHi;
1053     __m128i xmmAlphaSrcLo, xmmAlphaSrcHi;
1054     __m128i xmmAlphaDstLo, xmmAlphaDstHi;
1055
1056     /* call prefetch hint to optimize cache load*/
1057     cachePrefetch ((__m128i*)ps);
1058     cachePrefetch ((__m128i*)pd);
1059
1060     while (w && ((unsigned long) pd & 15))
1061     {
1062         s = *ps++;
1063         d = *pd;
1064
1065         *pd++ = coreCombineXorUPixelsse2 (s, d);
1066         w--;
1067     }
1068
1069     /* call prefetch hint to optimize cache load*/
1070     cachePrefetch ((__m128i*)ps);
1071     cachePrefetch ((__m128i*)pd);
1072
1073     while (w >= 4)
1074     {
1075         /* fill cache line with next memory */
1076         cachePrefetchNext ((__m128i*)ps);
1077         cachePrefetchNext ((__m128i*)pd);
1078
1079         xmmSrc = load128Unaligned ((__m128i*) ps);
1080         xmmDst = load128Aligned ((__m128i*) pd);
1081
1082         unpack_128_2x128 (xmmSrc, &xmmSrcLo, &xmmSrcHi);
1083         unpack_128_2x128 (xmmDst, &xmmDstLo, &xmmDstHi);
1084
1085         expandAlpha_2x128 (xmmSrcLo, xmmSrcHi, &xmmAlphaSrcLo, &xmmAlphaSrcHi);
1086         expandAlpha_2x128 (xmmDstLo, xmmDstHi, &xmmAlphaDstLo, &xmmAlphaDstHi);
1087
1088         negate_2x128 (xmmAlphaSrcLo, xmmAlphaSrcHi, &xmmAlphaSrcLo, &xmmAlphaSrcHi);
1089         negate_2x128 (xmmAlphaDstLo, xmmAlphaDstHi, &xmmAlphaDstLo, &xmmAlphaDstHi);
1090
1091         pixAddMultiply_2x128 ( &xmmSrcLo, &xmmSrcHi, &xmmAlphaDstLo, &xmmAlphaDstHi,
1092                                &xmmDstLo, &xmmDstHi, &xmmAlphaSrcLo, &xmmAlphaSrcHi,
1093                                &xmmDstLo, &xmmDstHi );
1094
1095         save128Aligned ((__m128i*)pd, pack_2x128_128 (xmmDstLo, xmmDstHi));
1096
1097         ps += 4;
1098         pd += 4;
1099         w -= 4;
1100     }
1101
1102     while (w)
1103     {
1104         s = *ps++;
1105         d = *pd;
1106
1107         *pd++ = coreCombineXorUPixelsse2 (s, d);
1108         w--;
1109     }
1110 }
1111
1112 static inline void
1113 coreCombineAddUsse2 (uint32_t* dst, const uint32_t* src, int width)
1114 {
1115     int w = width;
1116     uint32_t s,d;
1117     uint32_t* pd = dst;
1118     const uint32_t* ps = src;
1119
1120     /* call prefetch hint to optimize cache load*/
1121     cachePrefetch ((__m128i*)ps);
1122     cachePrefetch ((__m128i*)pd);
1123
1124     while (w && (unsigned long)pd & 15)
1125     {
1126         s = *ps++;
1127         d = *pd;
1128         *pd++ = _mm_cvtsi64_si32 (_mm_adds_pu8 (_mm_cvtsi32_si64 (s), _mm_cvtsi32_si64 (d)));
1129         w--;
1130     }
1131
1132     /* call prefetch hint to optimize cache load*/
1133     cachePrefetch ((__m128i*)ps);
1134     cachePrefetch ((__m128i*)pd);
1135
1136     while (w >= 4)
1137     {
1138         /* fill cache line with next memory */
1139         cachePrefetchNext ((__m128i*)ps);
1140         cachePrefetchNext ((__m128i*)pd);
1141
1142         save128Aligned( (__m128i*)pd,
1143                         _mm_adds_epu8( load128Unaligned((__m128i*)ps),
1144                                        load128Aligned  ((__m128i*)pd)) );
1145         pd += 4;
1146         ps += 4;
1147         w -= 4;
1148     }
1149
1150     while (w--)
1151     {
1152         s = *ps++;
1153         d = *pd;
1154         *pd++ = _mm_cvtsi64_si32 (_mm_adds_pu8 (_mm_cvtsi32_si64 (s), _mm_cvtsi32_si64 (d)));
1155     }
1156 }
1157
1158 static inline uint32_t
1159 coreCombineSaturateUPixelsse2 (uint32_t src, uint32_t dst)
1160 {
1161     __m64 ms = unpack_32_1x64 (src);
1162     __m64 md = unpack_32_1x64 (dst);
1163     uint32_t sa = src >> 24;
1164     uint32_t da = ~dst >> 24;
1165
1166     if (sa > da)
1167     {
1168         ms = pixMultiply_1x64 (ms, expandAlpha_1x64 (unpack_32_1x64 (FbIntDiv(da, sa) << 24)));
1169     }
1170
1171     return pack_1x64_32 (_mm_adds_pu16 (md, ms));
1172 }
1173
1174 static inline void
1175 coreCombineSaturateUsse2 (uint32_t *pd, const uint32_t *ps, int w)
1176 {
1177     uint32_t s,d;
1178
1179     uint32_t packCmp;
1180     __m128i xmmSrc, xmmDst;
1181
1182     /* call prefetch hint to optimize cache load*/
1183     cachePrefetch ((__m128i*)ps);
1184     cachePrefetch ((__m128i*)pd);
1185
1186     while (w && (unsigned long)pd & 15)
1187     {
1188         s = *ps++;
1189         d = *pd;
1190         *pd++ = coreCombineSaturateUPixelsse2 (s, d);
1191         w--;
1192     }
1193
1194     /* call prefetch hint to optimize cache load*/
1195     cachePrefetch ((__m128i*)ps);
1196     cachePrefetch ((__m128i*)pd);
1197
1198     while (w >= 4)
1199     {
1200         /* fill cache line with next memory */
1201         cachePrefetchNext ((__m128i*)ps);
1202         cachePrefetchNext ((__m128i*)pd);
1203
1204         xmmDst = load128Aligned  ((__m128i*)pd);
1205         xmmSrc = load128Unaligned((__m128i*)ps);
1206
1207         packCmp = _mm_movemask_epi8 (_mm_cmpgt_epi32 (_mm_srli_epi32 (xmmSrc, 24),
1208                                                       _mm_srli_epi32 (_mm_xor_si128 (xmmDst, Maskff000000), 24)));
1209
1210         /* if some alpha src is grater than respective ~alpha dst */
1211         if (packCmp)
1212         {
1213             s = *ps++;
1214             d = *pd;
1215             *pd++ = coreCombineSaturateUPixelsse2 (s, d);
1216
1217             s = *ps++;
1218             d = *pd;
1219             *pd++ = coreCombineSaturateUPixelsse2 (s, d);
1220
1221             s = *ps++;
1222             d = *pd;
1223             *pd++ = coreCombineSaturateUPixelsse2 (s, d);
1224
1225             s = *ps++;
1226             d = *pd;
1227             *pd++ = coreCombineSaturateUPixelsse2 (s, d);
1228         }
1229         else
1230         {
1231             save128Aligned ((__m128i*)pd, _mm_adds_epu8 (xmmDst, xmmSrc));
1232
1233             pd += 4;
1234             ps += 4;
1235         }
1236
1237         w -= 4;
1238     }
1239
1240     while (w--)
1241     {
1242         s = *ps++;
1243         d = *pd;
1244         *pd++ = coreCombineSaturateUPixelsse2 (s, d);
1245     }
1246 }
1247
1248 static inline void
1249 coreCombineSrcCsse2 (uint32_t* pd, const uint32_t* ps, const uint32_t *pm, int w)
1250 {
1251     uint32_t s, m;
1252
1253     __m128i xmmSrcLo, xmmSrcHi;
1254     __m128i xmmMaskLo, xmmMaskHi;
1255     __m128i xmmDstLo, xmmDstHi;
1256
1257     /* call prefetch hint to optimize cache load*/
1258     cachePrefetch ((__m128i*)ps);
1259     cachePrefetch ((__m128i*)pd);
1260     cachePrefetch ((__m128i*)pm);
1261
1262     while (w && (unsigned long)pd & 15)
1263     {
1264         s = *ps++;
1265         m = *pm++;
1266         *pd++ = pack_1x64_32 (pixMultiply_1x64 (unpack_32_1x64 (s), unpack_32_1x64 (m)));
1267         w--;
1268     }
1269
1270     /* call prefetch hint to optimize cache load*/
1271     cachePrefetch ((__m128i*)ps);
1272     cachePrefetch ((__m128i*)pd);
1273     cachePrefetch ((__m128i*)pm);
1274
1275     while (w >= 4)
1276     {
1277         /* fill cache line with next memory */
1278         cachePrefetchNext ((__m128i*)ps);
1279         cachePrefetchNext ((__m128i*)pd);
1280         cachePrefetchNext ((__m128i*)pm);
1281
1282         xmmSrcHi = load128Unaligned ((__m128i*)ps);
1283         xmmMaskHi = load128Unaligned ((__m128i*)pm);
1284
1285         unpack_128_2x128 (xmmSrcHi, &xmmSrcLo, &xmmSrcHi);
1286         unpack_128_2x128 (xmmMaskHi, &xmmMaskLo, &xmmMaskHi);
1287
1288         pixMultiply_2x128 (&xmmSrcLo, &xmmSrcHi, &xmmMaskLo, &xmmMaskHi, &xmmDstLo, &xmmDstHi);
1289
1290         save128Aligned( (__m128i*)pd, pack_2x128_128 (xmmDstLo, xmmDstHi));
1291
1292         ps += 4;
1293         pd += 4;
1294         pm += 4;
1295         w -= 4;
1296     }
1297
1298     while (w)
1299     {
1300         s = *ps++;
1301         m = *pm++;
1302         *pd++ = pack_1x64_32 (pixMultiply_1x64 (unpack_32_1x64 (s), unpack_32_1x64 (m)));
1303         w--;
1304     }
1305 }
1306
1307 static inline uint32_t
1308 coreCombineOverCPixelsse2 (uint32_t src, uint32_t mask, uint32_t dst)
1309 {
1310     __m64 s = unpack_32_1x64 (src);
1311     __m64 expAlpha = expandAlpha_1x64 (s);
1312     __m64 unpkMask = unpack_32_1x64 (mask);
1313     __m64 unpkDst  = unpack_32_1x64 (dst);
1314
1315     return pack_1x64_32 (inOver_1x64 (&s, &expAlpha, &unpkMask, &unpkDst));
1316 }
1317
1318 static inline void
1319 coreCombineOverCsse2 (uint32_t* pd, const uint32_t* ps, const uint32_t *pm, int w)
1320 {
1321     uint32_t s, m, d;
1322
1323     __m128i xmmAlphaLo, xmmAlphaHi;
1324     __m128i xmmSrcLo, xmmSrcHi;
1325     __m128i xmmDstLo, xmmDstHi;
1326     __m128i xmmMaskLo, xmmMaskHi;
1327
1328     /* call prefetch hint to optimize cache load*/
1329     cachePrefetch ((__m128i*)ps);
1330     cachePrefetch ((__m128i*)pd);
1331     cachePrefetch ((__m128i*)pm);
1332
1333     while (w && (unsigned long)pd & 15)
1334     {
1335         s = *ps++;
1336         m = *pm++;
1337         d = *pd;
1338
1339         *pd++ = coreCombineOverCPixelsse2 (s, m, d);
1340         w--;
1341     }
1342
1343     /* call prefetch hint to optimize cache load*/
1344     cachePrefetch ((__m128i*)ps);
1345     cachePrefetch ((__m128i*)pd);
1346     cachePrefetch ((__m128i*)pm);
1347
1348     while (w >= 4)
1349     {
1350         /* fill cache line with next memory */
1351         cachePrefetchNext ((__m128i*)ps);
1352         cachePrefetchNext ((__m128i*)pd);
1353         cachePrefetchNext ((__m128i*)pm);
1354
1355         xmmDstHi = load128Aligned ((__m128i*)pd);
1356         xmmSrcHi = load128Unaligned ((__m128i*)ps);
1357         xmmMaskHi = load128Unaligned ((__m128i*)pm);
1358
1359         unpack_128_2x128 (xmmDstHi, &xmmDstLo, &xmmDstHi);
1360         unpack_128_2x128 (xmmSrcHi, &xmmSrcLo, &xmmSrcHi);
1361         unpack_128_2x128 (xmmMaskHi, &xmmMaskLo, &xmmMaskHi);
1362
1363         expandAlpha_2x128 (xmmSrcLo, xmmSrcHi, &xmmAlphaLo, &xmmAlphaHi);
1364
1365         inOver_2x128 (&xmmSrcLo, &xmmSrcHi, &xmmAlphaLo, &xmmAlphaHi, &xmmMaskLo, &xmmMaskHi, &xmmDstLo, &xmmDstHi);
1366
1367         save128Aligned( (__m128i*)pd, pack_2x128_128 (xmmDstLo, xmmDstHi));
1368
1369         ps += 4;
1370         pd += 4;
1371         pm += 4;
1372         w -= 4;
1373     }
1374
1375     while (w)
1376     {
1377         s = *ps++;
1378         m = *pm++;
1379         d = *pd;
1380
1381         *pd++ = coreCombineOverCPixelsse2 (s, m, d);
1382         w--;
1383     }
1384 }
1385
1386 static inline uint32_t
1387 coreCombineOverReverseCPixelsse2 (uint32_t src, uint32_t mask, uint32_t dst)
1388 {
1389     __m64 d = unpack_32_1x64 (dst);
1390
1391         return pack_1x64_32(over_1x64 (d, expandAlpha_1x64 (d), pixMultiply_1x64 (unpack_32_1x64 (src), unpack_32_1x64 (mask))));
1392 }
1393
1394 static inline void
1395 coreCombineOverReverseCsse2 (uint32_t* pd, const uint32_t* ps, const uint32_t *pm, int w)
1396 {
1397     uint32_t s, m, d;
1398
1399     __m128i xmmAlphaLo, xmmAlphaHi;
1400     __m128i xmmSrcLo, xmmSrcHi;
1401     __m128i xmmDstLo, xmmDstHi;
1402     __m128i xmmMaskLo, xmmMaskHi;
1403
1404     /* call prefetch hint to optimize cache load*/
1405     cachePrefetch ((__m128i*)ps);
1406     cachePrefetch ((__m128i*)pd);
1407     cachePrefetch ((__m128i*)pm);
1408
1409     while (w && (unsigned long)pd & 15)
1410     {
1411         s = *ps++;
1412         m = *pm++;
1413         d = *pd;
1414
1415         *pd++ = coreCombineOverReverseCPixelsse2 (s, m, d);
1416         w--;
1417     }
1418
1419     /* call prefetch hint to optimize cache load*/
1420     cachePrefetch ((__m128i*)ps);
1421     cachePrefetch ((__m128i*)pd);
1422     cachePrefetch ((__m128i*)pm);
1423
1424     while (w >= 4)
1425     {
1426         /* fill cache line with next memory */
1427         cachePrefetchNext ((__m128i*)ps);
1428         cachePrefetchNext ((__m128i*)pd);
1429         cachePrefetchNext ((__m128i*)pm);
1430
1431         xmmDstHi = load128Aligned ((__m128i*)pd);
1432         xmmSrcHi = load128Unaligned ((__m128i*)ps);
1433         xmmMaskHi = load128Unaligned ((__m128i*)pm);
1434
1435         unpack_128_2x128 (xmmDstHi, &xmmDstLo, &xmmDstHi);
1436         unpack_128_2x128 (xmmSrcHi, &xmmSrcLo, &xmmSrcHi);
1437         unpack_128_2x128 (xmmMaskHi, &xmmMaskLo, &xmmMaskHi);
1438
1439         expandAlpha_2x128 (xmmDstLo, xmmDstHi, &xmmAlphaLo, &xmmAlphaHi);
1440         pixMultiply_2x128 (&xmmSrcLo, &xmmSrcHi, &xmmMaskLo, &xmmMaskHi, &xmmMaskLo, &xmmMaskHi);
1441
1442         over_2x128 (&xmmDstLo, &xmmDstHi, &xmmAlphaLo, &xmmAlphaHi, &xmmMaskLo, &xmmMaskHi);
1443
1444         save128Aligned( (__m128i*)pd, pack_2x128_128 (xmmMaskLo, xmmMaskHi));
1445
1446         ps += 4;
1447         pd += 4;
1448         pm += 4;
1449         w -= 4;
1450     }
1451
1452     while (w)
1453     {
1454         s = *ps++;
1455         m = *pm++;
1456         d = *pd;
1457
1458         *pd++ = coreCombineOverReverseCPixelsse2 (s, m, d);
1459         w--;
1460     }
1461 }
1462
1463 static inline void
1464 coreCombineInCsse2 (uint32_t *pd, uint32_t *ps, uint32_t *pm, int w)
1465 {
1466     uint32_t s, m, d;
1467
1468     __m128i xmmAlphaLo, xmmAlphaHi;
1469     __m128i xmmSrcLo, xmmSrcHi;
1470     __m128i xmmDstLo, xmmDstHi;
1471     __m128i xmmMaskLo, xmmMaskHi;
1472
1473     /* call prefetch hint to optimize cache load*/
1474     cachePrefetch ((__m128i*)ps);
1475     cachePrefetch ((__m128i*)pd);
1476     cachePrefetch ((__m128i*)pm);
1477
1478     while (w && (unsigned long)pd & 15)
1479     {
1480         s = *ps++;
1481         m = *pm++;
1482         d = *pd;
1483
1484         *pd++ = pack_1x64_32 (pixMultiply_1x64 (pixMultiply_1x64 (unpack_32_1x64 (s), unpack_32_1x64 (m)),
1485                                                 expandAlpha_1x64 (unpack_32_1x64 (d))));
1486         w--;
1487     }
1488
1489     /* call prefetch hint to optimize cache load*/
1490     cachePrefetch ((__m128i*)ps);
1491     cachePrefetch ((__m128i*)pd);
1492     cachePrefetch ((__m128i*)pm);
1493
1494     while (w >= 4)
1495     {
1496         /* fill cache line with next memory */
1497         cachePrefetchNext ((__m128i*)ps);
1498         cachePrefetchNext ((__m128i*)pd);
1499         cachePrefetchNext ((__m128i*)pm);
1500
1501         xmmDstHi = load128Aligned ((__m128i*)pd);
1502         xmmSrcHi = load128Unaligned ((__m128i*)ps);
1503         xmmMaskHi = load128Unaligned ((__m128i*)pm);
1504
1505         unpack_128_2x128 (xmmDstHi, &xmmDstLo, &xmmDstHi);
1506         unpack_128_2x128 (xmmSrcHi, &xmmSrcLo, &xmmSrcHi);
1507         unpack_128_2x128 (xmmMaskHi, &xmmMaskLo, &xmmMaskHi);
1508
1509         expandAlpha_2x128 (xmmDstLo, xmmDstHi, &xmmAlphaLo, &xmmAlphaHi);
1510         pixMultiply_2x128 (&xmmSrcLo, &xmmSrcHi, &xmmMaskLo, &xmmMaskHi, &xmmDstLo, &xmmDstHi);
1511
1512         pixMultiply_2x128 (&xmmDstLo, &xmmDstHi, &xmmAlphaLo, &xmmAlphaHi, &xmmDstLo, &xmmDstHi);
1513
1514         save128Aligned( (__m128i*)pd, pack_2x128_128 (xmmDstLo, xmmDstHi));
1515
1516         ps += 4;
1517         pd += 4;
1518         pm += 4;
1519         w -= 4;
1520     }
1521
1522     while (w)
1523     {
1524         s = *ps++;
1525         m = *pm++;
1526         d = *pd;
1527
1528         *pd++ = pack_1x64_32 (pixMultiply_1x64 (pixMultiply_1x64 (unpack_32_1x64 (s), unpack_32_1x64 (m)),
1529                                                 expandAlpha_1x64 (unpack_32_1x64 (d))));
1530         w--;
1531     }
1532 }
1533
1534 static inline void
1535 coreCombineInReverseCsse2 (uint32_t *pd, uint32_t *ps, uint32_t *pm, int w)
1536 {
1537     uint32_t s, m, d;
1538
1539     __m128i xmmAlphaLo, xmmAlphaHi;
1540     __m128i xmmSrcLo, xmmSrcHi;
1541     __m128i xmmDstLo, xmmDstHi;
1542     __m128i xmmMaskLo, xmmMaskHi;
1543
1544     /* call prefetch hint to optimize cache load*/
1545     cachePrefetch ((__m128i*)ps);
1546     cachePrefetch ((__m128i*)pd);
1547     cachePrefetch ((__m128i*)pm);
1548
1549     while (w && (unsigned long)pd & 15)
1550     {
1551         s = *ps++;
1552         m = *pm++;
1553         d = *pd;
1554
1555         *pd++ = pack_1x64_32 (pixMultiply_1x64 (unpack_32_1x64 (d),
1556                                                 pixMultiply_1x64 (unpack_32_1x64 (m),
1557                                                                   expandAlpha_1x64 (unpack_32_1x64 (s)))));
1558         w--;
1559     }
1560
1561     /* call prefetch hint to optimize cache load*/
1562     cachePrefetch ((__m128i*)ps);
1563     cachePrefetch ((__m128i*)pd);
1564     cachePrefetch ((__m128i*)pm);
1565
1566     while (w >= 4)
1567     {
1568         /* fill cache line with next memory */
1569         cachePrefetchNext ((__m128i*)ps);
1570         cachePrefetchNext ((__m128i*)pd);
1571         cachePrefetchNext ((__m128i*)pm);
1572
1573         xmmDstHi = load128Aligned ((__m128i*)pd);
1574         xmmSrcHi = load128Unaligned ((__m128i*)ps);
1575         xmmMaskHi = load128Unaligned ((__m128i*)pm);
1576
1577         unpack_128_2x128 (xmmDstHi, &xmmDstLo, &xmmDstHi);
1578         unpack_128_2x128 (xmmSrcHi, &xmmSrcLo, &xmmSrcHi);
1579         unpack_128_2x128 (xmmMaskHi, &xmmMaskLo, &xmmMaskHi);
1580
1581         expandAlpha_2x128 (xmmSrcLo, xmmSrcHi, &xmmAlphaLo, &xmmAlphaHi);
1582         pixMultiply_2x128 (&xmmMaskLo, &xmmMaskHi, &xmmAlphaLo, &xmmAlphaHi, &xmmAlphaLo, &xmmAlphaHi);
1583
1584         pixMultiply_2x128 (&xmmDstLo, &xmmDstHi, &xmmAlphaLo, &xmmAlphaHi, &xmmDstLo, &xmmDstHi);
1585
1586         save128Aligned( (__m128i*)pd, pack_2x128_128 (xmmDstLo, xmmDstHi));
1587
1588         ps += 4;
1589         pd += 4;
1590         pm += 4;
1591         w -= 4;
1592     }
1593
1594     while (w)
1595     {
1596         s = *ps++;
1597         m = *pm++;
1598         d = *pd;
1599
1600         *pd++ = pack_1x64_32 (pixMultiply_1x64 (unpack_32_1x64 (d),
1601                                                 pixMultiply_1x64 (unpack_32_1x64 (m),
1602                                                                   expandAlpha_1x64 (unpack_32_1x64 (s)))));
1603         w--;
1604     }
1605 }
1606
1607 static inline void
1608 coreCombineOutCsse2 (uint32_t *pd, uint32_t *ps, uint32_t *pm, int w)
1609 {
1610     uint32_t s, m, d;
1611
1612     __m128i xmmAlphaLo, xmmAlphaHi;
1613     __m128i xmmSrcLo, xmmSrcHi;
1614     __m128i xmmDstLo, xmmDstHi;
1615     __m128i xmmMaskLo, xmmMaskHi;
1616
1617     /* call prefetch hint to optimize cache load*/
1618     cachePrefetch ((__m128i*)ps);
1619     cachePrefetch ((__m128i*)pd);
1620     cachePrefetch ((__m128i*)pm);
1621
1622     while (w && (unsigned long)pd & 15)
1623     {
1624         s = *ps++;
1625         m = *pm++;
1626         d = *pd;
1627
1628         *pd++ = pack_1x64_32 (pixMultiply_1x64 (pixMultiply_1x64 (unpack_32_1x64 (s), unpack_32_1x64 (m)),
1629                                                 negate_1x64 (expandAlpha_1x64 (unpack_32_1x64 (d)))));
1630         w--;
1631     }
1632
1633     /* call prefetch hint to optimize cache load*/
1634     cachePrefetch ((__m128i*)ps);
1635     cachePrefetch ((__m128i*)pd);
1636     cachePrefetch ((__m128i*)pm);
1637
1638     while (w >= 4)
1639     {
1640         /* fill cache line with next memory */
1641         cachePrefetchNext ((__m128i*)ps);
1642         cachePrefetchNext ((__m128i*)pd);
1643         cachePrefetchNext ((__m128i*)pm);
1644
1645         xmmDstHi = load128Aligned ((__m128i*)pd);
1646         xmmSrcHi = load128Unaligned ((__m128i*)ps);
1647         xmmMaskHi = load128Unaligned ((__m128i*)pm);
1648
1649         unpack_128_2x128 (xmmDstHi, &xmmDstLo, &xmmDstHi);
1650         unpack_128_2x128 (xmmSrcHi, &xmmSrcLo, &xmmSrcHi);
1651         unpack_128_2x128 (xmmMaskHi, &xmmMaskLo, &xmmMaskHi);
1652
1653         expandAlpha_2x128 (xmmDstLo, xmmDstHi, &xmmAlphaLo, &xmmAlphaHi);
1654         negate_2x128 (xmmAlphaLo, xmmAlphaHi, &xmmAlphaLo, &xmmAlphaHi);
1655
1656         pixMultiply_2x128 (&xmmSrcLo, &xmmSrcHi, &xmmMaskLo, &xmmMaskHi, &xmmDstLo, &xmmDstHi);
1657         pixMultiply_2x128 (&xmmDstLo, &xmmDstHi, &xmmAlphaLo, &xmmAlphaHi, &xmmDstLo, &xmmDstHi);
1658
1659         save128Aligned( (__m128i*)pd, pack_2x128_128 (xmmDstLo, xmmDstHi));
1660
1661         ps += 4;
1662         pd += 4;
1663         pm += 4;
1664         w -= 4;
1665     }
1666
1667     while (w)
1668     {
1669         s = *ps++;
1670         m = *pm++;
1671         d = *pd;
1672
1673         *pd++ = pack_1x64_32 (pixMultiply_1x64 (pixMultiply_1x64 (unpack_32_1x64 (s), unpack_32_1x64 (m)),
1674                                                 negate_1x64 (expandAlpha_1x64 (unpack_32_1x64 (d)))));
1675         w--;
1676     }
1677 }
1678
1679 static inline void
1680 coreCombineOutReverseCsse2 (uint32_t *pd, uint32_t *ps, uint32_t *pm, int w)
1681 {
1682     uint32_t s, m, d;
1683
1684     __m128i xmmAlphaLo, xmmAlphaHi;
1685     __m128i xmmSrcLo, xmmSrcHi;
1686     __m128i xmmDstLo, xmmDstHi;
1687     __m128i xmmMaskLo, xmmMaskHi;
1688
1689     /* call prefetch hint to optimize cache load*/
1690     cachePrefetch ((__m128i*)ps);
1691     cachePrefetch ((__m128i*)pd);
1692     cachePrefetch ((__m128i*)pm);
1693
1694     while (w && (unsigned long)pd & 15)
1695     {
1696         s = *ps++;
1697         m = *pm++;
1698         d = *pd;
1699
1700         *pd++ = pack_1x64_32 (pixMultiply_1x64 (unpack_32_1x64 (d),
1701                                                 negate_1x64 (pixMultiply_1x64 (unpack_32_1x64 (m),
1702                                                                                expandAlpha_1x64 (unpack_32_1x64 (s))))));
1703         w--;
1704     }
1705
1706     /* call prefetch hint to optimize cache load*/
1707     cachePrefetch ((__m128i*)ps);
1708     cachePrefetch ((__m128i*)pd);
1709     cachePrefetch ((__m128i*)pm);
1710
1711     while (w >= 4)
1712     {
1713         /* fill cache line with next memory */
1714         cachePrefetchNext ((__m128i*)ps);
1715         cachePrefetchNext ((__m128i*)pd);
1716         cachePrefetchNext ((__m128i*)pm);
1717
1718         xmmDstHi = load128Aligned ((__m128i*)pd);
1719         xmmSrcHi = load128Unaligned ((__m128i*)ps);
1720         xmmMaskHi = load128Unaligned ((__m128i*)pm);
1721
1722         unpack_128_2x128 (xmmDstHi, &xmmDstLo, &xmmDstHi);
1723         unpack_128_2x128 (xmmSrcHi, &xmmSrcLo, &xmmSrcHi);
1724         unpack_128_2x128 (xmmMaskHi, &xmmMaskLo, &xmmMaskHi);
1725
1726         expandAlpha_2x128 (xmmSrcLo, xmmSrcHi, &xmmAlphaLo, &xmmAlphaHi);
1727
1728         pixMultiply_2x128 (&xmmMaskLo, &xmmMaskHi, &xmmAlphaLo, &xmmAlphaHi, &xmmMaskLo, &xmmMaskHi);
1729
1730         negate_2x128 (xmmMaskLo, xmmMaskHi, &xmmMaskLo, &xmmMaskHi);
1731
1732         pixMultiply_2x128 (&xmmDstLo, &xmmDstHi, &xmmMaskLo, &xmmMaskHi, &xmmDstLo, &xmmDstHi);
1733
1734         save128Aligned( (__m128i*)pd, pack_2x128_128 (xmmDstLo, xmmDstHi));
1735
1736         ps += 4;
1737         pd += 4;
1738         pm += 4;
1739         w -= 4;
1740     }
1741
1742     while (w)
1743     {
1744         s = *ps++;
1745         m = *pm++;
1746         d = *pd;
1747
1748         *pd++ = pack_1x64_32 (pixMultiply_1x64 (unpack_32_1x64 (d),
1749                                                 negate_1x64 (pixMultiply_1x64 (unpack_32_1x64 (m),
1750                                                                                expandAlpha_1x64 (unpack_32_1x64 (s))))));
1751         w--;
1752     }
1753 }
1754
1755 static inline uint32_t
1756 coreCombineAtopCPixelsse2 (uint32_t src, uint32_t mask, uint32_t dst)
1757 {
1758     __m64 m = unpack_32_1x64 (mask);
1759     __m64 s = unpack_32_1x64 (src);
1760     __m64 d = unpack_32_1x64 (dst);
1761     __m64 sa = expandAlpha_1x64 (s);
1762     __m64 da = expandAlpha_1x64 (d);
1763
1764     s = pixMultiply_1x64 (s, m);
1765     m = negate_1x64 (pixMultiply_1x64 (m, sa));
1766
1767     return pack_1x64_32 (pixAddMultiply_1x64 (&d, &m, &s, &da));
1768 }
1769
1770 static inline void
1771 coreCombineAtopCsse2 (uint32_t *pd, uint32_t *ps, uint32_t *pm, int w)
1772 {
1773     uint32_t s, m, d;
1774
1775     __m128i xmmSrcLo, xmmSrcHi;
1776     __m128i xmmDstLo, xmmDstHi;
1777     __m128i xmmAlphaSrcLo, xmmAlphaSrcHi;
1778     __m128i xmmAlphaDstLo, xmmAlphaDstHi;
1779     __m128i xmmMaskLo, xmmMaskHi;
1780
1781     /* call prefetch hint to optimize cache load*/
1782     cachePrefetch ((__m128i*)ps);
1783     cachePrefetch ((__m128i*)pd);
1784     cachePrefetch ((__m128i*)pm);
1785
1786     while (w && (unsigned long)pd & 15)
1787     {
1788         s = *ps++;
1789         m = *pm++;
1790         d = *pd;
1791
1792         *pd++ = coreCombineAtopCPixelsse2 (s, m, d);
1793         w--;
1794     }
1795
1796     /* call prefetch hint to optimize cache load*/
1797     cachePrefetch ((__m128i*)ps);
1798     cachePrefetch ((__m128i*)pd);
1799     cachePrefetch ((__m128i*)pm);
1800
1801     while (w >= 4)
1802     {
1803         /* fill cache line with next memory */
1804         cachePrefetchNext ((__m128i*)ps);
1805         cachePrefetchNext ((__m128i*)pd);
1806         cachePrefetchNext ((__m128i*)pm);
1807
1808         xmmDstHi = load128Aligned ((__m128i*)pd);
1809         xmmSrcHi = load128Unaligned ((__m128i*)ps);
1810         xmmMaskHi = load128Unaligned ((__m128i*)pm);
1811
1812         unpack_128_2x128 (xmmDstHi, &xmmDstLo, &xmmDstHi);
1813         unpack_128_2x128 (xmmSrcHi, &xmmSrcLo, &xmmSrcHi);
1814         unpack_128_2x128 (xmmMaskHi, &xmmMaskLo, &xmmMaskHi);
1815
1816         expandAlpha_2x128 (xmmSrcLo, xmmSrcHi, &xmmAlphaSrcLo, &xmmAlphaSrcHi);
1817         expandAlpha_2x128 (xmmDstLo, xmmDstHi, &xmmAlphaDstLo, &xmmAlphaDstHi);
1818
1819         pixMultiply_2x128 (&xmmSrcLo, &xmmSrcHi, &xmmMaskLo, &xmmMaskHi, &xmmSrcLo, &xmmSrcHi);
1820         pixMultiply_2x128 (&xmmMaskLo, &xmmMaskHi, &xmmAlphaSrcLo, &xmmAlphaSrcHi, &xmmMaskLo, &xmmMaskHi);
1821
1822         negate_2x128 (xmmMaskLo, xmmMaskHi, &xmmMaskLo, &xmmMaskHi);
1823
1824         pixAddMultiply_2x128 (&xmmDstLo, &xmmDstHi, &xmmMaskLo, &xmmMaskHi,
1825                               &xmmSrcLo, &xmmSrcHi, &xmmAlphaDstLo, &xmmAlphaDstHi,
1826                               &xmmDstLo, &xmmDstHi);
1827
1828         save128Aligned( (__m128i*)pd, pack_2x128_128 (xmmDstLo, xmmDstHi));
1829
1830         ps += 4;
1831         pd += 4;
1832         pm += 4;
1833         w -= 4;
1834     }
1835
1836     while (w)
1837     {
1838         s = *ps++;
1839         m = *pm++;
1840         d = *pd;
1841
1842         *pd++ = coreCombineAtopCPixelsse2 (s, m, d);
1843         w--;
1844     }
1845 }
1846
1847 static inline uint32_t
1848 coreCombineReverseAtopCPixelsse2 (uint32_t src, uint32_t mask, uint32_t dst)
1849 {
1850     __m64 m = unpack_32_1x64 (mask);
1851     __m64 s = unpack_32_1x64 (src);
1852     __m64 d = unpack_32_1x64 (dst);
1853
1854     __m64 da = negate_1x64 (expandAlpha_1x64 (d));
1855     __m64 sa = expandAlpha_1x64 (s);
1856
1857     s = pixMultiply_1x64 (s, m);
1858     m = pixMultiply_1x64 (m, sa);
1859
1860     return pack_1x64_32 (pixAddMultiply_1x64 (&d, &m, &s, &da));
1861 }
1862
1863 static inline void
1864 coreCombineReverseAtopCsse2 (uint32_t *pd, uint32_t *ps, uint32_t *pm, int w)
1865 {
1866     uint32_t s, m, d;
1867
1868     __m128i xmmSrcLo, xmmSrcHi;
1869     __m128i xmmDstLo, xmmDstHi;
1870     __m128i xmmAlphaSrcLo, xmmAlphaSrcHi;
1871     __m128i xmmAlphaDstLo, xmmAlphaDstHi;
1872     __m128i xmmMaskLo, xmmMaskHi;
1873
1874     /* call prefetch hint to optimize cache load*/
1875     cachePrefetch ((__m128i*)ps);
1876     cachePrefetch ((__m128i*)pd);
1877     cachePrefetch ((__m128i*)pm);
1878
1879     while (w && (unsigned long)pd & 15)
1880     {
1881         s = *ps++;
1882         m = *pm++;
1883         d = *pd;
1884
1885         *pd++ = coreCombineReverseAtopCPixelsse2 (s, m, d);
1886         w--;
1887     }
1888
1889     /* call prefetch hint to optimize cache load*/
1890     cachePrefetch ((__m128i*)ps);
1891     cachePrefetch ((__m128i*)pd);
1892     cachePrefetch ((__m128i*)pm);
1893
1894     while (w >= 4)
1895     {
1896         /* fill cache line with next memory */
1897         cachePrefetchNext ((__m128i*)ps);
1898         cachePrefetchNext ((__m128i*)pd);
1899         cachePrefetchNext ((__m128i*)pm);
1900
1901         xmmDstHi = load128Aligned ((__m128i*)pd);
1902         xmmSrcHi = load128Unaligned ((__m128i*)ps);
1903         xmmMaskHi = load128Unaligned ((__m128i*)pm);
1904
1905         unpack_128_2x128 (xmmDstHi, &xmmDstLo, &xmmDstHi);
1906         unpack_128_2x128 (xmmSrcHi, &xmmSrcLo, &xmmSrcHi);
1907         unpack_128_2x128 (xmmMaskHi, &xmmMaskLo, &xmmMaskHi);
1908
1909         expandAlpha_2x128 (xmmSrcLo, xmmSrcHi, &xmmAlphaSrcLo, &xmmAlphaSrcHi);
1910         expandAlpha_2x128 (xmmDstLo, xmmDstHi, &xmmAlphaDstLo, &xmmAlphaDstHi);
1911
1912         pixMultiply_2x128 (&xmmSrcLo, &xmmSrcHi, &xmmMaskLo, &xmmMaskHi, &xmmSrcLo, &xmmSrcHi);
1913         pixMultiply_2x128 (&xmmMaskLo, &xmmMaskHi, &xmmAlphaSrcLo, &xmmAlphaSrcHi, &xmmMaskLo, &xmmMaskHi);
1914
1915         negate_2x128 (xmmAlphaDstLo, xmmAlphaDstHi, &xmmAlphaDstLo, &xmmAlphaDstHi);
1916
1917         pixAddMultiply_2x128 (&xmmDstLo, &xmmDstHi, &xmmMaskLo, &xmmMaskHi,
1918                               &xmmSrcLo, &xmmSrcHi, &xmmAlphaDstLo, &xmmAlphaDstHi,
1919                               &xmmDstLo, &xmmDstHi);
1920
1921         save128Aligned( (__m128i*)pd, pack_2x128_128 (xmmDstLo, xmmDstHi));
1922
1923         ps += 4;
1924         pd += 4;
1925         pm += 4;
1926         w -= 4;
1927     }
1928
1929     while (w)
1930     {
1931         s = *ps++;
1932         m = *pm++;
1933         d = *pd;
1934
1935         *pd++ = coreCombineReverseAtopCPixelsse2 (s, m, d);
1936         w--;
1937     }
1938 }
1939
1940 static inline uint32_t
1941 coreCombineXorCPixelsse2 (uint32_t src, uint32_t mask, uint32_t dst)
1942 {
1943     __m64 a = unpack_32_1x64 (mask);
1944     __m64 s = unpack_32_1x64 (src);
1945     __m64 d = unpack_32_1x64 (dst);
1946
1947     __m64 alphaDst = negate_1x64 (pixMultiply_1x64 (a, expandAlpha_1x64 (s)));
1948     __m64 dest      = pixMultiply_1x64 (s, a);
1949     __m64 alphaSrc = negate_1x64 (expandAlpha_1x64 (d));
1950
1951     return pack_1x64_32 (pixAddMultiply_1x64 (&d,
1952                                               &alphaDst,
1953                                               &dest,
1954                                               &alphaSrc));
1955 }
1956
1957 static inline void
1958 coreCombineXorCsse2 (uint32_t *pd, uint32_t *ps, uint32_t *pm, int w)
1959 {
1960     uint32_t s, m, d;
1961
1962     __m128i xmmSrcLo, xmmSrcHi;
1963     __m128i xmmDstLo, xmmDstHi;
1964     __m128i xmmAlphaSrcLo, xmmAlphaSrcHi;
1965     __m128i xmmAlphaDstLo, xmmAlphaDstHi;
1966     __m128i xmmMaskLo, xmmMaskHi;
1967
1968     /* call prefetch hint to optimize cache load*/
1969     cachePrefetch ((__m128i*)ps);
1970     cachePrefetch ((__m128i*)pd);
1971     cachePrefetch ((__m128i*)pm);
1972
1973     while (w && (unsigned long)pd & 15)
1974     {
1975         s = *ps++;
1976         m = *pm++;
1977         d = *pd;
1978
1979         *pd++ = coreCombineXorCPixelsse2 (s, m, d);
1980         w--;
1981     }
1982
1983     /* call prefetch hint to optimize cache load*/
1984     cachePrefetch ((__m128i*)ps);
1985     cachePrefetch ((__m128i*)pd);
1986     cachePrefetch ((__m128i*)pm);
1987
1988     while (w >= 4)
1989     {
1990         /* fill cache line with next memory */
1991         cachePrefetchNext ((__m128i*)ps);
1992         cachePrefetchNext ((__m128i*)pd);
1993         cachePrefetchNext ((__m128i*)pm);
1994
1995         xmmDstHi = load128Aligned ((__m128i*)pd);
1996         xmmSrcHi = load128Unaligned ((__m128i*)ps);
1997         xmmMaskHi = load128Unaligned ((__m128i*)pm);
1998
1999         unpack_128_2x128 (xmmDstHi, &xmmDstLo, &xmmDstHi);
2000         unpack_128_2x128 (xmmSrcHi, &xmmSrcLo, &xmmSrcHi);
2001         unpack_128_2x128 (xmmMaskHi, &xmmMaskLo, &xmmMaskHi);
2002
2003         expandAlpha_2x128 (xmmSrcLo, xmmSrcHi, &xmmAlphaSrcLo, &xmmAlphaSrcHi);
2004         expandAlpha_2x128 (xmmDstLo, xmmDstHi, &xmmAlphaDstLo, &xmmAlphaDstHi);
2005
2006         pixMultiply_2x128 (&xmmSrcLo, &xmmSrcHi, &xmmMaskLo, &xmmMaskHi, &xmmSrcLo, &xmmSrcHi);
2007         pixMultiply_2x128 (&xmmMaskLo, &xmmMaskHi, &xmmAlphaSrcLo, &xmmAlphaSrcHi, &xmmMaskLo, &xmmMaskHi);
2008
2009         negate_2x128 (xmmAlphaDstLo, xmmAlphaDstHi, &xmmAlphaDstLo, &xmmAlphaDstHi);
2010         negate_2x128 (xmmMaskLo, xmmMaskHi, &xmmMaskLo, &xmmMaskHi);
2011
2012         pixAddMultiply_2x128 (&xmmDstLo, &xmmDstHi, &xmmMaskLo, &xmmMaskHi,
2013                               &xmmSrcLo, &xmmSrcHi, &xmmAlphaDstLo, &xmmAlphaDstHi,
2014                               &xmmDstLo, &xmmDstHi);
2015
2016         save128Aligned( (__m128i*)pd, pack_2x128_128 (xmmDstLo, xmmDstHi));
2017
2018         ps += 4;
2019         pd += 4;
2020         pm += 4;
2021         w -= 4;
2022     }
2023
2024     while (w)
2025     {
2026         s = *ps++;
2027         m = *pm++;
2028         d = *pd;
2029
2030         *pd++ = coreCombineXorCPixelsse2 (s, m, d);
2031         w--;
2032     }
2033 }
2034
2035 static inline void
2036 coreCombineAddCsse2 (uint32_t *pd, uint32_t *ps, uint32_t *pm, int w)
2037 {
2038     uint32_t s, m, d;
2039
2040     __m128i xmmSrcLo, xmmSrcHi;
2041     __m128i xmmDstLo, xmmDstHi;
2042     __m128i xmmMaskLo, xmmMaskHi;
2043
2044     /* call prefetch hint to optimize cache load*/
2045     cachePrefetch ((__m128i*)ps);
2046     cachePrefetch ((__m128i*)pd);
2047     cachePrefetch ((__m128i*)pm);
2048
2049     while (w && (unsigned long)pd & 15)
2050     {
2051         s = *ps++;
2052         m = *pm++;
2053         d = *pd;
2054
2055         *pd++ = pack_1x64_32 (_mm_adds_pu8 (pixMultiply_1x64 (unpack_32_1x64 (s),
2056                                                               unpack_32_1x64 (m)),
2057                                             unpack_32_1x64 (d)));
2058         w--;
2059     }
2060
2061     /* call prefetch hint to optimize cache load*/
2062     cachePrefetch ((__m128i*)ps);
2063     cachePrefetch ((__m128i*)pd);
2064     cachePrefetch ((__m128i*)pm);
2065
2066     while (w >= 4)
2067     {
2068         /* fill cache line with next memory */
2069         cachePrefetchNext ((__m128i*)ps);
2070         cachePrefetchNext ((__m128i*)pd);
2071         cachePrefetchNext ((__m128i*)pm);
2072
2073         xmmSrcHi = load128Unaligned ((__m128i*)ps);
2074         xmmMaskHi = load128Unaligned ((__m128i*)pm);
2075         xmmDstHi = load128Aligned ((__m128i*)pd);
2076
2077         unpack_128_2x128 (xmmSrcHi, &xmmSrcLo, &xmmSrcHi);
2078         unpack_128_2x128 (xmmMaskHi, &xmmMaskLo, &xmmMaskHi);
2079         unpack_128_2x128 (xmmDstHi, &xmmDstLo, &xmmDstHi);
2080
2081         pixMultiply_2x128 (&xmmSrcLo, &xmmSrcHi, &xmmMaskLo, &xmmMaskHi, &xmmSrcLo, &xmmSrcHi);
2082
2083         save128Aligned( (__m128i*)pd, pack_2x128_128 (_mm_adds_epu8 (xmmSrcLo, xmmDstLo),
2084                                                       _mm_adds_epu8 (xmmSrcHi, xmmDstHi)));
2085
2086         ps += 4;
2087         pd += 4;
2088         pm += 4;
2089         w -= 4;
2090     }
2091
2092     while (w)
2093     {
2094         s = *ps++;
2095         m = *pm++;
2096         d = *pd;
2097
2098         *pd++ = pack_1x64_32 (_mm_adds_pu8 (pixMultiply_1x64 (unpack_32_1x64 (s),
2099                                                               unpack_32_1x64 (m)),
2100                                             unpack_32_1x64 (d)));
2101         w--;
2102     }
2103 }
2104
2105 /* -------------------------------------------------------------------------------------------------
2106  * fbComposeSetupSSE2
2107  */
2108 static inline __m64
2109 createMask_16_64 (uint16_t mask)
2110 {
2111     return _mm_set1_pi16 (mask);
2112 }
2113
2114 static inline __m128i
2115 createMask_16_128 (uint16_t mask)
2116 {
2117     return _mm_set1_epi16 (mask);
2118 }
2119
2120 static inline __m64
2121 createMask_2x32_64 (uint32_t mask0, uint32_t mask1)
2122 {
2123     return _mm_set_pi32 (mask0, mask1);
2124 }
2125
2126 static inline __m128i
2127 createMask_2x32_128 (uint32_t mask0, uint32_t mask1)
2128 {
2129     return _mm_set_epi32 (mask0, mask1, mask0, mask1);
2130 }
2131
2132 /* SSE2 code patch for fbcompose.c */
2133
2134 static FASTCALL void
2135 sse2CombineMaskU (uint32_t *dst, const uint32_t *src, int width)
2136 {
2137     coreCombineReverseInUsse2 (dst, src, width);
2138     _mm_empty();
2139 }
2140
2141 static FASTCALL void
2142 sse2CombineOverU (uint32_t *dst, const uint32_t *src, int width)
2143 {
2144     coreCombineOverUsse2 (dst, src, width);
2145     _mm_empty();
2146 }
2147
2148 static FASTCALL void
2149 sse2CombineOverReverseU (uint32_t *dst, const uint32_t *src, int width)
2150 {
2151     coreCombineOverReverseUsse2 (dst, src, width);
2152     _mm_empty();
2153 }
2154
2155 static FASTCALL void
2156 sse2CombineInU (uint32_t *dst, const uint32_t *src, int width)
2157 {
2158     coreCombineInUsse2 (dst, src, width);
2159     _mm_empty();
2160 }
2161
2162 static FASTCALL void
2163 sse2CombineInReverseU (uint32_t *dst, const uint32_t *src, int width)
2164 {
2165     coreCombineReverseInUsse2 (dst, src, width);
2166     _mm_empty();
2167 }
2168
2169 static FASTCALL void
2170 sse2CombineOutU (uint32_t *dst, const uint32_t *src, int width)
2171 {
2172     coreCombineOutUsse2 (dst, src, width);
2173     _mm_empty();
2174 }
2175
2176 static FASTCALL void
2177 sse2CombineOutReverseU (uint32_t *dst, const uint32_t *src, int width)
2178 {
2179     coreCombineReverseOutUsse2 (dst, src, width);
2180     _mm_empty();
2181 }
2182
2183 static FASTCALL void
2184 sse2CombineAtopU (uint32_t *dst, const uint32_t *src, int width)
2185 {
2186     coreCombineAtopUsse2 (dst, src, width);
2187     _mm_empty();
2188 }
2189
2190 static FASTCALL void
2191 sse2CombineAtopReverseU (uint32_t *dst, const uint32_t *src, int width)
2192 {
2193     coreCombineReverseAtopUsse2 (dst, src, width);
2194     _mm_empty();
2195 }
2196
2197 static FASTCALL void
2198 sse2CombineXorU (uint32_t *dst, const uint32_t *src, int width)
2199 {
2200     coreCombineXorUsse2 (dst, src, width);
2201     _mm_empty();
2202 }
2203
2204 static FASTCALL void
2205 sse2CombineAddU (uint32_t *dst, const uint32_t *src, int width)
2206 {
2207     coreCombineAddUsse2 (dst, src, width);
2208     _mm_empty();
2209 }
2210
2211 static FASTCALL void
2212 sse2CombineSaturateU (uint32_t *dst, const uint32_t *src, int width)
2213 {
2214     coreCombineSaturateUsse2 (dst, src, width);
2215     _mm_empty();
2216 }
2217
2218 static FASTCALL void
2219 sse2CombineSrcC (uint32_t *dst, uint32_t *src, uint32_t *mask, int width)
2220 {
2221     coreCombineSrcCsse2 (dst, src, mask, width);
2222     _mm_empty();
2223 }
2224
2225 static FASTCALL void
2226 sse2CombineOverC (uint32_t *dst, uint32_t *src, uint32_t *mask, int width)
2227 {
2228     coreCombineOverCsse2 (dst, src, mask, width);
2229     _mm_empty();
2230 }
2231
2232 static FASTCALL void
2233 sse2CombineOverReverseC (uint32_t *dst, uint32_t *src, uint32_t *mask, int width)
2234 {
2235     coreCombineOverReverseCsse2 (dst, src, mask, width);
2236     _mm_empty();
2237 }
2238
2239 static FASTCALL void
2240 sse2CombineInC (uint32_t *dst, uint32_t *src, uint32_t *mask, int width)
2241 {
2242     coreCombineInCsse2 (dst, src, mask, width);
2243     _mm_empty();
2244 }
2245
2246 static FASTCALL void
2247 sse2CombineInReverseC (uint32_t *dst, uint32_t *src, uint32_t *mask, int width)
2248 {
2249     coreCombineInReverseCsse2 (dst, src, mask, width);
2250     _mm_empty();
2251 }
2252
2253 static FASTCALL void
2254 sse2CombineOutC (uint32_t *dst, uint32_t *src, uint32_t *mask, int width)
2255 {
2256     coreCombineOutCsse2 (dst, src, mask, width);
2257     _mm_empty();
2258 }
2259
2260 static FASTCALL void
2261 sse2CombineOutReverseC (uint32_t *dst, uint32_t *src, uint32_t *mask, int width)
2262 {
2263     coreCombineOutReverseCsse2 (dst, src, mask, width);
2264     _mm_empty();
2265 }
2266
2267 static FASTCALL void
2268 sse2CombineAtopC (uint32_t *dst, uint32_t *src, uint32_t *mask, int width)
2269 {
2270     coreCombineAtopCsse2 (dst, src, mask, width);
2271     _mm_empty();
2272 }
2273
2274 static FASTCALL void
2275 sse2CombineAtopReverseC (uint32_t *dst, uint32_t *src, uint32_t *mask, int width)
2276 {
2277     coreCombineReverseAtopCsse2 (dst, src, mask, width);
2278     _mm_empty();
2279 }
2280
2281 static FASTCALL void
2282 sse2CombineXorC (uint32_t *dst, uint32_t *src, uint32_t *mask, int width)
2283 {
2284     coreCombineXorCsse2 (dst, src, mask, width);
2285     _mm_empty();
2286 }
2287
2288 static FASTCALL void
2289 sse2CombineAddC (uint32_t *dst, uint32_t *src, uint32_t *mask, int width)
2290 {
2291     coreCombineAddCsse2 (dst, src, mask, width);
2292     _mm_empty();
2293 }
2294
2295 void
2296 fbComposeSetupSSE2(void)
2297 {
2298     static pixman_bool_t initialized = FALSE;
2299
2300     if (initialized)
2301         return;
2302
2303     /* check if we have SSE2 support and initialize accordingly */
2304     if (pixman_have_sse2())
2305     {
2306         /* SSE2 constants */
2307         Mask565r  = createMask_2x32_128 (0x00f80000, 0x00f80000);
2308         Mask565g1 = createMask_2x32_128 (0x00070000, 0x00070000);
2309         Mask565g2 = createMask_2x32_128 (0x000000e0, 0x000000e0);
2310         Mask565b  = createMask_2x32_128 (0x0000001f, 0x0000001f);
2311         MaskRed   = createMask_2x32_128 (0x00f80000, 0x00f80000);
2312         MaskGreen = createMask_2x32_128 (0x0000fc00, 0x0000fc00);
2313         MaskBlue  = createMask_2x32_128 (0x000000f8, 0x000000f8);
2314         Mask565FixRB = createMask_2x32_128 (0x00e000e0, 0x00e000e0);
2315         Mask565FixG = createMask_2x32_128  (0x0000c000, 0x0000c000);
2316         Mask0080 = createMask_16_128 (0x0080);
2317         Mask00ff = createMask_16_128 (0x00ff);
2318         Mask0101 = createMask_16_128 (0x0101);
2319         Maskffff = createMask_16_128 (0xffff);
2320         Maskff000000 = createMask_2x32_128 (0xff000000, 0xff000000);
2321         MaskAlpha = createMask_2x32_128 (0x00ff0000, 0x00000000);
2322
2323         /* MMX constants */
2324         xMask565rgb = createMask_2x32_64 (0x000001f0, 0x003f001f);
2325         xMask565Unpack = createMask_2x32_64 (0x00000084, 0x04100840);
2326
2327         xMask0080 = createMask_16_64 (0x0080);
2328         xMask00ff = createMask_16_64 (0x00ff);
2329         xMask0101 = createMask_16_64 (0x0101);
2330         xMaskAlpha = createMask_2x32_64 (0x00ff0000, 0x00000000);
2331
2332         /* SSE code patch for fbcompose.c */
2333         pixman_composeFunctions.combineU[PIXMAN_OP_OVER] = sse2CombineOverU;
2334         pixman_composeFunctions.combineU[PIXMAN_OP_OVER_REVERSE] = sse2CombineOverReverseU;
2335         pixman_composeFunctions.combineU[PIXMAN_OP_IN] = sse2CombineInU;
2336         pixman_composeFunctions.combineU[PIXMAN_OP_IN_REVERSE] = sse2CombineInReverseU;
2337         pixman_composeFunctions.combineU[PIXMAN_OP_OUT] = sse2CombineOutU;
2338
2339         pixman_composeFunctions.combineU[PIXMAN_OP_OUT_REVERSE] = sse2CombineOutReverseU;
2340         pixman_composeFunctions.combineU[PIXMAN_OP_ATOP] = sse2CombineAtopU;
2341         pixman_composeFunctions.combineU[PIXMAN_OP_ATOP_REVERSE] = sse2CombineAtopReverseU;
2342         pixman_composeFunctions.combineU[PIXMAN_OP_XOR] = sse2CombineXorU;
2343         pixman_composeFunctions.combineU[PIXMAN_OP_ADD] = sse2CombineAddU;
2344
2345         pixman_composeFunctions.combineU[PIXMAN_OP_SATURATE] = sse2CombineSaturateU;
2346
2347         pixman_composeFunctions.combineC[PIXMAN_OP_SRC] = sse2CombineSrcC;
2348         pixman_composeFunctions.combineC[PIXMAN_OP_OVER] = sse2CombineOverC;
2349         pixman_composeFunctions.combineC[PIXMAN_OP_OVER_REVERSE] = sse2CombineOverReverseC;
2350         pixman_composeFunctions.combineC[PIXMAN_OP_IN] = sse2CombineInC;
2351         pixman_composeFunctions.combineC[PIXMAN_OP_IN_REVERSE] = sse2CombineInReverseC;
2352         pixman_composeFunctions.combineC[PIXMAN_OP_OUT] = sse2CombineOutC;
2353         pixman_composeFunctions.combineC[PIXMAN_OP_OUT_REVERSE] = sse2CombineOutReverseC;
2354         pixman_composeFunctions.combineC[PIXMAN_OP_ATOP] = sse2CombineAtopC;
2355         pixman_composeFunctions.combineC[PIXMAN_OP_ATOP_REVERSE] = sse2CombineAtopReverseC;
2356         pixman_composeFunctions.combineC[PIXMAN_OP_XOR] = sse2CombineXorC;
2357         pixman_composeFunctions.combineC[PIXMAN_OP_ADD] = sse2CombineAddC;
2358
2359         pixman_composeFunctions.combineMaskU = sse2CombineMaskU;
2360     }
2361
2362     initialized = TRUE;
2363
2364     _mm_empty();
2365 }
2366
2367
2368 /* -------------------------------------------------------------------------------------------------
2369  * fbCompositeSolid_nx8888
2370  */
2371
2372 void
2373 fbCompositeSolid_nx8888sse2 (pixman_op_t op,
2374                             pixman_image_t * pSrc,
2375                             pixman_image_t * pMask,
2376                             pixman_image_t * pDst,
2377                             int16_t     xSrc,
2378                             int16_t     ySrc,
2379                             int16_t     xMask,
2380                             int16_t     yMask,
2381                             int16_t     xDst,
2382                             int16_t     yDst,
2383                             uint16_t    width,
2384                             uint16_t    height)
2385 {
2386     uint32_t    src;
2387     uint32_t    *dstLine, *dst, d;
2388     uint16_t    w;
2389     int dstStride;
2390     __m128i xmmSrc, xmmAlpha;
2391     __m128i xmmDst, xmmDstLo, xmmDstHi;
2392
2393     fbComposeGetSolid(pSrc, src, pDst->bits.format);
2394
2395     if (src >> 24 == 0)
2396         return;
2397
2398     fbComposeGetStart (pDst, xDst, yDst, uint32_t, dstStride, dstLine, 1);
2399
2400     xmmSrc = expandPixel_32_1x128 (src);
2401     xmmAlpha = expandAlpha_1x128 (xmmSrc);
2402
2403     while (height--)
2404     {
2405         dst = dstLine;
2406
2407         /* call prefetch hint to optimize cache load*/
2408         cachePrefetch ((__m128i*)dst);
2409
2410         dstLine += dstStride;
2411         w = width;
2412
2413         while (w && (unsigned long)dst & 15)
2414         {
2415             d = *dst;
2416             *dst++ = pack_1x64_32 (over_1x64 (_mm_movepi64_pi64 (xmmSrc),
2417                                               _mm_movepi64_pi64 (xmmAlpha),
2418                                               unpack_32_1x64 (d)));
2419             w--;
2420         }
2421
2422         cachePrefetch ((__m128i*)dst);
2423
2424         while (w >= 4)
2425         {
2426             /* fill cache line with next memory */
2427             cachePrefetchNext ((__m128i*)dst);
2428
2429             xmmDst = load128Aligned ((__m128i*)dst);
2430
2431             unpack_128_2x128 (xmmDst, &xmmDstLo, &xmmDstHi);
2432
2433             over_2x128 (&xmmSrc, &xmmSrc, &xmmAlpha, &xmmAlpha, &xmmDstLo, &xmmDstHi);
2434
2435             /* rebuid the 4 pixel data and save*/
2436             save128Aligned ((__m128i*)dst, pack_2x128_128 (xmmDstLo, xmmDstHi));
2437
2438             w -= 4;
2439             dst += 4;
2440         }
2441
2442         while (w)
2443         {
2444             d = *dst;
2445             *dst++ = pack_1x64_32 (over_1x64 (_mm_movepi64_pi64 (xmmSrc),
2446                                               _mm_movepi64_pi64 (xmmAlpha),
2447                                               unpack_32_1x64 (d)));
2448             w--;
2449         }
2450
2451     }
2452     _mm_empty();
2453 }
2454
2455 /* -------------------------------------------------------------------------------------------------
2456  * fbCompositeSolid_nx0565
2457  */
2458 void
2459 fbCompositeSolid_nx0565sse2 (pixman_op_t op,
2460                             pixman_image_t * pSrc,
2461                             pixman_image_t * pMask,
2462                             pixman_image_t * pDst,
2463                             int16_t     xSrc,
2464                             int16_t     ySrc,
2465                             int16_t     xMask,
2466                             int16_t     yMask,
2467                             int16_t     xDst,
2468                             int16_t     yDst,
2469                             uint16_t    width,
2470                             uint16_t    height)
2471 {
2472     uint32_t    src;
2473     uint16_t    *dstLine, *dst, d;
2474     uint16_t    w;
2475     int         dstStride;
2476     __m128i xmmSrc, xmmAlpha;
2477     __m128i xmmDst, xmmDst0, xmmDst1, xmmDst2, xmmDst3;
2478
2479     fbComposeGetSolid(pSrc, src, pDst->bits.format);
2480
2481     if (src >> 24 == 0)
2482         return;
2483
2484     fbComposeGetStart (pDst, xDst, yDst, uint16_t, dstStride, dstLine, 1);
2485
2486     xmmSrc = expandPixel_32_1x128 (src);
2487     xmmAlpha = expandAlpha_1x128 (xmmSrc);
2488
2489     while (height--)
2490     {
2491         dst = dstLine;
2492
2493         /* call prefetch hint to optimize cache load*/
2494         cachePrefetch ((__m128i*)dst);
2495
2496         dstLine += dstStride;
2497         w = width;
2498
2499         while (w && (unsigned long)dst & 15)
2500         {
2501             d = *dst;
2502
2503             *dst++ = pack565_32_16 (pack_1x64_32 (over_1x64 (_mm_movepi64_pi64 (xmmSrc),
2504                                                              _mm_movepi64_pi64 (xmmAlpha),
2505                                                              expand565_16_1x64 (d))));
2506             w--;
2507         }
2508
2509         /* call prefetch hint to optimize cache load*/
2510         cachePrefetch ((__m128i*)dst);
2511
2512         while (w >= 8)
2513         {
2514             /* fill cache line with next memory */
2515             cachePrefetchNext ((__m128i*)dst);
2516
2517             xmmDst = load128Aligned ((__m128i*)dst);
2518
2519             unpack565_128_4x128 (xmmDst, &xmmDst0, &xmmDst1, &xmmDst2, &xmmDst3);
2520
2521             over_2x128 (&xmmSrc, &xmmSrc, &xmmAlpha, &xmmAlpha, &xmmDst0, &xmmDst1);
2522             over_2x128 (&xmmSrc, &xmmSrc, &xmmAlpha, &xmmAlpha, &xmmDst2, &xmmDst3);
2523
2524             xmmDst = pack565_4x128_128 (&xmmDst0, &xmmDst1, &xmmDst2, &xmmDst3);
2525             save128Aligned ((__m128i*)dst, xmmDst);
2526
2527             dst += 8;
2528             w -= 8;
2529         }
2530
2531         while (w--)
2532         {
2533             d = *dst;
2534             *dst++ = pack565_32_16 (pack_1x64_32 (over_1x64 (_mm_movepi64_pi64 (xmmSrc),
2535                                                              _mm_movepi64_pi64 (xmmAlpha),
2536                                                              expand565_16_1x64 (d))));
2537         }
2538     }
2539
2540     _mm_empty();
2541 }
2542
2543 /* -------------------------------------------------------------------------------------------------
2544  * fbCompositeSolidMask_nx8888x8888C
2545  */
2546
2547 void
2548 fbCompositeSolidMask_nx8888x8888Csse2 (pixman_op_t op,
2549                                       pixman_image_t * pSrc,
2550                                       pixman_image_t * pMask,
2551                                       pixman_image_t * pDst,
2552                                       int16_t   xSrc,
2553                                       int16_t   ySrc,
2554                                       int16_t   xMask,
2555                                       int16_t   yMask,
2556                                       int16_t   xDst,
2557                                       int16_t   yDst,
2558                                       uint16_t  width,
2559                                       uint16_t  height)
2560 {
2561     uint32_t    src, srca;
2562     uint32_t    *dstLine, d;
2563     uint32_t    *maskLine, m;
2564     uint32_t    packCmp;
2565     int dstStride, maskStride;
2566
2567     __m128i xmmSrc, xmmAlpha;
2568     __m128i xmmDst, xmmDstLo, xmmDstHi;
2569     __m128i xmmMask, xmmMaskLo, xmmMaskHi;
2570
2571     __m64 mmxSrc, mmxAlpha, mmxMask, mmxDst;
2572
2573     fbComposeGetSolid(pSrc, src, pDst->bits.format);
2574
2575     srca = src >> 24;
2576     if (srca == 0)
2577         return;
2578
2579     fbComposeGetStart (pDst, xDst, yDst, uint32_t, dstStride, dstLine, 1);
2580     fbComposeGetStart (pMask, xMask, yMask, uint32_t, maskStride, maskLine, 1);
2581
2582     xmmSrc = _mm_unpacklo_epi8 (createMask_2x32_128 (src, src), _mm_setzero_si128 ());
2583     xmmAlpha = expandAlpha_1x128 (xmmSrc);
2584     mmxSrc   = _mm_movepi64_pi64 (xmmSrc);
2585     mmxAlpha = _mm_movepi64_pi64 (xmmAlpha);
2586
2587     while (height--)
2588     {
2589         int w = width;
2590         uint32_t *pm = (uint32_t *)maskLine;
2591         uint32_t *pd = (uint32_t *)dstLine;
2592
2593         dstLine += dstStride;
2594         maskLine += maskStride;
2595
2596         /* call prefetch hint to optimize cache load*/
2597         cachePrefetch ((__m128i*)pd);
2598         cachePrefetch ((__m128i*)pm);
2599
2600         while (w && (unsigned long)pd & 15)
2601         {
2602             m = *pm++;
2603
2604             if (m)
2605             {
2606                 d = *pd;
2607                 mmxMask = unpack_32_1x64 (m);
2608                 mmxDst = unpack_32_1x64 (d);
2609
2610                 *pd = pack_1x64_32 (inOver_1x64 (&mmxSrc,
2611                                                  &mmxAlpha,
2612                                                  &mmxMask,
2613                                                  &mmxDst));
2614             }
2615
2616             pd++;
2617             w--;
2618         }
2619
2620         /* call prefetch hint to optimize cache load*/
2621         cachePrefetch ((__m128i*)pd);
2622         cachePrefetch ((__m128i*)pm);
2623
2624         while (w >= 4)
2625         {
2626             /* fill cache line with next memory */
2627             cachePrefetchNext ((__m128i*)pd);
2628             cachePrefetchNext ((__m128i*)pm);
2629
2630             xmmMask = load128Unaligned ((__m128i*)pm);
2631
2632             packCmp = _mm_movemask_epi8 (_mm_cmpeq_epi32 (xmmMask, _mm_setzero_si128()));
2633
2634             /* if all bits in mask are zero, packCmp are equal to 0xffff */
2635             if (packCmp != 0xffff)
2636             {
2637                 xmmDst = load128Aligned ((__m128i*)pd);
2638
2639                 unpack_128_2x128 (xmmMask, &xmmMaskLo, &xmmMaskHi);
2640                 unpack_128_2x128 (xmmDst, &xmmDstLo, &xmmDstHi);
2641
2642                 inOver_2x128 (&xmmSrc, &xmmSrc, &xmmAlpha, &xmmAlpha, &xmmMaskLo, &xmmMaskHi, &xmmDstLo, &xmmDstHi);
2643
2644                 save128Aligned ((__m128i*)pd, pack_2x128_128 (xmmDstLo, xmmDstHi));
2645             }
2646
2647             pd += 4;
2648             pm += 4;
2649             w -= 4;
2650         }
2651
2652         while (w)
2653         {
2654             m = *pm++;
2655
2656             if (m)
2657             {
2658                 d = *pd;
2659                 mmxMask = unpack_32_1x64 (m);
2660                 mmxDst = unpack_32_1x64 (d);
2661
2662                 *pd = pack_1x64_32 (inOver_1x64 (&mmxSrc,
2663                                                  &mmxAlpha,
2664                                                  &mmxMask,
2665                                                  &mmxDst));
2666             }
2667
2668             pd++;
2669             w--;
2670         }
2671     }
2672
2673     _mm_empty();
2674 }
2675
2676
2677 /* -------------------------------------------------------------------------------------------------
2678  * fbCompositeSrc_8888x8x8888
2679  */
2680
2681 void
2682 fbCompositeSrc_8888x8x8888sse2 (pixman_op_t op,
2683                                pixman_image_t * pSrc,
2684                                pixman_image_t * pMask,
2685                                pixman_image_t * pDst,
2686                                int16_t  xSrc,
2687                                int16_t  ySrc,
2688                                int16_t      xMask,
2689                                int16_t      yMask,
2690                                int16_t      xDst,
2691                                int16_t      yDst,
2692                                uint16_t     width,
2693                                uint16_t     height)
2694 {
2695     uint32_t    *dstLine, *dst;
2696     uint32_t    *srcLine, *src;
2697     uint32_t    mask;
2698     uint16_t    w;
2699     int dstStride, srcStride;
2700
2701     __m128i xmmMask;
2702     __m128i xmmSrc, xmmSrcLo, xmmSrcHi;
2703     __m128i xmmDst, xmmDstLo, xmmDstHi;
2704     __m128i xmmAlphaLo, xmmAlphaHi;
2705
2706     fbComposeGetStart (pDst, xDst, yDst, uint32_t, dstStride, dstLine, 1);
2707     fbComposeGetStart (pSrc, xSrc, ySrc, uint32_t, srcStride, srcLine, 1);
2708     fbComposeGetSolid (pMask, mask, pDst->bits.format);
2709
2710     xmmMask = createMask_16_128 (mask >> 24);
2711
2712     while (height--)
2713     {
2714         dst = dstLine;
2715         dstLine += dstStride;
2716         src = srcLine;
2717         srcLine += srcStride;
2718         w = width;
2719
2720         /* call prefetch hint to optimize cache load*/
2721         cachePrefetch ((__m128i*)dst);
2722         cachePrefetch ((__m128i*)src);
2723
2724         while (w && (unsigned long)dst & 15)
2725         {
2726             uint32_t s = *src++;
2727             uint32_t d = *dst;
2728
2729             __m64 ms = unpack_32_1x64 (s);
2730             __m64 alpha    = expandAlpha_1x64 (ms);
2731             __m64 dest     = _mm_movepi64_pi64 (xmmMask);
2732             __m64 alphaDst = unpack_32_1x64 (d);
2733
2734             *dst++ = pack_1x64_32 (inOver_1x64 (&ms,
2735                                                 &alpha,
2736                                                 &dest,
2737                                                 &alphaDst));
2738
2739             w--;
2740         }
2741
2742         /* call prefetch hint to optimize cache load*/
2743         cachePrefetch ((__m128i*)dst);
2744         cachePrefetch ((__m128i*)src);
2745
2746         while (w >= 4)
2747         {
2748             /* fill cache line with next memory */
2749             cachePrefetchNext ((__m128i*)dst);
2750             cachePrefetchNext ((__m128i*)src);
2751
2752             xmmSrc = load128Unaligned ((__m128i*)src);
2753             xmmDst = load128Aligned ((__m128i*)dst);
2754
2755             unpack_128_2x128 (xmmSrc, &xmmSrcLo, &xmmSrcHi);
2756             unpack_128_2x128 (xmmDst, &xmmDstLo, &xmmDstHi);
2757             expandAlpha_2x128 (xmmSrcLo, xmmSrcHi, &xmmAlphaLo, &xmmAlphaHi);
2758
2759             inOver_2x128 (&xmmSrcLo, &xmmSrcHi, &xmmAlphaLo, &xmmAlphaHi, &xmmMask, &xmmMask, &xmmDstLo, &xmmDstHi);
2760
2761             save128Aligned( (__m128i*)dst, pack_2x128_128 (xmmDstLo, xmmDstHi));
2762
2763             dst += 4;
2764             src += 4;
2765             w -= 4;
2766         }
2767
2768         while (w)
2769         {
2770             uint32_t s = *src++;
2771             uint32_t d = *dst;
2772
2773             __m64 ms = unpack_32_1x64 (s);
2774             __m64 alpha = expandAlpha_1x64 (ms);
2775             __m64 mask  = _mm_movepi64_pi64 (xmmMask);
2776             __m64 dest  = unpack_32_1x64 (d);
2777
2778             *dst++ = pack_1x64_32 (inOver_1x64 (&ms,
2779                                                 &alpha,
2780                                                 &mask,
2781                                                 &dest));
2782
2783             w--;
2784         }
2785     }
2786
2787     _mm_empty();
2788 }
2789
2790 /* -------------------------------------------------------------------------------------------------
2791  * fbCompositeSrc_x888xnx8888
2792  */
2793 void
2794 fbCompositeSrc_x888xnx8888sse2 (pixman_op_t op,
2795                                pixman_image_t * pSrc,
2796                                pixman_image_t * pMask,
2797                                pixman_image_t * pDst,
2798                                int16_t  xSrc,
2799                                int16_t  ySrc,
2800                                int16_t      xMask,
2801                                int16_t      yMask,
2802                                int16_t      xDst,
2803                                int16_t      yDst,
2804                                uint16_t     width,
2805                                uint16_t     height)
2806 {
2807     uint32_t    *dstLine, *dst;
2808     uint32_t    *srcLine, *src;
2809     uint32_t    mask;
2810     int dstStride, srcStride;
2811     uint16_t    w;
2812
2813     __m128i xmmMask, xmmAlpha;
2814     __m128i xmmSrc, xmmSrcLo, xmmSrcHi;
2815     __m128i xmmDst, xmmDstLo, xmmDstHi;
2816
2817     fbComposeGetStart (pDst, xDst, yDst, uint32_t, dstStride, dstLine, 1);
2818     fbComposeGetStart (pSrc, xSrc, ySrc, uint32_t, srcStride, srcLine, 1);
2819     fbComposeGetSolid (pMask, mask, pDst->bits.format);
2820
2821     xmmMask = createMask_16_128 (mask >> 24);
2822     xmmAlpha = Mask00ff;
2823
2824     while (height--)
2825     {
2826         dst = dstLine;
2827         dstLine += dstStride;
2828         src = srcLine;
2829         srcLine += srcStride;
2830         w = width;
2831
2832         /* call prefetch hint to optimize cache load*/
2833         cachePrefetch ((__m128i*)dst);
2834         cachePrefetch ((__m128i*)src);
2835
2836         while (w && (unsigned long)dst & 15)
2837         {
2838             uint32_t s = (*src++) | 0xff000000;
2839             uint32_t d = *dst;
2840
2841             __m64 src   = unpack_32_1x64 (s);
2842             __m64 alpha = _mm_movepi64_pi64 (xmmAlpha);
2843             __m64 mask  = _mm_movepi64_pi64 (xmmMask);
2844             __m64 dest  = unpack_32_1x64 (d);
2845
2846             *dst++ = pack_1x64_32 (inOver_1x64 (&src,
2847                                                 &alpha,
2848                                                 &mask,
2849                                                 &dest));
2850
2851             w--;
2852         }
2853
2854         /* call prefetch hint to optimize cache load*/
2855         cachePrefetch ((__m128i*)dst);
2856         cachePrefetch ((__m128i*)src);
2857
2858         while (w >= 4)
2859         {
2860             /* fill cache line with next memory */
2861             cachePrefetchNext ((__m128i*)dst);
2862             cachePrefetchNext ((__m128i*)src);
2863
2864             xmmSrc = _mm_or_si128 (load128Unaligned ((__m128i*)src), Maskff000000);
2865             xmmDst = load128Aligned ((__m128i*)dst);
2866
2867             unpack_128_2x128 (xmmSrc, &xmmSrcLo, &xmmSrcHi);
2868             unpack_128_2x128 (xmmDst, &xmmDstLo, &xmmDstHi);
2869
2870             inOver_2x128 (&xmmSrcLo, &xmmSrcHi, &xmmAlpha, &xmmAlpha, &xmmMask, &xmmMask, &xmmDstLo, &xmmDstHi);
2871
2872             save128Aligned( (__m128i*)dst, pack_2x128_128 (xmmDstLo, xmmDstHi));
2873
2874             dst += 4;
2875             src += 4;
2876             w -= 4;
2877
2878         }
2879
2880         while (w)
2881         {
2882             uint32_t s = (*src++) | 0xff000000;
2883             uint32_t d = *dst;
2884
2885             __m64 src  = unpack_32_1x64 (s);
2886             __m64 alpha = _mm_movepi64_pi64 (xmmAlpha);
2887             __m64 mask  = _mm_movepi64_pi64 (xmmMask);
2888             __m64 dest  = unpack_32_1x64 (d);
2889
2890             *dst++ = pack_1x64_32 (inOver_1x64 (&src,
2891                                                 &alpha,
2892                                                 &mask,
2893                                                 &dest));
2894
2895             w--;
2896         }
2897     }
2898
2899     _mm_empty();
2900 }
2901
2902 /* -------------------------------------------------------------------------------------------------
2903  * fbCompositeSrc_8888x8888
2904  */
2905 void
2906 fbCompositeSrc_8888x8888sse2 (pixman_op_t op,
2907                              pixman_image_t * pSrc,
2908                              pixman_image_t * pMask,
2909                              pixman_image_t * pDst,
2910                              int16_t    xSrc,
2911                              int16_t    ySrc,
2912                              int16_t      xMask,
2913                              int16_t      yMask,
2914                              int16_t      xDst,
2915                              int16_t      yDst,
2916                              uint16_t     width,
2917                              uint16_t     height)
2918 {
2919     int         dstStride, srcStride;
2920     uint32_t    *dstLine, *dst;
2921     uint32_t    *srcLine, *src;
2922
2923     fbComposeGetStart (pDst, xDst, yDst, uint32_t, dstStride, dstLine, 1);
2924     fbComposeGetStart (pSrc, xSrc, ySrc, uint32_t, srcStride, srcLine, 1);
2925
2926     dst = dstLine;
2927     src = srcLine;
2928
2929     while (height--)
2930     {
2931         coreCombineOverUsse2 (dst, src, width);
2932
2933         dst += dstStride;
2934         src += srcStride;
2935     }
2936     _mm_empty();
2937 }
2938
2939 /* -------------------------------------------------------------------------------------------------
2940  * fbCompositeSrc_8888x0565
2941  */
2942 static inline uint16_t
2943 fbCompositeSrc_8888x0565pixel (uint32_t src, uint16_t dst)
2944 {
2945     __m64       ms;
2946
2947     ms = unpack_32_1x64 (src);
2948     return pack565_32_16( pack_1x64_32 (over_1x64 (ms,
2949                                                    expandAlpha_1x64 (ms),
2950                                                    expand565_16_1x64 (dst))));
2951 }
2952
2953 void
2954 fbCompositeSrc_8888x0565sse2 (pixman_op_t op,
2955                              pixman_image_t * pSrc,
2956                              pixman_image_t * pMask,
2957                              pixman_image_t * pDst,
2958                              int16_t      xSrc,
2959                              int16_t      ySrc,
2960                              int16_t      xMask,
2961                              int16_t      yMask,
2962                              int16_t      xDst,
2963                              int16_t      yDst,
2964                              uint16_t     width,
2965                              uint16_t     height)
2966 {
2967     uint16_t    *dstLine, *dst, d;
2968     uint32_t    *srcLine, *src, s;
2969     int dstStride, srcStride;
2970     uint16_t    w;
2971
2972     __m128i xmmAlphaLo, xmmAlphaHi;
2973     __m128i xmmSrc, xmmSrcLo, xmmSrcHi;
2974     __m128i xmmDst, xmmDst0, xmmDst1, xmmDst2, xmmDst3;
2975
2976     fbComposeGetStart (pDst, xDst, yDst, uint16_t, dstStride, dstLine, 1);
2977     fbComposeGetStart (pSrc, xSrc, ySrc, uint32_t, srcStride, srcLine, 1);
2978
2979 #if 0
2980     /* FIXME
2981      *
2982      * I copy the code from MMX one and keep the fixme.
2983      * If it's a problem there, probably is a problem here.
2984      */
2985     assert (pSrc->pDrawable == pMask->pDrawable);
2986 #endif
2987
2988     while (height--)
2989     {
2990         dst = dstLine;
2991         src = srcLine;
2992
2993         /* call prefetch hint to optimize cache load*/
2994         cachePrefetch ((__m128i*)src);
2995         cachePrefetch ((__m128i*)dst);
2996
2997         dstLine += dstStride;
2998         srcLine += srcStride;
2999         w = width;
3000
3001         /* Align dst on a 16-byte boundary */
3002         while (w &&
3003                ((unsigned long)dst & 15))
3004         {
3005             s = *src++;
3006             d = *dst;
3007
3008             *dst++ = fbCompositeSrc_8888x0565pixel (s, d);
3009             w--;
3010         }
3011
3012         /* call prefetch hint to optimize cache load*/
3013         cachePrefetch ((__m128i*)src);
3014         cachePrefetch ((__m128i*)dst);
3015
3016         /* It's a 8 pixel loop */
3017         while (w >= 8)
3018         {
3019             /* fill cache line with next memory */
3020             cachePrefetchNext ((__m128i*)src);
3021             cachePrefetchNext ((__m128i*)dst);
3022
3023             /* I'm loading unaligned because I'm not sure about the address alignment. */
3024             xmmSrc = load128Unaligned ((__m128i*) src);
3025             xmmDst = load128Aligned ((__m128i*) dst);
3026
3027             /* Unpacking */
3028             unpack_128_2x128 (xmmSrc, &xmmSrcLo, &xmmSrcHi);
3029             unpack565_128_4x128 (xmmDst, &xmmDst0, &xmmDst1, &xmmDst2, &xmmDst3);
3030             expandAlpha_2x128 (xmmSrcLo, xmmSrcHi, &xmmAlphaLo, &xmmAlphaHi);
3031
3032             /* I'm loading next 4 pixels from memory before to optimze the memory read. */
3033             xmmSrc = load128Unaligned ((__m128i*) (src+4));
3034
3035             over_2x128 (&xmmSrcLo, &xmmSrcHi, &xmmAlphaLo, &xmmAlphaHi, &xmmDst0, &xmmDst1);
3036
3037             /* Unpacking */
3038             unpack_128_2x128 (xmmSrc, &xmmSrcLo, &xmmSrcHi);
3039             expandAlpha_2x128 (xmmSrcLo, xmmSrcHi, &xmmAlphaLo, &xmmAlphaHi);
3040
3041             over_2x128 (&xmmSrcLo, &xmmSrcHi, &xmmAlphaLo, &xmmAlphaHi, &xmmDst2, &xmmDst3);
3042
3043             save128Aligned ((__m128i*)dst, pack565_4x128_128 (&xmmDst0, &xmmDst1, &xmmDst2, &xmmDst3));
3044
3045             w -= 8;
3046             dst += 8;
3047             src += 8;
3048         }
3049
3050         while (w--)
3051         {
3052             s = *src++;
3053             d = *dst;
3054
3055             *dst++ = fbCompositeSrc_8888x0565pixel (s, d);
3056         }
3057     }
3058
3059     _mm_empty();
3060 }
3061
3062 /* -------------------------------------------------------------------------------------------------
3063  * fbCompositeSolidMask_nx8x8888
3064  */
3065
3066 void
3067 fbCompositeSolidMask_nx8x8888sse2 (pixman_op_t op,
3068                                   pixman_image_t * pSrc,
3069                                   pixman_image_t * pMask,
3070                                   pixman_image_t * pDst,
3071                                   int16_t      xSrc,
3072                                   int16_t      ySrc,
3073                                   int16_t      xMask,
3074                                   int16_t      yMask,
3075                                   int16_t      xDst,
3076                                   int16_t      yDst,
3077                                   uint16_t     width,
3078                                   uint16_t     height)
3079 {
3080     uint32_t    src, srca;
3081     uint32_t    *dstLine, *dst;
3082     uint8_t     *maskLine, *mask;
3083     int dstStride, maskStride;
3084     uint16_t    w;
3085     uint32_t m, d;
3086
3087     __m128i xmmSrc, xmmAlpha, xmmDef;
3088     __m128i xmmDst, xmmDstLo, xmmDstHi;
3089     __m128i xmmMask, xmmMaskLo, xmmMaskHi;
3090
3091     __m64 mmxSrc, mmxAlpha, mmxMask, mmxDest;
3092
3093     fbComposeGetSolid(pSrc, src, pDst->bits.format);
3094
3095     srca = src >> 24;
3096     if (srca == 0)
3097         return;
3098
3099     fbComposeGetStart (pDst, xDst, yDst, uint32_t, dstStride, dstLine, 1);
3100     fbComposeGetStart (pMask, xMask, yMask, uint8_t, maskStride, maskLine, 1);
3101
3102     xmmDef = createMask_2x32_128 (src, src);
3103     xmmSrc = expandPixel_32_1x128 (src);
3104     xmmAlpha = expandAlpha_1x128 (xmmSrc);
3105     mmxSrc   = _mm_movepi64_pi64 (xmmSrc);
3106     mmxAlpha = _mm_movepi64_pi64 (xmmAlpha);
3107
3108     while (height--)
3109     {
3110         dst = dstLine;
3111         dstLine += dstStride;
3112         mask = maskLine;
3113         maskLine += maskStride;
3114         w = width;
3115
3116         /* call prefetch hint to optimize cache load*/
3117         cachePrefetch ((__m128i*)mask);
3118         cachePrefetch ((__m128i*)dst);
3119
3120         while (w && (unsigned long)dst & 15)
3121         {
3122             uint8_t m = *mask++;
3123
3124             if (m)
3125             {
3126                 d = *dst;
3127                 mmxMask = expandPixel_8_1x64 (m);
3128                 mmxDest = unpack_32_1x64 (d);
3129
3130                 *dst = pack_1x64_32 (inOver_1x64 (&mmxSrc,
3131                                                   &mmxAlpha,
3132                                                   &mmxMask,
3133                                                   &mmxDest));
3134             }
3135
3136             w--;
3137             dst++;
3138         }
3139
3140         /* call prefetch hint to optimize cache load*/
3141         cachePrefetch ((__m128i*)mask);
3142         cachePrefetch ((__m128i*)dst);
3143
3144         while (w >= 4)
3145         {
3146             /* fill cache line with next memory */
3147             cachePrefetchNext ((__m128i*)mask);
3148             cachePrefetchNext ((__m128i*)dst);
3149
3150             m = *((uint32_t*)mask);
3151
3152             if (srca == 0xff && m == 0xffffffff)
3153             {
3154                 save128Aligned ((__m128i*)dst, xmmDef);
3155             }
3156             else if (m)
3157             {
3158                 xmmDst = load128Aligned ((__m128i*) dst);
3159                 xmmMask = unpack_32_1x128 (m);
3160                 xmmMask = _mm_unpacklo_epi8 (xmmMask, _mm_setzero_si128());
3161
3162                 /* Unpacking */
3163                 unpack_128_2x128 (xmmDst, &xmmDstLo, &xmmDstHi);
3164                 unpack_128_2x128 (xmmMask, &xmmMaskLo, &xmmMaskHi);
3165
3166                 expandAlphaRev_2x128 (xmmMaskLo, xmmMaskHi, &xmmMaskLo, &xmmMaskHi);
3167
3168                 inOver_2x128 (&xmmSrc, &xmmSrc, &xmmAlpha, &xmmAlpha, &xmmMaskLo, &xmmMaskHi, &xmmDstLo, &xmmDstHi);
3169
3170                 save128Aligned ((__m128i*)dst, pack_2x128_128 (xmmDstLo, xmmDstHi));
3171             }
3172
3173             w -= 4;
3174             dst += 4;
3175             mask += 4;
3176         }
3177
3178         while (w)
3179         {
3180             uint8_t m = *mask++;
3181
3182             if (m)
3183             {
3184                 d = *dst;
3185                 mmxMask = expandPixel_8_1x64 (m);
3186                 mmxDest = unpack_32_1x64 (d);
3187
3188                 *dst = pack_1x64_32 (inOver_1x64 (&mmxSrc,
3189                                                   &mmxAlpha,
3190                                                   &mmxMask,
3191                                                   &mmxDest));
3192             }
3193
3194             w--;
3195             dst++;
3196         }
3197     }
3198
3199     _mm_empty();
3200 }
3201
3202 /* -------------------------------------------------------------------------------------------------
3203  * fbCompositeSolidMask_nx8x8888
3204  */
3205
3206 pixman_bool_t
3207 pixmanFillsse2 (uint32_t *bits,
3208                  int stride,
3209                  int bpp,
3210                  int x,
3211                  int y,
3212                  int width,
3213                  int height,
3214                  uint32_t data)
3215 {
3216     uint32_t    byte_width;
3217     uint8_t         *byte_line;
3218
3219     __m128i xmmDef;
3220
3221     if (bpp == 16 && (data >> 16 != (data & 0xffff)))
3222         return FALSE;
3223
3224     if (bpp != 16 && bpp != 32)
3225         return FALSE;
3226
3227     if (bpp == 16)
3228     {
3229         stride = stride * (int) sizeof (uint32_t) / 2;
3230         byte_line = (uint8_t *)(((uint16_t *)bits) + stride * y + x);
3231         byte_width = 2 * width;
3232         stride *= 2;
3233     }
3234     else
3235     {
3236         stride = stride * (int) sizeof (uint32_t) / 4;
3237         byte_line = (uint8_t *)(((uint32_t *)bits) + stride * y + x);
3238         byte_width = 4 * width;
3239         stride *= 4;
3240     }
3241
3242     cachePrefetch ((__m128i*)byte_line);
3243     xmmDef = createMask_2x32_128 (data, data);
3244
3245     while (height--)
3246     {
3247         int w;
3248         uint8_t *d = byte_line;
3249         byte_line += stride;
3250         w = byte_width;
3251
3252
3253         cachePrefetchNext ((__m128i*)d);
3254
3255         while (w >= 2 && ((unsigned long)d & 3))
3256         {
3257             *(uint16_t *)d = data;
3258             w -= 2;
3259             d += 2;
3260         }
3261
3262         while (w >= 4 && ((unsigned long)d & 15))
3263         {
3264             *(uint32_t *)d = data;
3265
3266             w -= 4;
3267             d += 4;
3268         }
3269
3270         cachePrefetchNext ((__m128i*)d);
3271
3272         while (w >= 128)
3273         {
3274             cachePrefetch (((__m128i*)d) + 12);
3275
3276             save128Aligned ((__m128i*)(d),     xmmDef);
3277             save128Aligned ((__m128i*)(d+16),  xmmDef);
3278             save128Aligned ((__m128i*)(d+32),  xmmDef);
3279             save128Aligned ((__m128i*)(d+48),  xmmDef);
3280             save128Aligned ((__m128i*)(d+64),  xmmDef);
3281             save128Aligned ((__m128i*)(d+80),  xmmDef);
3282             save128Aligned ((__m128i*)(d+96),  xmmDef);
3283             save128Aligned ((__m128i*)(d+112), xmmDef);
3284
3285             d += 128;
3286             w -= 128;
3287         }
3288
3289         if (w >= 64)
3290         {
3291             cachePrefetch (((__m128i*)d) + 8);
3292
3293             save128Aligned ((__m128i*)(d),     xmmDef);
3294             save128Aligned ((__m128i*)(d+16),  xmmDef);
3295             save128Aligned ((__m128i*)(d+32),  xmmDef);
3296             save128Aligned ((__m128i*)(d+48),  xmmDef);
3297
3298             d += 64;
3299             w -= 64;
3300         }
3301
3302         cachePrefetchNext ((__m128i*)d);
3303
3304         if (w >= 32)
3305         {
3306             save128Aligned ((__m128i*)(d),     xmmDef);
3307             save128Aligned ((__m128i*)(d+16),  xmmDef);
3308
3309             d += 32;
3310             w -= 32;
3311         }
3312
3313         if (w >= 16)
3314         {
3315             save128Aligned ((__m128i*)(d),     xmmDef);
3316
3317             d += 16;
3318             w -= 16;
3319         }
3320
3321         cachePrefetchNext ((__m128i*)d);
3322
3323         while (w >= 4)
3324         {
3325             *(uint32_t *)d = data;
3326
3327             w -= 4;
3328             d += 4;
3329         }
3330
3331         if (w >= 2)
3332         {
3333             *(uint16_t *)d = data;
3334             w -= 2;
3335             d += 2;
3336         }
3337     }
3338
3339     _mm_empty();
3340     return TRUE;
3341 }
3342
3343 void
3344 fbCompositeSolidMaskSrc_nx8x8888sse2 (pixman_op_t op,
3345                                      pixman_image_t * pSrc,
3346                                      pixman_image_t * pMask,
3347                                      pixman_image_t * pDst,
3348                                      int16_t      xSrc,
3349                                      int16_t      ySrc,
3350                                      int16_t      xMask,
3351                                      int16_t      yMask,
3352                                      int16_t      xDst,
3353                                      int16_t      yDst,
3354                                      uint16_t     width,
3355                                      uint16_t     height)
3356 {
3357     uint32_t    src, srca;
3358     uint32_t    *dstLine, *dst;
3359     uint8_t     *maskLine, *mask;
3360     int dstStride, maskStride;
3361     uint16_t    w;
3362     uint32_t    m;
3363
3364     __m128i xmmSrc, xmmDef;
3365     __m128i xmmMask, xmmMaskLo, xmmMaskHi;
3366
3367     fbComposeGetSolid(pSrc, src, pDst->bits.format);
3368
3369     srca = src >> 24;
3370     if (srca == 0)
3371     {
3372         pixmanFillsse2 (pDst->bits.bits, pDst->bits.rowstride,
3373                         PIXMAN_FORMAT_BPP (pDst->bits.format),
3374                         xDst, yDst, width, height, 0);
3375         return;
3376     }
3377
3378     fbComposeGetStart (pDst, xDst, yDst, uint32_t, dstStride, dstLine, 1);
3379     fbComposeGetStart (pMask, xMask, yMask, uint8_t, maskStride, maskLine, 1);
3380
3381     xmmDef = createMask_2x32_128 (src, src);
3382     xmmSrc = expandPixel_32_1x128 (src);
3383
3384     while (height--)
3385     {
3386         dst = dstLine;
3387         dstLine += dstStride;
3388         mask = maskLine;
3389         maskLine += maskStride;
3390         w = width;
3391
3392         /* call prefetch hint to optimize cache load*/
3393         cachePrefetch ((__m128i*)mask);
3394         cachePrefetch ((__m128i*)dst);
3395
3396         while (w && (unsigned long)dst & 15)
3397         {
3398             uint8_t m = *mask++;
3399
3400             if (m)
3401             {
3402                 *dst = pack_1x64_32 (pixMultiply_1x64 (_mm_movepi64_pi64 (xmmSrc), expandPixel_8_1x64 (m)));
3403             }
3404             else
3405             {
3406                 *dst = 0;
3407             }
3408
3409             w--;
3410             dst++;
3411         }
3412
3413         /* call prefetch hint to optimize cache load*/
3414         cachePrefetch ((__m128i*)mask);
3415         cachePrefetch ((__m128i*)dst);
3416
3417         while (w >= 4)
3418         {
3419             /* fill cache line with next memory */
3420             cachePrefetchNext ((__m128i*)mask);
3421             cachePrefetchNext ((__m128i*)dst);
3422
3423             m = *((uint32_t*)mask);
3424
3425             if (srca == 0xff && m == 0xffffffff)
3426             {
3427                 save128Aligned ((__m128i*)dst, xmmDef);
3428             }
3429             else if (m)
3430             {
3431                 xmmMask = unpack_32_1x128 (m);
3432                 xmmMask = _mm_unpacklo_epi8 (xmmMask, _mm_setzero_si128());
3433
3434                 /* Unpacking */
3435                 unpack_128_2x128 (xmmMask, &xmmMaskLo, &xmmMaskHi);
3436
3437                 expandAlphaRev_2x128 (xmmMaskLo, xmmMaskHi, &xmmMaskLo, &xmmMaskHi);
3438
3439                 pixMultiply_2x128 (&xmmSrc, &xmmSrc, &xmmMaskLo, &xmmMaskHi, &xmmMaskLo, &xmmMaskHi);
3440
3441                 save128Aligned ((__m128i*)dst, pack_2x128_128 (xmmMaskLo, xmmMaskHi));
3442             }
3443             else
3444             {
3445                 save128Aligned ((__m128i*)dst, _mm_setzero_si128());
3446             }
3447
3448             w -= 4;
3449             dst += 4;
3450             mask += 4;
3451         }
3452
3453         while (w)
3454         {
3455             uint8_t m = *mask++;
3456
3457             if (m)
3458             {
3459                 *dst = pack_1x64_32 (pixMultiply_1x64 (_mm_movepi64_pi64 (xmmSrc), expandPixel_8_1x64 (m)));
3460             }
3461             else
3462             {
3463                 *dst = 0;
3464             }
3465
3466             w--;
3467             dst++;
3468         }
3469     }
3470
3471     _mm_empty();
3472 }
3473
3474 /* -------------------------------------------------------------------------------------------------
3475  * fbCompositeSolidMask_nx8x0565
3476  */
3477
3478 void
3479 fbCompositeSolidMask_nx8x0565sse2 (pixman_op_t op,
3480                                   pixman_image_t * pSrc,
3481                                   pixman_image_t * pMask,
3482                                   pixman_image_t * pDst,
3483                                   int16_t      xSrc,
3484                                   int16_t      ySrc,
3485                                   int16_t      xMask,
3486                                   int16_t      yMask,
3487                                   int16_t      xDst,
3488                                   int16_t      yDst,
3489                                   uint16_t     width,
3490                                   uint16_t     height)
3491 {
3492     uint32_t    src, srca;
3493     uint16_t    *dstLine, *dst, d;
3494     uint8_t     *maskLine, *mask;
3495     int dstStride, maskStride;
3496     uint16_t    w;
3497     uint32_t m;
3498     __m64 mmxSrc, mmxAlpha, mmxMask, mmxDest;
3499
3500     __m128i xmmSrc, xmmAlpha;
3501     __m128i xmmMask, xmmMaskLo, xmmMaskHi;
3502     __m128i xmmDst, xmmDst0, xmmDst1, xmmDst2, xmmDst3;
3503
3504     fbComposeGetSolid(pSrc, src, pDst->bits.format);
3505
3506     srca = src >> 24;
3507     if (srca == 0)
3508         return;
3509
3510     fbComposeGetStart (pDst, xDst, yDst, uint16_t, dstStride, dstLine, 1);
3511     fbComposeGetStart (pMask, xMask, yMask, uint8_t, maskStride, maskLine, 1);
3512
3513     xmmSrc = expandPixel_32_1x128 (src);
3514     xmmAlpha = expandAlpha_1x128 (xmmSrc);
3515     mmxSrc = _mm_movepi64_pi64 (xmmSrc);
3516     mmxAlpha = _mm_movepi64_pi64 (xmmAlpha);
3517
3518     while (height--)
3519     {
3520         dst = dstLine;
3521         dstLine += dstStride;
3522         mask = maskLine;
3523         maskLine += maskStride;
3524         w = width;
3525
3526         /* call prefetch hint to optimize cache load*/
3527         cachePrefetch ((__m128i*)mask);
3528         cachePrefetch ((__m128i*)dst);
3529
3530         while (w && (unsigned long)dst & 15)
3531         {
3532             m = *mask++;
3533
3534             if (m)
3535             {
3536                 d = *dst;
3537                 mmxMask = expandAlphaRev_1x64 (unpack_32_1x64 (m));
3538                 mmxDest = expand565_16_1x64 (d);
3539
3540                 *dst = pack565_32_16 (pack_1x64_32 (inOver_1x64 (&mmxSrc,
3541                                                                  &mmxAlpha,
3542                                                                  &mmxMask,
3543                                                                  &mmxDest)));
3544             }
3545
3546             w--;
3547             dst++;
3548         }
3549
3550         /* call prefetch hint to optimize cache load*/
3551         cachePrefetch ((__m128i*)mask);
3552         cachePrefetch ((__m128i*)dst);
3553
3554         while (w >= 8)
3555         {
3556             /* fill cache line with next memory */
3557             cachePrefetchNext ((__m128i*)mask);
3558             cachePrefetchNext ((__m128i*)dst);
3559
3560             xmmDst = load128Aligned ((__m128i*) dst);
3561             unpack565_128_4x128 (xmmDst, &xmmDst0, &xmmDst1, &xmmDst2, &xmmDst3);
3562
3563             m = *((uint32_t*)mask);
3564             mask += 4;
3565
3566             if (m)
3567             {
3568                 xmmMask = unpack_32_1x128 (m);
3569                 xmmMask = _mm_unpacklo_epi8 (xmmMask, _mm_setzero_si128());
3570
3571                 /* Unpacking */
3572                 unpack_128_2x128 (xmmMask, &xmmMaskLo, &xmmMaskHi);
3573
3574                 expandAlphaRev_2x128 (xmmMaskLo, xmmMaskHi, &xmmMaskLo, &xmmMaskHi);
3575                 inOver_2x128 (&xmmSrc, &xmmSrc, &xmmAlpha, &xmmAlpha, &xmmMaskLo, &xmmMaskHi, &xmmDst0, &xmmDst1);
3576             }
3577
3578             m = *((uint32_t*)mask);
3579             mask += 4;
3580
3581             if (m)
3582             {
3583                 xmmMask = unpack_32_1x128 (m);
3584                 xmmMask = _mm_unpacklo_epi8 (xmmMask, _mm_setzero_si128());
3585
3586                 /* Unpacking */
3587                 unpack_128_2x128 (xmmMask, &xmmMaskLo, &xmmMaskHi);
3588
3589                 expandAlphaRev_2x128 (xmmMaskLo, xmmMaskHi, &xmmMaskLo, &xmmMaskHi);
3590                 inOver_2x128 (&xmmSrc, &xmmSrc, &xmmAlpha, &xmmAlpha, &xmmMaskLo, &xmmMaskHi, &xmmDst2, &xmmDst3);
3591             }
3592
3593             save128Aligned ((__m128i*)dst, pack565_4x128_128 (&xmmDst0, &xmmDst1, &xmmDst2, &xmmDst3));
3594
3595             w -= 8;
3596             dst += 8;
3597         }
3598
3599         while (w)
3600         {
3601             m = *mask++;
3602
3603             if (m)
3604             {
3605                 d = *dst;
3606                 mmxMask = expandAlphaRev_1x64 (unpack_32_1x64 (m));
3607                 mmxDest = expand565_16_1x64 (d);
3608
3609                 *dst = pack565_32_16 (pack_1x64_32 (inOver_1x64 (&mmxSrc,
3610                                                                  &mmxAlpha,
3611                                                                  &mmxMask,
3612                                                                  &mmxDest)));
3613             }
3614
3615             w--;
3616             dst++;
3617         }
3618     }
3619
3620     _mm_empty();
3621 }
3622
3623 /* -------------------------------------------------------------------------------------------------
3624  * fbCompositeSrc_8888RevNPx0565
3625  */
3626
3627 void
3628 fbCompositeSrc_8888RevNPx0565sse2 (pixman_op_t op,
3629                                   pixman_image_t * pSrc,
3630                                   pixman_image_t * pMask,
3631                                   pixman_image_t * pDst,
3632                                   int16_t      xSrc,
3633                                   int16_t      ySrc,
3634                                   int16_t      xMask,
3635                                   int16_t      yMask,
3636                                   int16_t      xDst,
3637                                   int16_t      yDst,
3638                                   uint16_t     width,
3639                                   uint16_t     height)
3640 {
3641     uint16_t    *dstLine, *dst, d;
3642     uint32_t    *srcLine, *src, s;
3643     int dstStride, srcStride;
3644     uint16_t    w;
3645     uint32_t    packCmp;
3646
3647     __m64 ms;
3648     __m128i xmmSrc, xmmSrcLo, xmmSrcHi;
3649     __m128i xmmDst, xmmDst0, xmmDst1, xmmDst2, xmmDst3;
3650
3651     fbComposeGetStart (pDst, xDst, yDst, uint16_t, dstStride, dstLine, 1);
3652     fbComposeGetStart (pSrc, xSrc, ySrc, uint32_t, srcStride, srcLine, 1);
3653
3654 #if 0
3655     /* FIXME
3656      *
3657      * I copy the code from MMX one and keep the fixme.
3658      * If it's a problem there, probably is a problem here.
3659      */
3660     assert (pSrc->pDrawable == pMask->pDrawable);
3661 #endif
3662
3663     while (height--)
3664     {
3665         dst = dstLine;
3666         dstLine += dstStride;
3667         src = srcLine;
3668         srcLine += srcStride;
3669         w = width;
3670
3671         /* call prefetch hint to optimize cache load*/
3672         cachePrefetch ((__m128i*)src);
3673         cachePrefetch ((__m128i*)dst);
3674
3675         while (w && (unsigned long)dst & 15)
3676         {
3677             s = *src++;
3678             d = *dst;
3679
3680             ms = unpack_32_1x64 (s);
3681
3682             *dst++ = pack565_32_16 (pack_1x64_32 (overRevNonPre_1x64(ms, expand565_16_1x64 (d))));
3683             w--;
3684         }
3685
3686         /* call prefetch hint to optimize cache load*/
3687         cachePrefetch ((__m128i*)src);
3688         cachePrefetch ((__m128i*)dst);
3689
3690         while (w >= 8)
3691         {
3692             /* fill cache line with next memory */
3693             cachePrefetchNext ((__m128i*)src);
3694             cachePrefetchNext ((__m128i*)dst);
3695
3696             /* First round */
3697             xmmSrc = load128Unaligned((__m128i*)src);
3698             xmmDst = load128Aligned  ((__m128i*)dst);
3699
3700             packCmp = packAlpha (xmmSrc);
3701
3702             unpack565_128_4x128 (xmmDst, &xmmDst0, &xmmDst1, &xmmDst2, &xmmDst3);
3703             unpack_128_2x128 (xmmSrc, &xmmSrcLo, &xmmSrcHi);
3704
3705             /* preload next round*/
3706             xmmSrc = load128Unaligned((__m128i*)(src+4));
3707             /* preload next round*/
3708
3709             if (packCmp == 0xffffffff)
3710             {
3711                 invertColors_2x128 (xmmSrcLo, xmmSrcHi, &xmmDst0, &xmmDst1);
3712             }
3713             else if (packCmp)
3714             {
3715                 overRevNonPre_2x128 (xmmSrcLo, xmmSrcHi, &xmmDst0, &xmmDst1);
3716             }
3717
3718             /* Second round */
3719             packCmp = packAlpha (xmmSrc);
3720
3721             unpack_128_2x128 (xmmSrc, &xmmSrcLo, &xmmSrcHi);
3722
3723             if (packCmp == 0xffffffff)
3724             {
3725                 invertColors_2x128 (xmmSrcLo, xmmSrcHi, &xmmDst2, &xmmDst3);
3726             }
3727             else if (packCmp)
3728             {
3729                 overRevNonPre_2x128 (xmmSrcLo, xmmSrcHi, &xmmDst2, &xmmDst3);
3730             }
3731
3732             save128Aligned ((__m128i*)dst, pack565_4x128_128 (&xmmDst0, &xmmDst1, &xmmDst2, &xmmDst3));
3733
3734             w -= 8;
3735             src += 8;
3736             dst += 8;
3737         }
3738
3739         while (w)
3740         {
3741             s = *src++;
3742             d = *dst;
3743
3744             ms = unpack_32_1x64 (s);
3745
3746             *dst++ = pack565_32_16 (pack_1x64_32 (overRevNonPre_1x64(ms, expand565_16_1x64 (d))));
3747             w--;
3748         }
3749     }
3750
3751     _mm_empty();
3752 }
3753
3754 /* "8888RevNP" is GdkPixbuf's format: ABGR, non premultiplied */
3755
3756 /* -------------------------------------------------------------------------------------------------
3757  * fbCompositeSrc_8888RevNPx8888
3758  */
3759
3760 void
3761 fbCompositeSrc_8888RevNPx8888sse2 (pixman_op_t op,
3762                                   pixman_image_t * pSrc,
3763                                   pixman_image_t * pMask,
3764                                   pixman_image_t * pDst,
3765                                   int16_t      xSrc,
3766                                   int16_t      ySrc,
3767                                   int16_t      xMask,
3768                                   int16_t      yMask,
3769                                   int16_t      xDst,
3770                                   int16_t      yDst,
3771                                   uint16_t     width,
3772                                   uint16_t     height)
3773 {
3774     uint32_t    *dstLine, *dst, d;
3775     uint32_t    *srcLine, *src, s;
3776     int dstStride, srcStride;
3777     uint16_t    w;
3778     uint32_t    packCmp;
3779
3780     __m128i xmmSrcLo, xmmSrcHi;
3781     __m128i xmmDstLo, xmmDstHi;
3782
3783     fbComposeGetStart (pDst, xDst, yDst, uint32_t, dstStride, dstLine, 1);
3784     fbComposeGetStart (pSrc, xSrc, ySrc, uint32_t, srcStride, srcLine, 1);
3785
3786 #if 0
3787     /* FIXME
3788      *
3789      * I copy the code from MMX one and keep the fixme.
3790      * If it's a problem there, probably is a problem here.
3791      */
3792     assert (pSrc->pDrawable == pMask->pDrawable);
3793 #endif
3794
3795     while (height--)
3796     {
3797         dst = dstLine;
3798         dstLine += dstStride;
3799         src = srcLine;
3800         srcLine += srcStride;
3801         w = width;
3802
3803         /* call prefetch hint to optimize cache load*/
3804         cachePrefetch ((__m128i*)src);
3805         cachePrefetch ((__m128i*)dst);
3806
3807         while (w && (unsigned long)dst & 15)
3808         {
3809             s = *src++;
3810             d = *dst;
3811
3812             *dst++ = pack_1x64_32 (overRevNonPre_1x64 (unpack_32_1x64 (s), unpack_32_1x64 (d)));
3813
3814             w--;
3815         }
3816
3817         /* call prefetch hint to optimize cache load*/
3818         cachePrefetch ((__m128i*)src);
3819         cachePrefetch ((__m128i*)dst);
3820
3821         while (w >= 4)
3822         {
3823             /* fill cache line with next memory */
3824             cachePrefetchNext ((__m128i*)src);
3825             cachePrefetchNext ((__m128i*)dst);
3826
3827             xmmSrcHi = load128Unaligned((__m128i*)src);
3828
3829             packCmp = packAlpha (xmmSrcHi);
3830
3831             unpack_128_2x128 (xmmSrcHi, &xmmSrcLo, &xmmSrcHi);
3832
3833             if (packCmp == 0xffffffff)
3834             {
3835                 invertColors_2x128( xmmSrcLo, xmmSrcHi, &xmmDstLo, &xmmDstHi);
3836
3837                 save128Aligned ((__m128i*)dst, pack_2x128_128 (xmmDstLo, xmmDstHi));
3838             }
3839             else if (packCmp)
3840             {
3841                 xmmDstHi = load128Aligned  ((__m128i*)dst);
3842
3843                 unpack_128_2x128 (xmmDstHi, &xmmDstLo, &xmmDstHi);
3844
3845                 overRevNonPre_2x128 (xmmSrcLo, xmmSrcHi, &xmmDstLo, &xmmDstHi);
3846
3847                 save128Aligned ((__m128i*)dst, pack_2x128_128 (xmmDstLo, xmmDstHi));
3848             }
3849
3850             w -= 4;
3851             dst += 4;
3852             src += 4;
3853         }
3854
3855         while (w)
3856         {
3857             s = *src++;
3858             d = *dst;
3859
3860             *dst++ = pack_1x64_32 (overRevNonPre_1x64 (unpack_32_1x64 (s), unpack_32_1x64 (d)));
3861
3862             w--;
3863         }
3864     }
3865
3866     _mm_empty();
3867 }
3868
3869 /* -------------------------------------------------------------------------------------------------
3870  * fbCompositeSolidMask_nx8888x0565C
3871  */
3872
3873 void
3874 fbCompositeSolidMask_nx8888x0565Csse2 (pixman_op_t op,
3875                                       pixman_image_t * pSrc,
3876                                       pixman_image_t * pMask,
3877                                       pixman_image_t * pDst,
3878                                       int16_t      xSrc,
3879                                       int16_t      ySrc,
3880                                       int16_t      xMask,
3881                                       int16_t      yMask,
3882                                       int16_t      xDst,
3883                                       int16_t      yDst,
3884                                       uint16_t     width,
3885                                       uint16_t     height)
3886 {
3887     uint32_t    src, srca;
3888     uint16_t    *dstLine, *dst, d;
3889     uint32_t    *maskLine, *mask, m;
3890     int dstStride, maskStride;
3891     int w;
3892     uint32_t packCmp;
3893
3894     __m128i xmmSrc, xmmAlpha;
3895     __m128i xmmMask, xmmMaskLo, xmmMaskHi;
3896     __m128i xmmDst, xmmDst0, xmmDst1, xmmDst2, xmmDst3;
3897
3898     __m64 mmxSrc, mmxAlpha, mmxMask, mmxDest;
3899
3900     fbComposeGetSolid(pSrc, src, pDst->bits.format);
3901
3902     srca = src >> 24;
3903     if (srca == 0)
3904         return;
3905
3906     fbComposeGetStart (pDst, xDst, yDst, uint16_t, dstStride, dstLine, 1);
3907     fbComposeGetStart (pMask, xMask, yMask, uint32_t, maskStride, maskLine, 1);
3908
3909     xmmSrc = expandPixel_32_1x128 (src);
3910     xmmAlpha = expandAlpha_1x128 (xmmSrc);
3911     mmxSrc = _mm_movepi64_pi64 (xmmSrc);
3912     mmxAlpha = _mm_movepi64_pi64 (xmmAlpha);
3913
3914     while (height--)
3915     {
3916         w = width;
3917         mask = maskLine;
3918         dst = dstLine;
3919         maskLine += maskStride;
3920         dstLine += dstStride;
3921
3922         /* call prefetch hint to optimize cache load*/
3923         cachePrefetch ((__m128i*)mask);
3924         cachePrefetch ((__m128i*)dst);
3925
3926         while (w && ((unsigned long)dst & 15))
3927         {
3928             m = *(uint32_t *) mask;
3929
3930             if (m)
3931             {
3932                 d = *dst;
3933                 mmxMask = unpack_32_1x64 (m);
3934                 mmxDest = expand565_16_1x64 (d);
3935
3936                 *dst = pack565_32_16 (pack_1x64_32 (inOver_1x64 (&mmxSrc,
3937                                                                  &mmxAlpha,
3938                                                                  &mmxMask,
3939                                                                  &mmxDest)));
3940             }
3941
3942             w--;
3943             dst++;
3944             mask++;
3945         }
3946
3947         /* call prefetch hint to optimize cache load*/
3948         cachePrefetch ((__m128i*)mask);
3949         cachePrefetch ((__m128i*)dst);
3950
3951         while (w >= 8)
3952         {
3953             /* fill cache line with next memory */
3954             cachePrefetchNext ((__m128i*)mask);
3955             cachePrefetchNext ((__m128i*)dst);
3956
3957             /* First round */
3958             xmmMask = load128Unaligned((__m128i*)mask);
3959             xmmDst = load128Aligned((__m128i*)dst);
3960
3961             packCmp = _mm_movemask_epi8 (_mm_cmpeq_epi32 (xmmMask, _mm_setzero_si128()));
3962
3963             unpack565_128_4x128 (xmmDst, &xmmDst0, &xmmDst1, &xmmDst2, &xmmDst3);
3964             unpack_128_2x128 (xmmMask, &xmmMaskLo, &xmmMaskHi);
3965
3966             /* preload next round*/
3967             xmmMask = load128Unaligned((__m128i*)(mask+4));
3968             /* preload next round*/
3969
3970             if (packCmp != 0xffff)
3971             {
3972                 inOver_2x128(&xmmSrc, &xmmSrc, &xmmAlpha, &xmmAlpha, &xmmMaskLo, &xmmMaskHi, &xmmDst0, &xmmDst1);
3973             }
3974
3975             /* Second round */
3976             packCmp = _mm_movemask_epi8 (_mm_cmpeq_epi32 (xmmMask, _mm_setzero_si128()));
3977
3978             unpack_128_2x128 (xmmMask, &xmmMaskLo, &xmmMaskHi);
3979
3980             if (packCmp != 0xffff)
3981             {
3982                 inOver_2x128(&xmmSrc, &xmmSrc, &xmmAlpha, &xmmAlpha, &xmmMaskLo, &xmmMaskHi, &xmmDst2, &xmmDst3);
3983             }
3984
3985             save128Aligned ((__m128i*)dst, pack565_4x128_128 (&xmmDst0, &xmmDst1, &xmmDst2, &xmmDst3));
3986
3987             w -= 8;
3988             dst += 8;
3989             mask += 8;
3990         }
3991
3992         while (w)
3993         {
3994             m = *(uint32_t *) mask;
3995
3996             if (m)
3997             {
3998                 d = *dst;
3999                 mmxMask = unpack_32_1x64 (m);
4000                 mmxDest = expand565_16_1x64 (d);
4001
4002                 *dst = pack565_32_16 (pack_1x64_32 (inOver_1x64 (&mmxSrc,
4003                                                                  &mmxAlpha,
4004                                                                  &mmxMask,
4005                                                                  &mmxDest)));
4006             }
4007
4008             w--;
4009             dst++;
4010             mask++;
4011         }
4012     }
4013
4014     _mm_empty ();
4015 }
4016
4017 /* -------------------------------------------------------------------------------------------------
4018  * fbCompositeIn_nx8x8
4019  */
4020
4021 void
4022 fbCompositeIn_nx8x8sse2 (pixman_op_t op,
4023                         pixman_image_t * pSrc,
4024                         pixman_image_t * pMask,
4025                         pixman_image_t * pDst,
4026                         int16_t      xSrc,
4027                         int16_t      ySrc,
4028                         int16_t      xMask,
4029                         int16_t      yMask,
4030                         int16_t      xDst,
4031                         int16_t      yDst,
4032                         uint16_t     width,
4033                         uint16_t     height)
4034 {
4035     uint8_t     *dstLine, *dst;
4036     uint8_t     *maskLine, *mask;
4037     int dstStride, maskStride;
4038     uint16_t    w, d, m;
4039     uint32_t    src;
4040     uint8_t     sa;
4041
4042     __m128i xmmAlpha;
4043     __m128i xmmMask, xmmMaskLo, xmmMaskHi;
4044     __m128i xmmDst, xmmDstLo, xmmDstHi;
4045
4046     fbComposeGetStart (pDst, xDst, yDst, uint8_t, dstStride, dstLine, 1);
4047     fbComposeGetStart (pMask, xMask, yMask, uint8_t, maskStride, maskLine, 1);
4048
4049     fbComposeGetSolid(pSrc, src, pDst->bits.format);
4050
4051     sa = src >> 24;
4052     if (sa == 0)
4053         return;
4054
4055     xmmAlpha = expandAlpha_1x128 (expandPixel_32_1x128 (src));
4056
4057     while (height--)
4058     {
4059         dst = dstLine;
4060         dstLine += dstStride;
4061         mask = maskLine;
4062         maskLine += maskStride;
4063         w = width;
4064
4065         /* call prefetch hint to optimize cache load*/
4066         cachePrefetch ((__m128i*)mask);
4067         cachePrefetch ((__m128i*)dst);
4068
4069         while (w && ((unsigned long)dst & 15))
4070         {
4071             m = (uint32_t) *mask++;
4072             d = (uint32_t) *dst;
4073
4074             *dst++ = (uint8_t) pack_1x64_32 (pixMultiply_1x64 (pixMultiply_1x64 (_mm_movepi64_pi64 (xmmAlpha), unpack_32_1x64 (m)),
4075                                                                unpack_32_1x64 (d)));
4076             w--;
4077         }
4078
4079         /* call prefetch hint to optimize cache load*/
4080         cachePrefetch ((__m128i*)mask);
4081         cachePrefetch ((__m128i*)dst);
4082
4083         while (w >= 16)
4084         {
4085             /* fill cache line with next memory */
4086             cachePrefetchNext ((__m128i*)mask);
4087             cachePrefetchNext ((__m128i*)dst);
4088
4089             xmmMask = load128Unaligned((__m128i*)mask);
4090             xmmDst = load128Aligned((__m128i*)dst);
4091
4092             unpack_128_2x128 (xmmMask, &xmmMaskLo, &xmmMaskHi);
4093             unpack_128_2x128 (xmmDst, &xmmDstLo, &xmmDstHi);
4094
4095             pixMultiply_2x128 (&xmmAlpha, &xmmAlpha, &xmmMaskLo, &xmmMaskHi, &xmmMaskLo, &xmmMaskHi);
4096             pixMultiply_2x128 (&xmmMaskLo, &xmmMaskHi, &xmmDstLo, &xmmDstHi, &xmmDstLo, &xmmDstHi);
4097
4098             save128Aligned ((__m128i*)dst, pack_2x128_128 (xmmDstLo, xmmDstHi));
4099
4100             mask += 16;
4101             dst += 16;
4102             w -= 16;
4103         }
4104
4105         while (w)
4106         {
4107             m = (uint32_t) *mask++;
4108             d = (uint32_t) *dst;
4109
4110             *dst++ = (uint8_t) pack_1x64_32 (pixMultiply_1x64 (pixMultiply_1x64 (_mm_movepi64_pi64 (xmmAlpha), unpack_32_1x64 (m)),
4111                                                                unpack_32_1x64 (d)));
4112             w--;
4113         }
4114     }
4115
4116     _mm_empty();
4117 }
4118
4119 /* -------------------------------------------------------------------------------------------------
4120  * fbCompositeIn_8x8
4121  */
4122
4123 void
4124 fbCompositeIn_8x8sse2 (pixman_op_t op,
4125                       pixman_image_t * pSrc,
4126                       pixman_image_t * pMask,
4127                       pixman_image_t * pDst,
4128                       int16_t      xSrc,
4129                       int16_t      ySrc,
4130                       int16_t      xMask,
4131                       int16_t      yMask,
4132                       int16_t      xDst,
4133                       int16_t      yDst,
4134                       uint16_t     width,
4135                       uint16_t     height)
4136 {
4137     uint8_t     *dstLine, *dst;
4138     uint8_t     *srcLine, *src;
4139     int srcStride, dstStride;
4140     uint16_t    w;
4141     uint32_t    s, d;
4142
4143     __m128i xmmSrc, xmmSrcLo, xmmSrcHi;
4144     __m128i xmmDst, xmmDstLo, xmmDstHi;
4145
4146     fbComposeGetStart (pDst, xDst, yDst, uint8_t, dstStride, dstLine, 1);
4147     fbComposeGetStart (pSrc, xSrc, ySrc, uint8_t, srcStride, srcLine, 1);
4148
4149     while (height--)
4150     {
4151         dst = dstLine;
4152         dstLine += dstStride;
4153         src = srcLine;
4154         srcLine += srcStride;
4155         w = width;
4156
4157         /* call prefetch hint to optimize cache load*/
4158         cachePrefetch ((__m128i*)src);
4159         cachePrefetch ((__m128i*)dst);
4160
4161         while (w && ((unsigned long)dst & 15))
4162         {
4163             s = (uint32_t) *src++;
4164             d = (uint32_t) *dst;
4165
4166             *dst++ = (uint8_t) pack_1x64_32 (pixMultiply_1x64 (unpack_32_1x64 (s),unpack_32_1x64 (d)));
4167             w--;
4168         }
4169
4170         /* call prefetch hint to optimize cache load*/
4171         cachePrefetch ((__m128i*)src);
4172         cachePrefetch ((__m128i*)dst);
4173
4174         while (w >= 16)
4175         {
4176             /* fill cache line with next memory */
4177             cachePrefetchNext ((__m128i*)src);
4178             cachePrefetchNext ((__m128i*)dst);
4179
4180             xmmSrc = load128Unaligned((__m128i*)src);
4181             xmmDst = load128Aligned((__m128i*)dst);
4182
4183             unpack_128_2x128 (xmmSrc, &xmmSrcLo, &xmmSrcHi);
4184             unpack_128_2x128 (xmmDst, &xmmDstLo, &xmmDstHi);
4185
4186             pixMultiply_2x128 (&xmmSrcLo, &xmmSrcHi, &xmmDstLo, &xmmDstHi, &xmmDstLo, &xmmDstHi);
4187
4188             save128Aligned ((__m128i*)dst, pack_2x128_128 (xmmDstLo, xmmDstHi));
4189
4190             src += 16;
4191             dst += 16;
4192             w -= 16;
4193         }
4194
4195         while (w)
4196         {
4197             s = (uint32_t) *src++;
4198             d = (uint32_t) *dst;
4199
4200             *dst++ = (uint8_t) pack_1x64_32 (pixMultiply_1x64 (unpack_32_1x64 (s),unpack_32_1x64 (d)));
4201             w--;
4202         }
4203     }
4204
4205     _mm_empty ();
4206 }
4207
4208 /* -------------------------------------------------------------------------------------------------
4209  * fbCompositeSrcAdd_8888x8x8
4210  */
4211
4212 void
4213 fbCompositeSrcAdd_8888x8x8sse2 (pixman_op_t op,
4214                                pixman_image_t * pSrc,
4215                                pixman_image_t * pMask,
4216                                pixman_image_t * pDst,
4217                                int16_t      xSrc,
4218                                int16_t      ySrc,
4219                                int16_t      xMask,
4220                                int16_t      yMask,
4221                                int16_t      xDst,
4222                                int16_t      yDst,
4223                                uint16_t     width,
4224                                uint16_t     height)
4225 {
4226     uint8_t     *dstLine, *dst;
4227     uint8_t     *maskLine, *mask;
4228     int dstStride, maskStride;
4229     uint16_t    w;
4230     uint32_t    src;
4231     uint8_t     sa;
4232     uint32_t m, d;
4233
4234     __m128i xmmAlpha;
4235     __m128i xmmMask, xmmMaskLo, xmmMaskHi;
4236     __m128i xmmDst, xmmDstLo, xmmDstHi;
4237
4238     fbComposeGetStart (pDst, xDst, yDst, uint8_t, dstStride, dstLine, 1);
4239     fbComposeGetStart (pMask, xMask, yMask, uint8_t, maskStride, maskLine, 1);
4240
4241     fbComposeGetSolid(pSrc, src, pDst->bits.format);
4242
4243     sa = src >> 24;
4244     if (sa == 0)
4245         return;
4246
4247     xmmAlpha = expandAlpha_1x128 (expandPixel_32_1x128 (src));
4248
4249     while (height--)
4250     {
4251         dst = dstLine;
4252         dstLine += dstStride;
4253         mask = maskLine;
4254         maskLine += maskStride;
4255         w = width;
4256
4257         /* call prefetch hint to optimize cache load*/
4258         cachePrefetch ((__m128i*)mask);
4259         cachePrefetch ((__m128i*)dst);
4260
4261         while (w && ((unsigned long)dst & 15))
4262         {
4263             m = (uint32_t) *mask++;
4264             d = (uint32_t) *dst;
4265
4266             *dst++ = (uint8_t) pack_1x64_32 (_mm_adds_pu16 (pixMultiply_1x64 (_mm_movepi64_pi64 (xmmAlpha), unpack_32_1x64 (m)),
4267                                                                               unpack_32_1x64 (d)));
4268             w--;
4269         }
4270
4271         /* call prefetch hint to optimize cache load*/
4272         cachePrefetch ((__m128i*)mask);
4273         cachePrefetch ((__m128i*)dst);
4274
4275         while (w >= 16)
4276         {
4277             /* fill cache line with next memory */
4278             cachePrefetchNext ((__m128i*)mask);
4279             cachePrefetchNext ((__m128i*)dst);
4280
4281             xmmMask = load128Unaligned((__m128i*)mask);
4282             xmmDst = load128Aligned((__m128i*)dst);
4283
4284             unpack_128_2x128 (xmmMask, &xmmMaskLo, &xmmMaskHi);
4285             unpack_128_2x128 (xmmDst, &xmmDstLo, &xmmDstHi);
4286
4287             pixMultiply_2x128 (&xmmAlpha, &xmmAlpha, &xmmMaskLo, &xmmMaskHi, &xmmMaskLo, &xmmMaskHi);
4288
4289             xmmDstLo = _mm_adds_epu16 (xmmMaskLo, xmmDstLo);
4290             xmmDstHi = _mm_adds_epu16 (xmmMaskHi, xmmDstHi);
4291
4292             save128Aligned ((__m128i*)dst, pack_2x128_128 (xmmDstLo, xmmDstHi));
4293
4294             mask += 16;
4295             dst += 16;
4296             w -= 16;
4297         }
4298
4299         while (w)
4300         {
4301             m = (uint32_t) *mask++;
4302             d = (uint32_t) *dst;
4303
4304             *dst++ = (uint8_t) pack_1x64_32 (_mm_adds_pu16 (pixMultiply_1x64 (_mm_movepi64_pi64 (xmmAlpha), unpack_32_1x64 (m)),
4305                                                                               unpack_32_1x64 (d)));
4306             w--;
4307         }
4308     }
4309
4310     _mm_empty();
4311 }
4312
4313 /* -------------------------------------------------------------------------------------------------
4314  * fbCompositeSrcAdd_8000x8000
4315  */
4316
4317 void
4318 fbCompositeSrcAdd_8000x8000sse2 (pixman_op_t op,
4319                                 pixman_image_t * pSrc,
4320                                 pixman_image_t * pMask,
4321                                 pixman_image_t * pDst,
4322                                 int16_t      xSrc,
4323                                 int16_t      ySrc,
4324                                 int16_t      xMask,
4325                                 int16_t      yMask,
4326                                 int16_t      xDst,
4327                                 int16_t      yDst,
4328                                 uint16_t     width,
4329                                 uint16_t     height)
4330 {
4331     uint8_t     *dstLine, *dst;
4332     uint8_t     *srcLine, *src;
4333     int dstStride, srcStride;
4334     uint16_t    w;
4335     uint16_t    t;
4336
4337     fbComposeGetStart (pSrc, xSrc, ySrc, uint8_t, srcStride, srcLine, 1);
4338     fbComposeGetStart (pDst, xDst, yDst, uint8_t, dstStride, dstLine, 1);
4339
4340     while (height--)
4341     {
4342         dst = dstLine;
4343         src = srcLine;
4344
4345         /* call prefetch hint to optimize cache load*/
4346         cachePrefetch ((__m128i*)src);
4347         cachePrefetch ((__m128i*)dst);
4348
4349         dstLine += dstStride;
4350         srcLine += srcStride;
4351         w = width;
4352
4353         /* Small head */
4354         while (w && (unsigned long)dst & 3)
4355         {
4356             t = (*dst) + (*src++);
4357             *dst++ = t | (0 - (t >> 8));
4358             w--;
4359         }
4360
4361         coreCombineAddUsse2 ((uint32_t*)dst, (uint32_t*)src, w >> 2);
4362
4363         /* Small tail */
4364         dst += w & 0xfffc;
4365         src += w & 0xfffc;
4366
4367         w &= 3;
4368
4369         while (w)
4370         {
4371             t = (*dst) + (*src++);
4372             *dst++ = t | (0 - (t >> 8));
4373             w--;
4374         }
4375     }
4376
4377     _mm_empty();
4378 }
4379
4380 /* -------------------------------------------------------------------------------------------------
4381  * fbCompositeSrcAdd_8888x8888
4382  */
4383 void
4384 fbCompositeSrcAdd_8888x8888sse2 (pixman_op_t    op,
4385                                 pixman_image_t *        pSrc,
4386                                 pixman_image_t *        pMask,
4387                                 pixman_image_t *         pDst,
4388                                 int16_t          xSrc,
4389                                 int16_t      ySrc,
4390                                 int16_t      xMask,
4391                                 int16_t      yMask,
4392                                 int16_t      xDst,
4393                                 int16_t      yDst,
4394                                 uint16_t     width,
4395                                 uint16_t     height)
4396 {
4397     uint32_t    *dstLine, *dst;
4398     uint32_t    *srcLine, *src;
4399     int dstStride, srcStride;
4400
4401     fbComposeGetStart (pSrc, xSrc, ySrc, uint32_t, srcStride, srcLine, 1);
4402     fbComposeGetStart (pDst, xDst, yDst, uint32_t, dstStride, dstLine, 1);
4403
4404     while (height--)
4405     {
4406         dst = dstLine;
4407         dstLine += dstStride;
4408         src = srcLine;
4409         srcLine += srcStride;
4410
4411         coreCombineAddUsse2 (dst, src, width);
4412     }
4413
4414     _mm_empty();
4415 }
4416
4417 /* -------------------------------------------------------------------------------------------------
4418  * fbCompositeCopyAreasse2
4419  */
4420
4421 pixman_bool_t
4422 pixmanBltsse2 (uint32_t *src_bits,
4423                 uint32_t *dst_bits,
4424                 int src_stride,
4425                 int dst_stride,
4426                 int src_bpp,
4427                 int dst_bpp,
4428                 int src_x, int src_y,
4429                 int dst_x, int dst_y,
4430                 int width, int height)
4431 {
4432     uint8_t *   src_bytes;
4433     uint8_t *   dst_bytes;
4434     int         byte_width;
4435
4436     if (src_bpp != dst_bpp)
4437         return FALSE;
4438
4439     if (src_bpp == 16)
4440     {
4441         src_stride = src_stride * (int) sizeof (uint32_t) / 2;
4442         dst_stride = dst_stride * (int) sizeof (uint32_t) / 2;
4443         src_bytes = (uint8_t *)(((uint16_t *)src_bits) + src_stride * (src_y) + (src_x));
4444         dst_bytes = (uint8_t *)(((uint16_t *)dst_bits) + dst_stride * (dst_y) + (dst_x));
4445         byte_width = 2 * width;
4446         src_stride *= 2;
4447         dst_stride *= 2;
4448     }
4449     else if (src_bpp == 32)
4450     {
4451         src_stride = src_stride * (int) sizeof (uint32_t) / 4;
4452         dst_stride = dst_stride * (int) sizeof (uint32_t) / 4;
4453         src_bytes = (uint8_t *)(((uint32_t *)src_bits) + src_stride * (src_y) + (src_x));
4454         dst_bytes = (uint8_t *)(((uint32_t *)dst_bits) + dst_stride * (dst_y) + (dst_x));
4455         byte_width = 4 * width;
4456         src_stride *= 4;
4457         dst_stride *= 4;
4458     }
4459     else
4460     {
4461         return FALSE;
4462     }
4463
4464     cachePrefetch ((__m128i*)src_bytes);
4465     cachePrefetch ((__m128i*)dst_bytes);
4466
4467     while (height--)
4468     {
4469         int w;
4470         uint8_t *s = src_bytes;
4471         uint8_t *d = dst_bytes;
4472         src_bytes += src_stride;
4473         dst_bytes += dst_stride;
4474         w = byte_width;
4475
4476         cachePrefetchNext ((__m128i*)s);
4477         cachePrefetchNext ((__m128i*)d);
4478
4479         while (w >= 2 && ((unsigned long)d & 3))
4480         {
4481             *(uint16_t *)d = *(uint16_t *)s;
4482             w -= 2;
4483             s += 2;
4484             d += 2;
4485         }
4486
4487         while (w >= 4 && ((unsigned long)d & 15))
4488         {
4489             *(uint32_t *)d = *(uint32_t *)s;
4490
4491             w -= 4;
4492             s += 4;
4493             d += 4;
4494         }
4495
4496         cachePrefetchNext ((__m128i*)s);
4497         cachePrefetchNext ((__m128i*)d);
4498
4499         while (w >= 64)
4500         {
4501             __m128i xmm0, xmm1, xmm2, xmm3;
4502
4503             /* 128 bytes ahead */
4504             cachePrefetch (((__m128i*)s) + 8);
4505             cachePrefetch (((__m128i*)d) + 8);
4506
4507             xmm0 = load128Unaligned ((__m128i*)(s));
4508             xmm1 = load128Unaligned ((__m128i*)(s+16));
4509             xmm2 = load128Unaligned ((__m128i*)(s+32));
4510             xmm3 = load128Unaligned ((__m128i*)(s+48));
4511
4512             save128Aligned ((__m128i*)(d),    xmm0);
4513             save128Aligned ((__m128i*)(d+16), xmm1);
4514             save128Aligned ((__m128i*)(d+32), xmm2);
4515             save128Aligned ((__m128i*)(d+48), xmm3);
4516
4517             s += 64;
4518             d += 64;
4519             w -= 64;
4520         }
4521
4522         cachePrefetchNext ((__m128i*)s);
4523         cachePrefetchNext ((__m128i*)d);
4524
4525         while (w >= 16)
4526         {
4527             save128Aligned ((__m128i*)d, load128Unaligned ((__m128i*)s) );
4528
4529             w -= 16;
4530             d += 16;
4531             s += 16;
4532         }
4533
4534         cachePrefetchNext ((__m128i*)s);
4535         cachePrefetchNext ((__m128i*)d);
4536
4537         while (w >= 4)
4538         {
4539             *(uint32_t *)d = *(uint32_t *)s;
4540
4541             w -= 4;
4542             s += 4;
4543             d += 4;
4544         }
4545
4546         if (w >= 2)
4547         {
4548             *(uint16_t *)d = *(uint16_t *)s;
4549             w -= 2;
4550             s += 2;
4551             d += 2;
4552         }
4553     }
4554
4555     _mm_empty();
4556
4557     return TRUE;
4558 }
4559
4560 void
4561 fbCompositeCopyAreasse2 (pixman_op_t       op,
4562                         pixman_image_t *        pSrc,
4563                         pixman_image_t *        pMask,
4564                         pixman_image_t *        pDst,
4565                         int16_t         xSrc,
4566                         int16_t         ySrc,
4567                         int16_t         xMask,
4568                         int16_t         yMask,
4569                         int16_t         xDst,
4570                         int16_t         yDst,
4571                         uint16_t                width,
4572                         uint16_t                height)
4573 {
4574     pixmanBltsse2 (pSrc->bits.bits,
4575                     pDst->bits.bits,
4576                     pSrc->bits.rowstride,
4577                     pDst->bits.rowstride,
4578                     PIXMAN_FORMAT_BPP (pSrc->bits.format),
4579                     PIXMAN_FORMAT_BPP (pDst->bits.format),
4580                     xSrc, ySrc, xDst, yDst, width, height);
4581 }
4582
4583 #if 0
4584 /* This code are buggy in MMX version, now the bug was translated to SSE2 version */
4585 void
4586 fbCompositeOver_x888x8x8888sse2 (pixman_op_t      op,
4587                                 pixman_image_t * pSrc,
4588                                 pixman_image_t * pMask,
4589                                 pixman_image_t * pDst,
4590                                 int16_t      xSrc,
4591                                 int16_t      ySrc,
4592                                 int16_t      xMask,
4593                                 int16_t      yMask,
4594                                 int16_t      xDst,
4595                                 int16_t      yDst,
4596                                 uint16_t     width,
4597                                 uint16_t     height)
4598 {
4599     uint32_t    *src, *srcLine, s;
4600     uint32_t    *dst, *dstLine, d;
4601     uint8_t         *mask, *maskLine;
4602     uint32_t    m;
4603     int          srcStride, maskStride, dstStride;
4604     uint16_t w;
4605
4606     __m128i xmmSrc, xmmSrcLo, xmmSrcHi;
4607     __m128i xmmDst, xmmDstLo, xmmDstHi;
4608     __m128i xmmMask, xmmMaskLo, xmmMaskHi;
4609
4610     fbComposeGetStart (pDst, xDst, yDst, uint32_t, dstStride, dstLine, 1);
4611     fbComposeGetStart (pMask, xMask, yMask, uint8_t, maskStride, maskLine, 1);
4612     fbComposeGetStart (pSrc, xSrc, ySrc, uint32_t, srcStride, srcLine, 1);
4613
4614     while (height--)
4615     {
4616         src = srcLine;
4617         srcLine += srcStride;
4618         dst = dstLine;
4619         dstLine += dstStride;
4620         mask = maskLine;
4621         maskLine += maskStride;
4622
4623         w = width;
4624
4625         /* call prefetch hint to optimize cache load*/
4626         cachePrefetch ((__m128i*)src);
4627         cachePrefetch ((__m128i*)dst);
4628         cachePrefetch ((__m128i*)mask);
4629
4630         while (w && (unsigned long)dst & 15)
4631         {
4632             s = 0xff000000 | *src++;
4633             m = (uint32_t) *mask++;
4634             d = *dst;
4635
4636             __m64 ms = unpack_32_1x64 (s);
4637
4638             if (m != 0xff)
4639             {
4640                 ms = inOver_1x64 (ms,
4641                                   xMask00ff,
4642                                   expandAlphaRev_1x64 (unpack_32_1x64 (m)),
4643                                   unpack_32_1x64 (d));
4644             }
4645
4646             *dst++ = pack_1x64_32 (ms);
4647             w--;
4648         }
4649
4650         /* call prefetch hint to optimize cache load*/
4651         cachePrefetch ((__m128i*)src);
4652         cachePrefetch ((__m128i*)dst);
4653         cachePrefetch ((__m128i*)mask);
4654
4655         while (w >= 4)
4656         {
4657             /* fill cache line with next memory */
4658             cachePrefetchNext ((__m128i*)src);
4659             cachePrefetchNext ((__m128i*)dst);
4660             cachePrefetchNext ((__m128i*)mask);
4661
4662             m = *(uint32_t*) mask;
4663             xmmSrc = _mm_or_si128 (load128Unaligned ((__m128i*)src), Maskff000000);
4664
4665             if (m == 0xffffffff)
4666             {
4667                 save128Aligned ((__m128i*)dst, xmmSrc);
4668             }
4669             else
4670             {
4671                 xmmDst = load128Aligned ((__m128i*)dst);
4672
4673                 xmmMask = _mm_unpacklo_epi16 (unpack_32_1x128 (m), _mm_setzero_si128());
4674
4675                 unpack_128_2x128 (xmmSrc, &xmmSrcLo, &xmmSrcHi);
4676                 unpack_128_2x128 (xmmMask, &xmmMaskLo, &xmmMaskHi);
4677                 unpack_128_2x128 (xmmDst, &xmmDstLo, &xmmDstHi);
4678
4679                 expandAlphaRev_2x128 (xmmMaskLo, xmmMaskHi, &xmmMaskLo, &xmmMaskHi);
4680
4681                 inOver_2x128 (xmmSrcLo, xmmSrcHi, Mask00ff, Mask00ff, xmmMaskLo, xmmMaskHi, &xmmDstLo, &xmmDstHi);
4682
4683                 save128Aligned( (__m128i*)dst, pack_2x128_128 (xmmDstLo, xmmDstHi));
4684             }
4685
4686             src += 4;
4687             dst += 4;
4688             mask += 4;
4689             w -= 4;
4690         }
4691
4692         while (w)
4693         {
4694             m = (uint32_t) *mask++;
4695
4696             if (m)
4697             {
4698                 s = 0xff000000 | *src;
4699
4700                 if (m == 0xff)
4701                 {
4702                     *dst = s;
4703                 }
4704                 else
4705                 {
4706                     d = *dst;
4707
4708                     *dst = pack_1x64_32 (inOver_1x64 (unpack_32_1x64 (s),
4709                                                       xMask00ff,
4710                                                       expandAlphaRev_1x64 (unpack_32_1x64 (m)),
4711                                                       unpack_32_1x64 (d)));
4712                 }
4713
4714             }
4715
4716             src++;
4717             dst++;
4718             w--;
4719         }
4720     }
4721
4722     _mm_empty();
4723 }
4724 #endif /* #if 0 */
4725
4726 #endif /* USE_SSE2 */