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