C++ L_shr函数代码示例

void Log2(
          Word32 x,         /* (i) input           */
          Word16 *int_comp, /* Q0 integer part     */
          Word16 *frac_comp /* Q15 fractional part */
          )
{
   Word16 exp, idx_man, sub_man, sub_tab;
   Word32 a0;

   if(x <= 0){
      *int_comp = 0;
      *frac_comp = 0;
   }
   else{
      exp = norm_l(x);                                   // normalization
      a0 = L_shl(x, exp);                                // Q30 mantissa, i.e. 1.xxx Q30

      /* use table look-up of man in [1.0, 2.0[ Q30 */
      a0 = L_shr(L_sub(a0, (Word32)0x40000000), 8);      // Q16 index into table - note zero'ing of leading 1
      idx_man = extract_h(a0);                           // Q0 index into table
      sub_man = extract_l(L_shr((a0 & 0xFFFF), 1));      // Q15 fractional sub_man
      a0 = L_deposit_h(tablog[idx_man]);                 // Q31
      sub_tab = sub(tablog[idx_man+1], tablog[idx_man]); // Q15
      a0 = L_mac(a0, sub_man, sub_tab);                  // Q31

      *frac_comp = intround(a0);                             // Q15
      *int_comp = sub(30, exp);                          // Q0
   }

   return;
}

void WebRtcG729fix_Log2(
  int32_t L_x,       /* (i) Q0 : input value                                 */
  int16_t *exponent, /* (o) Q0 : Integer part of Log2.   (range: 0<=val<=30) */
  int16_t *fraction  /* (o) Q15: Fractional  part of Log2. (range: 0<=val<1) */
)
{
  int16_t exp, i, a, tmp;
  int32_t L_y;

  if( L_x <= (int32_t)0 )
  {
    *exponent = 0;
    *fraction = 0;
    return;
  }

  exp = WebRtcSpl_NormW32(L_x);
  L_x = L_shl(L_x, exp );               /* L_x is normalized */

  *exponent = WebRtcSpl_SubSatW16(30, exp);

  L_x = L_shr(L_x, 9);
  i   = extract_h(L_x);                 /* Extract b25-b31 */
  L_x = L_shr(L_x, 1);
  a   = extract_l(L_x);                 /* Extract b10-b24 of fraction */
  a   = a & (int16_t)0x7fff;

  i   = WebRtcSpl_SubSatW16(i, 32);

  L_y = L_deposit_h(WebRtcG729fix_tablog[i]);         /* tablog[i] << 16        */
  tmp = WebRtcSpl_SubSatW16(WebRtcG729fix_tablog[i], WebRtcG729fix_tablog[i+1]);    /* tablog[i] - tablog[i+1] */
  L_y = L_msu(L_y, tmp, a);             /* L_y -= tmp*a*2        */

  *fraction = extract_h( L_y);
}

int32_t WebRtcG729fix_Inv_sqrt( /* (o) Q30 : output value (range: 0<=val<1) */
  int32_t L_x       /* (i) Q0  : input value    (range: 0<=val<=7fffffff)   */
)
{
  int16_t exp, i, a, tmp;
  int32_t L_y;

  if( L_x <= (int32_t)0) return ( (int32_t)0x3fffffffL);

  exp = WebRtcSpl_NormW32(L_x);
  L_x = L_shl(L_x, exp );               /* L_x is normalize */

  exp = WebRtcSpl_SubSatW16(30, exp);
  if( (exp & 1) == 0 )                  /* If exponent even -> shift right */
      L_x = L_shr(L_x, 1);

  exp = shr(exp, 1);
  exp = WebRtcSpl_AddSatW16(exp, 1);

  L_x = L_shr(L_x, 9);
  i   = extract_h(L_x);                 /* Extract b25-b31 */
  L_x = L_shr(L_x, 1);
  a   = extract_l(L_x);                 /* Extract b10-b24 */
  a   = a & (int16_t)0x7fff;

  i   = WebRtcSpl_SubSatW16(i, 16);

  L_y = L_deposit_h(WebRtcG729fix_tabsqr[i]);         /* tabsqr[i] << 16          */
  tmp = WebRtcSpl_SubSatW16(WebRtcG729fix_tabsqr[i], WebRtcG729fix_tabsqr[i+1]);    /* tabsqr[i] - tabsqr[i+1])  */
  L_y = L_msu(L_y, tmp, a);             /* L_y -=  tmp*a*2         */

  L_y = L_shr(L_y, exp);                /* denormalization */

  return(L_y);
}

Word16 sqrts(Word16 x)
{
   Word16 xb, y, exp, idx, sub_frac, sub_tab;
   Word32 a0;

   if(x <= 0){
      y = 0;
   }
   else{
      exp = norm_s(x);

      /* use 65-entry table */
      xb = shl(x, exp);                            // normalization of x
      idx = shr(xb, 9);                            // for 65 entry table
      a0 = L_deposit_h(tabsqrt[idx]);              // Q31 table look-up value
      sub_frac = shl((Word16)(xb & 0x01FF), 6);    // Q15 sub-fraction
      sub_tab = sub(tabsqrt[idx+1], tabsqrt[idx]); // Q15 table interval for interpolation
      a0 = L_mac(a0, sub_frac, sub_tab);           // Q31 linear interpolation between table entries
      if(exp & 0x0001){
         exp = shr(add(exp, 1), 1);                // normalization of sqrt()
         a0 = L_shr(a0, exp);
         y = intround(a0);                            // Q15
         a0 = L_mac(a0, 13573, y);                 // Q31 incorporate the missing "/sqrt(2)"
      }
      else{
         exp = shr(exp, 1);                        // normalization of sqrt()
         a0 = L_shr(a0, exp);                      // Q31
      }
      y = intround(a0);                               // Q15
   }

   return y;
}

Word32 Inv_sqrt(   /* (o) Q30 : output value   (range: 0<=val<1)           */
  Word32 L_x       /* (i) Q0  : input value    (range: 0<=val<=7fffffff)   */
)
{
  Word16 exp, i, a, tmp;
  Word32 L_y;

  if( L_x <= (Word32)0) return ( (Word32)0x3fffffffL);

  exp = norm_l(L_x);
  L_x = L_shl(L_x, exp );               /* L_x is normalize */

  exp = sub(30, exp);
  if( (exp & 1) == 0 )                  /* If exponent even -> shift right */
      L_x = L_shr(L_x, 1);

  exp = shr(exp, 1);
  exp = add(exp, 1);

  L_x = L_shr(L_x, 9);
  i   = extract_h(L_x);                 /* Extract b25-b31 */
  L_x = L_shr(L_x, 1);
  a   = extract_l(L_x);                 /* Extract b10-b24 */
  a   = a & (Word16)0x7fff;

  i   = sub(i, 16);

  L_y = L_deposit_h(tabsqr[i]);         /* tabsqr[i] << 16          */
  tmp = sub(tabsqr[i], tabsqr[i+1]);    /* tabsqr[i] - tabsqr[i+1])  */
  L_y = L_msu(L_y, tmp, a);             /* L_y -=  tmp*a*2         */

  L_y = L_shr(L_y, exp);                /* denormalization */

  return(L_y);
}

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

    if(den2 == 0) {

        return(1);
    }

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

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

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

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

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

        return(2);
    }
    else {

        return(1);
    }
}

void Cor_h_X(
    Word16 h[],        /* (i) Q12 :Impulse response of filters      */
    Word16 X[],        /* (i)     :Target vector                    */
    Word16 D[]         /* (o)     :Correlations between h[] and D[] */
    /*          Normalized to 13 bits            */
)
{
    Word16 i, j;
    Word32 s, max, L_temp;
    Word32 y32[L_SUBFR];

    /* first keep the result on 32 bits and find absolute maximum */

    max = 0;

    for (i = 0; i < L_SUBFR; i++)
    {
        s = 0;
        for (j = i; j <  L_SUBFR; j++)
            s = L_mac(s, X[j], h[j-i]);

        y32[i] = s;

        s = L_abs(s);
        L_temp =L_sub(s,max);
        if(L_temp>0L) {
            max = s;
        }
    }

    /* Find the number of right shifts to do on y32[]  */
    /* so that maximum is on 13 bits                   */

    j = norm_l(max);
    if( sub(j,16) > 0) {
        j = 16;
    }

    j = sub(18, j);

    if(j>=0)
    {
        for(i=0; i<L_SUBFR; i++) {
            D[i] = extract_l( L_shr(y32[i], j) );
        }
    }
    else
    {
        Word16 pj = abs_s(j);
        for(i=0; i<L_SUBFR; i++) {
            D[i] = extract_l( L_shr(y32[i], pj) );
        }
    }

    return;

}

/*
 * The decimation-in-time complex FFT is implemented below.
 * The input complex numbers are presented as real part followed by
 * imaginary part for each sample.  The counters are therefore
 * incremented by two to access the complex valued samples.
 */
void r_fft(Word16 * farray_ptr, Flag *pOverflow)
{

    Word16 ftmp1_real;
    Word16 ftmp1_imag;
    Word16 ftmp2_real;
    Word16 ftmp2_imag;
    Word32 Lftmp1_real;
    Word32 Lftmp1_imag;
    Word16 i;
    Word16 j;
    Word32 Ltmp1;

    /* Perform the complex FFT */
    c_fft(farray_ptr, pOverflow);

    /* First, handle the DC and foldover frequencies */
    ftmp1_real = *farray_ptr;
    ftmp2_real = *(farray_ptr + 1);
    *farray_ptr = add(ftmp1_real, ftmp2_real, pOverflow);
    *(farray_ptr + 1) = sub(ftmp1_real, ftmp2_real, pOverflow);

    /* Now, handle the remaining positive frequencies */
    for (i = 2, j = SIZE - i; i <= SIZE_BY_TWO; i = i + 2, j = SIZE - i)
    {
        ftmp1_real = add(*(farray_ptr + i), *(farray_ptr + j), pOverflow);
        ftmp1_imag = sub(*(farray_ptr + i + 1),
                         *(farray_ptr + j + 1), pOverflow);
        ftmp2_real = add(*(farray_ptr + i + 1),
                         *(farray_ptr + j + 1), pOverflow);
        ftmp2_imag = sub(*(farray_ptr + j),
                         *(farray_ptr + i), pOverflow);

        Lftmp1_real = L_deposit_h(ftmp1_real);
        Lftmp1_imag = L_deposit_h(ftmp1_imag);

        Ltmp1 = L_mac(Lftmp1_real, ftmp2_real, phs_tbl[i], pOverflow);
        Ltmp1 = L_msu(Ltmp1, ftmp2_imag, phs_tbl[i + 1], pOverflow);
        *(farray_ptr + i) = pv_round(L_shr(Ltmp1, 1, pOverflow), pOverflow);

        Ltmp1 = L_mac(Lftmp1_imag, ftmp2_imag, phs_tbl[i], pOverflow);
        Ltmp1 = L_mac(Ltmp1, ftmp2_real, phs_tbl[i + 1], pOverflow);
        *(farray_ptr + i + 1) = pv_round(L_shr(Ltmp1, 1, pOverflow), pOverflow);

        Ltmp1 = L_mac(Lftmp1_real, ftmp2_real, phs_tbl[j], pOverflow);
        Ltmp1 = L_mac(Ltmp1, ftmp2_imag, phs_tbl[j + 1], pOverflow);
        *(farray_ptr + j) = pv_round(L_shr(Ltmp1, 1, pOverflow), pOverflow);

        Ltmp1 = L_negate(Lftmp1_imag);
        Ltmp1 = L_msu(Ltmp1, ftmp2_imag, phs_tbl[j], pOverflow);
        Ltmp1 = L_mac(Ltmp1, ftmp2_real, phs_tbl[j + 1], pOverflow);
        *(farray_ptr + j + 1) = pv_round(L_shr(Ltmp1, 1, pOverflow), pOverflow);

    }
}                               /* end r_fft () */

Word32 sqrt_l_exp(      /* o : output value,                          Q31 */
    Word32 L_x,         /* i : input value,                           Q31 */
    Word16 *pExp,       /* o : right shift to be applied to result,   Q1  */
    Flag   *pOverflow   /* i : pointer to overflow flag */
)

{
    Word16 e;
    Word16 i;
    Word16 a;
    Word16 tmp;
    Word32 L_y;

    /*
          y = sqrt(x)
          x = f * 2^-e,   0.5 <= f < 1   (normalization)
          y = sqrt(f) * 2^(-e/2)
          a) e = 2k   --> y = sqrt(f)   * 2^-k  (k = e div 2,
                                                 0.707 <= sqrt(f) < 1)
          b) e = 2k+1 --> y = sqrt(f/2) * 2^-k  (k = e div 2,
                                                 0.5 <= sqrt(f/2) < 0.707)
     */

    if (L_x <= (Word32) 0)
    {
        *pExp = 0;
        return (Word32) 0;
    }

    e = norm_l(L_x) & 0xFFFE;               /* get next lower EVEN norm. exp  */
    L_x = L_shl(L_x, e, pOverflow);         /* L_x is normalized to [0.25..1) */
    *pExp = e;                              /* return 2*exponent (or Q1)      */

    L_x = L_shr(L_x, 9, pOverflow);
    i = (Word16)(L_x >> 16);               /* Extract b25-b31, 16 <= i <= 63
                                            because of normalization          */

    L_x = L_shr(L_x, 1, pOverflow);
    a = (Word16)(L_x);                      /* Extract b10-b24 */
    a &= (Word16) 0x7fff;

    i = sub(i, 16, pOverflow);              /* 0 <= i <= 47                   */

    L_y = L_deposit_h(sqrt_l_tbl[i]);       /* sqrt_l_tbl[i] << 16            */

    /* sqrt_l_tbl[i] - sqrt_l_tbl[i+1]) */
    tmp = sub(sqrt_l_tbl[i], sqrt_l_tbl[i + 1], pOverflow);

    L_y = L_msu(L_y, tmp, a, pOverflow);    /* L_y -= tmp*a*2                 */

    /* L_y = L_shr (L_y, *exp); */          /* denormalization done by caller */

    return (L_y);
}

/*----------------------------------------------------------------------------
 * 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;
}

int16_t WebRtcG729fix_Random(int16_t *seed)
{
  /* seed = seed*31821 + 13849; */
  *seed = extract_l(WebRtcSpl_AddSatW32(L_shr(L_mult(*seed, 31821), 1), 13849L));

  return(*seed);
}

void maxeloc(INT16 *maxloc,
			 INT32 *maxener,
			 INT16 *signal,
			 INT16 dp,
			 INT16 length,
			 INT16 ewl)
{
	INT32 ener;
	register int i;
	int tail, front;

	ener = 0;
	front = add(dp, ewl);
	tail = sub(dp, ewl);
	for (i = tail; i <= front; i++)
		ener = L_mac(ener, signal[i], signal[i]);

	*maxloc = 0;
	*maxener = ener;
	for (i = 1; i < length; i++)
	{
		front++;
		ener = L_msu(ener, signal[tail], signal[tail]);
		ener = L_mac(ener, signal[front], signal[front]);

		tail++;
		if (*maxener < ener)
		{
			*maxloc = i;
			*maxener = ener;
		}
	}
	*maxloc = add(*maxloc,dp);
	*maxener = L_shr(*maxener, 1);
}

void WebRtcG729fix_Lsp_lsf2(
  int16_t lsp[],    /* (i) Q15 : lsp[m] (range: -1<=val<1)   */
  int16_t lsf[],    /* (o) Q13 : lsf[m] (range: 0.0<=val<PI) */
  int16_t m         /* (i)     : LPC order                   */
)
{
  int16_t i, ind;
  int16_t offset;   /* in Q15 */
  int16_t freq;     /* normalized frequency in Q16 */
  int32_t L_tmp;

  ind = 63;           /* begin at end of table2 -1 */

  for(i= m-(int16_t)1; i >= 0; i--)
  {
    /* find value in table2 that is just greater than lsp[i] */
    while( WebRtcSpl_SubSatW16(WebRtcG729fix_table2[ind], lsp[i]) < 0 )
    {
      ind = WebRtcSpl_SubSatW16(ind,1);
      if ( ind <= 0 )
        break;
    }

    offset = WebRtcSpl_SubSatW16(lsp[i], WebRtcG729fix_table2[ind]);

    /* acos(lsp[i])= ind*512 + (slope_acos[ind]*offset >> 11) */

    L_tmp  = L_mult( WebRtcG729fix_slope_acos[ind], offset );   /* L_tmp in Q28 */
    freq = WebRtcSpl_AddSatW16(shl(ind, 9), extract_l(L_shr(L_tmp, 12)));
    lsf[i] = mult(freq, 25736);           /* 25736: 2.0*PI in Q12 */

  }
}

void WebRtcG729fix_Lsf_lsp2(
  int16_t lsf[],    /* (i) Q13 : lsf[m] (range: 0.0<=val<PI) */
  int16_t lsp[],    /* (o) Q15 : lsp[m] (range: -1<=val<1)   */
  int16_t m         /* (i)     : LPC order                   */
)
{
  int16_t i, ind;
  int16_t offset;   /* in Q8 */
  int16_t freq;     /* normalized frequency in Q15 */
  int32_t L_tmp;

  for(i=0; i<m; i++)
  {
/*    freq = abs_s(freq);*/
    freq = mult(lsf[i], 20861);          /* 20861: 1.0/(2.0*PI) in Q17 */
    ind    = shr(freq, 8);               /* ind    = b8-b15 of freq */
    offset = freq & (int16_t)0x00ff;      /* offset = b0-b7  of freq */

    if (ind > 63){
      ind = 63;                 /* 0 <= ind <= 63 */
    }

    /* lsp[i] = table2[ind]+ (slope_cos[ind]*offset >> 12) */

    L_tmp   = L_mult(WebRtcG729fix_slope_cos[ind], offset);   /* L_tmp in Q28 */
    lsp[i] = WebRtcSpl_AddSatW16(WebRtcG729fix_table2[ind], extract_l(L_shr(L_tmp, 13)));

  }
}

static void cor_h_x_e(
  Word16 h[],    /* (i) Q12 : impulse response of weighted synthesis filter */
  Word16 x[],    /* (i) Q0  : correlation between target and h[]            */
  Word16 dn[]    /* (o) Q0  : correlation between target and h[]            */
)
{
    Word16 i, j, k;
    Word32 s, y32[L_SUBFR], max, tot, L_tmp;

    /* first keep the result on 32 bits and find absolute maximum */
    tot = 5;
    for (k=0; k<NB_TRACK; k++) {
        max = 0;

        for (i=k; i<L_SUBFR; i+=STEP) {
            s = 0;
            for (j=i; j<L_SUBFR; j++) s = L_mac(s, x[j], h[j-i]);
            y32[i] = s;
            s = L_abs(s);
            L_tmp = L_sub(s, max);
            if (L_tmp > (Word32)0) max = s;
        }
        tot = L_add(tot, L_shr(max, 1));    /* tot += (2.0 x max) / 4.0 */
    }

    /* Find the number of right shifts to do on y32[] so that */
    /* 2.0 x sumation of all max of dn[] in each track not saturate. */
    j = sub(norm_l(tot), 2);     /* multiply tot by 4 */
    for (i=0; i<L_SUBFR; i++) {
        dn[i] = round(L_shl(y32[i], j));
    }
    return;
}