mdf_tm.h

一个开源的sip源代码

	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);

	}
}