/*
 * Copyright (c) 2005
 *	Eric Anholt.  All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR BE LIABLE
 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
 * SUCH DAMAGE.
 */
//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/liboilclasses.h"
#include "liboil/liboilfunction.h"
#include <emmintrin.h>
#include <xmmintrin.h>

/* TODO: If we have gcc 4.2 or above, do this. Otherwise, disable all SSE use */
#define SSE_FUNCTION __attribute__((force_align_arg_pointer))

SSE_FUNCTION static void
clamp_u8_sse (uint8_t *dest, uint8_t *src1, int n, uint8_t *src2_1,
    uint8_t *src3_1)
{
  __m128i xmm1, xmm2;
  uint8_t min = *src2_1;
  uint8_t max = *src3_1;

  /* Initial operations to align the destination pointer */
  for (; ((long)dest & 15) && (n > 0); n--) {
    uint8_t x = *src1++;
    if (x < min)
      x = min;
    if (x > max)
      x = max;
    *dest++ = x;
  }
  xmm1 = _mm_set1_epi8(min);
  xmm2 = _mm_set1_epi8(max);
  for (; n >= 16; n -= 16) {
    __m128i xmm0;
    xmm0 = _mm_loadu_si128((__m128i *)src1);
    xmm0 = _mm_max_epu8(xmm0, xmm1);
    xmm0 = _mm_min_epu8(xmm0, xmm2);
    _mm_store_si128((__m128i *)dest, xmm0);
    dest += 16;
    src1 += 16;
  }
  for (; n > 0; n--) {
    uint8_t x = *src1++;
    if (x < min)
      x = min;
    if (x > max)
      x = max;
    *dest++ = x;
  }
}
OIL_DEFINE_IMPL_FULL (clamp_u8_sse, clamp_u8, OIL_IMPL_FLAG_SSE2);

SSE_FUNCTION static void
clamp_s16_sse (int16_t *dest, int16_t *src1, int n, int16_t *src2_1,
    int16_t *src3_1)
{
  __m128i xmm1, xmm2;
  int16_t min = *src2_1;
  int16_t max = *src3_1;

  /* Initial operations to align the destination pointer */
  for (; ((long)dest & 15) && (n > 0); n--) {
    int16_t x = *src1++;
    if (x < min)
      x = min;
    if (x > max)
      x = max;
    *dest++ = x;
  }
  xmm1 = _mm_set1_epi16(min);
  xmm2 = _mm_set1_epi16(max);
  for (; n >= 8; n -= 8) {
    __m128i xmm0;
    xmm0 = _mm_loadu_si128((__m128i *)src1);
    xmm0 = _mm_max_epi16(xmm0, xmm1);
    xmm0 = _mm_min_epi16(xmm0, xmm2);
    _mm_store_si128((__m128i *)dest, xmm0);
    dest += 8;
    src1 += 8;
  }
  for (; n > 0; n--) {
    int16_t x = *src1++;
    if (x < min)
      x = min;
    if (x > max)
      x = max;
    *dest++ = x;
  }
}
OIL_DEFINE_IMPL_FULL (clamp_s16_sse, clamp_s16, OIL_IMPL_FLAG_SSE2);

SSE_FUNCTION static void
clamp_f32_sse (float *dest, const float *src1, int n, const float *src2_1,
    const float *src3_1)
{
  __m128 xmm1, xmm2;
  float min = *src2_1;
  float max = *src3_1;

  /* Initial operations to align the destination pointer */
  for (; ((long)dest & 15) && (n > 0); n--) {
    float x = *src1++;
    if (x < min)
      x = min;
    if (x > max)
      x = max;
    *dest++ = x;
  }
  xmm1 = _mm_set_ps1(min);
  xmm2 = _mm_set_ps1(max);
  for (; n >= 4; n -= 4) {
    __m128 xmm0;
    xmm0 = _mm_loadu_ps(src1);
    xmm0 = _mm_max_ps(xmm0, xmm1);
    xmm0 = _mm_min_ps(xmm0, xmm2);
    _mm_store_ps(dest, xmm0);
    dest += 4;
    src1 += 4;
  }
  for (; n > 0; n--) {
    float x = *src1++;
    if (x < min)
      x = min;
    if (x > max)
      x = max;
    *dest++ = x;
  }
}
OIL_DEFINE_IMPL_FULL (clamp_f32_sse, clamp_f32, OIL_IMPL_FLAG_SSE);

SSE_FUNCTION static void
clamp_f64_sse (double *dest, const double *src1, int n, const double *src2_1,
    const double *src3_1)
{
  __m128d xmm1, xmm2;
  double min = *src2_1;
  double max = *src3_1;

  /* Initial operations to align the destination pointer */
  for (; ((long)dest & 15) && (n > 0); n--) {
    double x = *src1++;
    if (x < min)
      x = min;
    if (x > max)
      x = max;
    *dest++ = x;
  }
  xmm1 = _mm_set1_pd(min);
  xmm2 = _mm_set1_pd(max);
  for (; n >= 2; n -= 2) {
    __m128d xmm0;
    xmm0 = _mm_loadu_pd(src1);
    xmm0 = _mm_max_pd(xmm0, xmm1);
    xmm0 = _mm_min_pd(xmm0, xmm2);
    _mm_store_pd(dest, xmm0);
    dest += 2;
    src1 += 2;
  }
  for (; n > 0; n--) {
    double x = *src1++;
    if (x < min)
      x = min;
    if (x > max)
      x = max;
    *dest++ = x;
  }
}
OIL_DEFINE_IMPL_FULL (clamp_f64_sse, clamp_f64,
    OIL_IMPL_FLAG_SSE | OIL_IMPL_FLAG_SSE2);

SSE_FUNCTION static void
clamplow_u8_sse (uint8_t *dest, const uint8_t *src1, int n,
    const uint8_t *src2_1)
{
  __m128i xmm1;
  uint8_t min = *src2_1;

  /* Initial operations to align the destination pointer */
  for (; ((long)dest & 15) && (n > 0); n--) {
    uint8_t x = *src1++;
    if (x < min)
      x = min;
    *dest++ = x;
  }
  xmm1 = _mm_set1_epi8(min);
  for (; n >= 16; n -= 16) {
    __m128i xmm0;
    xmm0 = _mm_loadu_si128((__m128i *)src1);
    xmm0 = _mm_max_epu8(xmm0, xmm1);
    _mm_store_si128((__m128i *)dest, xmm0);
    dest += 16;
    src1 += 16;
  }
  for (; n > 0; n--) {
    uint8_t x = *src1++;
    if (x < min)
      x = min;
    *dest++ = x;
  }
}
OIL_DEFINE_IMPL_FULL (clamplow_u8_sse, clamplow_u8, OIL_IMPL_FLAG_SSE2);

SSE_FUNCTION static void
clamplow_s16_sse (int16_t *dest, const int16_t *src1, int n,
    const int16_t *src2_1)
{
  __m128i xmm1;
  int16_t min = *src2_1;

  /* Initial operations to align the destination pointer */
  for (; ((long)dest & 15) && (n > 0); n--) {
    int16_t x = *src1++;
    if (x < min)
      x = min;
    *dest++ = x;
  }
  xmm1 = _mm_set1_epi16(min);
  for (; n >= 8; n -= 8) {
    __m128i xmm0;
    xmm0 = _mm_loadu_si128((__m128i *)src1);
    xmm0 = _mm_max_epi16(xmm0, xmm1);
    _mm_store_si128((__m128i *)dest, xmm0);
    dest += 8;
    src1 += 8;
  }
  for (; n > 0; n--) {
    int16_t x = *src1++;
    if (x < min)
      x = min;
    *dest++ = x;
  }
}
OIL_DEFINE_IMPL_FULL (clamplow_s16_sse, clamplow_s16, OIL_IMPL_FLAG_SSE2);

SSE_FUNCTION static void
clamplow_f32_sse (float *dest, const float *src1, int n, const float *src2_1)
{
  __m128 xmm1;
  float min = *src2_1;

  /* Initial operations to align the destination pointer */
  for (; ((long)dest & 15) && (n > 0); n--) {
    float x = *src1++;
    if (x < min)
      x = min;
    *dest++ = x;
  }
  xmm1 = _mm_set_ps1(min);
  for (; n >= 4; n -= 4) {
    __m128 xmm0;
    xmm0 = _mm_loadu_ps(src1);
    xmm0 = _mm_max_ps(xmm0, xmm1);
    _mm_store_ps(dest, xmm0);
    dest += 4;
    src1 += 4;
  }
  for (; n > 0; n--) {
    float x = *src1++;
    if (x < min)
      x = min;
    *dest++ = x;
  }
}
OIL_DEFINE_IMPL_FULL (clamplow_f32_sse, clamplow_f32, OIL_IMPL_FLAG_SSE);

SSE_FUNCTION static void
clamplow_f64_sse (double *dest, const double *src1, int n, const double *src2_1)
{
  __m128d xmm1;
  double min = *src2_1;

  /* Initial operations to align the destination pointer */
  for (; ((long)dest & 15) && (n > 0); n--) {
    double x = *src1++;
    if (x < min)
      x = min;
    *dest++ = x;
  }
  xmm1 = _mm_set1_pd(min);
  for (; n >= 2; n -= 2) {
    __m128d xmm0;
    xmm0 = _mm_loadu_pd(src1);
    xmm0 = _mm_max_pd(xmm0, xmm1);
    _mm_store_pd(dest, xmm0);
    dest += 2;
    src1 += 2;
  }
  for (; n > 0; n--) {
    double x = *src1++;
    if (x < min)
      x = min;
    *dest++ = x;
  }
}
OIL_DEFINE_IMPL_FULL (clamplow_f64_sse, clamplow_f64,
    OIL_IMPL_FLAG_SSE | OIL_IMPL_FLAG_SSE2);

SSE_FUNCTION static void
clamphigh_u8_sse (uint8_t *dest, const uint8_t *src1, int n,
    const uint8_t *src2_1)
{
  __m128i xmm1;
  uint8_t max = *src2_1;

  /* Initial operations to align the destination pointer */
  for (; ((long)dest & 15) && (n > 0); n--) {
    uint8_t x = *src1++;
    if (x > max)
      x = max;
    *dest++ = x;
  }
  xmm1 = _mm_set1_epi8(max);
  for (; n >= 16; n -= 16) {
    __m128i xmm0;
    xmm0 = _mm_loadu_si128((__m128i *)src1);
    xmm0 = _mm_min_epu8(xmm0, xmm1);
    _mm_store_si128((__m128i *)dest, xmm0);
    dest += 16;
    src1 += 16;
  }
  for (; n > 0; n--) {
    uint8_t x = *src1++;
    if (x > max)
      x = max;
    *dest++ = x;
  }
}
OIL_DEFINE_IMPL_FULL (clamphigh_u8_sse, clamphigh_u8, OIL_IMPL_FLAG_SSE2);

SSE_FUNCTION static void
clamphigh_s16_sse (int16_t *dest, const int16_t *src1, int n,
    const int16_t *src2_1)
{
  __m128i xmm1;
  int16_t max = *src2_1;

  /* Initial operations to align the destination pointer */
  for (; ((long)dest & 15) && (n > 0); n--) {
    int16_t x = *src1++;
    if (x > max)
      x = max;
    *dest++ = x;
  }
  xmm1 = _mm_set1_epi16(max);
  for (; n >= 8; n -= 8) {
    __m128i xmm0;
    xmm0 = _mm_loadu_si128((__m128i *)src1);
    xmm0 = _mm_min_epi16(xmm0, xmm1);
    _mm_store_si128((__m128i *)dest, xmm0);
    dest += 8;
    src1 += 8;
  }
  for (; n > 0; n--) {
    int16_t x = *src1++;
    if (x > max)
      x = max;
    *dest++ = x;
  }
}
OIL_DEFINE_IMPL_FULL (clamphigh_s16_sse, clamphigh_s16, OIL_IMPL_FLAG_SSE2);

SSE_FUNCTION static void
clamphigh_f32_sse (float *dest, const float *src1, int n, const float *src2_1)
{
  __m128 xmm1;
  float max = *src2_1;

  /* Initial operations to align the destination pointer */
  for (; ((long)dest & 15) && (n > 0); n--) {
    float x = *src1++;
    if (x > max)
      x = max;
    *dest++ = x;
  }
  xmm1 = _mm_set_ps1(max);
  for (; n >= 4; n -= 4) {
    __m128 xmm0;
    xmm0 = _mm_loadu_ps(src1);
    xmm0 = _mm_min_ps(xmm0, xmm1);
    _mm_store_ps(dest, xmm0);
    dest += 4;
    src1 += 4;
  }
  for (; n > 0; n--) {
    float x = *src1++;
    if (x > max)
      x = max;
    *dest++ = x;
  }
}
OIL_DEFINE_IMPL_FULL (clamphigh_f32_sse, clamphigh_f32, OIL_IMPL_FLAG_SSE);

SSE_FUNCTION static void
clamphigh_f64_sse (double *dest, const double *src1, int n, const double *src2_1)
{
  __m128d xmm1;
  double max = *src2_1;

  /* Initial operations to align the destination pointer */
  for (; ((long)dest & 15) && (n > 0); n--) {
    double x = *src1++;
    if (x > max)
      x = max;
    *dest++ = x;
  }
  xmm1 = _mm_set1_pd(max);
  for (; n >= 2; n -= 2) {
    __m128d xmm0;
    xmm0 = _mm_loadu_pd(src1);
    xmm0 = _mm_min_pd(xmm0, xmm1);
    _mm_store_pd(dest, xmm0);
    dest += 2;
    src1 += 2;
  }
  for (; n > 0; n--) {
    double x = *src1++;
    if (x > max)
      x = max;
    *dest++ = x;
  }
}
OIL_DEFINE_IMPL_FULL (clamphigh_f64_sse, clamphigh_f64,
    OIL_IMPL_FLAG_SSE | OIL_IMPL_FLAG_SSE2);


#ifdef	__SYMBIAN32__
 
OilFunctionImpl* __oil_function_impl_clamp_u8_sse, clamp_u8() {
		return &_oil_function_impl_clamp_u8_sse, clamp_u8;
}
#endif

#ifdef	__SYMBIAN32__
 
OilFunctionImpl* __oil_function_impl_clamp_s16_sse, clamp_s16() {
		return &_oil_function_impl_clamp_s16_sse, clamp_s16;
}
#endif

#ifdef	__SYMBIAN32__
 
OilFunctionImpl* __oil_function_impl_clamp_f32_sse, clamp_f32() {
		return &_oil_function_impl_clamp_f32_sse, clamp_f32;
}
#endif

#ifdef	__SYMBIAN32__
 
OilFunctionImpl* __oil_function_impl_clamp_f64_sse, clamp_f64() {
		return &_oil_function_impl_clamp_f64_sse, clamp_f64;
}
#endif

#ifdef	__SYMBIAN32__
 
OilFunctionImpl* __oil_function_impl_clamplow_u8_sse, clamplow_u8() {
		return &_oil_function_impl_clamplow_u8_sse, clamplow_u8;
}
#endif

#ifdef	__SYMBIAN32__
 
OilFunctionImpl* __oil_function_impl_clamplow_s16_sse, clamplow_s16() {
		return &_oil_function_impl_clamplow_s16_sse, clamplow_s16;
}
#endif

#ifdef	__SYMBIAN32__
 
OilFunctionImpl* __oil_function_impl_clamplow_f32_sse, clamplow_f32() {
		return &_oil_function_impl_clamplow_f32_sse, clamplow_f32;
}
#endif

#ifdef	__SYMBIAN32__
 
OilFunctionImpl* __oil_function_impl_clamplow_f64_sse, clamplow_f64() {
		return &_oil_function_impl_clamplow_f64_sse, clamplow_f64;
}
#endif

#ifdef	__SYMBIAN32__
 
OilFunctionImpl* __oil_function_impl_clamphigh_u8_sse, clamphigh_u8() {
		return &_oil_function_impl_clamphigh_u8_sse, clamphigh_u8;
}
#endif

#ifdef	__SYMBIAN32__
 
OilFunctionImpl* __oil_function_impl_clamphigh_s16_sse, clamphigh_s16() {
		return &_oil_function_impl_clamphigh_s16_sse, clamphigh_s16;
}
#endif

#ifdef	__SYMBIAN32__
 
OilFunctionImpl* __oil_function_impl_clamphigh_f32_sse, clamphigh_f32() {
		return &_oil_function_impl_clamphigh_f32_sse, clamphigh_f32;
}
#endif

#ifdef	__SYMBIAN32__
 
OilFunctionImpl* __oil_function_impl_clamphigh_f64_sse, clamphigh_f64() {
		return &_oil_function_impl_clamphigh_f64_sse, clamphigh_f64;
}
#endif