/* 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 WebRtcSpl_LpcToReflCoef(int16_t* a16, int use_order, int16_t* k16) { int m, k; int32_t tmp32[SPL_LPC_TO_REFL_COEF_MAX_AR_MODEL_ORDER]; int32_t tmp_inv_denom32; int16_t tmp_inv_denom16; k16[use_order - 1] = a16[use_order] << 3; // Q12<<3 => Q15 for (m = use_order - 1; m > 0; m--) { // (1 - k^2) in Q30 tmp_inv_denom32 = 1073741823 - k16[m] * k16[m]; // (1 - k^2) in Q15 tmp_inv_denom16 = (int16_t)(tmp_inv_denom32 >> 15); for (k = 1; k <= m; k++) { // tmp[k] = (a[k] - RC[m] * a[m-k+1]) / (1.0 - RC[m]*RC[m]); // [Q12<<16 - (Q15*Q12)<<1] = [Q28 - Q28] = Q28 tmp32[k] = (a16[k] << 16) - (k16[m] * a16[m - k + 1] << 1); tmp32[k] = WebRtcSpl_DivW32W16(tmp32[k], tmp_inv_denom16); //Q28/Q15 = Q13 } for (k = 1; k < m; k++) { a16[k] = (int16_t)(tmp32[k] >> 1); // Q13>>1 => Q12 } tmp32[m] = WEBRTC_SPL_SAT(8191, tmp32[m], -8191); k16[m - 1] = (int16_t)WEBRTC_SPL_LSHIFT_W32(tmp32[m], 2); //Q13<<2 => Q15 } return; }
WebRtc_UWord32 WebRtcNetEQ_ScaleTimestampExternalToInternal(const MCUInst_t *MCU_inst, WebRtc_UWord32 externalTS) { WebRtc_Word32 timestampDiff; WebRtc_UWord32 internalTS; /* difference between this and last incoming timestamp */ timestampDiff = externalTS - MCU_inst->externalTS; switch (MCU_inst->scalingFactor) { case kTSscalingTwo: { /* multiply with 2 */ timestampDiff = WEBRTC_SPL_LSHIFT_W32(timestampDiff, 1); break; } case kTSscalingTwoThirds: { /* multiply with 2/3 */ timestampDiff = WEBRTC_SPL_LSHIFT_W32(timestampDiff, 1); timestampDiff = WebRtcSpl_DivW32W16(timestampDiff, 3); break; } case kTSscalingFourThirds: { /* multiply with 4/3 */ timestampDiff = WEBRTC_SPL_LSHIFT_W32(timestampDiff, 2); timestampDiff = WebRtcSpl_DivW32W16(timestampDiff, 3); break; } default: { /* no scaling */ } } /* add the scaled difference to last scaled timestamp and save ... */ internalTS = MCU_inst->internalTS + timestampDiff; return internalTS; }
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 }
void WebRtcSpl_GetHanningWindow(int16_t *v, size_t size) { size_t jj; int16_t *vptr1; int32_t index; int32_t factor = ((int32_t)0x40000000); factor = WebRtcSpl_DivW32W16(factor, (int16_t)size); if (size < 513) index = (int32_t)-0x200000; else index = (int32_t)-0x100000; vptr1 = v; for (jj = 0; jj < size; jj++) { index += factor; (*vptr1++) = kHanningTable[index >> 22]; } }
void WebRtcIlbcfix_EnergyInverse( WebRtc_Word16 *energy, /* (i/o) Energy and inverse energy (in Q29) */ int noOfEnergies) /* (i) The length of the energy vector */ { WebRtc_Word32 Nom=(WebRtc_Word32)0x1FFFFFFF; WebRtc_Word16 *energyPtr; int i; /* Set the minimum energy value to 16384 to avoid overflow */ energyPtr=energy; for (i=0; i<noOfEnergies; i++) { (*energyPtr)=WEBRTC_SPL_MAX((*energyPtr),16384); energyPtr++; } /* Calculate inverse energy in Q29 */ energyPtr=energy; for (i=0; i<noOfEnergies; i++) { (*energyPtr) = (WebRtc_Word16)WebRtcSpl_DivW32W16(Nom, (*energyPtr)); energyPtr++; } }
int WebRtcNetEQ_UpdateIatStatistics(AutomodeInst_t *inst, int maxBufLen, uint16_t seqNumber, uint32_t timeStamp, int32_t fsHz, int mdCodec, int streamingMode) { uint32_t timeIat; /* inter-arrival time */ int i; int32_t tempsum = 0; /* temp summation */ int32_t tempvar; /* temporary variable */ int retval = 0; /* return value */ int16_t packetLenSamp; /* packet speech length in samples */ /****************/ /* Sanity check */ /****************/ if (maxBufLen <= 1 || fsHz <= 0) { /* maxBufLen must be at least 2 and fsHz must both be strictly positive */ return -1; } /****************************/ /* Update packet statistics */ /****************************/ /* Try calculating packet length from current and previous timestamps */ if (!WebRtcNetEQ_IsNewerTimestamp(timeStamp, inst->lastTimeStamp) || !WebRtcNetEQ_IsNewerSequenceNumber(seqNumber, inst->lastSeqNo)) { /* Wrong timestamp or sequence order; revert to backup plan */ packetLenSamp = inst->packetSpeechLenSamp; /* use stored value */ } else { /* calculate timestamps per packet */ packetLenSamp = (int16_t) WebRtcSpl_DivU32U16(timeStamp - inst->lastTimeStamp, seqNumber - inst->lastSeqNo); } /* Check that the packet size is positive; if not, the statistics cannot be updated. */ if (inst->firstPacketReceived && packetLenSamp > 0) { /* packet size ok */ /* calculate inter-arrival time in integer packets (rounding down) */ timeIat = WebRtcSpl_DivW32W16(inst->packetIatCountSamp, packetLenSamp); /* Special operations for streaming mode */ if (streamingMode != 0) { /* * Calculate IAT in Q8, including fractions of a packet (i.e., more accurate * than timeIat). */ int16_t timeIatQ8 = (int16_t) WebRtcSpl_DivW32W16( WEBRTC_SPL_LSHIFT_W32(inst->packetIatCountSamp, 8), packetLenSamp); /* * Calculate cumulative sum iat with sequence number compensation (ideal arrival * times makes this sum zero). */ inst->cSumIatQ8 += (timeIatQ8 - WEBRTC_SPL_LSHIFT_W32(seqNumber - inst->lastSeqNo, 8)); /* subtract drift term */ inst->cSumIatQ8 -= CSUM_IAT_DRIFT; /* ensure not negative */ inst->cSumIatQ8 = WEBRTC_SPL_MAX(inst->cSumIatQ8, 0); /* remember max */ if (inst->cSumIatQ8 > inst->maxCSumIatQ8) { inst->maxCSumIatQ8 = inst->cSumIatQ8; inst->maxCSumUpdateTimer = 0; } /* too long since the last maximum was observed; decrease max value */ if (inst->maxCSumUpdateTimer > (uint32_t) WEBRTC_SPL_MUL_32_16(fsHz, MAX_STREAMING_PEAK_PERIOD)) { inst->maxCSumIatQ8 -= 4; /* remove 1000*4/256 = 15.6 ms/s */ } } /* end of streaming mode */ /* check for discontinuous packet sequence and re-ordering */ if (WebRtcNetEQ_IsNewerSequenceNumber(seqNumber, inst->lastSeqNo + 1)) { /* Compensate for gap in the sequence numbers. * Reduce IAT with expected extra time due to lost packets, but ensure that * the IAT is not negative. */ timeIat -= WEBRTC_SPL_MIN(timeIat, (uint16_t) (seqNumber - (uint16_t) (inst->lastSeqNo + 1))); } else if (!WebRtcNetEQ_IsNewerSequenceNumber(seqNumber, inst->lastSeqNo)) { /* compensate for re-ordering */ timeIat += (uint16_t) (inst->lastSeqNo + 1 - seqNumber); } /* saturate IAT at maximum value */ timeIat = WEBRTC_SPL_MIN( timeIat, MAX_IAT ); /* update iatProb = forgetting_factor * iatProb for all elements */ for (i = 0; i <= MAX_IAT; i++) { int32_t tempHi, tempLo; /* Temporary variables */ /* * Multiply iatProbFact (Q15) with iatProb (Q30) and right-shift 15 steps * to come back to Q30. The operation is done in two steps: */ /* * 1) Multiply the high 16 bits (15 bits + sign) of iatProb. Shift iatProb * 16 steps right to get the high 16 bits in a int16_t prior to * multiplication, and left-shift with 1 afterwards to come back to * Q30 = (Q15 * (Q30>>16)) << 1. */ tempHi = WEBRTC_SPL_MUL_16_16(inst->iatProbFact, (int16_t) WEBRTC_SPL_RSHIFT_W32(inst->iatProb[i], 16)); tempHi = WEBRTC_SPL_LSHIFT_W32(tempHi, 1); /* left-shift 1 step */ /* * 2) Isolate and multiply the low 16 bits of iatProb. Right-shift 15 steps * afterwards to come back to Q30 = (Q15 * Q30) >> 15. */ tempLo = inst->iatProb[i] & 0x0000FFFF; /* sift out the 16 low bits */ tempLo = WEBRTC_SPL_MUL_16_U16(inst->iatProbFact, (uint16_t) tempLo); tempLo = WEBRTC_SPL_RSHIFT_W32(tempLo, 15); /* Finally, add the high and low parts */ inst->iatProb[i] = tempHi + tempLo; /* Sum all vector elements while we are at it... */ tempsum += inst->iatProb[i]; } /* * Increase the probability for the currently observed inter-arrival time * with 1 - iatProbFact. The factor is in Q15, iatProb in Q30; * hence, left-shift 15 steps to obtain result in Q30. */ inst->iatProb[timeIat] += (32768 - inst->iatProbFact) << 15; tempsum += (32768 - inst->iatProbFact) << 15; /* add to vector sum */ /* * Update iatProbFact (changes only during the first seconds after reset) * The factor converges to IAT_PROB_FACT. */ inst->iatProbFact += (IAT_PROB_FACT - inst->iatProbFact + 3) >> 2; /* iatProb should sum up to 1 (in Q30). */ tempsum -= 1 << 30; /* should be zero */ /* Check if it does, correct if it doesn't. */ if (tempsum > 0) { /* tempsum too large => decrease a few values in the beginning */ i = 0; while (i <= MAX_IAT && tempsum > 0) { /* Remove iatProb[i] / 16 from iatProb, but not more than tempsum */ tempvar = WEBRTC_SPL_MIN(tempsum, inst->iatProb[i] >> 4); inst->iatProb[i++] -= tempvar; tempsum -= tempvar; } }
/* MA filter */ void WebRtcIsacfix_NormLatticeFilterMa(int16_t orderCoef, int32_t *stateGQ15, int16_t *lat_inQ0, int16_t *filt_coefQ15, int32_t *gain_lo_hiQ17, int16_t lo_hi, int16_t *lat_outQ9) { int16_t sthQ15[MAX_AR_MODEL_ORDER]; int16_t cthQ15[MAX_AR_MODEL_ORDER]; int u, i, k, n; int16_t temp2,temp3; int16_t ord_1 = orderCoef+1; int32_t inv_cthQ16[MAX_AR_MODEL_ORDER]; int32_t gain32, fQtmp; int16_t gain16; int16_t gain_sh; int32_t tmp32, tmp32b; int32_t fQ15vec[HALF_SUBFRAMELEN]; int32_t gQ15[MAX_AR_MODEL_ORDER+1][HALF_SUBFRAMELEN]; int16_t sh; int16_t t16a; int16_t t16b; for (u=0;u<SUBFRAMES;u++) { int32_t temp1 = WEBRTC_SPL_MUL_16_16(u, HALF_SUBFRAMELEN); /* set the Direct Form coefficients */ temp2 = (int16_t)WEBRTC_SPL_MUL_16_16(u, orderCoef); temp3 = (int16_t)WEBRTC_SPL_MUL_16_16(2, u)+lo_hi; /* compute lattice filter coefficients */ memcpy(sthQ15, &filt_coefQ15[temp2], orderCoef * sizeof(int16_t)); WebRtcSpl_SqrtOfOneMinusXSquared(sthQ15, orderCoef, cthQ15); /* compute the gain */ gain32 = gain_lo_hiQ17[temp3]; gain_sh = WebRtcSpl_NormW32(gain32); gain32 = WEBRTC_SPL_LSHIFT_W32(gain32, gain_sh); //Q(17+gain_sh) for (k=0;k<orderCoef;k++) { gain32 = WEBRTC_SPL_MUL_16_32_RSFT15(cthQ15[k], gain32); //Q15*Q(17+gain_sh)>>15 = Q(17+gain_sh) inv_cthQ16[k] = WebRtcSpl_DivW32W16((int32_t)2147483647, cthQ15[k]); // 1/cth[k] in Q31/Q15 = Q16 } gain16 = (int16_t)(gain32 >> 16); // Q(1+gain_sh). /* normalized lattice filter */ /*****************************/ /* initial conditions */ for (i=0;i<HALF_SUBFRAMELEN;i++) { fQ15vec[i] = WEBRTC_SPL_LSHIFT_W32((int32_t)lat_inQ0[i + temp1], 15); //Q15 gQ15[0][i] = WEBRTC_SPL_LSHIFT_W32((int32_t)lat_inQ0[i + temp1], 15); //Q15 } fQtmp = fQ15vec[0]; /* get the state of f&g for the first input, for all orders */ for (i=1;i<ord_1;i++) { // Calculate f[i][0] = inv_cth[i-1]*(f[i-1][0] + sth[i-1]*stateG[i-1]); tmp32 = WEBRTC_SPL_MUL_16_32_RSFT15(sthQ15[i-1], stateGQ15[i-1]);//Q15*Q15>>15 = Q15 tmp32b= fQtmp + tmp32; //Q15+Q15=Q15 tmp32 = inv_cthQ16[i-1]; //Q16 t16a = (int16_t)(tmp32 >> 16); t16b = (int16_t) (tmp32-WEBRTC_SPL_LSHIFT_W32(((int32_t)t16a), 16)); if (t16b<0) t16a++; tmp32 = LATTICE_MUL_32_32_RSFT16(t16a, t16b, tmp32b); fQtmp = tmp32; // Q15 // Calculate g[i][0] = cth[i-1]*stateG[i-1] + sth[i-1]* f[i][0]; tmp32 = WEBRTC_SPL_MUL_16_32_RSFT15(cthQ15[i-1], stateGQ15[i-1]); //Q15*Q15>>15 = Q15 tmp32b = WEBRTC_SPL_MUL_16_32_RSFT15(sthQ15[i-1], fQtmp); //Q15*Q15>>15 = Q15 tmp32 = tmp32 + tmp32b;//Q15+Q15 = Q15 gQ15[i][0] = tmp32; // Q15 } /* filtering */ /* save the states */ for(k=0;k<orderCoef;k++) { // for 0 <= n < HALF_SUBFRAMELEN - 1: // f[k+1][n+1] = inv_cth[k]*(f[k][n+1] + sth[k]*g[k][n]); // g[k+1][n+1] = cth[k]*g[k][n] + sth[k]* f[k+1][n+1]; WebRtcIsacfix_FilterMaLoopFix(sthQ15[k], cthQ15[k], inv_cthQ16[k], &gQ15[k][0], &gQ15[k+1][1], &fQ15vec[1]); } fQ15vec[0] = fQtmp; for(n=0;n<HALF_SUBFRAMELEN;n++) { //gain32 >>= gain_sh; // Q(17+gain_sh) -> Q17 tmp32 = WEBRTC_SPL_MUL_16_32_RSFT16(gain16, fQ15vec[n]); //Q(1+gain_sh)*Q15>>16 = Q(gain_sh) sh = 9-gain_sh; //number of needed shifts to reach Q9 t16a = (int16_t) WEBRTC_SPL_SHIFT_W32(tmp32, sh); lat_outQ9[n + temp1] = t16a; } /* save the states */ for (i=0;i<ord_1;i++) { stateGQ15[i] = gQ15[i][HALF_SUBFRAMELEN-1]; } //process next frame } return; }
/* filter the signal using normalized lattice filter */ void WebRtcIsacfix_NormLatticeFilterAr(int16_t orderCoef, int16_t *stateGQ0, int32_t *lat_inQ25, int16_t *filt_coefQ15, int32_t *gain_lo_hiQ17, int16_t lo_hi, int16_t *lat_outQ0) { int ii,n,k,i,u; int16_t sthQ15[MAX_AR_MODEL_ORDER]; int16_t cthQ15[MAX_AR_MODEL_ORDER]; int32_t tmp32; int16_t tmpAR; int16_t ARfQ0vec[HALF_SUBFRAMELEN]; int16_t ARgQ0vec[MAX_AR_MODEL_ORDER+1]; int32_t inv_gain32; int16_t inv_gain16; int16_t den16; int16_t sh; int16_t temp2,temp3; int16_t ord_1 = orderCoef+1; for (u=0;u<SUBFRAMES;u++) { int32_t temp1 = WEBRTC_SPL_MUL_16_16(u, HALF_SUBFRAMELEN); //set the denominator and numerator of the Direct Form temp2 = (int16_t)WEBRTC_SPL_MUL_16_16(u, orderCoef); temp3 = (int16_t)WEBRTC_SPL_MUL_16_16(2, u) + lo_hi; for (ii=0; ii<orderCoef; ii++) { sthQ15[ii] = filt_coefQ15[temp2+ii]; } WebRtcSpl_SqrtOfOneMinusXSquared(sthQ15, orderCoef, cthQ15); /* Simulation of the 25 files shows that maximum value in the vector gain_lo_hiQ17[] is 441344, which means that it is log2((2^31)/441344) = 12.2 shifting bits from saturation. Therefore, it should be safe to use Q27 instead of Q17. */ tmp32 = WEBRTC_SPL_LSHIFT_W32(gain_lo_hiQ17[temp3], 10); // Q27 for (k=0;k<orderCoef;k++) { tmp32 = WEBRTC_SPL_MUL_16_32_RSFT15(cthQ15[k], tmp32); // Q15*Q27>>15 = Q27 } sh = WebRtcSpl_NormW32(tmp32); // tmp32 is the gain den16 = (int16_t) WEBRTC_SPL_SHIFT_W32(tmp32, sh-16); //Q(27+sh-16) = Q(sh+11) (all 16 bits are value bits) inv_gain32 = WebRtcSpl_DivW32W16((int32_t)2147483647, den16); // 1/gain in Q31/Q(sh+11) = Q(20-sh) //initial conditions inv_gain16 = (int16_t)(inv_gain32 >> 2); // 1/gain in Q(20-sh-2) = Q(18-sh) for (i=0;i<HALF_SUBFRAMELEN;i++) { tmp32 = WEBRTC_SPL_LSHIFT_W32(lat_inQ25[i + temp1], 1); //Q25->Q26 tmp32 = WEBRTC_SPL_MUL_16_32_RSFT16(inv_gain16, tmp32); //lat_in[]*inv_gain in (Q(18-sh)*Q26)>>16 = Q(28-sh) tmp32 = WEBRTC_SPL_SHIFT_W32(tmp32, -(28-sh)); // lat_in[]*inv_gain in Q0 ARfQ0vec[i] = (int16_t)WebRtcSpl_SatW32ToW16(tmp32); // Q0 } for (i=orderCoef-1;i>=0;i--) //get the state of f&g for the first input, for all orders { tmp32 = (cthQ15[i] * ARfQ0vec[0] - sthQ15[i] * stateGQ0[i] + 16384) >> 15; tmpAR = (int16_t)WebRtcSpl_SatW32ToW16(tmp32); // Q0 tmp32 = (sthQ15[i] * ARfQ0vec[0] + cthQ15[i] * stateGQ0[i] + 16384) >> 15; ARgQ0vec[i+1] = (int16_t)WebRtcSpl_SatW32ToW16(tmp32); // Q0 ARfQ0vec[0] = tmpAR; } ARgQ0vec[0] = ARfQ0vec[0]; // Filter ARgQ0vec[] and ARfQ0vec[] through coefficients cthQ15[] and sthQ15[]. WebRtcIsacfix_FilterArLoop(ARgQ0vec, ARfQ0vec, cthQ15, sthQ15, orderCoef); for(n=0;n<HALF_SUBFRAMELEN;n++) { lat_outQ0[n + temp1] = ARfQ0vec[n]; } /* cannot use memcpy in the following */ for (i=0;i<ord_1;i++) { stateGQ0[i] = ARgQ0vec[i]; } } return; }
/* filter the signal using normalized lattice filter */ void WebRtcIsacfix_NormLatticeFilterAr(size_t orderCoef, int16_t *stateGQ0, int32_t *lat_inQ25, int16_t *filt_coefQ15, int32_t *gain_lo_hiQ17, int16_t lo_hi, int16_t *lat_outQ0) { size_t ii, k, i; int n, u; int16_t sthQ15[MAX_AR_MODEL_ORDER]; int16_t cthQ15[MAX_AR_MODEL_ORDER]; int32_t tmp32; int16_t tmpAR; int16_t ARfQ0vec[HALF_SUBFRAMELEN]; int16_t ARgQ0vec[MAX_AR_MODEL_ORDER+1]; int32_t inv_gain32; int16_t inv_gain16; int16_t den16; int16_t sh; int16_t temp2,temp3; size_t ord_1 = orderCoef+1; for (u=0;u<SUBFRAMES;u++) { int32_t temp1 = u * HALF_SUBFRAMELEN; //set the denominator and numerator of the Direct Form temp2 = (int16_t)(u * orderCoef); temp3 = (int16_t)(2 * u + lo_hi); for (ii=0; ii<orderCoef; ii++) { sthQ15[ii] = filt_coefQ15[temp2+ii]; } WebRtcSpl_SqrtOfOneMinusXSquared(sthQ15, orderCoef, cthQ15); // Originally, this line was assumed to never overflow, since "[s]imulation // of the 25 files shows that maximum value in the vector gain_lo_hiQ17[] // is 441344, which means that it is log2((2^31)/441344) = 12.2 shifting // bits from saturation. Therefore, it should be safe to use Q27 instead of // Q17." However, a fuzzer test succeeded in provoking an overflow here, // which we ignore on the theory that only "abnormal" inputs cause // overflow. tmp32 = OverflowingLShiftS32(gain_lo_hiQ17[temp3], 10); // Q27 for (k=0;k<orderCoef;k++) { tmp32 = WEBRTC_SPL_MUL_16_32_RSFT15(cthQ15[k], tmp32); // Q15*Q27>>15 = Q27 } sh = WebRtcSpl_NormW32(tmp32); // tmp32 is the gain den16 = (int16_t) WEBRTC_SPL_SHIFT_W32(tmp32, sh-16); //Q(27+sh-16) = Q(sh+11) (all 16 bits are value bits) inv_gain32 = WebRtcSpl_DivW32W16((int32_t)2147483647, den16); // 1/gain in Q31/Q(sh+11) = Q(20-sh) //initial conditions inv_gain16 = (int16_t)(inv_gain32 >> 2); // 1/gain in Q(20-sh-2) = Q(18-sh) for (i=0;i<HALF_SUBFRAMELEN;i++) { tmp32 = OverflowingLShiftS32(lat_inQ25[i + temp1], 1); // Q25->Q26 tmp32 = WEBRTC_SPL_MUL_16_32_RSFT16(inv_gain16, tmp32); //lat_in[]*inv_gain in (Q(18-sh)*Q26)>>16 = Q(28-sh) tmp32 = WEBRTC_SPL_SHIFT_W32(tmp32, -(28-sh)); // lat_in[]*inv_gain in Q0 ARfQ0vec[i] = (int16_t)WebRtcSpl_SatW32ToW16(tmp32); // Q0 } // Get the state of f & g for the first input, for all orders. for (i = orderCoef; i > 0; i--) { tmp32 = (cthQ15[i - 1] * ARfQ0vec[0] - sthQ15[i - 1] * stateGQ0[i - 1] + 16384) >> 15; tmpAR = (int16_t)WebRtcSpl_SatW32ToW16(tmp32); // Q0 tmp32 = (sthQ15[i - 1] * ARfQ0vec[0] + cthQ15[i - 1] * stateGQ0[i - 1] + 16384) >> 15; ARgQ0vec[i] = (int16_t)WebRtcSpl_SatW32ToW16(tmp32); // Q0 ARfQ0vec[0] = tmpAR; } ARgQ0vec[0] = ARfQ0vec[0]; // Filter ARgQ0vec[] and ARfQ0vec[] through coefficients cthQ15[] and sthQ15[]. WebRtcIsacfix_FilterArLoop(ARgQ0vec, ARfQ0vec, cthQ15, sthQ15, orderCoef); for(n=0;n<HALF_SUBFRAMELEN;n++) { lat_outQ0[n + temp1] = ARfQ0vec[n]; } /* cannot use memcpy in the following */ for (i=0;i<ord_1;i++) { stateGQ0[i] = ARgQ0vec[i]; } } return; }
void WebRtcIlbcfix_Poly2Lsp( int16_t *a, /* (o) A coefficients in Q12 */ int16_t *lsp, /* (i) LSP coefficients in Q15 */ int16_t *old_lsp /* (i) old LSP coefficients that are used if the new coefficients turn out to be unstable */ ) { int16_t f[2][6]; /* f[0][] represents f1 and f[1][] represents f2 */ int16_t *a_i_ptr, *a_10mi_ptr; int16_t *f1ptr, *f2ptr; int32_t tmpW32; int16_t x, y, xlow, ylow, xmid, ymid, xhigh, yhigh, xint; int16_t shifts, sign; int i, j; int foundFreqs; int fi_select; /* Calculate the two polynomials f1(z) and f2(z) (the sum and the diff polynomial) f1[0] = f2[0] = 1.0; f1[i+1] = a[i+1] + a[10-i] - f1[i]; f2[i+1] = a[i+1] - a[10-i] - f1[i]; */ a_i_ptr = a + 1; a_10mi_ptr = a + 10; f1ptr = f[0]; f2ptr = f[1]; (*f1ptr) = 1024; /* 1.0 in Q10 */ (*f2ptr) = 1024; /* 1.0 in Q10 */ for (i = 0; i < 5; i++) { (*(f1ptr+1)) = (int16_t)(WEBRTC_SPL_RSHIFT_W32(((int32_t)(*a_i_ptr)+(*a_10mi_ptr)), 2) - (*f1ptr)); (*(f2ptr+1)) = (int16_t)(WEBRTC_SPL_RSHIFT_W32(((int32_t)(*a_i_ptr)-(*a_10mi_ptr)), 2) + (*f2ptr)); a_i_ptr++; a_10mi_ptr--; f1ptr++; f2ptr++; } /* find the LSPs using the Chebychev pol. evaluation */ fi_select = 0; /* selector between f1 and f2, start with f1 */ foundFreqs = 0; xlow = WebRtcIlbcfix_kCosGrid[0]; ylow = WebRtcIlbcfix_Chebyshev(xlow, f[fi_select]); /* Iterate until all the 10 LSP's have been found or all the grid points have been tried. If the 10 LSP's can not be found, set the LSP vector to previous LSP */ for (j = 1; j < COS_GRID_POINTS && foundFreqs < 10; j++) { xhigh = xlow; yhigh = ylow; xlow = WebRtcIlbcfix_kCosGrid[j]; ylow = WebRtcIlbcfix_Chebyshev(xlow, f[fi_select]); if (WEBRTC_SPL_MUL_16_16(ylow, yhigh) <= 0) { /* Run 4 times to reduce the interval */ for (i = 0; i < 4; i++) { /* xmid =(xlow + xhigh)/2 */ xmid = WEBRTC_SPL_RSHIFT_W16(xlow, 1) + WEBRTC_SPL_RSHIFT_W16(xhigh, 1); ymid = WebRtcIlbcfix_Chebyshev(xmid, f[fi_select]); if (WEBRTC_SPL_MUL_16_16(ylow, ymid) <= 0) { yhigh = ymid; xhigh = xmid; } else { ylow = ymid; xlow = xmid; } } /* Calculater xint by linear interpolation: xint = xlow - ylow*(xhigh-xlow)/(yhigh-ylow); */ x = xhigh - xlow; y = yhigh - ylow; if (y == 0) { xint = xlow; } else { sign = y; y = WEBRTC_SPL_ABS_W16(y); shifts = (int16_t)WebRtcSpl_NormW32(y)-16; y = WEBRTC_SPL_LSHIFT_W16(y, shifts); y = (int16_t)WebRtcSpl_DivW32W16(536838144, y); /* 1/(yhigh-ylow) */ tmpW32 = WEBRTC_SPL_MUL_16_16_RSFT(x, y, (19-shifts)); /* y=(xhigh-xlow)/(yhigh-ylow) */ y = (int16_t)(tmpW32&0xFFFF); if (sign < 0) { y = -y; } /* tmpW32 = ylow*(xhigh-xlow)/(yhigh-ylow) */ tmpW32 = WEBRTC_SPL_MUL_16_16_RSFT(ylow, y, 10); xint = xlow-(int16_t)(tmpW32&0xFFFF); } /* Store the calculated lsp */ lsp[foundFreqs] = (int16_t)xint; foundFreqs++; /* if needed, set xlow and ylow for next recursion */ if (foundFreqs<10) { xlow = xint; /* Swap between f1 and f2 (f[0][] and f[1][]) */ fi_select = ((fi_select+1)&0x1); ylow = WebRtcIlbcfix_Chebyshev(xlow, f[fi_select]); } } } /* Check if M roots found, if not then use the old LSP */ if (foundFreqs < 10) { WEBRTC_SPL_MEMCPY_W16(lsp, old_lsp, 10); } return; }