/* GStreamer

 * Copyright (C) <2016> Wim Taymans <wim.taymans@gmail.com>

 *

 * This library is free software; you can redistribute it and/or

 * modify it under the terms of the GNU Library General Public

 * License as published by the Free Software Foundation; either

 * version 2 of the License, or (at your option) any later version.

 *

 * This library is distributed in the hope that it will be useful,

 * but WITHOUT ANY WARRANTY; without even the implied warranty of

 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU

 * Library General Public License for more details.

 *

 * You should have received a copy of the GNU Library General Public

 * License along with this library; if not, write to the

 * Free Software Foundation, Inc., 51 Franklin St, Fifth Floor,

 * Boston, MA 02110-1301, USA.

 */

#ifdef HAVE_CONFIG_H

#  include "config.h"

#endif

#include "audio-resampler-x86-sse2.h"

#if defined (HAVE_EMMINTRIN_H) && defined(__SSE2__)

#include <emmintrin.h>

static inline void

inner_product_gint16_full_1_sse2 (gint16 * o, const gint16 * a,

    const gint16 * b, gint len, const gint16 * icoeff, gint bstride)

{

  gint i;

  __m128i sum, t;

  sum = _mm_setzero_si128 ();

  for (i = 0; i < len; i += 16) {

    t = _mm_loadu_si128 ((__m128i *) (a + i));

    sum =

        _mm_add_epi32 (sum, _mm_madd_epi16 (t,

            _mm_load_si128 ((__m128i *) (b + i + 0))));

    t = _mm_loadu_si128 ((__m128i *) (a + i + 8));

    sum =

        _mm_add_epi32 (sum, _mm_madd_epi16 (t,

            _mm_load_si128 ((__m128i *) (b + i + 8))));

  }

  sum = _mm_add_epi32 (sum, _mm_shuffle_epi32 (sum, _MM_SHUFFLE (2, 3, 2, 3)));

  sum = _mm_add_epi32 (sum, _mm_shuffle_epi32 (sum, _MM_SHUFFLE (1, 1, 1, 1)));

  sum = _mm_add_epi32 (sum, _mm_set1_epi32 (1 << (PRECISION_S16 - 1)));

  sum = _mm_srai_epi32 (sum, PRECISION_S16);

  sum = _mm_packs_epi32 (sum, sum);

  *o = _mm_extract_epi16 (sum, 0);

}

static inline void

inner_product_gint16_linear_1_sse2 (gint16 * o, const gint16 * a,

    const gint16 * b, gint len, const gint16 * icoeff, gint bstride)

{

  gint i = 0;

  __m128i sum[2], t;

  __m128i f = _mm_set_epi64x (0, *((gint64 *) icoeff));

  const gint16 *c[2] = { (gint16 *) ((gint8 *) b + 0 * bstride),

    (gint16 *) ((gint8 *) b + 1 * bstride)

  };

  sum[0] = sum[1] = _mm_setzero_si128 ();

  f = _mm_unpacklo_epi16 (f, sum[0]);

  for (; i < len; i += 16) {

    t = _mm_loadu_si128 ((__m128i *) (a + i + 0));

    sum[0] =

        _mm_add_epi32 (sum[0], _mm_madd_epi16 (t,

            _mm_load_si128 ((__m128i *) (c[0] + i + 0))));

    sum[1] =

        _mm_add_epi32 (sum[1], _mm_madd_epi16 (t,

            _mm_load_si128 ((__m128i *) (c[1] + i + 0))));

    t = _mm_loadu_si128 ((__m128i *) (a + i + 8));

    sum[0] =

        _mm_add_epi32 (sum[0], _mm_madd_epi16 (t,

            _mm_load_si128 ((__m128i *) (c[0] + i + 8))));

    sum[1] =

        _mm_add_epi32 (sum[1], _mm_madd_epi16 (t,

            _mm_load_si128 ((__m128i *) (c[1] + i + 8))));

  }

  sum[0] = _mm_srai_epi32 (sum[0], PRECISION_S16);

  sum[1] = _mm_srai_epi32 (sum[1], PRECISION_S16);

  sum[0] =

      _mm_madd_epi16 (sum[0], _mm_shuffle_epi32 (f, _MM_SHUFFLE (0, 0, 0, 0)));

  sum[1] =

      _mm_madd_epi16 (sum[1], _mm_shuffle_epi32 (f, _MM_SHUFFLE (1, 1, 1, 1)));

  sum[0] = _mm_add_epi32 (sum[0], sum[1]);

  sum[0] =

      _mm_add_epi32 (sum[0], _mm_shuffle_epi32 (sum[0], _MM_SHUFFLE (2, 3, 2,

              3)));

  sum[0] =

      _mm_add_epi32 (sum[0], _mm_shuffle_epi32 (sum[0], _MM_SHUFFLE (1, 1, 1,

              1)));

  sum[0] = _mm_add_epi32 (sum[0], _mm_set1_epi32 (1 << (PRECISION_S16 - 1)));

  sum[0] = _mm_srai_epi32 (sum[0], PRECISION_S16);

  sum[0] = _mm_packs_epi32 (sum[0], sum[0]);

  *o = _mm_extract_epi16 (sum[0], 0);

}

static inline void

inner_product_gint16_cubic_1_sse2 (gint16 * o, const gint16 * a,

    const gint16 * b, gint len, const gint16 * icoeff, gint bstride)

{

  gint i = 0;

  __m128i sum[4], t[4];

  __m128i f = _mm_set_epi64x (0, *((long long *) icoeff));

  const gint16 *c[4] = { (gint16 *) ((gint8 *) b + 0 * bstride),

    (gint16 *) ((gint8 *) b + 1 * bstride),

    (gint16 *) ((gint8 *) b + 2 * bstride),

    (gint16 *) ((gint8 *) b + 3 * bstride)

  };

  sum[0] = sum[1] = sum[2] = sum[3] = _mm_setzero_si128 ();

  f = _mm_unpacklo_epi16 (f, sum[0]);

  for (; i < len; i += 8) {

    t[0] = _mm_loadu_si128 ((__m128i *) (a + i));

    sum[0] =

        _mm_add_epi32 (sum[0], _mm_madd_epi16 (t[0],

            _mm_load_si128 ((__m128i *) (c[0] + i))));

    sum[1] =

        _mm_add_epi32 (sum[1], _mm_madd_epi16 (t[0],

            _mm_load_si128 ((__m128i *) (c[1] + i))));

    sum[2] =

        _mm_add_epi32 (sum[2], _mm_madd_epi16 (t[0],

            _mm_load_si128 ((__m128i *) (c[2] + i))));

    sum[3] =

        _mm_add_epi32 (sum[3], _mm_madd_epi16 (t[0],

            _mm_load_si128 ((__m128i *) (c[3] + i))));

  }

  t[0] = _mm_unpacklo_epi32 (sum[0], sum[1]);

  t[1] = _mm_unpacklo_epi32 (sum[2], sum[3]);

  t[2] = _mm_unpackhi_epi32 (sum[0], sum[1]);

  t[3] = _mm_unpackhi_epi32 (sum[2], sum[3]);

  sum[0] =

      _mm_add_epi32 (_mm_unpacklo_epi64 (t[0], t[1]), _mm_unpackhi_epi64 (t[0],

          t[1]));

  sum[2] =

      _mm_add_epi32 (_mm_unpacklo_epi64 (t[2], t[3]), _mm_unpackhi_epi64 (t[2],

          t[3]));

  sum[0] = _mm_add_epi32 (sum[0], sum[2]);

  sum[0] = _mm_srai_epi32 (sum[0], PRECISION_S16);

  sum[0] = _mm_madd_epi16 (sum[0], f);

  sum[0] =

      _mm_add_epi32 (sum[0], _mm_shuffle_epi32 (sum[0], _MM_SHUFFLE (2, 3, 2,

              3)));

  sum[0] =

      _mm_add_epi32 (sum[0], _mm_shuffle_epi32 (sum[0], _MM_SHUFFLE (1, 1, 1,

              1)));

  sum[0] = _mm_add_epi32 (sum[0], _mm_set1_epi32 (1 << (PRECISION_S16 - 1)));

  sum[0] = _mm_srai_epi32 (sum[0], PRECISION_S16);

  sum[0] = _mm_packs_epi32 (sum[0], sum[0]);

  *o = _mm_extract_epi16 (sum[0], 0);

}

static inline void

inner_product_gdouble_full_1_sse2 (gdouble * o, const gdouble * a,

    const gdouble * b, gint len, const gdouble * icoeff, gint bstride)

{

  gint i = 0;

  __m128d sum = _mm_setzero_pd ();

  for (; i < len; i += 8) {

    sum =

        _mm_add_pd (sum, _mm_mul_pd (_mm_loadu_pd (a + i + 0),

            _mm_load_pd (b + i + 0)));

    sum =

        _mm_add_pd (sum, _mm_mul_pd (_mm_loadu_pd (a + i + 2),

            _mm_load_pd (b + i + 2)));

    sum =

        _mm_add_pd (sum, _mm_mul_pd (_mm_loadu_pd (a + i + 4),

            _mm_load_pd (b + i + 4)));

    sum =

        _mm_add_pd (sum, _mm_mul_pd (_mm_loadu_pd (a + i + 6),

            _mm_load_pd (b + i + 6)));

  }

  sum = _mm_add_sd (sum, _mm_unpackhi_pd (sum, sum));

  _mm_store_sd (o, sum);

}

static inline void

inner_product_gdouble_linear_1_sse2 (gdouble * o, const gdouble * a,

    const gdouble * b, gint len, const gdouble * icoeff, gint bstride)

{

  gint i = 0;

  __m128d sum[2], t;

  const gdouble *c[2] = { (gdouble *) ((gint8 *) b + 0 * bstride),

    (gdouble *) ((gint8 *) b + 1 * bstride)

  };

  sum[0] = sum[1] = _mm_setzero_pd ();

  for (; i < len; i += 4) {

    t = _mm_loadu_pd (a + i + 0);

    sum[0] = _mm_add_pd (sum[0], _mm_mul_pd (t, _mm_load_pd (c[0] + i + 0)));

    sum[1] = _mm_add_pd (sum[1], _mm_mul_pd (t, _mm_load_pd (c[1] + i + 0)));

    t = _mm_loadu_pd (a + i + 2);

    sum[0] = _mm_add_pd (sum[0], _mm_mul_pd (t, _mm_load_pd (c[0] + i + 2)));

    sum[1] = _mm_add_pd (sum[1], _mm_mul_pd (t, _mm_load_pd (c[1] + i + 2)));

  }

  sum[0] = _mm_mul_pd (_mm_sub_pd (sum[0], sum[1]), _mm_load1_pd (icoeff));

  sum[0] = _mm_add_pd (sum[0], sum[1]);

  sum[0] = _mm_add_sd (sum[0], _mm_unpackhi_pd (sum[0], sum[0]));

  _mm_store_sd (o, sum[0]);

}

static inline void

inner_product_gdouble_cubic_1_sse2 (gdouble * o, const gdouble * a,

    const gdouble * b, gint len, const gdouble * icoeff, gint bstride)

{

  gint i;

  __m128d f[2], sum[4], t;

  const gdouble *c[4] = { (gdouble *) ((gint8 *) b + 0 * bstride),

    (gdouble *) ((gint8 *) b + 1 * bstride),

    (gdouble *) ((gint8 *) b + 2 * bstride),

    (gdouble *) ((gint8 *) b + 3 * bstride)

  };

  f[0] = _mm_loadu_pd (icoeff + 0);

  f[1] = _mm_loadu_pd (icoeff + 2);

  sum[0] = sum[1] = sum[2] = sum[3] = _mm_setzero_pd ();

  for (i = 0; i < len; i += 2) {

    t = _mm_loadu_pd (a + i + 0);

    sum[0] = _mm_add_pd (sum[0], _mm_mul_pd (t, _mm_load_pd (c[0] + i)));

    sum[1] = _mm_add_pd (sum[1], _mm_mul_pd (t, _mm_load_pd (c[1] + i)));

    sum[2] = _mm_add_pd (sum[2], _mm_mul_pd (t, _mm_load_pd (c[2] + i)));

    sum[3] = _mm_add_pd (sum[3], _mm_mul_pd (t, _mm_load_pd (c[3] + i)));

  }

  sum[0] =

      _mm_mul_pd (sum[0], _mm_shuffle_pd (f[0], f[0], _MM_SHUFFLE2 (0, 0)));

  sum[1] =

      _mm_mul_pd (sum[1], _mm_shuffle_pd (f[0], f[0], _MM_SHUFFLE2 (1, 1)));

  sum[2] =

      _mm_mul_pd (sum[2], _mm_shuffle_pd (f[1], f[1], _MM_SHUFFLE2 (0, 0)));

  sum[3] =

      _mm_mul_pd (sum[3], _mm_shuffle_pd (f[1], f[1], _MM_SHUFFLE2 (1, 1)));

  sum[0] = _mm_add_pd (sum[0], sum[1]);

  sum[2] = _mm_add_pd (sum[2], sum[3]);

  sum[0] = _mm_add_pd (sum[0], sum[2]);

  sum[0] = _mm_add_sd (sum[0], _mm_unpackhi_pd (sum[0], sum[0]));

  _mm_store_sd (o, sum[0]);

}

MAKE_RESAMPLE_FUNC (gint16, full, 1, sse2);

MAKE_RESAMPLE_FUNC (gint16, linear, 1, sse2);

MAKE_RESAMPLE_FUNC (gint16, cubic, 1, sse2);

MAKE_RESAMPLE_FUNC (gdouble, full, 1, sse2);

MAKE_RESAMPLE_FUNC (gdouble, linear, 1, sse2);

MAKE_RESAMPLE_FUNC (gdouble, cubic, 1, sse2);

void

interpolate_gint16_linear_sse2 (gpointer op, const gpointer ap,

    gint len, const gpointer icp, gint astride)

{

  gint i = 0;

  gint16 *o = op, *a = ap, *ic = icp;

  __m128i ta, tb, t1, t2;

  __m128i f = _mm_set_epi64x (0, *((gint64 *) ic));

  const gint16 *c[2] = { (gint16 *) ((gint8 *) a + 0 * astride),

    (gint16 *) ((gint8 *) a + 1 * astride)

  };

  f = _mm_unpacklo_epi32 (f, f);

  f = _mm_unpacklo_epi64 (f, f);

  for (; i < len; i += 8) {

    ta = _mm_load_si128 ((__m128i *) (c[0] + i));

    tb = _mm_load_si128 ((__m128i *) (c[1] + i));

    t1 = _mm_madd_epi16 (_mm_unpacklo_epi16 (ta, tb), f);

    t2 = _mm_madd_epi16 (_mm_unpackhi_epi16 (ta, tb), f);

    t1 = _mm_add_epi32 (t1, _mm_set1_epi32 (1 << (PRECISION_S16 - 1)));

    t2 = _mm_add_epi32 (t2, _mm_set1_epi32 (1 << (PRECISION_S16 - 1)));

    t1 = _mm_srai_epi32 (t1, PRECISION_S16);

    t2 = _mm_srai_epi32 (t2, PRECISION_S16);

    t1 = _mm_packs_epi32 (t1, t2);

    _mm_store_si128 ((__m128i *) (o + i), t1);

  }

}

void

interpolate_gint16_cubic_sse2 (gpointer op, const gpointer ap,

    gint len, const gpointer icp, gint astride)

{

  gint i = 0;

  gint16 *o = op, *a = ap, *ic = icp;

  __m128i ta, tb, tl1, tl2, th1, th2;

  __m128i f[2];

  const gint16 *c[4] = { (gint16 *) ((gint8 *) a + 0 * astride),

    (gint16 *) ((gint8 *) a + 1 * astride),

    (gint16 *) ((gint8 *) a + 2 * astride),

    (gint16 *) ((gint8 *) a + 3 * astride)

  };

  f[0] = _mm_set_epi16 (ic[1], ic[0], ic[1], ic[0], ic[1], ic[0], ic[1], ic[0]);

  f[1] = _mm_set_epi16 (ic[3], ic[2], ic[3], ic[2], ic[3], ic[2], ic[3], ic[2]);

  for (; i < len; i += 8) {

    ta = _mm_load_si128 ((__m128i *) (c[0] + i));

    tb = _mm_load_si128 ((__m128i *) (c[1] + i));

    tl1 = _mm_madd_epi16 (_mm_unpacklo_epi16 (ta, tb), f[0]);

    th1 = _mm_madd_epi16 (_mm_unpackhi_epi16 (ta, tb), f[0]);

    ta = _mm_load_si128 ((__m128i *) (c[2] + i));

    tb = _mm_load_si128 ((__m128i *) (c[3] + i));

    tl2 = _mm_madd_epi16 (_mm_unpacklo_epi16 (ta, tb), f[1]);

    th2 = _mm_madd_epi16 (_mm_unpackhi_epi16 (ta, tb), f[1]);

    tl1 = _mm_add_epi32 (tl1, tl2);

    th1 = _mm_add_epi32 (th1, th2);

    tl1 = _mm_add_epi32 (tl1, _mm_set1_epi32 (1 << (PRECISION_S16 - 1)));

    th1 = _mm_add_epi32 (th1, _mm_set1_epi32 (1 << (PRECISION_S16 - 1)));

    tl1 = _mm_srai_epi32 (tl1, PRECISION_S16);

    th1 = _mm_srai_epi32 (th1, PRECISION_S16);

    tl1 = _mm_packs_epi32 (tl1, th1);

    _mm_store_si128 ((__m128i *) (o + i), tl1);

  }

}

void

interpolate_gdouble_linear_sse2 (gpointer op, const gpointer ap,

    gint len, const gpointer icp, gint astride)

{

  gint i;

  gdouble *o = op, *a = ap, *ic = icp;

  __m128d f[2], t1, t2;

  const gdouble *c[2] = { (gdouble *) ((gint8 *) a + 0 * astride),

    (gdouble *) ((gint8 *) a + 1 * astride)

  };

  f[0] = _mm_load1_pd (ic + 0);

  f[1] = _mm_load1_pd (ic + 1);

  for (i = 0; i < len; i += 4) {

    t1 = _mm_mul_pd (_mm_load_pd (c[0] + i + 0), f[0]);

    t2 = _mm_mul_pd (_mm_load_pd (c[1] + i + 0), f[1]);

    _mm_store_pd (o + i + 0, _mm_add_pd (t1, t2));

    t1 = _mm_mul_pd (_mm_load_pd (c[0] + i + 2), f[0]);

    t2 = _mm_mul_pd (_mm_load_pd (c[1] + i + 2), f[1]);

    _mm_store_pd (o + i + 2, _mm_add_pd (t1, t2));

  }

}

void

interpolate_gdouble_cubic_sse2 (gpointer op, const gpointer ap,

    gint len, const gpointer icp, gint astride)

{

  gint i;

  gdouble *o = op, *a = ap, *ic = icp;

  __m128d f[4], t[4];

  const gdouble *c[4] = { (gdouble *) ((gint8 *) a + 0 * astride),

    (gdouble *) ((gint8 *) a + 1 * astride),

    (gdouble *) ((gint8 *) a + 2 * astride),

    (gdouble *) ((gint8 *) a + 3 * astride)

  };

  f[0] = _mm_load1_pd (ic + 0);

  f[1] = _mm_load1_pd (ic + 1);

  f[2] = _mm_load1_pd (ic + 2);

  f[3] = _mm_load1_pd (ic + 3);

  for (i = 0; i < len; i += 2) {

    t[0] = _mm_mul_pd (_mm_load_pd (c[0] + i + 0), f[0]);

    t[1] = _mm_mul_pd (_mm_load_pd (c[1] + i + 0), f[1]);

    t[2] = _mm_mul_pd (_mm_load_pd (c[2] + i + 0), f[2]);

    t[3] = _mm_mul_pd (_mm_load_pd (c[3] + i + 0), f[3]);

    t[0] = _mm_add_pd (t[0], t[1]);

    t[2] = _mm_add_pd (t[2], t[3]);

    _mm_store_pd (o + i + 0, _mm_add_pd (t[0], t[2]));

  }

}

#endif