g722.cpp

G722语音压缩算法

#include "g722.h"


static int x[24] =         /* storage for signal passing     */
                                   /* through the qmf                */
                {0, 0, 0, 0, 0, 0,
                0, 0, 0, 0, 0, 0,
                0, 0, 0, 0, 0, 0,
                0, 0, 0, 0, 0, 0};
static int q6[32] = {0, 35, 72, 110, 150, 190, 233, 276, 323,
		370, 422, 473, 530, 587, 650, 714, 786,
		858, 940, 1023, 1121, 1219, 1339, 1458,
		1612, 1765, 1980, 2195, 2557, 2919, 0, 0} ;
static const int iln[32] = {0, 63, 62, 31, 30, 29, 28, 27, 26, 25,
		24, 23, 22, 21, 20, 19, 18, 17, 16, 15, 14,
		13, 12, 11, 10, 9, 8, 7, 6, 5, 4, 0 } ;
static const int ilp[32] = {0, 61, 60, 59, 58, 57, 56, 55, 54, 53, 52,
		51, 50, 49, 48, 47, 46, 45, 44, 43, 42, 41,
		40, 39, 38, 37, 36, 35, 34, 33, 32, 0 } ;
static const int qm4[16] =
		{0,	-20456,	-12896,	-8968,
		-6288,	-4240,	-2584,	-1200,
		20456,	12896,	8968,	6288,
		4240,	2584,	1200,	0 } ;
static const int wl[8] = {-60, -30, 58, 172, 334, 538, 1198, 3042 } ;
static const int rl42[16] = {0, 7, 6, 5, 4, 3, 2, 1, 7, 6, 5, 4, 3, 2,
		1, 0 } ;
static int ilb[32] = {2048, 2093, 2139, 2186, 2233, 2282, 2332,
		2383, 2435, 2489, 2543, 2599, 2656, 2714,
		2774, 2834, 2896, 2960, 3025, 3091, 3158,
		3228, 3298, 3371, 3444, 3520, 3597, 3676,
		3756, 3838, 3922, 4008 } ;
static const int ihn[3] = { 0, 1, 0 } ;
static const int ihp[3] = { 0, 3, 2 } ;
static const int qm2[4] =
		{-7408,	-1616,	7408,	1616} ;
static const int wh[3] = {0, -214, 798} ;
static const int rh2[4] = {2, 1, 2, 1} ;

static const int qm6[64] = {-136, -136, -136, -136, -24808, -21904, -19008, -16704,
					-14984, -13512, -12280, -11192, -10232, -9360, -8576, -7856,
					-7192, -6576,  -6000,  -5456, -4944, -4464,  -4008,  -3576,
					-3168, -2776,  -2400,  -2032, -1688, -1360,  -1040,  -728,
					24808, 21904,  19008,  16704, 14984, 13512,  12280,  11192,
					10232, 9360,   8576,   7856, 7192,  6576,   6000,   5456,
					4944,  4464,   4008,   3576, 3168,  2776,   2400,   2032,
					1688,  1360,   1040,   728, 432,   136,    -432,   -136 } ;
//		static int tap[14]= {0, 3, -11, 12, 32, -210, 951, 3876,-805, 362,
//							-156, 53, -11, 0};
static int xd[12] =	{0, 0, 0, 0, 0, 0,0, 0, 0, 0, 0, 0} ;
static int xs[12] = {0, 0, 0, 0, 0, 0,0, 0, 0, 0, 0, 0} ;
int g722_init_state(pG722_Handle test)  //init object
{  	
	    test->ilowr = 0;
		test->dlowt = 0;
		test->rlow = 0;
		test->slow = 0;
		test->detlow = 32;
		test->ihigh = 0;
		test->dhigh = 0;
		test->rhigh = 0;	
		test->shigh = 0;
		test->dethigh = 8;
        return 1;
	}
int encoder_block4l (dl)
int dl ;
{
static int sl = 0;
	int wd1, wd2, wd3, wd4, wd5,y,z;
	static int spl = 0 ;
	static int szl = 0 ;
	static int rlt  [3] = { 0, 0, 0 } ;
	static int al   [3] = { 0, 0, 0 } ;
	static int plt  [3] = { 0, 0, 0 } ;
	static int dlt  [7] = { 0, 0, 0, 0, 0, 0, 0 } ;
	static int bl   [7] = { 0, 0, 0, 0, 0, 0, 0 } ;
	static int sg   [7] = { 0, 0, 0, 0, 0, 0, 0 } ;
/****************** pointer ****************************/
	int *sgp, *pltp, *alp, *dltp, *blp, *rltp;
	int *pltp_1, *dltp_1, *rltp_1;
/*************************************** BLOCK 4L, RECONS ***********/
	*dlt = dl;

	*rlt = sl + dl ;
/*
        if ( *rlt > 32767 ) *rlt = 32767;
        else if ( *rlt < -32768 ) *rlt = -32768;
*/
/*************************************** BLOCK 4L, PARREC ***********/

	*plt = dl + szl ;
/*
        if ( plt[0] > 32767 ) plt[0] = 32767;
        else if ( plt[0] < -32768 ) plt[0] = -32768;
*/
/*****************************BLOCK 4L, UPPOL2*************************/
	sgp = sg, pltp = plt, alp = al ;
		*sgp++ = *pltp++ >> 15 ;
		*sgp++ = *pltp++ >> 15 ;
		*sgp++ = *pltp++ >> 15 ;

	wd1 = *++alp << 2;

        if ( wd1 > 32767 ) wd1 = 32767;
        else if ( wd1 < -32768 ) wd1 = -32768;

	wd2= ( *sg == *(sg+1) )?  -wd1: wd1 ;
        if ( wd2 > 32767 ) wd2 = 32767;

	wd2 = wd2 >> 7 ;
	wd3= ( *sg == *(sg+2) )? 128: -128 ;
	wd4 = wd2 + wd3 ;
	wd5 = (*++alp * 32512) >> 15 ;
	
	*alp = wd4 + wd5 ;

	if ( *alp  >  12288 )  *alp =  12288 ;
	else if ( *alp  < -12288 )  *alp = -12288 ;

/************************************* BLOCK 4L, UPPOL1 ***************/

	*sg = *plt >> 15 ;
	*(sg+1) = *(plt+1) >> 15 ;
	wd1 = ( *sg == *(sg+1) )?  192 : -192 ;

	wd2 = (*--alp * 32640) >> 15 ;

	*alp = wd1 + wd2 ;
/*
        if ( *alp > 32767 ) *alp = 32767;
        else if ( *alp < -32768 ) *alp = -32768;
*/
	wd3 = (15360 - *++alp) ;
/*
        if ( wd3 > 32767 ) wd3 = 32767;
        else if ( wd3 < -32768 ) wd3 = -32768;
*/
	if ( *--alp >  wd3)  *alp =  wd3 ;
	else if ( *alp  < -wd3)  *alp = -wd3 ;

/*************************************** BLOCK 4L, UPZERO ************/
	wd1 = ( dl == 0 ) ? 0 : 128;
	*sg = dl >> 15 ;
	sgp = sg, dltp = dlt, blp = bl; 



z=0;
	while(z<6)
	{
	    *++sgp = *++dltp >> 15 ;
		wd2 = ( *sgp == *sg ) ? wd1 : -wd1 ;
		wd3 = (*++blp * 32640) >> 15 ;
		*blp = wd2 + wd3 ;
/*
                if ( *blp > 32767 ) *blp = 32767;
                else if ( *blp < -32768 ) *blp = -32768;
*/
		z++;
	}
/********************************* BLOCK 4L, DELAYA ******************/
	dltp_1 = dltp - 1; 
		*dltp--  = *dltp_1-- ;
		*dltp--  = *dltp_1-- ;
		*dltp--  = *dltp_1-- ;
		*dltp--  = *dltp_1-- ;
		*dltp--  = *dltp_1-- ;
		*dltp--  = *dltp_1-- ;

	rltp =rlt+2, pltp = plt+2 ;
        rltp_1 = rltp - 1, pltp_1 = pltp - 1;

		*rltp-- = *rltp_1--;
		*pltp-- = *pltp_1--; 
		*rltp-- = *rltp_1--;
		*pltp-- = *pltp_1--; 
/********************************* BLOCK 4L, FILTEP ******************/

	wd1 = ( *alp * *++rltp ) >> 14 ;

	wd2 = ( *++alp * *++rltp ) >> 14 ;

	spl = wd1 + wd2 ;
/*
        if ( spl > 32767 ) spl = 32767;
        else if ( spl < -32768 ) spl = -32768;
*/
/*************************************** BLOCK 4L, FILTEZ ***********/

	blp = blp - 6;

	
szl = (*++blp * *++dltp) >> 14 ;
y=1;	 
while(y<6)
	  {
		  szl += (*++blp * *++dltp) >> 14 ;
/*
                if ( szl > 32767 ) szl = 32767;
                else if ( szl < -32768 ) szl = -32768;
*/
          y++;}
/*********************************BLOCK 4L, PREDIC *******************/

	sl = spl + szl ;
/*
        if ( sl > 32767 ) sl = 32767;
        else if ( sl < -32768 ) sl = -32768;
*/

	return (sl) ;
}
/**************************** BLOCK 4H *******************************/

int encoder_block4h (d)
int d ;
{
static int sh = 0 ;
	int wd1, wd2, wd3, wd4, wd5,z,r;
	static int sph = 0 ;
	static int szh = 0 ;
	static int rh  [3] = { 0, 0, 0 } ;
	static int ah   [3] = { 0, 0, 0 } ;
	static int ph  [3] = { 0, 0, 0 } ;
	static int dh  [7] = { 0, 0, 0, 0, 0, 0, 0 } ;
	static int bh   [7] = { 0, 0, 0, 0, 0, 0, 0 } ;
	static int sg   [7] = { 0, 0, 0, 0, 0, 0, 0 } ;

	int *sgp, *bhp, *dhp, *php, *ahp, *rhp;
	int *dhp_1, *rhp_1, *php_1;
/*************************************** BLOCK 4H, RECONS ***********/

	*dh = d;
	*rh = sh + d ;
/*
        if ( rh[0] > 32767 ) rh[0] = 32767;
        else if ( rh[0] < -32768 ) rh[0] = -32768;
*/
/*************************************** BLOCK 4H, PARREC ***********/

	*ph = d + szh ;
/*
        if ( ph[0] > 32767 ) ph[0] = 32767;
        else if ( ph[0] < -32768 ) ph[0] = -32768;
*/
/*****************************BLOCK 4H, UPPOL2*************************/
	sgp = sg, php = ph, ahp =ah ;
	*sg = *ph >> 15 ;
	*++sgp = *++php >> 15 ;
	*++sgp = *++php >> 15 ;
	wd1 = (*++ahp) << 2;

        if ( wd1 > 32767 ) wd1 = 32767;
        else if ( wd1 < -32768 ) wd1 = -32768;

	wd2 = ( *sg == *--sgp ) ? - wd1 : wd1;
        if ( wd2 > 32767 ) wd2 = 32767;

	wd2 = wd2 >> 7 ;
	wd3 = ( *sg == *++sgp ) ? 128:-128 ;

	wd4 = wd2 + wd3 ;
	wd5 = (*++ahp * 32512) >> 15 ;

	*ahp = wd4 + wd5 ;
	if ( *ahp  >  12288 )  *ahp  =  12288 ;
	else if ( *ahp  < -12288 )  *ahp  = -12288 ;
/************************************* BLOCK 4H, UPPOL1 ***************/

	*sg = *ph >> 15 ;
	*--sgp  = *--php >> 15 ;
	wd1 = ( *sg == *sgp ) ? 192 : -192;

	wd2 = (*--ahp * 32640) >> 15 ;

 	*ahp  = wd1 + wd2 ;
/*
        if ( *ahp > 32767 ) *ahp = 32767;
        else if ( *ahp < -32768 ) *ahp = -32768;
*/
	wd3 = (15360 - *++ahp) ;
/*
        if ( wd3 > 32767 ) wd3 = 32767;
        else if ( wd3 < -32768 ) wd3 = -32768;
*/
	if ( *--ahp  >  wd3)  *ahp  =  wd3 ;
	else if ( *ahp  < -wd3)  *ahp  = -wd3 ;

/*************************************** BLOCK 4H, UPZERO ************/
	wd1 = ( d == 0 ) ? 0 : 128;

	*sg = d >> 15 ;
	dhp = dh, bhp = bh;

z=0;
while(z<6)
{
        *sgp = *++dhp >> 15 ;
		wd2 = ( *sgp++  == *sg ) ? wd1 : -wd1;
		wd3 = (*++bhp * 32640) >> 15 ;
		*bhp = wd2 + wd3 ;
		z++;
}
/*
                if ( *bhp > 32767 ) *bhp= 32767;
                else if ( *bhp < -32768 ) *bhp = -32768;
*/
/********************************* BLOCK 4H, DELAYA ******************/
	dhp_1 = dhp - 1;

		*dhp-- = *dhp_1-- ;
		*dhp-- = *dhp_1-- ;
		*dhp-- = *dhp_1-- ;
		*dhp-- = *dhp_1-- ;
		*dhp-- = *dhp_1-- ;
		*dhp-- = *dhp_1-- ;

	rhp = rh+2;
	php++;	
        rhp_1 = rhp - 1, php_1 = php - 1;

		*rhp-- = *rhp_1-- ;
		*php-- = *php_1-- ;
		*rhp-- = *rhp_1-- ;
		*php-- = *php_1-- ;
/********************************* BLOCK 4H, FILTEP ******************/

	wd1 = ( *ahp * *++rhp ) >> 14 ;

	wd2 = ( *++ahp * *++rhp ) >> 14 ;

	sph = wd1 + wd2 ;
/*
        if ( sph > 32767 ) sph = 32767;
        if ( sph < -32768 ) sph = -32768;
*/
/*************************************** BLOCK 4H, FILTEZ ***********/

	bhp = bhp -6;
szh = (*++bhp * *++dhp ) >> 14 ;
r=1;
while(r<6)
{
	szh += (*++bhp * *++dhp ) >> 14 ;
/*
                if ( szh > 32767 ) szh = 32767;
                else if ( szh < -32768 ) szh = -32768;
*/

	r++;
}
/*********************************BLOCK 4L, PREDIC *******************/

	sh = sph + szh ;
/*
        if ( sh > 32767 ) sh = 32767;
        if ( sh < -32768 ) sh = -32768;
*/
	return (sh) ;
}
/*void decoder_init(pG722 test)	//对象初始化
	{	test->ilowr = 0;
		test->dlowt = 0;
		test->rlow = 0;
		test->slow = 0;
		test->detlow = 32;
		test->ihigh = 0;
		test->dhigh = 0;
		test->rhigh = 0;	
		test->shigh = 0;
		test->dethigh = 8;
	}*/
int decoder_block4l(int dl)		//低子带自适应预测
	{	static int sl = 0 ;
		int wd1, wd2, wd3;
		static int spl = 0 ;
		static int szl = 0 ;
		static int rlt0=0, rlt1=0, rlt2=0;
		static int al1=0, al2=0;
		static int plt0=0, plt1=0, plt2=0;
		static int dlt  [7] = { 0, 0, 0, 0, 0, 0, 0 } ;
		static int bl   [7] = { 0, 0, 0, 0, 0, 0, 0 } ;
		static int bpl  [7] = { 0, 0, 0, 0, 0, 0, 0 } ;
		static int sg0=0, sg1=0, sg2=0;
		static int sg   [7] = { 0, 0, 0, 0, 0, 0, 0 } ;
		int *sgp, *dltp, *dltp_1, *blp, *bplp;
    dlt[0] = dl;
    /*************************************** BLOCK 4L, RECONS  ***********/ 
    rlt0 = sl + dl ;
    /*
      if (rlt0 > 32767) rlt0 = 32767;
      if (rlt0 < -32768) rlt0 = -32768;
    */
    /*************************************** BLOCK 4L, PARREC  ***********/
    plt0 = dl + szl ;
    /*
      if (plt0 > 32767) plt0 = 32767;
      if (plt0 < -32768) plt0 = -32768;
    */
    /*****************************BLOCK 4L,  UPPOL2*************************/
    sg0 = plt0 >> 15 ;
    sg1 = plt1 >> 15 ;
    sg2 = plt2 >> 15 ;
    wd1 = al1 << 2;
    if (wd1 > 32767)
		wd1 = 32767;
    else if (wd1 < -32768)
		wd1 = -32768;
    wd2= ( sg0 == sg1 )? - wd1 : wd1 ;
    /*
      if (wd2 > 32767) wd2 = 32767;
    */
    wd2 = wd2 >> 7 ;
    wd3= ( sg0 == sg2 )? 128 : - 128 ;
    wd2 = wd2 + wd3 ;
    wd3 = (al2 * 127) >> 7 ;
    al2 = wd2 + wd3 ;
    if ( al2 >  12288 )
		al2 =  12288 ;
    else if ( al2 < -12288 )
		al2 = -12288 ;
    /************************************* BLOCK 4L, UPPOL1  ***************/
    sg0 = plt0 >> 15 ;
    sg1 = plt1 >> 15 ;
    wd1= ( sg0 == sg1 )? 192 : - 192 ;
    wd2 = (al1 * 255) >> 8 ;
    al1 = wd1 + wd2 ;
    /*
      if (al1 > 32767) al1 = 32767;
      if (al1 < -32768) al1 = -32768;
    */ 
    wd3 = (15360 - al2) ;
    /*
      if (wd3 > 32767) wd3 = 32767;
      if (wd3 < -32768) wd3 = -32768;
    */
    if ( al1 >  wd3)
		al1 =  wd3 ;
    else if ( al1 < -wd3)
		al1 = -wd3 ;
    /*************************************** BLOCK 4L, UPZERO  ************/
    wd1= ( dl == 0 )?  0 : 128 ;
    sg0 = dl >> 15 ;
    sgp=sg, dltp=dlt, blp=bl, bplp=bpl;
	*++sgp = *++dltp >> 15 ;
	wd2= ( *sgp == sg0 )?  wd1 : - wd1 ;
	wd3 = (*++blp * 255) >> 8 ;
	*++bplp = wd2 + wd3 ;
	/*
  	if (*bplp > 32767) *bplp = 32767;
	else if (*bplp < -32768) *bplp = -32768;
	*/
	*++sgp = *++dltp >> 15 ;
	wd2= ( *sgp == sg0 )?  wd1 : - wd1 ;
	wd3 = (*++blp * 255) >> 8 ;
	*++bplp = wd2 + wd3 ;
	/*
  	if (*bplp > 32767) *bplp = 32767;
	else if (*bplp < -32768) *bplp = -32768;
	*/
	*++sgp = *++dltp >> 15 ;
	wd2= ( *sgp == sg0 )?  wd1 : - wd1 ;
	wd3 = (*++blp * 255) >> 8 ;
	*++bplp = wd2 + wd3 ;
	/*
  	if (*bplp > 32767) *bplp = 32767;
	else if (*bplp < -32768) *bplp = -32768;
	*/
	*++sgp = *++dltp >> 15 ;
	wd2= ( *sgp == sg0 )?  wd1 : - wd1 ;
	wd3 = (*++blp * 255) >> 8 ;
	*++bplp = wd2 + wd3 ;
	/*
  	if (*bplp > 32767) *bplp = 32767;
	else if (*bplp < -32768) *bplp = -32768;
	*/
	*++sgp = *++dltp >> 15 ;
	wd2= ( *sgp == sg0 )?  wd1 : - wd1 ;
	wd3 = (*++blp * 255) >> 8 ;
	*++bplp = wd2 + wd3 ;
	/*
  	if (*bplp > 32767) *bplp = 32767;
	else if (*bplp < -32768) *bplp = -32768;
	*/
	*++sgp = *++dltp >> 15 ;
	wd2= ( *sgp == sg0 )?  wd1 : - wd1 ;
	wd3 = (*++blp * 255) >> 8 ;
	*++bplp = wd2 + wd3 ;
	/*
  	if (*bplp > 32767) *bplp = 32767;
	else if (*bplp < -32768) *bplp = -32768;
	*/
    /********************************* BLOCK 4L, DELAYA  ******************/
    dltp=dlt+6, dltp_1=dltp-1, blp=bl, bplp=bpl;
	*dltp-- = *dltp_1--;
	*++blp  = *++bplp;
	*dltp-- = *dltp_1--;
	*++blp  = *++bplp;
	*dltp-- = *dltp_1--;
	*++blp  = *++bplp;
	*dltp-- = *dltp_1--;
	*++blp  = *++bplp;
	*dltp-- = *dltp_1--;
	*++blp  = *++bplp;
	*dltp-- = *dltp_1--;
	*++blp  = *++bplp;
	rlt2=rlt1;
    rlt1=rlt0;
    plt2=plt1;
    plt1=plt0;
    /********************************* BLOCK 4L, FILTEP  ******************/
    wd1 = ( al1 * rlt1 ) >> 14 ;
    wd2 = ( al2 * rlt2 ) >> 14 ;
    spl = wd1 + wd2 ;
    /*
      if (spl > 32767) spl = 32767;
      if (spl < -32768) spl = -32768;
    */ 
    /*************************************** BLOCK 4L, FILTEZ  ***********/
    dltp = dlt, blp=bl;
	szl = (*++blp * *++dltp)>>14;
	/*
	  if (szl > 32767) szl = 32767;
	  else if (szl < -32768) szl = -32768;
	*/
	szl += (*++blp * *++dltp)>>14;
	/*
	  if (szl > 32767) szl = 32767;
	  else if (szl < -32768) szl = -32768;
	*/
	szl += (*++blp * *++dltp)>>14;
	/*
	  if (szl > 32767) szl = 32767;