g722.cpp

G722语音压缩算法

#include "g722.h"


static INT32 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 INT32 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 INT32 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 INT32 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 INT32 qm4[16] =
		{0,	-20456,	-12896,	-8968,
		-6288,	-4240,	-2584,	-1200,
		20456,	12896,	8968,	6288,
		4240,	2584,	1200,	0 } ;
static const INT32 wl[8] = {-60, -30, 58, 172, 334, 538, 1198, 3042 } ;
static const INT32 rl42[16] = {0, 7, 6, 5, 4, 3, 2, 1, 7, 6, 5, 4, 3, 2,
		1, 0 } ;
static INT32 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 INT32 ihn[3] = { 0, 1, 0 } ;
static const INT32 ihp[3] = { 0, 3, 2 } ;
static const INT32 qm2[4] =
		{-7408,	-1616,	7408,	1616} ;
static const INT32 wh[3] = {0, -214, 798} ;
static const INT32 rh2[4] = {2, 1, 2, 1} ;

static const INT32 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 INT32 tap[14]= {0, 3, -11, 12, 32, -210, 951, 3876,-805, 362,
//							-156, 53, -11, 0};
static INT32 xd[12] =	{0, 0, 0, 0, 0, 0,0, 0, 0, 0, 0, 0} ;
static INT32 xs[12] = {0, 0, 0, 0, 0, 0,0, 0, 0, 0, 0, 0} ;
INT32 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;
	}
INT32 encoder_block4l (INT32 dl)
{
static INT32 sl = 0;
	INT32 wd1, wd2, wd3, wd4, wd5,y,z;
	static INT32 spl = 0 ;
	static INT32 szl = 0 ;
	static INT32 rlt  [3] = { 0, 0, 0 } ;
	static INT32 al   [3] = { 0, 0, 0 } ;
	static INT32 plt  [3] = { 0, 0, 0 } ;
	static INT32 dlt  [7] = { 0, 0, 0, 0, 0, 0, 0 } ;
	static INT32 bl   [7] = { 0, 0, 0, 0, 0, 0, 0 } ;
	static INT32 sg   [7] = { 0, 0, 0, 0, 0, 0, 0 } ;
/****************** poINT32er ****************************/
	INT32 *sgp, *pltp, *alp, *dltp, *blp, *rltp;
	INT32 *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 *******************************/

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

	INT32 *sgp, *bhp, *dhp, *php, *ahp, *rhp;
	INT32 *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;