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