pixman/pixman-sse.c

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