g729a_lpc.c

DSP上优化的G.729A代码

#include "../Common/typedef.h"
#include "../Include/G729A_basic_op.h"
#include "../Include/G729A_oper_32b.h"
#include "../Include/G729A_ld8a.h"
#include "../Include/G729A_tab_ld8a.h"

void G729AAutocorr(Word16 x[], Word16 m, Word16 r_h[], Word16 r_l[])
{
    extern Flag G729AOverflow;
    
	Word16 i, j, norm;
	Word16 y[G729A_L_WINDOW];				
	Word32 sum;
	
	Word32 *ptmp0, *ptmp1, *ptmp2;
	Word32 temp0, temp1;	
	
	ptmp0 = (Word32 *)&x[0];
	ptmp1 = (Word32 *)&G729A_hamwindow[0];
	ptmp2 = (Word32 *)&y[0];
	for(i = G729A_L_WINDOW>>1; i >0 ; i--)
	{
	    temp0 = _smpy(*ptmp0, *ptmp1);
	    temp1 = _smpyh(*ptmp0++, *ptmp1++);
	    
	    temp0 = temp0 + 0x00008000L;
	    temp1 = temp1 + 0x00008000L;

	    *ptmp2++ = _packh2(temp1, temp0);
	}
	
	do {
		G729AOverflow = 0;
		sum = 1;                   
		for(i=0; i<G729A_L_WINDOW; i++)
			sum = G729AL_mac(sum, y[i], y[i]);		
		if(G729AOverflow != 0)
		{
			for(i=0; i<G729A_L_WINDOW; i++)
			{
				y[i] = G729Ashr_s(y[i], 2);
			}
		}
	}while (G729AOverflow != 0);
	
	norm = G729Anorm_l(sum);
	sum  = _sshvl(sum, norm);
	r_h[0] = sum >> 16;
	r_l[0] = (Word16)_ssub(_sshvr(sum, 1), _smpy(r_h[0], 16384));
	
	for (i = 1; i <= m; i++)
	{
		sum = 0;
		for(j = 0; j < G729A_L_WINDOW - i; j++)
			sum = _sadd(sum, _smpy(y[j], y[j+i]));
		
		sum = _sshvl(sum, norm);
		r_h[i] = sum >> 16;
	    r_l[i] = (Word16)_ssub(_sshvr(sum, 1), _smpy(r_h[i], 16384));
	}
	
	return;
}

void G729ALag_window(Word16 m, Word16 r_h[], Word16 r_l[])
{
	Word16 i;
	Word32 x;
	Word32 tmp0, tmp1, tmp2;
	for(i = 1; i <= m; i++)
	{
		tmp0 = _smpy(r_h[i], G729A_lag_l[i-1])>>16;
		tmp1 = _smpy(r_l[i], G729A_lag_h[i-1])>>16;
		x    = _smpy(r_h[i], G729A_lag_h[i-1]);
		tmp0 = _sadd(tmp0, tmp1);
		tmp2 = _smpy(r_h[i], 16384);
		x    = _sadd(x, tmp0);
		r_h[i] = x >> 16;
	    r_l[i] = (Word16)_ssub(_sshvr(x, 1), tmp2);
	}
	return;
}

static Word16 G729Aold_A[G729A_M+1]={4096,0,0,0,0,0,0,0,0,0,0};
static Word16 G729Aold_rc[2]={0,0};

void G729ALevinson(Word16 Rh[], Word16 Rl[], Word16 A[], Word16 rc[])
{
	Word16 i, j;
	Word16 hi, lo;
	Word16 Kh, Kl;                          
	Word16 alp_h, alp_l, alp_exp;         
	Word16 Ah[G729A_M+1], Al[G729A_M+1];                      
	Word16 Anh[G729A_M+1], Anl[G729A_M+1];   
	Word32 t0, t1, t2;          
	Word32 tmp0, tmp1, tmp2, tmp3;                         
	Word32 *pTemp0, *pTemp1;
	
	tmp0 = Rh[1]<<16;
	tmp1 = Rl[1]<<1;
	t1  = _sadd(tmp0, tmp1);
	t2  = _abs(t1);
	t0  = G729ADiv_32(t2, Rh[0], Rl[0]);
	t0= (t1 > 0) ? -t0 : t0;
	Kh = t0 >> 16;
	tmp2 = _sshvr(t0, 1);
	Kl = (Word16)_ssub(tmp2, _smpy(Kh, 16384));
	rc[0] = Kh;
	t0 = _sshvr(t0,4); 
	Ah[1] = t0 >> 16;
	tmp3 = _sshvr(t0, 1);
	Al[1] = (Word16)_ssub(tmp3, _smpy(Ah[1], 16384)); 
	 
	t0 = _smpy(Kh, Kh);
	tmp0 = _smpy(Kh, Kl)>>15;
	t0 = _sadd(t0, tmp0);
	
	t0 = _abs(t0);
	t0 = _ssub((Word32)0x7fffffffL, t0);
	
	hi = t0 >> 16;
	tmp1= _sshvr(t0, 1);
	lo = (Word16)_ssub(tmp1, _smpy(hi, 16384));
	
	t0 = _smpy(Rh[0], hi);
	tmp0 = _smpy(Rh[0], lo)>>16;
	tmp1 = _smpy(Rl[0], hi)>>16;
	tmp0 = _sadd(tmp0, tmp1);
	t0 = _sadd(t0, tmp0);
	
	alp_exp = G729Anorm_l(t0);
	t0 = _sshvl(t0, alp_exp);
	alp_h = t0 >> 16;
	tmp1= _sshvr(t0, 1);
	alp_l = (Word16)_ssub(tmp1, _smpy(alp_h, 16384)); 
	
	for(i= 2; i<=G729A_M; i++)
	{		
		t0 = 0;
	    for(j=1; j<i; j++)
	    {
	        tmp0 = _smpy(Rh[j], Ah[i-j]);
	        tmp1 = _smpy(Rh[j], Al[i-j])>>16;
	        tmp2 = _smpy(Rl[j], Ah[i-j])>>16;
	        tmp1 = _sadd(tmp1, tmp2);
	        tmp0 = _sadd(tmp0, tmp1);
	        t0 = _sadd(t0, tmp0);
	    }
		t0 = _sshvl(t0, 4);                  
		t1 = _sadd(Rh[i]<<16, Rh[i]<<1);
		t0 = _sadd(t0, t1);                		
		t1 = _abs(t0);
		t2 = G729ADiv_32(t1, alp_h, alp_l);                       
		t2 = (t0 > 0) ? -t2 : t2;                  
		t2 = _sshvl(t2, alp_exp);             
		Kh = t2 >> 16;
	    tmp1= _sshvr(t2, 1);
	    Kl = (Word16)_ssub(tmp1, _smpy(Kh, 16384));                      
		rc[i-1] = Kh;
				
		if (_sadd2(G729Aabs_s(Kh), -32750) > 0)
		{
			pTemp0 = (Word32 *)&A[0];
			pTemp1 = (Word32 *)&G729Aold_A[0];
			for(j = G729A_M>>1; j > 0; j--)
			    *pTemp0++ = *pTemp1++;
			rc[0] = G729Aold_rc[0];         
			rc[1] = G729Aold_rc[1];
			return;
		}
		
		for(j=1; j<i; j++)
		{
			t0 = _smpy(Kh, Ah[i-j]);
	        t0 = _sadd(t0, _smpy(Kh, Al[i-j])>>16);
	        t0 = _sadd(t0, _smpy(Kl, Ah[i-j])>>16);
			t0 = _sadd(t0, _sadd(Ah[j]<<16, Al[j]<<1));
			Anh[j] = t0 >> 16;
	        Anl[j] = (Word16)_ssub(_sshvr(t0, 1), _smpy(Anh[j], 16384));
		}
		t2 = _sshvr(t2, 4); 
		Anh[j] = t2 >> 16;
	    Anl[j] = (Word16)_ssub(_sshvr(t2, 1), _smpy(Anh[j], 16384));                     
		t0 = _smpy(Kh, Kh);
		tmp0 = _smpy(Kh, Kl)>>15;
	    t0 = _sadd(t0, tmp0);
		t0 = _abs(t0);                         
		t0 = _ssub((Word32)0x7fffffffL, t0 );  
		hi = t0 >> 16;
	    lo = (Word16)_ssub(_sshvr(t0, 1), _smpy(hi, 16384));               
		t0 = _smpy(alp_h, hi);
	    t0 = _sadd(t0, _smpy(_smpy(alp_h, lo)>>16, 1));
	    t0 = _sadd(t0, _smpy(_smpy(alp_l, hi)>>16, 1));   
		
		j = G729Anorm_l(t0);
		t0 = _sshvl(t0, j);
		alp_h = t0 >> 16;
	    alp_l = (Word16)_ssub(_sshvr(t0, 1), _smpy(alp_h, 16384));    
		alp_exp = _sadd2(alp_exp, j);
		
		for(j=1; j<=i; j++)
		{
			Ah[j] =Anh[j];
			Al[j] =Anl[j];
		}
	}
	
	A[0] = 4096;
	for(i=1; i<=G729A_M; i++)
	{
		t0   = _sadd(Ah[i]<<16, Al[i]<<1);
		G729Aold_A[i] = A[i] = (Word16)(_sadd(_sshvl(t0, 1),(Word32)0x00008000L)>>16);
	}
	G729Aold_rc[0] = rc[0];
	G729Aold_rc[1] = rc[1];
	
	return;
}

static Word16 G729AChebps_11(Word16 x, Word16 f[], Word16 n);
static Word16 G729AChebps_10(Word16 x, Word16 f[], Word16 n);

void G729AAz_lsp(Word16 a[],	Word16 lsp[], Word16 old_lsp[])
{
	Word16 i, j, nf, ip;
	Word16 xlow, ylow, xhigh, yhigh, xmid, ymid, xint;
	Word16 x, y, sign, exp;
	Word16 *coef;
	Word16 f1[G729A_M/2+1], f2[G729A_M/2+1];
	Word32 t0, t1, L_temp;
	Flag   ovf_coef;
	Word16 (*pChebps)(Word16 x, Word16 f[], Word16 n);
	Word32 *pTemp0, *pTemp1;

	ovf_coef = 0;
	pChebps = G729AChebps_11;
	
	f1[0] = 2048;          
	f2[0] = 2048;        
	
	for (i = 0; i< G729A_NC; i++)
	{
		G729AOverflow = 0;
		t0 = G729AL_mult(a[i+1], 16384);               
		t0 = G729AL_mac(t0, a[G729A_M-i], 16384);            
		x  = G729Aextract_h(t0);
		if ( G729AOverflow ) 
		{
			ovf_coef = 1;      
		}	
		G729AOverflow = 0;
		f1[i+1] = G729Asub_s(x, f1[i]);    
		if ( G729AOverflow ) 
		{
			ovf_coef = 1;      
		}		
		G729AOverflow = 0;
		t0 = G729AL_mult(a[i+1], 16384);              
		t0 = G729AL_msu(t0, a[G729A_M-i], 16384);                 
		x  = G729Aextract_h(t0);
		if ( G729AOverflow ) 
		{
			ovf_coef = 1;      
		}	
		G729AOverflow = 0;
		f2[i+1] = G729Aadd_s(x, f2[i]);    
		if ( G729AOverflow ) 
		{
			ovf_coef = 1;      
		}
	}	
	if ( ovf_coef ) 
	{		
		pChebps = G729AChebps_10;
		
		f1[0] = 1024;          
		f2[0] = 1024;          
		
		for (i = 0; i< G729A_NC; i++)
		{
			t0 = _sshvl(a[i+1], 0xe);
			t1 = _sshvl(a[G729A_M-i], 0xe);               
			x = _sadd(t0, t1) >> 0x10;           
			
			f1[i+1] = _sadd2(x, -f1[i]);           
			
			f2[i+1] = _sadd2(x, f2[i]);           
		}
	}
	
	nf=0;          
	ip=0;            
	
	coef = f1;
	
	xlow = G729A_grid[0];
	ylow = (*pChebps)(xlow, coef, G729A_NC);
	
	j = 0;
	while ( (nf < G729A_M) && (j < G729A_GRID_POINTS) )
	{
		j = _sadd2(j,1);
		xhigh = xlow;
		yhigh = ylow;
		xlow  = G729A_grid[j];
		ylow  = (*pChebps)(xlow,coef,G729A_NC);
		
		L_temp = G729AL_mult(ylow ,yhigh);
		if ( L_temp <= (Word32)0)
		{			
			for (i = 0; i < 2; i++)
			{
				t0 = _sshvr(xlow, 1);
				t1 = _sshvr(xhigh, 1);
				t0 = _sadd2(t0, t1);
				xmid = _ext(t0, 16, 16);
				ymid = (*pChebps)(xmid, coef, G729A_NC);
				
				L_temp = G729AL_mult(ylow,ymid);
				if ( L_temp <= (Word32)0)
				{
					yhigh = ymid;
					xhigh = xmid;
				}
				else
				{
					ylow = ymid;
					xlow = xmid;
				}
			}			
			x   = G729Asub_s(xhigh, xlow);
			y   = G729Asub_s(yhigh, ylow);
			if(y == 0)
			{
				xint = xlow;
			}
			else
			{
				sign= y;
				y   = G729Aabs_s(y);
				exp = G729Anorm_s(y);
				y   = G729Ashl_s(y, exp);
				y   = G729Adiv_s( (Word16)16383, y);
				t0  = G729AL_mult(x, y);
				t0  = G729AL_shr(t0, G729Asub_s(20, exp) );
				y   = G729Aextract_l(t0);            
				
				if(sign < 0) y = G729Anegate(y);
				
				t0   = G729AL_mult(ylow, y);                
				t0   = G729AL_shr(t0, 11);                   
				xint = G729Asub_s(xlow, G729Aextract_l(t0));        
			}
			
			lsp[nf] = xint;
			xlow    = xint;
			nf =G729Aadd_s(nf,1);
			
			if(ip == 0)
			{
				ip = 1;
				coef = f2;
			}
			else
			{
				ip = 0;
				coef = f1;
			}
		
			ylow = (*pChebps)(xlow,coef,G729A_NC);
			
		}
	}
	if( _ssub(nf, G729A_M) < 0)
	{
	    pTemp0 = (Word32 *)&lsp[0];
		pTemp1 = (Word32 *)&old_lsp[0];
		for(i=G729A_M>>1; i>0; i--)
		    *pTemp0++ = *pTemp1++;
    }
	return;
}

static Word16 G729AChebps_11(Word16 x, Word16 f[], Word16 n)
{
	Word16 i, cheb;
	Word16 b0_h, b0_l, b1_h, b1_l, b2_h, b2_l;
	Word32 t0;
	
	b2_h = 256;                          
	b2_l = 0;
	t0 = _smpy(x, 512);                         
	t0 = _sadd(t0, _smpy(f[1], 4096));                
	b1_h = t0 >> 16;
	b1_l = (Word16)_ssub(_sshvr(t0, 1), _smpy(b1_h, 16384));
	
	for (i = 2; i<n; i++)
	{
		t0 = _sadd(_smpy(b1_h, x), _smpy(_smpy(b1_l, x)>>16, 1));            
		t0 = _sshl(t0, 1);
		t0 = _sadd(t0, _smpy(b2_h,(Word16)-32768L));        
		t0 = _ssub(t0, _smpy(b2_l, 1));
		t0 = _sadd(t0, _smpy(f[i], 4096));         
		
		b0_h = t0 >> 16;
	    b0_l = (Word16)_ssub(_sshvr(t0, 1), _smpy(b0_h, 16384));       
		
		b2_l = b1_l;                        
		b2_h = b1_h;
		b1_l = b0_l;                        
		b1_h = b0_h;
	}
	t0 = _sadd(_smpy(b1_h, x), _smpy(_smpy(b1_l, x)>>16, 1));         
	t0 = _sadd(t0, _smpy(b2_h,(Word16)-32768L));          
	t0 = _ssub(t0, _smpy(b2_l, 1));
	t0 = _sadd(t0, _smpy(f[i], 2048));           
	
	t0 = _sshl(t0, 6);                    
	cheb = t0>>16; 
	
	
	return(cheb);
}


static Word16 G729AChebps_10(Word16 x, Word16 f[], Word16 n)
{
	Word16 i, cheb;
	Word16 b0_h, b0_l, b1_h, b1_l, b2_h, b2_l;
	Word32 t0;
	
	b2_h = 128;                          
	b2_l = 0;
	t0 = _smpy(x, 256);                           
	t0 = _sadd(t0, _smpy(f[1], 4096));                 
	b1_h = t0 >> 16;
	b1_l = (Word16)_ssub(_sshvr(t0, 1), _smpy(b1_h, 16384)); 
	t0 = _sadd(_smpy(b1_h, x), _smpy(_smpy(b1_l, x)>>16, 1));
	for (i = 2; i<n; i++)
	{
		t0 = _sadd(_smpy(b1_h, x), _smpy(_smpy(b1_l, x)>>16, 1));            
		t0 = _sshl(t0, 1);
		t0 = _sadd(t0, _smpy(b2_h,(Word16)-32768L));        
		t0 = _ssub(t0, _smpy(b2_l, 1));
		t0 = _sadd(t0, _smpy(f[i], 4096));         
		
		b0_h = t0 >> 16;
	    b0_l = (Word16)_ssub(_sshvr(t0, 1), _smpy(b0_h, 16384));  
		
		b2_l = b1_l;                        
		b2_h = b1_h;
		b1_l = b0_l;                        
		b1_h = b0_h;
	}
	t0 = _sadd(_smpy(b1_h, x), _smpy(_smpy(b1_l, x)>>16, 1));  
	t0 = _sadd(t0, _smpy(b2_h,(Word16)-32768L));          
	t0 = _ssub(t0, _smpy(b2_l, 1));
	t0 = _sadd(t0, _smpy(f[i], 2048));           
	
	t0 = _sshl(t0, 7);                     
	cheb = t0>>16;                 
	
	return(cheb);
}