g722.~c

G722语音压缩算法

/* $Id: adpcm.c,v 1.7 2005/06/15 07:27:31 ael01 Exp $ */
/*************************************************************************/
/*                                                                       */
/*   SNU-RT Benchmark Suite for Worst Case Timing Analysis               */
/*   =====================================================               */
/*                              Collected and Modified by S.-S. Lim      */
/*                                           [e-mail]       */
/*                                         Real-Time Research Group      */
/*                                        Seoul National University      */
/*                                                                       */
/*                                                                       */
/*        < Features > - restrictions for our experimental environment   */
/*                                                                       */
/*          1. Completely structured.                                    */
/*               - There are no unconditional jumps.                     */
/*               - There are no exit from loop bodies.                   */
/*                 (There are no 'break' or 'return' in loop bodies)     */
/*          2. No 'switch' statements.                                   */
/*          3. No 'do..while' statements.                                */
/*          4. Expressions are restricted.                               */
/*               - There are no multiple expressions joined by 'or',     */
/*                'and' operations.                                      */
/*          5. No library calls.                                         */
/*               - All the functions needed are implemented in the       */
/*                 source file.                                          */
/*          6. PrINT32outs removed (Jan G)                                 */
/*                                                                       */
/*                                                                       */
/*                                                                       */
/*************************************************************************/
/*                                                                       */
/*  FILE: adpcm.c                                                        */
/*  SOURCE : C Algorithms for Real-Time DSP by P. M. Embree              */
/*                                                                       */
/*  DESCRIPTION :                                                        */
/*                                                                       */
/*     CCITT G.722 ADPCM (Adaptive Differential Pulse Code Modulation)   */
/*     algorithm.                                                        */
/*     16khz sample rate data is stored in the array test_data[SIZE].    */
/*     Results are stored in the array compressed[SIZE] and result[SIZE].*/
/*     Execution time is determined by the constant SIZE (default value  */
/*     is 2000).                                                         */
/*                                                                       */
/*  REMARK :                                                             */
/*                                                                       */
/*  EXECUTION TIME :                                                     */
/*                                                                       */
/*                                                                       */
/*************************************************************************/

/* To be able to run with printouts 
#include <stdio.h> */

/* common sampling rate for sound cards on IBM/PC */
#define SAMPLE_RATE 11025

#define PI 3141
#define SIZE 3
#define IN_END 4
//typedef long INT32;
typedef long INT32;
typedef short INT16;

/* COMPLEX STRUCTURE */

typedef struct {
    INT32 real, imag;
} COMPLEX;

/* function prototypes for fft and filter functions */
void fft(COMPLEX *,INT32);
INT32 fir_filter(INT32 input,INT32 *coef,INT32 n,INT32 *history);
INT32 iir_filter(INT32 input,INT32 *coef,INT32 n,INT32 *history);
INT32 gaussian(void);
INT32 my_abs(INT32 n);

//void setup_codec(INT32),key_down(),INT32_enable(),INT32_disable();
INT32 flags(INT32);

INT32 getinput(void);
void sendout(INT32),flush();

INT32 encode(INT32,INT32);
//INT32 encode(INT32,INT32,INT32 *);
//void decode(INT32);
void decode(INT32, INT32);
INT32 filtez(INT32 *bpl,INT32 *dlt);
void upzero(INT32 dlt,INT32 *dlti,INT32 *bli);
INT32 filtep(INT32 rlt1,INT32 al1,INT32 rlt2,INT32 al2);
INT32 quantl(INT32 el,INT32 detl);
/* INT32 invqxl(INT32 il,INT32 detl,INT32 *code_table,INT32 mode); */
INT32 logscl(INT32 il,INT32 nbl);
INT32 scalel(INT32 nbl,INT32 shift_constant);
INT32 uppol2(INT32 al1,INT32 al2,INT32 plt,INT32 plt1,INT32 plt2);
INT32 uppol1(INT32 al1,INT32 apl2,INT32 plt,INT32 plt1);
/* INT32 invqah(INT32 ih,INT32 deth); */
INT32 logsch(INT32 ih,INT32 nbh);
void reset();
INT32 my_fabs(INT32 n);
INT32 my_cos(INT32 n);
INT32 my_sin(INT32 n);

/* G722 C code */

/* variables for transimit quadrature mirror filter here */
INT32 tqmf[24];

/* QMF filter coefficients:
scaled by a factor of 4 compared to G722 CCITT recommendation */
INT32 h[24] = {
    12,   -44,   -44,   212,    48,  -624,   128,  1448,
  -840, -3220,  3804, 15504, 15504,  3804, -3220,  -840,
  1448,   128,  -624,    48,   212,   -44,   -44,    12
};

INT32 xl,xh;

INT32 data1,data2;

/* variables for receive quadrature mirror filter here */
INT32 accumc[11],accumd[11];

/* outputs of decode() */
INT32 xout1,xout2;

INT32 xs,xd;

/* variables for encoder (hi and lo) here */

INT32 il,szl,spl,sl,el;

INT32 qq4_code4_table[16] = {
     0,  -20456,  -12896,   -8968,   -6288,   -4240,   -2584,   -1200,
 20456,   12896,    8968,    6288,    4240,    2584,    1200,       0
};

INT32 qq5_code5_table[32] = {
  -280,    -280,  -23352,  -17560,  -14120,  -11664,   -9752,   -8184,
 -6864,   -5712,   -4696,   -3784,   -2960,   -2208,   -1520,    -880,
 23352,   17560,   14120,   11664,    9752,    8184,    6864,    5712,
  4696,    3784,    2960,    2208,    1520,     880,     280,    -280
};

INT32 qq6_code6_table[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
};

INT32 delay_bpl[6];

INT32 delay_dltx[6];

INT32 wl_code_table[16] = {
   -60,  3042,  1198,   538,   334,   172,    58,   -30,
  3042,  1198,   538,   334,   172,    58,   -30,   -60
};

INT32 wl_table[8] = {
   -60,   -30,    58,   172,   334,   538,  1198,  3042
};

INT32 ilb_table[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
};

INT32         nbl;                  /* delay line */
INT32         al1,al2;
INT32         plt,plt1,plt2;
INT32         rs;
INT32         dlt;
INT32         rlt,rlt1,rlt2;

/* decision levels - pre-multiplied by 8, 0 to indicate end */
INT32 decis_levl[30] = {
   280,   576,   880,  1200,  1520,  1864,  2208,  2584,
  2960,  3376,  3784,  4240,  4696,  5200,  5712,  6288,
  6864,  7520,  8184,  8968,  9752, 10712, 11664, 12896,
 14120, 15840, 17560, 20456, 23352, 32767
};

INT32         detl;

/* quantization table 31 INT32 to make quantl look-up easier,
last entry is for mil=30 case when wd is max */
INT32 quant26bt_pos[31] = {
    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,    32
};

/* quantization table 31 INT32 to make quantl look-up easier,
last entry is for mil=30 case when wd is max */
INT32 quant26bt_neg[31] = {
    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,     4
};


INT32         deth;
INT32         sh;         /* this comes from adaptive predictor */
INT32         eh;

INT32 qq2_code2_table[4] = {
  -7408,   -1616,   7408,  1616
};

INT32 wh_code_table[4] = {
   798,   -214,    798,   -214
};


INT32         dh,ih;
INT32         nbh,szh;
INT32         sph,ph,yh,rh;

INT32         delay_dhx[6];

INT32         delay_bph[6];

INT32         ah1,ah2;
INT32         ph1,ph2;
INT32         rh1,rh2;

/* variables for decoder here */
INT32         ilr,yl,rl;
INT32         dec_deth,dec_detl,dec_dlt;

INT32         dec_del_bpl[6];

INT32         dec_del_dltx[6];

INT32     dec_plt,dec_plt1,dec_plt2;
INT32     dec_szl,dec_spl,dec_sl;
INT32     dec_rlt1,dec_rlt2,dec_rlt;
INT32     dec_al1,dec_al2;
INT32     dl;
INT32     dec_nbl,dec_yh,dec_dh,dec_nbh;

/* variables used in filtez */
INT32         dec_del_bph[6];

INT32         dec_del_dhx[6];

INT32         dec_szh;
/* variables used in filtep */
INT32         dec_rh1,dec_rh2;
INT32         dec_ah1,dec_ah2;
INT32         dec_ph,dec_sph;

INT32     dec_sh,dec_rh;

INT32     dec_ph1,dec_ph2;

/* G722 encode function two INT32s in, one 8 bit output */

/* put input samples in xin1 = first value, xin2 = second value */
/* returns il and ih stored together */

/* MAX: 1 */
INT32 my_abs(INT32 n)
{
  INT32 m;

  if (n >= 0) m = n;
  else m = -n;
  return m;
}

/* MAX: 1 */
INT32 my_fabs(INT32 n)
{
  INT32 f;

  if (n >= 0) f = n;
  else f = -n;
  return f;
}

INT32 my_sin(INT32 rad)
{
  INT32 diff;
  INT32 app=0;

  INT32 inc = 1;

  /* MAX dependent on rad's value, say 50 */
  while (rad > 2*PI)
      rad -= 2*PI;
  /* MAX dependent on rad's value, say 50 */
  while (rad < -2*PI)
      rad += 2*PI;
   diff = rad;
   app = diff;
  diff = (diff * (-(rad*rad))) /
     ((2 * inc) * (2 * inc + 1));
  app = app + diff;
  inc++;
  /* REALLY: while(my_fabs(diff) >= 0.00001) { */
  /* MAX: 1000 */
  while(my_fabs(diff) >= 1)
  {
    diff = (diff * (-(rad*rad))) /
	((2 * inc) * (2 * inc + 1));
    app = app + diff;
    inc++;
  }

  return app;
}


INT32 my_cos(INT32 rad)
{
  return (my_sin (PI / 2 - rad));
}


/* MAX: 1 */
INT32 encode(INT32 xin1,INT32 xin2)
//INT32 encode(INT32 xin1,INT32 xin2,INT32 *out)
{
    INT32 i;
    INT32 *h_ptr,*tqmf_ptr,*tqmf_ptr1;
    //long int xa,xb;
    INT32 xa,xb;
    INT32 decis;

/* transmit quadrature mirror filters implemented here */
    h_ptr = h;
    tqmf_ptr = tqmf;
    xa = (INT32)(*tqmf_ptr++) * (*h_ptr++);
    xb = (INT32)(*tqmf_ptr++) * (*h_ptr++);
/* main multiply accumulate loop for samples and coefficients */
    /* MAX: 10 */
    for(i = 0 ; i < 10 ; i++)
    {
        xa += (INT32)(*tqmf_ptr++) * (*h_ptr++);
        xb += (INT32)(*tqmf_ptr++) * (*h_ptr++);
    }
/* final mult/accumulate */
    xa += (INT32)(*tqmf_ptr++) * (*h_ptr++);
    xb += (INT32)(*tqmf_ptr) * (*h_ptr++);

/* update delay line tqmf */
    tqmf_ptr1 = tqmf_ptr - 2;
    /* MAX: 22 */
    for(i = 0 ; i < 22 ; i++) *tqmf_ptr-- = *tqmf_ptr1--;
    *tqmf_ptr-- = xin1;
    *tqmf_ptr = xin2;

/* scale outputs */
    /*
    xl = (xa + xb) >> 15;
    xh = (xa - xb) >> 15;
    */ //06.6.9
    xl = (xa + xb) + (16384) >> 15;
    xh = (xa - xb) + (16384) >> 15;

/* end of quadrature mirror filter code */

/* starting with lower sub band encoder */

/* filtez - compute predictor output section - zero section */
    szl = filtez(delay_bpl,delay_dltx);

/* filtep - compute predictor output signal (pole section) */
    spl = filtep(rlt1,al1,rlt2,al2);

/* compute the predictor output value in the lower sub_band encoder */
    sl = szl + spl;
    el = xl - sl;

/* quantl: quantize the difference signal */
    il = quantl(el,detl);

/* invqxl: computes quantized difference signal */
/* for invqbl, truncate by 2 lsbs, so mode = 3 */
    //dlt = ((INT32)detl*qq4_code4_table[il >> 2]) >> 15; //06.6.9
   dlt = ((INT32)detl*qq4_code4_table[il >> 2]) + (16384) >> 15;

/* logscl: updates logarithmic quant. scale factor in low sub band */
    nbl = logscl(il,nbl);

/* scalel: compute the quantizer scale factor in the lower sub band */
/* calling parameters nbl and 8 (constant such that scalel can be scaleh) */
    detl = scalel(nbl,8);

/* parrec - simple addition to compute recontructed signal for adaptive pred */
    plt = dlt + szl;

/* upzero: update zero section predictor coefficients (sixth order)*/
/* calling parameters: dlt, dlt1, dlt2, ..., dlt6 from dlt */
/*  bpli (linear_buffer in which all six values are delayed */
/* return params:      updated bpli, delayed dltx */
    upzero(dlt,delay_dltx,delay_bpl);

/* uppol2- update second predictor coefficient apl2 and delay it as al2 */
/* calling parameters: al1, al2, plt, plt1, plt2 */
    al2 = uppol2(al1,al2,plt,plt1,plt2);

/* uppol1 :update first predictor coefficient apl1 and delay it as al1 */
/* calling parameters: al1, apl2, plt, plt1 */
    al1 = uppol1(al1,al2,plt,plt1);

/* recons : compute recontructed signal for adaptive predictor */
    rlt = sl + dlt;

/* done with lower sub_band encoder; now implement delays for next time*/
    rlt2 = rlt1;
    rlt1 = rlt;
    plt2 = plt1;
    plt1 = plt;

/* high band encode */

    szh = filtez(delay_bph,delay_dhx);

    sph = filtep(rh1,ah1,rh2,ah2);

/* predic: sh = sph + szh */
    sh = sph + szh;
/* subtra: eh = xh - sh */
    eh = xh - sh;

/* quanth - quantization of difference signal for higher sub-band */
/* quanth: in-place for speed params: eh, deth (has init. value) */
    if(eh >= 0)
    {
        ih = 3;     /* 2,3 are pos codes */
    }
    else
    {
        ih = 1;     /* 0,1 are neg codes */
    }
    //decis = (564*deth) >> 12; //06.6.9
    decis = (564*deth) + (1<<11) >> 12;
    if(my_abs(eh) > decis) ih--;     /* mih = 2 case */

/* invqah: compute the quantized difference signal, higher sub-band*/
    //dh = ((INT32)deth*qq2_code2_table[ih]) >> 15L ;   //06.6.9
    dh = ((INT32)deth*qq2_code2_table[ih]) + (16384) >> 15 ;

/* logsch: update logarithmic quantizer scale factor in hi sub-band*/
    nbh = logsch(ih,nbh);

/* note : scalel and scaleh use same code, different parameters */
    deth = scalel(nbh,10);

/* parrec - add pole predictor output to quantized diff. signal */
    ph = dh + szh;

/* upzero: update zero section predictor coefficients (sixth order) */
/* calling parameters: dh, dhi, bphi */
/* return params: updated bphi, delayed dhx */
    upzero(dh,delay_dhx,delay_bph);

/* uppol2: update second predictor coef aph2 and delay as ah2 */
/* calling params: ah1, ah2, ph, ph1, ph2 */
    ah2 = uppol2(ah1,ah2,ph,ph1,ph2);

/* uppol1:  update first predictor coef. aph2 and delay it as ah1 */
    ah1 = uppol1(ah1,ah2,ph,ph1);

/* recons for higher sub-band */
    yh = sh + dh;