1a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin/* 2a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin * Copyright (c) 2011 The WebRTC project authors. All Rights Reserved. 3a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin * 4a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin * Use of this source code is governed by a BSD-style license 5a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin * that can be found in the LICENSE file in the root of the source 6a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin * tree. An additional intellectual property rights grant can be found 7a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin * in the file PATENTS. All contributing project authors may 8a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin * be found in the AUTHORS file in the root of the source tree. 9a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin */ 10a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin 11a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin/* 12a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin * lattice.c 13a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin * 14a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin * Contains the normalized lattice filter routines (MA and AR) for iSAC codec 15a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin * 16a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin */ 17a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin 18a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin#include "codec.h" 19a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin#include "settings.h" 20a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin 21a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin#define LATTICE_MUL_32_32_RSFT16(a32a, a32b, b32) \ 22a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin ((WebRtc_Word32)(WEBRTC_SPL_MUL(a32a, b32) + (WEBRTC_SPL_MUL_16_32_RSFT16(a32b, b32)))) 23a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin/* This macro is FORBIDDEN to use elsewhere than in a function in this file and 24a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin its corresponding neon version. It might give unpredictable results, since a 25a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin general WebRtc_Word32*WebRtc_Word32 multiplication results in a 64 bit value. 26a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin The result is then shifted just 16 steps to the right, giving need for 48 27a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin bits, i.e. in the generel case, it will NOT fit in a WebRtc_Word32. In the 28a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin cases used in here, the WebRtc_Word32 will be enough, since (for a good 29a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin reason) the involved multiplicands aren't big enough to overflow a 30a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin WebRtc_Word32 after shifting right 16 bits. I have compared the result of a 31a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin multiplication between t32 and tmp32, done in two ways: 32a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin 1) Using (WebRtc_Word32) (((float)(tmp32))*((float)(tmp32b))/65536.0); 33a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin 2) Using LATTICE_MUL_32_32_RSFT16(t16a, t16b, tmp32b); 34a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin By running 25 files, I haven't found any bigger diff than 64 - this was in the 35a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin case when method 1) gave 650235648 and 2) gave 650235712. 36a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin*/ 37a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin 38a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin/* Function prototype: filtering ar_g_Q0[] and ar_f_Q0[] through an AR filter 39a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin with coefficients cth_Q15[] and sth_Q15[]. 40a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin Implemented for both generic and ARMv7 platforms. 41a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin */ 42a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkinvoid WebRtcIsacfix_FilterArLoop(int16_t* ar_g_Q0, 43a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin int16_t* ar_f_Q0, 44a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin int16_t* cth_Q15, 45a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin int16_t* sth_Q15, 46a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin int16_t order_coef); 47a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin 48a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin/* Inner loop used for function WebRtcIsacfix_NormLatticeFilterMa(). It does: 49a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin for 0 <= n < HALF_SUBFRAMELEN - 1: 50a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin *ptr2 = input2 * (*ptr2) + input0 * (*ptr0)); 51a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin *ptr1 = input1 * (*ptr0) + input0 * (*ptr2); 52a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin Note, function WebRtcIsacfix_FilterMaLoopNeon and WebRtcIsacfix_FilterMaLoopC 53a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin are not bit-exact. The accuracy by the ARM Neon function is same or better. 54a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin*/ 55a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkinvoid WebRtcIsacfix_FilterMaLoopC(int16_t input0, // Filter coefficient 56a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin int16_t input1, // Filter coefficient 57a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin int32_t input2, // Inverse coeff. (1/input1) 58a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin int32_t* ptr0, // Sample buffer 59a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin int32_t* ptr1, // Sample buffer 60a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin int32_t* ptr2) { // Sample buffer 61a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin int n = 0; 62a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin 63a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin // Separate the 32-bit variable input2 into two 16-bit integers (high 16 and 64a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin // low 16 bits), for using LATTICE_MUL_32_32_RSFT16 in the loop. 65a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin int16_t t16a = (int16_t)(input2 >> 16); 66a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin int16_t t16b = (int16_t)input2; 67a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin if (t16b < 0) t16a++; 68a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin 69a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin // The loop filtering the samples *ptr0, *ptr1, *ptr2 with filter coefficients 70a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin // input0, input1, and input2. 71a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin for(n = 0; n < HALF_SUBFRAMELEN - 1; n++, ptr0++, ptr1++, ptr2++) { 72a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin int32_t tmp32a = 0; 73a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin int32_t tmp32b = 0; 74a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin 75a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin // Calculate *ptr2 = input2 * (*ptr2 + input0 * (*ptr0)); 76a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin tmp32a = WEBRTC_SPL_MUL_16_32_RSFT15(input0, *ptr0); // Q15 * Q15 >> 15 = Q15 77a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin tmp32b = *ptr2 + tmp32a; // Q15 + Q15 = Q15 78a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin *ptr2 = LATTICE_MUL_32_32_RSFT16(t16a, t16b, tmp32b); 79a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin 80a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin // Calculate *ptr1 = input1 * (*ptr0) + input0 * (*ptr2); 81a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin tmp32a = WEBRTC_SPL_MUL_16_32_RSFT15(input1, *ptr0); // Q15*Q15>>15 = Q15 82a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin tmp32b = WEBRTC_SPL_MUL_16_32_RSFT15(input0, *ptr2); // Q15*Q15>>15 = Q15 83a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin *ptr1 = tmp32a + tmp32b; // Q15 + Q15 = Q15 84a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin } 85a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin} 86a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin 87a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin// Declare a function pointer. 88a6451827d543eb00824bc95097e47d0aac51ae93Alexander GutkinFilterMaLoopFix WebRtcIsacfix_FilterMaLoopFix; 89a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin 90a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin/* filter the signal using normalized lattice filter */ 91a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin/* MA filter */ 92a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkinvoid WebRtcIsacfix_NormLatticeFilterMa(WebRtc_Word16 orderCoef, 93a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin WebRtc_Word32 *stateGQ15, 94a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin WebRtc_Word16 *lat_inQ0, 95a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin WebRtc_Word16 *filt_coefQ15, 96a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin WebRtc_Word32 *gain_lo_hiQ17, 97a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin WebRtc_Word16 lo_hi, 98a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin WebRtc_Word16 *lat_outQ9) 99a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin{ 100a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin WebRtc_Word16 sthQ15[MAX_AR_MODEL_ORDER]; 101a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin WebRtc_Word16 cthQ15[MAX_AR_MODEL_ORDER]; 102a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin 103a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin int u, i, k, n; 104a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin WebRtc_Word16 temp2,temp3; 105a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin WebRtc_Word16 ord_1 = orderCoef+1; 106a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin WebRtc_Word32 inv_cthQ16[MAX_AR_MODEL_ORDER]; 107a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin 108a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin WebRtc_Word32 gain32, fQtmp; 109a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin WebRtc_Word16 gain16; 110a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin WebRtc_Word16 gain_sh; 111a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin 112a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin WebRtc_Word32 tmp32, tmp32b; 113a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin WebRtc_Word32 fQ15vec[HALF_SUBFRAMELEN]; 114a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin WebRtc_Word32 gQ15[MAX_AR_MODEL_ORDER+1][HALF_SUBFRAMELEN]; 115a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin WebRtc_Word16 sh; 116a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin WebRtc_Word16 t16a; 117a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin WebRtc_Word16 t16b; 118a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin 119a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin for (u=0;u<SUBFRAMES;u++) 120a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin { 121a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin int32_t temp1 = WEBRTC_SPL_MUL_16_16(u, HALF_SUBFRAMELEN); 122a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin 123a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin /* set the Direct Form coefficients */ 124a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin temp2 = (WebRtc_Word16)WEBRTC_SPL_MUL_16_16(u, orderCoef); 125a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin temp3 = (WebRtc_Word16)WEBRTC_SPL_MUL_16_16(2, u)+lo_hi; 126a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin 127a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin /* compute lattice filter coefficients */ 128a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin memcpy(sthQ15, &filt_coefQ15[temp2], orderCoef * sizeof(WebRtc_Word16)); 129a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin 130a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin WebRtcSpl_SqrtOfOneMinusXSquared(sthQ15, orderCoef, cthQ15); 131a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin 132a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin /* compute the gain */ 133a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin gain32 = gain_lo_hiQ17[temp3]; 134a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin gain_sh = WebRtcSpl_NormW32(gain32); 135a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin gain32 = WEBRTC_SPL_LSHIFT_W32(gain32, gain_sh); //Q(17+gain_sh) 136a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin 137a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin for (k=0;k<orderCoef;k++) 138a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin { 139a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin gain32 = WEBRTC_SPL_MUL_16_32_RSFT15(cthQ15[k], gain32); //Q15*Q(17+gain_sh)>>15 = Q(17+gain_sh) 140a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin inv_cthQ16[k] = WebRtcSpl_DivW32W16((WebRtc_Word32)2147483647, cthQ15[k]); // 1/cth[k] in Q31/Q15 = Q16 141a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin } 142a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin gain16 = (WebRtc_Word16) WEBRTC_SPL_RSHIFT_W32(gain32, 16); //Q(1+gain_sh) 143a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin 144a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin /* normalized lattice filter */ 145a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin /*****************************/ 146a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin 147a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin /* initial conditions */ 148a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin for (i=0;i<HALF_SUBFRAMELEN;i++) 149a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin { 150a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin fQ15vec[i] = WEBRTC_SPL_LSHIFT_W32((WebRtc_Word32)lat_inQ0[i + temp1], 15); //Q15 151a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin gQ15[0][i] = WEBRTC_SPL_LSHIFT_W32((WebRtc_Word32)lat_inQ0[i + temp1], 15); //Q15 152a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin } 153a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin 154a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin 155a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin fQtmp = fQ15vec[0]; 156a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin 157a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin /* get the state of f&g for the first input, for all orders */ 158a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin for (i=1;i<ord_1;i++) 159a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin { 160a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin // Calculate f[i][0] = inv_cth[i-1]*(f[i-1][0] + sth[i-1]*stateG[i-1]); 161a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin tmp32 = WEBRTC_SPL_MUL_16_32_RSFT15(sthQ15[i-1], stateGQ15[i-1]);//Q15*Q15>>15 = Q15 162a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin tmp32b= fQtmp + tmp32; //Q15+Q15=Q15 163a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin tmp32 = inv_cthQ16[i-1]; //Q16 164a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin t16a = (WebRtc_Word16) WEBRTC_SPL_RSHIFT_W32(tmp32, 16); 165a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin t16b = (WebRtc_Word16) (tmp32-WEBRTC_SPL_LSHIFT_W32(((WebRtc_Word32)t16a), 16)); 166a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin if (t16b<0) t16a++; 167a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin tmp32 = LATTICE_MUL_32_32_RSFT16(t16a, t16b, tmp32b); 168a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin fQtmp = tmp32; // Q15 169a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin 170a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin // Calculate g[i][0] = cth[i-1]*stateG[i-1] + sth[i-1]* f[i][0]; 171a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin tmp32 = WEBRTC_SPL_MUL_16_32_RSFT15(cthQ15[i-1], stateGQ15[i-1]); //Q15*Q15>>15 = Q15 172a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin tmp32b = WEBRTC_SPL_MUL_16_32_RSFT15(sthQ15[i-1], fQtmp); //Q15*Q15>>15 = Q15 173a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin tmp32 = tmp32 + tmp32b;//Q15+Q15 = Q15 174a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin gQ15[i][0] = tmp32; // Q15 175a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin } 176a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin 177a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin /* filtering */ 178a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin /* save the states */ 179a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin for(k=0;k<orderCoef;k++) 180a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin { 181a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin // for 0 <= n < HALF_SUBFRAMELEN - 1: 182a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin // f[k+1][n+1] = inv_cth[k]*(f[k][n+1] + sth[k]*g[k][n]); 183a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin // g[k+1][n+1] = cth[k]*g[k][n] + sth[k]* f[k+1][n+1]; 184a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin WebRtcIsacfix_FilterMaLoopFix(sthQ15[k], cthQ15[k], inv_cthQ16[k], 185a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin &gQ15[k][0], &gQ15[k+1][1], &fQ15vec[1]); 186a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin } 187a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin 188a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin fQ15vec[0] = fQtmp; 189a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin 190a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin for(n=0;n<HALF_SUBFRAMELEN;n++) 191a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin { 192a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin //gain32 = WEBRTC_SPL_RSHIFT_W32(gain32, gain_sh); // Q(17+gain_sh) -> Q17 193a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin tmp32 = WEBRTC_SPL_MUL_16_32_RSFT16(gain16, fQ15vec[n]); //Q(1+gain_sh)*Q15>>16 = Q(gain_sh) 194a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin sh = 9-gain_sh; //number of needed shifts to reach Q9 195a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin t16a = (WebRtc_Word16) WEBRTC_SPL_SHIFT_W32(tmp32, sh); 196a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin lat_outQ9[n + temp1] = t16a; 197a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin } 198a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin 199a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin /* save the states */ 200a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin for (i=0;i<ord_1;i++) 201a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin { 202a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin stateGQ15[i] = gQ15[i][HALF_SUBFRAMELEN-1]; 203a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin } 204a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin //process next frame 205a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin } 206a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin 207a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin return; 208a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin} 209a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin 210a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin 211a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin 212a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin 213a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin 214a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin/* ----------------AR filter-------------------------*/ 215a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin/* filter the signal using normalized lattice filter */ 216a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkinvoid WebRtcIsacfix_NormLatticeFilterAr(WebRtc_Word16 orderCoef, 217a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin WebRtc_Word16 *stateGQ0, 218a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin WebRtc_Word32 *lat_inQ25, 219a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin WebRtc_Word16 *filt_coefQ15, 220a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin WebRtc_Word32 *gain_lo_hiQ17, 221a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin WebRtc_Word16 lo_hi, 222a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin WebRtc_Word16 *lat_outQ0) 223a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin{ 224a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin int ii,n,k,i,u; 225a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin WebRtc_Word16 sthQ15[MAX_AR_MODEL_ORDER]; 226a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin WebRtc_Word16 cthQ15[MAX_AR_MODEL_ORDER]; 227a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin WebRtc_Word32 tmp32; 228a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin 229a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin 230a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin WebRtc_Word16 tmpAR; 231a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin WebRtc_Word16 ARfQ0vec[HALF_SUBFRAMELEN]; 232a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin WebRtc_Word16 ARgQ0vec[MAX_AR_MODEL_ORDER+1]; 233a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin 234a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin WebRtc_Word32 inv_gain32; 235a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin WebRtc_Word16 inv_gain16; 236a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin WebRtc_Word16 den16; 237a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin WebRtc_Word16 sh; 238a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin 239a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin WebRtc_Word16 temp2,temp3; 240a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin WebRtc_Word16 ord_1 = orderCoef+1; 241a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin 242a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin for (u=0;u<SUBFRAMES;u++) 243a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin { 244a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin int32_t temp1 = WEBRTC_SPL_MUL_16_16(u, HALF_SUBFRAMELEN); 245a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin 246a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin //set the denominator and numerator of the Direct Form 247a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin temp2 = (WebRtc_Word16)WEBRTC_SPL_MUL_16_16(u, orderCoef); 248a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin temp3 = (WebRtc_Word16)WEBRTC_SPL_MUL_16_16(2, u) + lo_hi; 249a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin 250a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin for (ii=0; ii<orderCoef; ii++) { 251a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin sthQ15[ii] = filt_coefQ15[temp2+ii]; 252a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin } 253a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin 254a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin WebRtcSpl_SqrtOfOneMinusXSquared(sthQ15, orderCoef, cthQ15); 255a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin 256a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin /* Simulation of the 25 files shows that maximum value in 257a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin the vector gain_lo_hiQ17[] is 441344, which means that 258a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin it is log2((2^31)/441344) = 12.2 shifting bits from 259a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin saturation. Therefore, it should be safe to use Q27 instead 260a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin of Q17. */ 261a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin 262a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin tmp32 = WEBRTC_SPL_LSHIFT_W32(gain_lo_hiQ17[temp3], 10); // Q27 263a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin 264a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin for (k=0;k<orderCoef;k++) { 265a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin tmp32 = WEBRTC_SPL_MUL_16_32_RSFT15(cthQ15[k], tmp32); // Q15*Q27>>15 = Q27 266a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin } 267a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin 268a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin sh = WebRtcSpl_NormW32(tmp32); // tmp32 is the gain 269a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin den16 = (WebRtc_Word16) WEBRTC_SPL_SHIFT_W32(tmp32, sh-16); //Q(27+sh-16) = Q(sh+11) (all 16 bits are value bits) 270a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin inv_gain32 = WebRtcSpl_DivW32W16((WebRtc_Word32)2147483647, den16); // 1/gain in Q31/Q(sh+11) = Q(20-sh) 271a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin 272a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin //initial conditions 273a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin inv_gain16 = (WebRtc_Word16) WEBRTC_SPL_RSHIFT_W32(inv_gain32, 2); // 1/gain in Q(20-sh-2) = Q(18-sh) 274a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin 275a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin for (i=0;i<HALF_SUBFRAMELEN;i++) 276a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin { 277a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin 278a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin tmp32 = WEBRTC_SPL_LSHIFT_W32(lat_inQ25[i + temp1], 1); //Q25->Q26 279a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin tmp32 = WEBRTC_SPL_MUL_16_32_RSFT16(inv_gain16, tmp32); //lat_in[]*inv_gain in (Q(18-sh)*Q26)>>16 = Q(28-sh) 280a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin tmp32 = WEBRTC_SPL_SHIFT_W32(tmp32, -(28-sh)); // lat_in[]*inv_gain in Q0 281a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin 282a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin ARfQ0vec[i] = (WebRtc_Word16)WebRtcSpl_SatW32ToW16(tmp32); // Q0 283a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin } 284a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin 285a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin for (i=orderCoef-1;i>=0;i--) //get the state of f&g for the first input, for all orders 286a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin { 287a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin tmp32 = WEBRTC_SPL_RSHIFT_W32(((WEBRTC_SPL_MUL_16_16(cthQ15[i],ARfQ0vec[0])) - (WEBRTC_SPL_MUL_16_16(sthQ15[i],stateGQ0[i])) + 16384), 15); 288a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin tmpAR = (WebRtc_Word16)WebRtcSpl_SatW32ToW16(tmp32); // Q0 289a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin 290a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin tmp32 = WEBRTC_SPL_RSHIFT_W32(((WEBRTC_SPL_MUL_16_16(sthQ15[i],ARfQ0vec[0])) + (WEBRTC_SPL_MUL_16_16(cthQ15[i], stateGQ0[i])) + 16384), 15); 291a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin ARgQ0vec[i+1] = (WebRtc_Word16)WebRtcSpl_SatW32ToW16(tmp32); // Q0 292a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin ARfQ0vec[0] = tmpAR; 293a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin } 294a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin ARgQ0vec[0] = ARfQ0vec[0]; 295a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin 296a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin // Filter ARgQ0vec[] and ARfQ0vec[] through coefficients cthQ15[] and sthQ15[]. 297a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin WebRtcIsacfix_FilterArLoop(ARgQ0vec, ARfQ0vec, cthQ15, sthQ15, orderCoef); 298a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin 299a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin for(n=0;n<HALF_SUBFRAMELEN;n++) 300a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin { 301a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin lat_outQ0[n + temp1] = ARfQ0vec[n]; 302a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin } 303a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin 304a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin 305a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin /* cannot use memcpy in the following */ 306a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin 307a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin for (i=0;i<ord_1;i++) 308a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin { 309a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin stateGQ0[i] = ARgQ0vec[i]; 310a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin } 311a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin } 312a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin 313a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin return; 314a6451827d543eb00824bc95097e47d0aac51ae93Alexander Gutkin} 315