TeaSpeak-Client/native/codec/libraries/speex/tmv/ltp_tm.h

480 lines
11 KiB
C
Raw Normal View History

2019-10-25 19:51:40 -04:00
/* Copyright (C) 2007 Hong Zhiqian */
/**
@file ltp_tm.h
@author Hong Zhiqian
@brief Various compatibility routines for Speex (TriMedia version)
*/
/*
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.
- Neither the name of the Xiph.org Foundation nor the names of its
contributors may be used to endorse or promote products derived from
this software without specific prior written permission.
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 FOUNDATION 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.
*/
#include <ops/custom_defs.h>
#include "profile_tm.h"
#ifdef FIXED_POINT
#define OVERRIDE_INNER_PROD
Int32 inner_prod(const Int16 * restrict x, const Int16 * restrict y, int len)
{
register int sum = 0;
INNERPROD_START();
if ( (int)x & 0x03 == 0 && (int)y & 0x03 == 0 )
{
register int i;
len >>= 1;
for ( i=0 ; i<len ; i+=4 )
{
register int x0, x1, y0, y1, x2, x3, y2, y3;
x0 = ld32x(x,i);
y0 = ld32x(x,i);
x1 = ld32x(x,i+1);
y1 = ld32x(y,i+1);
sum += (ifir16(x0,y0) + ifir16(x1,y1)) >> 6;
x2 = ld32x(x,i+2);
y2 = ld32x(x,i+2);
x3 = ld32x(x,i+3);
y3 = ld32x(x,i+3);
sum += (ifir16(x2,y2) + ifir16(x3,y3)) >> 6;
}
} else
{
len >>= 3;
while( len-- )
{
register int x0, x1, x2, x3, y0, y1, y2, y3;
x0 = pack16lsb(x[0],x[1]);
y0 = pack16lsb(y[0],y[1]);
x1 = pack16lsb(x[2],x[3]);
y1 = pack16lsb(y[2],y[3]);
sum += (ifir16(x0,y0) + ifir16(x1,y1)) >> 6;
x2 = pack16lsb(x[4],x[5]);
y2 = pack16lsb(y[4],y[5]);
x3 = pack16lsb(x[6],x[7]);
y3 = pack16lsb(y[6],y[7]);
sum += (ifir16(x2,y2) + ifir16(x3,y3)) >> 6;
x += 8;
y += 8;
}
}
INNERPROD_STOP();
return sum;
}
#define OVERRIDE_PITCH_XCORR
void pitch_xcorr(const Int16 *_x, const Int16 *_y, Int32 *corr, int len, int nb_pitch, char *stack)
{
register int sum_1, sum_2, sum_3, sum_4;
register int y10, y32, y54, y76, y21, y43, y65;
register int x10, x32;
register int i, j, k, limit;
TMDEBUG_ALIGNMEM(_x);
TMDEBUG_ALIGNMEM(_y);
PITCHXCORR_START();
limit = nb_pitch >> 1;
len >>= 1;
for (i=0 ; i<limit ; i+=2 )
{
sum_1 = sum_2 = sum_3 = sum_4 = 0;
y10 = ld32x(_y,i);
y32 = ld32x(_y,i+1);
for ( j=0 ; j<len ; j+=2 )
{
x10 = ld32x(_x,j);
x32 = ld32x(_x,j+1);
y54 = ld32x(_y,i+j+2);
y76 = ld32x(_y,i+j+3);
sum_1 += (ifir16(x10,y10) + ifir16(x32,y32)) >> 6;
sum_3 += (ifir16(x10,y32) + ifir16(x32,y54)) >> 6;
y21 = funshift2(y32,y10);
y43 = funshift2(y54,y32);
y65 = funshift2(y76,y54);
sum_2 += (ifir16(x10,y21) + ifir16(x32,y43)) >> 6;
sum_4 += (ifir16(x10,y43) + ifir16(x32,y65)) >> 6;
y10 = y54;
y32 = y76;
}
k = i << 1;
corr[nb_pitch-1-k]=sum_1;
corr[nb_pitch-2-k]=sum_2;
corr[nb_pitch-3-k]=sum_3;
corr[nb_pitch-4-k]=sum_4;
}
#ifndef REMARK_ON
(void)stack;
#endif
PITCHXCORR_STOP();
}
#ifndef ttisim
#define OVERRIDE_PITCH_GAIN_SEARCH_3TAP_VQ
static int pitch_gain_search_3tap_vq
(
const signed char *gain_cdbk,
int gain_cdbk_size,
Int16 *C16,
Int16 max_gain
)
{
register int pp = 0x00400040, p=64;
register int g10, g2, g20, g21, g02, g22, g01;
register int cb0, cb1, cb2, cb5432;
register int C10, C32, C54, C76, C98, C83, C2;
register int acc0, acc1, acc2, acc3, sum, gsum, bsum=-VERY_LARGE32;
register int i, best_cdbk=0;
register Int16 tmp;
TMDEBUG_ALIGNMEM(C16);
TMDEBUG_ALIGNMEM(gain_cdbk+2);
PITCHGAINSEARCH3TAPVQ_START();
tmp = ild16(gain_cdbk);
C98 = ld32x(C16,4);
C32 = ld32x(C16,1);
C10 = ld32(C16);
C54 = ld32x(C16,2);
C76 = ld32x(C16,3);
cb0 = sex8(tmp);
cb1 = sex8(tmp>>8);
C83 = funshift2(C98,C32);
C2 = sex16(C32);
gain_cdbk += 2;
#if (TM_UNROLL && TM_UNROLL_PITCHGAINSEARCH3TAPVQ > 0)
#pragma TCS_unroll=4
#pragma TCS_unrollexact=1
#endif
for ( i=0 ; i<gain_cdbk_size ; ++i )
{
cb5432 = ld32x(gain_cdbk,i);
cb2 = sex8(cb5432);
gsum = sex8(cb5432>>8);
sum = 0;
g10 = pack16lsb(cb1 + 32, cb0 + 32);
g2 = cb2 + 32;
g02 = pack16lsb(g10, g2);
acc0 = dspidualmul(g10,pp);
sum += ifir16(acc0,C10);
sum += p * g2 * C2;
g22 = pack16lsb(g02, g02);
g01 = funshift2(g10, g10);
acc1 = dspidualmul(g22, g01);
sum -= ifir16(acc1, C54);
acc2 = dspidualmul(g10, g10);
sum -= ifir16(acc2, C76);
g20 = pack16lsb(g2, g10);
g21 = funshift2(g2, g10);
acc3 = dspidualmul(g20, g21);
sum -= ifir16(acc3, C83);
if ( sum>bsum && gsum<=max_gain )
{ bsum = sum;
best_cdbk=i;
}
cb0 = sex8(cb5432 >> 16);
cb1 = asri(24,cb5432);
}
#if (TM_UNROLL && TM_UNROLL_PITCHGAINSEARCH3TAPVQ > 0)
#pragma TCS_unrollexact=0
#pragma TCS_unroll=0
#endif
PITCHGAINSEARCH3TAPVQ_STOP();
return best_cdbk;
}
#endif
#define OVERRIDE_COMPUTE_PITCH_ERROR
#ifndef OVERRIDE_PITCH_GAIN_SEARCH_3TAP_VQ
inline Int32 compute_pitch_error(Int16 *C, Int16 *g, Int16 pitch_control)
{
register int c10, c32, c54, c76, c98, c83;
register int g10, g32, g02, g22, g01, g21, g20;
register int pp, tmp0, tmp1, tmp2, tmp3;
register int sum = 0;
COMPUTEPITCHERROR_START();
g10 = ld32(g);
g32 = ld32x(g,1);
pp = pack16lsb(pitch_control,pitch_control);
c10 = ld32(C);
c32 = ld32x(C,1);
g02 = pack16lsb(g10,g32);
g22 = pack16lsb(g32,g32);
g01 = funshift2(g10,g10);
tmp0 = dspidualmul(g10,pp);
sum += ifir16(tmp0, c10);
sum += pitch_control * sex16(g32) * sex16(c32);
c54 = ld32x(C,2);
c76 = ld32x(C,3);
c98 = ld32x(C,4);
tmp1 = dspidualmul(g22,g01);
sum -= ifir16(tmp1, c54);
tmp2 = dspidualmul(g10,g10);
sum -= ifir16(tmp2,c76);
c83 = funshift2(c98,c32);
g20 = funshift2(g02,g02);
g21 = funshift2(g02,g10);
tmp3 = dspidualmul(g20,g21);
sum -= ifir16(tmp3,c83);
COMPUTEPITCHERROR_STOP();
return sum;
}
#endif
#define OVERRIDE_OPEN_LOOP_NBEST_PITCH
void open_loop_nbest_pitch(Int16 *sw, int start, int end, int len, int *pitch, Int16 *gain, int N, char *stack)
{
VARDECL(int *best_score);
VARDECL(int *best_ener);
VARDECL(Int32 *corr);
VARDECL(Int16 *corr16);
VARDECL(Int16 *ener16);
register int i, j, k, l, N4, N2;
register int _sw10, _sw32, _s0, _s2, limit;
register int *energy;
register int cshift=0, eshift=0;
register int scaledown = 0;
register int e0, _energy0;
ALLOC(corr16, end-start+1, Int16);
ALLOC(ener16, end-start+1, Int16);
ALLOC(corr, end-start+1, Int32);
ALLOC(best_score, N, int);
ALLOC(best_ener, N, int);
energy = corr;
N4 = N << 2;
N2 = N >> 1;
TMDEBUG_ALIGNMEM(sw);
TMDEBUG_ALIGNMEM(pitch);
TMDEBUG_ALIGNMEM(gain);
TMDEBUG_ALIGNMEM(best_score);
TMDEBUG_ALIGNMEM(best_ener);
TMDEBUG_ALIGNMEM(corr16);
TMDEBUG_ALIGNMEM(ener16);
OPENLOOPNBESTPITCH_START();
for ( i=0 ; i<N4 ; i+=4 )
{ st32d(i,best_score,-1);
st32d(i,best_ener,0);
st32d(i,pitch,start);
}
for ( j=asri(1,-end) ; j<N2 ; ++j )
{ register int _sw10;
_sw10 = ld32x(sw,j);
_sw10 = dspidualabs(_sw10);
if ( _sw10 & 0xC000C000 )
{ scaledown = 1;
break;
}
}
if ( scaledown )
{
for ( j=asri(1,-end),k=asli(1,-end) ; j<N2 ; ++j,k+=4 )
{ register int _sw10;
_sw10 = ld32x(sw,j);
_sw10 = dualasr(_sw10,1);
st32d(k, sw, _sw10);
}
}
energy[0] = _energy0 = inner_prod(sw-start, sw-start, len);
e0 = inner_prod(sw, sw, len);
j=asri(1,-start-1); k=j+20;
_sw10 = ld32x(sw,j);
_sw32 = ld32x(sw,k);
limit = end-1-start;
for ( i=1,--j,--k ; i<limit ; i+=2,--j,--k )
{ register int _energy1, __sw10, __sw32, __s0, __s2;
_s0 = sex16(_sw10);
_s2 = sex16(_sw32);
_energy1 = (_energy0 + ((_s0 * _s0) >> 6)) - ((_s2 * _s2) >> 6);
_energy0 = imax(0,_energy1);
energy[i] = _energy0;
__sw10 = ld32x(sw,j);
__sw32 = ld32x(sw,k);
__s0 = asri(16,__sw10);
__s2 = asri(16,__sw32);
_energy1 = (_energy0 + ((__s0 * __s0) >> 6)) - ((__s2 * __s2) >> 6);
_energy0 = imax(0,_energy1);
energy[i+1] = _energy0;
_sw10 = __sw10;
_sw32 = __sw32;
}
_s0 = sex16(_sw10);
_s2 = sex16(_sw32);
_energy0 = imax(0,(_energy0 + ((_s0 * _s0) >> 6)) - ((_s2 * _s2) >> 6));
energy[i] = _energy0;
eshift = normalize16(energy, ener16, 32766, end-start+1);
/* In fixed-point, this actually overrites the energy array (aliased to corr) */
pitch_xcorr(sw, sw-end, corr, len, end-start+1, stack);
/* Normalize to 180 so we can square it and it still fits in 16 bits */
cshift = normalize16(corr, corr16, 180, end-start+1);
/* If we scaled weighted input down, we need to scale it up again (OK, so we've just lost the LSB, who cares?) */
if ( scaledown )
{
for ( j=asri(1,-end),k=asli(1,-end) ; j<N2 ; ++j,k+=4 )
{ register int _sw10;
_sw10 = ld32x(sw,j);
_sw10 = dualasl(_sw10,1);
st32d(k, sw, _sw10);
}
}
/* Search for the best pitch prediction gain */
for ( i=start,l=0 ; i<end ; i+=2,++l )
{ register int _corr16, _c0, _c1;
register int _ener16, _e0, _e1;
_corr16 = ld32x(corr16,l);
_corr16 = dspidualmul(_corr16,_corr16);
_c0 = sex16(_corr16);
_c1 = asri(16,_corr16);
_ener16 = ld32x(ener16,l);
_ener16 = dspidualadd(_ener16,0x00010001);
_e0 = sex16(_ener16);
_e1 = asri(16,_ener16);
/* Instead of dividing the tmp by the energy, we multiply on the other side */
if ( (_c0 * best_ener[N-1]) > (best_score[N-1] * _e0) )
{
best_score[N-1] = _c0;
best_ener[N-1] = _e0;
pitch[N-1] = i;
for( j=0 ; j<N-1 ; ++j )
{ if ( (_c0 * best_ener[j]) > best_score[j] * _e0 )
{ for( k=N-1 ; k>j ; --k )
{
best_score[k]=best_score[k-1];
best_ener[k]=best_ener[k-1];
pitch[k]=pitch[k-1];
}
best_score[j]=_c0;
best_ener[j]=_e0;
pitch[j]=i;
break;
}
}
}
if ( (_c1 * best_ener[N-1]) > (best_score[N-1] * _e1) )
{
best_score[N-1] = _c1;
best_ener[N-1] = _e1;
pitch[N-1] = i+1;
for( j=0 ; j<N-1 ; ++j )
{ if ( (_c1 * best_ener[j]) > best_score[j] * _e1 )
{ for( k=N-1 ; k>j ; --k )
{
best_score[k]=best_score[k-1];
best_ener[k]=best_ener[k-1];
pitch[k]=pitch[k-1];
}
best_score[j]=_c1;
best_ener[j]=_e1;
pitch[j]=i+1;
break;
}
}
}
}
/* Compute open-loop gain if necessary */
if (gain)
{
for (j=0;j<N;j++)
{
spx_word16_t g;
i=pitch[j];
g = DIV32(SHL32(EXTEND32(corr16[i-start]),cshift), 10+SHR32(MULT16_16(spx_sqrt(e0),spx_sqrt(SHL32(EXTEND32(ener16[i-start]),eshift))),6));
gain[j] = imax(0,g);
}
}
OPENLOOPNBESTPITCH_STOP();
}
#endif