g722c3f.c

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

    float     dl;
    int           ilr,ih;
    int i;

/* split transmitted word from input floato ilr and ih */
    ilr = input & 0x3f;
    ih = input >> 6;

/* LOWER SUB_BAND DECODER */

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

    dec_szl = filtez(dec_del_bpl,dec_del_dltx);

/* filtep: compute predictor output signal for pole section */

    dec_spl = filtep(dec_rlt1,dec_al1,dec_rlt2,dec_al2);

    dec_sl = dec_spl + dec_szl;

/* invqxl: compute quantized difference signal for adaptive predic in low sb */
    dec_dlt = (dec_detl*qq4_code4_table[ilr >> 2])*SHIFT_15;

/* invqxl: compute quantized difference signal for decoder output in low sb */
    dl = (dec_detl*qq6_code6_table[ilr])*SHIFT_15;

    rl = dl + dec_sl;

/* logscl: quantizer scale factor adaptation in the lower sub-band */

    dec_nbl = logscl(ilr,dec_nbl);

/* scalel: computes quantizer scale factor in the lower sub band */

    dec_detl = scalel(dec_nbl,8);

/* parrec - add pole predictor output to quantized diff. signal(in place) */
/* for partially reconstructed signal */

    dec_plt = dec_dlt + dec_szl;

/* upzero: update zero section predictor coefficients */

    upzero(dec_dlt,dec_del_dltx,dec_del_bpl);

/* uppol2: update second predictor coefficient apl2 and delay it as al2 */

    dec_al2 = uppol2(dec_al1,dec_al2,dec_plt,dec_plt1,dec_plt2);

/* uppol1: update first predictor coef. (pole setion) */

    dec_al1 = uppol1(dec_al1,dec_al2,dec_plt,dec_plt1);

/* recons : compute recontructed signal for adaptive predictor */
    dec_rlt = dec_sl + dec_dlt;

/* done with lower sub band decoder, implement delays for next time */
    dec_rlt2 = dec_rlt1;
    dec_rlt1 = dec_rlt;
    dec_plt2 = dec_plt1;
    dec_plt1 = dec_plt;

/* HIGH SUB-BAND DECODER */

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

    dec_szh = filtez(dec_del_bph,dec_del_dhx);

/* filtep: compute predictor output signal for pole section */

    dec_sph = filtep(dec_rh1,dec_ah1,dec_rh2,dec_ah2);

/* predic:compute the predictor output value in the higher sub_band decoder */

    dec_sh = dec_sph + dec_szh;

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

    dec_dh = (dec_deth*qq2_code2_table[ih])*SHIFT_15;

/* logsch: update logarithmic quantizer scale factor in hi sub band */

    dec_nbh = logsch(ih,dec_nbh);

/* scalel: compute the quantizer scale factor in the higher sub band */

    dec_deth = scalel(dec_nbh,10);

/* parrec: compute partially recontructed signal */
    dec_ph = dec_dh + dec_szh;

/* upzero: update zero section predictor coefficients */

    upzero(dec_dh,dec_del_dhx,dec_del_bph);

/*uppol2: update second predictor coefficient aph2 and delay it as ah2 */

    dec_ah2 = uppol2(dec_ah1,dec_ah2,dec_ph,dec_ph1,dec_ph2);

/* uppol1: update first predictor coef. (pole setion) */

    dec_ah1 = uppol1(dec_ah1,dec_ah2,dec_ph,dec_ph1);

/* recons : compute recontructed signal for adaptive predictor */
    rh = dec_sh + dec_dh;

/* done with high band decode, implementing delays for next time here */
    dec_rh2 = dec_rh1;
    dec_rh1 = rh;
    dec_ph2 = dec_ph1;
    dec_ph1 = dec_ph;

/* end of higher sub_band decoder */

/* end with receive quadrature mirror filters */   
    xd = rl - rh;
    xs = rl + rh;

/* receive quadrature mirror filters implemented here */
    h_ptr = h;
    ac_ptr = accumc;
    ad_ptr = accumd;
    xa1 = xd * (*h_ptr++);
    xa2 = xs * (*h_ptr++);
/* main multiply accumulate loop for samples and coefficients */
/* replace:
    for(i = 0 ; i < 10 ; i++) {
        xa1 += (*ac_ptr++) * (*h_ptr++);
        xa2 += (*ad_ptr++) * (*h_ptr++);
    }
*/
        xa1 += (*ac_ptr++) * (*h_ptr++);
        xa2 += (*ad_ptr++) * (*h_ptr++);
        xa1 += (*ac_ptr++) * (*h_ptr++);
        xa2 += (*ad_ptr++) * (*h_ptr++);
        xa1 += (*ac_ptr++) * (*h_ptr++);
        xa2 += (*ad_ptr++) * (*h_ptr++);
        xa1 += (*ac_ptr++) * (*h_ptr++);
        xa2 += (*ad_ptr++) * (*h_ptr++);
        xa1 += (*ac_ptr++) * (*h_ptr++);
        xa2 += (*ad_ptr++) * (*h_ptr++);
        xa1 += (*ac_ptr++) * (*h_ptr++);
        xa2 += (*ad_ptr++) * (*h_ptr++);
        xa1 += (*ac_ptr++) * (*h_ptr++);
        xa2 += (*ad_ptr++) * (*h_ptr++);
        xa1 += (*ac_ptr++) * (*h_ptr++);
        xa2 += (*ad_ptr++) * (*h_ptr++);
        xa1 += (*ac_ptr++) * (*h_ptr++);
        xa2 += (*ad_ptr++) * (*h_ptr++);
        xa1 += (*ac_ptr++) * (*h_ptr++);
        xa2 += (*ad_ptr++) * (*h_ptr++);
/* final mult/accumulate */
    xa1 += (*ac_ptr) * (*h_ptr++);
    xa2 += (*ad_ptr) * (*h_ptr++);

/* scale by 2^14 */
    xout1 = xa1*SHIFT_14;
    xout2 = xa2*SHIFT_14;

/* update delay lines */
    ac_ptr1 = ac_ptr - 1;
    ad_ptr1 = ad_ptr - 1;
    for(i = 0 ; i < 5 ; i++) {
        *ac_ptr-- = *ac_ptr1--;
        *ad_ptr-- = *ad_ptr1--;
        *ac_ptr-- = *ac_ptr1--;
        *ad_ptr-- = *ad_ptr1--;
    }
    *ac_ptr = xd;
    *ad_ptr = xs;

}

/* clear all storage locations */

void reset()
{
    int i;

    detl = dec_detl = 32;   /* reset to min scale factor */
    deth = dec_deth = 8;
    nbl = al1 = al2 = plt1 = plt2 = rlt1 = rlt2 = 0;
    nbh = ah1 = ah2 = ph1 = ph2 = rh1 = rh2 = 0;
    dec_nbl = dec_al1 = dec_al2 = dec_plt1 = dec_plt2 = dec_rlt1 = dec_rlt2 = 0;
    dec_nbh = dec_ah1 = dec_ah2 = dec_ph1 = dec_ph2 = dec_rh1 = dec_rh2 = 0;

    for(i = 0 ; i < 6 ; i++) {
        delay_dltx[i] = 0;
        delay_dhx[i] = 0;
        dec_del_dltx[i] = 0;
        dec_del_dhx[i] = 0;
    }

    for(i = 0 ; i < 6 ; i++) {
        delay_bpl[i] = 0;
        delay_bph[i] = 0;
        dec_del_bpl[i] = 0;
        dec_del_bph[i] = 0;
    }

    for(i = 0 ; i < 24 ; i++) tqmf[i] = 0;

    for(i = 0 ; i < 11 ; i++) {
        accumc[i] = 0;
        accumd[i] = 0;
    }
}

/* filtez - compute predictor output signal (zero section) */
/* input: bpl1-6 and dlt1-6, output: szl */

inline float filtez(float *bpli,float *dlti)
{
    int i;
    float zl;
    register float *bpl,*dlt;
    bpl = bpli;
    dlt = dlti;
    zl = (*bpl++) * (*dlt++);
        zl += (*bpl++) * (*dlt++);
        zl += (*bpl++) * (*dlt++);
        zl += (*bpl++) * (*dlt++);
        zl += (*bpl++) * (*dlt++);
        zl += (*bpl++) * (*dlt++);

    return(zl*SHIFT_14);   /* x2 here */
}

/* filtep - compute predictor output signal (pole section) */
/* input rlt1-2 and al1-2, output spl */

inline float filtep(float rlt1,float al1,float rlt2,float al2)
{
    float pl;
    pl = al1*rlt1;
    pl += al2*rlt2;
    return(pl*SHIFT_14);   /* x2 here */
}

/* quantl - quantize the difference signal in the lower sub-band */
inline float quantl(float el,float detl)
{
    int mil;
    float ril;
    float wd,decis;

/* abs of difference signal */
    wd = abs(el);
    detl *= SHIFT_15;
/* determine mil based on decision levels and detl gain */
/* replaced:
    for(mil = 0 ; mil < 30 ; mil++) {
        decis = decis_levl[mil]*detl;
        if(wd < decis) break;
    }
*/
    decis = decis_levl[14]*detl;
    if(wd < decis) {
      for(mil = 0 ; mil < 14 ; mil++) {
        decis = decis_levl[mil]*detl;
        if(wd < decis) break;
      }
    }
    else {
      for(mil = 15 ; mil < 30 ; mil++) {
        decis = decis_levl[mil]*detl;
        if(wd < decis) break;
      }
    }
/* if mil=30 then wd is less than all decision levels */
    if(el >= 0.0f) ril = quant26bt_pos[mil];
    else ril = quant26bt_neg[mil];
    return(ril);
}

/* logscl - update the logarithmic quantizer scale factor in lower sub-band */
/* note that nbl is passed and returned */

inline int logscl(int il,int nbl)
{
    float wd;
    wd = nbl * C127_128;   /* leak factor 127/128 */
    nbl = wd + wl_code_table[il >> 2];
    if(nbl < 0) nbl = 0;
    if(nbl > 18432) nbl = 18432;
    return(nbl);
}

/* scalel: compute the quantizer scale factor in the lower or upper sub-band*/

inline float scalel(int nbl,int shift_constant)
{
    int wd1,wd2,wd3;
    wd1 = (nbl >> 6) & 31;
    wd2 = nbl >> 11;
    wd3 = ilb_table[wd1] >> (shift_constant + 1 - wd2);
    return(8.0f*wd3);
}

/* upzero - inputs: dlt, dlti[0-5], bli[0-5], outputs: updated bli[0-5] */
/* also implements delay of bli and update of dlti from dlt */

inline void upzero(float dlt,float *dlti,float *bli)
{
    int i;
    float wd2,wd3;
/*if dlt is zero, then no sum floato bli */
    if((int)dlt == 0) {
        for(i = 0 ; i < 6 ; i++) {
            bli[i] = C255_256*bli[i];    /* leak factor of 255/256 */
        }
    }
    else {
        for(i = 0 ; i < 6 ; i++) {
            if(dlt*dlti[i] >= 0.0f) wd2 = 128; else wd2 = -128;
            wd3 = C255_256*bli[i];    /* leak factor of 255/256 */
            bli[i] = wd2 + wd3;
        }
    }
/* implement delay line for dlt */
    dlti[5] = dlti[4];
    dlti[4] = dlti[3];
    dlti[3] = dlti[2];
    dlti[2] = dlti[1];
    dlti[1] = dlti[0];
    dlti[0] = dlt;
}

/* uppol2 - update second predictor coefficient (pole section) */
/* inputs: al1, al2, plt, plt1, plt2. outputs: apl2 */

inline float uppol2(float al1,float al2,float plt,float plt1,float plt2)
{
    float wd2;
    float apl2;
    wd2 = 4.0f*al1;
    if(plt*plt1 >= 0.0f) wd2 = -wd2;    /* check same sign */
    wd2 = wd2*SHIFT_7;                  /* gain of 1/128 */
    if(plt*plt2 >= 0.0f) {
        wd2 = wd2 + 128.0f;             /* same sign case */
    }
    else {
        wd2 = wd2 - 128.0f;
    }
    apl2 = wd2 + C127_128*al2;  /* leak factor of 127/128 */

/* apl2 is limited to +-.75 */
    if(apl2 > 12288.0f) apl2 = 12288.0f;
    if(apl2 < -12288.0f) apl2 = -12288.0f;
    return(apl2);
}

/* uppol1 - update first predictor coefficient (pole section) */
/* inputs: al1, apl2, plt, plt1. outputs: apl1 */

inline float uppol1(float al1,float apl2,float plt,float plt1)
{
    float wd2;
    float wd3,apl1;
    wd2 = al1*C255_256;   /* leak factor of 255/256 */
    if(plt*plt1 >= 0.0f) {
        apl1 = wd2 + 192.0f;      /* same sign case */
    }
    else {
        apl1 = wd2 - 192.0f;
    }
/* note: wd3= .9375-.75 is always positive */
    wd3 = 15360.0f - apl2;             /* limit value */
    if(apl1 > wd3) apl1 = wd3;
    if(apl1 < -wd3) apl1 = -wd3;
    return(apl1);
}

/* logsch - update the logarithmic quantizer scale factor in higher sub-band */
/* note that nbh is passed and returned */

inline int logsch(int ih,int nbh)
{
    float wd;
    wd = nbh*C127_128;       /* leak factor 127/128 */
    nbh = wd + wh_code_table[ih];
    if(nbh < 0) nbh = 0;
    if(nbh > 22528) nbh = 22528;
    return(nbh);
}