C++ WEBRTC_SPL_RSHIFT_W32函数代码示例

/* The conversion is implemented by the step-down algorithm */
void WebRtcSpl_AToK_JSK(
    WebRtc_Word16 *a16, /* Q11 */
    WebRtc_Word16 useOrder,
    WebRtc_Word16 *k16  /* Q15 */
                        )
{
  int m, k;
  WebRtc_Word32 tmp32[MAX_AR_MODEL_ORDER];
  WebRtc_Word32 tmp32b;
  WebRtc_Word32 tmp_inv_denum32;
  WebRtc_Word16 tmp_inv_denum16;

  k16[useOrder-1]= WEBRTC_SPL_LSHIFT_W16(a16[useOrder], 4); //Q11<<4 => Q15

  for (m=useOrder-1; m>0; m--) {
    tmp_inv_denum32 = ((WebRtc_Word32) 1073741823) - WEBRTC_SPL_MUL_16_16(k16[m], k16[m]); // (1 - k^2) in Q30
    tmp_inv_denum16 = (WebRtc_Word16) WEBRTC_SPL_RSHIFT_W32(tmp_inv_denum32, 15); // (1 - k^2) in Q15

    for (k=1; k<=m; k++) {
      tmp32b = WEBRTC_SPL_LSHIFT_W32((WebRtc_Word32)a16[k], 16) -
          WEBRTC_SPL_LSHIFT_W32(WEBRTC_SPL_MUL_16_16(k16[m], a16[m-k+1]), 1);

      tmp32[k] = WebRtcSpl_DivW32W16(tmp32b, tmp_inv_denum16); //Q27/Q15 = Q12
    }

    for (k=1; k<m; k++) {
      a16[k] = (WebRtc_Word16) WEBRTC_SPL_RSHIFT_W32(tmp32[k], 1); //Q12>>1 => Q11
    }

    tmp32[m] = WEBRTC_SPL_SAT(4092, tmp32[m], -4092);
    k16[m-1] = (WebRtc_Word16) WEBRTC_SPL_LSHIFT_W32(tmp32[m], 3); //Q12<<3 => Q15
  }

  return;
}

void WebRtcVad_Allpass(WebRtc_Word16 *in_vector,
                       WebRtc_Word16 *out_vector,
                       WebRtc_Word16 filter_coefficients,
                       int vector_length,
                       WebRtc_Word16 *filter_state)
{
    // The filter can only cause overflow (in the w16 output variable)
    // if more than 4 consecutive input numbers are of maximum value and
    // has the the same sign as the impulse responses first taps.
    // First 6 taps of the impulse response: 0.6399 0.5905 -0.3779
    // 0.2418 -0.1547 0.0990

    int n;
    WebRtc_Word16 tmp16;
    WebRtc_Word32 tmp32, in32, state32;

    state32 = WEBRTC_SPL_LSHIFT_W32(((WebRtc_Word32)(*filter_state)), 16); // Q31

    for (n = 0; n < vector_length; n++)
    {

        tmp32 = state32 + WEBRTC_SPL_MUL_16_16(filter_coefficients, (*in_vector));
        tmp16 = (WebRtc_Word16)WEBRTC_SPL_RSHIFT_W32(tmp32, 16);
        *out_vector++ = tmp16;
        in32 = WEBRTC_SPL_LSHIFT_W32(((WebRtc_Word32)(*in_vector)), 14);
        state32 = in32 - WEBRTC_SPL_MUL_16_16(filter_coefficients, tmp16);
        state32 = WEBRTC_SPL_LSHIFT_W32(state32, 1);
        in_vector += 2;
    }

    *filter_state = (WebRtc_Word16)WEBRTC_SPL_RSHIFT_W32(state32, 16);
}

/* Filter ar_g_Q0[] and ar_f_Q0[] through an AR filter with coefficients
 * cth_Q15[] and sth_Q15[].
 */
void WebRtcIsacfix_FilterArLoop(int16_t* ar_g_Q0,     // Input samples
                                int16_t* ar_f_Q0,     // Input samples
                                int16_t* cth_Q15,     // Filter coefficients
                                int16_t* sth_Q15,     // Filter coefficients
                                int16_t order_coef) { // order of the filter
  int n = 0;

  for (n = 0; n < HALF_SUBFRAMELEN - 1; n++) {
    int k = 0;
    int16_t tmpAR = 0;
    int32_t tmp32 = 0;
    int32_t tmp32_2 = 0;

    tmpAR = ar_f_Q0[n + 1];
    for (k = order_coef - 1; k >= 0; k--) {
      tmp32 = WEBRTC_SPL_RSHIFT_W32(((WEBRTC_SPL_MUL_16_16(cth_Q15[k], tmpAR))
              - (WEBRTC_SPL_MUL_16_16(sth_Q15[k], ar_g_Q0[k])) + 16384), 15);
      tmp32_2 = WEBRTC_SPL_RSHIFT_W32(((WEBRTC_SPL_MUL_16_16(sth_Q15[k], tmpAR))
                + (WEBRTC_SPL_MUL_16_16(cth_Q15[k], ar_g_Q0[k])) + 16384), 15);
      tmpAR   = (WebRtc_Word16)WebRtcSpl_SatW32ToW16(tmp32);
      ar_g_Q0[k + 1] = (WebRtc_Word16)WebRtcSpl_SatW32ToW16(tmp32_2);
    }
    ar_f_Q0[n + 1] = tmpAR;
    ar_g_Q0[0] = tmpAR;
  }
}

void WebRtcIsacfix_PitchFilterCore(int loopNumber,
                                   int16_t gain,
                                   int index,
                                   int16_t sign,
                                   int16_t* inputState,
                                   int16_t* outputBuf2,
                                   const int16_t* coefficient,
                                   int16_t* inputBuf,
                                   int16_t* outputBuf,
                                   int* index2) {
  int i = 0, j = 0;  /* Loop counters. */
  int16_t* ubufQQpos2 = &outputBuf2[PITCH_BUFFSIZE - (index + 2)];
  int16_t tmpW16 = 0;

  for (i = 0; i < loopNumber; i++) {
    int32_t tmpW32 = 0;

    /* Filter to get fractional pitch. */
    for (j = 0; j < PITCH_FRACORDER; j++) {
      tmpW32 += WEBRTC_SPL_MUL_16_16(ubufQQpos2[*index2 + j], coefficient[j]);
    }

    /* Saturate to avoid overflow in tmpW16. */
    tmpW32 = WEBRTC_SPL_SAT(536862719, tmpW32, -536879104);
    tmpW32 += 8192;
    tmpW16 = (int16_t)WEBRTC_SPL_RSHIFT_W32(tmpW32, 14);

    /* Shift low pass filter state. */
    memmove(&inputState[1], &inputState[0],
            (PITCH_DAMPORDER - 1) * sizeof(int16_t));
    inputState[0] = (int16_t)WEBRTC_SPL_MUL_16_16_RSFT_WITH_ROUND(
                      gain, tmpW16, 12);

    /* Low pass filter. */
    tmpW32 = 0;
    /* TODO(kma): Define a static inline function WebRtcSpl_DotProduct()
       in spl_inl.h to replace this and other similar loops. */
    for (j = 0; j < PITCH_DAMPORDER; j++) {
      tmpW32 += WEBRTC_SPL_MUL_16_16(inputState[j], kDampFilter[j]);
    }

    /* Saturate to avoid overflow in tmpW16. */
    tmpW32 = WEBRTC_SPL_SAT(1073725439, tmpW32, -1073758208);
    tmpW32 += 16384;
    tmpW16 = (int16_t)WEBRTC_SPL_RSHIFT_W32(tmpW32, 15);

    /* Subtract from input and update buffer. */
    tmpW32 = inputBuf[*index2] - WEBRTC_SPL_MUL_16_16(sign, tmpW16);
    outputBuf[*index2] = WebRtcSpl_SatW32ToW16(tmpW32);
    tmpW32 = inputBuf[*index2] + outputBuf[*index2];
    outputBuf2[*index2 + PITCH_BUFFSIZE] = WebRtcSpl_SatW32ToW16(tmpW32);

    (*index2)++;
  }
}

void WebRtcIlbcfix_CbMemEnergy(int16_t range, int16_t * CB,	/* (i) The CB memory (1:st section) */
			       int16_t * filteredCB,	/* (i) The filtered CB memory (2:nd section) */
			       int16_t lMem,	/* (i) Length of the CB memory */
			       int16_t lTarget,	/* (i) Length of the target vector */
			       int16_t * energyW16,	/* (o) Energy in the CB vectors */
			       int16_t * energyShifts,	/* (o) Shift value of the energy */
			       int16_t scale,	/* (i) The scaling of all energy values */
			       int16_t base_size	/* (i) Index to where the energy values should be stored */
    )
{
	int16_t *ppi, *ppo, *pp;
	int32_t energy, tmp32;

	/* Compute the energy and store it in a vector. Also the
	 * corresponding shift values are stored. The energy values
	 * are reused in all three stages. */

	/* Calculate the energy in the first block of 'lTarget' sampels. */
	ppi = CB + lMem - lTarget - 1;
	ppo = CB + lMem - 1;

	pp = CB + lMem - lTarget;
	energy = WebRtcSpl_DotProductWithScale(pp, pp, lTarget, scale);

	/* Normalize the energy and store the number of shifts */
	energyShifts[0] = (int16_t) WebRtcSpl_NormW32(energy);
	tmp32 = WEBRTC_SPL_LSHIFT_W32(energy, energyShifts[0]);
	energyW16[0] = (int16_t) WEBRTC_SPL_RSHIFT_W32(tmp32, 16);

	/* Compute the energy of the rest of the cb memory
	 * by step wise adding and subtracting the next
	 * sample and the last sample respectively. */
	WebRtcIlbcfix_CbMemEnergyCalc(energy, range, ppi, ppo, energyW16,
				      energyShifts, scale, 0);

	/* Next, precompute the energy values for the filtered cb section */
	pp = filteredCB + lMem - lTarget;

	energy = WebRtcSpl_DotProductWithScale(pp, pp, lTarget, scale);

	/* Normalize the energy and store the number of shifts */
	energyShifts[base_size] = (int16_t) WebRtcSpl_NormW32(energy);
	tmp32 = WEBRTC_SPL_LSHIFT_W32(energy, energyShifts[base_size]);
	energyW16[base_size] = (int16_t) WEBRTC_SPL_RSHIFT_W32(tmp32, 16);

	ppi = filteredCB + lMem - 1 - lTarget;
	ppo = filteredCB + lMem - 1;

	WebRtcIlbcfix_CbMemEnergyCalc(energy, range, ppi, ppo, energyW16,
				      energyShifts, scale, base_size);
}

/* Autocorrelation function in fixed point. NOTE! Different from SPLIB-version in how it scales the signal. */
int WebRtcIsacfix_AutocorrFix(
    WebRtc_Word32          *r,
    const WebRtc_Word16 *x,
    WebRtc_Word16          N,
    WebRtc_Word16          order,
    WebRtc_Word16          *scale)
{
  int  j, i;
  WebRtc_Word16  scaling;
  WebRtc_Word32 sum, prod, newsum;
  G_CONST WebRtc_Word16    *xptr1;
  G_CONST WebRtc_Word16    *xptr2;

  sum=0;
  scaling=0;
  /* Calculate r[0] and how much scaling is needed */
  for (i=0; i < N; i++) {
    prod = WEBRTC_SPL_MUL_16_16_RSFT(x[i],x[i],scaling);
    newsum = sum+prod;
    /* If sum gets less than 0 we have overflow and need to scale the signal */
    if(newsum<0) {
      scaling++;
      sum=WEBRTC_SPL_RSHIFT_W32(sum, 1);
      prod=WEBRTC_SPL_RSHIFT_W32(prod, 1);
    }
    sum += prod;
  }
  r[0]=sum;

  /* Perform the actual correlation calculation */
  for (i = 1; i < order + 1; i++)
  {
    int loops=(N-i);
    sum = 0;
    xptr1=(G_CONST WebRtc_Word16 *)x;
    xptr2=(G_CONST WebRtc_Word16 *)&x[i];

    for (j = loops;j > 0; j--)
    {
      sum += WEBRTC_SPL_MUL_16_16_RSFT(*xptr1++,*xptr2++,scaling);
    }

    r[i] = sum;
  }

  *scale = scaling;

  return(order + 1);
}

void WebRtcIlbcfix_SortSq(
    WebRtc_Word16 *xq,   /* (o) the quantized value */
    WebRtc_Word16 *index,  /* (o) the quantization index */
    WebRtc_Word16 x,   /* (i) the value to quantize */
    const WebRtc_Word16 *cb, /* (i) the quantization codebook */
    WebRtc_Word16 cb_size  /* (i) the size of the quantization codebook */
                          ){
  int i;

  if (x <= cb[0]) {
    *index = 0;
    *xq = cb[0];
  } else {
    i = 0;
    while ((x > cb[i]) && (i < (cb_size-1))) {
      i++;
    }

    if (x > WEBRTC_SPL_RSHIFT_W32(( (WebRtc_Word32)cb[i] + cb[i - 1] + 1),1)) {
      *index = i;
      *xq = cb[i];
    } else {
      *index = i - 1;
      *xq = cb[i - 1];
    }
  }
}

int WebRtcNetEQ_RTCPUpdate(WebRtcNetEQ_RTCP_t *RTCP_inst, uint16_t uw16_seqNo,
                           uint32_t uw32_timeStamp, uint32_t uw32_recTime)
{
    int16_t w16_SeqDiff;
    int32_t w32_TimeDiff;
    int32_t w32_JitterDiff;

    /*
     * Update number of received packets, and largest packet number received.
     */
    RTCP_inst->received++;
    w16_SeqDiff = uw16_seqNo - RTCP_inst->max_seq;
    if (w16_SeqDiff >= 0)
    {
        if (uw16_seqNo < RTCP_inst->max_seq)
        {
            /* Wrap around detected */
            RTCP_inst->cycles++;
        }
        RTCP_inst->max_seq = uw16_seqNo;
    }

    /* Calculate Jitter, and update previous timestamps */
    /* Note that the value in RTCP_inst->jitter is in Q4. */
    if (RTCP_inst->received > 1)
    {
        w32_TimeDiff = (uw32_recTime - (uw32_timeStamp - RTCP_inst->transit));
        w32_TimeDiff = WEBRTC_SPL_ABS_W32(w32_TimeDiff);
        w32_JitterDiff = WEBRTC_SPL_LSHIFT_W16(w32_TimeDiff, 4) - RTCP_inst->jitter;
        RTCP_inst->jitter = RTCP_inst->jitter + WEBRTC_SPL_RSHIFT_W32((w32_JitterDiff + 8), 4);
    }
    RTCP_inst->transit = (uw32_timeStamp - uw32_recTime);
    return 0;
}

static __inline int32_t CalcLrIntQ(int32_t fixVal,
                                   int16_t qDomain) {
  int32_t intgr;
  int32_t roundVal;

  roundVal = WEBRTC_SPL_LSHIFT_W32((int32_t)1,  qDomain - 1);
  intgr = WEBRTC_SPL_RSHIFT_W32(fixVal + roundVal, qDomain);

  return intgr;
}

WebRtc_UWord32 WebRtcNetEQ_ScaleTimestampInternalToExternal(const MCUInst_t *MCU_inst,
                                                            WebRtc_UWord32 internalTS)
{
    WebRtc_Word32 timestampDiff;
    WebRtc_UWord32 externalTS;

    /* difference between this and last incoming timestamp */
    timestampDiff = (WebRtc_Word32) internalTS - MCU_inst->internalTS;

    switch (MCU_inst->scalingFactor)
    {
        case kTSscalingTwo:
        {
            /* divide by 2 */
            timestampDiff = WEBRTC_SPL_RSHIFT_W32(timestampDiff, 1);
            break;
        }
        case kTSscalingTwoThirds:
        {
            /* multiply with 3/2 */
            timestampDiff = WEBRTC_SPL_MUL_32_16(timestampDiff, 3);
            timestampDiff = WEBRTC_SPL_RSHIFT_W32(timestampDiff, 1);
            break;
        }
        case kTSscalingFourThirds:
        {
            /* multiply with 3/4 */
            timestampDiff = WEBRTC_SPL_MUL_32_16(timestampDiff, 3);
            timestampDiff = WEBRTC_SPL_RSHIFT_W32(timestampDiff, 2);
            break;
        }
        default:
        {
            /* no scaling */
        }
    }

    /* add the scaled difference to last scaled timestamp and save ... */
    externalTS = MCU_inst->externalTS + timestampDiff;

    return externalTS;
}

/* Compute the energy of the rest of the cb memory
 * by step wise adding and subtracting the next
 * sample and the last sample respectively */
void WebRtcIlbcfix_CbMemEnergyCalc(
    WebRtc_Word32 energy,   /* (i) input start energy */
    WebRtc_Word16 range,   /* (i) number of iterations */
    WebRtc_Word16 *ppi,   /* (i) input pointer 1 */
    WebRtc_Word16 *ppo,   /* (i) input pointer 2 */
    WebRtc_Word16 *energyW16,  /* (o) Energy in the CB vectors */
    WebRtc_Word16 *energyShifts, /* (o) Shift value of the energy */
    WebRtc_Word16 scale,   /* (i) The scaling of all energy values */
    WebRtc_Word16 base_size  /* (i) Index to where the energy values should be stored */
                                   )
{
  WebRtc_Word16 j,shft;
  WebRtc_Word32 tmp;
  WebRtc_Word16 *eSh_ptr;
  WebRtc_Word16 *eW16_ptr;


  eSh_ptr  = &energyShifts[1+base_size];
  eW16_ptr = &energyW16[1+base_size];

  for(j=0;j<range-1;j++) {

    /* Calculate next energy by a +/-
       operation on the edge samples */
    tmp  = WEBRTC_SPL_MUL_16_16(*ppi, *ppi);
    tmp -= WEBRTC_SPL_MUL_16_16(*ppo, *ppo);
    energy += WEBRTC_SPL_RSHIFT_W32(tmp, scale);
    energy = WEBRTC_SPL_MAX(energy, 0);

    ppi--;
    ppo--;

    /* Normalize the energy into a WebRtc_Word16 and store
       the number of shifts */

    shft = (WebRtc_Word16)WebRtcSpl_NormW32(energy);
    *eSh_ptr++ = shft;

    tmp = WEBRTC_SPL_LSHIFT_W32(energy, shft);
    *eW16_ptr++ = (WebRtc_Word16)WEBRTC_SPL_RSHIFT_W32(tmp, 16);
  }
}

static __inline int32_t Log2Q8( uint32_t x ) {

  int32_t zeros, lg2;
  int16_t frac;

  zeros=WebRtcSpl_NormU32(x);
  frac=(int16_t)WEBRTC_SPL_RSHIFT_W32(((uint32_t)(WEBRTC_SPL_LSHIFT_W32(x, zeros))&0x7FFFFFFF), 23);
  /* log2(magn(i)) */

  lg2= (WEBRTC_SPL_LSHIFT_W32((31-zeros), 8)+frac);
  return lg2;

}

static void AllpassFilterForDec32(WebRtc_Word16         *InOut16, //Q0
                                  const WebRtc_Word32   *APSectionFactors, //Q15
                                  WebRtc_Word16         lengthInOut,
                                  WebRtc_Word32          *FilterState) //Q16
{
  int n, j;
  WebRtc_Word32 a, b;

  for (j=0; j<ALLPASSSECTIONS; j++) {
    for (n=0;n<lengthInOut;n+=2){
      a = WEBRTC_SPL_MUL_16_32_RSFT16(InOut16[n], APSectionFactors[j]); //Q0*Q31=Q31 shifted 16 gives Q15
      a = WEBRTC_SPL_LSHIFT_W32(a, 1); // Q15 -> Q16
      b = WEBRTC_SPL_ADD_SAT_W32(a, FilterState[j]); //Q16+Q16=Q16
      a = WEBRTC_SPL_MUL_16_32_RSFT16(
          (WebRtc_Word16) WEBRTC_SPL_RSHIFT_W32(b, 16),
          -APSectionFactors[j]); //Q0*Q31=Q31 shifted 16 gives Q15
      FilterState[j] = WEBRTC_SPL_ADD_SAT_W32(
          WEBRTC_SPL_LSHIFT_W32(a,1),
          WEBRTC_SPL_LSHIFT_W32((WebRtc_UWord32)InOut16[n], 16)); // Q15<<1 + Q0<<16 = Q16 + Q16 = Q16
      InOut16[n] = (WebRtc_Word16) WEBRTC_SPL_RSHIFT_W32(b, 16); //Save as Q0
    }
  }
}

WebRtc_Word32 WebRtcVad_GaussianProbability(WebRtc_Word16 in_sample,
                                            WebRtc_Word16 mean,
                                            WebRtc_Word16 std,
                                            WebRtc_Word16 *delta)
{
    WebRtc_Word16 tmp16, tmpDiv, tmpDiv2, expVal, tmp16_1, tmp16_2;
    WebRtc_Word32 tmp32, y32;

    // Calculate tmpDiv=1/std, in Q10
    tmp32 = (WebRtc_Word32)WEBRTC_SPL_RSHIFT_W16(std,1) + (WebRtc_Word32)131072; // 1 in Q17
    tmpDiv = (WebRtc_Word16)WebRtcSpl_DivW32W16(tmp32, std); // Q17/Q7 = Q10

    // Calculate tmpDiv2=1/std^2, in Q14
    tmp16 = WEBRTC_SPL_RSHIFT_W16(tmpDiv, 2); // From Q10 to Q8
    tmpDiv2 = (WebRtc_Word16)WEBRTC_SPL_MUL_16_16_RSFT(tmp16, tmp16, 2); // (Q8 * Q8)>>2 = Q14

    tmp16 = WEBRTC_SPL_LSHIFT_W16(in_sample, 3); // Q7
    tmp16 = tmp16 - mean; // Q7 - Q7 = Q7

    // To be used later, when updating noise/speech model
    // delta = (x-m)/std^2, in Q11
    *delta = (WebRtc_Word16)WEBRTC_SPL_MUL_16_16_RSFT(tmpDiv2, tmp16, 10); //(Q14*Q7)>>10 = Q11

    // Calculate tmp32=(x-m)^2/(2*std^2), in Q10
    tmp32 = (WebRtc_Word32)WEBRTC_SPL_MUL_16_16_RSFT(*delta, tmp16, 9); // One shift for /2

    // Calculate expVal ~= exp(-(x-m)^2/(2*std^2)) ~= exp2(-log2(exp(1))*tmp32)
    if (tmp32 < kCompVar)
    {
        // Calculate tmp16 = log2(exp(1))*tmp32 , in Q10
        tmp16 = (WebRtc_Word16)WEBRTC_SPL_MUL_16_16_RSFT((WebRtc_Word16)tmp32,
                                                         kLog10Const, 12);
        tmp16 = -tmp16;
        tmp16_2 = (WebRtc_Word16)(0x0400 | (tmp16 & 0x03FF));
        tmp16_1 = (WebRtc_Word16)(tmp16 ^ 0xFFFF);
        tmp16 = (WebRtc_Word16)WEBRTC_SPL_RSHIFT_W16(tmp16_1, 10);
        tmp16 += 1;
        // Calculate expVal=log2(-tmp32), in Q10
        expVal = (WebRtc_Word16)WEBRTC_SPL_RSHIFT_W32((WebRtc_Word32)tmp16_2, tmp16);

    } else
    {
        expVal = 0;
    }

    // Calculate y32=(1/std)*exp(-(x-m)^2/(2*std^2)), in Q20
    y32 = WEBRTC_SPL_MUL_16_16(tmpDiv, expVal); // Q10 * Q10 = Q20

    return y32; // Q20
}

static __inline WebRtc_Word32 log2_Q8_LPC( WebRtc_UWord32 x ) {

  WebRtc_Word32 zeros, lg2;
  WebRtc_Word16 frac;

  zeros=WebRtcSpl_NormU32(x);
  frac=(WebRtc_Word16)WEBRTC_SPL_RSHIFT_W32(((WebRtc_UWord32)WEBRTC_SPL_LSHIFT_W32(x, zeros)&0x7FFFFFFF), 23);

  /* log2(x) */

  lg2= (WEBRTC_SPL_LSHIFT_W16((31-zeros), 8)+frac);
  return lg2;

}