preprocess_tm.h

一个开源的sip源代码

/* Copyright (C) 2007 Hong Zhiqian */
/**
   @file preprocess_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"

#ifdef FIXED_POINT
#define OVERRIDE_PREPROCESS_ANALYSIS
static void preprocess_analysis(SpeexPreprocessState * restrict st, spx_int16_t * restrict x)
{
	register int i, j, framesize = st->frame_size;
	register int N = st->ps_size;
	register int N3 = 2*N - framesize;
	register int N4 = framesize - N3;
	register int * restrict ps = st->ps;
	register int * restrict frame;
	register int * restrict inbuf;
	register int * restrict ptr;
	register int max_val;

	frame = (int*)(st->frame);
	inbuf = (int*)(st->inbuf);
	ptr = (int*)(st->frame+N3);

	TMDEBUG_ALIGNMEM(x);
   	TMDEBUG_ALIGNMEM(frame);
	TMDEBUG_ALIGNMEM(inbuf);

	PREPROCESSANAYLSIS_START();

	N3 >>= 1;
	framesize >>= 1;
	max_val = 0;
	
	for ( i=0,j=0 ; i<N3 ; i+=2,j+=8 )
	{	register int r1, r2;

		r1 = ld32x(inbuf,i);
		r2 = ld32x(inbuf,i+1);

		st32d(j,   frame, r1);
		st32d(j+4, frame, r2);
	}

	for ( i=0,j=0 ; i<framesize ; i+=2,j+=8 )
	{	register int r1, r2;

		r1 = ld32x(x, i);
		r2 = ld32x(x, i+1);

		st32d(j,  ptr, r1);
		st32d(j+4,ptr, r2);
	}
   
	for ( i=0,j=0,ptr=(int*)(x+N4) ; i<N3 ; i+=2,j+=8 )
	{	register int r1, r2;

		r1 = ld32x(ptr, i);
		r2 = ld32x(ptr, i+1);

		st32d(j,  inbuf, r1);
		st32d(j+4,inbuf, r2);
	}
#if (TM_UNROLL && TM_UNROLL_PREPROCESSANALYSIS)
#pragma TCS_unroll=2
#pragma TCS_unrollexact=1
#endif
   for ( i=0,j=0,ptr=(int*)st->window ; i<N ; ++i,j+=4 )
   {	register int f10, w10, r0, r1;

		f10 = ld32x(frame, i);
		w10 = ld32x(ptr,   i);

		r0  = (sex16(f10) * sex16(w10)) >> 15;
		r1	= (asri(16,f10) * asri(16,w10)) >> 15;

		max_val = imax(iabs(sex16(r0)), max_val);
		max_val = imax(iabs(sex16(r1)), max_val);

		r0	= pack16lsb(r1, r0);
		st32d(j, frame, r0);
	}
#if (TM_UNROLL && TM_UNROLL_PREPROCESSANALYSIS)
#pragma TCS_unrollexact=0
#pragma TCS_unroll=0
#endif
   
	max_val = 14 - spx_ilog2(max_val);
	st->frame_shift = max_val;

	if ( max_val != 0 )
	{
#if (TM_UNROLL && TM_UNROLL_PREPROCESSANALYSIS)
#pragma TCS_unroll=4
#pragma TCS_unrollexact=1
#endif
		for ( i=0,j=0 ; i<N ; ++i,j+=4 )
		{	register int f10;

			f10 = ld32x(frame, i);
			f10 = dualasl(f10, max_val);
			st32d(j, frame, f10);

		}
#if (TM_UNROLL && TM_UNROLL_PREPROCESSANALYSIS)
#pragma TCS_unrollexact=0
#pragma TCS_unroll=0
#endif
	}

	spx_fft(st->fft_lookup, st->frame, st->ft);     
	power_spectrum(st->ft, ps, N << 1);
   
#if (TM_UNROLL && TM_UNROLL_PREPROCESSANALYSIS)
#pragma TCS_unroll=4
#pragma TCS_unrollexact=1
#endif
	for ( i=0,ptr=(int*)st->ps,max_val<<=1,j=((1<<((max_val))>>1)) ;i<N ; ++i )
	{	
		ps[i] = (ps[i] + j) >> max_val;
	}
#if (TM_UNROLL && TM_UNROLL_PREPROCESSANALYSIS)
#pragma TCS_unrollexact=0
#pragma TCS_unroll=0
#endif

	filterbank_compute_bank32(st->bank, ps, ps+N);

	PREPROCESSANAYLSIS_STOP();
}

#define _MULT16_32_Q15(a,b,c) ADD32(MULT16_16((a),(b)), SHR(MULT16_16((a),(c)),15))

#define OVERRIDE_UPDATE_NOISE_PROB
static void update_noise_prob(SpeexPreprocessState * restrict st)
{
	register int i;
	register int min_range, nb_adapt;
	register int N = st->ps_size;
	register int * restrict Smin = (int*)st->Smin;
	register int * restrict Stmp = (int*)st->Stmp;
	register int * restrict S = (int*)st->S;
	
	UPDATENOISEPROB_START();

	{
		register int psi_lsb, psi_msb, ips_lsb, ips_msb, psii_lsb, psii_msb;
		register int psiii_lsb, psiii_msb;
		register int q8, q05, q2, q1;
		register int *ps = (int*)st->ps;
		register int si_lsb, si_msb, sii_lsb, sii_msb;
	
		q8	= QCONST16(.8f,15);
		q05 = QCONST16(.05f,15);
		q2	= QCONST16(.2f,15);
		q1	= QCONST16(.1f,15);

		ips_lsb	 = ps[0];
		psi_lsb	 = ps[1];
		si_lsb	 = S[0];
		ips_msb	 = ips_lsb >> 15;
		psi_msb	 = psi_lsb >> 15;
		si_msb	 = si_lsb >> 15;

		ips_lsb	 &= 0x00007fff;
		psi_lsb	 &= 0x00007fff;
		si_lsb	 &= 0x00007fff;
		
		S[0] = _MULT16_32_Q15(q8,si_msb,si_lsb) +  _MULT16_32_Q15(q2,ips_msb,ips_lsb);

		for ( i=1 ; i<N-1 ; i+=2 )
		{
			psii_lsb = ps[i+1];
			si_lsb	 = S[i];

			psii_msb = psii_lsb >> 15;
			si_msb	 = si_lsb >> 15;
			si_lsb	 &= 0x00007fff;
			psii_lsb &= 0x00007fff;

			S[i]= _MULT16_32_Q15(q8,si_msb,si_lsb) + 
				  _MULT16_32_Q15(q05,ips_msb,ips_lsb) + 
				  _MULT16_32_Q15(q1,psi_msb,psi_lsb) + 
				  _MULT16_32_Q15(q05,psii_msb,psii_lsb);
			
			psiii_lsb = ps[i+2];
			sii_lsb	 = S[i+1];

			sii_msb	 =  sii_lsb >> 15;
			psiii_msb=  psiii_lsb >> 15;
			sii_lsb	 &= 0x00007fff;
			psiii_lsb&= 0x00007fff;

			S[i+1]= _MULT16_32_Q15(q8,sii_msb,sii_lsb) + 
					_MULT16_32_Q15(q05,psi_msb,psi_lsb) + 
					_MULT16_32_Q15(q1,psii_msb,psii_lsb) + 
					_MULT16_32_Q15(q05,psiii_msb,psiii_lsb);

			ips_lsb = psii_lsb;
			ips_msb = psii_msb;
			psi_lsb = psiii_lsb;
			psi_msb = psiii_msb;
		}
   
		S[N-1] = MULT16_32_Q15(q8,S[N-1]) + MULT16_32_Q15(q2,ps[N-1]);
	}

	nb_adapt = st->nb_adapt;
   
	if ( nb_adapt==1 )
	{	for ( i=0 ; i<N ; ++i )
			Smin[i] = Stmp[i] = 0;

	}

	min_range = mux(nb_adapt < 100,		15,
				mux(nb_adapt < 1000,	50,
				mux(nb_adapt < 10000,	150, 300)));

	if ( st->min_count > min_range )
	{
		st->min_count = 0;

#if (TM_UNROLL && TM_UNROLL_UPDATENOISEPROB)
#pragma TCS_unroll=2
#pragma TCS_unrollexact=1
#endif
		for ( i=0 ; i<N ; ++i )
		{	register int si, stmpi;

			si		= S[i];
			stmpi	= Stmp[i];
				
			Smin[i] = imin(stmpi,si);
			Stmp[i] = si;
		}
#if (TM_UNROLL && TM_UNROLL_UPDATENOISEPROB)
#pragma TCS_unrollexact=0
#pragma TCS_unroll=0
#endif
	} else 
	{

#if (TM_UNROLL && TM_UNROLL_UPDATENOISEPROB)
#pragma TCS_unroll=2
#pragma TCS_unrollexact=1
#endif
		for ( i=0 ; i<N ; ++i )
		{	register int si, stmpi, smini;

			si		= S[i];
			stmpi	= Stmp[i];
			smini	= Smin[i];

			Smin[i] = imin(smini,si);
			Stmp[i] = imin(stmpi,si);      
		}
#if (TM_UNROLL && TM_UNROLL_UPDATENOISEPROB)
#pragma TCS_unrollexact=0
#pragma TCS_unroll=0
#endif
	}


	{
		register int q4;
		register int * restrict update_prob = (int*)st->update_prob;

		q4 = QCONST16(.4f,15);

#if (TM_UNROLL && TM_UNROLL_UPDATENOISEPROB)
#pragma TCS_unroll=4
#pragma TCS_unrollexact=1
#endif
		for ( i=0 ; i<N ; ++i )
		{	register int si;
			register int smini;

			si = S[i];
			smini = Smin[i];
			update_prob[i] = mux(MULT16_32_Q15(q4,si) > ADD32(smini,20), 1, 0);
		}
#if (TM_UNROLL && TM_UNROLL_UPDATENOISEPROB)
#pragma TCS_unrollexact=0
#pragma TCS_unroll=0
#endif
	}

	UPDATENOISEPROB_STOP();
}

#else

#define OVERRIDE_PREPROCESS_ANALYSIS
static void preprocess_analysis(SpeexPreprocessState * restrict st, spx_int16_t * restrict x)
{
	register int i;
	register int framesize = st->frame_size;
	register int N = st->ps_size;
	register int N3 = 2*N - framesize;
	register int N4 = framesize - N3;
	register float * restrict ps = st->ps;
	register float * restrict frame = st->frame;
	register float * restrict inbuf = st->inbuf;

	PREPROCESSANAYLSIS_START();

#if (TM_UNROLL && TM_UNROLL_PREPROCESSANALYSIS)
#pragma TCS_unroll=4
#pragma TCS_unrollexact=1
#endif
	for ( i=0 ; i<N3 ; ++i )
	{
		frame[i] = inbuf[i];
	}
#if (TM_UNROLL && TM_UNROLL_PREPROCESSANALYSIS)
#pragma TCS_unrollexact=0
#pragma TCS_unroll=0
#endif

#if (TM_UNROLL && TM_UNROLL_PREPROCESSANALYSIS)
#pragma TCS_unroll=4
#pragma TCS_unrollexact=1
#endif
   	for ( i=0 ; i<framesize ; ++i )
	{	frame[N3+i] = x[i];
	}
#if (TM_UNROLL && TM_UNROLL_PREPROCESSANALYSIS)
#pragma TCS_unrollexact=0
#pragma TCS_unroll=0
#endif

#if (TM_UNROLL && TM_UNROLL_PREPROCESSANALYSIS)
#pragma TCS_unroll=4
#pragma TCS_unrollexact=1
#endif
	for ( i=0,x+=N4 ; i<N3 ; ++i )
	{	inbuf[i]  = x[i];
	}
#if (TM_UNROLL && TM_UNROLL_PREPROCESSANALYSIS)
#pragma TCS_unrollexact=0
#pragma TCS_unroll=0
#endif

	inbuf = st->window;

#if (TM_UNROLL && TM_UNROLL_PREPROCESSANALYSIS)
#pragma TCS_unroll=4
#pragma TCS_unrollexact=1
#endif
	for ( i=0 ; i<2*N ; ++i )
	{
		frame[i] = frame[i] * inbuf[i];
	}
#if (TM_UNROLL && TM_UNROLL_PREPROCESSANALYSIS)
#pragma TCS_unrollexact=0
#pragma TCS_unroll=0
#endif

	spx_fft(st->fft_lookup, frame, st->ft);         
	power_spectrum(st->ft, ps, N << 1);
	filterbank_compute_bank32(st->bank, ps, ps+N);

	PREPROCESSANAYLSIS_STOP();
}


#define OVERRIDE_UPDATE_NOISE_PROB
static void update_noise_prob(SpeexPreprocessState * restrict st)
{
   
	register float * restrict S = st->S;
   	register float * restrict ps = st->ps;
	register int N = st->ps_size;
    register int min_range;
	register int i;
	register int nb_adapt;
	register float * restrict Smin = st->Smin;
	register float * restrict Stmp = st->Stmp;

	UPDATENOISEPROB_START();

	{
		register float ips, psi;

		ips  = ps[0];
		psi  = ps[1];

		S[0] = .8f * S[0] + .2f * ips;

		for ( i=1 ; i<N-1 ; i+=2 )
		{
			register float psii, psiii;

			psii	= ps[i+1];
			psiii	= ps[i+2];
			S[i]	= .8f * S[i]	+ .05f * ips + .1f * psi  + .05f * psii;
			S[i+1]	= .8f * S[i+1]	+ .05f * psi + .1f * psii + .05f * psiii; 
			ips		= psii;
			psi		= psiii;
		}

		S[N-1] = .8f * S[N-1] + .2f * ps[N-1];
	}

	nb_adapt = st->nb_adapt;
   
   	if ( nb_adapt==1 )
	{	
		for (i=0;i<N;i++)
			Smin[i] = st->Stmp[i] = 0;
	}

	min_range = mux(nb_adapt < 100,		15,
				mux(nb_adapt < 1000,	50,
				mux(nb_adapt < 10000,	150, 300)));

   
	if ( st->min_count > min_range )
	{
		st->min_count = 0;
#if (TM_UNROLL && TM_UNROLL_UPDATENOISEPROB)
#pragma TCS_unroll=4
#pragma TCS_unrollexact=1
#endif
		for ( i=0 ; i<N ; ++i )
		{
			register float stmpi, si;

			stmpi	= Stmp[i];
			si		= S[i];

			Smin[i] = fmin(stmpi,si);
			Stmp[i] = si;
		}
#if (TM_UNROLL && TM_UNROLL_UPDATENOISEPROB)
#pragma TCS_unrollexact=0
#pragma TCS_unroll=0
#endif

	} else 
	{
		register float * restrict Smin = st->Smin;

#if (TM_UNROLL && TM_UNROLL_UPDATENOISEPROB)
#pragma TCS_unroll=4
#pragma TCS_unrollexact=1
#endif
		for ( i=0 ; i<N ; ++i )
		{
			register float stmpi, si, smini;

			stmpi	= Stmp[i];
			si		= S[i];
			smini	= Smin[i];
			
			Smin[i] = fmin(smini,si);
			Stmp[i] = fmin(stmpi,si);      
		}
#if (TM_UNROLL && TM_UNROLL_UPDATENOISEPROB)
#pragma TCS_unrollexact=0
#pragma TCS_unroll=0
#endif
	}

	{
		register int * restrict update_prob = (int*)st->update_prob;

#if (TM_UNROLL && TM_UNROLL_UPDATENOISEPROB)
#pragma TCS_unroll=4
#pragma TCS_unrollexact=1
#endif
		for (i=0;i<N;i++)
		{	register float si;
			register float smini;

			si		= S[i];
			smini	= Smin[i];
			update_prob[i] = mux( (.4 * si) > (smini + 20.f), 1, 0);
			
		}
#if (TM_UNROLL && TM_UNROLL_UPDATENOISEPROB)
#pragma TCS_unrollexact=0
#pragma TCS_unroll=0
#endif
	}

	UPDATENOISEPROB_STOP();
}


#define OVERRIDE_COMPUTE_GAIN_FLOOR
static void compute_gain_floor(
	int noise_suppress, 
	int effective_echo_suppress, 
	float * restrict noise, 
	float * restrict echo, 
	float * gain_floor, 
	int len
)
{
	register int i;
	register float echo_floor;
	register float noise_floor;

	COMPUTEGAINFLOOR_START();

	noise_floor = exp(.2302585f*noise_suppress);
	echo_floor = exp(.2302585f*effective_echo_suppress);

#if (TM_UNROLL && TM_UNROLL_COMPUTEGAINFLOOR)
#pragma TCS_unroll=4
#pragma TCS_unrollexact=1
#endif
	for (i=0;i<len;i++)
	{	register float noisei, echoi;

		noisei = noise[i];
		echoi  = echo[i];

		gain_floor[i] = FRAC_SCALING * sqrt(noise_floor * noisei +	echo_floor * echoi) / sqrt(1+noisei+echoi);

	}
#if (TM_UNROLL && TM_UNROLL_COMPUTEGAINFLOOR)
#pragma TCS_unrollexact=0
#pragma TCS_unroll=0
#endif

	COMPUTEGAINFLOOR_STOP();
}

#endif

static inline spx_word32_t hypergeom_gain(spx_word32_t xx);
static inline spx_word16_t qcurve(spx_word16_t x);
static void compute_gain_floor(int noise_suppress, int effective_echo_suppress, spx_word32_t *noise, spx_word32_t *echo, spx_word16_t *gain_floor, int len);
void speex_echo_get_residual(SpeexEchoState *st, spx_word32_t *Yout, int len);