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
}
}
}
void WebRtcIsacfix_Time2SpecC(int16_t *inre1Q9,
int16_t *inre2Q9,
int16_t *outreQ7,
int16_t *outimQ7)
{
int k;
int32_t tmpreQ16[FRAMESAMPLES/2], tmpimQ16[FRAMESAMPLES/2];
int16_t tmp1rQ14, tmp1iQ14;
int32_t xrQ16, xiQ16, yrQ16, yiQ16;
int32_t v1Q16, v2Q16;
int16_t factQ19, sh;
/* Multiply with complex exponentials and combine into one complex vector */
factQ19 = 16921; // 0.5/sqrt(240) in Q19 is round(.5/sqrt(240)*(2^19)) = 16921
for (k = 0; k < FRAMESAMPLES/2; k++) {
tmp1rQ14 = WebRtcIsacfix_kCosTab1[k];
tmp1iQ14 = WebRtcIsacfix_kSinTab1[k];
xrQ16 = WEBRTC_SPL_RSHIFT_W32(WEBRTC_SPL_MUL_16_16(tmp1rQ14, inre1Q9[k]) + WEBRTC_SPL_MUL_16_16(tmp1iQ14, inre2Q9[k]), 7);
xiQ16 = WEBRTC_SPL_RSHIFT_W32(WEBRTC_SPL_MUL_16_16(tmp1rQ14, inre2Q9[k]) - WEBRTC_SPL_MUL_16_16(tmp1iQ14, inre1Q9[k]), 7);
tmpreQ16[k] = WEBRTC_SPL_RSHIFT_W32(WEBRTC_SPL_MUL_16_32_RSFT16(factQ19, xrQ16)+4, 3); // (Q16*Q19>>16)>>3 = Q16
tmpimQ16[k] = WEBRTC_SPL_RSHIFT_W32(WEBRTC_SPL_MUL_16_32_RSFT16(factQ19, xiQ16)+4, 3); // (Q16*Q19>>16)>>3 = Q16
}
xrQ16 = WebRtcSpl_MaxAbsValueW32(tmpreQ16, FRAMESAMPLES/2);
yrQ16 = WebRtcSpl_MaxAbsValueW32(tmpimQ16, FRAMESAMPLES/2);
if (yrQ16>xrQ16) {
xrQ16 = yrQ16;
}
sh = WebRtcSpl_NormW32(xrQ16);
sh = sh-24; //if sh becomes >=0, then we should shift sh steps to the left, and the domain will become Q(16+sh)
//if sh becomes <0, then we should shift -sh steps to the right, and the domain will become Q(16+sh)
//"Fastest" vectors
if (sh>=0) {
for (k=0; k<FRAMESAMPLES/2; k++) {
inre1Q9[k] = (int16_t) WEBRTC_SPL_LSHIFT_W32(tmpreQ16[k], sh); //Q(16+sh)
inre2Q9[k] = (int16_t) WEBRTC_SPL_LSHIFT_W32(tmpimQ16[k], sh); //Q(16+sh)
}
} else {
int32_t round = WEBRTC_SPL_LSHIFT_W32((int32_t)1, -sh-1);
for (k=0; k<FRAMESAMPLES/2; k++) {
inre1Q9[k] = (int16_t) WEBRTC_SPL_RSHIFT_W32(tmpreQ16[k]+round, -sh); //Q(16+sh)
inre2Q9[k] = (int16_t) WEBRTC_SPL_RSHIFT_W32(tmpimQ16[k]+round, -sh); //Q(16+sh)
}
}
/* Get DFT */
WebRtcIsacfix_FftRadix16Fastest(inre1Q9, inre2Q9, -1); // real call
//"Fastest" vectors
if (sh>=0) {
for (k=0; k<FRAMESAMPLES/2; k++) {
tmpreQ16[k] = WEBRTC_SPL_RSHIFT_W32((int32_t)inre1Q9[k], sh); //Q(16+sh) -> Q16
tmpimQ16[k] = WEBRTC_SPL_RSHIFT_W32((int32_t)inre2Q9[k], sh); //Q(16+sh) -> Q16
}
} else {
for (k=0; k<FRAMESAMPLES/2; k++) {
tmpreQ16[k] = WEBRTC_SPL_LSHIFT_W32((int32_t)inre1Q9[k], -sh); //Q(16+sh) -> Q16
tmpimQ16[k] = WEBRTC_SPL_LSHIFT_W32((int32_t)inre2Q9[k], -sh); //Q(16+sh) -> Q16
}
}
/* Use symmetry to separate into two complex vectors and center frames in time around zero */
for (k = 0; k < FRAMESAMPLES/4; k++) {
xrQ16 = tmpreQ16[k] + tmpreQ16[FRAMESAMPLES/2 - 1 - k];
yiQ16 = -tmpreQ16[k] + tmpreQ16[FRAMESAMPLES/2 - 1 - k];
xiQ16 = tmpimQ16[k] - tmpimQ16[FRAMESAMPLES/2 - 1 - k];
yrQ16 = tmpimQ16[k] + tmpimQ16[FRAMESAMPLES/2 - 1 - k];
tmp1rQ14 = -WebRtcIsacfix_kSinTab2[FRAMESAMPLES/4 - 1 - k];
tmp1iQ14 = WebRtcIsacfix_kSinTab2[k];
v1Q16 = WEBRTC_SPL_MUL_16_32_RSFT14(tmp1rQ14, xrQ16) - WEBRTC_SPL_MUL_16_32_RSFT14(tmp1iQ14, xiQ16);
v2Q16 = WEBRTC_SPL_MUL_16_32_RSFT14(tmp1iQ14, xrQ16) + WEBRTC_SPL_MUL_16_32_RSFT14(tmp1rQ14, xiQ16);
outreQ7[k] = (int16_t) WEBRTC_SPL_RSHIFT_W32(v1Q16, 9);
outimQ7[k] = (int16_t) WEBRTC_SPL_RSHIFT_W32(v2Q16, 9);
v1Q16 = -WEBRTC_SPL_MUL_16_32_RSFT14(tmp1iQ14, yrQ16) - WEBRTC_SPL_MUL_16_32_RSFT14(tmp1rQ14, yiQ16);
v2Q16 = -WEBRTC_SPL_MUL_16_32_RSFT14(tmp1rQ14, yrQ16) + WEBRTC_SPL_MUL_16_32_RSFT14(tmp1iQ14, yiQ16);
outreQ7[FRAMESAMPLES/2 - 1 - k] = (int16_t)WEBRTC_SPL_RSHIFT_W32(v1Q16, 9); //CalcLrIntQ(v1Q16, 9);
outimQ7[FRAMESAMPLES/2 - 1 - k] = (int16_t)WEBRTC_SPL_RSHIFT_W32(v2Q16, 9); //CalcLrIntQ(v2Q16, 9);
}
}
void WebRtcIsacfix_Spec2TimeC(int16_t *inreQ7, int16_t *inimQ7, int32_t *outre1Q16, int32_t *outre2Q16)
{
int k;
int16_t tmp1rQ14, tmp1iQ14;
int32_t xrQ16, xiQ16, yrQ16, yiQ16;
int32_t tmpInRe, tmpInIm, tmpInRe2, tmpInIm2;
int16_t factQ11;
int16_t sh;
for (k = 0; k < FRAMESAMPLES/4; k++) {
/* Move zero in time to beginning of frames */
tmp1rQ14 = -WebRtcIsacfix_kSinTab2[FRAMESAMPLES/4 - 1 - k];
tmp1iQ14 = WebRtcIsacfix_kSinTab2[k];
tmpInRe = WEBRTC_SPL_LSHIFT_W32((int32_t) inreQ7[k], 9); // Q7 -> Q16
tmpInIm = WEBRTC_SPL_LSHIFT_W32((int32_t) inimQ7[k], 9); // Q7 -> Q16
tmpInRe2 = WEBRTC_SPL_LSHIFT_W32((int32_t) inreQ7[FRAMESAMPLES/2 - 1 - k], 9); // Q7 -> Q16
tmpInIm2 = WEBRTC_SPL_LSHIFT_W32((int32_t) inimQ7[FRAMESAMPLES/2 - 1 - k], 9); // Q7 -> Q16
xrQ16 = WEBRTC_SPL_MUL_16_32_RSFT14(tmp1rQ14, tmpInRe) + WEBRTC_SPL_MUL_16_32_RSFT14(tmp1iQ14, tmpInIm);
xiQ16 = WEBRTC_SPL_MUL_16_32_RSFT14(tmp1rQ14, tmpInIm) - WEBRTC_SPL_MUL_16_32_RSFT14(tmp1iQ14, tmpInRe);
yrQ16 = -WEBRTC_SPL_MUL_16_32_RSFT14(tmp1rQ14, tmpInIm2) - WEBRTC_SPL_MUL_16_32_RSFT14(tmp1iQ14, tmpInRe2);
yiQ16 = -WEBRTC_SPL_MUL_16_32_RSFT14(tmp1rQ14, tmpInRe2) + WEBRTC_SPL_MUL_16_32_RSFT14(tmp1iQ14, tmpInIm2);
/* Combine into one vector, z = x + j * y */
outre1Q16[k] = xrQ16 - yiQ16;
outre1Q16[FRAMESAMPLES/2 - 1 - k] = xrQ16 + yiQ16;
outre2Q16[k] = xiQ16 + yrQ16;
outre2Q16[FRAMESAMPLES/2 - 1 - k] = -xiQ16 + yrQ16;
}
/* Get IDFT */
tmpInRe = WebRtcSpl_MaxAbsValueW32(outre1Q16, 240);
tmpInIm = WebRtcSpl_MaxAbsValueW32(outre2Q16, 240);
if (tmpInIm>tmpInRe) {
tmpInRe = tmpInIm;
}
sh = WebRtcSpl_NormW32(tmpInRe);
sh = sh-24; //if sh becomes >=0, then we should shift sh steps to the left, and the domain will become Q(16+sh)
//if sh becomes <0, then we should shift -sh steps to the right, and the domain will become Q(16+sh)
//"Fastest" vectors
if (sh>=0) {
for (k=0; k<240; k++) {
inreQ7[k] = (int16_t) WEBRTC_SPL_LSHIFT_W32(outre1Q16[k], sh); //Q(16+sh)
inimQ7[k] = (int16_t) WEBRTC_SPL_LSHIFT_W32(outre2Q16[k], sh); //Q(16+sh)
}
} else {
int32_t round = WEBRTC_SPL_LSHIFT_W32((int32_t)1, -sh-1);
for (k=0; k<240; k++) {
inreQ7[k] = (int16_t) WEBRTC_SPL_RSHIFT_W32(outre1Q16[k]+round, -sh); //Q(16+sh)
inimQ7[k] = (int16_t) WEBRTC_SPL_RSHIFT_W32(outre2Q16[k]+round, -sh); //Q(16+sh)
}
}
WebRtcIsacfix_FftRadix16Fastest(inreQ7, inimQ7, 1); // real call
//"Fastest" vectors
if (sh>=0) {
for (k=0; k<240; k++) {
outre1Q16[k] = WEBRTC_SPL_RSHIFT_W32((int32_t)inreQ7[k], sh); //Q(16+sh) -> Q16
outre2Q16[k] = WEBRTC_SPL_RSHIFT_W32((int32_t)inimQ7[k], sh); //Q(16+sh) -> Q16
}
} else {
for (k=0; k<240; k++) {
outre1Q16[k] = WEBRTC_SPL_LSHIFT_W32((int32_t)inreQ7[k], -sh); //Q(16+sh) -> Q16
outre2Q16[k] = WEBRTC_SPL_LSHIFT_W32((int32_t)inimQ7[k], -sh); //Q(16+sh) -> Q16
}
}
/* Divide through by the normalizing constant: */
/* scale all values with 1/240, i.e. with 273 in Q16 */
/* 273/65536 ~= 0.0041656 */
/* 1/240 ~= 0.0041666 */
for (k=0; k<240; k++) {
outre1Q16[k] = WEBRTC_SPL_MUL_16_32_RSFT16(273, outre1Q16[k]);
outre2Q16[k] = WEBRTC_SPL_MUL_16_32_RSFT16(273, outre2Q16[k]);
}
/* Demodulate and separate */
factQ11 = 31727; // sqrt(240) in Q11 is round(15.49193338482967*2048) = 31727
for (k = 0; k < FRAMESAMPLES/2; k++) {
tmp1rQ14 = WebRtcIsacfix_kCosTab1[k];
tmp1iQ14 = WebRtcIsacfix_kSinTab1[k];
xrQ16 = WEBRTC_SPL_MUL_16_32_RSFT14(tmp1rQ14, outre1Q16[k]) - WEBRTC_SPL_MUL_16_32_RSFT14(tmp1iQ14, outre2Q16[k]);
xiQ16 = WEBRTC_SPL_MUL_16_32_RSFT14(tmp1rQ14, outre2Q16[k]) + WEBRTC_SPL_MUL_16_32_RSFT14(tmp1iQ14, outre1Q16[k]);
xrQ16 = WEBRTC_SPL_MUL_16_32_RSFT11(factQ11, xrQ16);
xiQ16 = WEBRTC_SPL_MUL_16_32_RSFT11(factQ11, xiQ16);
outre2Q16[k] = xiQ16;
outre1Q16[k] = xrQ16;
}
}
/* 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 WebRtcIsacfix_Time2SpecC(int16_t *inre1Q9,
int16_t *inre2Q9,
int16_t *outreQ7,
int16_t *outimQ7)
{
int k;
int32_t tmpreQ16[FRAMESAMPLES/2], tmpimQ16[FRAMESAMPLES/2];
int16_t tmp1rQ14, tmp1iQ14;
int32_t xrQ16, xiQ16, yrQ16, yiQ16;
int32_t v1Q16, v2Q16;
int16_t factQ19, sh;
/* Multiply with complex exponentials and combine into one complex vector */
factQ19 = 16921; // 0.5/sqrt(240) in Q19 is round(.5/sqrt(240)*(2^19)) = 16921
for (k = 0; k < FRAMESAMPLES/2; k++) {
tmp1rQ14 = WebRtcIsacfix_kCosTab1[k];
tmp1iQ14 = WebRtcIsacfix_kSinTab1[k];
xrQ16 = (tmp1rQ14 * inre1Q9[k] + tmp1iQ14 * inre2Q9[k]) >> 7;
xiQ16 = (tmp1rQ14 * inre2Q9[k] - tmp1iQ14 * inre1Q9[k]) >> 7;
// Q-domains below: (Q16*Q19>>16)>>3 = Q16
tmpreQ16[k] = (WEBRTC_SPL_MUL_16_32_RSFT16(factQ19, xrQ16) + 4) >> 3;
tmpimQ16[k] = (WEBRTC_SPL_MUL_16_32_RSFT16(factQ19, xiQ16) + 4) >> 3;
}
xrQ16 = WebRtcSpl_MaxAbsValueW32(tmpreQ16, FRAMESAMPLES/2);
yrQ16 = WebRtcSpl_MaxAbsValueW32(tmpimQ16, FRAMESAMPLES/2);
if (yrQ16>xrQ16) {
xrQ16 = yrQ16;
}
sh = WebRtcSpl_NormW32(xrQ16);
sh = sh-24; //if sh becomes >=0, then we should shift sh steps to the left, and the domain will become Q(16+sh)
//if sh becomes <0, then we should shift -sh steps to the right, and the domain will become Q(16+sh)
//"Fastest" vectors
if (sh>=0) {
for (k=0; k<FRAMESAMPLES/2; k++) {
inre1Q9[k] = (int16_t)(tmpreQ16[k] << sh); // Q(16+sh)
inre2Q9[k] = (int16_t)(tmpimQ16[k] << sh); // Q(16+sh)
}
} else {
int32_t round = 1 << (-sh - 1);
for (k=0; k<FRAMESAMPLES/2; k++) {
inre1Q9[k] = (int16_t)((tmpreQ16[k] + round) >> -sh); // Q(16+sh)
inre2Q9[k] = (int16_t)((tmpimQ16[k] + round) >> -sh); // Q(16+sh)
}
}
/* Get DFT */
WebRtcIsacfix_FftRadix16Fastest(inre1Q9, inre2Q9, -1); // real call
//"Fastest" vectors
if (sh>=0) {
for (k=0; k<FRAMESAMPLES/2; k++) {
tmpreQ16[k] = inre1Q9[k] >> sh; // Q(16+sh) -> Q16
tmpimQ16[k] = inre2Q9[k] >> sh; // Q(16+sh) -> Q16
}
} else {
for (k=0; k<FRAMESAMPLES/2; k++) {
// Contains a function for the core loop in the normalized lattice MA
// filter routine for iSAC codec, optimized for ARM Neon platform.
// It does:
// for 0 <= n < HALF_SUBFRAMELEN - 1:
// *ptr2 = input2 * (*ptr2) + input0 * (*ptr0));
// *ptr1 = input1 * (*ptr0) + input0 * (*ptr2);
// Output is not bit-exact with the reference C code, due to the replacement
// of WEBRTC_SPL_MUL_16_32_RSFT15 and LATTICE_MUL_32_32_RSFT16 with Neon
// instructions. The difference should not be bigger than 1.
void WebRtcIsacfix_FilterMaLoopNeon(int16_t input0, // Filter coefficient
int16_t input1, // Filter coefficient
int32_t input2, // Inverse coefficient
int32_t* ptr0, // Sample buffer
int32_t* ptr1, // Sample buffer
int32_t* ptr2) // Sample buffer
{
int n = 0;
int loop = (HALF_SUBFRAMELEN - 1) >> 3;
int loop_tail = (HALF_SUBFRAMELEN - 1) & 0x7;
int32x4_t input0_v = vdupq_n_s32((int32_t)input0 << 16);
int32x4_t input1_v = vdupq_n_s32((int32_t)input1 << 16);
int32x4_t input2_v = vdupq_n_s32(input2);
int32x4_t tmp0a, tmp1a, tmp2a, tmp3a;
int32x4_t tmp0b, tmp1b, tmp2b, tmp3b;
int32x4_t ptr0va, ptr1va, ptr2va;
int32x4_t ptr0vb, ptr1vb, ptr2vb;
// Unroll to process 8 samples at once.
for (n = 0; n < loop; n++) {
ptr0va = vld1q_s32(ptr0);
ptr0vb = vld1q_s32(ptr0 + 4);
ptr0 += 8;
ptr2va = vld1q_s32(ptr2);
ptr2vb = vld1q_s32(ptr2 + 4);
// Calculate tmp0 = (*ptr0) * input0.
tmp0a = vqrdmulhq_s32(ptr0va, input0_v);
tmp0b = vqrdmulhq_s32(ptr0vb, input0_v);
// Calculate tmp1 = (*ptr0) * input1.
tmp1a = vqrdmulhq_s32(ptr0va, input1_v);
tmp1b = vqrdmulhq_s32(ptr0vb, input1_v);
// Calculate tmp2 = tmp0 + *(ptr2).
tmp2a = vaddq_s32(tmp0a, ptr2va);
tmp2b = vaddq_s32(tmp0b, ptr2vb);
tmp2a = vshlq_n_s32(tmp2a, 15);
tmp2b = vshlq_n_s32(tmp2b, 15);
// Calculate *ptr2 = input2 * tmp2.
ptr2va = vqrdmulhq_s32(tmp2a, input2_v);
ptr2vb = vqrdmulhq_s32(tmp2b, input2_v);
vst1q_s32(ptr2, ptr2va);
vst1q_s32(ptr2 + 4, ptr2vb);
ptr2 += 8;
// Calculate tmp3 = ptr2v * input0.
tmp3a = vqrdmulhq_s32(ptr2va, input0_v);
tmp3b = vqrdmulhq_s32(ptr2vb, input0_v);
// Calculate *ptr1 = tmp1 + tmp3.
ptr1va = vaddq_s32(tmp1a, tmp3a);
ptr1vb = vaddq_s32(tmp1b, tmp3b);
vst1q_s32(ptr1, ptr1va);
vst1q_s32(ptr1 + 4, ptr1vb);
ptr1 += 8;
}
// Process four more samples.
if (loop_tail & 0x4) {
ptr0va = vld1q_s32(ptr0);
ptr2va = vld1q_s32(ptr2);
ptr0 += 4;
// Calculate tmp0 = (*ptr0) * input0.
tmp0a = vqrdmulhq_s32(ptr0va, input0_v);
// Calculate tmp1 = (*ptr0) * input1.
tmp1a = vqrdmulhq_s32(ptr0va, input1_v);
// Calculate tmp2 = tmp0 + *(ptr2).
tmp2a = vaddq_s32(tmp0a, ptr2va);
tmp2a = vshlq_n_s32(tmp2a, 15);
// Calculate *ptr2 = input2 * tmp2.
ptr2va = vqrdmulhq_s32(tmp2a, input2_v);
vst1q_s32(ptr2, ptr2va);
ptr2 += 4;
// Calculate tmp3 = *(ptr2) * input0.
tmp3a = vqrdmulhq_s32(ptr2va, input0_v);
// Calculate *ptr1 = tmp1 + tmp3.
ptr1va = vaddq_s32(tmp1a, tmp3a);
vst1q_s32(ptr1, ptr1va);
ptr1 += 4;
}
// Process two more samples.
if (loop_tail & 0x2) {
int32x2_t ptr0v_tail, ptr2v_tail, ptr1v_tail;
int32x2_t tmp0_tail, tmp1_tail, tmp2_tail, tmp3_tail;
ptr0v_tail = vld1_s32(ptr0);
ptr2v_tail = vld1_s32(ptr2);
ptr0 += 2;
// Calculate tmp0 = (*ptr0) * input0.
tmp0_tail = vqrdmulh_s32(ptr0v_tail, vget_low_s32(input0_v));
// Calculate tmp1 = (*ptr0) * input1.
tmp1_tail = vqrdmulh_s32(ptr0v_tail, vget_low_s32(input1_v));
// Calculate tmp2 = tmp0 + *(ptr2).
tmp2_tail = vadd_s32(tmp0_tail, ptr2v_tail);
tmp2_tail = vshl_n_s32(tmp2_tail, 15);
// Calculate *ptr2 = input2 * tmp2.
ptr2v_tail = vqrdmulh_s32(tmp2_tail, vget_low_s32(input2_v));
vst1_s32(ptr2, ptr2v_tail);
ptr2 += 2;
// Calculate tmp3 = *(ptr2) * input0.
tmp3_tail = vqrdmulh_s32(ptr2v_tail, vget_low_s32(input0_v));
// Calculate *ptr1 = tmp1 + tmp3.
ptr1v_tail = vadd_s32(tmp1_tail, tmp3_tail);
vst1_s32(ptr1, ptr1v_tail);
ptr1 += 2;
}
// Process one more sample.
if (loop_tail & 0x1) {
int16_t t16a = (int16_t)(input2 >> 16);
int16_t t16b = (int16_t)input2;
if (t16b < 0) t16a++;
int32_t tmp32a;
int32_t tmp32b;
// Calculate *ptr2 = input2 * (*ptr2 + input0 * (*ptr0)).
tmp32a = WEBRTC_SPL_MUL_16_32_RSFT15(input0, *ptr0);
tmp32b = *ptr2 + tmp32a;
*ptr2 = (int32_t)(WEBRTC_SPL_MUL(t16a, tmp32b) +
(WEBRTC_SPL_MUL_16_32_RSFT16(t16b, tmp32b)));
// Calculate *ptr1 = input1 * (*ptr0) + input0 * (*ptr2).
tmp32a = WEBRTC_SPL_MUL_16_32_RSFT15(input1, *ptr0);
tmp32b = WEBRTC_SPL_MUL_16_32_RSFT15(input0, *ptr2);
*ptr1 = tmp32a + tmp32b;
}
