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