/* 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,
size_t 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 += ubufQQpos2[*index2 + j] * coefficient[j];
}
/* Saturate to avoid overflow in tmpW16. */
tmpW32 = WEBRTC_SPL_SAT(536862719, tmpW32, -536879104);
tmpW32 += 8192;
tmpW16 = (int16_t)(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 += inputState[j] * kDampFilter[j];
}
/* Saturate to avoid overflow in tmpW16. */
tmpW32 = WEBRTC_SPL_SAT(1073725439, tmpW32, -1073758208);
tmpW32 += 16384;
tmpW16 = (int16_t)(tmpW32 >> 15);
/* Subtract from input and update buffer. */
tmpW32 = inputBuf[*index2] - sign * tmpW16;
outputBuf[*index2] = WebRtcSpl_SatW32ToW16(tmpW32);
tmpW32 = inputBuf[*index2] + outputBuf[*index2];
outputBuf2[*index2 + PITCH_BUFFSIZE] = WebRtcSpl_SatW32ToW16(tmpW32);
(*index2)++;
}
}
void WebRtcSpl_SynthesisQMF(const int16_t* low_band, const int16_t* high_band,
size_t band_length, int16_t* out_data,
int32_t* filter_state1, int32_t* filter_state2)
{
int32_t tmp;
int32_t half_in1[kMaxBandFrameLength];
int32_t half_in2[kMaxBandFrameLength];
int32_t filter1[kMaxBandFrameLength];
int32_t filter2[kMaxBandFrameLength];
size_t i;
int16_t k;
assert(band_length <= kMaxBandFrameLength);
// Obtain the sum and difference channels out of upper and lower-band channels.
// Also shift to Q10 domain.
for (i = 0; i < band_length; i++)
{
tmp = (int32_t)low_band[i] + (int32_t)high_band[i];
half_in1[i] = WEBRTC_SPL_LSHIFT_W32(tmp, 10);
tmp = (int32_t)low_band[i] - (int32_t)high_band[i];
half_in2[i] = WEBRTC_SPL_LSHIFT_W32(tmp, 10);
}
// all-pass filter the sum and difference channels
WebRtcSpl_AllPassQMF(half_in1, band_length, filter1,
WebRtcSpl_kAllPassFilter2, filter_state1);
WebRtcSpl_AllPassQMF(half_in2, band_length, filter2,
WebRtcSpl_kAllPassFilter1, filter_state2);
// The filtered signals are even and odd samples of the output. Combine
// them. The signals are Q10 should shift them back to Q0 and take care of
// saturation.
for (i = 0, k = 0; i < band_length; i++)
{
tmp = (filter2[i] + 512) >> 10;
out_data[k++] = WebRtcSpl_SatW32ToW16(tmp);
tmp = (filter1[i] + 512) >> 10;
out_data[k++] = WebRtcSpl_SatW32ToW16(tmp);
}
}
void WebRtcSpl_AnalysisQMF(const int16_t* in_data, size_t in_data_length,
int16_t* low_band, int16_t* high_band,
int32_t* filter_state1, int32_t* filter_state2)
{
size_t i;
int16_t k;
int32_t tmp;
int32_t half_in1[kMaxBandFrameLength];
int32_t half_in2[kMaxBandFrameLength];
int32_t filter1[kMaxBandFrameLength];
int32_t filter2[kMaxBandFrameLength];
const size_t band_length = in_data_length / 2;
assert(in_data_length % 2 == 0);
assert(band_length <= kMaxBandFrameLength);
// Split even and odd samples. Also shift them to Q10.
for (i = 0, k = 0; i < band_length; i++, k += 2)
{
half_in2[i] = WEBRTC_SPL_LSHIFT_W32((int32_t)in_data[k], 10);
half_in1[i] = WEBRTC_SPL_LSHIFT_W32((int32_t)in_data[k + 1], 10);
}
// All pass filter even and odd samples, independently.
WebRtcSpl_AllPassQMF(half_in1, band_length, filter1,
WebRtcSpl_kAllPassFilter1, filter_state1);
WebRtcSpl_AllPassQMF(half_in2, band_length, filter2,
WebRtcSpl_kAllPassFilter2, filter_state2);
// Take the sum and difference of filtered version of odd and even
// branches to get upper & lower band.
for (i = 0; i < band_length; i++)
{
tmp = (filter1[i] + filter2[i] + 1024) >> 11;
low_band[i] = WebRtcSpl_SatW32ToW16(tmp);
tmp = (filter1[i] - filter2[i] + 1024) >> 11;
high_band[i] = WebRtcSpl_SatW32ToW16(tmp);
}
}
int WebRtcSpl_DownsampleFast(WebRtc_Word16 *in_ptr, WebRtc_Word16 in_length,
WebRtc_Word16 *out_ptr, WebRtc_Word16 out_length,
WebRtc_Word16 *B, WebRtc_Word16 B_length, WebRtc_Word16 factor,
WebRtc_Word16 delay)
{
WebRtc_Word32 o;
int i, j;
WebRtc_Word16 *downsampled_ptr = out_ptr;
WebRtc_Word16 *b_ptr;
WebRtc_Word16 *x_ptr;
WebRtc_Word16 endpos = delay
+ (WebRtc_Word16)WEBRTC_SPL_MUL_16_16(factor, (out_length - 1)) + 1;
if (in_length < endpos)
{
return -1;
}
for (i = delay; i < endpos; i += factor)
{
b_ptr = &B[0];
x_ptr = &in_ptr[i];
o = (WebRtc_Word32)2048; // Round val
for (j = 0; j < B_length; j++)
{
o += WEBRTC_SPL_MUL_16_16(*b_ptr++, *x_ptr--);
}
o = WEBRTC_SPL_RSHIFT_W32(o, 12);
// If output is higher than 32768, saturate it. Same with negative side
*downsampled_ptr++ = WebRtcSpl_SatW32ToW16(o);
}
return 0;
}
// Update the noise estimation information.
static void UpdateNoiseEstimateNeon(NsxInst_t* inst, int offset) {
// int i = 0;
const int16_t kExp2Const = 11819; // Q13
int16_t* ptr_noiseEstLogQuantile = NULL;
int16_t* ptr_noiseEstQuantile = NULL;
int16x4_t kExp2Const16x4 = vdup_n_s16(kExp2Const);
int32x4_t twentyOne32x4 = vdupq_n_s32(21);
int32x4_t constA32x4 = vdupq_n_s32(0x1fffff);
int32x4_t constB32x4 = vdupq_n_s32(0x200000);
int16_t tmp16 = WebRtcSpl_MaxValueW16(inst->noiseEstLogQuantile + offset,
inst->magnLen);
// Guarantee a Q-domain as high as possible and still fit in int16
inst->qNoise = 14 - (int) WEBRTC_SPL_MUL_16_16_RSFT_WITH_ROUND(kExp2Const,
tmp16,
21);
int32x4_t qNoise32x4 = vdupq_n_s32(inst->qNoise);
for (ptr_noiseEstLogQuantile = &inst->noiseEstLogQuantile[offset],
ptr_noiseEstQuantile = &inst->noiseEstQuantile[0];
ptr_noiseEstQuantile < &inst->noiseEstQuantile[inst->magnLen - 3];
ptr_noiseEstQuantile += 4, ptr_noiseEstLogQuantile += 4) {
// tmp32no2 = WEBRTC_SPL_MUL_16_16(kExp2Const,
// inst->noiseEstLogQuantile[offset + i]);
int16x4_t v16x4 = vld1_s16(ptr_noiseEstLogQuantile);
int32x4_t v32x4B = vmull_s16(v16x4, kExp2Const16x4);
// tmp32no1 = (0x00200000 | (tmp32no2 & 0x001FFFFF)); // 2^21 + frac
int32x4_t v32x4A = vandq_s32(v32x4B, constA32x4);
v32x4A = vorrq_s32(v32x4A, constB32x4);
// tmp16 = (int16_t) WEBRTC_SPL_RSHIFT_W32(tmp32no2, 21);
v32x4B = vshrq_n_s32(v32x4B, 21);
// tmp16 -= 21;// shift 21 to get result in Q0
v32x4B = vsubq_s32(v32x4B, twentyOne32x4);
// tmp16 += (int16_t) inst->qNoise;
// shift to get result in Q(qNoise)
v32x4B = vaddq_s32(v32x4B, qNoise32x4);
// if (tmp16 < 0) {
// tmp32no1 = WEBRTC_SPL_RSHIFT_W32(tmp32no1, -tmp16);
// } else {
// tmp32no1 = WEBRTC_SPL_LSHIFT_W32(tmp32no1, tmp16);
// }
v32x4B = vshlq_s32(v32x4A, v32x4B);
// tmp16 = WebRtcSpl_SatW32ToW16(tmp32no1);
v16x4 = vqmovn_s32(v32x4B);
//inst->noiseEstQuantile[i] = tmp16;
vst1_s16(ptr_noiseEstQuantile, v16x4);
}
// Last iteration:
// inst->quantile[i]=exp(inst->lquantile[offset+i]);
// in Q21
int32_t tmp32no2 = WEBRTC_SPL_MUL_16_16(kExp2Const,
*ptr_noiseEstLogQuantile);
int32_t tmp32no1 = (0x00200000 | (tmp32no2 & 0x001FFFFF)); // 2^21 + frac
tmp16 = (int16_t) WEBRTC_SPL_RSHIFT_W32(tmp32no2, 21);
tmp16 -= 21;// shift 21 to get result in Q0
tmp16 += (int16_t) inst->qNoise; //shift to get result in Q(qNoise)
if (tmp16 < 0) {
tmp32no1 = WEBRTC_SPL_RSHIFT_W32(tmp32no1, -tmp16);
} else {
tmp32no1 = WEBRTC_SPL_LSHIFT_W32(tmp32no1, tmp16);
}
*ptr_noiseEstQuantile = WebRtcSpl_SatW32ToW16(tmp32no1);
}
int16_t WebRtcIsacfix_DecodeImpl(int16_t *signal_out16,
ISACFIX_DecInst_t *ISACdec_obj,
int16_t *current_framesamples)
{
int k;
int err;
int16_t BWno;
int16_t len = 0;
int16_t model;
int16_t Vector_Word16_1[FRAMESAMPLES/2];
int16_t Vector_Word16_2[FRAMESAMPLES/2];
int32_t Vector_Word32_1[FRAMESAMPLES/2];
int32_t Vector_Word32_2[FRAMESAMPLES/2];
int16_t lofilt_coefQ15[ORDERLO*SUBFRAMES]; //refl. coeffs
int16_t hifilt_coefQ15[ORDERHI*SUBFRAMES]; //refl. coeffs
int32_t gain_lo_hiQ17[2*SUBFRAMES];
int16_t PitchLags_Q7[PITCH_SUBFRAMES];
int16_t PitchGains_Q12[PITCH_SUBFRAMES];
int16_t AvgPitchGain_Q12;
int16_t tmp_1, tmp_2;
int32_t tmp32a, tmp32b;
int16_t gainQ13;
int16_t frame_nb; /* counter */
int16_t frame_mode; /* 0 for 20ms and 30ms, 1 for 60ms */
int16_t processed_samples;
/* PLC */
int16_t overlapWin[ 240 ];
(ISACdec_obj->bitstr_obj).W_upper = 0xFFFFFFFF;
(ISACdec_obj->bitstr_obj).streamval = 0;
(ISACdec_obj->bitstr_obj).stream_index = 0;
(ISACdec_obj->bitstr_obj).full = 1;
/* decode framelength and BW estimation - not used, only for stream pointer*/
err = WebRtcIsacfix_DecodeFrameLen(&ISACdec_obj->bitstr_obj, current_framesamples);
if (err<0) // error check
return err;
frame_mode = (int16_t)WEBRTC_SPL_DIV(*current_framesamples, MAX_FRAMESAMPLES); /* 0, or 1 */
processed_samples = (int16_t)WEBRTC_SPL_DIV(*current_framesamples, frame_mode+1); /* either 320 (20ms) or 480 (30, 60 ms) */
err = WebRtcIsacfix_DecodeSendBandwidth(&ISACdec_obj->bitstr_obj, &BWno);
if (err<0) // error check
return err;
/* one loop if it's one frame (20 or 30ms), 2 loops if 2 frames bundled together (60ms) */
for (frame_nb = 0; frame_nb <= frame_mode; frame_nb++) {
/* decode & dequantize pitch parameters */
err = WebRtcIsacfix_DecodePitchGain(&(ISACdec_obj->bitstr_obj), PitchGains_Q12);
if (err<0) // error check
return err;
err = WebRtcIsacfix_DecodePitchLag(&ISACdec_obj->bitstr_obj, PitchGains_Q12, PitchLags_Q7);
if (err<0) // error check
return err;
AvgPitchGain_Q12 = (int16_t)(((int32_t)PitchGains_Q12[0] + PitchGains_Q12[1] + PitchGains_Q12[2] + PitchGains_Q12[3])>>2);
/* decode & dequantize FiltCoef */
err = WebRtcIsacfix_DecodeLpc(gain_lo_hiQ17, lofilt_coefQ15, hifilt_coefQ15,
&ISACdec_obj->bitstr_obj, &model);
if (err<0) // error check
return err;
/* decode & dequantize spectrum */
len = WebRtcIsacfix_DecodeSpec(&ISACdec_obj->bitstr_obj, Vector_Word16_1, Vector_Word16_2, AvgPitchGain_Q12);
if (len < 0) // error check
return len;
// Why does this need Q16 in and out? /JS
WebRtcIsacfix_Spec2Time(Vector_Word16_1, Vector_Word16_2, Vector_Word32_1, Vector_Word32_2);
for (k=0; k<FRAMESAMPLES/2; k++) {
Vector_Word16_1[k] = (int16_t)WEBRTC_SPL_RSHIFT_W32(Vector_Word32_1[k]+64, 7); //Q16 -> Q9
}
/* ---- If this is recovery frame ---- */
if( (ISACdec_obj->plcstr_obj).used == PLC_WAS_USED )
{
(ISACdec_obj->plcstr_obj).used = PLC_NOT_USED;
if( (ISACdec_obj->plcstr_obj).B < 1000 )
{
(ISACdec_obj->plcstr_obj).decayCoeffPriodic = 4000;
}
ISACdec_obj->plcstr_obj.decayCoeffPriodic = WEBRTC_SPL_WORD16_MAX; /* DECAY_RATE is in Q15 */
ISACdec_obj->plcstr_obj.decayCoeffNoise = WEBRTC_SPL_WORD16_MAX; /* DECAY_RATE is in Q15 */
ISACdec_obj->plcstr_obj.pitchCycles = 0;
PitchGains_Q12[0] = (int16_t)WEBRTC_SPL_MUL_16_16_RSFT(PitchGains_Q12[0], 700, 10 );
/* ---- Add-overlap ---- */
WebRtcSpl_GetHanningWindow( overlapWin, RECOVERY_OVERLAP );
for( k = 0; k < RECOVERY_OVERLAP; k++ )
Vector_Word16_1[k] = WebRtcSpl_AddSatW16(
(int16_t)WEBRTC_SPL_MUL_16_16_RSFT( (ISACdec_obj->plcstr_obj).overlapLP[k], overlapWin[RECOVERY_OVERLAP - k - 1], 14),
(int16_t)WEBRTC_SPL_MUL_16_16_RSFT( Vector_Word16_1[k], overlapWin[k], 14) );
}
/* --- Store side info --- */
if( frame_nb == frame_mode )
{
/* --- LPC info */
WEBRTC_SPL_MEMCPY_W16( (ISACdec_obj->plcstr_obj).lofilt_coefQ15, &lofilt_coefQ15[(SUBFRAMES-1)*ORDERLO], ORDERLO );
WEBRTC_SPL_MEMCPY_W16( (ISACdec_obj->plcstr_obj).hifilt_coefQ15, &hifilt_coefQ15[(SUBFRAMES-1)*ORDERHI], ORDERHI );
(ISACdec_obj->plcstr_obj).gain_lo_hiQ17[0] = gain_lo_hiQ17[(SUBFRAMES-1) * 2];
(ISACdec_obj->plcstr_obj).gain_lo_hiQ17[1] = gain_lo_hiQ17[(SUBFRAMES-1) * 2 + 1];
/* --- LTP info */
(ISACdec_obj->plcstr_obj).AvgPitchGain_Q12 = PitchGains_Q12[3];
(ISACdec_obj->plcstr_obj).lastPitchGain_Q12 = PitchGains_Q12[3];
(ISACdec_obj->plcstr_obj).lastPitchLag_Q7 = PitchLags_Q7[3];
if( PitchLags_Q7[3] < 3000 )
(ISACdec_obj->plcstr_obj).lastPitchLag_Q7 += PitchLags_Q7[3];
WEBRTC_SPL_MEMCPY_W16( (ISACdec_obj->plcstr_obj).prevPitchInvIn, Vector_Word16_1, FRAMESAMPLES/2 );
}
/* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ */
/* inverse pitch filter */
WebRtcIsacfix_PitchFilter(Vector_Word16_1, Vector_Word16_2, &ISACdec_obj->pitchfiltstr_obj, PitchLags_Q7, PitchGains_Q12, 4);
if( frame_nb == frame_mode )
{
WEBRTC_SPL_MEMCPY_W16( (ISACdec_obj->plcstr_obj).prevPitchInvOut, &(Vector_Word16_2[FRAMESAMPLES/2 - (PITCH_MAX_LAG + 10)]), PITCH_MAX_LAG );
}
/* reduce gain to compensate for pitch enhancer */
/* gain = 1.0f - 0.45f * AvgPitchGain; */
tmp32a = WEBRTC_SPL_MUL_16_16_RSFT(AvgPitchGain_Q12, 29, 0); // Q18
tmp32b = 262144 - tmp32a; // Q18
gainQ13 = (int16_t) WEBRTC_SPL_RSHIFT_W32(tmp32b, 5); // Q13
for (k = 0; k < FRAMESAMPLES/2; k++)
{
Vector_Word32_1[k] = (int32_t) WEBRTC_SPL_LSHIFT_W32(WEBRTC_SPL_MUL_16_16(Vector_Word16_2[k], gainQ13), 3); // Q25
}
/* perceptual post-filtering (using normalized lattice filter) */
WebRtcIsacfix_NormLatticeFilterAr(ORDERLO, (ISACdec_obj->maskfiltstr_obj).PostStateLoGQ0,
Vector_Word32_1, lofilt_coefQ15, gain_lo_hiQ17, 0, Vector_Word16_1);
/* --- Store Highpass Residual --- */
for (k = 0; k < FRAMESAMPLES/2; k++)
Vector_Word32_1[k] = WEBRTC_SPL_LSHIFT_W32(Vector_Word32_2[k], 9); // Q16 -> Q25
for( k = 0; k < PITCH_MAX_LAG + 10; k++ )
(ISACdec_obj->plcstr_obj).prevHP[k] = Vector_Word32_1[FRAMESAMPLES/2 - (PITCH_MAX_LAG + 10) + k];
WebRtcIsacfix_NormLatticeFilterAr(ORDERHI, (ISACdec_obj->maskfiltstr_obj).PostStateHiGQ0,
Vector_Word32_1, hifilt_coefQ15, gain_lo_hiQ17, 1, Vector_Word16_2);
/* recombine the 2 bands */
/* Form the polyphase signals, and compensate for DC offset */
for (k=0;k<FRAMESAMPLES/2;k++) {
tmp_1 = (int16_t)WebRtcSpl_SatW32ToW16(((int32_t)Vector_Word16_1[k]+Vector_Word16_2[k] + 1)); /* Construct a new upper channel signal*/
tmp_2 = (int16_t)WebRtcSpl_SatW32ToW16(((int32_t)Vector_Word16_1[k]-Vector_Word16_2[k])); /* Construct a new lower channel signal*/
Vector_Word16_1[k] = tmp_1;
Vector_Word16_2[k] = tmp_2;
}
WebRtcIsacfix_FilterAndCombine1(Vector_Word16_1, Vector_Word16_2, signal_out16 + frame_nb * processed_samples, &ISACdec_obj->postfiltbankstr_obj);
}
return len;
}
/* 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;
}