g723codec.c

视频会议源码

#include <stdlib.h>
#include <stdio.h>
#include "g723codec.h"

void VoiceActivityDetectSize_G723(int* pVADsize)
{
   *pVADsize = sizeof(G723_VADmemory);
   *pVADsize = (*pVADsize+7)&(~7);
}

static  const short  ScfTab[11] = {
   9170, 9170, 9170, 9170,10289,11544,12953,14533,16306,18296,20529
};

void VoiceActivityDetect_G723(const Ipp16s *pSrc, const Ipp16s *pNoiseLPC,
         const Ipp16s *pOlp, int SinDet, int *pVad, int *pAen, char* pVADmem)
{
   G723_VADmemory* vadMem =  (G723_VADmemory*)pVADmem;
   int i,j ;
   int  Acc0,Acc1 ;
   Ipp64s z;
   short  Tmp0, Tmp, Nmult ;
   short  MinOLP ;

   int  vad = 1 ;

   vadMem->Polp[2] = pOlp[0] ;
   vadMem->Polp[3] = pOlp[1] ;

   /* Find Minimum pitch period */
   MinOLP = pitchmax;
   for ( i = 0 ; i < 4 ; i ++ ) {
       if ( MinOLP > vadMem->Polp[i] )
           MinOLP = vadMem->Polp[i] ;
   }

   /* How many olps are multiplies of their min */
   Nmult = 0 ;
   for ( i = 0 ; i < 4 ; i++ ) {
       Tmp = MinOLP ;
       for ( j = 0 ; j < 8 ; j++ ) {
           Tmp0 = Tmp - vadMem->Polp[i];
           if(Tmp0 < 0) Tmp0 = -Tmp0;
           if ( Tmp0 <= 3 ) Nmult++ ;
           Tmp += MinOLP;
       }
   }

   /* Update adaptation enable counter if not periodic and not sine */
   if ( (Nmult == 4) || (SinDet == 0) )
       vadMem->Aen += 2 ;
   else
       vadMem->Aen -- ;

   /* Clip it */
   if ( vadMem->Aen > 6 )
       vadMem->Aen = 6 ;
   if ( vadMem->Aen < 0 )
       vadMem->Aen = 0 ;

   /* Inverse filter the data */
   
    z = 0 ;
    for ( i = sfsize ; i < fsize ; i ++ ) {

        Acc0 = pSrc[i] * (short)0x2000 ;
        for ( j = 0 ; j < lpclen ; j ++ )
            Acc0 -= pSrc[i-j-1] * pNoiseLPC[j] ;
        Tmp0 = ( Acc0 + (short)0x4000 )>>15 ;
        z += Tmp0 * Tmp0 ;
    }
 //   z <<= 1;
    if(z > IPP_MAX_32S) Acc1 = IPP_MAX_32S;
    else if(z < IPP_MIN_32S) Acc1 = IPP_MIN_32S;
    else Acc1 = (int)z;
    /* Scale the rezidual energy */
    Acc1 = MulC_32s(2913,  Acc1 ) ;
   /* Clip noise level in any case */
   if ( vadMem->Nlev > vadMem->Penr ) {
      Acc0 = vadMem->Penr - (vadMem->Penr>>2);
      vadMem->Nlev = Acc0 + (vadMem->Nlev>>2);
   }


   /* Update the noise level, if adaptation is enabled */
   if ( !vadMem->Aen ) {
      vadMem->Nlev = vadMem->Nlev + (vadMem->Nlev>>5);
   }
   /* Decay Nlev by small amount */
   else {
      vadMem->Nlev = vadMem->Nlev - (vadMem->Nlev>>11);
   }

   /* Update previous energy */
   vadMem->Penr = Acc1 ;

   /* CLip Noise Level */
   if ( vadMem->Nlev < (int)0x80 )
       vadMem->Nlev = (int)0x80 ;
   if ( vadMem->Nlev > (int)0x1ffff )
       vadMem->Nlev = (int)0x1ffff ;

   /* Compute the treshold */
   Acc0 = vadMem->Nlev<<13;
   Tmp0 = Norm_32s(&Acc0);
   Tmp = (Acc0>>15)& (short)0x7e00;

   Acc0 = ScfTab[Tmp0] << 15;
   Acc0 += Tmp * (ScfTab[Tmp0-1] - ScfTab[Tmp0]); 
   Tmp = Acc0 >> 15;


   Tmp0 = (short)(vadMem->Nlev>>2);
   Acc0 = Tmp0*Tmp;
   Acc0 >>= 10;

   /* treshold */
   if ( Acc0 > Acc1 )                        // Acc0 is threshold, Acc1 is energy
       vad = 0 ;

   /* update counters */
   if ( vad ) {
       vadMem->Vcnt++ ;
       vadMem->Hcnt++ ;
   } else {
       vadMem->Vcnt-- ;
       if ( vadMem->Vcnt < 0 )
           vadMem->Vcnt = 0 ;
   }
   if ( vadMem->Vcnt >= 2 ) {
       vadMem->Hcnt = 6 ;
       if ( vadMem->Vcnt >= 3 )
           vadMem->Vcnt = 3 ;
   }
   if ( vadMem->Hcnt ) {
       vad = 1 ;
       if ( vadMem->Vcnt == 0 )
           vadMem->Hcnt -- ;
   }
   /* Update periodicy detector */
   vadMem->Polp[0] = vadMem->Polp[2] ;
   vadMem->Polp[1] = vadMem->Polp[3] ;

   *pAen = vadMem->Aen; /* adaptation enable counter  */
   *pVad = vad; /* VAD desision : 0 - noise, 1 - voice  */

}

void LSFQuantInv_G723_32s16s(Ipp32s valQLsfIndex, 
	 Ipp16s * pSrcPrevLsf, Ipp16s * pDstLsf, Ipp16s Crc)
{
	int	i, j;
	Ipp16s	iTemp1, iTemp2;
	Ipp16s	iPredict;
	Ipp16s	iDiff;
	Ipp16s	iLsf[lpclen];
	Ipp32s	nIndex1, nIndex2, nIndex3;	
	
	/* initialize the coefficients */
	if ( Crc == 1 ) {
		nIndex1 = 0;
		nIndex2 = 0;
		nIndex3 = 0;
		iPredict = InvQuanB_Crc1;
		iDiff = InvQuanDelta_Crc1;
	} else {
		nIndex1 = valQLsfIndex >> 16;
		nIndex2 = (valQLsfIndex >> 8) - (nIndex1 << 8);
		nIndex3 = valQLsfIndex - (nIndex1 << 16) - (nIndex2 << 8);
		iPredict = InvQuanB_Crc0;
		iDiff = InvQuanDelta_Crc0;
	}

	/* quantize */
	nIndex1 *= 3;
	nIndex2 *= 3;
	nIndex3 *= 4;
	for ( i = 0; i < 3; i ++ ) {
		iLsf[i] = Band0Tb8[nIndex1+i];
	}
	for ( i = 0; i < 3; i ++ ) {
		iLsf[3+i] = Band1Tb8[nIndex2+i];
	}
	for ( i = 0; i < 4; i ++ ) {
		iLsf[6+i] = Band2Tb8[nIndex3+i];
	}

	/* calculate the LSF coefficients from DC components and previous LSF */
	for ( i = 0; i < lpclen; i ++ ) {
		iTemp1 = LspDcTable[i];
		iTemp2 = pSrcPrevLsf[i] - iTemp1;
		iTemp2 = (Ipp16s)(((Ipp32s)iTemp2 * iPredict + (short)0x4000) >> 15);
		iLsf[i] += iTemp1 + iTemp2;
	}

	/* check stability */
	for ( i = 0; i < 10; i ++ ) {
		/* adjust the LSF coefficients */
		if ( iLsf[0] < (short)0x180 ) {
			iLsf[0] = (short)0x180;
		}
		if ( iLsf[lpclen-1] > (short)0x7e00 ) {
			iLsf[lpclen-1] = (short)0x7e00;
		}
		for ( j = 1; j < lpclen; j ++ ) {
			iTemp1 = iLsf[j-1] - iLsf[j] + iDiff;
			if ( iTemp1 > 0 ) {
				iTemp1 >>= 1;
				iLsf[j-1] -= iTemp1;
       				iLsf[j] += iTemp1;
			}
		}

		iTemp2 = 0;
		for ( j = 1; j < lpclen; j ++ ) {
			iTemp1 = iLsf[j-1] - iLsf[j] + iDiff - 4;
			if ( iTemp1 > 0 ) {
				iTemp2 = 1;
			}
		}
		if ( iTemp2 == 0 ) {
			break;
		}
	}

	/* output the LSF coefficients */
	if ( iTemp2 == 1 ) {
		for ( i = 0; i < lpclen; i ++ ) {
			pDstLsf[i] = pSrcPrevLsf[i];
		}
	} else {
		for ( i = 0; i < lpclen; i ++ ) {
			pDstLsf[i] = iLsf[i];
		}
	}

	return;
} /* _appsLSFQuantInv_G723 */


/*******lsp interpolation*******/  
void LspInterpolation_G723(short *lsp,short *prelsp,short *lpc)
{
  short tmp;
  short buf[lpclen];
  short temp[lpclen];
  int k;
//  printf("Welcome to LspInterpolation_G723\n");
  tmp=(short)0xe000;             // -0.25*2^15
  for(k=0;k<4;k++) {
    ippsMulC_16s_Sfs(tmp,prelsp,buf,lpclen,15);
    ippsMulC_16s_Sfs(tmp,lsp,temp,lpclen,15);
    ippsAdd_16s_I(prelsp,buf,lpclen);
    ippsSub_16s_I(temp,buf,lpclen);
    
    ippsLSFToLPC_G723_16s(buf,&lpc[k*lpclen]);
    tmp+=(short)0xe000;
   }
   return;
}
  
void   UpdateSineDetector(short *SinDet)    // if the abs(*sineDet) contains figure 1 no less than 14 bits, let it be negative, or positive
{
   int i, x, nBits;
  // printf("Welcome to UpdateSineDetector\n");
   *SinDet &= (short)0x7fff ;

   x= *SinDet;
   nBits = 0;
   for ( i = 0 ; i < 15 ; i ++ ) {
       nBits += x&1;
       x >>= 1;
   }
   if ( nBits >= 14 )
       *SinDet |= (short)0x8000 ;
}

/********Pack data for send*******/
void PackLine(Line *line,short rate,char *out_bytes,int *out_len,short Ftyp)
{
 char *bs;
 char bit;
 int dd,k;
// printf("Welcome to PackLine\n");
 bs=out_bytes;
 bit=0;
 
 for(k=0;k<24;k++)
     bs[k]=0;
 
 switch (Ftyp) {

     case 0 : {
           dd = 3;
           *out_len=1;
           break;
	   }
     case 2 : {
           dd = 2;
           *out_len=4;
           break;
	   }
     default : {
           if(rate==0) {
             dd=0;
             *out_len=24;
           }else {
             dd=1;
             *out_len=20;
           }
           break;
	   }
    }
 //pack control bits
 AddToBitstream(dd,&bs,&bit,2);
if(Ftyp==1) {
 //pack lspid
 dd=line->lspID;
 AddToBitstream(dd,&bs,&bit,24);
 
 //pack lag
 dd=line->olp[0]-pitchmin;
 AddToBitstream(dd,&bs,&bit,7);
 dd=line->closelag[1];
 AddToBitstream(dd,&bs,&bit,2);
 dd=line->olp[1]-pitchmin;
 AddToBitstream(dd,&bs,&bit,7);
 dd=line->closelag[3];
 AddToBitstream(dd,&bs,&bit,2);
 
 // pack combination gain
 for(k=0;k<4;k++) {
   dd=line->adptgid[k]*FixedCodeGainLev+line->ampindex[k]; 
   if(rate==0) 
     dd+=line->traindrt[k]<<11;
   AddToBitstream(dd,&bs,&bit,12);
   }
       
  //pack grid
  for(k=0;k<4;k++) {
    dd=line->grid[k];
    AddToBitstream(dd,&bs,&bit,1);
    }
    
 //pack pulse position
 if(rate==0) {//high bit rate
   dd=0;
   AddToBitstream(dd,&bs,&bit,1);                                   //1 bit reserved
   
   dd=(line->pos[0]>>16)*9+(line->pos[1]>>14);                                 //high 4 bits combine 13bits , each 4 bits not more than 9
   dd=dd*90+(line->pos[2]>>16)*9+(line->pos[3]>>14);
   AddToBitstream(dd,&bs,&bit,13);
   
   dd=line->pos[0]&(int)0xffff;
   AddToBitstream(dd,&bs,&bit,16);
   dd=line->pos[1]&(int)0x3fff;
   AddToBitstream(dd,&bs,&bit,14);
   dd=line->pos[2]&(int)0xffff;
   AddToBitstream(dd,&bs,&bit,16);
   dd=line->pos[3]&(int)0x3fff;
   AddToBitstream(dd,&bs,&bit,14);
   }
  else {   // low bit rate
    for(k=0;k<4;k++) {
      dd=line->pos[k];
      AddToBitstream(dd,&bs,&bit,12);
      }
    }
    
  //pack pulse sign
  if(rate==0) //high bit rate
   for(k=0;k<4;k++) {
    dd=line->sign[k];
    AddToBitstream(dd,&bs,&bit,6-(k&(int)0x01));
    }
  else
   for(k=0;k<4;k++) {
     dd=line->sign[k];
     AddToBitstream(dd,&bs,&bit,4);
     }
 }else if(Ftyp==2) {
   dd=line->lspID;
   AddToBitstream(dd,&bs,&bit,24);
   
   dd=line->ampindex[0];
   AddToBitstream(dd,&bs,&bit,6);
  }
    return;
}
   
/*******AddToBitStream**********/
void AddToBitstream(int dd,char **bs,char *bit_offset,int num)            //bit_offset points to the bit to loaded first(0-7)
{
 int j,data; 
 char tmp;
 
// printf("Welcome to AddToBitstream\n");
  data=dd;
  for(j=0;j<num;j++) {
    tmp=(char)(data&(int)0x01);
    tmp<<=(*bit_offset);
    (**bs)+=tmp;
    data>>=1;
    *bit_offset+=1;
    if(*bit_offset>7) {
      *bit_offset=0;
      (*bs)++;
      }
   }
  // printf("Current debug used message!  var=%d \n",dd);
}
       
    
  
/******Update previous Error*******/
void UpdateErr(int *preErr,short olp,short closelag,short gain_id,short rate)
{
  short beta;
  int i0=0,i1=0,lag,ilag,e0,e1;
  Ipp64s s64;
//  printf("Welcome to UpdateErr\n");
  if((rate==0)&&(olp<sfsize-2))  
     beta=tabgain85[gain_id];
  else
     beta=tabgain170[gain_id];
   
  lag=olp-1+closelag;
 /* 
  printf("Current debug used message!  olp=%d \n",beta);
  printf("Current debug used message!  closelag=%d \n",closelag);
  printf("Current debug used message!  lag=%d \n",lag);
 */ 
  if(lag > 30) {
        ilag = (lag * 273)>>13;   /*  x 1/30 */
        if(30*(ilag+1) != lag) {
            if(ilag == 1) {                       // lag from 31 to 59
               if(preErr[0] <= preErr[1]){
                  i0 = 1;
                  i1 = 1;
               }
            }else {                             // lag 61-89 and 91-119 and 121-142
               i0 = ilag-2;
               i1 = ilag-1;
               if(preErr[i1] > preErr[i0]){
                  i0 = i1;
               }
               if(preErr[i1] <= preErr[ilag]){
                  i1 = ilag;
               }
            }
        }
        else {  /* lag = 60, 90, 120 */
           i0 = ilag-1;
           i1 = ilag;
        }                                       // i0,i1 find the bigger Err from lag-2 to lag
    }
    e0=MulC_32s(beta,preErr[i0]);
    s64=e0*4+4;
    if(s64>IPP_MAX_32S)
      e0=IPP_MAX_32S;
    else if(s64<IPP_MIN_32S)
      e0=IPP_MIN_32S;
    else
      e0=(int)s64;
    e1=MulC_32s(beta,preErr[i1]);
    s64=e1*4+4;
    if(s64>IPP_MAX_32S)
      e1=IPP_MAX_32S;
    else if(s64<IPP_MIN_32S)
      e1=IPP_MIN_32S;
    else