/* Copyright (C) 2007 Hong Zhiqian */ | |
/** | |
@file mdf_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" | |
// shifted power spectrum to fftwrap.c so that optimisation can be shared between mdf.c and preprocess.c | |
#define OVERRIDE_POWER_SPECTRUM | |
#ifdef FIXED_POINT | |
#else | |
#define OVERRIDE_FILTER_DC_NOTCH16 | |
void filter_dc_notch16( | |
const spx_int16_t * restrict in, | |
float radius, | |
float * restrict out, | |
int len, | |
float * restrict mem | |
) | |
{ | |
register int i; | |
register float den2, r1; | |
register float mem0, mem1; | |
FILTERDCNOTCH16_START(); | |
r1 = 1 - radius; | |
den2 = (radius * radius) + (0.7 * r1 * r1); | |
mem0 = mem[0]; | |
mem1 = mem[1]; | |
#if (TM_UNROLL && TM_UNROLL_FILTERDCNOTCH16) | |
#pragma TCS_unroll=4 | |
#pragma TCS_unrollexact=1 | |
#endif | |
for ( i=0 ; i<len ; ++i ) | |
{ | |
register float vin = in[i]; | |
register float vout = mem0 + vin; | |
register float rvout = radius * vout; | |
mem0 = mem1 + 2 * (-vin + rvout); | |
mem1 = vin - (den2 * vout); | |
out[i] = rvout; | |
} | |
#if (TM_UNROLL && TM_UNROLL_FILTERDCNOTCH16) | |
#pragma TCS_unrollexact=0 | |
#pragma TCS_unroll=0 | |
#endif | |
mem[0] = mem0; | |
mem[1] = mem1; | |
FILTERDCNOTCH16_STOP(); | |
} | |
#define OVERRIDE_MDF_INNER_PROD | |
float mdf_inner_prod( | |
const float * restrict x, | |
const float * restrict y, | |
int len | |
) | |
{ | |
register float sum = 0; | |
MDFINNERPROD_START(); | |
len >>= 1; | |
#if (TM_UNROLL && TM_UNROLL_MDFINNERPRODUCT) | |
#pragma TCS_unroll=4 | |
#pragma TCS_unrollexact=1 | |
#endif | |
while(len--) | |
{ | |
register float acc0, acc1; | |
acc0 = (*x++) * (*y++); | |
acc1 = (*x++) * (*y++); | |
sum += acc0 + acc1; | |
} | |
#if (TM_UNROLL && TM_UNROLL_MDFINNERPRODUCT) | |
#pragma TCS_unrollexact=0 | |
#pragma TCS_unroll=0 | |
#endif | |
MDFINNERPROD_STOP(); | |
return sum; | |
} | |
#define OVERRIDE_SPECTRAL_MUL_ACCUM | |
void spectral_mul_accum( | |
const float * restrict X, | |
const float * restrict Y, | |
float * restrict acc, | |
int N, int M | |
) | |
{ | |
register int i, j; | |
register float Xi, Yi, Xii, Yii; | |
register int _N; | |
SPECTRALMULACCUM_START(); | |
acc[0] = X[0] * Y[0]; | |
_N = N-1; | |
for ( i=1 ; i<_N ; i+=2 ) | |
{ | |
Xi = X[i]; | |
Yi = Y[i]; | |
Xii = X[i+1]; | |
Yii = Y[i+1]; | |
acc[i] = (Xi * Yi - Xii * Yii); | |
acc[i+1]= (Xii * Yi + Xi * Yii); | |
} | |
acc[_N] = X[_N] * Y[_N]; | |
for ( j=1,X+=N,Y+=N ; j<M ; j++ ) | |
{ | |
acc[0] += X[0] * Y[0]; | |
for ( i=1 ; i<N-1 ; i+=2 ) | |
{ | |
Xi = X[i]; | |
Yi = Y[i]; | |
Xii = X[i+1]; | |
Yii = Y[i+1]; | |
acc[i] += (Xi * Yi - Xii * Yii); | |
acc[i+1]+= (Xii * Yi + Xi * Yii); | |
} | |
acc[_N] += X[_N] * Y[_N]; | |
X += N; | |
Y += N; | |
} | |
SPECTRALMULACCUM_STOP(); | |
} | |
#define OVERRIDE_WEIGHTED_SPECTRAL_MUL_CONJ | |
void weighted_spectral_mul_conj( | |
const float * restrict w, | |
const float p, | |
const float * restrict X, | |
const float * restrict Y, | |
float * restrict prod, | |
int N | |
) | |
{ | |
register int i, j; | |
register int _N; | |
WEIGHTEDSPECTRALMULCONJ_START(); | |
prod[0] = p * w[0] * X[0] * Y[0]; | |
_N = N-1; | |
for (i=1,j=1;i<_N;i+=2,j++) | |
{ | |
register float W; | |
register float Xi, Yi, Xii, Yii; | |
Xi = X[i]; | |
Yi = Y[i]; | |
Xii = X[i+1]; | |
Yii = Y[i+1]; | |
W = p * w[j]; | |
prod[i] = W * (Xi * Yi + Xii * Yii); | |
prod[i+1]= W * (Xi * Yii - Xii * Yi); | |
} | |
prod[_N] = p * w[j] * X[_N] * Y[_N]; | |
WEIGHTEDSPECTRALMULCONJ_STOP(); | |
} | |
#define OVERRIDE_MDF_ADJUST_PROP | |
void mdf_adjust_prop( | |
const float * restrict W, | |
int N, | |
int M, | |
float * restrict prop | |
) | |
{ | |
register int i, j; | |
register float max_sum = 1; | |
register float prop_sum = 1; | |
MDFADJUSTPROP_START(); | |
for ( i=0 ; i<M ; ++i ) | |
{ | |
register float tmp = 1; | |
register int k = i * N; | |
register int l = k + N; | |
register float propi; | |
#if (TM_UNROLL && TM_UNROLL_MDFADJUSTPROP) | |
#pragma TCS_unroll=4 | |
#pragma TCS_unrollexact=1 | |
#endif | |
for ( j=k ; j<l ; ++j ) | |
{ | |
register float wi = W[j]; | |
tmp += wi * wi; | |
} | |
#if (TM_UNROLL && TM_UNROLL_MDFADJUSTPROP) | |
#pragma TCS_unrollexact=0 | |
#pragma TCS_unroll=0 | |
#endif | |
propi = spx_sqrt(tmp); | |
prop[i]= propi; | |
max_sum= fmux(propi > max_sum, propi, max_sum); | |
} | |
for ( i=0 ; i<M ; ++i ) | |
{ | |
register float propi = prop[i]; | |
propi += .1f * max_sum; | |
prop_sum += propi; | |
prop[i] = propi; | |
} | |
prop_sum = 0.99f / prop_sum; | |
for ( i=0 ; i<M ; ++i ) | |
{ prop[i] = prop_sum * prop[i]; | |
} | |
MDFADJUSTPROP_STOP(); | |
} | |
#define OVERRIDE_SPEEX_ECHO_GET_RESIDUAL | |
void speex_echo_get_residual( | |
SpeexEchoState * restrict st, | |
float * restrict residual_echo, | |
int len | |
) | |
{ | |
register int i; | |
register float leak2, leake; | |
register int N; | |
register float * restrict window; | |
register float * restrict last_y; | |
register float * restrict y; | |
SPEEXECHOGETRESIDUAL_START(); | |
window = st->window; | |
last_y = st->last_y; | |
y = st->y; | |
N = st->window_size; | |
#if (TM_UNROLL && TM_UNROLL_SPEEXECHOGETRESIDUAL) | |
#pragma TCS_unroll=4 | |
#pragma TCS_unrollexact=1 | |
#endif | |
for (i=0;i<N;i++) | |
{ y[i] = window[i] * last_y[i]; | |
} | |
#if (TM_UNROLL && TM_UNROLL_SPEEXECHOGETRESIDUAL) | |
#pragma TCS_unrollexact=0 | |
#pragma TCS_unroll=0 | |
#endif | |
spx_fft(st->fft_table, st->y, st->Y); | |
power_spectrum(st->Y, residual_echo, N); | |
leake = st->leak_estimate; | |
leak2 = fmux(leake > .5, 1, 2 * leake); | |
N = st->frame_size; | |
#if (TM_UNROLL && TM_UNROLL_SPEEXECHOGETRESIDUAL) | |
#pragma TCS_unroll=4 | |
#pragma TCS_unrollexact=1 | |
#endif | |
for ( i=0 ; i<N ; ++i ) | |
{ residual_echo[i] *= leak2; | |
} | |
#if (TM_UNROLL && TM_UNROLL_SPEEXECHOGETRESIDUAL) | |
#pragma TCS_unrollexact=0 | |
#pragma TCS_unroll=0 | |
#endif | |
residual_echo[N] *= leak2; | |
#ifndef NO_REMARK | |
(void)len; | |
#endif | |
SPEEXECHOGETRESIDUAL_STOP(); | |
} | |
#endif | |
void mdf_preemph( | |
SpeexEchoState * restrict st, | |
spx_word16_t * restrict x, | |
const spx_int16_t * restrict far_end, | |
int framesize | |
) | |
{ | |
register spx_word16_t preemph = st->preemph; | |
register spx_word16_t memX = st->memX; | |
register spx_word16_t memD = st->memD; | |
register spx_word16_t * restrict input = st->input; | |
register int i; | |
#ifdef FIXED_POINT | |
register int saturated = st->saturated; | |
#endif | |
#if (TM_UNROLL && TM_UNROLL_SPEEXECHOCANCELLATION) | |
#pragma TCS_unroll=4 | |
#pragma TCS_unrollexact=1 | |
#endif | |
for ( i=0 ; i<framesize ; ++i ) | |
{ | |
register spx_int16_t far_endi = far_end[i]; | |
register spx_word32_t tmp32; | |
register spx_word16_t inputi = input[i]; | |
tmp32 = SUB32(EXTEND32(far_endi), EXTEND32(MULT16_16_P15(preemph,memX))); | |
#ifdef FIXED_POINT | |
saturated = mux(iabs(tmp32) > 32767, M+1, saturated); | |
tmp32 = iclipi(tmp32,32767); | |
#endif | |
x[i] = EXTRACT16(tmp32); | |
memX = far_endi; | |
tmp32 = SUB32(EXTEND32(inputi), EXTEND32(MULT16_16_P15(preemph, memD))); | |
#ifdef FIXED_POINT | |
saturated = mux( ((tmp32 > 32767) && (saturated == 0)), 1, | |
mux( ((tmp32 <-32767) && (saturated == 0)), 1, saturated )); | |
tmp32 = iclipi(tmp32,32767); | |
#endif | |
memD = inputi; | |
input[i] = tmp32; | |
} | |
#if (TM_UNROLL && TM_UNROLL_SPEEXECHOCANCELLATION) | |
#pragma TCS_unrollexact=0 | |
#pragma TCS_unroll=0 | |
#endif | |
st->memD = memD; | |
st->memX = memX; | |
#ifdef FIXED_POINT | |
st->saturated = saturated; | |
#endif | |
} | |
void mdf_sub( | |
spx_word16_t * restrict dest, | |
const spx_word16_t * restrict src1, | |
const spx_word16_t * restrict src2, | |
int framesize | |
) | |
{ | |
register int i; | |
#if (TM_UNROLL && TM_UNROLL_SPEEXECHOCANCELLATION) | |
#pragma TCS_unroll=4 | |
#pragma TCS_unrollexact=1 | |
#endif | |
#ifdef FIXED_POINT | |
for ( i=0,framesize<<=1 ; i<framesize ; i+=4 ) | |
{ register int src1i, src2i, desti; | |
src1i = ld32d(src1,i); | |
src2i = ld32d(src2,i); | |
desti = dspidualsub(src1i,src2i); | |
st32d(i, dest, desti); | |
} | |
#else | |
for ( i=0 ; i<framesize ; ++i ) | |
{ dest[i] = src1[i] - src2[i]; | |
} | |
#endif | |
#if (TM_UNROLL && TM_UNROLL_SPEEXECHOCANCELLATION) | |
#pragma TCS_unrollexact=0 | |
#pragma TCS_unroll=0 | |
#endif | |
} | |
void mdf_sub_int( | |
spx_word16_t * restrict dest, | |
const spx_int16_t * restrict src1, | |
const spx_int16_t * restrict src2, | |
int framesize | |
) | |
{ | |
register int i, j; | |
#if (TM_UNROLL && TM_UNROLL_SPEEXECHOCANCELLATION) | |
#pragma TCS_unroll=4 | |
#pragma TCS_unrollexact=1 | |
#endif | |
#ifdef FIXED_POINT | |
for ( i=0,framesize<<=1 ; i<framesize ; i+=4 ) | |
{ register int src1i, src2i, desti; | |
src1i = ld32d(src1,i); | |
src2i = ld32d(src2,i); | |
desti = dspidualsub(src1i,src2i); | |
st32d(i, dest, desti); | |
} | |
#else | |
for ( i=0,j=0 ; i<framesize ; i+=2,++j ) | |
{ register int src1i, src2i, desti; | |
src1i = ld32d(src1,j); | |
src2i = ld32d(src2,j); | |
desti = dspidualsub(src1i,src2i); | |
dest[i] = sex16(desti); | |
dest[i+1] = asri(16,desti); | |
} | |
#endif | |
#if (TM_UNROLL && TM_UNROLL_SPEEXECHOCANCELLATION) | |
#pragma TCS_unrollexact=0 | |
#pragma TCS_unroll=0 | |
#endif | |
} | |
void mdf_compute_weight_gradient( | |
SpeexEchoState * restrict st, | |
spx_word16_t * restrict X, | |
int N, | |
int M | |
) | |
{ | |
register int i, j; | |
register spx_word32_t * restrict PHI = st->PHI; | |
for (j=M-1;j>=0;j--) | |
{ | |
register spx_word32_t * restrict W = &(st->W[j*N]); | |
weighted_spectral_mul_conj( | |
st->power_1, | |
FLOAT_SHL(PSEUDOFLOAT(st->prop[j]),-15), | |
&X[(j+1)*N], | |
st->E, | |
st->PHI, | |
N); | |
#if (TM_UNROLL && TM_UNROLL_SPEEXECHOCANCELLATION) | |
#pragma TCS_unroll=4 | |
#pragma TCS_unrollexact=1 | |
#endif | |
for (i=0;i<N;i++) | |
{ W[i] = ADD32(W[i],PHI[i]); | |
} | |
#if (TM_UNROLL && TM_UNROLL_SPEEXECHOCANCELLATION) | |
#pragma TCS_unrollexact=0 | |
#pragma TCS_unroll=0 | |
#endif | |
} | |
} | |
void mdf_update_weight( | |
SpeexEchoState * restrict st, | |
int N, | |
int M, | |
int framesize | |
) | |
{ | |
register int j; | |
register int cancel_count = st->cancel_count; | |
register spx_word16_t * restrict wtmp = st->wtmp; | |
#ifdef FIXED_POINT | |
register spx_word16_t * restrict wtmp2 = st->wtmp2; | |
register int i; | |
#endif | |
for ( j=0 ; j<M ; j++ ) | |
{ | |
register spx_word32_t * restrict W = &(st->W[j*N]); | |
if (j==0 || cancel_count%(M-1) == j-1) | |
{ | |
#ifdef FIXED_POINT | |
#if (TM_UNROLL && TM_UNROLL_SPEEXECHOCANCELLATION) | |
#pragma TCS_unroll=4 | |
#pragma TCS_unrollexact=1 | |
#endif | |
for ( i=0 ; i<N ; i++ ) | |
wtmp2[i] = EXTRACT16(PSHR32(W[i],NORMALIZE_SCALEDOWN+16)); | |
#if (TM_UNROLL && TM_UNROLL_SPEEXECHOCANCELLATION) | |
#pragma TCS_unrollexact=0 | |
#pragma TCS_unroll=0 | |
#endif | |
spx_ifft(st->fft_table, wtmp2, wtmp); | |
memset(wtmp, 0, framesize * sizeof(spx_word16_t)); | |
#if (TM_UNROLL && TM_UNROLL_SPEEXECHOCANCELLATION) | |
#pragma TCS_unroll=4 | |
#pragma TCS_unrollexact=1 | |
#endif | |
for (j=framesize; j<N ; ++j) | |
{ wtmp[j]=SHL16(wtmp[j],NORMALIZE_SCALEUP); | |
} | |
#if (TM_UNROLL && TM_UNROLL_SPEEXECHOCANCELLATION) | |
#pragma TCS_unrollexact=0 | |
#pragma TCS_unroll=0 | |
#endif | |
spx_fft(st->fft_table, wtmp, wtmp2); | |
#if (TM_UNROLL && TM_UNROLL_SPEEXECHOCANCELLATION) | |
#pragma TCS_unroll=4 | |
#pragma TCS_unrollexact=1 | |
#endif | |
for (i=0;i<N;i++) | |
{ W[i] -= SHL32(EXTEND32(wtmp2[i]),16+NORMALIZE_SCALEDOWN-NORMALIZE_SCALEUP-1); | |
} | |
#if (TM_UNROLL && TM_UNROLL_SPEEXECHOCANCELLATION) | |
#pragma TCS_unrollexact=0 | |
#pragma TCS_unroll=0 | |
#endif | |
#else | |
spx_ifft(st->fft_table, W, wtmp); | |
memset(&wtmp[framesize], 0, (N-framesize) * sizeof(spx_word16_t)); | |
spx_fft(st->fft_table, wtmp, W); | |
#endif | |
} | |
} | |
} | |
#ifdef TWO_PATH | |
// first four parameters is passed by registers | |
// generate faster performance with 4 parameters functions | |
spx_word32_t mdf_update_foreground( | |
SpeexEchoState * restrict st, | |
spx_word32_t Dbf, | |
spx_word32_t Sff, | |
spx_word32_t See | |
) | |
{ | |
register spx_word32_t Davg1 = st->Davg1; | |
register spx_word32_t Davg2 = st->Davg2; | |
register spx_word32_t Dvar1 = st->Dvar1; | |
register spx_word32_t Dvar2 = st->Dvar2; | |
register spx_word16_t * restrict input = st->input; | |
register int framesize = st->frame_size; | |
register spx_word16_t * restrict xx = st->x + framesize; | |
register spx_word16_t * restrict y = st->y + framesize; | |
register spx_word16_t * restrict ee = st->e + framesize; | |
register int update_foreground; | |
register int i; | |
register int N = st->window_size; | |
register int M = st->M; | |
#ifdef FIXED_POINT | |
register spx_word32_t sc0 = SUB32(Sff,See); | |
register spx_float_t sc1 = FLOAT_MUL32U(Sff,Dbf); | |
Davg1 = ADD32(MULT16_32_Q15(QCONST16(.6f,15),Davg1), MULT16_32_Q15(QCONST16(.4f,15),sc0)); | |
Davg2 = ADD32(MULT16_32_Q15(QCONST16(.85f,15),Davg2), MULT16_32_Q15(QCONST16(.15f,15),sc0)); | |
Dvar1 = FLOAT_ADD( | |
FLOAT_MULT(VAR1_SMOOTH,Dvar1), | |
FLOAT_MUL32U(MULT16_32_Q15(QCONST16(.4f,15),Sff), | |
MULT16_32_Q15(QCONST16(.4f,15),Dbf))); | |
Dvar2 = FLOAT_ADD( | |
FLOAT_MULT(VAR2_SMOOTH,Dvar2), | |
FLOAT_MUL32U(MULT16_32_Q15(QCONST16(.15f,15),Sff), | |
MULT16_32_Q15(QCONST16(.15f,15),Dbf))); | |
#else | |
register spx_word32_t sc0 = Sff - See; | |
register spx_word32_t sc1 = Sff * Dbf; | |
Davg1 = .6*Davg1 + .4*sc0; | |
Davg2 = .85*Davg2 + .15*sc0; | |
Dvar1 = VAR1_SMOOTH*Dvar1 + .16*sc1; | |
Dvar2 = VAR2_SMOOTH*Dvar2 + .0225*sc1; | |
#endif | |
update_foreground = | |
mux( FLOAT_GT(FLOAT_MUL32U(sc0, VABS(sc0)), sc1), 1, | |
mux( FLOAT_GT(FLOAT_MUL32U(Davg1, VABS(Davg1)), FLOAT_MULT(VAR1_UPDATE,(Dvar1))), 1, | |
mux( FLOAT_GT(FLOAT_MUL32U(Davg2, VABS(Davg2)), FLOAT_MULT(VAR2_UPDATE,(Dvar2))), 1, 0))); | |
if ( update_foreground ) | |
{ | |
register spx_word16_t * restrict windowf = st->window + framesize; | |
register spx_word16_t * restrict window = st->window; | |
st->Davg1 = st->Davg2 = 0; | |
st->Dvar1 = st->Dvar2 = FLOAT_ZERO; | |
memcpy(st->foreground, st->W, N*M*sizeof(spx_word32_t)); | |
#if (TM_UNROLL && TM_UNROLL_SPEEXECHOCANCELLATION) | |
#pragma TCS_unroll=4 | |
#pragma TCS_unrollexact=1 | |
#endif | |
for ( i=0 ; i<framesize ; ++i) | |
{ register spx_word16_t wi = window[i]; | |
register spx_word16_t wfi = windowf[i]; | |
register spx_word16_t ei = ee[i]; | |
register spx_word16_t yi = y[i]; | |
ee[i] = MULT16_16_Q15(wfi,ei) + MULT16_16_Q15(wi,yi); | |
} | |
#if (TM_UNROLL && TM_UNROLL_SPEEXECHOCANCELLATION) | |
#pragma TCS_unrollexact=0 | |
#pragma TCS_unroll=0 | |
#endif | |
} else | |
{ | |
register int reset_background; | |
reset_background = | |
mux( FLOAT_GT(FLOAT_MUL32U(-(sc0),VABS(sc0)), FLOAT_MULT(VAR_BACKTRACK,sc1)), 1, | |
mux( FLOAT_GT(FLOAT_MUL32U(-(Davg1), VABS(Davg1)), FLOAT_MULT(VAR_BACKTRACK,Dvar1)), 1, | |
mux( FLOAT_GT(FLOAT_MUL32U(-(Davg2), VABS(Davg2)), FLOAT_MULT(VAR_BACKTRACK,Dvar2)), 1, 0))); | |
if ( reset_background ) | |
{ | |
memcpy(st->W, st->foreground, N*M*sizeof(spx_word32_t)); | |
memcpy(y, ee, framesize * sizeof(spx_word16_t)); | |
mdf_sub(xx,input,y,framesize); | |
See = Sff; | |
st->Davg1 = st->Davg2 = 0; | |
st->Dvar1 = st->Dvar2 = FLOAT_ZERO; | |
} else | |
{ | |
st->Davg1 = Davg1; | |
st->Davg2 = Davg2; | |
st->Dvar1 = Dvar1; | |
st->Dvar2 = Dvar2; | |
} | |
} | |
return See; | |
} | |
#endif | |
void mdf_compute_error_signal( | |
SpeexEchoState * restrict st, | |
const spx_int16_t * restrict in, | |
spx_int16_t * restrict out, | |
int framesize | |
) | |
{ | |
register spx_word16_t preemph = st->preemph; | |
register spx_word16_t memE = st->memE; | |
register int saturated = st->saturated; | |
register spx_word16_t * restrict e = st->e; | |
register spx_word16_t * restrict ee = st->e + framesize; | |
register spx_word16_t * restrict input = st->input; | |
register spx_word16_t * restrict xx = st->x + framesize; | |
register int i; | |
#if (TM_UNROLL && TM_UNROLL_SPEEXECHOCANCELLATION) | |
#pragma TCS_unroll=4 | |
#pragma TCS_unrollexact=1 | |
#endif | |
for ( i=0 ; i<framesize ; ++i ) | |
{ | |
register spx_word32_t tmp_out; | |
register spx_int16_t ini = in[i]; | |
register int flg; | |
#ifdef FIXED_POINT | |
#ifdef TWO_PATH | |
tmp_out = SUB32(EXTEND32(input[i]), EXTEND32(ee[i])); | |
tmp_out = iclipi(tmp_out,32767); | |
#else | |
tmp_out = SUB32(EXTEND32(input[i]), EXTEND32(y[i])); | |
tmp_out = iclipi(tmp_out,32767); | |
#endif | |
#else | |
#ifdef TWO_PATH | |
tmp_out = SUB32(EXTEND32(input[i]), EXTEND32(ee[i])); | |
#else | |
tmp_out = SUB32(EXTEND32(input[i]), EXTEND32(y[i])); | |
#endif | |
tmp_out = | |
fmux( tmp_out > 32767, 32767, | |
fmux( tmp_out < -32768, -32768, tmp_out)); | |
#endif | |
tmp_out = ADD32(tmp_out, EXTEND32(MULT16_16_P15(preemph,memE))); | |
flg = iabs(ini) >= 32000; | |
tmp_out = VMUX( flg, 0, tmp_out); | |
saturated = mux( flg && (saturated == 0), 1, saturated); | |
out[i] = (spx_int16_t)tmp_out; | |
memE = tmp_out; | |
} | |
#if (TM_UNROLL && TM_UNROLL_SPEEXECHOCANCELLATION) | |
#pragma TCS_unrollexact=0 | |
#pragma TCS_unroll=0 | |
#endif | |
st->memE = memE; | |
st->saturated = saturated; | |
memset(e, 0, framesize * sizeof(spx_word16_t)); | |
memcpy(ee, xx, framesize * sizeof(spx_word16_t)); | |
} | |
inline int mdf_check( | |
SpeexEchoState * restrict st, | |
spx_int16_t * out, | |
spx_word32_t Syy, | |
spx_word32_t Sxx, | |
spx_word32_t See, | |
spx_word32_t Sff, | |
spx_word32_t Sdd | |
) | |
{ | |
register int N = st->window_size; | |
register spx_word32_t N1e9 = N * 1e9; | |
register int screwed_up = st->screwed_up; | |
register int framesize = st->frame_size; | |
if (!(Syy>=0 && Sxx>=0 && See >= 0) | |
#ifndef FIXED_POINT | |
|| !(Sff < N1e9 && Syy < N1e9 && Sxx < N1e9 ) | |
#endif | |
) | |
{ | |
screwed_up += 50; | |
memset(out, 0, framesize * sizeof(spx_int16_t)); | |
} else | |
{ screwed_up = mux( SHR32(Sff, 2) > ADD32(Sdd, SHR32(MULT16_16(N, 10000),6)), screwed_up+1, 0); | |
} | |
st->screwed_up = screwed_up; | |
return screwed_up; | |
} | |
void mdf_smooth( | |
spx_word32_t * restrict power, | |
spx_word32_t * restrict Xf, | |
int framesize, | |
int M | |
) | |
{ | |
register spx_word16_t ss, ss_1, pf, xff; | |
register int j; | |
#ifdef FIXED_POINT | |
ss=DIV32_16(11469,M); | |
ss_1 = SUB16(32767,ss); | |
#else | |
ss=.35/M; | |
ss_1 = 1-ss; | |
#endif | |
#if (TM_UNROLL && TM_UNROLL_SPEEXECHOCANCELLATION) | |
#pragma TCS_unroll=4 | |
#pragma TCS_unrollexact=1 | |
#endif | |
for ( j=0 ; j<framesize ; ++j ) | |
{ register spx_word32_t pi = power[j]; | |
register spx_word32_t xfi = Xf[j]; | |
power[j] = MULT16_32_Q15(ss_1,pi) + 1 + MULT16_32_Q15(ss,xfi); | |
} | |
#if (TM_UNROLL && TM_UNROLL_SPEEXECHOCANCELLATION) | |
#pragma TCS_unrollexact=0 | |
#pragma TCS_unroll=0 | |
#endif | |
pf = power[framesize]; | |
xff = Xf[framesize]; | |
power[framesize] = MULT16_32_Q15(ss_1,pf) + 1 + MULT16_32_Q15(ss,xff); | |
} | |
void mdf_compute_filtered_spectra_crosscorrelations( | |
SpeexEchoState * restrict st, | |
spx_word32_t Syy, | |
spx_word32_t See, | |
int framesize | |
) | |
{ | |
register spx_float_t Pey = FLOAT_ONE; | |
register spx_float_t Pyy = FLOAT_ONE; | |
register spx_word16_t spec_average = st->spec_average; | |
register spx_word32_t * restrict pRf = st->Rf; | |
register spx_word32_t * restrict pYf = st->Yf; | |
register spx_word32_t * restrict pEh = st->Eh; | |
register spx_word32_t * restrict pYh = st->Yh; | |
register spx_word16_t beta0 = st->beta0; | |
register spx_word16_t beta_max = st->beta_max; | |
register spx_float_t alpha, alpha_1; | |
register spx_word32_t tmp32, tmpx; | |
register spx_float_t sPey = st->Pey; | |
register spx_float_t sPyy = st->Pyy; | |
register spx_float_t tmp; | |
register spx_word16_t leak_estimate; | |
register int j; | |
register spx_float_t Eh, Yh; | |
register spx_word32_t _Ehj, _Rfj, _Yfj, _Yhj; | |
#ifdef FIXED_POINT | |
register spx_word16_t spec_average1 = SUB16(32767,spec_average); | |
#else | |
register spx_word16_t spec_average1 = 1 - spec_average; | |
#endif | |
#if (TM_UNROLL && TM_UNROLL_SPEEXECHOCANCELLATION) | |
#pragma TCS_unroll=4 | |
#pragma TCS_unrollexact=1 | |
#endif | |
for (j=framesize; j>0 ; --j) | |
{ | |
_Ehj = pEh[j]; | |
_Rfj = pRf[j]; | |
_Yfj = pYf[j]; | |
_Yhj = pYh[j]; | |
Eh = PSEUDOFLOAT(_Rfj - _Ehj); | |
Yh = PSEUDOFLOAT(_Yfj - _Yhj); | |
Pey = FLOAT_ADD(Pey,FLOAT_MULT(Eh,Yh)); | |
Pyy = FLOAT_ADD(Pyy,FLOAT_MULT(Yh,Yh)); | |
pEh[j] = MAC16_32_Q15(MULT16_32_Q15(spec_average1, _Ehj), spec_average, _Rfj); | |
pYh[j] = MAC16_32_Q15(MULT16_32_Q15(spec_average1, _Yhj), spec_average, _Yfj); | |
} | |
#if (TM_UNROLL && TM_UNROLL_SPEEXECHOCANCELLATION) | |
#pragma TCS_unrollexact=0 | |
#pragma TCS_unroll=0 | |
#endif | |
_Ehj = pEh[0]; | |
_Rfj = pRf[0]; | |
_Yfj = pYf[0]; | |
_Yhj = pYh[0]; | |
Eh = PSEUDOFLOAT(_Rfj - _Ehj); | |
Yh = PSEUDOFLOAT(_Yfj - _Yhj); | |
Pey = FLOAT_ADD(Pey,FLOAT_MULT(Eh,Yh)); | |
Pyy = FLOAT_ADD(Pyy,FLOAT_MULT(Yh,Yh)); | |
pEh[0] = MAC16_32_Q15(MULT16_32_Q15(spec_average1, _Ehj), spec_average, _Rfj); | |
pYh[0] = MAC16_32_Q15(MULT16_32_Q15(spec_average1, _Yhj), spec_average, _Yfj); | |
Pyy = FLOAT_SQRT(Pyy); | |
Pey = FLOAT_DIVU(Pey,Pyy); | |
tmp32 = MULT16_32_Q15(beta0,Syy); | |
tmpx = MULT16_32_Q15(beta_max,See); | |
tmp32 = VMUX(tmp32 > tmpx, tmpx, tmp32); | |
alpha = FLOAT_DIV32(tmp32, See); | |
alpha_1 = FLOAT_SUB(FLOAT_ONE, alpha); | |
sPey = FLOAT_ADD(FLOAT_MULT(alpha_1,sPey) , FLOAT_MULT(alpha,Pey)); | |
sPyy = FLOAT_ADD(FLOAT_MULT(alpha_1,sPyy) , FLOAT_MULT(alpha,Pyy)); | |
tmp = FLOAT_MULT(MIN_LEAK,sPyy); | |
#ifndef FIXED_POINT | |
sPyy = VMUX(FLOAT_LT(sPyy, FLOAT_ONE), FLOAT_ONE, sPyy); | |
sPey = VMUX(FLOAT_LT(sPey, tmp), tmp, sPey); | |
sPey = VMUX(FLOAT_LT(sPey, sPyy), sPey, sPyy); | |
#else | |
sPyy = FLOAT_LT(sPyy, FLOAT_ONE) ? FLOAT_ONE : sPyy; | |
sPey = FLOAT_LT(sPey, tmp) ? tmp : sPey; | |
sPey = FLOAT_LT(sPey, sPyy) ? sPey : sPyy; | |
#endif | |
leak_estimate = FLOAT_EXTRACT16(FLOAT_SHL(FLOAT_DIVU(sPey, sPyy),14)); | |
leak_estimate = VMUX( leak_estimate > 16383, 32767, SHL16(leak_estimate,1)); | |
st->Pey = sPey; | |
st->Pyy = sPyy; | |
st->leak_estimate = leak_estimate; | |
} | |
inline spx_word16_t mdf_compute_RER( | |
spx_word32_t See, | |
spx_word32_t Syy, | |
spx_word32_t Sey, | |
spx_word32_t Sxx, | |
spx_word16_t leake | |
) | |
{ | |
register spx_word16_t RER; | |
#ifdef FIXED_POINT | |
register spx_word32_t tmp32; | |
register spx_word32_t tmp; | |
spx_float_t bound = PSEUDOFLOAT(Sey); | |
tmp32 = MULT16_32_Q15(leake,Syy); | |
tmp32 = ADD32(SHR32(Sxx,13), ADD32(tmp32, SHL32(tmp32,1))); | |
bound = FLOAT_DIVU(FLOAT_MULT(bound, bound), PSEUDOFLOAT(ADD32(1,Syy))); | |
tmp = FLOAT_EXTRACT32(bound); | |
tmp32 = imux( FLOAT_GT(bound, PSEUDOFLOAT(See)), See, | |
imux( tmp32 < tmp, tmp, tmp32)); | |
tmp = SHR32(See,1); | |
tmp32 = imux(tmp32 > tmp, tmp, tmp32); | |
RER = FLOAT_EXTRACT16(FLOAT_SHL(FLOAT_DIV32(tmp32,See),15)); | |
#else | |
register spx_word32_t r0; | |
r0 = (Sey * Sey)/(1 + See * Syy); | |
RER = (.0001*Sxx + 3.* MULT16_32_Q15(leake,Syy)) / See; | |
RER = fmux( RER < r0, r0, RER); | |
RER = fmux( RER > .5, .5, RER); | |
#endif | |
return RER; | |
} | |
void mdf_adapt( | |
SpeexEchoState * restrict st, | |
spx_word16_t RER, | |
spx_word32_t Syy, | |
spx_word32_t See, | |
spx_word32_t Sxx | |
) | |
{ | |
register spx_float_t * restrict power_1 = st->power_1; | |
register spx_word32_t * restrict power = st->power; | |
register int adapted = st->adapted; | |
register spx_word32_t sum_adapt = st->sum_adapt; | |
register spx_word16_t leake = st->leak_estimate; | |
register int framesize = st->frame_size; | |
register int i; | |
register int M = st->M; | |
adapted = mux( !adapted && sum_adapt > QCONST32(M,15) && | |
MULT16_32_Q15(leake,Syy) > MULT16_32_Q15(QCONST16(.03f,15),Syy), 1, adapted); | |
if ( adapted ) | |
{ register spx_word32_t * restrict Yf = st->Yf; | |
register spx_word32_t * restrict Rf = st->Rf; | |
register spx_word32_t r, e, e2; | |
#if (TM_UNROLL && TM_UNROLL_SPEEXECHOCANCELLATION) | |
#pragma TCS_unroll=4 | |
#pragma TCS_unrollexact=1 | |
#endif | |
for ( i=0 ; i<framesize ; ++i ) | |
{ | |
r = SHL32(Yf[i],3); | |
r = MULT16_32_Q15(leake,r); | |
e = SHL32(Rf[i],3)+1; | |
#ifdef FIXED_POINT | |
e2 = SHR32(e,1); | |
r = mux( r > e2, e2, r); | |
#else | |
e2 = e * .5; | |
r = fmux( r > e2, e2, r); | |
#endif | |
r = MULT16_32_Q15(QCONST16(.7,15),r) + | |
MULT16_32_Q15(QCONST16(.3,15),(spx_word32_t)(MULT16_32_Q15(RER,e))); | |
power_1[i] = FLOAT_SHL(FLOAT_DIV32_FLOAT(r,FLOAT_MUL32U(e,power[i]+10)),WEIGHT_SHIFT+16); | |
} | |
#if (TM_UNROLL && TM_UNROLL_SPEEXECHOCANCELLATION) | |
#pragma TCS_unrollexact=0 | |
#pragma TCS_unroll=0 | |
#endif | |
r = SHL32(Yf[framesize],3); | |
r = MULT16_32_Q15(leake,r); | |
e = SHL32(Rf[framesize],3)+1; | |
#ifdef FIXED_POINT | |
e2 = SHR32(e,1); | |
r = mux( r > e2, e2, r); | |
#else | |
e2 = e * .5; | |
r = fmux( r > e2, e2, r); | |
#endif | |
r = MULT16_32_Q15(QCONST16(.7,15),r) + | |
MULT16_32_Q15(QCONST16(.3,15),(spx_word32_t)(MULT16_32_Q15(RER,e))); | |
power_1[framesize] = FLOAT_SHL(FLOAT_DIV32_FLOAT(r,FLOAT_MUL32U(e,power[framesize]+10)),WEIGHT_SHIFT+16); | |
} else | |
{ | |
register spx_word16_t adapt_rate=0; | |
register int N = st->window_size; | |
if ( Sxx > SHR32(MULT16_16(N, 1000),6) ) | |
{ register spx_word32_t tmp32, tmp32q; | |
tmp32 = MULT16_32_Q15(QCONST16(.25f, 15), Sxx); | |
#ifdef FIXED_POINT | |
tmp32q = SHR32(See,2); | |
tmp32 = mux(tmp32 > tmp32q, tmp32q, tmp32); | |
#else | |
tmp32q = 0.25 * See; | |
tmp32 = fmux(tmp32 > tmp32q, tmp32q, tmp32); | |
#endif | |
adapt_rate = FLOAT_EXTRACT16(FLOAT_SHL(FLOAT_DIV32(tmp32, See),15)); | |
} | |
#if (TM_UNROLL && TM_UNROLL_SPEEXECHOCANCELLATION) | |
#pragma TCS_unroll=4 | |
#pragma TCS_unrollexact=1 | |
#endif | |
for (i=0;i<framesize;i++) | |
power_1[i] = FLOAT_SHL(FLOAT_DIV32(EXTEND32(adapt_rate),ADD32(power[i],10)),WEIGHT_SHIFT+1); | |
#if (TM_UNROLL && TM_UNROLL_SPEEXECHOCANCELLATION) | |
#pragma TCS_unrollexact=0 | |
#pragma TCS_unroll=0 | |
#endif | |
power_1[framesize] = FLOAT_SHL(FLOAT_DIV32(EXTEND32(adapt_rate),ADD32(power[framesize],10)),WEIGHT_SHIFT+1); | |
sum_adapt = ADD32(sum_adapt,adapt_rate); | |
} | |
st->sum_adapt = sum_adapt; | |
st->adapted = adapted; | |
} | |
#define OVERRIDE_ECHO_CANCELLATION | |
void speex_echo_cancellation( | |
SpeexEchoState * restrict st, | |
const spx_int16_t * restrict in, | |
const spx_int16_t * restrict far_end, | |
spx_int16_t * restrict out | |
) | |
{ | |
register int framesize = st->frame_size; | |
register spx_word16_t * restrict x = st->x; | |
register spx_word16_t * restrict xx = st->x + framesize; | |
register spx_word16_t * restrict yy = st->y + framesize; | |
register spx_word16_t * restrict ee = st->e + framesize; | |
register spx_word32_t Syy, See, Sxx, Sdd, Sff; | |
register spx_word16_t RER; | |
register spx_word32_t Sey; | |
register int j; | |
register int N,M; | |
#ifdef TWO_PATH | |
register spx_word32_t Dbf; | |
#endif | |
N = st->window_size; | |
M = st->M; | |
st->cancel_count++; | |
filter_dc_notch16(in, st->notch_radius, st->input, framesize, st->notch_mem); | |
mdf_preemph(st, xx, far_end, framesize); | |
{ | |
register spx_word16_t * restrict X = st->X; | |
for ( j=M-1 ; j>=0 ; j-- ) | |
{ register spx_word16_t * restrict Xdes = &(X[(j+1)*N]); | |
register spx_word16_t * restrict Xsrc = &(X[j*N]); | |
memcpy(Xdes, Xsrc, N * sizeof(spx_word16_t)); | |
} | |
spx_fft(st->fft_table, x, X); | |
memcpy(st->last_y, st->x, N * sizeof(spx_word16_t)); | |
Sxx = mdf_inner_prod(xx, xx, framesize); | |
memcpy(x, xx, framesize * sizeof(spx_word16_t)); | |
#ifdef TWO_PATH | |
spectral_mul_accum(st->X, st->foreground, st->Y, N, M); | |
spx_ifft(st->fft_table, st->Y, st->e); | |
mdf_sub(xx, st->input, ee, framesize); | |
Sff = mdf_inner_prod(xx, xx, framesize); | |
#endif | |
mdf_adjust_prop (st->W, N, M, st->prop); | |
if (st->saturated == 0) | |
{ mdf_compute_weight_gradient(st, X, N, M); | |
} else | |
{ st->saturated--; | |
} | |
} | |
mdf_update_weight(st, N, M, framesize); | |
spectral_mul_accum(st->X, st->W, st->Y, N, M); | |
spx_ifft(st->fft_table, st->Y, st->y); | |
#ifdef TWO_PATH | |
mdf_sub(xx, ee, yy, framesize); | |
Dbf = 10+mdf_inner_prod(xx, xx, framesize); | |
#endif | |
mdf_sub(xx, st->input, yy, framesize); | |
See = mdf_inner_prod(xx, xx, framesize); | |
#ifndef TWO_PATH | |
Sff = See; | |
#else | |
See = mdf_update_foreground(st,Dbf,Sff,See); | |
#endif | |
mdf_compute_error_signal(st, in, out, framesize); | |
Sey = mdf_inner_prod(ee, yy, framesize); | |
Syy = mdf_inner_prod(yy, yy, framesize); | |
Sdd = mdf_inner_prod(st->input, st->input, framesize); | |
if ( mdf_check(st,out,Syy,Sxx,See,Sff,Sdd) >= 50 ) | |
{ speex_warning("The echo canceller started acting funny and got slapped (reset). It swears it will behave now."); | |
speex_echo_state_reset(st); | |
return; | |
} | |
See = MAX32(See, SHR32(MULT16_16(N, 100),6)); | |
spx_fft(st->fft_table, st->e, st->E); | |
memset(st->y, 0, framesize * sizeof(spx_word16_t)); | |
spx_fft(st->fft_table, st->y, st->Y); | |
power_spectrum(st->E, st->Rf, N); | |
power_spectrum(st->Y, st->Yf, N); | |
power_spectrum(st->X, st->Xf, N); | |
mdf_smooth(st->power,st->Xf,framesize, M); | |
mdf_compute_filtered_spectra_crosscorrelations(st,Syy,See,framesize); | |
RER = mdf_compute_RER(See,Syy,Sey,Sxx,st->leak_estimate); | |
mdf_adapt(st, RER, Syy, See, Sxx); | |
if ( st->adapted ) | |
{ register spx_word16_t * restrict last_yy = st->last_y + framesize; | |
memcpy(st->last_y, last_yy, framesize * sizeof(spx_word16_t)); | |
mdf_sub_int(last_yy, in, out, framesize); | |
} | |
} | |