g722_32c.c

dsp AD公司ADSP21的代码,里面有FFT FIR IIR EQULIZER G722_21F 等可以在项目中直接应用的代码.此代码的来源是ADI公司自己出版的书籍,此书在美国购得

#include <libap.h>
#include <macros.h>

/* g722 encode and decode demo for DSP32C */

/* floating point version of G722 code */

/* float constants replace shifts */

#define SHIFT_15 3.0517578e-5
#define SHIFT_14 6.1035156e-5
#define SHIFT_12 2.4414062e-4
#define SHIFT_11 4.8828125e-4
#define SHIFT_8  3.9062500e-3
#define SHIFT_7  7.8125000e-3
#define SHIFT_6  1.5625000e-2
#define SHIFT_5  3.1250000e-2
#define C255_256 0.99609375
#define C127_128 0.99218750

int encode();
void decode();
float filtez();
void upzero();
float filtep();
float quantl();
int logscl();
float scalel();
float uppol2();
float uppol1();
int logsch();
void reset();

/* G722 C code translated from some assembly code */

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

/* QMF filter coefficients:
scaled by a factor of 4 compared to G722 CCITT recomendation */
float 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
};

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

/* outputs of decode */
float xout1,xout2;

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

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

float 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
};

float 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
};

float delay_bpl[6];

float delay_dltx[6];

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

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

int 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
};

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

/* decision levels - pre-multiplied by 8, 0 to indicate end */
float 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
};

float         detl;

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

float         deth;

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

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

int           nbh;

float         delay_dhx[6];

float         delay_bph[6];

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

/* variables for decoder here */
float         dec_deth,dec_detl,dec_dlt;

float         dec_del_bpl[6];

float         dec_del_dltx[6];

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

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

float         dec_del_dhx[6];

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

float     dec_sh,dec_rh;

float     dec_ph1,dec_ph2;

/* G722 encode function two floats in, one float out */

int encode(xin1,xin2)
    float xin1,xin2;
{
    register int   i;
    register float *h_ptr;
    register float *tqmf_ptr,*tqmf_ptr1;
    register float xa,xb;
    float xl,xh;
    float decis;
    float         sh;         /* this comes from adaptive predictor */
    float         eh;
    float         dh;
    int           il,ih;
    float szh,sph,ph,yh; 
    float szl,spl,sl,el;

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

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

/* update delay line tqmf */
    tqmf_ptr1 = tqmf_ptr - 2;
    for(i = 0 ; i < 11 ; i++) {
        *tqmf_ptr-- = *tqmf_ptr1--;
        *tqmf_ptr-- = *tqmf_ptr1--;
    }
    *tqmf_ptr-- = xin1;
    *tqmf_ptr = xin2;

    xl = (xa + xb)*SHIFT_15;
    xh = (xa - xb)*SHIFT_15;

/* end of quadrature mirror filter code */

/* into regular encoder segment here */
/* 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: does both invqal and invqbl- computes quantized difference signal */
/* for invqbl, truncate by 2 lsbs, so mode = 3 */

/* invqal case with mode = 3 */
    dlt = (detl*qq4_code4_table[il >> 2])*SHIFT_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, dlti(circ pofloater for delaying */
/* 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) */
/* return: ih */
    if(eh >= 0.0) {
        ih = 3;     /* 2,3 are pos codes */
    }
    else {
        ih = 1;     /* 0,1 are neg codes */
    }
    decis = (564.0*deth)*SHIFT_12;
    if(abs(eh) > decis) ih--;     /* mih = 2 case */

/* invqah: in-place compute the quantized difference signal
 in the higher sub-band*/

    dh = (deth*qq2_code2_table[ih])*SHIFT_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(in place)*/
    ph = dh + szh;

/* upzero: update zero section predictor coefficients (sixth order) */
/* calling parameters: dh, dhi(circ), bphi (circ) */
/* 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 */
/* return params:  aph2 */

    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;

/* done with higher sub-band encoder, now Delay for next time */
    rh2 = rh1;
    rh1 = yh;
    ph2 = ph1;
    ph1 = ph;

/* multiplexing ih and il to get signals together */
    return(il | (ih << 6));
}

/* decode function, result in xout1 and xout2 */

void decode(input)
    int input;
{
    register int i;
    register float xa1,xa2;    /* qmf accumulators */
    register float *h_ptr;
    register float *ac_ptr,*ac_ptr1,*ad_ptr,*ad_ptr1;
    int           ilr,ih;
    float xs,xd;
    float         rl,rh;
    float     dl;

/* split transmitted word from input floato ilr and ih */
    ilr = input & 0x3f;
    ih = SHIFT_6*(float)input;

/* LOWER SUB_BAND DECODER */

/* filtez: compute predictor output for zero section */