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