vad1.c

AMR-NB 的编码实现,纯C, VC下建立工程即可用.

#include "vad.h"

#include "basic_op.h"


static void first_filter_stage(Word16 in[],  Word16 out[],  Word16 data[] )
{

  Word16 temp0, temp1, temp2, temp3,temp4,temp5, i;
  Word16 data0, data1;
  
  data0 = data[0];                               
  data1 = data[1];                                   
 
  for (i = 0; i < 40; i++)
  {
 
     temp0 = (in[4*i+0]>>2) - (COEFF5_1*data0 >>15 );  
     temp3 = (in[4*i+1]>>2) - (COEFF5_2* data1 >> 15);		 
     temp1 = data0 + (COEFF5_1* temp0 >> 15);      
     temp2 = data1 + (COEFF5_2* temp3 >> 15);

     data0 = (in[4*i+2]>>2) -  (COEFF5_1* temp0 >> 15);
     data1 = (in[4*i+3]>>2) -  (COEFF5_2* temp3 >>15 );
     temp4 = temp0 + (COEFF5_1*data0>>15);
     temp5 = temp3 + (COEFF5_2*data1>> 15 );

     out[4*i+0] =  ( temp1+ temp2);            out[4*i+1] =  ( temp1- temp2);
     out[4*i+2] =  ( temp4+ temp5);            out[4*i+3] =  ( temp4 - temp5);     

	 
  } 
  
  data[0] = data0;                                     
  data[1] = data1;     
}

/****************************************************************************
 *
 *     Function     : filter5
 *     Purpose      : Fifth-order half-band lowpass/highpass filter pair with
 *                    decimation.
 *
 ***************************************************************************/
static void filter5(Word16 *in0,   Word16 *in1,    Word16 data[]  )
{
  Word16 temp0, temp1, temp2, temp3;

  temp0 =  *in0 - (COEFF5_1*data[0] >>15);
  temp3 =  *in1 - (COEFF5_2*data[1]>>15);
  
  temp1 = data[0] + (COEFF5_1*temp0>>15);
  temp2 = data[1] + (COEFF5_2*temp3>>15);
  


  data[0] = temp0; 
  data[1] = temp3;       
  

  *in0 = (temp1+ temp2) >>1;              
  *in1 = (temp1- temp2) >>1;            
}

/****************************************************************************
 *
 *     Function     : filter3
 *     Purpose      : Third-order half-band lowpass/highpass filter pair with
 *                          decimation.
 *     Return value : 
 *
 ***************************************************************************/
static void filter3(Word16 *in0,  Word16 *in1, Word16 *data )
{
  Word16 temp1, temp2;

  temp1 = *in1 - (COEFF3 *(*data) >>15) ;
  temp2 =  *data + (COEFF3* temp1 >>15);
  
 *data = temp1;                         

   *in1 = (*in0-temp2)>>1;        
   *in0 = (*in0+temp2)>>1;

}

/****************************************************************************
 *
 *     Function     : level_calculation
 *     Purpose      : Calculate signal level in a sub-band. Level is calculated  by summing absolute values of the input data.
 *     Return value : signal level
 *
 ***************************************************************************/
static Word16 level_calculation( Word16 data[],  Word16 *sub_level, Word16 count1,  Word16 count2,Word16 ind_m,Word16 ind_a, Word16 scale  )
{
  Word32 l_temp1, l_temp2;
  Word16 level, i;

  l_temp1 = 0L;     
  
  for (i = count1; i < count2; i++)
  {
       l_temp1 += abs(data[ind_m*i+ind_a]);
  }

  l_temp1 <<= 1;
  
  l_temp2 = l_temp1 + (*sub_level << (16-scale));
 
  *sub_level = (Word16)(l_temp1 >= ( MAX_32 >>scale) ? 0x7fff : (l_temp1<<scale) >> 16);
  
  for (i = 0; i < count1; i++)
  {  
     l_temp2  += abs(data[ind_m*i+ind_a])<<1;   		
  }

  level =  (Word16)(l_temp2 >= (MAX_32 >>scale) ? 0x7fff  :((l_temp2 << scale)>>16));
  
  
  return level;

}

/****************************************************************************
 *
 *     Function     :  filter_bank
 *     Purpose      : Divides input signal into 9-bands and calculas level of
 *                           the signal in each band 
 *
 ***************************************************************************/
static void filter_bank(vadState1 *st, Word16 in[], Word16 level[]    )
{
	Word16 i, index1 ;
	Word16 tmp_buf[FRAME_LEN];

	Word16 temp0, temp1, temp2, temp3, temp4,temp5, temp6, temp7,temp8,temp9,temp10;
	Word16 data0, data1,data2,data3, data10,data11,data20,data21,data22;

	Word32 l_temp1, l_temp2,Lsum0, Lsum1,Lsum2,Lsum3,Lsum4,Lsum5,Lsum6, Lsum7, Lsum8, Lsum9;
	Word16 * pswIn;

	pswIn = in;


	/* calculate the filter bank */
	//first_filter_stage(in, tmp_buf, st->a_data5[0]); 
	data0 = st->a_data5[0][0];   data1 = st->a_data5[0][1];  

	for (i = 0; i < 40; i++)
	{
		index1 = i<<2;

		temp0 = (pswIn[index1+0]>>2) - (COEFF5_1*data0 >>15 );  
		temp3 = (pswIn[index1+1]>>2) - (COEFF5_2* data1 >> 15);

		data2  = (pswIn[index1+2]>>2) -  (COEFF5_1* temp0 >> 15);
		data3  = (pswIn[index1+3]>>2) -  (COEFF5_2* temp3 >>15 );

		temp1 = data0  + (COEFF5_1* temp0 >>15);      
		temp2 = data1  + (COEFF5_2* temp3 >>15);
		temp4 = temp0 + (COEFF5_1* data2  >>15);
		temp5 = temp3 + (COEFF5_2* data3  >>15 );

		tmp_buf[index1+0] =  ( temp1+ temp2);            tmp_buf[index1+1] =  ( temp1- temp2);
		tmp_buf[index1+2] =  ( temp4+ temp5);            tmp_buf[index1+3] =  ( temp4 - temp5);     

		data0 = data2;  data1 = data3;

	} 

	st->a_data5[0][0] = data0;    st->a_data5[0][1] = data1; 
	data10 = st->a_data5[1][0]; data11 = st->a_data5[1][1];
	data20 = st->a_data5[2][0]; data21 = st->a_data5[2][1];

	for (i = 0; i < 40; i++)
	{
		index1 = i<<2;

		data0 = data10;  data1 = data11;
		data2 = data20;  data3 = data21;

		temp0 =  tmp_buf[index1]     - (COEFF5_1*data0 >>15);
		temp3 =  tmp_buf[index1+2] - (COEFF5_2*data1 >>15);
		temp4 =  tmp_buf[index1+1]   - (COEFF5_1*data2 >>15);
		temp7 =  tmp_buf[index1+3]   - (COEFF5_2*data3 >>15);

		temp1 = data0 + (COEFF5_1*temp0>>15); temp2 = data1 + (COEFF5_2*temp3>>15);  
		temp5 = data2 + (COEFF5_1*temp4>>15);temp6 = data3 + (COEFF5_2*temp7>>15);

		data10 = temp0; data11 = temp3;   
		data20 = temp4; data21 = temp7;   

		tmp_buf[index1]      = (temp1+ temp2) >>1;           tmp_buf[index1+2] = (temp1- temp2) >>1;  
		tmp_buf[index1+1]  = (temp5+ temp6) >>1;           tmp_buf[index1+3]  = (temp5- temp6) >>1;


	}


	st->a_data5[1][0] = data10;  st->a_data5[1][1] = data11;
	st->a_data5[2][0] = data20;  st->a_data5[2][1] = data21;


	data10 = st->a_data3[0];  data11 = st->a_data3[1]; data20 = st->a_data3[4];
	data21 = st->a_data3[2]; data22  = st->a_data3[3];

	for (i = 0; i < 20; i++)
	{
		index1 = i<<3;

		temp1 =  tmp_buf[index1+4] - (COEFF3 *data10 >>15) ;
		temp3 =  tmp_buf[index1+6] - (COEFF3 *data11 >>15) ;
		temp5 =  tmp_buf[index1+7] - (COEFF3 *data20  >>15) ;

		temp2 =  data10 + (COEFF3* temp1 >>15);
		temp4 =  data11 + (COEFF3* temp3 >>15);
		temp6 =  data20  + (COEFF3* temp5 >>15);

		tmp_buf[index1+4] = (tmp_buf[index1]-temp2)>>1;        
		tmp_buf[index1]     = (tmp_buf[index1]+temp2)>>1;
		tmp_buf[index1+6] = (tmp_buf[index1+2] - temp4)>>1;   
		tmp_buf[index1+2] = (tmp_buf[index1+2] + temp4)>>1;  
		tmp_buf[index1+7] = (tmp_buf[index1+3] - temp6)>>1;        
		tmp_buf[index1+3] = (tmp_buf[index1+3] + temp6)>>1;



		data10  = temp1;    
		data11  = temp3;   
		data20  = temp5;                         

	}

	for (i = 0; i < 10; i++)
	{
		index1 = i<<4;
		temp7   =  tmp_buf[index1+8] -  (COEFF3 *data21  >>15) ;
		temp9   =  tmp_buf[index1+12]- (COEFF3 *data22  >>15) ;

		temp8   =  data21  + (COEFF3* temp7 >>15);
		temp10 =  data22  + (COEFF3* temp9 >>15);

		tmp_buf[index1+8]   = (tmp_buf[index1+0] - temp8)>>1;        
		tmp_buf[index1+0]   = (tmp_buf[index1+0] + temp8)>>1;
		tmp_buf[index1+12] = (tmp_buf[index1+4] - temp10)>>1;        
		tmp_buf[index1+4]   = (tmp_buf[index1+4] + temp10)>>1;

		data21  = temp7;    
		data22  = temp9;                         
	}

	st->a_data3[0] = data10;  st->a_data3[1] = data11; st->a_data3[4] = data20;
	st->a_data3[2] = data21;  st->a_data3[3] = data22;

  
	/* calculate levels in each frequency band */
	/* 3000 - 4000 Hz*/
	Lsum0 =  abs(tmp_buf[129]) + abs(tmp_buf[133]) + abs(tmp_buf[137]) + abs(tmp_buf[141]);
	Lsum1 =  abs(tmp_buf[145]) + abs(tmp_buf[149]) + abs(tmp_buf[153]) + abs(tmp_buf[157]);

	l_temp1  = (Lsum0+Lsum1) <<1;
	l_temp2 =  l_temp1 + (st->sub_level[8] <<1);

	Lsum2 =     abs(tmp_buf[1])  +  abs(tmp_buf[5]) +  abs(tmp_buf[9])  +  abs(tmp_buf[13]) ;   		
	Lsum3 =     abs(tmp_buf[17]) +  abs(tmp_buf[21])+  abs(tmp_buf[25])+  abs(tmp_buf[29]) ; 
	Lsum4 =	    abs(tmp_buf[33]) +  abs(tmp_buf[37])+  abs(tmp_buf[41])+  abs(tmp_buf[45]) ;
	Lsum5 =	    abs(tmp_buf[49]) +  abs(tmp_buf[53])+  abs(tmp_buf[57])+  abs(tmp_buf[61]) ;
	Lsum6 =	    abs(tmp_buf[65]) +  abs(tmp_buf[69])+  abs(tmp_buf[73])+  abs(tmp_buf[77]) ;
	Lsum7 =	    abs(tmp_buf[81]) +  abs(tmp_buf[85])+  abs(tmp_buf[89])+  abs(tmp_buf[93]) ;
	Lsum8 =	    abs(tmp_buf[97])   +  abs(tmp_buf[101])  +  abs(tmp_buf[105]) +  abs(tmp_buf[109]) ;
	Lsum9 =	    abs(tmp_buf[113])  +  abs(tmp_buf[117])  +  abs(tmp_buf[121]) +  abs(tmp_buf[125]) ;

	l_temp2 += (Lsum2+Lsum3+Lsum4+Lsum5+Lsum6+Lsum7+Lsum8+Lsum9 )<<1;

	st->sub_level[8] = (Word16)(l_temp1 >= 0xffff  ? 0x7fff : l_temp1>>1);
	level[8]  =  (Word16)(l_temp2 >=  0xffff  ? 0x7fff  :((l_temp2 << 15)>>16));

  
	/* 2500 - 3000 Hz*/  

	l_temp1 = ( abs(tmp_buf[135]) + abs(tmp_buf[143])+ abs(tmp_buf[151])+ abs(tmp_buf[159]) )<<1;     
	l_temp2 = l_temp1 + st->sub_level[7] ; 

	st->sub_level[7] = (Word16)(l_temp1 >= 0x7fff ? 0x7fff : l_temp1);

	Lsum2 = abs(tmp_buf[7])+ abs(tmp_buf[15])+ abs(tmp_buf[23])+ abs(tmp_buf[31]) ;  
	Lsum3 = abs(tmp_buf[39])+ abs(tmp_buf[47])+ abs(tmp_buf[55])+ abs(tmp_buf[63]) ; 
	Lsum4 = abs(tmp_buf[71])+ abs(tmp_buf[79])+ abs(tmp_buf[87])+ abs(tmp_buf[95]) ; 
	Lsum5 = abs(tmp_buf[103])+ abs(tmp_buf[111])+ abs(tmp_buf[119])+ abs(tmp_buf[127]) ; 

	l_temp2 += (Lsum2+Lsum3+Lsum4+Lsum5)<<1;  
	level[7]  =  (Word16)(l_temp2 >= 0x7fff ? 0x7fff  :l_temp2) ;


	/* 2000 - 2500 Hz*/
	l_temp1 = (abs(tmp_buf[131])+abs(tmp_buf[139])+abs(tmp_buf[147])+abs(tmp_buf[155]))<<1;
	l_temp2 = l_temp1 +st->sub_level[6] ;

	st->sub_level[6] = (Word16)(l_temp1 >= 0x7fff ? 0x7fff : l_temp1);

	Lsum2 = abs(tmp_buf[3])+abs(tmp_buf[11])+abs(tmp_buf[19])+abs(tmp_buf[27]); 
	Lsum3 = abs(tmp_buf[35])+abs(tmp_buf[43])+abs(tmp_buf[51])+abs(tmp_buf[59]);  
	Lsum4 = abs(tmp_buf[67])+abs(tmp_buf[75])+abs(tmp_buf[83])+abs(tmp_buf[91]);  
	Lsum5 = abs(tmp_buf[99])+abs(tmp_buf[107])+abs(tmp_buf[115])+abs(tmp_buf[123]);  

	l_temp2 += (Lsum2+Lsum3+Lsum4+Lsum5)<<1;  
	level[6]  =  (Word16)(l_temp2 >= 0x7fff ? 0x7fff  : l_temp2) ;


	/* 1500 - 2000 Hz*/
	l_temp1 = (abs(tmp_buf[130])+abs(tmp_buf[138])+abs(tmp_buf[146])+abs(tmp_buf[154]))<<1;  
	l_temp2 = l_temp1 + st->sub_level[5] ; 
	st->sub_level[5] = (Word16)(l_temp1 >=  0x7fff ? 0x7fff : l_temp1);

	Lsum2 = abs(tmp_buf[2])+abs(tmp_buf[10])+abs(tmp_buf[18])+abs(tmp_buf[26]); 
	Lsum3 = abs(tmp_buf[34])+abs(tmp_buf[42])+abs(tmp_buf[50])+abs(tmp_buf[58]);  
	Lsum4 = abs(tmp_buf[66])+abs(tmp_buf[74])+abs(tmp_buf[82])+abs(tmp_buf[90]);  
	Lsum5 = abs(tmp_buf[98])+abs(tmp_buf[106])+abs(tmp_buf[114])+abs(tmp_buf[122]);  

	l_temp2 += (Lsum2+Lsum3+Lsum4+Lsum5)<<1;
	level[5]  = (Word16)( l_temp2 >= 0x7fff ? 0x7fff  : l_temp2);


	/* 1000 - 1500 Hz*/
	l_temp1 = (abs(tmp_buf[134])+abs(tmp_buf[142])+abs(tmp_buf[150])+abs(tmp_buf[158]))<<1;
	l_temp2 = (Word16)(l_temp1 + st->sub_level[4] );

	st->sub_level[4] = (Word16)(l_temp1 >= 0x7fff ? 0x7fff : l_temp1);

	Lsum2 = abs(tmp_buf[6])+abs(tmp_buf[14])+abs(tmp_buf[22])+abs(tmp_buf[30]); 
	Lsum3 = abs(tmp_buf[38])+abs(tmp_buf[46])+abs(tmp_buf[54])+abs(tmp_buf[62]);  
	Lsum4 = abs(tmp_buf[70])+abs(tmp_buf[78])+abs(tmp_buf[86])+abs(tmp_buf[94]);  
	Lsum5 = abs(tmp_buf[102])+abs(tmp_buf[110])+abs(tmp_buf[118])+abs(tmp_buf[126]);  

	l_temp2 += (Lsum2+Lsum3+Lsum4+Lsum5)<<1;
	level[4]  = (Word16)(l_temp2 >= 0x7fff ? 0x7fff  :l_temp2);

	/* 750 - 1000 Hz*/