Exemple #1
0
/*-----------------------------------------------------------*
 * procedure Dec_cng:                                        *
 *           ~~~~~~~~                                        *
 *                     Receives frame type                   *
 *                     0  :  for untransmitted frames        *
 *                     2  :  for SID frames                  *
 *                     Decodes SID frames                    *
 *                     Computes current frame excitation     *
 *                     Computes current frame LSPs
 *-----------------------------------------------------------*/
void Dec_cng(
  Word16 past_ftyp,     /* (i)   : past frame type                      */
  Word16 sid_sav,       /* (i)   : energy to recover SID gain           */
  Word16 sh_sid_sav,    /* (i)   : corresponding scaling factor         */
  Word16 *parm,         /* (i)   : coded SID parameters                 */
  Word16 *exc,          /* (i/o) : excitation array                     */
  Word16 *lsp_old,      /* (i/o) : previous lsp                         */
  Word16 *A_t,          /* (o)   : set of interpolated LPC coefficients */
  Word16 *seed,         /* (i/o) : random generator seed                */
  Word16 freq_prev[MA_NP][M]
                        /* (i/o) : previous LPS for quantization        */
)
{
  Word16 temp, ind;
  Word16 dif;

  dif = sub(past_ftyp, 1);
  
  /* SID Frame */
  /*************/
  if(parm[0] != 0) {

    sid_gain = tab_Sidgain[(int)parm[4]];

    /* Inverse quantization of the LSP */
    sid_lsfq_decode(&parm[1], lspSid, freq_prev);

  }

  /* non SID Frame */
  /*****************/
  else {

    /* Case of 1st SID frame erased : quantize-decode   */
    /* energy estimate stored in sid_gain         */
    if(dif > 0) {
      Qua_Sidgain(&sid_sav, &sh_sid_sav, 0, &temp, &ind);
      sid_gain = tab_Sidgain[(int)ind];
    }

  }

  if(dif > 0) {
    cur_gain = sid_gain;
  }
  else {
    cur_gain = mult_r(cur_gain, A_GAIN0);
    cur_gain = add(cur_gain, mult_r(sid_gain, A_GAIN1));
  }

  Calc_exc_rand(cur_gain, exc, seed, FLAG_DEC);

  /* Interpolate the Lsp vectors */
  Int_qlpc(lsp_old, lspSid, A_t);
  Copy(lspSid, lsp_old, M);

  return;
}
Exemple #2
0
/*----------------------------------------------------------------------------
 * 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;
}
Exemple #3
0
long interpolation_cos129( short freq )
{
    short  sin_data,cos_data,count,temp ;
    long  Ltemp,Lresult;


    /* cos(x)=cos(a)+(x-a)sin(a)-pow((a-x),2)*cos(a) */

    count=shr(abs_s(freq ),7 );

    temp=sub( extract_l(L_mult( count,64)) , freq );
    /* (a-x)sin a  */
    /* Scale factor for (a-x): 3217=pi2/64 */

    sin_data=sin129_table [ count];
    cos_data=cos129_table [count];

    Ltemp=L_mpy_ls(L_mult(3217,temp),sin_data);

    /* (a-x) sin(a) - [(a-x)*(a-x)*cos(a)] /2 */
    /* Scale factor for (a-x)*(a-x):  20213=pi2*pi2/64 */

    Ltemp=L_sub(Ltemp,
                L_mpy_ls(L_mult(mult_r(10106,temp),temp),cos_data));

    /* Scaled up by 64/2 times */
    Ltemp=L_shl( Ltemp ,6 );
    Lresult=  L_add(L_deposit_h(cos_data), (Ltemp)) ;

    return(Lresult);
}
Exemple #4
0
/*-----------------------------------------------------*
 * Function Autocorr()                                 *
 *                                                     *
 *   Compute autocorrelations of signal with windowing *
 *                                                     *
 *-----------------------------------------------------*/
void Autocorr(
  Word16 x[],      /* (i)    : Input signal                      */
  Word16 m,        /* (i)    : LPC order                         */
  Word16 r_h[],    /* (o)    : Autocorrelations  (msb)           */
  Word16 r_l[]     /* (o)    : Autocorrelations  (lsb)           */
)
{
  Word16 i, j, norm;
  Word16 y[L_WINDOW];
  Word32 sum;

  extern Flag Overflow;

  /* Windowing of signal */

  for(i=0; i<L_WINDOW; i++)
  {
    y[i] = mult_r(x[i], hamwindow[i]);
  }

  /* Compute r[0] and test for overflow */

  do {
    Overflow = 0;
    sum = 1;                   /* Avoid case of all zeros */
    for(i=0; i<L_WINDOW; i++)
      sum = L_mac(sum, y[i], y[i]);

    /* If overflow divide y[] by 4 */

    if(Overflow != 0)
    {
      for(i=0; i<L_WINDOW; i++)
      {
        y[i] = shr(y[i], 2);
      }
    }
  }while (Overflow != 0);

  /* Normalization of r[0] */

  norm = norm_l(sum);
  sum  = L_shl(sum, norm);
  L_Extract(sum, &r_h[0], &r_l[0]);     /* Put in DPF format (see oper_32b) */

  /* r[1] to r[m] */

  for (i = 1; i <= m; i++)
  {
    sum = 0;
    for(j=0; j<L_WINDOW-i; j++)
      sum = L_mac(sum, y[j], y[j+i]);

    sum = L_shl(sum, norm);
    L_Extract(sum, &r_h[i], &r_l[i]);
  }
  return;
}
Exemple #5
0
Word32 fnLog2(Word32 L_Input)
{

	static Word16
	    swC0 = -0x2b2a, swC1 = 0x7fc5, swC2 = -0x54d0;

	Word16 siShiftCnt, swInSqrd, swIn;
	Word32 LwIn;

/*_________________________________________________________________________
 |                                                                         |
 |                              Executable Code                            |
 |_________________________________________________________________________|
*/

	/* normalize input and store shifts required */
	/* ----------------------------------------- */

	siShiftCnt = norm_l(L_Input);
	LwIn = L_shl(L_Input, siShiftCnt);
	siShiftCnt = add(siShiftCnt, 1);
	siShiftCnt = negate(siShiftCnt);

	/* calculate x*x*c0 */
	/* ---------------- */

	swIn = extract_h(LwIn);
	swInSqrd = mult_r(swIn, swIn);
	LwIn = L_mult(swInSqrd, swC0);

	/* add x*c1 */
	/* --------- */

	LwIn = L_mac(LwIn, swIn, swC1);

	/* add c2 */
	/* ------ */

	LwIn = L_add(LwIn, L_deposit_h(swC2));

	/* apply *(4/32) */
	/* ------------- */

	LwIn = L_shr(LwIn, 3);
	LwIn = LwIn & 0x03ffffff;
	siShiftCnt = shl(siShiftCnt, 10);
	LwIn = L_add(LwIn, L_deposit_h(siShiftCnt));

	/* return log2 */
	/* ----------- */

	return (LwIn);
}
Exemple #6
0
static Word16 Cmp_filt(Word16 *RCoeff, Word16 sh_RCoeff, Word16 *acf,
                                        Word16 alpha, Word16 FracThresh)
{
  Word32 L_temp0, L_temp1;
  Word16 temp1, temp2, sh[2], ind;
  Word16 i;
  Word16 diff, flag;
  extern Flag Overflow;

  sh[0] = 0;
  sh[1] = 0;
  ind = 1;
  flag = 0;
  do {
    Overflow = 0;
    temp1 = shr(RCoeff[0], sh[0]);
    temp2 = shr(acf[0], sh[1]);
    L_temp0 = L_shr(L_mult(temp1, temp2),1);
    for(i=1; i <= M; i++) {
      temp1 = shr(RCoeff[i], sh[0]);
      temp2 = shr(acf[i], sh[1]);
      L_temp0 = L_mac(L_temp0, temp1, temp2);
    }
    if(Overflow != 0) {
      sh[(int)ind] = add(sh[(int)ind], 1);
      ind = sub(1, ind);
    }
    else flag = 1;
  } while (flag == 0);
  
  
  temp1 = mult_r(alpha, FracThresh);
  L_temp1 = L_add(L_deposit_l(temp1), L_deposit_l(alpha));
  temp1 = add(sh_RCoeff, 9);  /* 9 = Lpc_justif. * 2 - 16 + 1 */
  temp2 = add(sh[0], sh[1]);
  temp1 = sub(temp1, temp2);
  L_temp1 = L_shl(L_temp1, temp1);
  
  L_temp0 = L_sub(L_temp0, L_temp1);
  if(L_temp0 > 0L) diff = 1;
  else diff = 0;

  return(diff);
}
Exemple #7
0
static Word16 Quant_Energy(
  Word32 L_x,    /* (i)  : Energy                 */
  Word16 sh,     /* (i)  : Exponent of the energy */
  Word16 *enerq  /* (o)  : quantized energy in dB */
)
{

  Word16 exp, frac;
  Word16 e_tmp, temp, index;

  Log2(L_x, &exp, &frac);
  temp = sub(exp, sh);
  e_tmp = shl(temp, 10);
  e_tmp = add(e_tmp, mult_r(frac, 1024)); /* 2^10 x log2(L_x . 2^-sh) */
  /* log2(ener) = 10log10(ener) / K */
  /* K = 10 Log2 / Log10 */

  temp = sub(e_tmp, -2721);      /* -2721 -> -8dB */
  if(temp <= 0) {
    *enerq = -12;
    return(0);
  }

  temp = sub(e_tmp, 22111);      /* 22111 -> 65 dB */  
  if(temp > 0) {
    *enerq = 66;
    return(31);
  }

  temp = sub(e_tmp, 4762);       /* 4762 -> 14 dB */
  if(temp <= 0){
    e_tmp = add(e_tmp, 3401);
    index = mult(e_tmp, 24);
    if (index < 1) index = 1;
    *enerq = sub(shl(index, 2), 8);
    return(index);
  }

  e_tmp = sub(e_tmp, 340);
  index = sub(shr(mult(e_tmp, 193), 2), 1);
  if (index < 6) index = 6;
  *enerq = add(shl(index, 1), 4);
  return(index);
}
Exemple #8
0
/*----------------------------------------------------------------------------
 *   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_filte(
 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  long_h_st,     /* input : impulse response length                   */
 Word16 m_pst               /* input : LPC order    */
)
{
    Word16 h[LONG_H_ST_E];
    Word32 L_temp, L_g0;
    Word16 g0, temp;
    Word16 i;

    /* compute i.r. of composed filter apond2 / apond1 */
    Syn_filte(m_pst, apond1, apond2, h, long_h_st, mem_zero, 0);

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

    /* 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;
}
Exemple #9
0
/*
**
** Function:           Calc_Exc_Rand()
**
** Description:        Computation of random excitation for inactive frames:
**                     Adaptive codebook entry selected randomly
**                     Higher rate innovation pattern selected randomly
**                     Computes innovation gain to match curGain
**
** Links to text:
**
** Arguments:
**
**  Word16 curGain     current average gain to match
**  Word16 *PrevExc    previous/current excitation (updated)
**  Word16 *DataEXc    current frame excitation
**  Word16 *nRandom    random generator status (input/output)
**
** Outputs:
**
**  Word16 *PrevExc
**  Word16 *DataExc
**  Word16 *nRandom
**
** Return value:       None
**
*/
void Calc_Exc_Rand(Word16 curGain, Word16 *PrevExc, Word16 *DataExc,
                                      Word16 *nRandom, LINEDEF *Line)
{
    int i, i_subfr, iblk;
    Word16 temp, temp2;
    Word16 j;
    Word16 TabPos[2*NbPulsBlk], TabSign[2*NbPulsBlk];
    Word16 *ptr_TabPos, *ptr_TabSign;
    Word16 *ptr1, *curExc;
    Word16 sh1, x1, x2, inter_exc, delta, b0;
    Word32 L_acc, L_c, L_temp;
    Word16 tmp[SubFrLen/Sgrid];
    Word16 offset[SubFrames];
    Word16 tempExc[SubFrLenD];

 /*
  * generate LTP codes
  */
    Line->Olp[0] = random_number(21, nRandom) + (Word16)123;
    Line->Olp[1] = random_number(21, nRandom) + (Word16)123;
    for(i_subfr=0; i_subfr<SubFrames; i_subfr++) {  /* in [1, NbFilt] */
        Line->Sfs[i_subfr].AcGn = random_number(NbFilt, nRandom) + (Word16)1;
    }
    Line->Sfs[0].AcLg = 1;
    Line->Sfs[1].AcLg = 0;
    Line->Sfs[2].AcLg = 1;
    Line->Sfs[3].AcLg = 3;


 /*
  * Random innovation :
  * Selection of the grids, signs and pulse positions
  */

    /* Signs and Grids */
    ptr_TabSign = TabSign;
    ptr1 = offset;
    for(iblk=0; iblk<SubFrames/2; iblk++) {
        temp    = random_number((1 << (NbPulsBlk+2)), nRandom);
        *ptr1++ = temp & (Word16)0x0001;
        temp    = shr(temp, 1);
        *ptr1++ = add( (Word16) SubFrLen, (Word16) (temp & 0x0001) );
        for(i=0; i<NbPulsBlk; i++) {
            *ptr_TabSign++= shl(sub((temp & (Word16)0x0002), 1), 14);
            temp = shr(temp, 1);
        }
    }

    /* Positions */
    ptr_TabPos  = TabPos;
    for(i_subfr=0; i_subfr<SubFrames; i_subfr++) {

        for(i=0; i<(SubFrLen/Sgrid); i++) tmp[i] = (Word16)i;
        temp = (SubFrLen/Sgrid);
        for(i=0; i<Nb_puls[i_subfr]; i++) {
            j = random_number(temp, nRandom);
            *ptr_TabPos++ = add(shl(tmp[(int)j],1), offset[i_subfr]);
            temp = sub(temp, 1);
            tmp[(int)j] = tmp[(int)temp];
        }
    }

 /*
  * Compute fixed codebook gains
  */

    ptr_TabPos = TabPos;
    ptr_TabSign = TabSign;
    curExc = DataExc;
    i_subfr = 0;
    for(iblk=0; iblk<SubFrames/2; iblk++) {

        /* decode LTP only */
        Decod_Acbk(curExc, &PrevExc[0], Line->Olp[iblk],
                    Line->Sfs[i_subfr].AcLg, Line->Sfs[i_subfr].AcGn);
        Decod_Acbk(&curExc[SubFrLen], &PrevExc[SubFrLen], Line->Olp[iblk],
            Line->Sfs[i_subfr+1].AcLg, Line->Sfs[i_subfr+1].AcGn);

        temp2 = 0;
        for(i=0; i<SubFrLenD; i++) {
            temp = abs_s(curExc[i]);
            if(temp > temp2) temp2 = temp;
        }
        if(temp2 == 0) sh1 = 0;
        else {
            sh1 = sub(4,norm_s(temp2)); /* 4 bits of margin  */
            if(sh1 < -2) sh1 = -2;
        }

        L_temp = 0L;
        for(i=0; i<SubFrLenD; i++) {
            temp  = shr(curExc[i], sh1); /* left if sh1 < 0 */
            L_temp = L_mac(L_temp, temp, temp);
            tempExc[i] = temp;
        }  /* ener_ltp x 2**(-2sh1+1) */

        L_acc = 0L;
        for(i=0; i<NbPulsBlk; i++) {
            L_acc = L_mac(L_acc, tempExc[(int)ptr_TabPos[i]], ptr_TabSign[i]);
        }
        inter_exc = extract_h(L_shl(L_acc, 1)); /* inter_exc x 2-sh1 */

        /* compute SubFrLenD x curGain**2 x 2**(-2sh1+1)    */
        /* curGain input = 2**5 curGain                     */
//        L_acc = L_mult(curGain, SubFrLen);
		L_MULT(curGain, SubFrLen, L_acc);
        L_acc = L_shr(L_acc, 6);
        temp  = extract_l(L_acc);   /* SubFrLen x curGain : avoids overflows */
 //       L_acc = L_mult(temp, curGain);
		L_MULT(temp, curGain, L_acc);
        temp = shl(sh1, 1);
        temp = add(temp, 4);
        L_acc = L_shr(L_acc, temp); /* SubFrLenD x curGain**2 x 2**(-2sh1+1) */

        /* c = (ener_ltp - SubFrLenD x curGain**2)/nb_pulses_blk */
        /* compute L_c = c >> 2sh1-1                                */
        L_acc = L_sub(L_temp, L_acc);
        /* x 1/nb_pulses_blk */
        L_c  = L_mls(L_acc, InvNbPulsBlk);

/*
 * Solve EQ(X) = X**2 + 2 b0 X + c
 */
        /* delta = b0 x b0 - c */
        b0 = mult_r(inter_exc, InvNbPulsBlk);   /* b0 >> sh1 */
        L_acc = L_msu(L_c, b0, b0);             /* (c - b0**2) >> 2sh1-1 */
        L_acc = L_negate(L_acc);                /* delta x 2**(-2sh1+1) */

        /* Case delta <= 0 */
        if(L_acc <= 0) {  /* delta <= 0 */
            x1 = negate(b0);        /* sh1 */
        }

        /* Case delta > 0 */
        else {
            delta = Sqrt_lbc(L_acc);  /* >> sh1 */
            x1 = sub(delta, b0);      /* x1 >> sh1 */
            x2 = add(b0, delta);      /* (-x2) >> sh1 */
            if(abs_s(x2) < abs_s(x1)) {
                x1 = negate(x2);
            }
        }

        /* Update DataExc */
        sh1 = add(sh1, 1);
        temp = shl(x1, sh1);
        if(temp > (2*Gexc_Max)) temp = (2*Gexc_Max);
        if(temp < -(2*Gexc_Max)) temp = -(2*Gexc_Max);
        for(i=0; i<NbPulsBlk; i++) {
            j = *ptr_TabPos++;
            curExc[(int)j] = add(curExc[(int)j], mult(temp,
                                                (*ptr_TabSign++)) );
        }

        /* update PrevExc */
        ptr1 = PrevExc;
        for(i=SubFrLenD; i<PitchMax; i++)   *ptr1++ = PrevExc[i];
        for(i=0; i<SubFrLenD; i++)  *ptr1++ = curExc[i];

        curExc += SubFrLenD;
        i_subfr += 2;

    } /* end of loop on LTP blocks */

    return;
}
Exemple #10
0
/*
**
** Function:           Qua_SidGain()
**
** Description:        Quantization of Sid gain
**                     Pseudo-log quantizer in 3 segments
**                     1st segment : length = 16, resolution = 2
**                     2nd segment : length = 16, resolution = 4
**                     3rd segment : length = 32, resolution = 8
**                     quantizes a sum of energies
**
** Links to text:
**
** Arguments:
**
**  Word16 *Ener        table of the energies
**  Word16 *shEner      corresponding scaling factors
**  Word16 nq           if nq >= 1 : quantization of nq energies
**                      for SID gain calculation in function Cod_Cng()
**                      if nq = 0 : in function Comp_Info(),
**                      quantization of saved estimated excitation energy
**
** Outputs:             None
**
**
** Return value:       index of quantized energy
**
*/
Word16 Qua_SidGain(Word16 *Ener, Word16 *shEner, Word16 nq)
{
    Word16 temp, iseg, iseg_p1;
    Word16 j, j2, k, exp;
    Word32 L_x, L_y;
    Word16 sh1;
    Word32 L_acc;
    int i;

    if(nq == 0) {
         /* Quantize energy saved for frame erasure case                */
         /* L_x = 2 x average_ener                                      */
         temp = shl(*shEner, 1);
         temp = sub(16, temp);
         L_acc = L_deposit_l(*Ener);
         L_acc = L_shl(L_acc, temp); /* may overflow, and >> if temp < 0 */
         L_x = L_mls(L_acc, fact[0]);
    }

    else {

 /*
  * Compute weighted average of energies
  * Ener[i] = enerR[i] x 2**(shEner[i]-14)
  * L_x = k[nq] x SUM(i=0->nq-1) enerR[i]
  * with k[nq] =  2 x fact_mul x fact_mul / nq x Frame
  */
         sh1 = shEner[0];
         for(i=1; i<nq; i++) {
             if(shEner[i] < sh1) sh1 = shEner[i];
         }
         for(i=0, L_x=0L; i<nq; i++) {
             temp = sub(shEner[i], sh1);
             temp = shr(Ener[i], temp);
             temp = mult_r(fact[nq], temp);
             L_x = L_add(L_x, L_deposit_l(temp));
         }
         temp = sub(15, sh1);
         L_x = L_shl(L_x, temp);
    }

    /* Quantize L_x */
    if(L_x >= L_bseg[2]) return(63);

    /* Compute segment number iseg */
    if(L_x >= L_bseg[1]) {
        iseg = 2;
        exp = 4;
    }
    else {
        exp  = 3;
        if(L_x >= L_bseg[0]) iseg = 1;
        else iseg = 0;
    }

    iseg_p1 = add(iseg,1);
    j = shl(1, exp);
    k = shr(j,1);

    /* Binary search in segment iseg */
    for(i=0; i<exp; i++) {
        temp = add(base[iseg], shl(j, iseg_p1));
//        L_y = L_mult(temp, temp);
		L_MULT(temp, temp, L_y);
        if(L_x >= L_y) j = add(j, k);
        else j = sub(j, k);
        k = shr(k, 1);
    }

    temp = add(base[iseg], shl(j, iseg_p1));
    L_y = L_mult(temp, temp);
    L_y = L_sub(L_y, L_x);
    if(L_y <= 0L) {
        j2    = add(j, 1);
        temp  = add(base[iseg], shl(j2, iseg_p1));
        L_acc = L_mult(temp, temp);
        L_acc = L_sub(L_x, L_acc);
        if(L_y > L_acc) temp = add(shl(iseg,4), j);
        else temp = add(shl(iseg,4), j2);
    }
    else {
        j2    = sub(j, 1);
        temp  = add(base[iseg], shl(j2, iseg_p1));
        L_acc = L_mult(temp, temp);
        L_acc = L_sub(L_x, L_acc);
        if(L_y < L_acc) temp = add(shl(iseg,4), j);
        else temp = add(shl(iseg,4), j2);
    }
    return(temp);
}
Exemple #11
0
void fndppf(short *delay, short *beta, short *buf, short dmin, short dmax, short length)
{
	static short b = -10224;	/* rom storage */

	static short a[3] =
	{-18739, 16024, -4882};		/* a[] scaled down by 4 */

	short dnew = 0;
	short sum;
	long Lsum;
	register short m, i, n;
	static short DECbuf[FrameSize / 4];
	long Lcorrmax, Lcmax, Ltmp;
	short tap1;
	short M1, M2, dnewtmp = 0;
	static short lastgoodpitch = 0;
	static short lastbeta = 0;
	static short memory[3];
	static int FirstTime = 1;
	short Lsum_scale;
	short shift, Lcorr_scale, Lcmax_scale;
	short n1, n2, nq, nq1;
	long Ltempf;
	/* init static variables (should be in init routine for implementation) */
	if (FirstTime)
	{
		FirstTime = 0;
		n1 = (shr(FrameSize, 2));
		for (i = 0; i < n1; i++)
			DECbuf[i] = 0;
		memory[0] = memory[1] = memory[2] = 0;
	}

	/* Shift  memory of DECbuf */
	for (i = 0; i < shr(length, 3); i++)
	{
		DECbuf[i] = DECbuf[i + shr(length, 3)];
	}
	/* filter signal and decimate */
	for (i = 0, n = shr(length, 3); i < shr(length, 1); i++)
	{
		Ltempf = L_shr(L_deposit_h(buf[i + shr(length, 1)]), 4);
		Ltempf = L_msu(Ltempf, memory[0], a[0]);
		Ltempf = L_msu(Ltempf, memory[1], a[1]);
		Ltempf = L_msu(Ltempf, memory[2], a[2]);
		Ltempf = L_shl(Ltempf, 2);

		shift = 0;
		if ((i + 1) % 4 == 0)
		{
			Lsum = L_add(Ltempf, L_deposit_h(memory[2]));
			Lsum = L_mac(Lsum, memory[0], b);
			Lsum = L_mac(Lsum, memory[1], b);

			DECbuf[n++] = round(L_shl(Lsum, 1));
		}
		memory[2] = memory[1];
		memory[1] = memory[0];
		memory[0] = round(Ltempf);
	}

	/* perform first search for best delay value in decimated domain */
	Lcorrmax = (LW_MIN);
        Lcorr_scale = 1;

	for (m = shr(dmin, 2); m <= shr(dmax, 2); m++)
	{
                n1 = 1;
		for (i = 0, Lsum = 0; i < sub(shr(length, 2), m); i++)
		{
			Ltempf = L_mult(DECbuf[i], DECbuf[i + m]);
			Ltempf = L_shr(Ltempf, n1);
			Lsum = L_add(Lsum, Ltempf);
			if (L_abs(Lsum) >= 0x40000000)
			{
				Lsum = L_shr(Lsum, 1);
				n1++;
			}
		}
		if (
		    ((Lcorr_scale >= n1) && (L_shr(Lsum, sub(Lcorr_scale, n1)) > Lcorrmax))
		 || ((Lcorr_scale < n1) && (Lsum > L_shr(Lcorrmax, sub(n1, Lcorr_scale))))
		 ) 
		{
			Lcorrmax = Lsum;
			Lcorr_scale = n1;
			dnew = m;
		}

	}

	/* Compare against lastgoodpitch */
	if (lastgoodpitch != 0 && (abs_s(sub(lastgoodpitch, shl(dnew, 2))) > 2))
	{
		M1 = sub(shr(lastgoodpitch, 2), 2);
		if (M1 < shr(dmin, 2))
			M1 = shr(dmin, 2);
		M2 = add(M1, 4);
		if (M2 > shr(dmax, 2))
			M2 = shr(dmax, 2);

		Lcmax = LW_MIN;
                Lcmax_scale = 1;
		for (m = M1; m <= M2; m++)
		{
                        n1 = 1;
			for (i = 0, Lsum = 0; i < sub(shr(length, 2), m); i++)
			{
				Ltempf = L_mult(DECbuf[i], DECbuf[i + m]);
				Ltempf = L_shr(Ltempf, n1);
				Lsum = L_add(Lsum, Ltempf);
				if (L_abs(Lsum) >= 0x40000000)
				{
					Lsum = L_shr(Lsum, 1);
					n1++;
				}
			}

			if (
			    ((Lcmax_scale >= n1) && (L_shr(Lsum, sub(Lcmax_scale, n1)) > Lcmax))
			 || ((Lcmax_scale < n1) && (Lsum > L_shr(Lcmax, sub(n1, Lcmax_scale))))
			   ) 
			{					/* Gives some bias to low delays */
				Lcmax = Lsum;
				Lcmax_scale = n1;
				dnewtmp = m;
			}
		}
		Lsum = L_mpy_ls(Lcorrmax, 27361);
		if (
		    ((Lcmax_scale >= Lcorr_scale) && (L_shr(Lsum, sub(Lcmax_scale, Lcorr_scale)) < Lcmax))
		 || ((Lcmax_scale < Lcorr_scale) && (Lsum < L_shr(Lcmax, sub(Lcorr_scale, Lcmax_scale))))
		   ) 
		{
			dnew = dnewtmp;
		}
	}

	/* perform first search for best delay value in non-decimated buffer */
	M1 = Max(sub(shl(dnew, 2), 3), dmin);
	if (M1 < dmin)
		M1 = dmin;
	M2 = Min(add(shl(dnew, 2), 3), dmax);
	if (M2 > dmax)
		M2 = dmax;
	Lcorrmax = LW_MIN;
        Lcorr_scale = 1;
	for (m = M1; m <= M2; m++)
	{
                n1 = 1;
		for (i = 0, Lsum = 0; i < sub(length, m); i++)
		{
			Ltempf = L_mult(buf[i], buf[i + m]);

			Ltempf = L_shr(Ltempf, n1);
			Lsum = L_add(Lsum, Ltempf);
			if (L_abs(Lsum) >= 0x40000000)
			{
				Lsum = L_shr(Lsum, 1);
				n1++;
			}
		}

		if (
		    ((Lcorr_scale >= n1) && (L_shr(Lsum, sub(Lcorr_scale, n1)) > Lcorrmax))
		 || ((Lcorr_scale < n1) && (Lsum > L_shr(Lcorrmax, sub(n1, Lcorr_scale))))
		   ) 
		{
			Lcorrmax = Lsum;
			Lcorr_scale = n1;
			dnew = m;
		}
	}
        Lsum_scale = 1;
	for (i = 0, Lsum = 0; i < sub(length, dnew); i++)
	{
		Ltempf = L_mult(buf[i + dnew], buf[i + dnew]);
		Ltempf = L_shr(Ltempf, Lsum_scale);
		Lsum = L_add(Lsum, Ltempf);
		if (L_abs(Lsum) >= 0x40000000)
		{
			Lsum = L_shr(Lsum, 1);
			Lsum_scale++;
		}
	}

        Lcmax_scale = 1;
	for (i = 0, Lcmax = 0; i < length - dnew; i++)
	{
		Ltempf = L_mult(buf[i], buf[i]);
		Ltempf = L_shr(Ltempf, Lcmax_scale);
		Lcmax = L_add(Lcmax, Ltempf);
		if (L_abs(Lcmax) >= 0x40000000)
		{
			Lcmax = L_shr(Lcmax, 1);
			Lcmax_scale++;
		}
	}
	nq = norm_l(Lsum);
	Lsum = L_shl(Lsum, nq);
	nq1 = norm_l(Lcmax);
	Lcmax = L_shl(Lcmax, nq1);
	Lsum = L_mpy_ll(Lsum, Lcmax);
	n1 = norm_l(Lsum);
	Lsum = L_shl(Lsum, n1);
	sum = sqroot(Lsum);
	n1 = add(add(n1, nq), nq1);
	n1 = sub(sub(n1, Lcmax_scale), Lsum_scale);
	n2 = shr(n1, 1);

	if (n1 & 1)
		Lsum = L_mult(sum, 23170);
	else
		Lsum = L_deposit_h(sum);

	n2 = add(n2, Lcorr_scale);
	Lcorrmax = L_shl(Lcorrmax, n2);

	if ((Lsum == 0) || (Lcorrmax <= 0))
		*beta = 0;
	else if (Lcorrmax > Lsum)
		*beta = 0x7fff;
	else
		*beta = round(L_divide(Lcorrmax, Lsum));

	/* perform search for best delay value in around old pitch delay */
	if (lastgoodpitch != 0)
	{
		M1 = lastgoodpitch - 6;
		M2 = lastgoodpitch + 6;

		if (M1 < dmin)
			M1 = dmin;
		if (M2 > dmax)
			M2 = dmax;

		if (dnew > M2 || dnew < M1)
		{
			Lcmax = LW_MIN;
                        Lcmax_scale = 1;
			for (m = M1; m <= M2; m++)
			{
                                n1 = 1;
				for (i = 0, Lsum = 0; i < length - m; i++)
				{
					Ltempf = L_mult(buf[i], buf[i + m]);
					Ltempf = L_shr(Ltempf, n1);
					Lsum = L_add(Lsum, Ltempf);
					if (L_abs(Lsum) >= 0x40000000)
					{
						Lsum = L_shr(Lsum, 1);
						n1++;
					}
				}

				if (
				    ((Lcmax_scale >= n1) && (L_shr(Lsum, sub(Lcmax_scale, n1)) > Lcmax))
				 || ((Lcmax_scale < n1) && (Lsum > L_shr(Lcmax, sub(n1, Lcmax_scale))))
				   ) 
				{
					Lcmax = Lsum;
					dnewtmp = m;
					Lcmax_scale = n1;
				}
			}
                        Lcorr_scale = 1;
			for (i = 0, Ltmp = 0; i < length - dnewtmp; i++)
			{
				Ltempf = L_mult(buf[i + dnewtmp], buf[i + dnewtmp]);
				Ltempf = L_shr(Ltempf, Lcorr_scale);
				Ltmp = L_add(Ltmp, Ltempf);
				if (L_abs(Ltmp) >= 0x40000000)
				{
					Ltmp = L_shr(Ltmp, 1);
					Lcorr_scale++;
				}

			}
                        Lsum_scale = 1;
			for (i = 0, Lsum = 0; i < length - dnewtmp; i++)
			{

				Ltempf = L_mult(buf[i], buf[i]);
				Ltempf = L_shr(Ltempf, Lsum_scale);
				Lsum = L_add(Lsum, Ltempf);
				if (L_abs(Lsum) >= 0x40000000)
				{
					Lsum = L_shr(Lsum, 1);
					Lsum_scale++;
				}
			}

			nq = norm_l(Ltmp);
			Ltmp = L_shl(Ltmp, nq);
			nq1 = norm_l(Lsum);
			Lsum = L_shl(Lsum, nq1);
			Ltmp = L_mpy_ll(Ltmp, Lsum);
			n1 = norm_l(Ltmp);
			Ltmp = L_shl(Ltmp, n1);
			sum = sqroot(Ltmp);

			n1 = add(add(n1, nq), nq1);
			n1 = sub(sub(n1, Lsum_scale), Lcorr_scale);
			n2 = shr(n1, 1);

			if (n1 & 1)
				Ltmp = L_mult(sum, 23170);
			else
				Ltmp = L_deposit_h(sum);

			n2 = add(n2, Lcmax_scale);
			Lcmax = L_shl(Lcmax, n2);

			if ((Ltmp == 0) || (Lcmax <= 0))
				tap1 = 0;
			else if (Lcmax >= Ltmp)
				tap1 = 0x7fff;
			else
				tap1 = round(L_divide(Lcmax, Ltmp));

			/* Replace dnew with dnewtmp if tap1 is large enough */
			if ((dnew > M2 && (shr(tap1, 1) > mult_r(9830, *beta))) ||
				(dnew < M1 && (shr(tap1, 1) > mult_r(19661, *beta))))
			{
				dnew = dnewtmp;
				*beta = (tap1);
			}
		}
	}

	*delay = dnew;
	if (*beta > 13107)
	{
		lastgoodpitch = dnew;
		lastbeta = *beta;
	}
	else
	{
		lastbeta = mult_r(24576, lastbeta);
		if (lastbeta < 9830)
			lastgoodpitch = 0;
	}
}
Exemple #12
0
/*-----------------------------------------------------------*
 * procedure Calc_exc_rand                                   *
 *           ~~~~~~~~~~~~~                                   *
 *   Computes comfort noise excitation                       *
 *   for SID and not-transmitted frames                      *
 *-----------------------------------------------------------*/
void Calc_exc_rand(
  Word32 L_exc_err[4],
  Word16 cur_gain,      /* (i)   :   target sample gain                 */
  Word16 *exc,          /* (i/o) :   excitation array                   */
  Word16 *seed,         /* (i)   :   current Vad decision               */
  Flag flag_cod         /* (i)   :   encoder/decoder flag               */
)
{
  Word16 i, j, i_subfr;
  Word16 temp1, temp2;
  Word16 pos[4];
  Word16 sign[4];
  Word16 t0, frac;
  Word16 *cur_exc;
  Word16 g, Gp, Gp2;
  Word16 excg[L_SUBFR], excs[L_SUBFR];
  Word32 L_acc, L_ener, L_k;
  Word16 max, hi, lo, inter_exc;
  Word16 sh;
  Word16 x1, x2;
  
  if(cur_gain == 0) {

    for(i=0; i<L_FRAME; i++) {
      exc[i] = 0;
    }
    Gp = 0;
    t0 = add(L_SUBFR,1);
    for (i_subfr = 0;  i_subfr < L_FRAME; i_subfr += L_SUBFR) {
      if(flag_cod != FLAG_DEC) update_exc_err(L_exc_err, Gp, t0);
    }

    return;
  }

  
  
  /* Loop on subframes */
  
  cur_exc = exc;
  
  for (i_subfr = 0;  i_subfr < L_FRAME; i_subfr += L_SUBFR) {

    /* generate random adaptive codebook & fixed codebook parameters */
    /*****************************************************************/
    temp1 = Random(seed);
    frac = sub((temp1 & (Word16)0x0003), 1);
    if(sub(frac, 2) == 0) frac = 0;
    temp1 = shr(temp1, 2);
    t0 = add((temp1 & (Word16)0x003F), 40);
    temp1 = shr(temp1, 6);
    temp2 = temp1 & (Word16)0x0007;
    pos[0] = add(shl(temp2, 2), temp2); /* 5 * temp2 */
    temp1 = shr(temp1, 3);
    sign[0] = temp1 & (Word16)0x0001;
    temp1 = shr(temp1, 1);
    temp2 = temp1 & (Word16)0x0007;
    temp2 = add(shl(temp2, 2), temp2);
    pos[1] = add(temp2, 1);     /* 5 * x + 1 */
    temp1 = shr(temp1, 3);
    sign[1] = temp1 & (Word16)0x0001;
    temp1 = Random(seed);
    temp2 = temp1 & (Word16)0x0007;
    temp2 = add(shl(temp2, 2), temp2);
    pos[2] = add(temp2, 2);     /* 5 * x + 2 */
    temp1 = shr(temp1, 3);
    sign[2] = temp1 & (Word16)0x0001;
    temp1 = shr(temp1, 1);
    temp2 = temp1 & (Word16)0x000F;
    pos[3] = add((temp2 & (Word16)1), 3); /* j+3*/
    temp2 = (shr(temp2, 1)) & (Word16)7;
    temp2 = add(shl(temp2, 2), temp2); /* 5i */
    pos[3] = add(pos[3], temp2);
    temp1 = shr(temp1, 4);
    sign[3] = temp1 & (Word16)0x0001;
    Gp = Random(seed) & (Word16)0x1FFF; /* < 0.5 Q14 */
    Gp2 = shl(Gp, 1);           /* Q15 */


    /* Generate gaussian excitation */
    /********************************/
    L_acc = 0L;
    for(i=0; i<L_SUBFR; i++) {
      temp1 = Gauss(seed);
      L_acc = L_mac(L_acc, temp1, temp1);
      excg[i] = temp1;
    }

/*
    Compute fact = alpha x cur_gain * sqrt(L_SUBFR / Eg)
    with Eg = SUM(i=0->39) excg[i]^2
    and alpha = 0.5
    alpha x sqrt(L_SUBFR)/2 = 1 + FRAC1
*/
    L_acc = Inv_sqrt(L_shr(L_acc,1));  /* Q30 */
    L_Extract(L_acc, &hi, &lo);
    /* cur_gain = cur_gainR << 3 */
    temp1 = mult_r(cur_gain, FRAC1);
    temp1 = add(cur_gain, temp1);
    /* <=> alpha x cur_gainR x 2^2 x sqrt(L_SUBFR) */

    L_acc = Mpy_32_16(hi, lo, temp1);   /* fact << 17 */
    sh = norm_l(L_acc);
    temp1 = extract_h(L_shl(L_acc, sh));  /* fact << (sh+1) */

    sh = sub(sh, 14);
    for(i=0; i<L_SUBFR; i++) {
      temp2 = mult_r(excg[i], temp1);
      temp2 = shr_r(temp2, sh);   /* shl if sh < 0 */
      excg[i] = temp2;
    }

    /* generate random  adaptive excitation */
    /****************************************/
    Pred_lt_3(cur_exc, t0, frac, L_SUBFR);


    /* compute adaptive + gaussian exc -> cur_exc */
    /**********************************************/
    max = 0;
    for(i=0; i<L_SUBFR; i++) {
      temp1 = mult_r(cur_exc[i], Gp2);
      temp1 = add(temp1, excg[i]); /* may overflow */
      cur_exc[i] = temp1;
      temp1 = abs_s(temp1);
      if(sub(temp1,max) > 0) max = temp1;
    }

    /* rescale cur_exc -> excs */
    if(max == 0) sh = 0;
    else {
      sh = sub(3, norm_s(max));
      if(sh <= 0) sh = 0;
    }
    for(i=0; i<L_SUBFR; i++) {
      excs[i] = shr(cur_exc[i], sh);
    }

    /* Compute fixed code gain */
    /***************************/

    /**********************************************************/
    /*** Solve EQ(X) = 4 X**2 + 2 b X + c                     */
    /**********************************************************/

    L_ener = 0L;
    for(i=0; i<L_SUBFR; i++) {
      L_ener = L_mac(L_ener, excs[i], excs[i]);
    } /* ener x 2^(-2sh + 1) */

    /* inter_exc = b >> sh */
    inter_exc = 0;
    for(i=0; i<4; i++) {
      j = pos[i];
      if(sign[i] == 0) {
        inter_exc = sub(inter_exc, excs[j]);
      }
      else {
        inter_exc = add(inter_exc, excs[j]);
      }
    }

    /* Compute k = cur_gainR x cur_gainR x L_SUBFR */
    L_acc = L_mult(cur_gain, L_SUBFR);
    L_acc = L_shr(L_acc, 6);
    temp1 = extract_l(L_acc);   /* cur_gainR x L_SUBFR x 2^(-2) */
    L_k   = L_mult(cur_gain, temp1); /* k << 2 */
    temp1 = add(1, shl(sh,1));
    L_acc = L_shr(L_k, temp1);  /* k x 2^(-2sh+1) */

    /* Compute delta = b^2 - 4 c */
    L_acc = L_sub(L_acc, L_ener); /* - 4 c x 2^(-2sh-1) */
    inter_exc = shr(inter_exc, 1);
    L_acc = L_mac(L_acc, inter_exc, inter_exc); /* 2^(-2sh-1) */
    sh = add(sh, 1);
    /* inter_exc = b x 2^(-sh) */
    /* L_acc = delta x 2^(-2sh+1) */

    if(L_acc < 0) {

      /* adaptive excitation = 0 */
      Copy(excg, cur_exc, L_SUBFR);
      temp1 = abs_s(excg[(int)pos[0]]) | abs_s(excg[(int)pos[1]]);
      temp2 = abs_s(excg[(int)pos[2]]) | abs_s(excg[(int)pos[3]]);
      temp1 = temp1 | temp2;
      sh = ((temp1 & (Word16)0x4000) == 0) ? (Word16)1 : (Word16)2;
      inter_exc = 0;
      for(i=0; i<4; i++) {
        temp1 = shr(excg[(int)pos[i]], sh);
        if(sign[i] == 0) {
          inter_exc = sub(inter_exc, temp1);
        }
        else {
          inter_exc = add(inter_exc, temp1);
        }
      } /* inter_exc = b >> sh */
      L_Extract(L_k, &hi, &lo);
      L_acc = Mpy_32_16(hi, lo, K0); /* k x (1- alpha^2) << 2 */
      temp1 = sub(shl(sh, 1), 1); /* temp1 > 0 */
      L_acc = L_shr(L_acc, temp1); /* 4k x (1 - alpha^2) << (-2sh+1) */
      L_acc = L_mac(L_acc, inter_exc, inter_exc); /* delta << (-2sh+1) */
      Gp = 0;
    }

    temp2 = Sqrt(L_acc);        /* >> sh */
    x1 = sub(temp2, inter_exc);
    x2 = negate(add(inter_exc, temp2)); /* x 2^(-sh+2) */
    if(sub(abs_s(x2),abs_s(x1)) < 0) x1 = x2;
    temp1 = sub(2, sh);
    g = shr_r(x1, temp1);       /* shl if temp1 < 0 */
    if(g >= 0) {
      if(sub(g, G_MAX) > 0) g = G_MAX;
    }
    else {
      if(add(g, G_MAX) < 0) g = negate(G_MAX);
    }

    /* Update cur_exc with ACELP excitation */
    for(i=0; i<4; i++) {
      j = pos[i];
      if(sign[i] != 0) {
        cur_exc[j] = add(cur_exc[j], g);
      }
      else {
        cur_exc[j] = sub(cur_exc[j], g);
      }
    }

    if(flag_cod != FLAG_DEC) update_exc_err(L_exc_err, Gp, t0);

    cur_exc += L_SUBFR;


  } /* end of loop on subframes */
  
  return;
}
Exemple #13
0
void Coder_ld8g(
  Word16 ana[],     /* (o)     : analysis parameters                        */
 Word16 frame,                   /* input : frame counter */
  Word16 dtx_enable,               /* input : DTX enable flag */
  Word16 rate           /* input   : rate selector/frame  =0 6.4kbps , =1 8kbps,= 2 11.8 kbps*/
)
{

  /* LPC analysis */
    Word16 r_l_fwd[NP+1], r_h_fwd[NP+1];    /* Autocorrelations low and hi (forward) */
    Word32 r_bwd[M_BWDP1];      /* Autocorrelations (backward) */
    Word16 r_l_bwd[M_BWDP1];      /* Autocorrelations low (backward) */
    Word16 r_h_bwd[M_BWDP1];      /* Autocorrelations high (backward) */
    Word16 rc_fwd[M];                 /* Reflection coefficients : forward analysis */
    Word16 rc_bwd[M_BWD];         /* Reflection coefficients : backward analysis */
    Word16 A_t_fwd[MP1*2];          /* A(z) forward unquantized for the 2 subframes */
    Word16 A_t_fwd_q[MP1*2];      /* A(z) forward quantized for the 2 subframes */
    Word16 A_t_bwd[2*M_BWDP1];    /* A(z) backward for the 2 subframes */
    Word16 *Aq;           /* A(z) "quantized" for the 2 subframes */
    Word16 *Ap;           /* A(z) "unquantized" for the 2 subframes */
    Word16 *pAp, *pAq;
    Word16 Ap1[M_BWDP1];          /* A(z) with spectral expansion         */
    Word16 Ap2[M_BWDP1];          /* A(z) with spectral expansion         */
    Word16 lsp_new[M], lsp_new_q[M]; /* LSPs at 2th subframe                 */
    Word16 lsf_int[M];               /* Interpolated LSF 1st subframe.       */
    Word16 lsf_new[M];
    Word16 lp_mode;                  /* Backward / Forward Indication mode */
    Word16 m_ap, m_aq, i_gamma;
    Word16 code_lsp[2];

    /* Other vectors */

    Word16 h1[L_SUBFR];            /* Impulse response h1[]              */
    Word16 xn[L_SUBFR];            /* Target vector for pitch search     */
    Word16 xn2[L_SUBFR];           /* Target vector for codebook search  */
    Word16 code[L_SUBFR];          /* Fixed codebook excitation          */
    Word16 y1[L_SUBFR];            /* Filtered adaptive excitation       */
    Word16 y2[L_SUBFR];            /* Filtered fixed codebook excitation */
    Word16 g_coeff[4];             /* Correlations between xn & y1       */
    Word16 res2[L_SUBFR];          /* residual after long term prediction*/
    Word16 g_coeff_cs[5];
    Word16 exp_g_coeff_cs[5];      /* Correlations between xn, y1, & y2
                                     <y1,y1>, -2<xn,y1>,
                                          <y2,y2>, -2<xn,y2>, 2<y1,y2> */
    /* Scalars */
    Word16 i, j, k, i_subfr;
    Word16 T_op, T0, T0_min, T0_max, T0_frac;
    Word16 gain_pit, gain_code, index;
    Word16 taming, pit_sharp;
    Word16 sat_filter;
    Word32 L_temp;
    Word16 freq_cur[M];

    /* For G.729B */
    Word16 rh_nbe[MP1];             
    Word16 lsfq_mem[MA_NP][M];
    Word16 exp_R0, Vad;
    Word16 tmp1, tmp2,avg_lag;
    
    Word16 temp, Energy_db;
    
/*------------------------------------------------------------------------*
 *  - Perform LPC analysis:                                               *
 *       * autocorrelation + lag windowing                                *
 *       * Levinson-durbin algorithm to find a[]                          *
 *       * convert a[] to lsp[]                                           *
 *       * quantize and code the LSPs                                     *
 *       * find the interpolated LSPs and convert to a[] for the 2        *
 *         subframes (both quantized and unquantized)                     *
 *------------------------------------------------------------------------*/
    /* ------------------- */
    /* LP Forward analysis */
    /* ------------------- */
    Autocorrg(p_window, NP, r_h_fwd, r_l_fwd, &exp_R0);    /* Autocorrelations */
    Copy(r_h_fwd, rh_nbe, MP1);
    Lag_window(NP, r_h_fwd, r_l_fwd);                     /* Lag windowing    */
    Levinsong(M, r_h_fwd, r_l_fwd, &A_t_fwd[MP1], rc_fwd, old_A_fwd, old_rc_fwd,&temp ); /* Levinson Durbin  */
    Az_lsp(&A_t_fwd[MP1], lsp_new, lsp_old);      /* From A(z) to lsp */
    
    /* For G.729B */
    /* ------ VAD ------- */
    if (dtx_enable == 1) {
        Lsp_lsf(lsp_new, lsf_new, M);
        vadg(rc_fwd[1], lsf_new, r_h_fwd, r_l_fwd, exp_R0, p_window, frame,
            pastVad, ppastVad, &Vad, &Energy_db);
        
        musdetect( rate, r_h_fwd[0], r_l_fwd[0], exp_R0,rc_fwd ,lag_buf , pgain_buf,
            prev_lp_mode, frame,pastVad, &Vad, Energy_db);
        
        Update_cng(rh_nbe, exp_R0, Vad);
    }
    else Vad = 1;
    

    /* -------------------- */
    /* LP Backward analysis */
    /* -------------------- */
    /* -------------------- */
    /* LP Backward analysis */
    /* -------------------- */
    if ( (rate-(1-Vad))== G729E) {
        
        /* LPC recursive Window as in G728 */
        autocorr_hyb_window(synth, r_bwd, rexp); /* Autocorrelations */
        
        Lag_window_bwd(r_bwd, r_h_bwd, r_l_bwd);  /* Lag windowing    */
        
        /* Fixed Point Levinson (as in G729) */
        Levinsong(M_BWD, r_h_bwd, r_l_bwd, &A_t_bwd[M_BWDP1], rc_bwd,
            old_A_bwd, old_rc_bwd, &temp );
        
        /* Tests saturation of A_t_bwd */
        sat_filter = 0;
        for (i=M_BWDP1; i<2*M_BWDP1; i++) if (A_t_bwd[i] >= 32767) sat_filter = 1;
        if (sat_filter == 1) Copy(A_t_bwd_mem, &A_t_bwd[M_BWDP1], M_BWDP1);
        else Copy(&A_t_bwd[M_BWDP1], A_t_bwd_mem, M_BWDP1);
        
        /* Additional bandwidth expansion on backward filter */
        Weight_Az(&A_t_bwd[M_BWDP1], GAMMA_BWD, M_BWD, &A_t_bwd[M_BWDP1]);
    }
    /*--------------------------------------------------*
    * Update synthesis signal for next frame.          *
    *--------------------------------------------------*/
    Copy(&synth[L_FRAME], &synth[0], MEM_SYN_BWD);
    
    /*--------------------------------------------------------------------*
    * Find interpolated LPC parameters in all subframes (unquantized).                                                  *
    * The interpolated parameters are in array A_t[] of size (M+1)*4     *
    *--------------------------------------------------------------------*/
    if( prev_lp_mode == 0) {
        Int_lpc(lsp_old, lsp_new, lsf_int, lsf_new, A_t_fwd);
    }
    else {
        /* no interpolation */
        /* unquantized */
        Lsp_Az(lsp_new, A_t_fwd);           /* Subframe 1 */
        Lsp_lsf(lsp_new, lsf_new, M);  /* transformation from LSP to LSF (freq.domain) */
        Copy(lsf_new, lsf_int, M);      /* Subframe 1 */
        
    }
    
    if(Vad == 1) {
        /* ---------------- */
        /* LSP quantization */
        /* ---------------- */
        Qua_lspe(lsp_new, lsp_new_q, code_lsp, freq_prev, freq_cur);

        /*--------------------------------------------------------------------*
        * Find interpolated LPC parameters in all subframes (quantized)  *
        * the quantized interpolated parameters are in array Aq_t[]      *
        *--------------------------------------------------------------------*/
        if( prev_lp_mode == 0) {
            Int_qlpc(lsp_old_q, lsp_new_q, A_t_fwd_q);
        }
        else {
            /* no interpolation */
            Lsp_Az(lsp_new_q, &A_t_fwd_q[MP1]);              /* Subframe 2 */
            Copy(&A_t_fwd_q[MP1], A_t_fwd_q, MP1);      /* Subframe 1 */
        }
        /*---------------------------------------------------------------------*
        * - Decision for the switch Forward / Backward                        *
        *---------------------------------------------------------------------*/
        if(rate == G729E) {
            set_lpc_modeg(speech, A_t_fwd_q, A_t_bwd, &lp_mode,
                lsp_new, lsp_old, &bwd_dominant, prev_lp_mode, prev_filter,
                &C_int, &glob_stat, &stat_bwd, &val_stat_bwd);
        }
        else {
            update_bwd( &lp_mode, &bwd_dominant, &C_int, &glob_stat);
        }
    }
    else update_bwd( &lp_mode, &bwd_dominant, &C_int, &glob_stat);
    
    /* ---------------------------------- */
    /* update the LSPs for the next frame */
    /* ---------------------------------- */
    Copy(lsp_new, lsp_old, M);
    
    /*----------------------------------------------------------------------*
    * - Find the weighted input speech w_sp[] for the whole speech frame   *
    *----------------------------------------------------------------------*/
    if(lp_mode == 0) {
        m_ap = M;
        if (bwd_dominant == 0) Ap = A_t_fwd;
        else Ap = A_t_fwd_q;
        perc_var(gamma1, gamma2, lsf_int, lsf_new, rc_fwd);
    }
    else {
        if (bwd_dominant == 0) {
            m_ap = M;
            Ap = A_t_fwd;
        }
        else {
            m_ap = M_BWD;
            Ap = A_t_bwd;
        }
        perc_vare(gamma1, gamma2, bwd_dominant);
    }
    pAp = Ap;
    for (i=0; i<2; i++) {
        Weight_Az(pAp, gamma1[i], m_ap, Ap1);
        Weight_Az(pAp, gamma2[i], m_ap, Ap2);
        Residue(m_ap, Ap1, &speech[i*L_SUBFR], &wsp[i*L_SUBFR], L_SUBFR);
        Syn_filte(m_ap,  Ap2, &wsp[i*L_SUBFR], &wsp[i*L_SUBFR], L_SUBFR,
            &mem_w[M_BWD-m_ap], 0);
        for(j=0; j<M_BWD; j++) mem_w[j] = wsp[i*L_SUBFR+L_SUBFR-M_BWD+j];
        pAp += m_ap+1;
    }

    /* ---------------------- */
    /* Case of Inactive frame */
    /* ---------------------- */
    if (Vad == 0){

        for (i=0; i<MA_NP; i++) Copy(&freq_prev[i][0], &lsfq_mem[i][0], M);
        Cod_cngg(exc, pastVad, lsp_old_q, old_A_fwd, old_rc_fwd, A_t_fwd_q, ana, lsfq_mem, &seed);
        for (i=0; i<MA_NP; i++) Copy(&lsfq_mem[i][0], &freq_prev[i][0], M);
        ppastVad = pastVad;
        pastVad = Vad;

        /* UPDATE wsp, mem_w, mem_syn, mem_err, and mem_w0 */
        pAp  = A_t_fwd;     /* pointer to interpolated LPC parameters           */
        pAq = A_t_fwd_q;    /* pointer to interpolated quantized LPC parameters */
        i_gamma = 0;
        for(i_subfr=0; i_subfr < L_FRAME; i_subfr += L_SUBFR) {
            Weight_Az(pAp, gamma1[i_gamma], M, Ap1);
            Weight_Az(pAp, gamma2[i_gamma], M, Ap2);
              i_gamma = add(i_gamma,1);

            /* update mem_syn */
            Syn_filte(M, pAq, &exc[i_subfr], &synth_ptr[i_subfr], L_SUBFR, &mem_syn[M_BWD-M], 0);
            for(j=0; j<M_BWD; j++) mem_syn[j] = synth_ptr[i_subfr+L_SUBFR-M_BWD+j];

            /* update mem_w0 */
            for (i=0; i<L_SUBFR; i++)
                error[i] = speech[i_subfr+i] - synth_ptr[i_subfr+i];
            Residue(M, Ap1, error, xn, L_SUBFR);
            Syn_filte(M, Ap2, xn, xn, L_SUBFR, &mem_w0[M_BWD-M], 0);
            for(j=0; j<M_BWD; j++) mem_w0[j] = xn[L_SUBFR-M_BWD+j];

            /* update mem_err */
            for (i = L_SUBFR-M_BWD, j = 0; i < L_SUBFR; i++, j++)
                mem_err[j] = error[i];

            for (i= 0; i< 4; i++)
                pgain_buf[i] = pgain_buf[i+1];
            pgain_buf[4] =  8192;

            pAp += MP1;
            pAq += MP1;
        }
        /* update previous filter for next frame */
        Copy(&A_t_fwd_q[MP1], prev_filter, MP1);
        for(i=MP1; i <M_BWDP1; i++) prev_filter[i] = 0;
        prev_lp_mode = lp_mode;

        sharp = SHARPMIN;

        /* Update memories for next frames */
        Copy(&old_speech[L_FRAME], &old_speech[0], L_TOTAL-L_FRAME);
        Copy(&old_wsp[L_FRAME], &old_wsp[0], PIT_MAX);
        Copy(&old_exc[L_FRAME], &old_exc[0], PIT_MAX+L_INTERPOL);
        return;

    }  /* End of inactive frame case */

    /* -------------------- */
    /* Case of Active frame */
    /* -------------------- */
    *ana++ = rate+ (Word16)2; /* bit rate mode */

    if(lp_mode == 0) {
        m_aq = M;
        Aq = A_t_fwd_q;
        /* update previous filter for next frame */
        Copy(&Aq[MP1], prev_filter, MP1);
        for(i=MP1; i <M_BWDP1; i++) prev_filter[i] = 0;
        for(j=MP1; j<M_BWDP1; j++) ai_zero[j] = 0;
    }
    else {
        m_aq = M_BWD;
        Aq = A_t_bwd;
        if (bwd_dominant == 0) {
            for(j=MP1; j<M_BWDP1; j++) ai_zero[j] = 0;
        }
        /* update previous filter for next frame */
        Copy(&Aq[M_BWDP1], prev_filter, M_BWDP1);
    }

    if(dtx_enable == 1) {
        seed = INIT_SEED;
        ppastVad = pastVad;
        pastVad = Vad;
    }
    if (rate == G729E) *ana++ = lp_mode;

    /*----------------------------------------------------------------------*
    * - Find the weighted input speech w_sp[] for the whole speech frame   *
    * - Find the open-loop pitch delay                                     *
    *----------------------------------------------------------------------*/
    if( lp_mode == 0) {
        Copy(lsp_new_q, lsp_old_q, M);
        Lsp_prev_update(freq_cur, freq_prev);
        *ana++ = code_lsp[0];
        *ana++ = code_lsp[1];
    }

    /* Find open loop pitch lag */
    T_op = Pitch_ol(wsp, PIT_MIN, PIT_MAX, L_FRAME);

    for (i= 0; i< 4; i++)
        lag_buf[i] = lag_buf[i+1];

        avg_lag = add(lag_buf[0],lag_buf[1]);
        avg_lag = add(avg_lag,lag_buf[2]);
        avg_lag = add(avg_lag,lag_buf[3]);
        avg_lag = mult_r(avg_lag,8192);

        tmp1 = sub(T_op,shl(avg_lag,1)); 
        tmp2 = sub(T_op,add(shl(avg_lag,1),avg_lag));
        if( sub(abs_s(tmp1), 4)<0){
           lag_buf[4] = shr(T_op,1);
        }
        else if( sub(abs_s(tmp2),6)<0){
           lag_buf[4] = mult(T_op,10923);
        }
    else{
        lag_buf[4] = T_op;
        }


    /* Range for closed loop pitch search in 1st subframe */
    T0_min = sub(T_op, 3);
    if (sub(T0_min,PIT_MIN)<0) {
        T0_min = PIT_MIN;
    }

    T0_max = add(T0_min, 6);
    if (sub(T0_max ,PIT_MAX)>0)
    {
        T0_max = PIT_MAX;
        T0_min = sub(T0_max, 6);
    }

    /*------------------------------------------------------------------------*
    *          Loop for every subframe in the analysis frame                 *
    *------------------------------------------------------------------------*
    *  To find the pitch and innovation parameters. The subframe size is     *
    *  L_SUBFR and the loop is repeated 2 times.                             *
    *     - find the weighted LPC coefficients                               *
    *     - find the LPC residual signal res[]                               *
    *     - compute the target signal for pitch search                       *
    *     - compute impulse response of weighted synthesis filter (h1[])     *
    *     - find the closed-loop pitch parameters                            *
    *     - encode the pitch delay                                           *
    *     - update the impulse response h1[] by including fixed-gain pitch   *
    *     - find target vector for codebook search                           *
    *     - codebook search                                                  *
    *     - encode codebook address                                          *
    *     - VQ of pitch and codebook gains                                   *
    *     - find synthesis speech                                            *
    *     - update states of weighting filter                                *
    *------------------------------------------------------------------------*/
    pAp  = Ap;     /* pointer to interpolated "unquantized"LPC parameters           */
    pAq = Aq;    /* pointer to interpolated "quantized" LPC parameters */

    i_gamma = 0;

    for (i_subfr = 0;  i_subfr < L_FRAME; i_subfr += L_SUBFR) {

        /*---------------------------------------------------------------*
        * Find the weighted LPC coefficients for the weighting filter.  *
        *---------------------------------------------------------------*/
        Weight_Az(pAp, gamma1[i_gamma], m_ap, Ap1);
        Weight_Az(pAp, gamma2[i_gamma], m_ap, Ap2);

        /*---------------------------------------------------------------*
        * Compute impulse response, h1[], of weighted synthesis filter  *
        *---------------------------------------------------------------*/
        for (i = 0; i <=m_ap; i++) ai_zero[i] = Ap1[i];
        Syn_filte(m_aq,  pAq, ai_zero, h1, L_SUBFR, zero, 0);
        Syn_filte(m_ap,  Ap2, h1, h1, L_SUBFR, zero, 0);

        /*------------------------------------------------------------------------*
        *                                                                        *
        *          Find the target vector for pitch search:                      *
        *          ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~                       *
        *                                                                        *
        *              |------|  res[n]                                          *
        *  speech[n]---| A(z) |--------                                          *
        *              |------|       |   |--------| error[n]  |------|          *
        *                    zero -- (-)--| 1/A(z) |-----------| W(z) |-- target *
        *                    exc          |--------|           |------|          *
        *                                                                        *
        * Instead of subtracting the zero-input response of filters from         *
        * the weighted input speech, the above configuration is used to          *
        * compute the target vector. This configuration gives better performance *
        * with fixed-point implementation. The memory of 1/A(z) is updated by    *
        * filtering (res[n]-exc[n]) through 1/A(z), or simply by subtracting     *
        * the synthesis speech from the input speech:                            *
        *    error[n] = speech[n] - syn[n].                                      *
        * The memory of W(z) is updated by filtering error[n] through W(z),      *
        * or more simply by subtracting the filtered adaptive and fixed          *
        * codebook excitations from the target:                                  *
        *     target[n] - gain_pit*y1[n] - gain_code*y2[n]                       *
        * as these signals are already available.                                *
        *                                                                        *
        *------------------------------------------------------------------------*/
        Residue(m_aq, pAq, &speech[i_subfr], &exc[i_subfr], L_SUBFR);   /* LPC residual */
        for (i=0; i<L_SUBFR; i++) res2[i] = exc[i_subfr+i];
        Syn_filte(m_aq,  pAq, &exc[i_subfr], error, L_SUBFR,
                &mem_err[M_BWD-m_aq], 0);
        Residue(m_ap, Ap1, error, xn, L_SUBFR);
        Syn_filte(m_ap,  Ap2, xn, xn, L_SUBFR, &mem_w0[M_BWD-m_ap], 0);    /* target signal xn[]*/

        /*----------------------------------------------------------------------*
        *                 Closed-loop fractional pitch search                  *
        *----------------------------------------------------------------------*/
        T0 = Pitch_fr3(&exc[i_subfr], xn, h1, L_SUBFR, T0_min, T0_max,
                               i_subfr, &T0_frac);

        index = Enc_lag3(T0, T0_frac, &T0_min, &T0_max,PIT_MIN,PIT_MAX,
                            i_subfr);

        *ana++ = index;

        if ( (i_subfr == 0) ) {
            *ana = Parity_Pitch(index);
            if( rate == G729E) {
                *ana ^= (shr(index, 1) & 0x0001);
            }
            ana++;
        }
       /*-----------------------------------------------------------------*
        *   - find unity gain pitch excitation (adaptive codebook entry)  *
        *     with fractional interpolation.                              *
        *   - find filtered pitch exc. y1[]=exc[] convolve with h1[])     *
        *   - compute pitch gain and limit between 0 and 1.2              *
        *   - update target vector for codebook search                    *
        *   - find LTP residual.                                          *
        *-----------------------------------------------------------------*/

        Pred_lt_3(&exc[i_subfr], T0, T0_frac, L_SUBFR);

        Convolve(&exc[i_subfr], h1, y1, L_SUBFR);

        gain_pit = G_pitch(xn, y1, g_coeff, L_SUBFR);


        /* clip pitch gain if taming is necessary */
        taming = test_err(T0, T0_frac);
        if( taming == 1){
            if (sub(gain_pit, GPCLIP) > 0) {
                gain_pit = GPCLIP;
            }
        }

        /* xn2[i]   = xn[i] - y1[i] * gain_pit  */
        for (i = 0; i < L_SUBFR; i++) {
            L_temp = L_mult(y1[i], gain_pit);
            L_temp = L_shl(L_temp, 1);               /* gain_pit in Q14 */
            xn2[i] = sub(xn[i], extract_h(L_temp));
        }

        /*-----------------------------------------------------*
        * - Innovative codebook search.                       *
        *-----------------------------------------------------*/
        switch (rate) {

            case G729:    /* 8 kbit/s */
            {

             /* case 8 kbit/s */
                index = ACELP_Codebook(xn2, h1, T0, sharp, i_subfr, code, y2, &i);
                *ana++ = index;        /* Positions index */
                *ana++ = i;            /* Signs index     */
                break;
            }

            case G729E:    /* 11.8 kbit/s */
            {

           /*-----------------------------------------------------------------*
            * Include fixed-gain pitch contribution into impulse resp. h[]    *
            *-----------------------------------------------------------------*/
            pit_sharp = shl(sharp, 1);        /* From Q14 to Q15 */
            if(T0 < L_SUBFR) {
                for (i = T0; i < L_SUBFR; i++){   /* h[i] += pitch_sharp*h[i-T0] */
                  h1[i] = add(h1[i], mult(h1[i-T0], pit_sharp));
                }
            }
            /* calculate residual after long term prediction */
            /* res2[i] -= exc[i+i_subfr] * gain_pit */
            for (i = 0; i < L_SUBFR; i++) {
                L_temp = L_mult(exc[i+i_subfr], gain_pit);
                L_temp = L_shl(L_temp, 1);               /* gain_pit in Q14 */
                res2[i] = sub(res2[i], extract_h(L_temp));
            }
            if (lp_mode == 0) ACELP_10i40_35bits(xn2, res2, h1, code, y2, ana); /* Forward */
            else ACELP_12i40_44bits(xn2, res2, h1, code, y2, ana); /* Backward */
            ana += 5;

           /*-----------------------------------------------------------------*
            * Include fixed-gain pitch contribution into code[].              *
            *-----------------------------------------------------------------*/
            if(T0 < L_SUBFR) {
                for (i = T0; i < L_SUBFR; i++) {   /* code[i] += pitch_sharp*code[i-T0] */
                    code[i] = add(code[i], mult(code[i-T0], pit_sharp));
                }
            }
            break;

        }
            default : {
                printf("Unrecognized bit rate\n");
                exit(-1);
            }
        }  /* end of switch */

        /*-----------------------------------------------------*
        * - Quantization of gains.                            *
        *-----------------------------------------------------*/

        g_coeff_cs[0]     = g_coeff[0];                   /* <y1,y1> */
        exp_g_coeff_cs[0] = negate(g_coeff[1]);           /* Q-Format:XXX -> JPN  */
        g_coeff_cs[1]     = negate(g_coeff[2]);           /* (xn,y1) -> -2<xn,y1> */
        exp_g_coeff_cs[1] = negate(add(g_coeff[3], 1));   /* Q-Format:XXX -> JPN  */

        Corr_xy2( xn, y1, y2, g_coeff_cs, exp_g_coeff_cs );  /* Q0 Q0 Q12 ^Qx ^Q0 */
                         /* g_coeff_cs[3]:exp_g_coeff_cs[3] = <y2,y2>   */
                         /* g_coeff_cs[4]:exp_g_coeff_cs[4] = -2<xn,y2> */
                         /* g_coeff_cs[5]:exp_g_coeff_cs[5] = 2<y1,y2>  */

        index = Qua_gain(code, g_coeff_cs, exp_g_coeff_cs, L_SUBFR,
                &gain_pit, &gain_code, taming);

        *ana++ = index;

        /*------------------------------------------------------------*
        * - Update pitch sharpening "sharp" with quantized gain_pit  *
        *------------------------------------------------------------*/
        for (i= 0; i< 4; i++)
            pgain_buf[i] = pgain_buf[i+1];
        pgain_buf[4] = gain_pit;

        sharp = gain_pit;
        if (sub(sharp, SHARPMAX) > 0) sharp = SHARPMAX;
        else {
            if (sub(sharp, SHARPMIN) < 0) sharp = SHARPMIN;
        }

        /*------------------------------------------------------*
        * - Find the total excitation                          *
        * - find synthesis speech corresponding to exc[]       *
        * - update filters memories for finding the target     *
        *   vector in the next subframe                        *
        *   (update error[-m..-1] and mem_w_err[])             *
        *   update error function for taming process           *
        *------------------------------------------------------*/
        for (i = 0; i < L_SUBFR;  i++) {
            /* exc[i] = gain_pit*exc[i] + gain_code*code[i]; */
            /* exc[i]  in Q0   gain_pit in Q14               */
            /* code[i] in Q13  gain_cod in Q1                */

            L_temp = L_mult(exc[i+i_subfr], gain_pit);
            L_temp = L_mac(L_temp, code[i], gain_code);
            L_temp = L_shl(L_temp, 1);
            exc[i+i_subfr] = round(L_temp);
        }

        update_exc_err(gain_pit, T0);

        Syn_filte(m_aq,  pAq, &exc[i_subfr], &synth_ptr[i_subfr], L_SUBFR,
                &mem_syn[M_BWD-m_aq], 0);
        for(j=0; j<M_BWD; j++) mem_syn[j] = synth_ptr[i_subfr+L_SUBFR-M_BWD+j];

        for (i = L_SUBFR-M_BWD, j = 0; i < L_SUBFR; i++, j++) {
            mem_err[j] = sub(speech[i_subfr+i], synth_ptr[i_subfr+i]);
            temp       = extract_h(L_shl( L_mult(y1[i], gain_pit),  1) );
            k          = extract_h(L_shl( L_mult(y2[i], gain_code), 2) );
            mem_w0[j]  = sub(xn[i], add(temp, k));
        }
        pAp   += m_ap+1;
        pAq   += m_aq+1;
        i_gamma = add(i_gamma,1);
    }

    /*--------------------------------------------------*
    * Update signal for next frame.                    *
    * -> shift to the left by L_FRAME:                 *
    *     speech[], wsp[] and  exc[]                   *
    *--------------------------------------------------*/
    Copy(&old_speech[L_FRAME], &old_speech[0], L_TOTAL-L_FRAME);
    Copy(&old_wsp[L_FRAME], &old_wsp[0], PIT_MAX);
    Copy(&old_exc[L_FRAME], &old_exc[0], PIT_MAX+L_INTERPOL);
    prev_lp_mode = lp_mode;
    return;
}
Exemple #14
0
/*
**
** Function:            Lsp_Svq()
**
** Description:     Performs the search of the VQ tables to find
**          the optimum LSP indices for all three bands.
**          For each band, the search finds the index which 
**          minimizes the weighted squared error between 
**          the table entry and the target.
**
** Links to text:   Section 2.5
**
** Arguments:       
**
**  Word16 Tv[]     VQ target vector (10 words)
**  Word16 Wvect[]      VQ weight vector (10 words)
**
** Outputs:         None
**
** Return value:    
**
**  Word32      Long word packed with the 3 VQ indices.  Band 0
**          corresponds to bits [23:16], band 1 corresponds
**          to bits [15:8], and band 2 corresponds to bits [7:0].
**              
*/
Word32   Lsp_Svq( Word16 *Tv, Word16 *Wvect )
{
    int   i,j,k ;

    Word32   Rez,Indx    ;
    Word32   Acc0,Acc1   ;

    Word16   Tmp[LpcOrder] ;
    Word16  *LspQntPnt  ;


 /*
  * Initialize the return value
  */
    Rez = (Word32) 0 ;

 /*
  * Quantize each band separately
  */
    for ( k = 0 ; k < LspQntBands ; k ++ ) {

 /*
  * Search over the entire VQ table to find the index that
  * minimizes the error.
  */

        /* Initialize the search */
        Acc1 = (Word32) -1 ;
        Indx = (Word32) 0 ;
        LspQntPnt = BandQntTable[k] ;

        for ( i = 0 ; i < LspCbSize ; i ++ ) {

 /*
  * Generate the metric, which is the negative error with the
  * constant component removed.
  */
            for ( j = 0 ; j < BandInfoTable[k][1] ; j ++ )
                Tmp[j] = mult_r( Wvect[BandInfoTable[k][0]+j],
                                                            LspQntPnt[j] ) ;

            Acc0 = (Word32) 0 ;
            for ( j = 0 ; j < BandInfoTable[k][1] ; j ++ )
                Acc0 = L_mac( Acc0, Tv[BandInfoTable[k][0]+j], Tmp[j] ) ;
            Acc0 = L_shl( Acc0, (Word16) 1 ) ;
            for ( j = 0 ; j < BandInfoTable[k][1] ; j ++ )
                Acc0 = L_msu( Acc0, LspQntPnt[j], Tmp[j] ) ;

            LspQntPnt += BandInfoTable[k][1] ;

 /*
  * Compare the metric to the previous maximum and select the
  * new index
  */
            if ( Acc0 > Acc1 ) {
                Acc1 = Acc0 ;
                Indx = (Word32) i ;
            }
        }

 /*
  * Pack the result with the optimum index for this band
  */
        Rez = L_shl( Rez, (Word16) LspCbBits ) ;
 //       Rez = L_add( Rez, Indx ) ;
		L_ADD(Rez, Indx, Rez);
    }

    return Rez ;
}
Exemple #15
0
/*----------------------------------------------------------------------------
 *  pst_ltp - harmonic postfilter
 *----------------------------------------------------------------------------
 */
static void pst_ltpe(
 Word16 t0,             /* input : pitch delay given by coder */
 Word16 *ptr_sig_in,    /* input : postfilter input filter (residu2) */
 Word16 *ptr_sig_pst0,  /* output: harmonic postfilter output */
 Word16 *vo,             /* output: voicing decision 0 = uv,  > 0 delay */
 Word16 gamma_harm      /* input: harmonic postfilter coefficient */
)
{

/**** Declare variables                                 */
    Word16 i;
    Word16 temp;
    Word16 ltpdel, phase;
    Word16 num_gltp, den_gltp;
    Word16 num2_gltp, den2_gltp;
    Word16 sh_num, sh_den;
    Word16 sh_num2, sh_den2;
    Word16 gain_plt;
    Word16 y_up[SIZ_Y_UP];
    Word16 *ptr_y_up;
    Word16 off_yup;
    Word16 *ptr_sig;
    Word16 sig_cadr[SIZ_RES2], *ptr_sig_cadr;
    Word16 nb_sh_sig;
    Word32 L_temp;

    /* input signal justified on 13 bits */
    ptr_sig = ptr_sig_in - MEM_RES2;

    temp = 0;
    for(i=0; i<SIZ_RES2; i++) {
        temp |= abs_s(ptr_sig[i]);
    }
    nb_sh_sig = sub(3, norm_s(temp));

    for(i=0; i<SIZ_RES2; i++) {   /* nb_sh_sig may be >0, <0 or =0 */
        sig_cadr[i] = shr( ptr_sig[i], nb_sh_sig);
    }

    ptr_sig_cadr = sig_cadr + MEM_RES2;

    /* Sub optimal delay search */
    search_del(t0, ptr_sig_cadr, &ltpdel, &phase, &num_gltp, &den_gltp,
                            &sh_num, &sh_den, y_up, &off_yup);
    *vo = ltpdel;

    if(num_gltp == 0)   {
        Copy(ptr_sig_in, ptr_sig_pst0, L_SUBFR);
    }

    else {
        if(phase == 0) {
            ptr_y_up = ptr_sig_in - ltpdel;
        }

        else {

            /* Filtering with long filter */
            compute_ltp_l(ptr_sig_cadr, ltpdel, phase, ptr_sig_pst0,
                &num2_gltp, &den2_gltp, &sh_num2, &sh_den2);

            if(select_ltp(num_gltp, den_gltp, sh_num, sh_den,
                num2_gltp, den2_gltp, sh_num2, sh_den2) == 1) {

                /* select short filter */
                temp   = sub(phase, 1);
                L_temp = L_mult(temp, L_SUBFRP1);
                L_temp = L_shr(L_temp, 1);
                temp   = extract_l(L_temp);
                temp   = add(temp, off_yup);
                /* ptr_y_up = y_up + (phase-1) * L_SUBFRP1 + off_yup */
                ptr_y_up = y_up + temp;
            }
            else {
                /* select long filter */
                num_gltp = num2_gltp;
                den_gltp = den2_gltp;
                sh_num   = sh_num2;
                sh_den   = sh_den2;
                ptr_y_up = ptr_sig_pst0;
            }

            /* rescale y_up */
            for(i=0; i<L_SUBFR; i++) {  /* nb_sh_sig may be >0, <0 or =0 */
                ptr_y_up[i] = shl(ptr_y_up[i], nb_sh_sig);
            }
        }

        temp = sub(sh_num,sh_den);
        if(temp >= 0) den_gltp = shr(den_gltp, temp);
        else {
            num_gltp = shl(num_gltp, temp); /*  >> (-temp) */
        }

        if(sub(num_gltp ,den_gltp)>=0) {
            /* beta bounded to 1 */
            if (gamma_harm == GAMMA_HARM)   gain_plt = MIN_GPLT;
            else {
                num_gltp = mult_r(den_gltp, gamma_harm);
                num_gltp = shr(num_gltp, 1);
                den_gltp = shr(den_gltp, 1);
                temp     = add(den_gltp, num_gltp);
                gain_plt = div_s(den_gltp, temp);
            }
        }
        else {
            /*gain_plt = den_gltp x 2**15 / (den_gltp + 0.5 num_gltp)  */
            /* shift 1 bit to avoid overflows in add                    */
            if (gamma_harm == GAMMA_HARM) {
                num_gltp = shr(num_gltp, 2);
            }
            else {
                num_gltp = mult_r(num_gltp, gamma_harm);
                num_gltp = shr(num_gltp, 1);
            }
            den_gltp = shr(den_gltp, 1);
            temp     = add(den_gltp, num_gltp);
            gain_plt = div_s(den_gltp, temp);
            
        }

        /** filtering by H0(z) = harmonic filter **/
        filt_plt(ptr_sig_in, ptr_y_up, ptr_sig_pst0, gain_plt);

    }

    return;
}
Word16 vad2 (Word16 * farray_ptr, vadState2 * st)
{

	/*
	 * The channel table is defined below.  In this table, the
	 * lower and higher frequency coefficients for each of the 16
	 * channels are specified.  The table excludes the coefficients
	 * with numbers 0 (DC), 1, and 64 (Foldover frequency).
	 */

	const static Word16 ch_tbl[NUM_CHAN][2] =
	{

		{2, 3},
		{4, 5},
		{6, 7},
		{8, 9},
		{10, 11},
		{12, 13},
		{14, 16},
		{17, 19},
		{20, 22},
		{23, 26},
		{27, 30},
		{31, 35},
		{36, 41},
		{42, 48},
		{49, 55},
		{56, 63}

	};

	/* channel energy scaling table - allows efficient division by number
         * of DFT bins in the channel: 1/2, 1/3, 1/4, etc.
	 */

	const static Word16 ch_tbl_sh[NUM_CHAN] =
	{
		16384, 16384, 16384, 16384, 16384, 16384, 10923, 10923,
		10923, 8192, 8192, 6554, 5461, 4681, 4681, 4096
	};

	/*
	 * The voice metric table is defined below.  It is a non-
	 * linear table with a deadband near zero.  It maps the SNR
	 * index (quantized SNR value) to a number that is a measure
	 * of voice quality.
	 */

	const static Word16 vm_tbl[90] =
	{
		2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2,
		3, 3, 3, 3, 3, 4, 4, 4, 5, 5, 5, 6, 6, 7, 7, 7,
		8, 8, 9, 9, 10, 10, 11, 12, 12, 13, 13, 14, 15,
		15, 16, 17, 17, 18, 19, 20, 20, 21, 22, 23, 24,
		24, 25, 26, 27, 28, 28, 29, 30, 31, 32, 33, 34,
		35, 36, 37, 37, 38, 39, 40, 41, 42, 43, 44, 45,
		46, 47, 48, 49, 50, 50, 50, 50, 50, 50, 50, 50,
		50, 50
	};

	/* hangover as a function of peak SNR (3 dB steps) */
	const static Word16 hangover_table[20] =
	{
		30, 30, 30, 30, 30, 30, 28, 26, 24, 22, 20, 18, 16, 14, 12, 10, 8, 8, 8, 8
	};

	/* burst sensitivity as a function of peak SNR (3 dB steps) */
	const static Word16 burstcount_table[20] =
	{
		8, 8, 8, 8, 8, 8, 8, 8, 7, 6, 5, 4, 4, 4, 4, 4, 4, 4, 4, 4
	};

	/* voice metric sensitivity as a function of peak SNR (3 dB steps) */
	const static Word16 vm_threshold_table[20] =
	{
                34, 34, 34, 34, 34, 34, 34, 34, 34, 34, 34, 40, 51, 71, 100, 139, 191, 257, 337, 432
	};


	/* State tables that use 22,9 or 27,4 scaling for ch_enrg[] */

   const static Word16 noise_floor_chan[2] =	{NOISE_FLOOR_CHAN_0, NOISE_FLOOR_CHAN_1};
	const  static Word16 min_chan_enrg[2] =	{MIN_CHAN_ENRG_0, MIN_CHAN_ENRG_1};
	const static Word16 ine_noise[2] = 		{INE_NOISE_0, INE_NOISE_1};
	const static Word16 fbits[2] = 		{FRACTIONAL_BITS_0, FRACTIONAL_BITS_1};
	const static Word16 state_change_shift_r[2] = {STATE_1_TO_0_SHIFT_R, STATE_0_TO_1_SHIFT_R};

	/* Energy scale table given 30,1 input scaling (also account for -6 dB shift on input) */
	const static Word16 enrg_norm_shift[2] = 	{(FRACTIONAL_BITS_0-1+2), (FRACTIONAL_BITS_1-1+2)};


	/* Automatic variables */

	Word32 Lenrg;				/* scaled as 30,1 */
	Word32 Ltne;				/* scaled as 22,9 */
	Word32 Ltce;				/* scaled as 22,9 or 27,4 */

	Word16 tne_db;				/* scaled as 7,8 */
	Word16 tce_db;				/* scaled as 7,8 */

	Word16 input_buffer[FRM_LEN];		/* used for block normalising input data */
	Word16 data_buffer[FFT_LEN];		/* used for in-place FFT */

	Word16 ch_snr[NUM_CHAN];		/* scaled as 7,8 */
	Word16 ch_snrq;				/* scaled as 15,0 (in 0.375 dB steps) */
	Word16 vm_sum;				/* scaled as 15,0 */
	Word16 ch_enrg_dev;			/* scaled as 7,8 */

	Word32 Lpeak;				/* maximum channel energy */
	Word16 p2a_flag;			/* flag to indicate spectral peak-to-average ratio > 10 dB */

	Word16 ch_enrg_db[NUM_CHAN];		/* scaled as 7,8 */
	Word16 ch_noise_db;			/* scaled as 7,8 */

	Word16 alpha;				/* scaled as 0,15 */
	Word16 one_m_alpha;			/* scaled as 0,15 */
	Word16 update_flag;			/* set to indicate a background noise estimate update */

	Word16 i, j, j1, j2;			/* Scratch variables */
	Word16 hi1, lo1;

	Word32 Ltmp, Ltmp1, Ltmp2;
	Word16 tmp;

	Word16 normb_shift;		/* block norm shift count */

	Word16 ivad;			/* intermediate VAD decision (return value) */
	Word16 tsnrq;			/* total signal-to-noise ratio (quantized 3 dB steps) scaled as 15,0 */
	Word16 xt;			/* instantaneous frame SNR in dB, scaled as 7,8 */

	Word16 state_change;


	/* Increment frame counter */
	st->Lframe_cnt = L_add(st->Lframe_cnt, 1);

	/* Block normalize the input */
	normb_shift = block_norm(farray_ptr, input_buffer, FRM_LEN, FFT_HEADROOM);

	/* Pre-emphasize the input data and store in the data buffer with the appropriate offset */
	for (i = 0; i < DELAY; i++)
	{
		data_buffer[i] = 0;									move16();
	}

	st->pre_emp_mem = shr_r(st->pre_emp_mem, sub(st->last_normb_shift, normb_shift));
	st->last_normb_shift = normb_shift;								move16();

	data_buffer[DELAY] = add(input_buffer[0], mult(PRE_EMP_FAC, st->pre_emp_mem));			move16();

	for (i = DELAY + 1, j = 1; i < DELAY + FRM_LEN; i++, j++)
	{
		data_buffer[i] = add(input_buffer[j], mult(PRE_EMP_FAC, input_buffer[j-1]));		move16();
	}
	st->pre_emp_mem = input_buffer[FRM_LEN-1];							move16();

	for (i = DELAY + FRM_LEN; i < FFT_LEN; i++)
	{
		data_buffer[i] = 0;									move16();
	}


	/* Perform FFT on the data buffer */
	r_fft(data_buffer);


	/* Use normb_shift factor to determine the scaling of the energy estimates */
	state_change = 0;										move16();
													test();
	if (st->shift_state == 0)
	{												test();
		if (sub(normb_shift, -FFT_HEADROOM+2) <= 0)
		{
			state_change = 1;								move16();
			st->shift_state = 1;								move16();
		}
	}
	else
	{												test();
		if (sub(normb_shift, -FFT_HEADROOM+5) >= 0)
		{
			state_change = 1;								move16();
			st->shift_state = 0;								move16();
		}
	}

	/* Scale channel energy estimate */								test();
	if (state_change)
	{
		for (i = LO_CHAN; i <= HI_CHAN; i++)
		{
			st->Lch_enrg[i] = L_shr(st->Lch_enrg[i], state_change_shift_r[st->shift_state]);	move32();
		}
	}


	/* Estimate the energy in each channel */
													test();
	if (L_sub(st->Lframe_cnt, 1) == 0)
	{
		alpha = 32767;										move16();
		one_m_alpha = 0;									move16();
	}
	else
	{
		alpha = CEE_SM_FAC;									move16();
		one_m_alpha = ONE_MINUS_CEE_SM_FAC;							move16();
	}

	for (i = LO_CHAN; i <= HI_CHAN; i++)
	{
		Lenrg = 0;										move16();
		j1 = ch_tbl[i][0];									move16();
		j2 = ch_tbl[i][1];									move16();

		for (j = j1; j <= j2; j++)
		{
			Lenrg = L_mac(Lenrg, data_buffer[2 * j], data_buffer[2 * j]);
			Lenrg = L_mac(Lenrg, data_buffer[2 * j + 1], data_buffer[2 * j + 1]);
		}

		/* Denorm energy & scale 30,1 according to the state */
		Lenrg = L_shr_r(Lenrg, sub(shl(normb_shift, 1), enrg_norm_shift[st->shift_state]));

		/* integrate over time: e[i] = (1-alpha)*e[i] + alpha*enrg/num_bins_in_chan */
		tmp = mult(alpha, ch_tbl_sh[i]);
		L_Extract (Lenrg, &hi1, &lo1);
		Ltmp = Mpy_32_16(hi1, lo1, tmp);

		L_Extract (st->Lch_enrg[i], &hi1, &lo1);
		st->Lch_enrg[i] = L_add(Ltmp, Mpy_32_16(hi1, lo1, one_m_alpha));			move32();
													test();
		if (L_sub(st->Lch_enrg[i], min_chan_enrg[st->shift_state]) < 0)
		{
			st->Lch_enrg[i] = min_chan_enrg[st->shift_state];				move32();
		}

	}


	/* Compute the total channel energy estimate (Ltce) */
	Ltce = 0;											move16();
	for (i = LO_CHAN; i <= HI_CHAN; i++)
	{
		Ltce = L_add(Ltce, st->Lch_enrg[i]);
	}


	/* Calculate spectral peak-to-average ratio, set flag if p2a > 10 dB */
	Lpeak = 0;											move32();
	for (i = LO_CHAN+2; i <= HI_CHAN; i++)	/* Sine waves not valid for low frequencies */
	{												test();
		if (L_sub(st->Lch_enrg [i], Lpeak) > 0)
		{
			Lpeak = st->Lch_enrg [i];							move32();
		}
	}

	/* Set p2a_flag if peak (dB) > average channel energy (dB) + 10 dB */
	/*   Lpeak > Ltce/num_channels * 10^(10/10)                        */
	/*   Lpeak > (10/16)*Ltce                                          */

	L_Extract (Ltce, &hi1, &lo1);
	Ltmp = Mpy_32_16(hi1, lo1, 20480);
													test();
	if (L_sub(Lpeak, Ltmp) > 0)
	{
		p2a_flag = TRUE;									move16();
	}
	else
	{
		p2a_flag = FALSE;									move16();
	}


	/* Initialize channel noise estimate to either the channel energy or fixed level  */
	/*   Scale the energy appropriately to yield state 0 (22,9) scaling for noise */
													test();
	if (L_sub(st->Lframe_cnt, 4) <= 0)
	{												test();
		if (p2a_flag == TRUE)
		{
			for (i = LO_CHAN; i <= HI_CHAN; i++)
			{
				st->Lch_noise[i] = INE_NOISE_0;						move32();
			}
		}
		else
		{
			for (i = LO_CHAN; i <= HI_CHAN; i++)
			{										test();
				if (L_sub(st->Lch_enrg[i], ine_noise[st->shift_state]) < 0)
				{
					st->Lch_noise[i] = INE_NOISE_0;					move32();
				}
				else
				{									test();
					if (st->shift_state == 1)
					{
						st->Lch_noise[i] = L_shr(st->Lch_enrg[i], state_change_shift_r[0]);
													move32();
					}
					else
					{
						st->Lch_noise[i] = st->Lch_enrg[i];			move32();
					}
				}
			}
		}
	}


	/* Compute the channel energy (in dB), the channel SNRs, and the sum of voice metrics */
	vm_sum = 0;											move16();
	for (i = LO_CHAN; i <= HI_CHAN; i++)
	{
		ch_enrg_db[i] = fn10Log10(st->Lch_enrg[i], fbits[st->shift_state]);			move16();
		ch_noise_db = fn10Log10(st->Lch_noise[i], FRACTIONAL_BITS_0);

		ch_snr[i] = sub(ch_enrg_db[i], ch_noise_db);						move16();

		/* quantize channel SNR in 3/8 dB steps (scaled 7,8 => 15,0) */
		/*   ch_snr = round((snr/(3/8))>>8)                          */
		/*          = round(((0.6667*snr)<<2)>>8)                    */
		/*          = round((0.6667*snr)>>6)                         */

		ch_snrq = shr_r(mult(21845, ch_snr[i]), 6);

		/* Accumulate the sum of voice metrics	*/						test();
		if (sub(ch_snrq, 89) < 0)
		{											test();
			if (ch_snrq > 0)
			{
				j = ch_snrq;								move16();
			}
			else
			{
				j = 0;									move16();
			}
		}
		else
		{
			j = 89;										move16();
		}
		vm_sum = add(vm_sum, vm_tbl[j]);
	}


	/* Initialize NOMINAL peak voice energy and average noise energy, calculate instantaneous SNR */ 
												test(),test(),logic16();
	if (L_sub(st->Lframe_cnt, 4) <= 0 || st->fupdate_flag == TRUE)
	{
		/* tce_db = (96 - 22 - 10*log10(64) (due to FFT)) scaled as 7,8 */
		tce_db = 14320;										move16();
		st->negSNRvar = 0;									move16();
		st->negSNRbias = 0;									move16();

		/* Compute the total noise estimate (Ltne) */
		Ltne = 0;										move32();
		for (i = LO_CHAN; i <= HI_CHAN; i++)
		{
			Ltne = L_add(Ltne, st->Lch_noise[i]);
		}

		/* Get total noise in dB */
		tne_db = fn10Log10(Ltne, FRACTIONAL_BITS_0);

		/* Initialise instantaneous and long-term peak signal-to-noise ratios */
		xt = sub(tce_db, tne_db);
		st->tsnr = xt;										move16();
	}
	else
	{
		/* Calculate instantaneous frame signal-to-noise ratio */
		/* xt = 10*log10( sum(2.^(ch_snr*0.1*log2(10)))/length(ch_snr) ) */
		Ltmp1 = 0;										move32();
		for (i=LO_CHAN; i<=HI_CHAN; i++) {
			/* Ltmp2 = ch_snr[i] * 0.1 * log2(10); (ch_snr scaled as 7,8) */
			Ltmp2 = L_shr(L_mult(ch_snr[i], 10885), 8);
			L_Extract(Ltmp2, &hi1, &lo1);
			hi1 = add(hi1, 3);			/* 2^3 to compensate for negative SNR */
			Ltmp1 = L_add(Ltmp1, Pow2(hi1, lo1));
		}
		xt = fn10Log10(Ltmp1, 4+3);			/* average by 16, inverse compensation 2^3 */

		/* Estimate long-term "peak" SNR */							test(),test();
		if (sub(xt, st->tsnr) > 0)
		{
			/* tsnr = 0.9*tsnr + 0.1*xt; */
			st->tsnr = round(L_add(L_mult(29491, st->tsnr), L_mult(3277, xt)));
		}
		/* else if (xt > 0.625*tsnr) */	
		else if (sub(xt, mult(20480, st->tsnr)) > 0)
		{
			/* tsnr = 0.998*tsnr + 0.002*xt; */
			st->tsnr = round(L_add(L_mult(32702, st->tsnr), L_mult(66, xt)));
		}
	}

	/* Quantize the long-term SNR in 3 dB steps, limit to 0 <= tsnrq <= 19 */
	tsnrq = shr(mult(st->tsnr, 10923), 8);

	/* tsnrq = min(19, max(0, tsnrq)); */								test(),test();
	if (sub(tsnrq, 19) > 0)
	{
		tsnrq = 19;										move16();
	}
	else if (tsnrq < 0)
	{
		tsnrq = 0;										move16();
	}

	/* Calculate the negative SNR sensitivity bias */
													test();
	if (xt < 0)
	{
		/* negSNRvar = 0.99*negSNRvar + 0.01*xt*xt; */
		/*   xt scaled as 7,8 => xt*xt scaled as 14,17, shift to 7,8 and round */
		tmp = round(L_shl(L_mult(xt, xt), 7));
		st->negSNRvar = round(L_add(L_mult(32440, st->negSNRvar), L_mult(328, tmp)));

		/* if (negSNRvar > 4.0) negSNRvar = 4.0;  */						test();
		if (sub(st->negSNRvar, 1024) > 0)
		{
			st->negSNRvar = 1024;								move16();
		}

		/* negSNRbias = max(12.0*(negSNRvar - 0.65), 0.0); */
		tmp = mult_r(shl(sub(st->negSNRvar, 166), 4), 24576);					test();

		if (tmp < 0)
		{
			st->negSNRbias = 0;								move16();
		}
		else
		{
			st->negSNRbias = shr(tmp, 8);
		}
	}


	/* Determine VAD as a function of the voice metric sum and quantized SNR */

	tmp = add(vm_threshold_table[tsnrq], st->negSNRbias);						test();
	if (sub(vm_sum, tmp) > 0)
	{
		ivad = 1;										move16();
		st->burstcount = add(st->burstcount, 1);						test();
		if (sub(st->burstcount, burstcount_table[tsnrq]) > 0)
		{
			st->hangover = hangover_table[tsnrq];						move16();
		}
	}
	else
	{
		st->burstcount = 0;									move16();
		st->hangover = sub(st->hangover, 1);							test();
		if (st->hangover <= 0)
		{
			ivad = 0;									move16();
			st->hangover = 0;								move16();
		}
		else
		{
			ivad = 1;									move16();
		}
	}


	/* Calculate log spectral deviation */
	ch_enrg_dev = 0;										move16();
													test();
	if (L_sub(st->Lframe_cnt, 1) == 0)
	{
		for (i = LO_CHAN; i <= HI_CHAN; i++)
		{
			st->ch_enrg_long_db[i] = ch_enrg_db[i];						move16();
		}
	}
	else
	{
		for (i = LO_CHAN; i <= HI_CHAN; i++)
		{
			tmp = abs_s(sub(st->ch_enrg_long_db[i], ch_enrg_db[i]));
			ch_enrg_dev = add(ch_enrg_dev, tmp);
		}
	}

	/*
	 * Calculate long term integration constant as a function of instantaneous SNR
	 * (i.e., high SNR (tsnr dB) -> slower integration (alpha = HIGH_ALPHA),
	 *         low SNR (0 dB) -> faster integration (alpha = LOW_ALPHA)
	 */

	/* alpha = HIGH_ALPHA - ALPHA_RANGE * (tsnr - xt) / tsnr, low <= alpha <= high */
	tmp = sub(st->tsnr, xt);						test(),logic16(),test(),test();
	if (tmp <= 0 || st->tsnr <= 0)
	{
		alpha = HIGH_ALPHA;								move16();
		one_m_alpha = 32768L-HIGH_ALPHA;						move16();
	}
	else if (sub(tmp, st->tsnr) > 0)
	{
		alpha = LOW_ALPHA;								move16();
		one_m_alpha = 32768L-LOW_ALPHA;							move16();
	}
	else
	{
		tmp = div_s(tmp, st->tsnr);
		alpha = sub(HIGH_ALPHA, mult(ALPHA_RANGE, tmp));
		one_m_alpha = sub(32767, alpha);
	}

	/* Calc long term log spectral energy */
	for (i = LO_CHAN; i <= HI_CHAN; i++)
	{
		Ltmp1 = L_mult(one_m_alpha, ch_enrg_db[i]);
		Ltmp2 = L_mult(alpha, st->ch_enrg_long_db[i]);
		st->ch_enrg_long_db[i] = round(L_add(Ltmp1, Ltmp2));
	}


	/* Set or clear the noise update flags */
	update_flag = FALSE;										move16();
	st->fupdate_flag = FALSE;									move16();
													test(),test();
	if (sub(vm_sum, UPDATE_THLD) <= 0)
	{												test();
		if (st->burstcount == 0)
		{
			update_flag = TRUE;								move16();
			st->update_cnt = 0;								move16();
		}
	}
	else if (L_sub(Ltce, noise_floor_chan[st->shift_state]) > 0)
	{												test();
		if (sub(ch_enrg_dev, DEV_THLD) < 0)
		{											test();
			if (p2a_flag == FALSE)
			{										test();
				if (st->LTP_flag == FALSE)
				{
					st->update_cnt = add(st->update_cnt, 1);			test();
					if (sub(st->update_cnt, UPDATE_CNT_THLD) >= 0)
					{
						update_flag = TRUE;					move16();
						st->fupdate_flag = TRUE;				move16();
					}
				}
			}
		}
	}
													test();
	if (sub(st->update_cnt, st->last_update_cnt) == 0)
	{
		st->hyster_cnt = add(st->hyster_cnt, 1);
	}
	else
	{
		st->hyster_cnt = 0;									move16();
	}

	st->last_update_cnt = st->update_cnt;								move16();
													test();
	if (sub(st->hyster_cnt, HYSTER_CNT_THLD) > 0)
	{
		st->update_cnt = 0;									move16();
	}


	/* Conditionally update the channel noise estimates */
													test();
	if (update_flag == TRUE)
	{
		/* Check shift state */									test();
		if (st->shift_state == 1)
		{
			/* get factor to shift ch_enrg[] from state 1 to 0 (noise always state 0) */
			tmp = state_change_shift_r[0];							move16();
		}
		else
		{
			/* No shift if already state 0 */
			tmp = 0;									move16();
		}

		/* Update noise energy estimate */
		for (i = LO_CHAN; i <= HI_CHAN; i++)
		{											test();
			/* integrate over time: en[i] = (1-alpha)*en[i] + alpha*e[n] */
			/* (extract with shift compensation for state 1) */
			L_Extract (L_shr(st->Lch_enrg[i], tmp), &hi1, &lo1);
			Ltmp = Mpy_32_16(hi1, lo1, CNE_SM_FAC);

			L_Extract (st->Lch_noise[i], &hi1, &lo1);
			st->Lch_noise[i] = L_add(Ltmp, Mpy_32_16(hi1, lo1, ONE_MINUS_CNE_SM_FAC));	move32();

			/* Limit low level noise */							test();
			if (L_sub(st->Lch_noise[i], MIN_NOISE_ENRG_0) < 0)
			{
				st->Lch_noise[i] = MIN_NOISE_ENRG_0;					move32();
			}
		}
	}

	return(ivad);
}								/* end of vad2 () */
Exemple #17
0
/*-----------------------------------------------------------*
 * procedure Cod_cng:                                        *
 *           ~~~~~~~~                                        *
 *   computes DTX decision                                   *
 *   encodes SID frames                                      *
 *   computes CNG excitation for encoder update              *
 *-----------------------------------------------------------*/
void Cod_cng(
  DtxStatus *handle,
  Word16 *exc,          /* (i/o) : excitation array                     */
  Word16 pastVad,       /* (i)   : previous VAD decision                */
  Word16 *lsp_old_q,    /* (i/o) : previous quantized lsp               */
  Word16 *Aq,           /* (o)   : set of interpolated LPC coefficients */
  Word16 *ana,          /* (o)   : coded SID parameters                 */
  Word16 freq_prev[MA_NP][M],
                        /* (i/o) : previous LPS for quantization        */
  Word16 *seed          /* (i/o) : random generator seed                */
)
{

  Word16 i;

  Word16 curAcf[MP1];
  Word16 bid[M], zero[MP1];
  Word16 curCoeff[MP1];
  Word16 lsp_new[M];
  Word16 *lpcCoeff;
  Word16 cur_igain;
  Word16 energyq, temp;

  /* Update Ener and sh_ener */
  for(i = NB_GAIN-1; i>=1; i--) {
    handle->ener[i] = handle->ener[i-1];
    handle->sh_ener[i] = handle->sh_ener[i-1];
  }

  /* Compute current Acfs */
  Calc_sum_acf(handle->Acf, handle->sh_Acf, curAcf, &(handle->sh_ener[0]), NB_CURACF);

  /* Compute LPC coefficients and residual energy */
  if(curAcf[0] == 0) {
    handle->ener[0] = 0;                /* should not happen */
  }
  else {
    Set_zero(zero, MP1);
    Levinson(handle, curAcf, zero, curCoeff, bid, &(handle->ener[0]));
  }

  /* if first frame of silence => SID frame */
  if(pastVad != 0) {
    ana[0] = 2;
    handle->count_fr0 = 0;
    handle->nb_ener = 1;
    Qua_Sidgain(handle->ener, handle->sh_ener, handle->nb_ener, &energyq, &cur_igain);

  }
  else {
    handle->nb_ener = add(handle->nb_ener, 1);
    if(sub(handle->nb_ener, NB_GAIN) > 0) handle->nb_ener = NB_GAIN;
    Qua_Sidgain(handle->ener, handle->sh_ener, handle->nb_ener, &energyq, &cur_igain);
      
    /* Compute stationarity of current filter   */
    /* versus reference filter                  */
    if(Cmp_filt(handle->RCoeff, handle->sh_RCoeff, curAcf, handle->ener[0], FRAC_THRESH1) != 0) {
      handle->flag_chang = 1;
    }
      
    /* compare energy difference between current frame and last frame */
    temp = abs_s(sub(handle->prev_energy, energyq));
    temp = sub(temp, 2);
    if (temp > 0) handle->flag_chang = 1;
      
    handle->count_fr0 = add(handle->count_fr0, 1);
    if(sub(handle->count_fr0, FR_SID_MIN) < 0) {
      ana[0] = 0;               /* no transmission */
    }
    else {
      if(handle->flag_chang != 0) {
        ana[0] = 2;             /* transmit SID frame */
      }
      else{
        ana[0] = 0;
      }
        
      handle->count_fr0 = FR_SID_MIN;   /* to avoid overflow */
    }
  }


  if(sub(ana[0], 2) == 0) {
      
    /* Reset frame count and change flag */
    handle->count_fr0 = 0;
    handle->flag_chang = 0;
      
    /* Compute past average filter */
    Calc_pastfilt(handle, handle->pastCoeff);
    Calc_RCoeff(handle->pastCoeff, handle->RCoeff, &(handle->sh_RCoeff));

    /* Compute stationarity of current filter   */
    /* versus past average filter               */


    /* if stationary */
    /* transmit average filter => new ref. filter */
    if(Cmp_filt(handle->RCoeff, handle->sh_RCoeff, curAcf, handle->ener[0], FRAC_THRESH2) == 0) {
      lpcCoeff = handle->pastCoeff;
    }

    /* else */
    /* transmit current filter => new ref. filter */
    else {
      lpcCoeff = curCoeff;
      Calc_RCoeff(curCoeff, handle->RCoeff, &(handle->sh_RCoeff));
    }

    /* Compute SID frame codes */

    Az_lsp(lpcCoeff, lsp_new, lsp_old_q); /* From A(z) to lsp */

    /* LSP quantization */
    lsfq_noise(lsp_new, handle->lspSid_q, freq_prev, &ana[1]);

    handle->prev_energy = energyq;
    ana[4] = cur_igain;
    handle->sid_gain = tab_Sidgain[cur_igain];


  } /* end of SID frame case */

  /* Compute new excitation */
  if(pastVad != 0) {
    handle->cur_gain = handle->sid_gain;
  }
  else {
    handle->cur_gain = mult_r(handle->cur_gain, A_GAIN0);
    handle->cur_gain = add(handle->cur_gain, mult_r(handle->sid_gain, A_GAIN1));
  }

  Calc_exc_rand(handle->L_exc_err, handle->cur_gain, exc, seed, FLAG_COD);

  Int_qlpc(lsp_old_q, handle->lspSid_q, Aq);
  for(i=0; i<M; i++) {
    lsp_old_q[i]   = handle->lspSid_q[i];
  }

  /* Update sumAcf if fr_cur = 0 */
  if(handle->fr_cur == 0) {
    Update_sumAcf(handle);
  }

  return;
}
Exemple #18
0
/*----------------------------------------------------------------------------
 *   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;
}
Exemple #19
0
/*
**
** Function:            AtoLsp()
**
** Description:     Transforms 10 LPC coefficients to the 10
**          corresponding LSP frequencies for a subframe.
**          This transformation is done once per frame,
**          for subframe 3 only.  The transform algorithm
**          generates sum and difference polynomials from
**          the LPC coefficients.  It then evaluates the
**          sum and difference polynomials at uniform
**          intervals of pi/256 along the unit circle.
**          Intervals where a sign change occurs are
**          interpolated to find the zeros of the
**          polynomials, which are the LSP frequencies.
**
** Links to text:   Section 2.5
**
** Arguments:       
**
**  Word16 *LspVect     Empty Buffer
**  Word16 Lpc[]        Unquantized LPC coefficients (10 words)
**  Word16 PrevLsp[]    LSP frequencies from the previous frame (10 words)
**
** Outputs:
**
**  Word16 LspVect[]    LSP frequencies for the current frame (10 words)
**
** Return value:        None
**
**/
void AtoLsp( Word16 *LspVect, Word16 *Lpc, Word16 *PrevLsp )
{

    int   i,j,k ;

    Word32   Lpq[LpcOrder+2] ;
    Word16   Spq[LpcOrder+2] ;

    Word16   Exp   ;
    Word16   LspCnt ;

    Word32   PrevVal,CurrVal   ;
    Word32   Acc0,Acc1   ;


 /*
  * Perform a bandwidth expansion on the LPC coefficients.  This
  * scales the poles of the LPC synthesis filter by a factor of
  * 0.994.
  */
    for ( i = 0 ; i < LpcOrder ; i ++ )
        LspVect[i] = mult_r( Lpc[i], BandExpTable[i] ) ;


 /*
  * Compute the sum and difference polynomials with the roots at z =
  * -1 (sum) or z = +1 (difference) removed.  Let these polynomials
  * be P(z) and Q(z) respectively, and let their coefficients be
  * {p_i} amd {q_i}.  The coefficients are stored in the array Lpq[]
  * as follows: p_0, q_0, p_1, q_1, ..., p_5, q_5.  There is no need
  * to store the other coefficients because of symmetry.
  */


 /*
  * Set p_0 = q_0 = 1.  The LPC coefficients are already scaled by
  *  1/4.  P(z) and Q(z) are scaled by an additional scaling factor of
  *  1/16, for an overall factor of 1/64 = 0x02000000L.
  */

    Lpq[0] = Lpq[1] = (Word32) 0x02000000L ;

 /*
  * This loop computes the coefficients of P(z) and Q(z).  The long
  * division (to remove the real zeros) is done recursively.
  */
    for ( i = 0 ; i < LpcOrder/2 ; i ++ ) {

        /* P(z) */
        Acc0 = L_negate( Lpq[2*i+0] ) ;
        Acc1 = L_deposit_h( LspVect[i] ) ;
        Acc1 = L_shr( Acc1, (Word16) 4 ) ;
        Acc0 = L_sub( Acc0, Acc1 ) ;
        Acc1 = L_deposit_h( LspVect[LpcOrder-1-i] ) ;
        Acc1 = L_shr( Acc1, (Word16) 4 ) ;
        Acc0 = L_sub( Acc0, Acc1 ) ;
        Lpq[2*i+2] = Acc0 ;

        /* Q(z) */
        Acc0 = Lpq[2*i+1] ;
        Acc1 = L_deposit_h( LspVect[i] ) ;
        Acc1 = L_shr( Acc1, (Word16) 4 ) ;

        Acc0 = L_sub( Acc0, Acc1 ) ;
        Acc1 = L_deposit_h( LspVect[LpcOrder-1-i] ) ;
        Acc1 = L_shr( Acc1, (Word16) 4 ) ;
 //       Acc0 = L_add( Acc0, Acc1 ) ;
		L_ADD(Acc0, Acc1, Acc0);
        Lpq[2*i+3] = Acc0 ;
    }

 /*
  * Divide p_5 and q_5 by 2 for proper weighting during polynomial
  * evaluation.
  */
    Lpq[LpcOrder+0] = L_shr( Lpq[LpcOrder+0], (Word16) 1 ) ;
    Lpq[LpcOrder+1] = L_shr( Lpq[LpcOrder+1], (Word16) 1 ) ;

 /*
  * Normalize the polynomial coefficients and convert to shorts
  */

    /* Find the maximum */
    Acc1 = L_abs( Lpq[0] ) ;
    for ( i = 1 ; i < LpcOrder+2 ; i ++ ) {
        Acc0 = L_abs( Lpq[i] ) ;
        if ( Acc0 > Acc1 )
            Acc1 = Acc0 ;
    }

    /* Compute the normalization factor */
    Exp = norm_l( Acc1 ) ;


    /* Normalize and convert to shorts */
    for ( i = 0 ; i < LpcOrder+2 ; i ++ ) {
        Acc0 = L_shl( Lpq[i], Exp ) ;
        Spq[i] = round( Acc0 ) ;
    }

 /*
  * Initialize the search loop
  */

 /*
  * The variable k is a flag that indicates which polynomial (sum or
  * difference) the algorithm is currently evaluating.  Start with
  * the sum.
  */
    k = 0 ;

    /* Evaluate the sum polynomial at frequency zero */
    PrevVal = (Word32) 0 ;
    for ( j = 0 ; j <= LpcOrder/2 ; j ++ )
        PrevVal = L_mac( PrevVal, Spq[2*j], CosineTable[0] ) ;


 /*
  * Search loop.  Evaluate P(z) and Q(z) at uniform intervals of
  * pi/256 along the unit circle.  Check for zero crossings.  The
  * zeros of P(w) and Q(w) alternate, so only one of them need by
  * evaluated at any given step.
  */
    LspCnt = (Word16) 0 ;
    for ( i = 1 ; i < CosineTableSize/2 ; i ++ ) {

        /* Evaluate the selected polynomial */
        CurrVal = (Word32) 0 ;
        for ( j = 0 ; j <= LpcOrder/2 ; j ++ )
            CurrVal = L_mac( CurrVal, Spq[LpcOrder-2*j+k],
                                    CosineTable[i*j%CosineTableSize] ) ;

        /* Check for a sign change, indicating a zero crossing */
        if ( (CurrVal ^ PrevVal) < (Word32) 0 ) {

 /*
  * Interpolate to find the bottom 7 bits of the
  * zero-crossing frequency
  */
            Acc0 = L_abs( CurrVal ) ;
            Acc1 = L_abs( PrevVal ) ;
 //           Acc0 = L_add( Acc0, Acc1 ) ;
			L_ADD(Acc0, Acc1, Acc0);

            /* Normalize the sum */
            Exp = norm_l( Acc0 ) ;
            Acc0 = L_shl( Acc0, Exp ) ;
            Acc1 = L_shl( Acc1, Exp ) ;

            Acc1 = L_shr( Acc1, (Word16) 8 ) ;

            LspVect[LspCnt] = div_l( Acc1, extract_h( Acc0 ) ) ;

 /*
  * Add the upper part of the zero-crossing frequency,
  * i.e. bits 7-15
  */
            Exp = shl( (Word16) (i-1), (Word16) 7 ) ;
            LspVect[LspCnt] = add( LspVect[LspCnt], Exp ) ;
            LspCnt ++ ;

            /* Check if all zeros have been found */
            if ( LspCnt == (Word16) LpcOrder )
                break ;

 /*
  * Switch the pointer between sum and difference polynomials
  */
            k ^= 1 ;

 /*
  * Evaluate the new polynomial at the current frequency
  */
            CurrVal = (Word32) 0 ;
            for ( j = 0 ; j <= LpcOrder/2 ; j ++ )
                CurrVal = L_mac( CurrVal, Spq[LpcOrder-2*j+k],
                                    CosineTable[i*j%CosineTableSize] ) ;
        }

        /* Update the previous value */
        PrevVal = CurrVal ;
    }


 /*
  * Check if all 10 zeros were found.  If not, ignore the results of
  * the search and use the previous frame's LSP frequencies instead.
  */
    if ( LspCnt != (Word16) LpcOrder ) {
        for ( j = 0 ; j < LpcOrder ; j ++ )
            LspVect[j] = PrevLsp[j] ;
    }

    return ;
}
Exemple #20
0
/*
**
** Function:            Lsp_Inq()
**
** Description:     Performs inverse vector quantization of the
**          LSP frequencies.  The LSP vector is divided
**          into 3 sub-vectors, or bands, of dimension 3,
**          3, and 4.  Each band is inverse quantized
**          separately using a different VQ table.  Each
**          table has 256 entries, so each VQ index is 8
**          bits.  (Only the LSP vector for subframe 3 is
**          quantized per frame.)
**
** Links to text:   Sections 2.6, 3.2
**
** Arguments:
**
**  Word16 *Lsp     Empty buffer
**  Word16 PrevLsp[]    Quantized LSP frequencies from the previous frame
**               (10 words)
**  Word32 LspId        Long word packed with the 3 VQ indices.  Band 0
**               corresponds to bits [23:16], band 1 corresponds
**               to bits [15:8], and band 2 corresponds to bits
**               [7:0].
**  Word16 Crc      Frame erasure indicator
**
** Outputs:
**
**  Word16 Lsp[]        Quantized LSP frequencies for current frame (10
**               words)
**
** Return value:         None
**
*/
void Lsp_Inq( Word16 *Lsp, Word16 *PrevLsp, Word32 LspId, Word16 Crc )
{
    int  i,j   ;

    Word16  *LspQntPnt  ;


    Word16   Scon  ;
    Word16   Lprd  ;

    Word16   Tmp   ;
    Flag     Test  ;


 /*
  * Check for frame erasure.  If a frame erasure has occurred, the
  * resulting VQ table entries are zero.  In addition, a different
  * fixed predictor and minimum frequency separation are used.
  */
    if ( Crc == (Word16) 0 ) {
        Scon = (Word16) 0x0100 ;
        Lprd = LspPrd0 ;
    }
    else {
        LspId = (Word32) 0 ;
        Scon = (Word16) 0x0200 ;
        Lprd = LspPrd1 ;
    }


 /*
  * Inverse quantize the 10th-order LSP vector.  Each band is done
  * separately.
  */
    for ( i = LspQntBands-1; i >= 0 ; i -- ) {

 /*
  * Get the VQ table entry corresponding to the transmitted index
  */
        Tmp = (Word16) ( LspId & (Word32) 0x000000ff ) ;
        LspId >>= 8 ;

        LspQntPnt = BandQntTable[i] ;

        for ( j = 0 ; j < BandInfoTable[i][1] ; j ++ )
            Lsp[BandInfoTable[i][0] + j] =
                                LspQntPnt[Tmp*BandInfoTable[i][1] + j] ;
    }

 /*
  * Subtract the DC component from the previous frame's quantized
  * vector
  */
    for ( j = 0 ; j < LpcOrder ; j ++ )
        PrevLsp[j] = sub(PrevLsp[j], LspDcTable[j] ) ;

 /*
  * Generate the prediction vector using a fixed first-order
  * predictor based on the previous frame's (DC-free) quantized
  * vector
  */
    for ( j = 0 ; j < LpcOrder ; j ++ ) {
        Tmp = mult_r( PrevLsp[j], Lprd ) ;
        Lsp[j] = add( Lsp[j], Tmp ) ;
    }

 /*
  * Add the DC component back to the previous quantized vector,
  * which is needed in later routines
  */
    for ( j = 0 ; j < LpcOrder ; j ++ ) {
        PrevLsp[j] = add( PrevLsp[j], LspDcTable[j] ) ;
        Lsp[j] = add( Lsp[j], LspDcTable[j] ) ;
    }


 /*
  * Perform a stability test on the quantized LSP frequencies.  This
  * test checks that the frequencies are ordered, with a minimum
  * separation between each.  If the test fails, the frequencies are
  * iteratively modified until the test passes.  If after 10
  * iterations the test has not passed, the previous frame's
  * quantized LSP vector is used.
  */
    for ( i = 0 ; i < LpcOrder ; i ++ ) {

        /* Check the first frequency */
        if ( Lsp[0] < (Word16) 0x180 )
            Lsp[0] = (Word16) 0x180 ;

        /* Check the last frequency */
        if ( Lsp[LpcOrder-1] > (Word16) 0x7e00 )
            Lsp[LpcOrder-1] = (Word16) 0x7e00 ;

        /* Perform the modification */
        for ( j = 1 ; j < LpcOrder ; j ++ ) {

            Tmp = add( Scon, Lsp[j-1] ) ;
            Tmp = sub( Tmp, Lsp[j] ) ;
            if ( Tmp > (Word16) 0 ) {
                Tmp = shr( Tmp, (Word16) 1 ) ;
                Lsp[j-1] = sub( Lsp[j-1], Tmp ) ;
                Lsp[j] = add( Lsp[j], Tmp ) ;
            }
        }

        Test = False ;

 /*
  * Test the modified frequencies for stability.  Break out of
  * the loop if the frequencies are stable.
  */
        for ( j = 1 ; j < LpcOrder ; j ++ ) {
            Tmp = add( Lsp[j-1], Scon ) ;
            Tmp = sub( Tmp, (Word16) 4 ) ;
            Tmp = sub( Tmp, Lsp[j] ) ;
            if ( Tmp > (Word16) 0 )
                Test = True ;
        }

        if ( Test == False )
            break ;
    }


 /*
  * Return the result of the stability check.  True = not stable,
  * False = stable.
  */
    if ( Test == True) {
        for ( j = 0 ; j < LpcOrder ; j ++ )
            Lsp[j] = PrevLsp[j] ;
    }

    return;
}
Exemple #21
0
void BV16_Decode(
                 struct BV16_Bit_Stream     *bs,
                 struct BV16_Decoder_State  *ds,
                 Word16    *x)
{
   Word32 lgq, lg_el;
   Word16 gainq;         /* Q3 */
   Word16 pp;
   Word32 a0;
   Word16 gain_exp;
   Word16 i;
   Word16 a0hi, a0lo;
   Word16 ltsym[LTMOFF+FRSZ];
   Word16 xq[LXQ];
   Word16 a[LPCO+1];
   Word16 lspq[LPCO];       /* Q15 */
   Word16 cbs[VDIM*CBSZ];
   Word16 bq[3];         /* Q15 */
   Word32 bss;
   Word32  E;
   
   /* set frame erasure flags */
   if (ds->cfecount != 0) {
      ds->ngfae = 1;
   } else {
      ds->ngfae++;
      if (ds->ngfae>LGPORDER) ds->ngfae=LGPORDER+1;
   }
   
   /* reset frame erasure counter */
   ds->cfecount = 0;
   
   /* decode pitch period */
   pp = (bs->ppidx + MINPP);
   
   /* decode spectral information */
   lspdec(lspq,bs->lspidx,ds->lsppm,ds->lsplast);
   lsp2a(lspq,a);
   W16copy(ds->lsplast, lspq, LPCO);
   
   /* decode pitch taps */
   pp3dec(bs->bqidx, bq);
   
   /* decode gain */
   a0 = gaindec(&lgq,bs->gidx,ds->lgpm,ds->prevlg,ds->level,
      &ds->nggalgc,&lg_el);
   
   /* gain normalization */
   gain_exp = sub(norm_l(a0), 2);
   /* scale down quantized gain by 1.5, 1/1.5=2/3 (21845 Q15) */
   L_Extract(a0, &a0hi, &a0lo);
   a0 = Mpy_32_16(a0hi, a0lo, 21845);  
   gainq = intround(L_shl(a0, gain_exp));
   
   
   /* scale the scalar quantizer codebook to current signal level */
   for (i=0;i<(VDIM*CBSZ);i++) cbs[i] = mult_r(gainq, cccb[i]);
   
   /* copy state memory to buffer */
   W16copy(xq, ds->xq, XQOFF);
   W16copy(ltsym, ds->ltsym, LTMOFF);
   
   /* decoding of the excitation signal with integrated long-term */
   /* and short-term synthesis */
   excdec_w_synth(xq+XQOFF,ltsym+LTMOFF,ds->stsym,bs->qvidx,bq,cbs,pp,
      a,gain_exp,&E);
   
   ds->E = E;
   
   /* update the remaining state memory */
   W16copy(ds->ltsym, ltsym+FRSZ, LTMOFF);
   W16copy(ds->xq, xq+FRSZ, XQOFF);
   ds->pp_last = pp;
   W16copy(ds->bq_last, bq, 3);
   
   /* level estimation */
   estlevel(lg_el,&ds->level,&ds->lmax,&ds->lmin,&ds->lmean,&ds->x1,
      ds->ngfae, ds->nggalgc,&ds->estl_alpha_min);

   /* adaptive postfiltering */
   postfilter(xq, pp, &(ds->ma_a), ds->b_prv, &(ds->pp_prv), x);

   /* scale signal up by 1.5 */
   for(i=0; i<FRSZ; i++)
      x[i] = add(x[i], shr(x[i],1));
   
   W16copy(ds->atplc, a, LPCO+1);
   bss = L_add(L_add(bq[0], bq[1]), bq[2]);
   if (bss > 32768)
      bss = 32768;
   else if (bss < 0)
      bss = 0;
   ds->per = add(shr(ds->per, 1), (Word16)L_shr(bss, 1));
   
}
Exemple #22
0
/*-----------------------------------------------------------*
 * procedure Calc_exc_rand                                   *
 *           ~~~~~~~~~~~~~                                   *
 *   Computes comfort noise excitation                       *
 *   for SID and not-transmitted frames                      *
 *-----------------------------------------------------------*/
void WebRtcG729fix_Calc_exc_rand(
  int32_t L_exc_err[],
  int16_t cur_gain,      /* (i)   :   target sample gain                 */
  int16_t *exc,          /* (i/o) :   excitation array                   */
  int16_t *seed,         /* (i)   :   current Vad decision               */
  int flag_cod           /* (i)   :   encoder/decoder flag               */
)
{
  int16_t i, j, i_subfr;
  int16_t temp1, temp2;
  int16_t pos[4];
  int16_t sign[4];
  int16_t t0, frac;
  int16_t *cur_exc;
  int16_t g, Gp, Gp2;
  int16_t excg[L_SUBFR], excs[L_SUBFR];
  int32_t L_acc, L_ener, L_k;
  int16_t max, hi, lo, inter_exc;
  int16_t sh;
  int16_t x1, x2;
  
  if (cur_gain == 0) {
    WebRtcSpl_ZerosArrayW16(exc, L_FRAME);
    Gp = 0;
    t0 = WebRtcSpl_AddSatW16(L_SUBFR,1);
    for (i_subfr = 0;  i_subfr < L_FRAME; i_subfr += L_SUBFR) {
      if (flag_cod != FLAG_DEC)
        WebRtcG729fix_update_exc_err(L_exc_err, Gp, t0);
    }
    return;
  }
  
  /* Loop on subframes */
  
  cur_exc = exc;
  
  for (i_subfr = 0;  i_subfr < L_FRAME; i_subfr += L_SUBFR) {

    /* generate random adaptive codebook & fixed codebook parameters */
    /*****************************************************************/
    temp1 = WebRtcG729fix_Random(seed);
    frac = WebRtcSpl_SubSatW16((temp1 & (int16_t)0x0003), 1);
    if(frac == 2) frac = 0;
    temp1 = shr(temp1, 2);
    t0 = WebRtcSpl_AddSatW16((temp1 & (int16_t)0x003F), 40);
    temp1 = shr(temp1, 6);
    temp2 = temp1 & (int16_t)0x0007;
    pos[0] = WebRtcSpl_AddSatW16(shl(temp2, 2), temp2); /* 5 * temp2 */
    temp1 = shr(temp1, 3);
    sign[0] = temp1 & (int16_t)0x0001;
    temp1 = shr(temp1, 1);
    temp2 = temp1 & (int16_t)0x0007;
    temp2 = WebRtcSpl_AddSatW16(shl(temp2, 2), temp2);
    pos[1] = WebRtcSpl_AddSatW16(temp2, 1);     /* 5 * x + 1 */
    temp1 = shr(temp1, 3);
    sign[1] = temp1 & (int16_t)0x0001;
    temp1 = WebRtcG729fix_Random(seed);
    temp2 = temp1 & (int16_t)0x0007;
    temp2 = WebRtcSpl_AddSatW16(shl(temp2, 2), temp2);
    pos[2] = WebRtcSpl_AddSatW16(temp2, 2);     /* 5 * x + 2 */
    temp1 = shr(temp1, 3);
    sign[2] = temp1 & (int16_t)0x0001;
    temp1 = shr(temp1, 1);
    temp2 = temp1 & (int16_t)0x000F;
    pos[3] = WebRtcSpl_AddSatW16((temp2 & (int16_t)1), 3); /* j+3*/
    temp2 = (shr(temp2, 1)) & (int16_t)7;
    temp2 = WebRtcSpl_AddSatW16(shl(temp2, 2), temp2); /* 5i */
    pos[3] = WebRtcSpl_AddSatW16(pos[3], temp2);
    temp1 = shr(temp1, 4);
    sign[3] = temp1 & (int16_t)0x0001;
    Gp = WebRtcG729fix_Random(seed) & (int16_t)0x1FFF; /* < 0.5 Q14 */
    Gp2 = shl(Gp, 1);           /* Q15 */


    /* Generate gaussian excitation */
    /********************************/
    L_acc = 0L;
    for(i=0; i<L_SUBFR; i++) {
      temp1 = Gauss(seed);
      L_acc = L_mac(L_acc, temp1, temp1);
      excg[i] = temp1;
    }

/*
    Compute fact = alpha x cur_gain * sqrt(L_SUBFR / Eg)
    with Eg = SUM(i=0->39) excg[i]^2
    and alpha = 0.5
    alpha x sqrt(L_SUBFR)/2 = 1 + FRAC1
*/
    L_acc = WebRtcG729fix_Inv_sqrt(L_shr(L_acc,1));  /* Q30 */
    WebRtcG729fix_L_Extract(L_acc, &hi, &lo);
    /* cur_gain = cur_gainR << 3 */
    temp1 = mult_r(cur_gain, FRAC1);
    temp1 = WebRtcSpl_AddSatW16(cur_gain, temp1);
    /* <=> alpha x cur_gainR x 2^2 x sqrt(L_SUBFR) */

    L_acc = WebRtcG729fix_Mpy_32_16(hi, lo, temp1);   /* fact << 17 */
    sh = WebRtcSpl_NormW32(L_acc);
    temp1 = extract_h(L_shl(L_acc, sh));  /* fact << (sh+1) */

    sh = WebRtcSpl_SubSatW16(sh, 14);
    for (i = 0; i < L_SUBFR; i++) {
      temp2 = mult_r(excg[i], temp1);
      temp2 = shr_r(temp2, sh);   /* shl if sh < 0 */
      excg[i] = temp2;
    }

    /* generate random  adaptive excitation */
    /****************************************/
    WebRtcG729fix_Pred_lt_3(cur_exc, t0, frac, L_SUBFR);


    /* compute adaptive + gaussian exc -> cur_exc */
    /**********************************************/
    max = 0;
    for(i = 0; i < L_SUBFR; i++) {
      temp1 = mult_r(cur_exc[i], Gp2);
      temp1 = WebRtcSpl_AddSatW16(temp1, excg[i]); /* may overflow */
      cur_exc[i] = temp1;
      temp1 = abs_s(temp1);
      if (temp1 > max)
        max = temp1;
    }

    /* rescale cur_exc -> excs */
    if (max == 0)
      sh = 0;
    else {
      sh = WebRtcSpl_SubSatW16(3, WebRtcSpl_NormW16(max));
      if (sh <= 0)
        sh = 0;
    }
    for (i = 0; i < L_SUBFR; i++) {
      excs[i] = shr(cur_exc[i], sh);
    }

    /* Compute fixed code gain */
    /***************************/

    /**********************************************************/
    /*** Solve EQ(X) = 4 X**2 + 2 b X + c                     */
    /**********************************************************/

    L_ener = 0L;
    for (i = 0; i < L_SUBFR; i++) {
      L_ener = L_mac(L_ener, excs[i], excs[i]);
    } /* ener x 2^(-2sh + 1) */

    /* inter_exc = b >> sh */
    inter_exc = 0;
    for (i = 0; i < 4; i++) {
      j = pos[i];
      if (sign[i] == 0) {
        inter_exc = WebRtcSpl_SubSatW16(inter_exc, excs[j]);
      }
      else {
        inter_exc = WebRtcSpl_AddSatW16(inter_exc, excs[j]);
      }
    }

    /* Compute k = cur_gainR x cur_gainR x L_SUBFR */
    L_acc = L_mult(cur_gain, L_SUBFR);
    L_acc = L_shr(L_acc, 6);
    temp1 = extract_l(L_acc);   /* cur_gainR x L_SUBFR x 2^(-2) */
    L_k   = L_mult(cur_gain, temp1); /* k << 2 */
    temp1 = WebRtcSpl_AddSatW16(1, shl(sh,1));
    L_acc = L_shr(L_k, temp1);  /* k x 2^(-2sh+1) */

    /* Compute delta = b^2 - 4 c */
    L_acc = WebRtcSpl_SubSatW32(L_acc, L_ener); /* - 4 c x 2^(-2sh-1) */
    inter_exc = shr(inter_exc, 1);
    L_acc = L_mac(L_acc, inter_exc, inter_exc); /* 2^(-2sh-1) */
    sh = WebRtcSpl_AddSatW16(sh, 1);
    /* inter_exc = b x 2^(-sh) */
    /* L_acc = delta x 2^(-2sh+1) */

    if (L_acc < 0) {

      /* adaptive excitation = 0 */
      WEBRTC_SPL_MEMCPY_W16(cur_exc, excg, L_SUBFR);
      temp1 = abs_s(excg[(int)pos[0]]) | abs_s(excg[(int)pos[1]]);
      temp2 = abs_s(excg[(int)pos[2]]) | abs_s(excg[(int)pos[3]]);
      temp1 = temp1 | temp2;
      sh = ((temp1 & (int16_t)0x4000) == 0) ? (int16_t)1 : (int16_t)2;
      inter_exc = 0;
      for(i=0; i<4; i++) {
        temp1 = shr(excg[(int)pos[i]], sh);
        if(sign[i] == 0) {
          inter_exc = WebRtcSpl_SubSatW16(inter_exc, temp1);
        }
        else {
          inter_exc = WebRtcSpl_AddSatW16(inter_exc, temp1);
        }
      } /* inter_exc = b >> sh */
      WebRtcG729fix_L_Extract(L_k, &hi, &lo);
      L_acc = WebRtcG729fix_Mpy_32_16(hi, lo, K0); /* k x (1- alpha^2) << 2 */
      temp1 = WebRtcSpl_SubSatW16(shl(sh, 1), 1); /* temp1 > 0 */
      L_acc = L_shr(L_acc, temp1); /* 4k x (1 - alpha^2) << (-2sh+1) */
      L_acc = L_mac(L_acc, inter_exc, inter_exc); /* delta << (-2sh+1) */
      Gp = 0;
    }

    temp2 = Sqrt(L_acc);        /* >> sh */
    x1 = WebRtcSpl_SubSatW16(temp2, inter_exc);
    x2 = negate(WebRtcSpl_AddSatW16(inter_exc, temp2)); /* x 2^(-sh+2) */
    if(abs_s(x2) < abs_s(x1)) x1 = x2;
    temp1 = WebRtcSpl_SubSatW16(2, sh);
    g = shr_r(x1, temp1);       /* shl if temp1 < 0 */
    if (g >= 0) {
      if (g > G_MAX)
        g = G_MAX;
    }
    else {
      if (WebRtcSpl_AddSatW16(g, G_MAX) < 0)
        g = negate(G_MAX);
    }

    /* Update cur_exc with ACELP excitation */
    for (i = 0; i < 4; i++) {
      j = pos[i];
      if (sign[i] != 0) {
        cur_exc[j] = WebRtcSpl_AddSatW16(cur_exc[j], g);
      }
      else {
        cur_exc[j] = WebRtcSpl_SubSatW16(cur_exc[j], g);
      }
    }

    if (flag_cod != FLAG_DEC)
      WebRtcG729fix_update_exc_err(L_exc_err, Gp, t0);

    cur_exc += L_SUBFR;
  } /* end of loop on subframes */
  
  return;
}
Exemple #23
0
Word32 fnExp2(Word32 L_Input)
{

/*_________________________________________________________________________
 |                                                                         |
 |                           Local Static Variables                        |
 |_________________________________________________________________________|
*/
	static Word16 pswPCoefE[3] =
	{					/* c2,   c1,    c0 */
		0x15ef, 0x556f, 0x7fbd
	};

/*_________________________________________________________________________
 |                                                                         |
 |                            Automatic Variables                          |
 |_________________________________________________________________________|
*/

	Word16 swTemp1, swTemp2, swTemp3, swTemp4;
	Word32 LwIn;

/*_________________________________________________________________________
 |                                                                         |
 |                              Executable Code                            |
 |_________________________________________________________________________|
*/

	/* initialize */
	/* ---------- */

	swTemp3 = 0x0020;

	/* determine normlization shift count */
	/* ---------------------------------- */

	swTemp1 = extract_h(L_Input);
	LwIn = L_mult(swTemp1, swTemp3);
	swTemp2 = extract_h(LwIn);

	/* determine un-normalized shift count */
	/* ----------------------------------- */

	swTemp3 = -0x0001;
	swTemp4 = sub(swTemp3, swTemp2);

	/* normalize input */
	/* --------------- */

	LwIn = LwIn & 0xffff;
	LwIn = L_add(LwIn, L_deposit_h(swTemp3));

	LwIn = L_shr(LwIn, 1);
	swTemp1 = extract_l(LwIn);

	/* calculate x*x*c2 */
	/* ---------------- */

	swTemp2 = mult_r(swTemp1, swTemp1);
	LwIn = L_mult(swTemp2, pswPCoefE[0]);

	/* calculate x*x*c2 + x*c1 */
	/* ----------------------- */

	LwIn = L_mac(LwIn, swTemp1, pswPCoefE[1]);

	/* calculate x*x*c2 + x*c1 + c0 */
	/* --------------------------- */

	LwIn = L_add(LwIn, L_deposit_h(pswPCoefE[2]));

	/* un-normalize exponent if its requires it */
	/* ---------------------------------------- */

	if (swTemp4 > 0)
	{
		LwIn = L_shr(LwIn, swTemp4);
	}

	/* return result */
	/* ------------- */

	return (LwIn);
}
Exemple #24
0
/*
**
** Function:            Lsp_Qnt()
**
** Description:     Vector quantizes the LSP frequencies.  The LSP
**          vector is divided into 3 sub-vectors, or
**          bands, of dimension 3, 3, and 4.  Each band is
**          quantized separately using a different VQ
**          table.  Each table has 256 entries, so the
**          quantization generates three indices of 8 bits
**          each.  (Only the LSP vector for subframe 3 is
**          quantized per frame.)
**
** Links to text:   Section 2.5
**
** Arguments:       
**
**  Word16 CurrLsp[]    Unquantized LSP frequencies for the current frame (10 words)
**  Word16 PrevLsp[]    LSP frequencies from the previous frame (10 words)
**
** Outputs:             Quantized LSP frequencies for the current frame (10 words)
**
** Return value:
**
**  Word32      Long word packed with the 3 VQ indices.  Band 0
**          corresponds to bits [23:16], band 1 corresponds
**          to bits [15:8], and band 2 corresponds to bits [7:0].
**          (Bit 0 is the least significant.)
**
*/
Word32   Lsp_Qnt( Word16 *CurrLsp, Word16 *PrevLsp )
{
    int   i ;

    Word16   Wvect[LpcOrder] ;

    Word16   Tmp0,Tmp1   ;
    Word16   Exp   ;


 /*
  * Compute the VQ weighting vector.  The weights assign greater
  * precision to those frequencies that are closer together.
  */

    /* Compute the end differences */
    Wvect[0] = sub( CurrLsp[1], CurrLsp[0] ) ;
    Wvect[LpcOrder-1] = sub( CurrLsp[LpcOrder-1], CurrLsp[LpcOrder-2] ) ;

    /* Compute the rest of the differences */
    for ( i = 1 ; i < LpcOrder-1 ; i ++ ) {
        Tmp0 = sub( CurrLsp[i+1], CurrLsp[i] ) ;
        Tmp1 = sub( CurrLsp[i], CurrLsp[i-1] ) ;
        if ( Tmp0 > Tmp1 )
            Wvect[i] = Tmp1 ;
        else
            Wvect[i] = Tmp0 ;
    }

    /* Invert the differences */
    Tmp0 = (Word16) 0x0020 ;
    for ( i = 0 ; i < LpcOrder ; i ++ ) {

        if ( Wvect[i] > Tmp0 )
            Wvect[i] = div_s( Tmp0, Wvect[i] ) ;
        else
            Wvect[i] = MAX_16 ;
    }

    /* Normalize the weight vector */
    Tmp0 = (Word16) 0 ;
    for ( i = 0 ; i < LpcOrder ; i ++ )
        if ( Wvect[i] > Tmp0 )
            Tmp0 = Wvect[i] ;

    Exp = norm_s( Tmp0 ) ;
    for ( i = 0 ; i < LpcOrder ; i ++ )
        Wvect[i] = shl( Wvect[i], Exp ) ;


 /*
  * Compute the VQ target vector.  This is the residual that remains
  * after subtracting both the DC and predicted
  * components.
  */

 /*
  * Subtract the DC component from both the current and previous LSP
  * vectors.
  */
    for ( i = 0 ; i < LpcOrder ; i ++ ) {
        CurrLsp[i] = sub( CurrLsp[i], LspDcTable[i] ) ;
        PrevLsp[i] = sub( PrevLsp[i], LspDcTable[i] ) ;
    }

 /*
  * Generate the prediction vector and subtract it.  Use a constant
  * first-order predictor based on the previous (DC-free) LSP
  * vector.
  */
    for ( i = 0 ; i < LpcOrder ; i ++ ) {
        Tmp0 = mult_r( PrevLsp[i], (Word16) LspPrd0 ) ;
        CurrLsp[i] = sub( CurrLsp[i], Tmp0 ) ;
    }

 /*
  * Add the DC component back to the previous LSP vector.  This
  * vector is needed in later routines.
  */
    for ( i = 0 ; i < LpcOrder ; i ++ )
        PrevLsp[i] = add( PrevLsp[i], LspDcTable[i] ) ;

 /*
  * Do the vector quantization for all three bands
  */
    return Lsp_Svq( CurrLsp, Wvect ) ;
}
Exemple #25
0
Word16 coarsepitch(
                   Word16  *xw,           /* (i) (normalized) weighted signal */
                   struct BV16_Encoder_State *cstate) /* (i/o) Coder State */
{
   
   Word16   s;    /* Q2 */
   Word16   a, b; 
   Word16   im;
   Word16   maxdev, flag, mpflag;
   Word32   eni, deltae;
   Word32   cc;
   Word16   ah,al, bh, bl;
   Word32   a0, a1, a2, a3;
   Word32   *lp0; 
   Word16   exp, new_exp;  
   Word16   *fp0, *fp1, *fp2, *fp3, *sp; 
   Word16   *fp1_h, *fp1_l, *fp2_h, *fp2_l;
   Word16   cor2max, cor2max_exp;
   Word16   cor2m, cor2m_exp;
   Word16   s0, t0, t1, exp0, exp1, e2, e3;
   Word16   threshold;
   Word16   mplth;      /* Q2 */
   Word16   i, j, k, n, npeaks, imax, idx[MAXPPD-MINPPD+1];
   Word16   cpp;
   Word16 plag[HMAXPPD], cor2[MAXPPD1], cor2_exp[MAXPPD1];
   Word16 cor2i[HMAXPPD], cor2i_exp[HMAXPPD], xwd[LXD];
   Word16 tmp_h[DFO+FRSZ], tmp_l[DFO+FRSZ]; /* DPF Q7 */
   Word32 cor[MAXPPD1], energy[MAXPPD1], lxwd[FRSZD];
   Word16 energy_man[MAXPPD1], energy_exp[MAXPPD1];
   Word16 energyi_man[HMAXPPD], energyi_exp[HMAXPPD];
   Word16 energym_man, energym_exp;
   Word16 energymax_man, energymax_exp;
      
   /* Lowpass filter xw() to 800 hz; shift & output into xwd() */
   
   /* AP and AZ filtering and decimation */
   
   fp1_h = tmp_h + DFO;
   fp1_l = tmp_l + DFO;
   
   sp = xw;
   a1 = 1;
   
   
   for (i=0;i<DFO;i++) tmp_h[i] = cstate->dfm_h[2*i+1];
   for (i=0;i<DFO;i++) tmp_l[i] = cstate->dfm_h[2*i];
   
   lp0 = lxwd;
   
   for (i=0;i<FRSZD;i++) {
      for (k=0;k<DECF;k++) {
         a0 = L_shr(L_deposit_h(*sp++),10);
         fp2_h = fp1_h-1;
         fp2_l = fp1_l-1;
         for (j=0;j<DFO;j++)  
            a0=L_sub(a0,Mpy_32(*fp2_h--,*fp2_l--,adf_h[j+1],adf_l[j+1]));
         a0 = L_shl(a0, 2);             /* adf Q13 */
         L_Extract(a0, fp1_h++, fp1_l++);
      }
      fp2_h = fp1_h-1;
      fp2_l = fp1_l-1;
      a0 = Mpy_32_16(*fp2_h--, *fp2_l--, bdf[0]);
      for (j=0;j<DFO;j++)
         a0=L_add(a0,Mpy_32_16(*fp2_h--,*fp2_l--,bdf[j+1]));
      
      *lp0++ = a0;
      a0 = L_abs(a0);
      
      if (a1 < a0) 
         a1 = a0;
   }
   
   /* copy temp buffer to memory */
   fp1_h -= DFO;
   fp1_l -= DFO;
   for (i=0;i<DFO;i++) {
      cstate->dfm_h[2*i+1] = fp1_h[i];
      cstate->dfm_h[2*i] = fp1_l[i];
   }
   
   lp0 = lxwd;
   new_exp = sub(norm_l(a1), 3);       /* headroom to avoid overflow */
   exp = sub(cstate->xwd_exp,new_exp); /* increase in bit-resolution */
   
   if (exp < 0) { /* Descending signal level */
      new_exp = cstate->xwd_exp;
      exp = 0;
   }
   
   for (i=0;i<XDOFF;i++) 
      xwd[i] = shr(cstate->xwd[i], exp);
   
   /* fill-in new exponent */
   fp0 = xwd + XDOFF;
   for (i=0;i<FRSZD;i++) 
      fp0[i] = intround(L_shl(lp0[i],new_exp));
   
   /* update signal memory for next frame */
   exp0 = 1;
   for (i=0;i<XDOFF;i++) {
      exp1 = abs_s(xwd[FRSZD+i]);
      
      if (exp1 > exp0) 
         exp0 = exp1;
   }
   exp0 = sub(norm_s(exp0),3); /* extra exponent for next frame */
   
   exp = sub(exp0, exp);
   
   if (exp >=0)
   {
      for (i=0;i<XDOFF-FRSZD;i++)  
         cstate->xwd[i] = shl(cstate->xwd[i+FRSZD], exp);
   }
   else
   {
      exp = -exp;
      if (exp >=15)
         exp = 15;
      for (i=0;i<XDOFF-FRSZD;i++)  
         cstate->xwd[i] = shr(cstate->xwd[i+FRSZD], exp);
   }
   for (;i<XDOFF;i++) 
      cstate->xwd[i] = shl(xwd[FRSZD+i],exp0);
   
   
   cstate->xwd_exp = add(new_exp, exp0);
   
   /* Compute correlation & energy of prediction basis vector */
   
   /* reset local buffers */
   for (i=0;i<MAXPPD1;i++) 
      cor[i] = energy[i] = 0;
   
   fp0 = xwd+MAXPPD1;
   fp1 = xwd+MAXPPD1-M1;
   a0 = a1 = 0; 
   for (i=0;i<(LXD-MAXPPD1);i++) {
      a0 = L_mac0(a0, *fp1, *fp1);
      a1 = L_mac0(a1, *fp0++, *fp1++);
   }
   cor[M1-1] = a1;
   energy[M1-1] = a0;
   energy_exp[M1-1] = norm_l(energy[M1-1]);
   energy_man[M1-1] = extract_h(L_shl(energy[M1-1], energy_exp[M1-1]));
   s0 = cor2_exp[M1-1] = norm_l(a1);
   t0 = extract_h(L_shl(a1, s0));
   cor2[M1-1] = extract_h(L_mult(t0, t0));
   
   if (a1 < 0) 
      cor2[M1-1] = negate(cor2[M1-1]);
   
   fp2 = xwd+LXD-M1-1;
   fp3 = xwd+MAXPPD1-M1-1;
   for (i=M1;i<M2;i++) {
      fp0 = xwd+MAXPPD1;
      fp1 = xwd+MAXPPD1-i-1;
      a1 = 0;
      for (j=0;j<(LXD-MAXPPD1);j++) 
         a1 = L_mac0(a1,*fp0++,*fp1++); 
      cor[i] = a1;
      a0 = L_msu0(a0, *fp2, *fp2);
      a0 = L_mac0(a0, *fp3, *fp3);
      fp2--; fp3--;
      energy[i] = a0;
      energy_exp[i] = norm_l(energy[i]);
      energy_man[i] = extract_h(L_shl(energy[i], energy_exp[i]));
      s0 = cor2_exp[i] = norm_l(a1);
      t0 = extract_h(L_shl(a1, s0));
      cor2[i] = extract_h(L_mult(t0, t0));
      
      if (a1 < 0) 
         cor2[i] = negate(cor2[i]);
   }
   
   /* Find positive correlation peaks */
   /* Find maximum of cor*cor/energy among positive correlation peaks */ 
   
   npeaks = 0;
   n = MINPPD-1;
   while ((npeaks < MAX_NPEAKS) && (n<MAXPPD)) {
      
      if (cor[n]>0) { 
         a0   = L_mult(energy_man[n-1],cor2[n]);
         a1   = L_mult(energy_man[n], cor2[n-1]);
         exp0 = shl(sub(cor2_exp[n], cor2_exp[n-1]),1);
         exp0 = add(exp0, energy_exp[n-1]);
         exp0 = sub(exp0, energy_exp[n]);
         
         if (exp0>=0) 
            a0 = L_shr(a0, exp0);
         else 
            a1 = L_shl(a1, exp0);
         
         if (a0 > a1) { 
            
            a0   = L_mult(energy_man[n+1],cor2[n]);
            a1   = L_mult(energy_man[n], cor2[n+1]);
            exp0 = shl(sub(cor2_exp[n], cor2_exp[n+1]),1);
            exp0 = add(exp0, energy_exp[n+1]);
            exp0 = sub(exp0, energy_exp[n]);
            
            if (exp0>=0) 
               a0 = L_shr(a0, exp0);
            else 
               a1 = L_shl(a1, exp0);
            
            if (a0 > a1) {
               idx[npeaks] = n;
               npeaks++; 
            }
         }
    }
    
    n++;
    
  }
  
  /* Return early if there is no peak or only one peak */
  
  if (npeaks == 0){   /* if there are no positive peak, */
     return MINPPD*DECF; /* return minimum pitch period in decimated domain */
  }
  
  if (npeaks == 1){   /* if there is exactly one peak, */
     return (idx[0]+1)*DECF; /* return the time lag for this single peak */
  }
  
  /* If program proceeds to here, there are 2 or more peaks */
  cor2max=(Word16) 0x8000;
  cor2max_exp= (Word16) 0;
  energymax_man=1;
  energymax_exp=0;
  
  imax=0;
  for (i=0; i < npeaks; i++) {
     
  /* Use quadratic interpolation to find the interpolated cor[] and
     energy[] corresponding to interpolated peak of cor2[]/energy[] */
     /* first calculate coefficients of y(x)=ax^2+bx+c; */
     n=idx[i];
     a0=L_sub(L_shr(L_add(cor[n+1],cor[n-1]),1),cor[n]);
     L_Extract(a0, &ah, &al);
     a0=L_shr(L_sub(cor[n+1],cor[n-1]),1);
     L_Extract(a0, &bh, &bl);
     cc=cor[n];
     
     /* Initialize variables before searching for interpolated peak */
     im=0;
     cor2m_exp = cor2_exp[n];
     cor2m = cor2[n];
     energym_exp = energy_exp[n];
     energym_man = energy_man[n];
     eni=energy[n];
     
     /* Determine which side the interpolated peak falls in, then
     do the search in the appropriate range */
     
     a0   = L_mult(energy_man[n-1],cor2[n+1]);
     a1   = L_mult(energy_man[n+1], cor2[n-1]);
     exp0 = shl(sub(cor2_exp[n+1], cor2_exp[n-1]),1);
     exp0 = add(exp0, energy_exp[n-1]);
     exp0 = sub(exp0, energy_exp[n+1]);
     
     if (exp0>=0) 
        a0 = L_shr(a0, exp0);
     else 
        a1 = L_shl(a1, exp0);
     
     if (a0 > a1) {  /* if right side */
        
        deltae = L_shr(L_sub(energy[n+1], eni), 2);
        
        for (k = 0; k < HDECF; k++) {
           a0=L_add(L_add(Mpy_32_16(ah,al,x2[k]),Mpy_32_16(bh,bl,x[k])),cc);
           eni = L_add(eni, deltae);
           a1 = eni;
           exp0 = norm_l(a0);
           s0 = extract_h(L_shl(a0, exp0));
           s0 = extract_h(L_mult(s0, s0));
           e2 = energym_exp;
           t0 = energym_man;
           a2 = L_mult(t0, s0);
           e3 = norm_l(a1);
           t1 = extract_h(L_shl(a1, e3));
           a3 = L_mult(t1, cor2m);
           exp1 = shl(sub(exp0, cor2m_exp),1);
           exp1 = add(exp1, e2);
           exp1 = sub(exp1, e3);
           
           if (exp1>=0) 
              a2 = L_shr(a2, exp1);
           else 
              a3 = L_shl(a3, exp1);
           
           if (a2 > a3) {
              im = k+1;
              cor2m = s0;
              cor2m_exp = exp0;
              energym_exp = e3;
              energym_man = t1;
           }
        }        
     } else {    /* if interpolated peak is on the left side */
        
        deltae = L_shr(L_sub(energy[n-1], eni), 2);
        for (k = 0; k < HDECF; k++) {
           a0=L_add(L_sub(Mpy_32_16(ah,al,x2[k]),Mpy_32_16(bh,bl,x[k])),cc);
           eni = L_add(eni, deltae);
           a1=eni;
           
           exp0 = norm_l(a0);
           s0 = extract_h(L_shl(a0, exp0));
           s0 = extract_h(L_mult(s0, s0));
           e2 = energym_exp;
           t0 = energym_man;
           a2 = L_mult(t0, s0);
           e3 = norm_l(a1);
           t1 = extract_h(L_shl(a1, e3));
           a3 = L_mult(t1, cor2m);
           exp1 = shl(sub(exp0, cor2m_exp),1);
           exp1 = add(exp1, e2);
           exp1 = sub(exp1, e3);
           
           if (exp1>=0) 
              a2 = L_shr(a2, exp1);
           else 
              a3 = L_shl(a3, exp1);
           
           if (a2 > a3) {
              im = -k-1;
              cor2m = s0;
              cor2m_exp = exp0;
              energym_exp = e3;
              energym_man = t1;
           }
        }        
     }
     
     /* Search done; assign cor2[] and energy[] corresponding to 
     interpolated peak */ 
     plag[i]=add(shl(add(idx[i],1),2),im); /* lag of interp. peak */
     cor2i[i]=cor2m;
     cor2i_exp[i]=cor2m_exp;
     /* interpolated energy[] of i-th interpolated peak */
     energyi_exp[i] = energym_exp;
     energyi_man[i] = energym_man;
     
     /* Search for global maximum of interpolated cor2[]/energy[] peak */
     a0 = L_mult(cor2m,energymax_man);
     a1 = L_mult(cor2max, energyi_man[i]);
     exp0 = shl(sub(cor2m_exp, cor2max_exp),1);
     exp0 = add(exp0, energymax_exp);
     exp0 = sub(exp0, energyi_exp[i]);
     
     if (exp0 >=0) 
        a0 = L_shr(a0, exp0);
     else 
        a1 = L_shl(a1, exp0);
     
     if (a0 > a1) {
        imax=i;
        cor2max=cor2m;
        cor2max_exp=cor2m_exp;
        energymax_exp = energyi_exp[i];
        energymax_man = energyi_man[i];
     }
  }
  cpp=plag[imax]; /* first candidate for coarse pitch period */
  mplth=plag[npeaks-1]; /* set mplth to the lag of last peak */
  
  /* Find the largest peak (if there is any) around the last pitch */
  maxdev= shr(cstate->cpplast,2); /* maximum deviation from last pitch */
  im = -1;
  cor2m=(Word16) 0x8000;
  cor2m_exp= (Word16) 0;
  energym_man = 1;
  energym_exp = 0;
  
  for (i=0;i<npeaks;i++) {  /* loop thru the peaks before the largest peak */
     
     if (abs_s(sub(plag[i],cstate->cpplast)) <= maxdev) {
        a0 = L_mult(cor2i[i],energym_man);
        a1 = L_mult(cor2m, energyi_man[i]);
        exp0 = shl(sub(cor2i_exp[i], cor2m_exp),1);
        exp0 = add(exp0, energym_exp);
        exp0 = sub(exp0, energyi_exp[i]);
        
        if (exp0 >=0) 
           a0 = L_shr(a0, exp0);
        else 
           a1 = L_shl(a1, exp0);
        
        if (a0 > a1) {
           im=i;
           cor2m=cor2i[i];
           cor2m_exp=cor2i_exp[i];
           energym_man = energyi_man[i];
           energym_exp = energyi_exp[i];
        }   
     }
  } /* if there is no peaks around last pitch, then im is still -1 */
  
  
  /* Now see if we should pick any alternatice peak */
  /* first, search first half of pitch range, see if any qualified peak
  has large enough peaks at every multiple of its lag */
  i=0;
  
  while (2*plag[i] < mplth) {
     
     /* Determine the appropriate threshold for this peak */
     
     if (i != im) {  /* if not around last pitch, */
        threshold = TH1;    /* use a higher threshold */
     } else {        /* if around last pitch */
        threshold = TH2;    /* use a lower threshold */
     }
     
     /* If threshold exceeded, test peaks at multiples of this lag */
     a0 = L_mult(cor2i[i],energymax_man);
     t1 = extract_h(L_mult(energyi_man[i], threshold));
     a1 = L_mult(cor2max, t1);
     exp0 = shl(sub(cor2i_exp[i], cor2max_exp),1);
     exp0 = add(exp0, energymax_exp);
     exp0 = sub(exp0, energyi_exp[i]);
     
     if (exp0 >=0) 
        a0 = L_shr(a0, exp0);
     else 
        a1 = L_shl(a1, exp0);
     
     if (a0 > a1) {
        flag=1;  
        j=i+1;
        k=0;
        s=shl(plag[i],1); /* initialize t to twice the current lag */
        
        while (s<=mplth) { /* loop thru all multiple lag <= mplth */
           
           mpflag=0;   /* initialize multiple pitch flag to 0 */
           t0 = mult_r(s,MPDTH); 
           a=sub(s, t0);   /* multiple pitch range lower bound */
           b=add(s, t0);   /* multiple pitch range upper bound */
           while (j < npeaks) { /* loop thru peaks with larger lags */
              
              if (plag[j] > b) { /* if range exceeded, */
                 break;          /* break the innermost while loop */
              }       /* if didn't break, then plag[j] <= b */
              
              if (plag[j] > a) { /* if current peak lag within range, */
                 /* then check if peak value large enough */
                 a0 = L_mult(cor2i[j],energymax_man);
                 if (k<4) 
                    t1 = MPTH[k];
                 else 
                    t1 = MPTH4;
                 t1 = extract_h(L_mult(t1, energyi_man[j]));
                 a1 = L_mult(cor2max, t1);
                 exp0 = shl(sub(cor2i_exp[j], cor2max_exp),1);
                 exp0 = add(exp0, energymax_exp);
                 exp0 = sub(exp0, energyi_exp[j]);
                 
                 if (exp0 >=0) 
                    a0 = L_shr(a0, exp0);
                 else 
                    a1 = L_shl(a1, exp0);
                 
                 if (a0 > a1) {
                    mpflag=1; /* if peak large enough, set mpflag, */
                    break; /* and break the innermost while loop */
                 } 
              }
              j++;
           }
           /* if no qualified peak found at this multiple lag */
           
           if (mpflag == 0) { 
              flag=0;     /* disqualify the lag plag[i] */
              break;      /* and break the while (s<=mplth) loop */
           }
           k++;
           s = add(s, plag[i]); /* update s to the next multiple pitch lag */
           
        }
        /* if there is a qualified peak at every multiple of plag[i], */
        
        if (flag == 1) { 
           cpp = plag[i];   /* then accept this as final pitch */

           return cpp;         /* and return to calling function */
        }
    }       
    i++;
    
    if (i == npeaks)
       break;      /* to avoid out of array bound error */
  }
  
  /* If program proceeds to here, none of the peaks with lags < 0.5*mplth
  qualifies as the final pitch. in this case, check if
  there is any peak large enough around last pitch.  if so, use its
  lag as the final pitch. */
  
  if (im != -1) {   /* if there is at least one peak around last pitch */
     
     if (im == imax) { /* if this peak is also the global maximum, */
        return cpp;   /* return first pitch candidate at global max */
     }
     
     if (im < imax) { /* if lag of this peak < lag of global max, */
        a0 = L_mult(cor2m,energymax_man);
        t1 = extract_h(L_mult(energym_man, LPTH2));
        a1 = L_mult(cor2max, t1);
        exp0 = shl(sub(cor2m_exp, cor2max_exp),1);
        exp0 = add(exp0, energymax_exp);
        exp0 = sub(exp0, energym_exp);
        
        if (exp0 >=0) 
           a0 = L_shr(a0, exp0);
        else 
           a1 = L_shl(a1, exp0);
        
        if (a0 > a1) {
           
           if (plag[im] > HMAXPPD*DECF) {
              cpp=plag[im];
              
              return cpp;
           }
           for (k=2; k<=5;k++) { /* check if current candidate pitch */
              s=mult(plag[imax],invk[k-2]); /* is a sub-multiple of */
              t0 = mult_r(s,SMDTH);
              a=sub(s, t0);      /* the time lag of */
              b=add(s, t0);       /* the global maximum peak */
              
              if (plag[im]>a && plag[im]<b) {     /* if so, */
                 cpp=plag[im];       /* accept this peak, */
                 
                 return cpp;         /* and return as pitch */
              }
           }
        }
     } else {           /* if lag of this peak > lag of global max, */
        a0 = L_mult(cor2m,energymax_man);
        t1 = extract_h(L_mult(energym_man, LPTH1));
        a1 = L_mult(cor2max, t1);
        exp0 = shl(sub(cor2m_exp, cor2max_exp),1);
        exp0 = add(exp0, energymax_exp);
        exp0 = sub(exp0, energym_exp);
        
        if (exp0 >=0) 
           a0 = L_shr(a0, exp0);
        else 
           a1 = L_shl(a1, exp0);
        
        if (a0 > a1) {
           cpp = plag[im];  /* if this peak is large enough, */
           
           return cpp;         /* accept its lag */ 
        }
     }
  }
  
  /* If program proceeds to here, we have no choice but to accept the
  lag of the global maximum */
  return cpp;
  
}
Exemple #26
0
void schur_recursion (
    Word32 L_av1[],
    Word16 vpar[]
)
{
    Word16 acf[9], pp[9], kk[9], temp;
    Word16 i, k, m, n;

    /*** Schur recursion with 16-bit arithmetic ***/

      
    if (L_av1[0] == 0)
    {
        for (i = 0; i < 8; i++)
        {
            vpar[i] = 0;                                 
        }
        return;
    }
    temp = norm_l (L_av1[0]);

    for (k = 0; k <= 8; k++)
    {
		if(temp<=0)
			acf[k] = extract_h (L_shr (L_av1[k], -temp));     
		else
			acf[k] = extract_h (L_shl (L_av1[k], temp));     
    }

    /*** Initialize arrays pp[..] and kk[..] for the recursion: ***/

    for (i = 1; i <= 7; i++)
    {
        kk[9 - i] = acf[i];                              
    }

    for (i = 0; i <= 8; i++)
    {
        pp[i] = acf[i];                                  
    }




    /*** Compute Parcor coefficients: ***/

    for (n = 0; n < 8; n++)
    {
         
        if ((pp[0] == 0) ||
            (sub (pp[0], abs_s (pp[1])) < 0))
        {
            for (i = n; i < 8; i++)
            {
                vpar[i] = 0;                             
            }
            return;
        }
        vpar[n] = div_s (abs_s (pp[1]), pp[0]);          

                                                 
        if (pp[1] > 0)
        {
            vpar[n] = negate (vpar[n]);                  
        }
         
        if (sub (n, 7) == 0)
        {
            return;
        }
        /*** Schur recursion: ***/

        pp[0] = add (pp[0], mult_r (pp[1], vpar[n]));    

        for (m = 1; m <= 7 - n; m++)
        {
            pp[m] = add (pp[1 + m], mult_r (kk[9 - m], vpar[n]));
                                                         
            kk[9 - m] = add (kk[9 - m], mult_r (pp[1 + m], vpar[n]));
                                                         
        }
    }

    return;
}