pst.c

ITU-T G.729 Source code(已经验证过的)

    /* Local variables */
    int n, i;
    Word16 *ptr2;
    Word16 temp;
    Word32 L_acc;

    temp  = sub(phase, 1);
    temp  = shl(temp, L2_LH2_L);
    ptr_h = tab_hup_l + temp;   /* tab_hup_l + LH2_L * (phase-1) */

    temp  = sub(LH_UP_L, ltpdel);
    ptr2  = s_in + temp; ;

    /* Compute y_up */
    for(n = 0; n<L_SUBFR; n++) {
        L_acc = 0L;
        for(i=0; i<LH2_L; i++) {
            L_acc = L_mac(L_acc, ptr_h[i], (*ptr2--));
        }
        y_up[n] = round(L_acc);
        ptr2 += LH2_L_P1;
    }


    /* Compute num */
    L_acc = 0L;
    for(n=0; n<L_SUBFR; n++)    {
        L_acc = L_mac(L_acc, y_up[n], s_in[n]);
    }
    if(L_acc < 0L) {
        *num = 0;
        *sh_num = 0;
    }
    else {
        temp = sub(16, norm_l(L_acc));
        if(temp < 0) {
            temp = 0;
        }
        L_acc = L_shr(L_acc, temp);   /* with temp >= 0 */
        *num = extract_l(L_acc);
        *sh_num = temp;
    }

    /* Compute den */
    L_acc = 0L;
    for(n=0; n<L_SUBFR; n++)    {
        L_acc = L_mac(L_acc, y_up[n], y_up[n]);
    }
    temp = sub(16, norm_l(L_acc));
    if(temp < 0) {
        temp = 0;
    }
    L_acc = L_shr(L_acc, temp);     /* with temp >= 0 */
    *den = extract_l(L_acc);
    *sh_den = temp;

    return;
}

/*----------------------------------------------------------------------------
 *  select_ltp : selects best of (gain1, gain2)
 *  with gain1 = num1 * 2** sh_num1 / den1 * 2** sh_den1
 *  and  gain2 = num2 * 2** sh_num2 / den2 * 2** sh_den2
 *----------------------------------------------------------------------------
 */
static Word16 select_ltp(  /* output : 1 = 1st gain, 2 = 2nd gain */
 Word16 num1,       /* input : numerator of gain1 */
 Word16 den1,       /* input : denominator of gain1 */
 Word16 sh_num1,    /* input : just. factor for num1 */
 Word16 sh_den1,    /* input : just. factor for den1 */
 Word16 num2,       /* input : numerator of gain2 */
 Word16 den2,       /* input : denominator of gain2 */
 Word16 sh_num2,    /* input : just. factor for num2 */
 Word16 sh_den2)    /* input : just. factor for den2 */
{
    Word32 L_temp1, L_temp2;
    Word16 temp1, temp2;
    Word16 hi, lo;
    Word32 L_temp;

    if(den2 == 0) {

        return(1);
    }

    /* compares criteria = num**2/den */
    L_temp1 = L_mult(num1, num1);
    L_Extract(L_temp1, &hi, &lo);
    L_temp1 = Mpy_32_16(hi, lo, den2);

    L_temp2 = L_mult(num2, num2);
    L_Extract(L_temp2, &hi, &lo);
    L_temp2 = Mpy_32_16(hi, lo, den1);

    /* temp1 = sh_den2 + 2 * sh_num1 */
    temp1 = shl(sh_num1, 1);
    temp1 = add(temp1, sh_den2);
    /* temp2 = sh_den1 + 2 * sh_num2; */
    temp2 = shl(sh_num2, 1);
    temp2 = add(temp2, sh_den1);

    if(sub(temp2 ,temp1)>0) {
        temp2 = sub(temp2, temp1);
        L_temp1 = L_shr(L_temp1, temp2);    /* temp2 > 0 */
    }
    else {
        if(sub(temp1 ,temp2) >0){
            temp1 = sub(temp1, temp2);
            L_temp2 = L_shr(L_temp2, temp1);    /* temp1 > 0 */
        }
    }

    L_temp = L_sub(L_temp2,L_temp1);
    if(L_temp>0L) {

        return(2);
    }
    else {

        return(1);
    }
}

/*----------------------------------------------------------------------------
 *   calc_st_filt -  computes impulse response of A(gamma2) / A(gamma1)
 *   controls gain : computation of energy impulse response as
 *                    SUMn  (abs (h[n])) and computes parcor0
 *----------------------------------------------------------------------------
 */
static void calc_st_filt(
 Word16 *apond2,     /* input : coefficients of numerator */
 Word16 *apond1,     /* input : coefficients of denominator */
 Word16 *parcor0,    /* output: 1st parcor calcul. on composed filter */
 Word16 *sig_ltp_ptr    /* in/out: input of 1/A(gamma1) : scaled by 1/g0 */
)
{
    Word16 h[LONG_H_ST];
    Word32 L_temp, L_g0;
    Word16 g0, temp;
    int i;

    /* compute i.r. of composed filter apond2 / apond1 */
    Syn_filt(apond1, apond2, h, LONG_H_ST, mem_zero, 0);

    /* compute 1st parcor */
    calc_rc0_h(h, parcor0);

    /* compute g0 */
    L_g0 = 0L;
    for(i=0; i<LONG_H_ST; i++) {
        L_temp = L_deposit_l(abs_s(h[i]));
        L_g0   = L_add(L_g0, L_temp);
    }
    g0 = extract_h(L_shl(L_g0, 14));

    /* Scale signal input of 1/A(gamma1) */
    if(sub(g0, 1024)>0) {
        temp = div_s(1024, g0);     /* temp = 2**15 / gain0 */
        for(i=0; i<L_SUBFR; i++) {
            sig_ltp_ptr[i] = mult_r(sig_ltp_ptr[i], temp);
        }
    }

    return;
}

/*----------------------------------------------------------------------------
 * calc_rc0_h - computes 1st parcor from composed filter impulse response
 *----------------------------------------------------------------------------
 */
static void calc_rc0_h(
 Word16 *h,      /* input : impulse response of composed filter */
 Word16 *rc0     /* output: 1st parcor */
)
{
    Word16 acf0, acf1;
    Word32 L_acc;
    Word16 temp, sh_acf;
    Word16 *ptrs;
    int i;


    /* computation of the autocorrelation function acf */
    L_acc  = 0L;
    for(i=0; i<LONG_H_ST; i++) L_acc = L_mac(L_acc, h[i], h[i]);
    sh_acf = norm_l(L_acc);
    L_acc  = L_shl(L_acc, sh_acf);
    acf0   = extract_h(L_acc);

    L_acc  = 0L;
    ptrs   = h;
    for(i=0; i<LONG_H_ST-1; i++){
        temp = *ptrs++;
        L_acc = L_mac(L_acc, temp, *ptrs);
    }
    L_acc = L_shl(L_acc, sh_acf);
    acf1  = extract_h(L_acc);

    /* Compute 1st parcor */
    /**********************/
    if( sub(acf0, abs_s(acf1))<0) {
        *rc0 = 0;
        return;
    }
    *rc0 = div_s(abs_s(acf1), acf0);
    if(acf1 > 0) {
        *rc0 = negate(*rc0);
    }

    return;
}


/*----------------------------------------------------------------------------
 * filt_mu - tilt filtering with : (1 + mu z-1) * (1/1-|mu|)
 *   computes y[n] = (1/1-|mu|) (x[n]+mu*x[n-1])
 *----------------------------------------------------------------------------
 */
static void filt_mu(
 Word16 *sig_in,     /* input : input signal (beginning at sample -1) */
 Word16 *sig_out,    /* output: output signal */
 Word16 parcor0      /* input : parcor0 (mu = parcor0 * gamma3) */
)
{
    int n;
    Word16 mu, mu2, ga, temp;
    Word32 L_acc, L_temp, L_fact;
    Word16 fact, sh_fact1;
    Word16 *ptrs;

    if(parcor0 > 0) {
        mu      = mult_r(parcor0, GAMMA3_PLUS);
        /* GAMMA3_PLUS < 0.5 */
        sh_fact1 = 15;                   /* sh_fact + 1 */
        fact     = (Word16)0x4000;       /* 2**sh_fact */
        L_fact   = (Word32)0x00004000L;
    }
    else {
        mu       = mult_r(parcor0, GAMMA3_MINUS);
        /* GAMMA3_MINUS < 0.9375 */
        sh_fact1 = 12;                   /* sh_fact + 1 */
        fact     = (Word16)0x0800;       /* 2**sh_fact */
        L_fact   = (Word32)0x00000800L;
    }

    temp = sub(1, abs_s(mu));
    mu2  = add(32767, temp);    /* 2**15 (1 - |mu|) */
    ga   = div_s(fact, mu2);    /* 2**sh_fact / (1 - |mu|) */

    ptrs = sig_in;     /* points on sig_in(-1) */
    mu   = shr(mu, 1);          /* to avoid overflows   */

    for(n=0; n<L_SUBFR; n++) {
        temp   = *ptrs++;
        L_temp = L_deposit_l(*ptrs);
        L_acc  = L_shl(L_temp, 15);         /* sig_in(n) * 2**15 */
        L_temp = L_mac(L_acc, mu, temp);
        L_temp = L_add(L_temp, 0x00004000L);
        temp   = extract_l(L_shr(L_temp,15));
        /* ga x temp x 2 with rounding */
        L_temp = L_add(L_mult(temp, ga),L_fact);
        L_temp = L_shr(L_temp, sh_fact1); /* mult. temp x ga */
        sig_out[n] = sature(L_temp);
    }
    return;
}

/*----------------------------------------------------------------------------
 *   scale_st  - control of the subframe gain
 *   gain[n] = AGC_FAC * gain[n-1] + (1 - AGC_FAC) g_in/g_out
 *----------------------------------------------------------------------------
 */
static void scale_st(
 Word16 *sig_in,     /* input : postfilter input signal */
 Word16 *sig_out,    /* in/out: postfilter output signal */
 Word16 *gain_prec   /* in/out: last value of gain for subframe */
)
{

    int i;
    Word16 scal_in, scal_out;
    Word32 L_acc, L_temp;
    Word16 s_g_in, s_g_out, temp, sh_g0, g0;
    Word16 gain;

    /* compute input gain */
    L_acc = 0L;
    for(i=0; i<L_SUBFR; i++) {
        L_temp  = L_abs(L_deposit_l(sig_in[i]));
        L_acc   = L_add(L_acc, L_temp);
    }

    if(L_acc == 0L) {
        g0 = 0;
    }
    else {
        scal_in = norm_l(L_acc);
        L_acc   = L_shl(L_acc, scal_in);
        s_g_in  = extract_h(L_acc);    /* normalized */

        /* Compute output gain */
        L_acc = 0L;
        for(i=0; i<L_SUBFR; i++) {
            L_temp  = L_abs(L_deposit_l(sig_out[i]));
            L_acc   = L_add(L_acc, L_temp);
        }
        if(L_acc == 0L) {
            *gain_prec = 0;
            return;
        }
        scal_out = norm_l(L_acc);
        L_acc    = L_shl(L_acc, scal_out);
        s_g_out  = extract_h(L_acc);  /* normalized */

        sh_g0    = add(scal_in, 1);
        sh_g0    = sub(sh_g0, scal_out);    /* scal_in - scal_out + 1 */
        if(sub(s_g_in ,s_g_out)<0) {
            g0 = div_s(s_g_in, s_g_out);    /* s_g_in/s_g_out in Q15 */
        }
        else {
            temp  = sub(s_g_in, s_g_out);   /* sufficient since normalized */
            g0    = shr(div_s(temp, s_g_out), 1);
            g0    = add(g0, (Word16)0x4000);/* s_g_in/s_g_out in Q14 */
            sh_g0 = sub(sh_g0, 1);
        }
        /* L_gain_in/L_gain_out in Q14              */
        /* overflows if L_gain_in > 2 * L_gain_out  */
        g0 = shr(g0, sh_g0);        /* sh_g0 may be >0, <0, or =0 */
        g0 = mult_r(g0, AGC_FAC1);  /* L_gain_in/L_gain_out * AGC_FAC1      */

    }

    /* gain(n) = AGC_FAC gain(n-1) + AGC_FAC1 gain_in/gain_out          */
    /* sig_out(n) = gain(n) sig_out(n)                                  */
    gain = *gain_prec;
    for(i=0; i<L_SUBFR; i++) {
        temp = mult_r(AGC_FAC, gain);
        gain = add(temp, g0);            /* in Q14 */
        L_temp = L_mult(gain, sig_out[i]);
        L_temp = L_shl(L_temp, 1);
        sig_out[i] = round(L_temp);
    }
    *gain_prec = gain;
    return;
}