/*

 * Copyright © 2004 Red Hat, Inc.

 * Copyright © 2004 Nicholas Miell

 * Copyright © 2005 Trolltech AS

 *

 * Permission to use, copy, modify, distribute, and sell this software and its

 * documentation for any purpose is hereby granted without fee, provided that

 * the above copyright notice appear in all copies and that both that

 * copyright notice and this permission notice appear in supporting

 * documentation, and that the name of Red Hat not be used in advertising or

 * publicity pertaining to distribution of the software without specific,

 * written prior permission.  Red Hat makes no representations about the

 * suitability of this software for any purpose.  It is provided "as is"

 * without express or implied warranty.

 *

 * THE COPYRIGHT HOLDERS DISCLAIM ALL WARRANTIES WITH REGARD TO THIS

 * SOFTWARE, INCLUDING ALL IMPLIED WARRANTIES OF MERCHANTABILITY AND

 * FITNESS, IN NO EVENT SHALL THE COPYRIGHT HOLDERS BE LIABLE FOR ANY

 * SPECIAL, INDIRECT OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES

 * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN

 * AN ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING

 * OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS

 * SOFTWARE.

 *

 * Author:  Søren Sandmann (sandmann@redhat.com)

 * Minor Improvements: Nicholas Miell (nmiell@gmail.com)

 * MMX code paths for fbcompose.c by Lars Knoll (lars@trolltech.com) 

 *

 * Based on work by Owen Taylor

 */

//Portions Copyright (c)  2008-2009 Nokia Corporation and/or its subsidiary(-ies). All rights reserved. 

#ifdef HAVE_CONFIG_H

#include "config.h"

#endif

#include <liboil/liboil.h>

#include <liboil/liboilfunction.h>

#include <xmmintrin.h> /* for _mm_shuffle_pi16 and _MM_SHUFFLE */

typedef uint32_t CARD32;

typedef uint16_t CARD16;

typedef int16_t INT16;

typedef uint8_t CARD8;

typedef uint64_t ullong;

typedef CARD32* PicturePtr;

typedef CARD32* FbBits;

typedef int FbStride;

#include "fbmmx.h"

#include "fbpict.h"

#define CHECKPOINT()

OIL_DECLARE_CLASS (composite_in_argb);

OIL_DECLARE_CLASS (composite_in_argb_const_src);

OIL_DECLARE_CLASS (composite_in_argb_const_mask);

OIL_DECLARE_CLASS (composite_over_argb);

OIL_DECLARE_CLASS (composite_over_argb_const_src);

OIL_DECLARE_CLASS (composite_add_argb);

OIL_DECLARE_CLASS (composite_add_argb_const_src);

OIL_DECLARE_CLASS (composite_in_over_argb);

OIL_DECLARE_CLASS (composite_in_over_argb_const_src);

OIL_DECLARE_CLASS (composite_in_over_argb_const_mask);

OIL_DECLARE_CLASS (composite_over_u8);

OIL_DECLARE_CLASS (composite_add_u8);

/* --------------- MMX code patch for fbcompose.c --------------------- */

#if 0

static void

mmxCombineMaskU (uint32_t *dest, const uint32_t *src, const uint8_t *mask, int width)

{

    const __m64 mmx_0 = _mm_setzero_si64();

    const __m64 mmx_4x0080 = (__m64) 0x0080008000800080ULL;

    const uint32_t *end = mask + width;

    while (mask < end) {

        __m64 a = MmxTo(*mask);

        __m64 s = MmxTo(*src);

        a = MmxAlpha(a);

        MmxMul(s, a);

        *dest = MmxFrom(s);

        ++src;

        ++dest;

        ++mask;

    }

    _mm_empty();

}

#endif

#ifdef ENABLE_BROKEN_IMPLS

static void

mmxCombineOverU (uint32_t *dest, const uint32_t *src, int width)

{

    const __m64 mmx_0 = _mm_setzero_si64();

    const __m64 mmx_4x0080 = (__m64) 0x0080008000800080ULL;

    const __m64 mmx_4x00ff = (__m64) 0x00ff00ff00ff00ffULL;

    const uint32_t *end = dest + width;

    while (dest < end) {

        __m64 x, y, a;

        x = MmxTo(*src);

        y = MmxTo(*dest);

        a = MmxAlpha(x);

        a = MmxNegate(a);

        MmxMulAdd(y, a, x);

        *dest = MmxFrom(y);

        ++dest;

        ++src;

    }

    _mm_empty();

}

OIL_DEFINE_IMPL_FULL(mmxCombineOverU, composite_over_argb, OIL_IMPL_FLAG_MMX);

#endif

#if 0

static FASTCALL void

mmxCombineOverReverseU (CARD32 *dest, const CARD32 *src, int width)

{

    const __m64 mmx_0 = _mm_setzero_si64();

    const __m64 mmx_4x0080 = (__m64) 0x0080008000800080ULL;

    const __m64 mmx_4x00ff = (__m64) 0x00ff00ff00ff00ffULL;

    const CARD32 *end = dest + width;

    while (dest < end) {

        __m64 x, y, a;

        x = MmxTo(*dest);

        y = MmxTo(*src);

        a = MmxAlpha(x);

        a = MmxNegate(a);

        MmxMulAdd(y, a, x);

        *dest = MmxFrom(y);

        ++dest;

        ++src;

    }

    _mm_empty();

}

#endif

#if 0

static void

mmxCombineInU (CARD32 *dest, const CARD32 *src, int width)

{

    const __m64 mmx_0 = _mm_setzero_si64();

    const __m64 mmx_4x0080 = (__m64) 0x0080008000800080ULL;

    const CARD32 *end = dest + width;

    while (dest < end) {

        __m64 x, a;

        x = MmxTo(*src);

        a = MmxTo(*dest);

        a = MmxAlpha(a);

        MmxMul(x, a);

        *dest = MmxFrom(x);

        ++dest;

        ++src;

    }

    _mm_empty();

}

#endif

#if 0

static FASTCALL void

mmxCombineInReverseU (CARD32 *dest, const CARD32 *src, int width)

{

    const __m64 mmx_0 = _mm_setzero_si64();

    const __m64 mmx_4x0080 = (__m64) 0x0080008000800080ULL;

    const CARD32 *end = dest + width;

    while (dest < end) {

        __m64 x, a;

        x = MmxTo(*dest);

        a = MmxTo(*src);

        a = MmxAlpha(a);

        MmxMul(x, a);

        *dest = MmxFrom(x);

        ++dest;

        ++src;

    }

    _mm_empty();

}

#endif

#if 0

static FASTCALL void

mmxCombineOutU (CARD32 *dest, const CARD32 *src, int width)

{

    const __m64 mmx_0 = _mm_setzero_si64();

    const __m64 mmx_4x0080 = (__m64) 0x0080008000800080ULL;

    const __m64 mmx_4x00ff = (__m64) 0x00ff00ff00ff00ffULL;

    const CARD32 *end = dest + width;

    while (dest < end) {

        __m64 x, a;

        x = MmxTo(*src);

        a = MmxTo(*dest);

        a = MmxAlpha(a);

        a = MmxNegate(a);

        MmxMul(x, a);

        *dest = MmxFrom(x);

        ++dest;

        ++src;

    }

    _mm_empty();

}

#endif

#if 0

static FASTCALL void

mmxCombineOutReverseU (CARD32 *dest, const CARD32 *src, int width)

{

    const __m64 mmx_0 = _mm_setzero_si64();

    const __m64 mmx_4x0080 = (__m64) 0x0080008000800080ULL;

    const __m64 mmx_4x00ff = (__m64) 0x00ff00ff00ff00ffULL;

    const CARD32 *end = dest + width;

    while (dest < end) {

        __m64 x, a;

        x = MmxTo(*dest);

        a = MmxTo(*src);

        a = MmxAlpha(a);

        a = MmxNegate(a);

        MmxMul(x, a);

        *dest = MmxFrom(x);

        ++dest;

        ++src;

    }

    _mm_empty();

}

static FASTCALL void

mmxCombineAtopU (CARD32 *dest, const CARD32 *src, int width)

{

    const __m64 mmx_0 = _mm_setzero_si64();

    const __m64 mmx_4x0080 = (__m64) 0x0080008000800080ULL;

    const __m64 mmx_4x00ff = (__m64) 0x00ff00ff00ff00ffULL;

    const CARD32 *end = dest + width;

    while (dest < end) {

        __m64 s, da, d, sia;

        s = MmxTo(*src);

        d = MmxTo(*dest);

        sia = MmxAlpha(s);

        sia = MmxNegate(sia);

        da = MmxAlpha(d);

        MmxAddMul(s, da, d, sia);

        *dest = MmxFrom(s);

        ++dest;

        ++src;

    }

    _mm_empty();

}

static FASTCALL void

mmxCombineAtopReverseU (CARD32 *dest, const CARD32 *src, int width)

{

    const __m64 mmx_0 = _mm_setzero_si64();

    const __m64 mmx_4x0080 = (__m64) 0x0080008000800080ULL;

    const __m64 mmx_4x00ff = (__m64) 0x00ff00ff00ff00ffULL;

    const CARD32 *end;

    end = dest + width;

    while (dest < end) {

        __m64 s, dia, d, sa;

        s = MmxTo(*src);

        d = MmxTo(*dest);

        sa = MmxAlpha(s);

        dia = MmxAlpha(d);

        dia = MmxNegate(dia);

        MmxAddMul(s, dia, d, sa);

        *dest = MmxFrom(s);

        ++dest;

        ++src;

    }

    _mm_empty();

}

static FASTCALL void

mmxCombineXorU (CARD32 *dest, const CARD32 *src, int width)

{

    const __m64 mmx_0 = _mm_setzero_si64();

    const __m64 mmx_4x0080 = (__m64) 0x0080008000800080ULL;

    const __m64 mmx_4x00ff = (__m64) 0x00ff00ff00ff00ffULL;

    const CARD32 *end = dest + width;

    while (dest < end) {

        __m64 s, dia, d, sia;

        s = MmxTo(*src);

        d = MmxTo(*dest);

        sia = MmxAlpha(s);

        dia = MmxAlpha(d);

        sia = MmxNegate(sia);

        dia = MmxNegate(dia);

        MmxAddMul(s, dia, d, sia);

        *dest = MmxFrom(s);

        ++dest;

        ++src;

    }

    _mm_empty();

}

#endif

static void

mmxCombineAddU (uint32_t *dest, const uint32_t *src, int width)

{

    const __m64 mmx_0 = _mm_setzero_si64();

    const uint32_t *end = dest + width;

    while (dest < end) {

        __m64 s, d;

        s = MmxTo(*src);

        d = MmxTo(*dest);

        s = MmxAdd(s, d);

        *dest = MmxFrom(s);

        ++dest;

        ++src;

    }

    _mm_empty();

}

OIL_DEFINE_IMPL_FULL(mmxCombineAddU, composite_add_argb, OIL_IMPL_FLAG_MMX | OIL_IMPL_FLAG_SSE);

#if 0

static FASTCALL void

mmxCombineSaturateU (CARD32 *dest, const CARD32 *src, int width)

{

    const __m64 mmx_0 = _mm_setzero_si64();

    const __m64 mmx_4x0080 = (__m64) 0x0080008000800080ULL;

    const CARD32 *end = dest + width;

    while (dest < end) {

        CARD32 s = *src;

        CARD32 d = *dest;

        __m64 ms = MmxTo(s);

        __m64 md = MmxTo(d);

        CARD32 sa = s >> 24;

        CARD32 da = ~d >> 24;

        if (sa > da) {

            __m64 msa = MmxTo(FbIntDiv(da, sa));

            msa = MmxAlpha(msa);

            MmxMul(ms, msa);

        }

        MmxAdd(md, ms);

        *dest = MmxFrom(md);

        ++src;

        ++dest;

    }

    _mm_empty();

}

static FASTCALL void

mmxCombineSrcC (CARD32 *dest, CARD32 *src, CARD32 *mask, int width)

{

    const __m64 mmx_0 = _mm_setzero_si64();

    const __m64 mmx_4x0080 = (__m64) 0x0080008000800080ULL;

    const CARD32 *end = src + width;

    while (src < end) {

        __m64 a = MmxTo(*mask);

        __m64 s = MmxTo(*src);

        MmxMul(s, a);

        *dest = MmxFrom(s);

        ++src;

        ++mask;

        ++dest;

    }

    _mm_empty();

}

static FASTCALL void

mmxCombineOverC (CARD32 *dest, CARD32 *src, CARD32 *mask, int width)

{

    const __m64 mmx_0 = _mm_setzero_si64();

    const __m64 mmx_4x0080 = (__m64) 0x0080008000800080ULL;

    const __m64 mmx_4x00ff = (__m64) 0x00ff00ff00ff00ffULL;

    const CARD32 *end = src + width;

    while (src < end) {

        __m64 a = MmxTo(*mask);

        __m64 s = MmxTo(*src);

        __m64 d = MmxTo(*dest);

        __m64 sa = MmxAlpha(s);

        MmxMul(s, a);

        MmxMul(a, sa);

        a = MmxNegate(a);

        MmxMulAdd(d, a, s);

        *dest = MmxFrom(d);

        ++src;

        ++dest;

        ++mask;

    }

    _mm_empty();

}

static FASTCALL void

mmxCombineOverReverseC (CARD32 *dest, CARD32 *src, CARD32 *mask, int width)

{

    const __m64 mmx_0 = _mm_setzero_si64();

    const __m64 mmx_4x0080 = (__m64) 0x0080008000800080ULL;

    const __m64 mmx_4x00ff = (__m64) 0x00ff00ff00ff00ffULL;

    const CARD32 *end = src + width;

    while (src < end) {

        __m64 a = MmxTo(*mask);

        __m64 s = MmxTo(*src);

        __m64 d = MmxTo(*dest);

        __m64 da = MmxAlpha(d);

        da = MmxNegate(da);

        MmxMul(s, a);

        MmxMulAdd(s, da, d);

        *dest = MmxFrom(s);

        ++src;

        ++dest;

        ++mask;

    }

    _mm_empty();

}

static FASTCALL void

mmxCombineInC (CARD32 *dest, CARD32 *src, CARD32 *mask, int width)

{

    const __m64 mmx_0 = _mm_setzero_si64();

    const __m64 mmx_4x0080 = (__m64) 0x0080008000800080ULL;

    const CARD32 *end = src + width;

    while (src < end) {

        __m64 a = MmxTo(*mask);

        __m64 s = MmxTo(*src);

        __m64 d = MmxTo(*dest);

        __m64 da = MmxAlpha(d);

        MmxMul(s, a);

        MmxMul(s, da);

        *dest = MmxFrom(s);

        ++src;

        ++dest;

        ++mask;

    }

    _mm_empty();

}

static FASTCALL void

mmxCombineInReverseC (CARD32 *dest, CARD32 *src, CARD32 *mask, int width)

{

    const __m64 mmx_0 = _mm_setzero_si64();

    const __m64 mmx_4x0080 = (__m64) 0x0080008000800080ULL;

    const CARD32 *end = src + width;

    while (src < end) {

        __m64 a = MmxTo(*mask);

        __m64 s = MmxTo(*src);

        __m64 d = MmxTo(*dest);

        __m64 sa = MmxAlpha(s);

        MmxMul(a, sa);

        MmxMul(d, a);

        *dest = MmxFrom(d);

        ++src;

        ++dest;

        ++mask;

    }

    _mm_empty();

}

static FASTCALL void

mmxCombineOutC (CARD32 *dest, CARD32 *src, CARD32 *mask, int width)

{

    const __m64 mmx_0 = _mm_setzero_si64();

    const __m64 mmx_4x0080 = (__m64) 0x0080008000800080ULL;

    const __m64 mmx_4x00ff = (__m64) 0x00ff00ff00ff00ffULL;

    const CARD32 *end = src + width;

    while (src < end) {

        __m64 a = MmxTo(*mask);

        __m64 s = MmxTo(*src);

        __m64 d = MmxTo(*dest);

        __m64 da = MmxAlpha(d);

        da = MmxNegate(da);

        MmxMul(s, a);

        MmxMul(s, da);

        *dest = MmxFrom(s);

        ++src;

        ++dest;

        ++mask;

    }

    _mm_empty();

}

static FASTCALL void

mmxCombineOutReverseC (CARD32 *dest, CARD32 *src, CARD32 *mask, int width)

{

    const __m64 mmx_0 = _mm_setzero_si64();

    const __m64 mmx_4x0080 = (__m64) 0x0080008000800080ULL;

    const __m64 mmx_4x00ff = (__m64) 0x00ff00ff00ff00ffULL;

    const CARD32 *end = src + width;

    while (src < end) {

        __m64 a = MmxTo(*mask);

        __m64 s = MmxTo(*src);

        __m64 d = MmxTo(*dest);

        __m64 sa = MmxAlpha(s);

        MmxMul(a, sa);

        a = MmxNegate(a);

        MmxMul(d, a);

        *dest = MmxFrom(d);

        ++src;

        ++dest;

        ++mask;

    }

    _mm_empty();

}

static FASTCALL void

mmxCombineAtopC (CARD32 *dest, CARD32 *src, CARD32 *mask, int width)

{

    const __m64 mmx_0 = _mm_setzero_si64();

    const __m64 mmx_4x0080 = (__m64) 0x0080008000800080ULL;

    const __m64 mmx_4x00ff = (__m64) 0x00ff00ff00ff00ffULL;

    const CARD32 *end = src + width;

    while (src < end) {

        __m64 a = MmxTo(*mask);

        __m64 s = MmxTo(*src);

        __m64 d = MmxTo(*dest);

        __m64 da = MmxAlpha(d);

        __m64 sa = MmxAlpha(s); 

        MmxMul(s, a);

        MmxMul(a, sa);

        a = MmxNegate(a);

        MmxAddMul(d, a, s, da);

        *dest = MmxFrom(d);

        ++src;

        ++dest;

        ++mask;

    }

    _mm_empty();

}

static FASTCALL void

mmxCombineAtopReverseC (CARD32 *dest, CARD32 *src, CARD32 *mask, int width)

{

    const __m64 mmx_0 = _mm_setzero_si64();

    const __m64 mmx_4x0080 = (__m64) 0x0080008000800080ULL;

    const __m64 mmx_4x00ff = (__m64) 0x00ff00ff00ff00ffULL;

    const CARD32 *end = src + width;

    while (src < end) {

        __m64 a = MmxTo(*mask);

        __m64 s = MmxTo(*src);

        __m64 d = MmxTo(*dest);

        __m64 da = MmxAlpha(d);

        __m64 sa = MmxAlpha(s)

        MmxMul(s, a);

        MmxMul(a, sa);

        da = MmxNegate(da);

        MmxAddMul(d, a, s, da);

        *dest = MmxFrom(d);

        ++src;

        ++dest;

        ++mask;

    }

    _mm_empty();

}

static FASTCALL void

mmxCombineXorC (CARD32 *dest, CARD32 *src, CARD32 *mask, int width)

{

    const __m64 mmx_0 = _mm_setzero_si64();

    const __m64 mmx_4x0080 = (__m64) 0x0080008000800080ULL;

    const __m64 mmx_4x00ff = (__m64) 0x00ff00ff00ff00ffULL;

    const CARD32 *end = src + width;

    while (src < end) {

        __m64 a = MmxTo(*mask);

        __m64 s = MmxTo(*src);

        __m64 d = MmxTo(*dest);

        __m64 da = MmxAlpha(d);

        __m64 sa = MmxAlpha(s);

        MmxMul(s, a);

        MmxMul(a, sa);

        da = MmxNegate(da);

        a = MmxNegate(a);

        MmxAddMul(d, a, s, da);

        *dest = MmxFrom(d);

        ++src;

        ++dest;

        ++mask;

    }

    _mm_empty();

}

static FASTCALL void

mmxCombineAddC (CARD32 *dest, CARD32 *src, CARD32 *mask, int width)

{

    const __m64 mmx_0 = _mm_setzero_si64();

    const __m64 mmx_4x0080 = (__m64) 0x0080008000800080ULL;

    const CARD32 *end = src + width;

    while (src < end) {

        __m64 a = MmxTo(*mask);

        __m64 s = MmxTo(*src);

        __m64 d = MmxTo(*dest);

        MmxMul(s, a);

        d = MmxAdd(s, d);

        *dest = MmxFrom(d);

        ++src;

        ++dest;

        ++mask;

    }

    _mm_empty();

}

extern FbComposeFunctions composeFunctions;

void fbComposeSetupMMX(void)

{

    /* check if we have MMX support and initialize accordingly */

    if (fbHaveMMX()) {

        composeFunctions.combineU[PictOpOver] = mmxCombineOverU;

        composeFunctions.combineU[PictOpOverReverse] = mmxCombineOverReverseU;

        composeFunctions.combineU[PictOpIn] = mmxCombineInU;

        composeFunctions.combineU[PictOpInReverse] = mmxCombineInReverseU;

        composeFunctions.combineU[PictOpOut] = mmxCombineOutU;

        composeFunctions.combineU[PictOpOutReverse] = mmxCombineOutReverseU;

        composeFunctions.combineU[PictOpAtop] = mmxCombineAtopU;

        composeFunctions.combineU[PictOpAtopReverse] = mmxCombineAtopReverseU;

        composeFunctions.combineU[PictOpXor] = mmxCombineXorU;

        composeFunctions.combineU[PictOpAdd] = mmxCombineAddU;

        composeFunctions.combineU[PictOpSaturate] = mmxCombineSaturateU;

        composeFunctions.combineC[PictOpSrc] = mmxCombineSrcC;

        composeFunctions.combineC[PictOpOver] = mmxCombineOverC;

        composeFunctions.combineC[PictOpOverReverse] = mmxCombineOverReverseC;

        composeFunctions.combineC[PictOpIn] = mmxCombineInC;

        composeFunctions.combineC[PictOpInReverse] = mmxCombineInReverseC;

        composeFunctions.combineC[PictOpOut] = mmxCombineOutC;

        composeFunctions.combineC[PictOpOutReverse] = mmxCombineOutReverseC;

        composeFunctions.combineC[PictOpAtop] = mmxCombineAtopC;

        composeFunctions.combineC[PictOpAtopReverse] = mmxCombineAtopReverseC;

        composeFunctions.combineC[PictOpXor] = mmxCombineXorC;

        composeFunctions.combineC[PictOpAdd] = mmxCombineAddC;

        composeFunctions.combineMaskU = mmxCombineMaskU;

    } 

}

#endif

/* ------------------ MMX code paths called from fbpict.c ----------------------- */

typedef union {

  __m64 m64;

  uint64_t ull;

} m64_ull;

typedef struct

{

    m64_ull mmx_4x00ff;

    m64_ull mmx_4x0080;

    m64_ull mmx_565_rgb;

    m64_ull mmx_565_unpack_multiplier;

    m64_ull mmx_565_r;

    m64_ull mmx_565_g;

    m64_ull mmx_565_b;

    m64_ull mmx_mask_0;

    m64_ull mmx_mask_1;

    m64_ull mmx_mask_2;

    m64_ull mmx_mask_3;

    m64_ull mmx_full_alpha;

    m64_ull mmx_ffff0000ffff0000;

    m64_ull mmx_0000ffff00000000;

    m64_ull mmx_000000000000ffff;

} MMXData;

static const MMXData c =

{

    .mmx_4x00ff.ull =			0x00ff00ff00ff00ffULL,

    .mmx_4x0080.ull =			0x0080008000800080ULL,

    .mmx_565_rgb.ull =			0x000001f0003f001fULL,

    .mmx_565_r.ull =			0x000000f800000000ULL,

    .mmx_565_g.ull =			0x0000000000fc0000ULL,

    .mmx_565_b.ull =			0x00000000000000f8ULL,

    .mmx_mask_0.ull =			0xffffffffffff0000ULL,

    .mmx_mask_1.ull =			0xffffffff0000ffffULL,

    .mmx_mask_2.ull =			0xffff0000ffffffffULL,

    .mmx_mask_3.ull =			0x0000ffffffffffffULL,

    .mmx_full_alpha.ull =			0x00ff000000000000ULL,

    .mmx_565_unpack_multiplier.ull =	0x0000008404100840ULL,

    .mmx_ffff0000ffff0000.ull =		0xffff0000ffff0000ULL,

    .mmx_0000ffff00000000.ull =		0x0000ffff00000000ULL,

    .mmx_000000000000ffff.ull =		0x000000000000ffffULL,

};

#define MC(x) ((__m64) c.mmx_##x.m64)

static __inline__ __m64

shift (__m64 v, int s)

{

    if (s > 0)

	return _mm_slli_si64 (v, s);

    else if (s < 0)

	return _mm_srli_si64 (v, -s);

    else

	return v;

}

static __inline__ __m64

negate (__m64 mask)

{

    return _mm_xor_si64 (mask, MC(4x00ff));

}

static __inline__ __m64

pix_multiply (__m64 a, __m64 b)

{

    __m64 res;

    res = _mm_mullo_pi16 (a, b);

    res = _mm_adds_pu16 (res, MC(4x0080));

    res = _mm_adds_pu16 (res, _mm_srli_pi16 (res, 8));

    res = _mm_srli_pi16 (res, 8);

    return res;

}

static __inline__ __m64

expand_alpha (__m64 pixel)

{

    return _mm_shuffle_pi16 (pixel, _MM_SHUFFLE(3, 3, 3, 3));

}

static __inline__ __m64

expand_alpha_rev (__m64 pixel)

{

    return _mm_shuffle_pi16 (pixel, _MM_SHUFFLE(0, 0, 0, 0));

}    

static __inline__ __m64

invert_colors (__m64 pixel)

{

    return _mm_shuffle_pi16 (pixel, _MM_SHUFFLE(3, 0, 1, 2));

}

/* Notes about writing mmx code

 *

 * give memory operands as the second operand. If you give it as the

 * first, gcc will first load it into a register, then use that

 * register

 *

 *   ie. use

 *

 *         _mm_mullo_pi16 (x, mmx_constant);

 *

 *   not

 *

 *         _mm_mullo_pi16 (mmx_constant, x);

 *

 * Also try to minimize dependencies. i.e. when you need a value, try

 * to calculate it from a value that was calculated as early as

 * possible.

 */

static __inline__ __m64

over (__m64 src, __m64 srca, __m64 dest)

{

    return  _mm_adds_pu8 (src, pix_multiply(dest, negate(srca)));

}

static __inline__ __m64

over_rev_non_pre (__m64 src, __m64 dest)

{

    __m64 srca = expand_alpha (src);

    __m64 srcfaaa = _mm_or_si64 (srca, MC(full_alpha));

    return over(pix_multiply(invert_colors(src), srcfaaa), srca, dest);

}

static __inline__ __m64

in (__m64 src,

    __m64 mask)

{

    return pix_multiply (src, mask);

}

static __inline__ __m64

in_over (__m64 src,

	 __m64 srca,

	 __m64 mask,

	 __m64 dest)

{

    return over(in(src, mask), pix_multiply(srca, mask), dest);

}

static __inline__ __m64

load8888 (CARD32 v)

{

    return _mm_unpacklo_pi8 (_mm_cvtsi32_si64 (v), _mm_setzero_si64());

}

static __inline__ __m64

pack8888 (__m64 lo, __m64 hi)

{

    __m64 r;

    r = _mm_packs_pu16 (lo, hi);

    return r;

}

static __inline__ CARD32

store8888 (__m64 v)

{

    return _mm_cvtsi64_si32(pack8888(v, _mm_setzero_si64()));

}

/* Expand 16 bits positioned at @pos (0-3) of a mmx register into

 *

 *    00RR00GG00BB

 * 

 * --- Expanding 565 in the low word ---

 * 

 * m = (m << (32 - 3)) | (m << (16 - 5)) | m;

 * m = m & (01f0003f001f);

 * m = m * (008404100840);

 * m = m >> 8;

 * 

 * Note the trick here - the top word is shifted by another nibble to

 * avoid it bumping into the middle word

 */

static __inline__ __m64

expand565 (__m64 pixel, int pos)

{

    __m64 p = pixel;

    __m64 t1, t2;

    /* move pixel to low 16 bit and zero the rest */

    p = shift (shift (p, (3 - pos) * 16), -48); 

    t1 = shift (p, 36 - 11);

    t2 = shift (p, 16 - 5);

    p = _mm_or_si64 (t1, p);

    p = _mm_or_si64 (t2, p);

    p = _mm_and_si64 (p, MC(565_rgb));

    pixel = _mm_mullo_pi16 (p, MC(565_unpack_multiplier));

    return _mm_srli_pi16 (pixel, 8);

}

static __inline__ __m64

expand8888 (__m64 in, int pos)

{

    if (pos == 0)

	return _mm_unpacklo_pi8 (in, _mm_setzero_si64());

    else

	return _mm_unpackhi_pi8 (in, _mm_setzero_si64());

}

static __inline__ __m64

pack565 (__m64 pixel, __m64 target, int pos)

{

    __m64 p = pixel;

    __m64 t = target;

    __m64 r, g, b;

    r = _mm_and_si64 (p, MC(565_r));

    g = _mm_and_si64 (p, MC(565_g));

    b = _mm_and_si64 (p, MC(565_b));

    r = shift (r, - (32 - 8) + pos * 16);

    g = shift (g, - (16 - 3) + pos * 16);

    b = shift (b, - (0  + 3) + pos * 16);

    if (pos == 0)

	t = _mm_and_si64 (t, MC(mask_0));

    else if (pos == 1)

	t = _mm_and_si64 (t, MC(mask_1));

    else if (pos == 2)

	t = _mm_and_si64 (t, MC(mask_2));

    else if (pos == 3)

	t = _mm_and_si64 (t, MC(mask_3));

    p = _mm_or_si64 (r, t);

    p = _mm_or_si64 (g, p);

    return _mm_or_si64 (b, p);

}

#ifdef ENABLE_BROKEN_IMPLS

/* broken.  See Debian bug #340932 */

static void

fbCompositeSolid_nx8888mmx (uint32_t *dst, uint32_t *src, int w)

{

    __m64	vsrc, vsrca;

    vsrc = load8888 (*src);

    vsrca = expand_alpha (vsrc);

    while (w && (unsigned long)dst & 7)

    {

        *dst = store8888(over(vsrc, vsrca, load8888(*dst)));

        w--;

        dst++;

    }

    while (w >= 2)

    {

        __m64 vdest;

        __m64 dest0, dest1;

        vdest = *(__m64 *)dst;

        dest0 = over(vsrc, vsrca, expand8888(vdest, 0));

        dest1 = over(vsrc, vsrca, expand8888(vdest, 1));

        *(__m64 *)dst = pack8888(dest0, dest1);

        dst += 2;

        w -= 2;

    }

    while (w)

    {

        *dst = store8888(over(vsrc, vsrca, load8888(*dst)));

        w--;

        dst++;

    }

    _mm_empty();

}

OIL_DEFINE_IMPL_FULL(fbCompositeSolid_nx8888mmx, composite_over_argb_const_src,

    OIL_IMPL_FLAG_MMX| OIL_IMPL_FLAG_MMXEXT);

#endif

#if 0

void

fbCompositeSolid_nx0565mmx (CARD8	op,

			    PicturePtr pSrc,

			    PicturePtr pMask,

			    PicturePtr pDst,

			    INT16	xSrc,

			    INT16	ySrc,

			    INT16	xMask,

			    INT16	yMask,

			    INT16	xDst,

			    INT16	yDst,

			    CARD16	width,

			    CARD16	height)

{

    CARD32	src;

    CARD16	*dstLine, *dst;

    CARD16	w;

    FbStride	dstStride;

    __m64	vsrc, vsrca;

    CHECKPOINT();

    fbComposeGetSolid(pSrc, src, pDst->format);

    if (src >> 24 == 0)

	return;

    fbComposeGetStart (pDst, xDst, yDst, CARD16, dstStride, dstLine, 1);

    vsrc = load8888 (src);

    vsrca = expand_alpha (vsrc);

    while (height--)

    {

	dst = dstLine;

	dstLine += dstStride;

	w = width;

	CHECKPOINT();

	while (w && (unsigned long)dst & 7)

	{

	    ullong d = *dst;

	    __m64 vdest = expand565 ((__m64)d, 0);

	    vdest = pack565(over(vsrc, vsrca, vdest), vdest, 0);