preprocess_tm.h

一个开源的sip源代码

#ifndef FIXED_POINT
static void speex_compute_agc(SpeexPreprocessState *st, spx_word16_t Pframe, spx_word16_t *ft);
#endif

void preprocess_residue_echo(
	SpeexPreprocessState * restrict st,
	int N,
	int NM
)
{
	if (st->echo_state)
	{
		register spx_word32_t * restrict r_echo = st->residual_echo;
		register spx_word32_t * restrict e_noise = st->echo_noise;
		register int i;

#ifndef FIXED_POINT
		register spx_word32_t r;
#endif

		speex_echo_get_residual(st->echo_state, r_echo, N);

#ifndef FIXED_POINT
		r = r_echo[0];
		if (!(r >=0 && r < N*1e9f) )
		{	
			memset(r_echo, 0, N * sizeof(spx_word32_t));
		}
#endif

#if (TM_UNROLL && TM_UNROLL_SPEEXPREPROCESSRUN)
#pragma TCS_unroll=4
#pragma TCS_unrollexact=1
#endif
		for (i=0;i<N;i++)
		{	register spx_word32_t eni = e_noise[i];
			e_noise[i] = MAX32(MULT16_32_Q15(QCONST16(.6f,15),eni), r_echo[i]);
		}
#if (TM_UNROLL && TM_UNROLL_SPEEXPREPROCESSRUN)
#pragma TCS_unrollexact=0
#pragma TCS_unroll=0
#endif
		filterbank_compute_bank32(st->bank, e_noise, e_noise+N);
   
	} else 
	{	memset(st->echo_noise, 0, (NM) * sizeof(spx_word32_t));
	}
}

void preprocess_update_noise(
	SpeexPreprocessState * restrict st,
	spx_word32_t * restrict ps,
	int N
)
{
	register spx_word16_t beta, beta_1;
	register int * restrict up = st->update_prob;
	register spx_word32_t * restrict noise = st->noise;
	register int i;

	beta = MAX16(QCONST16(.03,15),DIV32_16(Q15_ONE,st->nb_adapt));
	beta_1 = Q15_ONE-beta;

#if (TM_UNROLL && TM_UNROLL_SPEEXPREPROCESSRUN)
#pragma TCS_unroll=4
#pragma TCS_unrollexact=1
#endif
	for (i=0;i<N;i++)
	{	register spx_word32_t ni = noise[i];
		register spx_word32_t psi = ps[i];

		if ( !up[i] || psi < PSHR32(ni, NOISE_SHIFT) )
		{	noise[i] =	MAX32(EXTEND32(0),MULT16_32_Q15(beta_1,ni) + 
						MULT16_32_Q15(beta,SHL32(psi,NOISE_SHIFT)));
		}
	}
#if (TM_UNROLL && TM_UNROLL_SPEEXPREPROCESSRUN)
#pragma TCS_unrollexact=0
#pragma TCS_unroll=0
#endif
	filterbank_compute_bank32(st->bank, noise, noise+N);
}

void preprocess_compute_SNR(
	SpeexPreprocessState * restrict st,
	spx_word32_t * restrict ps,
	int NM
)
{
	register spx_word32_t * restrict noise = st->noise;
	register spx_word32_t * restrict echo = st->echo_noise;
	register spx_word32_t * restrict reverb = st->reverb_estimate;
	register spx_word16_t * restrict post = st->post;
	register spx_word32_t * restrict old_ps = st->old_ps;
	register spx_word16_t * restrict prior = st->prior;
	register int i;

#if (TM_UNROLL && TM_UNROLL_SPEEXPREPROCESSRUN)
#pragma TCS_unroll=4
#pragma TCS_unrollexact=1
#endif
	for ( i=0 ; i<NM ; i++)
	{
		register spx_word16_t gamma;
		register spx_word32_t tot_noise;
		register spx_word16_t posti;
		register spx_word32_t opsi;
		register spx_word16_t priori;

		tot_noise = ADD32(ADD32(ADD32(EXTEND32(1), PSHR32(noise[i],NOISE_SHIFT)), echo[i]) , reverb[i]);
      
		posti = SUB16(DIV32_16_Q8(ps[i],tot_noise), QCONST16(1.f,SNR_SHIFT));
		posti = MIN16(posti, QCONST16(100.f,SNR_SHIFT));
		post[i] = posti;

		opsi = old_ps[i];

		gamma = QCONST16(.1f,15)+MULT16_16_Q15(QCONST16(.89f,15),SQR16_Q15(DIV32_16_Q15(opsi,ADD32(opsi,tot_noise))));
      
		priori = EXTRACT16(PSHR32(ADD32(MULT16_16(gamma,MAX16(0,posti)), MULT16_16(Q15_ONE-gamma,DIV32_16_Q8(opsi,tot_noise))), 15));
		prior[i]=MIN16(priori, QCONST16(100.f,SNR_SHIFT));
	}
#if (TM_UNROLL && TM_UNROLL_SPEEXPREPROCESSRUN)
#pragma TCS_unrollexact=0
#pragma TCS_unroll=0
#endif
}

spx_word32_t preprocess_smooth_SNR(
	SpeexPreprocessState * restrict st,
	int N,
	int NM
)
{
	register spx_word16_t * restrict zeta = st->zeta;
	register spx_word16_t * restrict prior = st->prior;
	register spx_word32_t Zframe;
	register spx_word16_t iprior, priori;
	register int _N = N-1;
	register int i;

	iprior = prior[0];
	priori = prior[1];
	zeta[0] = PSHR32(ADD32(MULT16_16(QCONST16(.7f,15),zeta[0]), MULT16_16(QCONST16(.3f,15),iprior)),15);
   
#if (TM_UNROLL && TM_UNROLL_SPEEXPREPROCESSRUN)
#pragma TCS_unroll=2
#pragma TCS_unrollexact=1
#endif
	for ( i=1 ; i<_N ; i++)
	{	register spx_word16_t zetai = zeta[i];
		register spx_word16_t priorii = prior[i+1];

		zeta[i] = PSHR32(ADD32(ADD32(ADD32(MULT16_16(QCONST16(.7f,15),zetai), MULT16_16(QCONST16(.15f,15),priori)),
                  MULT16_16(QCONST16(.075f,15),iprior)), MULT16_16(QCONST16(.075f,15),priorii)),15);

		iprior = priori;
		priori = priorii;
	}
#if (TM_UNROLL && TM_UNROLL_SPEEXPREPROCESSRUN)
#pragma TCS_unrollexact=0
#pragma TCS_unroll=0
#endif

	for (i=_N; i<NM ; i++)
	{	register spx_word16_t zetai = zeta[i];
		
		priori = prior[i];
		zeta[i] = PSHR32(ADD32(MULT16_16(QCONST16(.7f,15),zetai), MULT16_16(QCONST16(.3f,15),priori)),15);
	}

	Zframe = 0;
   
#if (TM_UNROLL && TM_UNROLL_SPEEXPREPROCESSRUN)
#pragma TCS_unroll=4
#pragma TCS_unrollexact=1
#endif
	for ( i=N ; i<NM ; i++ )
	{	Zframe = ADD32(Zframe, EXTEND32(zeta[i]));
	}
#if (TM_UNROLL && TM_UNROLL_SPEEXPREPROCESSRUN)
#pragma TCS_unrollexact=0
#pragma TCS_unroll=0
#endif

	return Zframe;
}

void preprocess_compute_emgain(
	SpeexPreprocessState * restrict st,
	spx_word32_t * restrict ps,
	spx_word16_t Pframe,
	int NM
)
{
	register spx_word16_t * restrict zeta = st->zeta;
	register spx_word16_t * restrict prior = st->prior;
	register spx_word16_t * restrict gain = st->gain;
	register spx_word32_t * restrict old_ps = st->old_ps;
	register spx_word16_t * restrict post = st->post;
	register spx_word16_t * restrict gain2 = st->gain2;
	register int i;
	register int N=st->ps_size;
	
	for ( i=N ; i<NM ; ++i )
	{
		register spx_word32_t theta;
		register spx_word32_t MM;
		register spx_word16_t prior_ratio;
		register spx_word16_t P1;
		register spx_word16_t q;

#ifdef FIXED_POINT
		register spx_word16_t tmp;
#endif
		register spx_word16_t priori = prior[i];
		
		prior_ratio = PDIV32_16(SHL32(EXTEND32(priori), 15), ADD16(priori, SHL32(1,SNR_SHIFT)));
		theta = MULT16_32_P15(prior_ratio, QCONST32(1.f,EXPIN_SHIFT)+SHL32(EXTEND32(post[i]),EXPIN_SHIFT-SNR_SHIFT));

		MM = hypergeom_gain(theta);
		gain[i] = EXTRACT16(MIN32(Q15_ONE, MULT16_32_Q15(prior_ratio, MM)));
		old_ps[i] = MULT16_32_P15(QCONST16(.2f,15),old_ps[i]) + MULT16_32_P15(MULT16_16_P15(QCONST16(.8f,15),SQR16_Q15(gain[i])),ps[i]);

		P1 = QCONST16(.199f,15)+MULT16_16_Q15(QCONST16(.8f,15),qcurve (zeta[i]));
		q = Q15_ONE-MULT16_16_Q15(Pframe,P1);

#ifdef FIXED_POINT
		theta = MIN32(theta, EXTEND32(32767));
		tmp = MULT16_16_Q15((SHL32(1,SNR_SHIFT)+priori),EXTRACT16(MIN32(Q15ONE,SHR32(spx_exp(-EXTRACT16(theta)),1))));
		tmp = MIN16(QCONST16(3.,SNR_SHIFT), tmp);
		tmp = EXTRACT16(PSHR32(MULT16_16(PDIV32_16(SHL32(EXTEND32(q),8),(Q15_ONE-q)),tmp),8));
		gain2[i]=DIV32_16(SHL32(EXTEND32(32767),SNR_SHIFT), ADD16(256,tmp));
#else
		gain2[i]=1/(1.f + (q/(1.f-q))*(1+priori)*exp(-theta));
#endif
	}

	filterbank_compute_psd16(st->bank,gain2+N, gain2);
	filterbank_compute_psd16(st->bank,gain+N, gain);
}

void preprocess_compute_linear_gain(
	SpeexPreprocessState * restrict st,
	spx_word32_t * restrict ps,	
	int N
)
{
	register spx_word16_t * restrict gain_floor = st->gain_floor;
	register spx_word16_t * restrict prior = st->prior;
	register spx_word16_t * restrict gain = st->gain;
	register spx_word32_t * restrict old_ps = st->old_ps;
	register spx_word16_t * restrict post = st->post;
	register spx_word16_t * restrict gain2 = st->gain2;
	register int i;

	filterbank_compute_psd16(st->bank,gain_floor+N,gain_floor);

#if (TM_UNROLL && TM_UNROLL_SPEEXPREPROCESSRUN)
#pragma TCS_unroll=4
#pragma TCS_unrollexact=1
#endif   
	for (i=0;i<N;i++)
    {
         register spx_word32_t MM;
         register spx_word32_t theta;
         register spx_word16_t prior_ratio;
         register spx_word16_t tmp;
         register spx_word16_t p;
         register spx_word16_t g;
		 register spx_word16_t gfi = gain_floor[i];

		 prior_ratio = PDIV32_16(SHL32(EXTEND32(st->prior[i]), 15), ADD16(prior[i], SHL32(1,SNR_SHIFT)));
         theta = MULT16_32_P15(prior_ratio, QCONST32(1.f,EXPIN_SHIFT)+SHL32(EXTEND32(post[i]),EXPIN_SHIFT-SNR_SHIFT));
		 MM = hypergeom_gain(theta);

		 g = EXTRACT16(MIN32(Q15_ONE, MULT16_32_Q15(prior_ratio, MM)));
		 p = gain2[i];
         
		 g = VMUX( MULT16_16_Q15(QCONST16(.333f,15),g) > gain[i], MULT16_16(3,gain[i]), g);

         old_ps[i]= MULT16_32_P15(QCONST16(.2f,15),old_ps[i]) + 
					MULT16_32_P15(MULT16_16_P15(QCONST16(.8f,15),SQR16_Q15(g)),ps[i]);
         
		 g = VMUX( g < gfi, gfi, g );
         gain[i] = g;

         tmp =	MULT16_16_P15(p,spx_sqrt(SHL32(EXTEND32(g),15))) + 
				MULT16_16_P15(SUB16(Q15_ONE,p),spx_sqrt(SHL32(EXTEND32(gfi),15)));

         gain2[i]=SQR16_Q15(tmp);

         /* Use this if you want a log-domain MMSE estimator instead */
         /* gain2[i] = pow(g, p) * pow(gfi,1.f-p);*/
    }
#if (TM_UNROLL && TM_UNROLL_SPEEXPREPROCESSRUN)
#pragma TCS_unrollexact=0
#pragma TCS_unroll=0
#endif
}


#if 0
void preprocess_compute_bark_gain(
	SpeexPreprocessState * restrict st,
	int N,
	int NM
)
{
	register spx_word16_t * restrict gain_floor = st->gain_floor;
	register spx_word16_t * restrict gain = st->gain;
	register spx_word16_t * restrict gain2 = st->gain2;
	register int i;

    for (i=N;i<NM;i++)
    {
		register spx_word16_t tmp;
		register spx_word16_t p = gain2[i];
		register spx_word16_t gaini;
		register spx_word16_t gfi = gain_floor[i];

		gaini = MAX16(gain[i], gfi); 

		gain[i] = gaini;

		tmp =	MULT16_16_P15(p,spx_sqrt(SHL32(EXTEND32(gaini),15))) + 
			MULT16_16_P15(SUB16(Q15_ONE,p),spx_sqrt(SHL32(EXTEND32(gfi),15)));

		gain2[i]=SQR16_Q15(tmp);
    }

    filterbank_compute_psd16(st->bank,gain2+N, gain2);
}
#endif

void preprocess_apply_gain(
	SpeexPreprocessState * restrict st,
	int N
)
{
	register spx_word16_t * restrict ft = st->ft;
	register spx_word16_t * restrict gain2 = st->gain2;
	register int j, i;

	ft[0] = MULT16_16_P15(gain2[0],ft[0]);

	for (i=1,j=1; i<N ; i++,j+=2)
	{
		register spx_word16_t gain2i = gain2[i];
		register spx_word16_t ftj = ft[j];
		register spx_word16_t ftjj = ft[j+1];

		ft[j] = MULT16_16_P15(gain2i,ftj);
		ft[j+1] = MULT16_16_P15(gain2i,ftjj);
	}

	ft[(N<<1)-1] = MULT16_16_P15(gain2[N-1],ft[(N<<1)-1]);
}

#ifdef FIXED_POINT
void preprocess_scale(
	SpeexPreprocessState * restrict st,
	int N
)
{
	register spx_word16_t * restrict frame = st->frame;
	register int shift = st->frame_shift;
	register int i;
	register int N2 = N << 1;

#if (TM_UNROLL && TM_UNROLL_SPEEXPREPROCESSRUN)
#pragma TCS_unroll=4
#pragma TCS_unrollexact=1
#endif    
	for ( i=0 ; i<N2 ;i++)
	{	register spx_word16_t framei = frame[i];
		
		frame[i] = PSHR16(framei,shift);
	}
#if (TM_UNROLL && TM_UNROLL_SPEEXPREPROCESSRUN)
#pragma TCS_unrollexact=0
#pragma TCS_unroll=0
#endif
}

#else

void preprocess_apply_agc(
	SpeexPreprocessState * restrict st,
	int N				  
)
{				  
	register spx_word16_t max_sample=0;
	register spx_word16_t * restrict frame = st->frame;
	register int i;
	register int N2 = N << 1;

#if (TM_UNROLL && TM_UNROLL_SPEEXPREPROCESSRUN)
#pragma TCS_unroll=4
#pragma TCS_unrollexact=1
#endif    
	for (i=0;i<N2;i++)
	{	register spx_word16_t framei = VABS(frame[i]);

		max_sample = VMUX( framei > max_sample, framei, max_sample);
	}
#if (TM_UNROLL && TM_UNROLL_SPEEXPREPROCESSRUN)
#pragma TCS_unrollexact=0
#pragma TCS_unroll=0
#endif    

	if ( max_sample > 28000.f )
	{
		float damp = 28000.f/max_sample;

#if (TM_UNROLL && TM_UNROLL_SPEEXPREPROCESSRUN)
#pragma TCS_unroll=4
#pragma TCS_unrollexact=1
#endif  
		for ( i=0 ; i< N2 ; i++ )
		{	frame[i] *= damp;
		}
#if (TM_UNROLL && TM_UNROLL_SPEEXPREPROCESSRUN)
#pragma TCS_unrollexact=0
#pragma TCS_unroll=0
#endif  
	}
}
#endif


void preprocess_update(
	SpeexPreprocessState * restrict st,
	spx_int16_t * restrict x,
	int N	
)
{
	register spx_word16_t * restrict frame = st->frame;
	register spx_word16_t * restrict window = st->window;
	register spx_word16_t * restrict outbuf = st->outbuf;
	register int framesize = st->frame_size;
	register int N2 = N << 1;
	register int N3 = N2 - framesize;
	register int N4 = (framesize) - N3;
	register int i;

#if (TM_UNROLL && TM_UNROLL_SPEEXPREPROCESSRUN)
#pragma TCS_unroll=4
#pragma TCS_unrollexact=1
#endif  
	for ( i=0 ; i<N2 ; i++)
	{	register spx_word16_t fi = frame[i];
		register spx_word16_t wi = window[i];

		frame[i] = MULT16_16_Q15(fi, wi);
	}
	for (i=0;i<N3;i++)
	{	x[i] = outbuf[i] + frame[i];
	}
#if (TM_UNROLL && TM_UNROLL_SPEEXPREPROCESSRUN)
#pragma TCS_unrollexact=0
#pragma TCS_unroll=0
#endif 

	for ( i=0;i<N4;i++)
	{	x[N3+i] = frame[N3+i];
	}
	
	memcpy(outbuf, frame+framesize, (N3) * sizeof(spx_word16_t));
}

#define OVERRIDE_SPEEX_PREPROCESS_RUN
int speex_preprocess_run(SpeexPreprocessState * restrict st, spx_int16_t * restrict x)
{
	register int i, N, M, NM;
	register spx_word32_t * restrict ps=st->ps;
	register spx_word32_t Zframe;
	register spx_word16_t Pframe;

	st->nb_adapt++;
	st->min_count++;
	N = st->ps_size;
	M = st->nbands;
	NM = N + M;

	preprocess_residue_echo(st, N, NM);
	preprocess_analysis(st, x);
	update_noise_prob(st);
	preprocess_update_noise(st, ps, N);

	if ( st->nb_adapt == 1 )
	{	memcpy(st->old_ps, ps, (NM) * sizeof(spx_word32_t));
	}

	preprocess_compute_SNR(st, ps, NM);
	Zframe = preprocess_smooth_SNR(st, N, NM);
	

	{
	register spx_word16_t effective_echo_suppress;
	
	Pframe = QCONST16(.1f,15)+MULT16_16_Q15(QCONST16(.899f,15),qcurve(DIV32_16(Zframe,M)));
	effective_echo_suppress =	EXTRACT16(PSHR32(ADD32(MULT16_16(SUB16(Q15_ONE,Pframe), st->echo_suppress), 
								MULT16_16(Pframe, st->echo_suppress_active)),15));
	compute_gain_floor(st->noise_suppress, effective_echo_suppress, st->noise+N, st->echo_noise+N, st->gain_floor+N, M);
    
	}

	preprocess_compute_emgain(st, ps, Pframe, NM);
	preprocess_compute_linear_gain(st, ps, N);

   
	if (!st->denoise_enabled)
	{
	   register spx_word16_t * restrict gain2 = st->gain2;

#if (TM_UNROLL && TM_UNROLL_SPEEXPREPROCESSRUN)
#pragma TCS_unroll=4
#pragma TCS_unrollexact=1
#endif  
		for ( i=0 ; i<NM ; i++ )
		{   gain2[i] = Q15_ONE;
		}
#if (TM_UNROLL && TM_UNROLL_SPEEXPREPROCESSRUN)
#pragma TCS_unrollexact=0
#pragma TCS_unroll=0
#endif
	}
     
	preprocess_apply_gain(st, N);

#ifndef FIXED_POINT
	if (st->agc_enabled)
	{	speex_compute_agc(st, Pframe, st->ft);
	}
#endif


   spx_ifft(st->fft_lookup, st->ft, st->frame);

#ifdef FIXED_POINT
	preprocess_scale(st, N);
#endif

#ifndef FIXED_POINT
	if ( st->agc_enabled )
	{	preprocess_apply_agc(st, N);
	}
#endif

	preprocess_update(st, x, N);

	if ( st->vad_enabled )
	{
		if (Pframe > st->speech_prob_start || (st->was_speech && Pframe > st->speech_prob_continue))
		{	st->was_speech=1;
			return 1;
	  
		} else
		{	st->was_speech=0;
			return 0;
		}
	} else 
	{	return 1;
	}
}