218 lines
7.5 KiB
C
218 lines
7.5 KiB
C
|
/* Copyright (c) 2007-2008 CSIRO
|
||
|
|
Copyright (c) 2007-2009 Xiph.Org Foundation
|
||
|
|
Copyright (c) 2007-2016 Jean-Marc Valin */
|
||
|
|
/*
|
||
|
|
Redistribution and use in source and binary forms, with or without
|
||
|
|
modification, are permitted provided that the following conditions
|
||
|
|
are met:
|
||
|
|
|
||
|
|
- Redistributions of source code must retain the above copyright
|
||
|
|
notice, this list of conditions and the following disclaimer.
|
||
|
|
|
||
|
|
- 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 COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||
|
|
``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 COPYRIGHT OWNER
|
||
|
|
OR CONTRIBUTORS 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.
|
||
|
|
*/
|
||
|
|
|
||
|
|
#ifdef HAVE_CONFIG_H
|
||
|
|
#include "config.h"
|
||
|
|
#endif
|
||
|
|
|
||
|
|
#include <xmmintrin.h>
|
||
|
|
#include <emmintrin.h>
|
||
|
|
#include "celt_lpc.h"
|
||
|
|
#include "stack_alloc.h"
|
||
|
|
#include "mathops.h"
|
||
|
|
#include "vq.h"
|
||
|
|
#include "x86cpu.h"
|
||
|
|
|
||
|
|
|
||
|
|
#ifndef FIXED_POINT
|
||
|
|
|
||
|
|
opus_val16 op_pvq_search_sse2(celt_norm *_X, int *iy, int K, int N, int arch)
|
||
|
|
{
|
||
|
|
int i, j;
|
||
|
|
int pulsesLeft;
|
||
|
|
float xy, yy;
|
||
|
|
VARDECL(celt_norm, y);
|
||
|
|
VARDECL(celt_norm, X);
|
||
|
|
VARDECL(float, signy);
|
||
|
|
__m128 signmask;
|
||
|
|
__m128 sums;
|
||
|
|
__m128i fours;
|
||
|
|
SAVE_STACK;
|
||
|
|
|
||
|
|
(void)arch;
|
||
|
|
/* All bits set to zero, except for the sign bit. */
|
||
|
|
signmask = _mm_set_ps1(-0.f);
|
||
|
|
fours = _mm_set_epi32(4, 4, 4, 4);
|
||
|
|
ALLOC(y, N+3, celt_norm);
|
||
|
|
ALLOC(X, N+3, celt_norm);
|
||
|
|
ALLOC(signy, N+3, float);
|
||
|
|
|
||
|
|
OPUS_COPY(X, _X, N);
|
||
|
|
X[N] = X[N+1] = X[N+2] = 0;
|
||
|
|
sums = _mm_setzero_ps();
|
||
|
|
for (j=0;j<N;j+=4)
|
||
|
|
{
|
||
|
|
__m128 x4, s4;
|
||
|
|
x4 = _mm_loadu_ps(&X[j]);
|
||
|
|
s4 = _mm_cmplt_ps(x4, _mm_setzero_ps());
|
||
|
|
/* Get rid of the sign */
|
||
|
|
x4 = _mm_andnot_ps(signmask, x4);
|
||
|
|
sums = _mm_add_ps(sums, x4);
|
||
|
|
/* Clear y and iy in case we don't do the projection. */
|
||
|
|
_mm_storeu_ps(&y[j], _mm_setzero_ps());
|
||
|
|
_mm_storeu_si128((__m128i*)&iy[j], _mm_setzero_si128());
|
||
|
|
_mm_storeu_ps(&X[j], x4);
|
||
|
|
_mm_storeu_ps(&signy[j], s4);
|
||
|
|
}
|
||
|
|
sums = _mm_add_ps(sums, _mm_shuffle_ps(sums, sums, _MM_SHUFFLE(1, 0, 3, 2)));
|
||
|
|
sums = _mm_add_ps(sums, _mm_shuffle_ps(sums, sums, _MM_SHUFFLE(2, 3, 0, 1)));
|
||
|
|
|
||
|
|
xy = yy = 0;
|
||
|
|
|
||
|
|
pulsesLeft = K;
|
||
|
|
|
||
|
|
/* Do a pre-search by projecting on the pyramid */
|
||
|
|
if (K > (N>>1))
|
||
|
|
{
|
||
|
|
__m128i pulses_sum;
|
||
|
|
__m128 yy4, xy4;
|
||
|
|
__m128 rcp4;
|
||
|
|
opus_val32 sum = _mm_cvtss_f32(sums);
|
||
|
|
/* If X is too small, just replace it with a pulse at 0 */
|
||
|
|
/* Prevents infinities and NaNs from causing too many pulses
|
||
|
|
to be allocated. 64 is an approximation of infinity here. */
|
||
|
|
if (!(sum > EPSILON && sum < 64))
|
||
|
|
{
|
||
|
|
X[0] = QCONST16(1.f,14);
|
||
|
|
j=1; do
|
||
|
|
X[j]=0;
|
||
|
|
while (++j<N);
|
||
|
|
sums = _mm_set_ps1(1.f);
|
||
|
|
}
|
||
|
|
/* Using K+e with e < 1 guarantees we cannot get more than K pulses. */
|
||
|
|
rcp4 = _mm_mul_ps(_mm_set_ps1((float)(K+.8)), _mm_rcp_ps(sums));
|
||
|
|
xy4 = yy4 = _mm_setzero_ps();
|
||
|
|
pulses_sum = _mm_setzero_si128();
|
||
|
|
for (j=0;j<N;j+=4)
|
||
|
|
{
|
||
|
|
__m128 rx4, x4, y4;
|
||
|
|
__m128i iy4;
|
||
|
|
x4 = _mm_loadu_ps(&X[j]);
|
||
|
|
rx4 = _mm_mul_ps(x4, rcp4);
|
||
|
|
iy4 = _mm_cvttps_epi32(rx4);
|
||
|
|
pulses_sum = _mm_add_epi32(pulses_sum, iy4);
|
||
|
|
_mm_storeu_si128((__m128i*)&iy[j], iy4);
|
||
|
|
y4 = _mm_cvtepi32_ps(iy4);
|
||
|
|
xy4 = _mm_add_ps(xy4, _mm_mul_ps(x4, y4));
|
||
|
|
yy4 = _mm_add_ps(yy4, _mm_mul_ps(y4, y4));
|
||
|
|
/* double the y[] vector so we don't have to do it in the search loop. */
|
||
|
|
_mm_storeu_ps(&y[j], _mm_add_ps(y4, y4));
|
||
|
|
}
|
||
|
|
pulses_sum = _mm_add_epi32(pulses_sum, _mm_shuffle_epi32(pulses_sum, _MM_SHUFFLE(1, 0, 3, 2)));
|
||
|
|
pulses_sum = _mm_add_epi32(pulses_sum, _mm_shuffle_epi32(pulses_sum, _MM_SHUFFLE(2, 3, 0, 1)));
|
||
|
|
pulsesLeft -= _mm_cvtsi128_si32(pulses_sum);
|
||
|
|
xy4 = _mm_add_ps(xy4, _mm_shuffle_ps(xy4, xy4, _MM_SHUFFLE(1, 0, 3, 2)));
|
||
|
|
xy4 = _mm_add_ps(xy4, _mm_shuffle_ps(xy4, xy4, _MM_SHUFFLE(2, 3, 0, 1)));
|
||
|
|
xy = _mm_cvtss_f32(xy4);
|
||
|
|
yy4 = _mm_add_ps(yy4, _mm_shuffle_ps(yy4, yy4, _MM_SHUFFLE(1, 0, 3, 2)));
|
||
|
|
yy4 = _mm_add_ps(yy4, _mm_shuffle_ps(yy4, yy4, _MM_SHUFFLE(2, 3, 0, 1)));
|
||
|
|
yy = _mm_cvtss_f32(yy4);
|
||
|
|
}
|
||
|
|
X[N] = X[N+1] = X[N+2] = -100;
|
||
|
|
y[N] = y[N+1] = y[N+2] = 100;
|
||
|
|
celt_sig_assert(pulsesLeft>=0);
|
||
|
|
|
||
|
|
/* This should never happen, but just in case it does (e.g. on silence)
|
||
|
|
we fill the first bin with pulses. */
|
||
|
|
if (pulsesLeft > N+3)
|
||
|
|
{
|
||
|
|
opus_val16 tmp = (opus_val16)pulsesLeft;
|
||
|
|
yy = MAC16_16(yy, tmp, tmp);
|
||
|
|
yy = MAC16_16(yy, tmp, y[0]);
|
||
|
|
iy[0] += pulsesLeft;
|
||
|
|
pulsesLeft=0;
|
||
|
|
}
|
||
|
|
|
||
|
|
for (i=0;i<pulsesLeft;i++)
|
||
|
|
{
|
||
|
|
int best_id;
|
||
|
|
__m128 xy4, yy4;
|
||
|
|
__m128 max, max2;
|
||
|
|
__m128i count;
|
||
|
|
__m128i pos;
|
||
|
|
/* The squared magnitude term gets added anyway, so we might as well
|
||
|
|
add it outside the loop */
|
||
|
|
yy = ADD16(yy, 1);
|
||
|
|
xy4 = _mm_load1_ps(&xy);
|
||
|
|
yy4 = _mm_load1_ps(&yy);
|
||
|
|
max = _mm_setzero_ps();
|
||
|
|
pos = _mm_setzero_si128();
|
||
|
|
count = _mm_set_epi32(3, 2, 1, 0);
|
||
|
|
for (j=0;j<N;j+=4)
|
||
|
|
{
|
||
|
|
__m128 x4, y4, r4;
|
||
|
|
x4 = _mm_loadu_ps(&X[j]);
|
||
|
|
y4 = _mm_loadu_ps(&y[j]);
|
||
|
|
x4 = _mm_add_ps(x4, xy4);
|
||
|
|
y4 = _mm_add_ps(y4, yy4);
|
||
|
|
y4 = _mm_rsqrt_ps(y4);
|
||
|
|
r4 = _mm_mul_ps(x4, y4);
|
||
|
|
/* Update the index of the max. */
|
||
|
|
pos = _mm_max_epi16(pos, _mm_and_si128(count, _mm_castps_si128(_mm_cmpgt_ps(r4, max))));
|
||
|
|
/* Update the max. */
|
||
|
|
max = _mm_max_ps(max, r4);
|
||
|
|
/* Update the indices (+4) */
|
||
|
|
count = _mm_add_epi32(count, fours);
|
||
|
|
}
|
||
|
|
/* Horizontal max */
|
||
|
|
max2 = _mm_max_ps(max, _mm_shuffle_ps(max, max, _MM_SHUFFLE(1, 0, 3, 2)));
|
||
|
|
max2 = _mm_max_ps(max2, _mm_shuffle_ps(max2, max2, _MM_SHUFFLE(2, 3, 0, 1)));
|
||
|
|
/* Now that max2 contains the max at all positions, look at which value(s) of the
|
||
|
|
partial max is equal to the global max. */
|
||
|
|
pos = _mm_and_si128(pos, _mm_castps_si128(_mm_cmpeq_ps(max, max2)));
|
||
|
|
pos = _mm_max_epi16(pos, _mm_unpackhi_epi64(pos, pos));
|
||
|
|
pos = _mm_max_epi16(pos, _mm_shufflelo_epi16(pos, _MM_SHUFFLE(1, 0, 3, 2)));
|
||
|
|
best_id = _mm_cvtsi128_si32(pos);
|
||
|
|
|
||
|
|
/* Updating the sums of the new pulse(s) */
|
||
|
|
xy = ADD32(xy, EXTEND32(X[best_id]));
|
||
|
|
/* We're multiplying y[j] by two so we don't have to do it here */
|
||
|
|
yy = ADD16(yy, y[best_id]);
|
||
|
|
|
||
|
|
/* Only now that we've made the final choice, update y/iy */
|
||
|
|
/* Multiplying y[j] by 2 so we don't have to do it everywhere else */
|
||
|
|
y[best_id] += 2;
|
||
|
|
iy[best_id]++;
|
||
|
|
}
|
||
|
|
|
||
|
|
/* Put the original sign back */
|
||
|
|
for (j=0;j<N;j+=4)
|
||
|
|
{
|
||
|
|
__m128i y4;
|
||
|
|
__m128i s4;
|
||
|
|
y4 = _mm_loadu_si128((__m128i*)&iy[j]);
|
||
|
|
s4 = _mm_castps_si128(_mm_loadu_ps(&signy[j]));
|
||
|
|
y4 = _mm_xor_si128(_mm_add_epi32(y4, s4), s4);
|
||
|
|
_mm_storeu_si128((__m128i*)&iy[j], y4);
|
||
|
|
}
|
||
|
|
RESTORE_STACK;
|
||
|
|
return yy;
|
||
|
|
}
|
||
|
|
|
||
|
|
#endif
|