diff options
Diffstat (limited to '3rdparty/speex/tmv/mdf_tm.h')
| -rw-r--r-- | 3rdparty/speex/tmv/mdf_tm.h | 1192 |
1 files changed, 0 insertions, 1192 deletions
diff --git a/3rdparty/speex/tmv/mdf_tm.h b/3rdparty/speex/tmv/mdf_tm.h deleted file mode 100644 index 0a6128e9..00000000 --- a/3rdparty/speex/tmv/mdf_tm.h +++ /dev/null @@ -1,1192 +0,0 @@ -/* 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); - - } -} - - - |