cannam@154: /* Copyright (c) 2007-2008 CSIRO
cannam@154:    Copyright (c) 2007-2009 Xiph.Org Foundation
cannam@154:    Copyright (c) 2007-2016 Jean-Marc Valin */
cannam@154: /*
cannam@154:    Redistribution and use in source and binary forms, with or without
cannam@154:    modification, are permitted provided that the following conditions
cannam@154:    are met:
cannam@154: 
cannam@154:    - Redistributions of source code must retain the above copyright
cannam@154:    notice, this list of conditions and the following disclaimer.
cannam@154: 
cannam@154:    - Redistributions in binary form must reproduce the above copyright
cannam@154:    notice, this list of conditions and the following disclaimer in the
cannam@154:    documentation and/or other materials provided with the distribution.
cannam@154: 
cannam@154:    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
cannam@154:    ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
cannam@154:    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
cannam@154:    A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER
cannam@154:    OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
cannam@154:    EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
cannam@154:    PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
cannam@154:    PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
cannam@154:    LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
cannam@154:    NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
cannam@154:    SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
cannam@154: */
cannam@154: 
cannam@154: #ifdef HAVE_CONFIG_H
cannam@154: #include "config.h"
cannam@154: #endif
cannam@154: 
cannam@154: #include <xmmintrin.h>
cannam@154: #include <emmintrin.h>
cannam@154: #include "celt_lpc.h"
cannam@154: #include "stack_alloc.h"
cannam@154: #include "mathops.h"
cannam@154: #include "vq.h"
cannam@154: #include "x86cpu.h"
cannam@154: 
cannam@154: 
cannam@154: #ifndef FIXED_POINT
cannam@154: 
cannam@154: opus_val16 op_pvq_search_sse2(celt_norm *_X, int *iy, int K, int N, int arch)
cannam@154: {
cannam@154:    int i, j;
cannam@154:    int pulsesLeft;
cannam@154:    float xy, yy;
cannam@154:    VARDECL(celt_norm, y);
cannam@154:    VARDECL(celt_norm, X);
cannam@154:    VARDECL(float, signy);
cannam@154:    __m128 signmask;
cannam@154:    __m128 sums;
cannam@154:    __m128i fours;
cannam@154:    SAVE_STACK;
cannam@154: 
cannam@154:    (void)arch;
cannam@154:    /* All bits set to zero, except for the sign bit. */
cannam@154:    signmask = _mm_set_ps1(-0.f);
cannam@154:    fours = _mm_set_epi32(4, 4, 4, 4);
cannam@154:    ALLOC(y, N+3, celt_norm);
cannam@154:    ALLOC(X, N+3, celt_norm);
cannam@154:    ALLOC(signy, N+3, float);
cannam@154: 
cannam@154:    OPUS_COPY(X, _X, N);
cannam@154:    X[N] = X[N+1] = X[N+2] = 0;
cannam@154:    sums = _mm_setzero_ps();
cannam@154:    for (j=0;j<N;j+=4)
cannam@154:    {
cannam@154:       __m128 x4, s4;
cannam@154:       x4 = _mm_loadu_ps(&X[j]);
cannam@154:       s4 = _mm_cmplt_ps(x4, _mm_setzero_ps());
cannam@154:       /* Get rid of the sign */
cannam@154:       x4 = _mm_andnot_ps(signmask, x4);
cannam@154:       sums = _mm_add_ps(sums, x4);
cannam@154:       /* Clear y and iy in case we don't do the projection. */
cannam@154:       _mm_storeu_ps(&y[j], _mm_setzero_ps());
cannam@154:       _mm_storeu_si128((__m128i*)&iy[j], _mm_setzero_si128());
cannam@154:       _mm_storeu_ps(&X[j], x4);
cannam@154:       _mm_storeu_ps(&signy[j], s4);
cannam@154:    }
cannam@154:    sums = _mm_add_ps(sums, _mm_shuffle_ps(sums, sums, _MM_SHUFFLE(1, 0, 3, 2)));
cannam@154:    sums = _mm_add_ps(sums, _mm_shuffle_ps(sums, sums, _MM_SHUFFLE(2, 3, 0, 1)));
cannam@154: 
cannam@154:    xy = yy = 0;
cannam@154: 
cannam@154:    pulsesLeft = K;
cannam@154: 
cannam@154:    /* Do a pre-search by projecting on the pyramid */
cannam@154:    if (K > (N>>1))
cannam@154:    {
cannam@154:       __m128i pulses_sum;
cannam@154:       __m128 yy4, xy4;
cannam@154:       __m128 rcp4;
cannam@154:       opus_val32 sum = _mm_cvtss_f32(sums);
cannam@154:       /* If X is too small, just replace it with a pulse at 0 */
cannam@154:       /* Prevents infinities and NaNs from causing too many pulses
cannam@154:          to be allocated. 64 is an approximation of infinity here. */
cannam@154:       if (!(sum > EPSILON && sum < 64))
cannam@154:       {
cannam@154:          X[0] = QCONST16(1.f,14);
cannam@154:          j=1; do
cannam@154:             X[j]=0;
cannam@154:          while (++j<N);
cannam@154:          sums = _mm_set_ps1(1.f);
cannam@154:       }
cannam@154:       /* Using K+e with e < 1 guarantees we cannot get more than K pulses. */
cannam@154:       rcp4 = _mm_mul_ps(_mm_set_ps1((float)(K+.8)), _mm_rcp_ps(sums));
cannam@154:       xy4 = yy4 = _mm_setzero_ps();
cannam@154:       pulses_sum = _mm_setzero_si128();
cannam@154:       for (j=0;j<N;j+=4)
cannam@154:       {
cannam@154:          __m128 rx4, x4, y4;
cannam@154:          __m128i iy4;
cannam@154:          x4 = _mm_loadu_ps(&X[j]);
cannam@154:          rx4 = _mm_mul_ps(x4, rcp4);
cannam@154:          iy4 = _mm_cvttps_epi32(rx4);
cannam@154:          pulses_sum = _mm_add_epi32(pulses_sum, iy4);
cannam@154:          _mm_storeu_si128((__m128i*)&iy[j], iy4);
cannam@154:          y4 = _mm_cvtepi32_ps(iy4);
cannam@154:          xy4 = _mm_add_ps(xy4, _mm_mul_ps(x4, y4));
cannam@154:          yy4 = _mm_add_ps(yy4, _mm_mul_ps(y4, y4));
cannam@154:          /* double the y[] vector so we don't have to do it in the search loop. */
cannam@154:          _mm_storeu_ps(&y[j], _mm_add_ps(y4, y4));
cannam@154:       }
cannam@154:       pulses_sum = _mm_add_epi32(pulses_sum, _mm_shuffle_epi32(pulses_sum, _MM_SHUFFLE(1, 0, 3, 2)));
cannam@154:       pulses_sum = _mm_add_epi32(pulses_sum, _mm_shuffle_epi32(pulses_sum, _MM_SHUFFLE(2, 3, 0, 1)));
cannam@154:       pulsesLeft -= _mm_cvtsi128_si32(pulses_sum);
cannam@154:       xy4 = _mm_add_ps(xy4, _mm_shuffle_ps(xy4, xy4, _MM_SHUFFLE(1, 0, 3, 2)));
cannam@154:       xy4 = _mm_add_ps(xy4, _mm_shuffle_ps(xy4, xy4, _MM_SHUFFLE(2, 3, 0, 1)));
cannam@154:       xy = _mm_cvtss_f32(xy4);
cannam@154:       yy4 = _mm_add_ps(yy4, _mm_shuffle_ps(yy4, yy4, _MM_SHUFFLE(1, 0, 3, 2)));
cannam@154:       yy4 = _mm_add_ps(yy4, _mm_shuffle_ps(yy4, yy4, _MM_SHUFFLE(2, 3, 0, 1)));
cannam@154:       yy = _mm_cvtss_f32(yy4);
cannam@154:    }
cannam@154:    X[N] = X[N+1] = X[N+2] = -100;
cannam@154:    y[N] = y[N+1] = y[N+2] = 100;
cannam@154:    celt_sig_assert(pulsesLeft>=0);
cannam@154: 
cannam@154:    /* This should never happen, but just in case it does (e.g. on silence)
cannam@154:       we fill the first bin with pulses. */
cannam@154:    if (pulsesLeft > N+3)
cannam@154:    {
cannam@154:       opus_val16 tmp = (opus_val16)pulsesLeft;
cannam@154:       yy = MAC16_16(yy, tmp, tmp);
cannam@154:       yy = MAC16_16(yy, tmp, y[0]);
cannam@154:       iy[0] += pulsesLeft;
cannam@154:       pulsesLeft=0;
cannam@154:    }
cannam@154: 
cannam@154:    for (i=0;i<pulsesLeft;i++)
cannam@154:    {
cannam@154:       int best_id;
cannam@154:       __m128 xy4, yy4;
cannam@154:       __m128 max, max2;
cannam@154:       __m128i count;
cannam@154:       __m128i pos;
cannam@154:       /* The squared magnitude term gets added anyway, so we might as well
cannam@154:          add it outside the loop */
cannam@154:       yy = ADD16(yy, 1);
cannam@154:       xy4 = _mm_load1_ps(&xy);
cannam@154:       yy4 = _mm_load1_ps(&yy);
cannam@154:       max = _mm_setzero_ps();
cannam@154:       pos = _mm_setzero_si128();
cannam@154:       count = _mm_set_epi32(3, 2, 1, 0);
cannam@154:       for (j=0;j<N;j+=4)
cannam@154:       {
cannam@154:          __m128 x4, y4, r4;
cannam@154:          x4 = _mm_loadu_ps(&X[j]);
cannam@154:          y4 = _mm_loadu_ps(&y[j]);
cannam@154:          x4 = _mm_add_ps(x4, xy4);
cannam@154:          y4 = _mm_add_ps(y4, yy4);
cannam@154:          y4 = _mm_rsqrt_ps(y4);
cannam@154:          r4 = _mm_mul_ps(x4, y4);
cannam@154:          /* Update the index of the max. */
cannam@154:          pos = _mm_max_epi16(pos, _mm_and_si128(count, _mm_castps_si128(_mm_cmpgt_ps(r4, max))));
cannam@154:          /* Update the max. */
cannam@154:          max = _mm_max_ps(max, r4);
cannam@154:          /* Update the indices (+4) */
cannam@154:          count = _mm_add_epi32(count, fours);
cannam@154:       }
cannam@154:       /* Horizontal max */
cannam@154:       max2 = _mm_max_ps(max, _mm_shuffle_ps(max, max, _MM_SHUFFLE(1, 0, 3, 2)));
cannam@154:       max2 = _mm_max_ps(max2, _mm_shuffle_ps(max2, max2, _MM_SHUFFLE(2, 3, 0, 1)));
cannam@154:       /* Now that max2 contains the max at all positions, look at which value(s) of the
cannam@154:          partial max is equal to the global max. */
cannam@154:       pos = _mm_and_si128(pos, _mm_castps_si128(_mm_cmpeq_ps(max, max2)));
cannam@154:       pos = _mm_max_epi16(pos, _mm_unpackhi_epi64(pos, pos));
cannam@154:       pos = _mm_max_epi16(pos, _mm_shufflelo_epi16(pos, _MM_SHUFFLE(1, 0, 3, 2)));
cannam@154:       best_id = _mm_cvtsi128_si32(pos);
cannam@154: 
cannam@154:       /* Updating the sums of the new pulse(s) */
cannam@154:       xy = ADD32(xy, EXTEND32(X[best_id]));
cannam@154:       /* We're multiplying y[j] by two so we don't have to do it here */
cannam@154:       yy = ADD16(yy, y[best_id]);
cannam@154: 
cannam@154:       /* Only now that we've made the final choice, update y/iy */
cannam@154:       /* Multiplying y[j] by 2 so we don't have to do it everywhere else */
cannam@154:       y[best_id] += 2;
cannam@154:       iy[best_id]++;
cannam@154:    }
cannam@154: 
cannam@154:    /* Put the original sign back */
cannam@154:    for (j=0;j<N;j+=4)
cannam@154:    {
cannam@154:       __m128i y4;
cannam@154:       __m128i s4;
cannam@154:       y4 = _mm_loadu_si128((__m128i*)&iy[j]);
cannam@154:       s4 = _mm_castps_si128(_mm_loadu_ps(&signy[j]));
cannam@154:       y4 = _mm_xor_si128(_mm_add_epi32(y4, s4), s4);
cannam@154:       _mm_storeu_si128((__m128i*)&iy[j], y4);
cannam@154:    }
cannam@154:    RESTORE_STACK;
cannam@154:    return yy;
cannam@154: }
cannam@154: 
cannam@154: #endif