/* 1D parabolic interpolation . All input and output values are in Q8 */
static __inline void Intrp1DQ8(int32_t *x, int32_t *fx, int32_t *y, int32_t *fy) {
int16_t sign1=1, sign2=1;
int32_t r32, q32, t32, nom32, den32;
int16_t t16, tmp16, tmp16_1;
if ((fx[0]>0) && (fx[2]>0)) {
r32=fx[1]-fx[2];
q32=fx[0]-fx[1];
nom32=q32+r32;
den32=WEBRTC_SPL_MUL_32_16((q32-r32), 2);
if (nom32<0)
sign1=-1;
if (den32<0)
sign2=-1;
/* t = (q32+r32)/(2*(q32-r32)) = (fx[0]-fx[1] + fx[1]-fx[2])/(2 * fx[0]-fx[1] - (fx[1]-fx[2]))*/
/* (Signs are removed because WebRtcSpl_DivResultInQ31 can't handle negative numbers) */
t32=WebRtcSpl_DivResultInQ31(WEBRTC_SPL_MUL_32_16(nom32, sign1),WEBRTC_SPL_MUL_32_16(den32, sign2)); /* t in Q31, without signs */
t16=(int16_t)WEBRTC_SPL_RSHIFT_W32(t32, 23); /* Q8 */
t16=t16*sign1*sign2; /* t in Q8 with signs */
*y = x[0]+t16; /* Q8 */
// *y = x[1]+t16; /* Q8 */
/* The following code calculates fy in three steps */
/* fy = 0.5 * t * (t-1) * fx[0] + (1-t*t) * fx[1] + 0.5 * t * (t+1) * fx[2]; */
/* Part I: 0.5 * t * (t-1) * fx[0] */
tmp16_1=(int16_t)WEBRTC_SPL_MUL_16_16(t16,t16); /* Q8*Q8=Q16 */
tmp16_1 = WEBRTC_SPL_RSHIFT_W16(tmp16_1,2); /* Q16>>2 = Q14 */
t16 = (int16_t)WEBRTC_SPL_MUL_16_16(t16, 64); /* Q8<<6 = Q14 */
tmp16 = tmp16_1-t16;
*fy = WEBRTC_SPL_MUL_16_32_RSFT15(tmp16, fx[0]); /* (Q14 * Q8 >>15)/2 = Q8 */
/* Part II: (1-t*t) * fx[1] */
tmp16 = 16384-tmp16_1; /* 1 in Q14 - Q14 */
*fy += WEBRTC_SPL_MUL_16_32_RSFT14(tmp16, fx[1]);/* Q14 * Q8 >> 14 = Q8 */
/* Part III: 0.5 * t * (t+1) * fx[2] */
tmp16 = tmp16_1+t16;
*fy += WEBRTC_SPL_MUL_16_32_RSFT15(tmp16, fx[2]);/* (Q14 * Q8 >>15)/2 = Q8 */
} else {
*y = x[0];
*fy= fx[1];
}
}
void WebRtcIsacfix_PitchFilterGains(const int16_t* indatQ0,
PitchFiltstr* pfp,
int16_t* lagsQ7,
int16_t* gainsQ12) {
int k, n, m, ind, pos, pos3QQ;
int16_t ubufQQ[PITCH_INTBUFFSIZE];
int16_t oldLagQ7, lagdeltaQ7, curLagQ7;
const int16_t* fracoeffQQ = NULL;
int16_t scale;
int16_t cnt = 0, frcQQ, indW16 = 0, tmpW16;
int32_t tmpW32, tmp2W32, csum1QQ, esumxQQ;
// Set up buffer and states.
memcpy(ubufQQ, pfp->ubufQQ, sizeof(pfp->ubufQQ));
oldLagQ7 = pfp->oldlagQ7;
// No interpolation if pitch lag step is big.
if ((WEBRTC_SPL_MUL_16_16_RSFT(lagsQ7[0], 3, 1) < oldLagQ7) ||
(lagsQ7[0] > WEBRTC_SPL_MUL_16_16_RSFT(oldLagQ7, 3, 1))) {
oldLagQ7 = lagsQ7[0];
}
ind = 0;
pos = ind + PITCH_BUFFSIZE;
scale = 0;
for (k = 0; k < PITCH_SUBFRAMES; k++) {
// Calculate interpolation steps.
lagdeltaQ7 = lagsQ7[k] - oldLagQ7;
lagdeltaQ7 = (int16_t)WEBRTC_SPL_MUL_16_16_RSFT_WITH_ROUND(
lagdeltaQ7, kDivFactor, 15);
curLagQ7 = oldLagQ7;
oldLagQ7 = lagsQ7[k];
csum1QQ = 1;
esumxQQ = 1;
// Same as function WebRtcIsacfix_PitchFilter(), we break the pitch
// filtering into two for-loops (5 x 12) below.
for (cnt = 0; cnt < kSegments; cnt++) {
// Update parameters for each segment.
curLagQ7 += lagdeltaQ7;
indW16 = (int16_t)CalcLrIntQ(curLagQ7, 7);
tmpW16 = WEBRTC_SPL_LSHIFT_W16(indW16, 7);
tmpW16 -= curLagQ7;
frcQQ = WEBRTC_SPL_RSHIFT_W16(tmpW16, 4);
frcQQ += 4;
if (frcQQ == PITCH_FRACS) {
frcQQ = 0;
}
fracoeffQQ = kIntrpCoef[frcQQ];
pos3QQ = pos - (indW16 + 4);
for (n = 0; n < PITCH_SUBFRAME_LEN / kSegments; n++) {
// Filter to get fractional pitch.
tmpW32 = 0;
for (m = 0; m < PITCH_FRACORDER; m++) {
tmpW32 += WEBRTC_SPL_MUL_16_16(ubufQQ[pos3QQ + m], fracoeffQQ[m]);
}
// Subtract from input and update buffer.
ubufQQ[pos] = indatQ0[ind];
tmp2W32 = WEBRTC_SPL_MUL_16_32_RSFT14(indatQ0[ind], tmpW32);
tmpW32 += 8192;
tmpW16 = (int16_t)WEBRTC_SPL_RSHIFT_W32(tmpW32, 14);
tmpW32 = WEBRTC_SPL_MUL_16_16(tmpW16, tmpW16);
if ((tmp2W32 > 1073700000) || (csum1QQ > 1073700000) ||
(tmpW32 > 1073700000) || (esumxQQ > 1073700000)) { // 2^30
scale++;
csum1QQ = WEBRTC_SPL_RSHIFT_W32(csum1QQ, 1);
esumxQQ = WEBRTC_SPL_RSHIFT_W32(esumxQQ, 1);
}
tmp2W32 = WEBRTC_SPL_RSHIFT_W32(tmp2W32, scale);
csum1QQ += tmp2W32;
tmpW32 = WEBRTC_SPL_RSHIFT_W32(tmpW32, scale);
esumxQQ += tmpW32;
ind++;
pos++;
pos3QQ++;
}
}
if (csum1QQ < esumxQQ) {
tmp2W32 = WebRtcSpl_DivResultInQ31(csum1QQ, esumxQQ);
// Gain should be half the correlation.
tmpW32 = WEBRTC_SPL_RSHIFT_W32(tmp2W32, 20);
} else {
tmpW32 = 4096;
}
gainsQ12[k] = (int16_t)WEBRTC_SPL_SAT(PITCH_MAX_GAIN_Q12, tmpW32, 0);
}
// Export buffer and states.
memcpy(pfp->ubufQQ, ubufQQ + PITCH_FRAME_LEN, sizeof(pfp->ubufQQ));
pfp->oldlagQ7 = lagsQ7[PITCH_SUBFRAMES - 1];
pfp->oldgainQ12 = gainsQ12[PITCH_SUBFRAMES - 1];
}
