mdf_tm.h

一个开源的sip源代码

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,