xref: /aosp_15_r20/external/libxaac/encoder/iusace_acelp_tools.c (revision 15dc779a375ca8b5125643b829a8aa4b70d7f451) 
1 /******************************************************************************
2  *                                                                            *
3  * Copyright (C) 2023 The Android Open Source Project
4  *
5  * Licensed under the Apache License, Version 2.0 (the "License");
6  * you may not use this file except in compliance with the License.
7  * You may obtain a copy of the License at:
8  *
9  * http://www.apache.org/licenses/LICENSE-2.0
10  *
11  * Unless required by applicable law or agreed to in writing, software
12  * distributed under the License is distributed on an "AS IS" BASIS,
13  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
14  * See the License for the specific language governing permissions and
15  * limitations under the License.
16  *
17  *****************************************************************************
18  * Originally developed and contributed by Ittiam Systems Pvt. Ltd, Bangalore
19  */
20 
21 #include <string.h>
22 #include <math.h>
23 #include "ixheaac_type_def.h"
24 #include "iusace_cnst.h"
25 #include "iusace_lpd_rom.h"
26 
iusace_acelp_ir_vec_corr1(FLOAT32 * ir,FLOAT32 * vec,UWORD8 track,FLOAT32 * sign,FLOAT32 (* corr_ir)[16],FLOAT32 * corr_out,WORD32 * dn2_pos,WORD32 num_pluse_pos)27 static VOID iusace_acelp_ir_vec_corr1(FLOAT32 *ir, FLOAT32 *vec, UWORD8 track, FLOAT32 *sign,
28                                       FLOAT32 (*corr_ir)[16], FLOAT32 *corr_out, WORD32 *dn2_pos,
29                                       WORD32 num_pluse_pos) {
30   WORD16 i, j;
31   WORD32 dn;
32   WORD32 *dn2;
33   FLOAT32 *p0;
34   FLOAT32 s;
35   dn2 = &dn2_pos[track * 8];
36   p0 = corr_ir[track];
37   for (i = 0; i < num_pluse_pos; i++) {
38     dn = dn2[i];
39     s = 0.0F;
40     for (j = 0; j < (LEN_SUBFR - dn); j++) {
41       s += ir[j] * vec[dn + j];
42     }
43     corr_out[dn >> 2] = sign[dn] * s + p0[dn >> 2];
44   }
45 }
46 
iusace_acelp_ir_vec_corr2(FLOAT32 * ir,FLOAT32 * vec,UWORD8 track,FLOAT32 * sign,FLOAT32 (* corr_ir)[16],FLOAT32 * corr_out)47 static VOID iusace_acelp_ir_vec_corr2(FLOAT32 *ir, FLOAT32 *vec, UWORD8 track, FLOAT32 *sign,
48                                       FLOAT32 (*corr_ir)[16], FLOAT32 *corr_out) {
49   WORD32 i, j;
50   FLOAT32 *p0;
51   FLOAT32 s;
52   p0 = corr_ir[track];
53   for (i = 0; i < 16; i++) {
54     s = 0.0F;
55     for (j = 0; j < LEN_SUBFR - track; j++) {
56       s += ir[j] * vec[track + j];
57     }
58     corr_out[i] = s * sign[track] + p0[i];
59     track += 4;
60   }
61 }
62 
iusace_acelp_get_2p_pos(WORD32 nb_pos_ix,UWORD8 track_p1,UWORD8 track_p2,FLOAT32 * corr_pulses,FLOAT32 * ener_pulses,WORD32 * pos_p1,WORD32 * pos_p2,FLOAT32 * dn,WORD32 * dn2,FLOAT32 * corr_p1,FLOAT32 * corr_p2,FLOAT32 (* corr_p1p2)[256])63 static VOID iusace_acelp_get_2p_pos(WORD32 nb_pos_ix, UWORD8 track_p1, UWORD8 track_p2,
64                                     FLOAT32 *corr_pulses, FLOAT32 *ener_pulses, WORD32 *pos_p1,
65                                     WORD32 *pos_p2, FLOAT32 *dn, WORD32 *dn2, FLOAT32 *corr_p1,
66                                     FLOAT32 *corr_p2, FLOAT32 (*corr_p1p2)[256]) {
67   WORD32 x, x2, y, x_save = 0, y_save = 0, i, *pos_x;
68   FLOAT32 ps0, alp0;
69   FLOAT32 ps1, ps2, sq, sqk;
70   FLOAT32 alp1, alp2, alpk;
71   FLOAT32 *p1, *p2;
72   FLOAT32 s;
73   pos_x = &dn2[track_p1 << 3];
74   ps0 = *corr_pulses;
75   alp0 = *ener_pulses;
76   sqk = -1.0F;
77   alpk = 1.0F;
78 
79   for (i = 0; i < nb_pos_ix; i++) {
80     x = pos_x[i];
81     x2 = x >> 2;
82 
83     ps1 = ps0 + dn[x];
84     alp1 = alp0 + corr_p1[x2];
85     p1 = corr_p2;
86     p2 = &corr_p1p2[track_p1][x2 << 4];
87     for (y = track_p2; y < LEN_SUBFR; y += 4) {
88       ps2 = ps1 + dn[y];
89       alp2 = alp1 + (*p1++) + (*p2++);
90       sq = ps2 * ps2;
91       s = (alpk * sq) - (sqk * alp2);
92       if (s > 0.0F) {
93         sqk = sq;
94         alpk = alp2;
95         y_save = y;
96         x_save = x;
97       }
98     }
99   }
100   *corr_pulses = ps0 + dn[x_save] + dn[y_save];
101   *ener_pulses = alpk;
102   *pos_p1 = x_save;
103   *pos_p2 = y_save;
104 }
105 
iusace_acelp_get_1p_pos(UWORD8 track_p1,UWORD8 track_p2,FLOAT32 * corr_pulses,FLOAT32 * alp,WORD32 * pos_p1,FLOAT32 * dn,FLOAT32 * corr_p1,FLOAT32 * corr_p2)106 static VOID iusace_acelp_get_1p_pos(UWORD8 track_p1, UWORD8 track_p2, FLOAT32 *corr_pulses,
107                                     FLOAT32 *alp, WORD32 *pos_p1, FLOAT32 *dn, FLOAT32 *corr_p1,
108                                     FLOAT32 *corr_p2) {
109   WORD32 x, x_save = 0;
110   FLOAT32 ps0, alp0;
111   FLOAT32 ps1, sq, sqk;
112   FLOAT32 alp1, alpk;
113   FLOAT32 s;
114 
115   ps0 = *corr_pulses;
116   alp0 = *alp;
117   sqk = -1.0F;
118   alpk = 1.0F;
119 
120   for (x = track_p1; x < LEN_SUBFR; x += 4) {
121     ps1 = ps0 + dn[x];
122     alp1 = alp0 + corr_p1[x >> 2];
123     sq = ps1 * ps1;
124     s = (alpk * sq) - (sqk * alp1);
125     if (s > 0.0F) {
126       sqk = sq;
127       alpk = alp1;
128       x_save = x;
129     }
130   }
131 
132   if (track_p2 != track_p1) {
133     for (x = track_p2; x < LEN_SUBFR; x += 4) {
134       ps1 = ps0 + dn[x];
135       alp1 = alp0 + corr_p2[x >> 2];
136       sq = ps1 * ps1;
137       s = (alpk * sq) - (sqk * alp1);
138       if (s > 0.0F) {
139         sqk = sq;
140         alpk = alp1;
141         x_save = x;
142       }
143     }
144   }
145 
146   *corr_pulses = ps0 + dn[x_save];
147   *alp = alpk;
148   *pos_p1 = x_save;
149 }
150 
iusace_acelp_quant_1p_n1bits(WORD32 pos_pulse,WORD32 num_bits_pos)151 static WORD32 iusace_acelp_quant_1p_n1bits(WORD32 pos_pulse, WORD32 num_bits_pos) {
152   WORD32 mask;
153   WORD32 index;
154   mask = ((1 << num_bits_pos) - 1);
155 
156   index = (pos_pulse & mask);
157   if ((pos_pulse & 16) != 0) {
158     index += 1 << num_bits_pos;
159   }
160   return (index);
161 }
162 
iusace_acelp_quant_2p_2n1bits(WORD32 pos_p1,WORD32 pos_p2,WORD32 num_bits_pos)163 static WORD32 iusace_acelp_quant_2p_2n1bits(WORD32 pos_p1, WORD32 pos_p2, WORD32 num_bits_pos) {
164   WORD32 mask;
165   WORD32 index;
166   mask = ((1 << num_bits_pos) - 1);
167 
168   if (((pos_p2 ^ pos_p1) & 16) == 0) {
169     if ((pos_p1 - pos_p2) <= 0) {
170       index = ((pos_p1 & mask) << num_bits_pos) + (pos_p2 & mask);
171     } else {
172       index = ((pos_p2 & mask) << num_bits_pos) + (pos_p1 & mask);
173     }
174     if ((pos_p1 & 16) != 0) {
175       index += 1 << (2 * num_bits_pos);
176     }
177   } else {
178     if (((pos_p1 & mask) - (pos_p2 & mask)) <= 0) {
179       index = ((pos_p2 & mask) << num_bits_pos) + (pos_p1 & mask);
180       if ((pos_p2 & 16) != 0) {
181         index += 1 << (2 * num_bits_pos);
182       }
183     } else {
184       index = ((pos_p1 & mask) << num_bits_pos) + (pos_p2 & mask);
185       if ((pos_p1 & 16) != 0) {
186         index += 1 << (2 * num_bits_pos);
187       }
188     }
189   }
190   return (index);
191 }
192 
iusace_acelp_quant_3p_3n1bits(WORD32 pos_p1,WORD32 pos_p2,WORD32 pos_p3,WORD32 num_bits_pos)193 static WORD32 iusace_acelp_quant_3p_3n1bits(WORD32 pos_p1, WORD32 pos_p2, WORD32 pos_p3,
194                                             WORD32 num_bits_pos) {
195   WORD32 nb_pos;
196   WORD32 index;
197   nb_pos = (1 << (num_bits_pos - 1));
198 
199   if (((pos_p1 ^ pos_p2) & nb_pos) == 0) {
200     index = iusace_acelp_quant_2p_2n1bits(pos_p1, pos_p2, (num_bits_pos - 1));
201     index += (pos_p1 & nb_pos) << num_bits_pos;
202     index += iusace_acelp_quant_1p_n1bits(pos_p3, num_bits_pos) << (2 * num_bits_pos);
203   } else if (((pos_p1 ^ pos_p3) & nb_pos) == 0) {
204     index = iusace_acelp_quant_2p_2n1bits(pos_p1, pos_p3, (num_bits_pos - 1));
205     index += (pos_p1 & nb_pos) << num_bits_pos;
206     index += iusace_acelp_quant_1p_n1bits(pos_p2, num_bits_pos) << (2 * num_bits_pos);
207   } else {
208     index = iusace_acelp_quant_2p_2n1bits(pos_p2, pos_p3, (num_bits_pos - 1));
209     index += (pos_p2 & nb_pos) << num_bits_pos;
210     index += iusace_acelp_quant_1p_n1bits(pos_p1, num_bits_pos) << (2 * num_bits_pos);
211   }
212   return (index);
213 }
214 
iusace_acelp_quant_4p_4n1bits(WORD32 pos_p1,WORD32 pos_p2,WORD32 pos_p3,WORD32 pos_p4,WORD32 num_bits_pos)215 static WORD32 iusace_acelp_quant_4p_4n1bits(WORD32 pos_p1, WORD32 pos_p2, WORD32 pos_p3,
216                                             WORD32 pos_p4, WORD32 num_bits_pos) {
217   WORD32 nb_pos;
218   WORD32 index;
219   nb_pos = (1 << (num_bits_pos - 1));
220 
221   if (((pos_p1 ^ pos_p2) & nb_pos) == 0) {
222     index = iusace_acelp_quant_2p_2n1bits(pos_p1, pos_p2, (num_bits_pos - 1));
223     index += (pos_p1 & nb_pos) << num_bits_pos;
224     index += iusace_acelp_quant_2p_2n1bits(pos_p3, pos_p4, num_bits_pos) << (2 * num_bits_pos);
225   } else if (((pos_p1 ^ pos_p3) & nb_pos) == 0) {
226     index = iusace_acelp_quant_2p_2n1bits(pos_p1, pos_p3, (num_bits_pos - 1));
227     index += (pos_p1 & nb_pos) << num_bits_pos;
228     index += iusace_acelp_quant_2p_2n1bits(pos_p2, pos_p4, num_bits_pos) << (2 * num_bits_pos);
229   } else {
230     index = iusace_acelp_quant_2p_2n1bits(pos_p2, pos_p3, (num_bits_pos - 1));
231     index += (pos_p2 & nb_pos) << num_bits_pos;
232     index += iusace_acelp_quant_2p_2n1bits(pos_p1, pos_p4, num_bits_pos) << (2 * num_bits_pos);
233   }
234   return (index);
235 }
236 
iusace_acelp_quant_4p_4nbits(WORD32 * pos_pulses,WORD32 num_bits_pos)237 static WORD32 iusace_acelp_quant_4p_4nbits(WORD32 *pos_pulses, WORD32 num_bits_pos) {
238   WORD32 i, j, k, nb_pos, n_1;
239   WORD32 pos_a[4], pos_b[4];
240   WORD32 index = 0;
241   n_1 = num_bits_pos - 1;
242   nb_pos = (1 << n_1);
243   i = 0;
244   j = 0;
245   for (k = 0; k < 4; k++) {
246     if ((pos_pulses[k] & nb_pos) == 0) {
247       pos_a[i++] = pos_pulses[k];
248     } else {
249       pos_b[j++] = pos_pulses[k];
250     }
251   }
252   switch (i) {
253     case 0:
254       index = 1 << ((4 * num_bits_pos) - 3);
255       index += iusace_acelp_quant_4p_4n1bits(pos_b[0], pos_b[1], pos_b[2], pos_b[3], n_1);
256       break;
257     case 1:
258       index = iusace_acelp_quant_1p_n1bits(pos_a[0], n_1) << ((3 * n_1) + 1);
259       index += iusace_acelp_quant_3p_3n1bits(pos_b[0], pos_b[1], pos_b[2], n_1);
260       break;
261     case 2:
262       index = iusace_acelp_quant_2p_2n1bits(pos_a[0], pos_a[1], n_1) << ((2 * n_1) + 1);
263       index += iusace_acelp_quant_2p_2n1bits(pos_b[0], pos_b[1], n_1);
264       break;
265     case 3:
266       index = iusace_acelp_quant_3p_3n1bits(pos_a[0], pos_a[1], pos_a[2], n_1) << num_bits_pos;
267       index += iusace_acelp_quant_1p_n1bits(pos_b[0], n_1);
268       break;
269     case 4:
270       index = iusace_acelp_quant_4p_4n1bits(pos_a[0], pos_a[1], pos_a[2], pos_a[3], n_1);
271       break;
272   }
273   index += (i & 3) << ((4 * num_bits_pos) - 2);
274   return (index);
275 }
276 
iusace_acelp_quant_5p_5nbits(WORD32 * pos_pulses,WORD32 num_bits_pos)277 static WORD32 iusace_acelp_quant_5p_5nbits(WORD32 *pos_pulses, WORD32 num_bits_pos) {
278   WORD32 i, j, k, nb_pos, n_1;
279   WORD32 pos_a[5], pos_b[5];
280   WORD32 index = 0;
281   n_1 = num_bits_pos - 1;
282   nb_pos = (1 << n_1);
283   i = 0;
284   j = 0;
285   for (k = 0; k < 5; k++) {
286     if ((pos_pulses[k] & nb_pos) == 0) {
287       pos_a[i++] = pos_pulses[k];
288     } else {
289       pos_b[j++] = pos_pulses[k];
290     }
291   }
292   switch (i) {
293     case 0:
294       index = 1 << ((5 * num_bits_pos) - 1);
295       index += iusace_acelp_quant_3p_3n1bits(pos_b[0], pos_b[1], pos_b[2], n_1)
296                << ((2 * num_bits_pos) + 1);
297       index += iusace_acelp_quant_2p_2n1bits(pos_b[3], pos_b[4], num_bits_pos);
298       break;
299     case 1:
300       index = 1 << ((5 * num_bits_pos) - 1);
301       index += iusace_acelp_quant_3p_3n1bits(pos_b[0], pos_b[1], pos_b[2], n_1)
302                << ((2 * num_bits_pos) + 1);
303       index += iusace_acelp_quant_2p_2n1bits(pos_b[3], pos_a[0], num_bits_pos);
304       break;
305     case 2:
306       index = 1 << ((5 * num_bits_pos) - 1);
307       index += iusace_acelp_quant_3p_3n1bits(pos_b[0], pos_b[1], pos_b[2], n_1)
308                << ((2 * num_bits_pos) + 1);
309       index += iusace_acelp_quant_2p_2n1bits(pos_a[0], pos_a[1], num_bits_pos);
310       break;
311     case 3:
312       index = iusace_acelp_quant_3p_3n1bits(pos_a[0], pos_a[1], pos_a[2], n_1)
313               << ((2 * num_bits_pos) + 1);
314       index += iusace_acelp_quant_2p_2n1bits(pos_b[0], pos_b[1], num_bits_pos);
315       break;
316     case 4:
317       index = iusace_acelp_quant_3p_3n1bits(pos_a[0], pos_a[1], pos_a[2], n_1)
318               << ((2 * num_bits_pos) + 1);
319       index += iusace_acelp_quant_2p_2n1bits(pos_a[3], pos_b[0], num_bits_pos);
320       break;
321     case 5:
322       index = iusace_acelp_quant_3p_3n1bits(pos_a[0], pos_a[1], pos_a[2], n_1)
323               << ((2 * num_bits_pos) + 1);
324       index += iusace_acelp_quant_2p_2n1bits(pos_a[3], pos_a[4], num_bits_pos);
325       break;
326   }
327   return (index);
328 }
329 
iusace_acelp_quant_6p_6n_2bits(WORD32 * pos_pulses,WORD32 num_bits_pos)330 static WORD32 iusace_acelp_quant_6p_6n_2bits(WORD32 *pos_pulses, WORD32 num_bits_pos) {
331   WORD32 i, j, k, nb_pos, n_1;
332   WORD32 pos_a[6], pos_b[6];
333   WORD32 index = 0;
334   n_1 = num_bits_pos - 1;
335   nb_pos = 1 << n_1;
336   i = 0;
337   j = 0;
338   for (k = 0; k < 6; k++) {
339     if ((pos_pulses[k] & nb_pos) == 0) {
340       pos_a[i++] = pos_pulses[k];
341     } else {
342       pos_b[j++] = pos_pulses[k];
343     }
344   }
345 
346   switch (i) {
347     case 0:
348       index = 1 << ((6 * num_bits_pos) - 5);
349       index += iusace_acelp_quant_5p_5nbits(pos_b, n_1) << num_bits_pos;
350       index += iusace_acelp_quant_1p_n1bits(pos_b[5], n_1);
351       break;
352     case 1:
353       index = 1 << ((6 * num_bits_pos) - 5);
354       index += iusace_acelp_quant_5p_5nbits(pos_b, n_1) << num_bits_pos;
355       index += iusace_acelp_quant_1p_n1bits(pos_a[0], n_1);
356       break;
357     case 2:
358       index = 1 << ((6 * num_bits_pos) - 5);
359       index += iusace_acelp_quant_4p_4nbits(pos_b, n_1) << ((2 * n_1) + 1);
360       index += iusace_acelp_quant_2p_2n1bits(pos_a[0], pos_a[1], n_1);
361       break;
362     case 3:
363       index = iusace_acelp_quant_3p_3n1bits(pos_a[0], pos_a[1], pos_a[2], n_1) << ((3 * n_1) + 1);
364       index += iusace_acelp_quant_3p_3n1bits(pos_b[0], pos_b[1], pos_b[2], n_1);
365       break;
366     case 4:
367       i = 2;
368       index = iusace_acelp_quant_4p_4nbits(pos_a, n_1) << ((2 * n_1) + 1);
369       index += iusace_acelp_quant_2p_2n1bits(pos_b[0], pos_b[1], n_1);
370       break;
371     case 5:
372       i = 1;
373       index = iusace_acelp_quant_5p_5nbits(pos_a, n_1) << num_bits_pos;
374       index += iusace_acelp_quant_1p_n1bits(pos_b[0], n_1);
375       break;
376     case 6:
377       i = 0;
378       index = iusace_acelp_quant_5p_5nbits(pos_a, n_1) << num_bits_pos;
379       index += iusace_acelp_quant_1p_n1bits(pos_a[5], n_1);
380       break;
381   }
382   index += (i & 3) << ((6 * num_bits_pos) - 4);
383   return (index);
384 }
385 
iusace_acelp_tgt_ir_corr(FLOAT32 * x,FLOAT32 * ir_wsyn,FLOAT32 * corr_out)386 VOID iusace_acelp_tgt_ir_corr(FLOAT32 *x, FLOAT32 *ir_wsyn, FLOAT32 *corr_out) {
387   WORD16 i, j;
388   FLOAT32 sum;
389   for (i = 0; i < LEN_SUBFR; i++) {
390     sum = 0.0F;
391     for (j = i; j < LEN_SUBFR; j++) {
392       sum += x[j] * ir_wsyn[j - i];
393     }
394     corr_out[i] = sum;
395   }
396 }
397 
iusace_acelp_tgt_cb_corr2(FLOAT32 * xn,FLOAT32 * y1,FLOAT32 * corr_out)398 FLOAT32 iusace_acelp_tgt_cb_corr2(FLOAT32 *xn, FLOAT32 *y1, FLOAT32 *corr_out) {
399   FLOAT32 gain;
400   FLOAT32 t0, t1;
401   WORD16 i;
402   t0 = xn[0] * y1[0];
403   t1 = y1[0] * y1[0];
404   for (i = 1; i < LEN_SUBFR; i += 7) {
405     t0 += xn[i] * y1[i];
406     t1 += y1[i] * y1[i];
407     t0 += xn[i + 1] * y1[i + 1];
408     t1 += y1[i + 1] * y1[i + 1];
409     t0 += xn[i + 2] * y1[i + 2];
410     t1 += y1[i + 2] * y1[i + 2];
411     t0 += xn[i + 3] * y1[i + 3];
412     t1 += y1[i + 3] * y1[i + 3];
413     t0 += xn[i + 4] * y1[i + 4];
414     t1 += y1[i + 4] * y1[i + 4];
415     t0 += xn[i + 5] * y1[i + 5];
416     t1 += y1[i + 5] * y1[i + 5];
417     t0 += xn[i + 6] * y1[i + 6];
418     t1 += y1[i + 6] * y1[i + 6];
419   }
420   corr_out[0] = t1;
421   corr_out[1] = -2.0F * t0 + 0.01F;
422 
423   if (t1) {
424     gain = t0 / t1;
425   } else {
426     gain = 1.0F;
427   }
428   if (gain < 0.0) {
429     gain = 0.0;
430   } else if (gain > 1.2F) {
431     gain = 1.2F;
432   }
433   return gain;
434 }
435 
iusace_acelp_tgt_cb_corr1(FLOAT32 * xn,FLOAT32 * y1,FLOAT32 * y2,FLOAT32 * corr_out)436 VOID iusace_acelp_tgt_cb_corr1(FLOAT32 *xn, FLOAT32 *y1, FLOAT32 *y2, FLOAT32 *corr_out) {
437   WORD32 i;
438   FLOAT32 temp1, temp2, temp3;
439   temp1 = 0.01F + y2[0] * y2[0];
440   temp2 = 0.01F + xn[0] * y2[0];
441   temp3 = 0.01F + y1[0] * y2[0];
442   temp1 += y2[1] * y2[1];
443   temp2 += xn[1] * y2[1];
444   temp3 += y1[1] * y2[1];
445   temp1 += y2[2] * y2[2];
446   temp2 += xn[2] * y2[2];
447   temp3 += y1[2] * y2[2];
448   temp1 += y2[3] * y2[3];
449   temp2 += xn[3] * y2[3];
450   temp3 += y1[3] * y2[3];
451   for (i = 4; i < LEN_SUBFR; i += 6) {
452     temp1 += y2[i] * y2[i];
453     temp2 += xn[i] * y2[i];
454     temp3 += y1[i] * y2[i];
455     temp1 += y2[i + 1] * y2[i + 1];
456     temp2 += xn[i + 1] * y2[i + 1];
457     temp3 += y1[i + 1] * y2[i + 1];
458     temp1 += y2[i + 2] * y2[i + 2];
459     temp2 += xn[i + 2] * y2[i + 2];
460     temp3 += y1[i + 2] * y2[i + 2];
461     temp1 += y2[i + 3] * y2[i + 3];
462     temp2 += xn[i + 3] * y2[i + 3];
463     temp3 += y1[i + 3] * y2[i + 3];
464     temp1 += y2[i + 4] * y2[i + 4];
465     temp2 += xn[i + 4] * y2[i + 4];
466     temp3 += y1[i + 4] * y2[i + 4];
467     temp1 += y2[i + 5] * y2[i + 5];
468     temp2 += xn[i + 5] * y2[i + 5];
469     temp3 += y1[i + 5] * y2[i + 5];
470   }
471   corr_out[2] = temp1;
472   corr_out[3] = -2.0F * temp2;
473   corr_out[4] = 2.0F * temp3;
474 }
475 
iusace_acelp_cb_target_update(FLOAT32 * x,FLOAT32 * new_x,FLOAT32 * cb_vec,FLOAT32 gain)476 VOID iusace_acelp_cb_target_update(FLOAT32 *x, FLOAT32 *new_x, FLOAT32 *cb_vec, FLOAT32 gain) {
477   WORD16 i;
478   for (i = 0; i < LEN_SUBFR; i++) {
479     new_x[i] = x[i] - gain * cb_vec[i];
480   }
481 }
482 
iusace_acelp_cb_exc(FLOAT32 * corr_input,FLOAT32 * lp_residual,FLOAT32 * ir_wsyn,WORD16 * alg_cb_exc_out,FLOAT32 * filt_cb_exc,WORD32 num_bits_cb,WORD32 * acelp_param_out,FLOAT32 * scratch_acelp_ir_buf)483 VOID iusace_acelp_cb_exc(FLOAT32 *corr_input, FLOAT32 *lp_residual, FLOAT32 *ir_wsyn,
484                          WORD16 *alg_cb_exc_out, FLOAT32 *filt_cb_exc, WORD32 num_bits_cb,
485                          WORD32 *acelp_param_out, FLOAT32 *scratch_acelp_ir_buf) {
486   FLOAT32 sign[LEN_SUBFR], vec[LEN_SUBFR];
487   FLOAT32 corr_x[16], corr_y[16];
488   FLOAT32 *ir_buf = scratch_acelp_ir_buf;
489   FLOAT32 corr_ir[4][16];
490   FLOAT32 corr_p1p2[4][256];
491   FLOAT32 dn2[LEN_SUBFR];
492   WORD32 pulse_pos[NPMAXPT * 4] = {0};
493   WORD32 codvec[MAX_NUM_PULSES] = {0};
494   WORD32 num_pulse_position[10] = {0};
495   WORD32 pos_max[4];
496   WORD32 dn2_pos[8 * 4];
497   UWORD8 ipos[MAX_NUM_PULSES] = {0};
498   WORD32 i, j, k, st, pos = 0, index, track, num_pulses = 0, num_iter = 4;
499   WORD32 l_index;
500   FLOAT32 psk, ps, alpk, alp = 0.0F;
501   FLOAT32 val;
502   FLOAT32 s, cor;
503   FLOAT32 *p0, *p1, *p2, *p3, *psign;
504   FLOAT32 *p1_ir_buf, *p2_ir_buf, *p3_ir_buf, *p4_ir_buf, *ir_sign_inv;
505   switch (num_bits_cb) {
506     case ACELP_NUM_BITS_20:
507       num_iter = 4;
508       alp = 2.0;
509       num_pulses = 4;
510       num_pulse_position[0] = 4;
511       num_pulse_position[1] = 8;
512       break;
513     case ACELP_NUM_BITS_28:
514       num_iter = 4;
515       alp = 1.5;
516       num_pulses = 6;
517       num_pulse_position[0] = 4;
518       num_pulse_position[1] = 8;
519       num_pulse_position[2] = 8;
520       break;
521 
522     case ACELP_NUM_BITS_36:
523       num_iter = 4;
524       alp = 1.0;
525       num_pulses = 8;
526       num_pulse_position[0] = 4;
527       num_pulse_position[1] = 8;
528       num_pulse_position[2] = 8;
529       break;
530     case ACELP_NUM_BITS_44:
531       num_iter = 4;
532       alp = 1.0;
533       num_pulses = 10;
534       num_pulse_position[0] = 4;
535       num_pulse_position[1] = 6;
536       num_pulse_position[2] = 8;
537       num_pulse_position[3] = 8;
538       break;
539     case ACELP_NUM_BITS_52:
540       num_iter = 4;
541       alp = 1.0;
542       num_pulses = 12;
543       num_pulse_position[0] = 4;
544       num_pulse_position[1] = 6;
545       num_pulse_position[2] = 8;
546       num_pulse_position[3] = 8;
547       break;
548     case ACELP_NUM_BITS_64:
549       num_iter = 3;
550       alp = 0.8F;
551       num_pulses = 16;
552       num_pulse_position[0] = 4;
553       num_pulse_position[1] = 4;
554       num_pulse_position[2] = 6;
555       num_pulse_position[3] = 6;
556       num_pulse_position[4] = 8;
557       num_pulse_position[5] = 8;
558       break;
559   }
560 
561   val = (lp_residual[0] * lp_residual[0]) + 1.0F;
562   cor = (corr_input[0] * corr_input[0]) + 1.0F;
563   for (i = 1; i < LEN_SUBFR; i += 7) {
564     val += (lp_residual[i] * lp_residual[i]);
565     cor += (corr_input[i] * corr_input[i]);
566     val += (lp_residual[i + 1] * lp_residual[i + 1]);
567     cor += (corr_input[i + 1] * corr_input[i + 1]);
568     val += (lp_residual[i + 2] * lp_residual[i + 2]);
569     cor += (corr_input[i + 2] * corr_input[i + 2]);
570     val += (lp_residual[i + 3] * lp_residual[i + 3]);
571     cor += (corr_input[i + 3] * corr_input[i + 3]);
572     val += (lp_residual[i + 4] * lp_residual[i + 4]);
573     cor += (corr_input[i + 4] * corr_input[i + 4]);
574     val += (lp_residual[i + 5] * lp_residual[i + 5]);
575     cor += (corr_input[i + 5] * corr_input[i + 5]);
576     val += (lp_residual[i + 6] * lp_residual[i + 6]);
577     cor += (corr_input[i + 6] * corr_input[i + 6]);
578   }
579   s = (FLOAT32)sqrt(cor / val);
580   for (j = 0; j < LEN_SUBFR; j++) {
581     cor = (s * lp_residual[j]) + (alp * corr_input[j]);
582     if (cor >= 0.0F) {
583       sign[j] = 1.0F;
584       vec[j] = -1.0F;
585       dn2[j] = cor;
586     } else {
587       sign[j] = -1.0F;
588       vec[j] = 1.0F;
589       corr_input[j] = -corr_input[j];
590       dn2[j] = -cor;
591     }
592   }
593   for (i = 0; i < 4; i++) {
594     for (k = 0; k < 8; k++) {
595       ps = -1;
596       for (j = i; j < LEN_SUBFR; j += 4) {
597         if (dn2[j] > ps) {
598           ps = dn2[j];
599           pos = j;
600         }
601       }
602       dn2[pos] = (FLOAT32)k - 8;
603       dn2_pos[i * 8 + k] = pos;
604     }
605     pos_max[i] = dn2_pos[i * 8];
606   }
607 
608   memset(ir_buf, 0, LEN_SUBFR * sizeof(FLOAT32));
609   memset(ir_buf + (2 * LEN_SUBFR), 0, LEN_SUBFR * sizeof(FLOAT32));
610   p1_ir_buf = ir_buf + LEN_SUBFR;
611   ir_sign_inv = ir_buf + (3 * LEN_SUBFR);
612   memcpy(p1_ir_buf, ir_wsyn, LEN_SUBFR * sizeof(FLOAT32));
613   ir_sign_inv[0] = -p1_ir_buf[0];
614   ir_sign_inv[1] = -p1_ir_buf[1];
615   ir_sign_inv[2] = -p1_ir_buf[2];
616   ir_sign_inv[3] = -p1_ir_buf[3];
617   for (i = 4; i < LEN_SUBFR; i += 6) {
618     ir_sign_inv[i] = -p1_ir_buf[i];
619     ir_sign_inv[i + 1] = -p1_ir_buf[i + 1];
620     ir_sign_inv[i + 2] = -p1_ir_buf[i + 2];
621     ir_sign_inv[i + 3] = -p1_ir_buf[i + 3];
622     ir_sign_inv[i + 4] = -p1_ir_buf[i + 4];
623     ir_sign_inv[i + 5] = -p1_ir_buf[i + 5];
624   }
625 
626   p0 = &corr_ir[0][16 - 1];
627   p1 = &corr_ir[1][16 - 1];
628   p2 = &corr_ir[2][16 - 1];
629   p3 = &corr_ir[3][16 - 1];
630   p2_ir_buf = p1_ir_buf;
631   cor = 0.0F;
632   for (i = 0; i < 16; i++) {
633     cor += (*p2_ir_buf) * (*p2_ir_buf);
634     p2_ir_buf++;
635     *p3-- = cor * 0.5F;
636     cor += (*p2_ir_buf) * (*p2_ir_buf);
637     p2_ir_buf++;
638     *p2-- = cor * 0.5F;
639     cor += (*p2_ir_buf) * (*p2_ir_buf);
640     p2_ir_buf++;
641     *p1-- = cor * 0.5F;
642     cor += (*p2_ir_buf) * (*p2_ir_buf);
643     p2_ir_buf++;
644     *p0-- = cor * 0.5F;
645   }
646   pos = 256 - 1;
647   p4_ir_buf = p1_ir_buf + 1;
648   for (k = 0; k < 16; k++) {
649     p3 = &corr_p1p2[2][pos];
650     p2 = &corr_p1p2[1][pos];
651     p1 = &corr_p1p2[0][pos];
652     if (k == 15) {
653       p0 = &corr_p1p2[3][pos - 15];
654     } else {
655       p0 = &corr_p1p2[3][pos - 16];
656     }
657     cor = 0.0F;
658     p2_ir_buf = p1_ir_buf;
659     p3_ir_buf = p4_ir_buf;
660     for (i = k + 1; i < 16; i++) {
661       cor += (*p2_ir_buf) * (*p3_ir_buf);
662       p2_ir_buf++;
663       p3_ir_buf++;
664       *p3 = cor;
665       cor += (*p2_ir_buf) * (*p3_ir_buf);
666       p2_ir_buf++;
667       p3_ir_buf++;
668       *p2 = cor;
669       cor += (*p2_ir_buf) * (*p3_ir_buf);
670       p2_ir_buf++;
671       p3_ir_buf++;
672       *p1 = cor;
673       cor += (*p2_ir_buf) * (*p3_ir_buf);
674       p2_ir_buf++;
675       p3_ir_buf++;
676       *p0 = cor;
677       p3 -= (16 + 1);
678       p2 -= (16 + 1);
679       p1 -= (16 + 1);
680       p0 -= (16 + 1);
681     }
682     cor += (*p2_ir_buf) * (*p3_ir_buf);
683     p2_ir_buf++;
684     p3_ir_buf++;
685     *p3 = cor;
686     cor += (*p2_ir_buf) * (*p3_ir_buf);
687     p2_ir_buf++;
688     p3_ir_buf++;
689     *p2 = cor;
690     cor += (*p2_ir_buf) * (*p3_ir_buf);
691     p2_ir_buf++;
692     p3_ir_buf++;
693     *p1 = cor;
694     pos -= 16;
695     p4_ir_buf += 4;
696   }
697   pos = 256 - 1;
698   p4_ir_buf = p1_ir_buf + 3;
699   for (k = 0; k < 16; k++) {
700     p3 = &corr_p1p2[3][pos];
701     p2 = &corr_p1p2[2][pos - 1];
702     p1 = &corr_p1p2[1][pos - 1];
703     p0 = &corr_p1p2[0][pos - 1];
704     cor = 0.0F;
705     p2_ir_buf = p1_ir_buf;
706     p3_ir_buf = p4_ir_buf;
707     for (i = k + 1; i < 16; i++) {
708       cor += (*p2_ir_buf) * (*p3_ir_buf);
709       p2_ir_buf++;
710       p3_ir_buf++;
711       *p3 = cor;
712       cor += (*p2_ir_buf) * (*p3_ir_buf);
713       p2_ir_buf++;
714       p3_ir_buf++;
715       *p2 = cor;
716       cor += (*p2_ir_buf) * (*p3_ir_buf);
717       p2_ir_buf++;
718       p3_ir_buf++;
719       *p1 = cor;
720       cor += (*p2_ir_buf) * (*p3_ir_buf);
721       p2_ir_buf++;
722       p3_ir_buf++;
723       *p0 = cor;
724       p3 -= (16 + 1);
725       p2 -= (16 + 1);
726       p1 -= (16 + 1);
727       p0 -= (16 + 1);
728     }
729     cor += (*p2_ir_buf) * (*p3_ir_buf);
730     p2_ir_buf++;
731     p3_ir_buf++;
732     *p3 = cor;
733     pos--;
734     p4_ir_buf += 4;
735   }
736 
737   p0 = &corr_p1p2[0][0];
738   for (k = 0; k < 4; k++) {
739     for (i = k; i < LEN_SUBFR; i += 4) {
740       psign = sign;
741       if (psign[i] < 0.0F) {
742         psign = vec;
743       }
744       j = (k + 1) % 4;
745       p0[0] = p0[0] * psign[j];
746       p0[1] = p0[1] * psign[j + 4];
747       p0[2] = p0[2] * psign[j + 8];
748       p0[3] = p0[3] * psign[j + 12];
749       p0[4] = p0[4] * psign[j + 16];
750       p0[5] = p0[5] * psign[j + 20];
751       p0[6] = p0[6] * psign[j + 24];
752       p0[7] = p0[7] * psign[j + 28];
753       p0[8] = p0[8] * psign[j + 32];
754       p0[9] = p0[9] * psign[j + 36];
755       p0[10] = p0[10] * psign[j + 40];
756       p0[11] = p0[11] * psign[j + 44];
757       p0[12] = p0[12] * psign[j + 48];
758       p0[13] = p0[13] * psign[j + 52];
759       p0[14] = p0[14] * psign[j + 56];
760       p0[15] = p0[15] * psign[j + 60];
761       p0 += 16;
762     }
763   }
764   psk = -1.0;
765   alpk = 1.0;
766   for (k = 0; k < num_iter; k++) {
767     for (i = 0; i < num_pulses - (num_pulses % 3); i += 3) {
768       ipos[i] = iusace_acelp_ipos[(k * 4) + i];
769       ipos[i + 1] = iusace_acelp_ipos[(k * 4) + i + 1];
770       ipos[i + 2] = iusace_acelp_ipos[(k * 4) + i + 2];
771     }
772     for (; i < num_pulses; i++) {
773       ipos[i] = iusace_acelp_ipos[(k * 4) + i];
774     }
775 
776     if ((num_bits_cb == 20) | (num_bits_cb == 28) | (num_bits_cb == 12) | (num_bits_cb == 16)) {
777       pos = 0;
778       ps = 0.0F;
779       alp = 0.0F;
780       memset(vec, 0, LEN_SUBFR * sizeof(FLOAT32));
781       if (num_bits_cb == 28) {
782         ipos[4] = 0;
783         ipos[5] = 1;
784       }
785 
786       if (num_bits_cb == 16) {
787         ipos[0] = 0;
788         ipos[1] = 2;
789         ipos[2] = 1;
790         ipos[3] = 3;
791       }
792     } else if ((num_bits_cb == 36) | (num_bits_cb == 44)) {
793       pos = 2;
794       pulse_pos[0] = pos_max[ipos[0]];
795       pulse_pos[1] = pos_max[ipos[1]];
796       ps = corr_input[pulse_pos[0]] + corr_input[pulse_pos[1]];
797       alp = corr_ir[ipos[0]][pulse_pos[0] >> 2] + corr_ir[ipos[1]][pulse_pos[1] >> 2] +
798             corr_p1p2[ipos[0]][((pulse_pos[0] >> 2) << 4) + (pulse_pos[1] >> 2)];
799       if (sign[pulse_pos[0]] < 0.0) {
800         p0 = ir_sign_inv - pulse_pos[0];
801       } else {
802         p0 = p1_ir_buf - pulse_pos[0];
803       }
804       if (sign[pulse_pos[1]] < 0.0) {
805         p1 = ir_sign_inv - pulse_pos[1];
806       } else {
807         p1 = p1_ir_buf - pulse_pos[1];
808       }
809       vec[0] = p0[0] + p1[0];
810       vec[1] = p0[1] + p1[1];
811       vec[2] = p0[2] + p1[2];
812       vec[3] = p0[3] + p1[3];
813       for (i = 4; i < LEN_SUBFR; i += 6) {
814         vec[i] = p0[i] + p1[i];
815         vec[i + 1] = p0[i + 1] + p1[i + 1];
816         vec[i + 2] = p0[i + 2] + p1[i + 2];
817         vec[i + 3] = p0[i + 3] + p1[i + 3];
818         vec[i + 4] = p0[i + 4] + p1[i + 4];
819         vec[i + 5] = p0[i + 5] + p1[i + 5];
820       }
821       if (num_bits_cb == 44) {
822         ipos[8] = 0;
823         ipos[9] = 1;
824       }
825     } else {
826       pos = 4;
827       pulse_pos[0] = pos_max[ipos[0]];
828       pulse_pos[1] = pos_max[ipos[1]];
829       pulse_pos[2] = pos_max[ipos[2]];
830       pulse_pos[3] = pos_max[ipos[3]];
831       ps = corr_input[pulse_pos[0]] + corr_input[pulse_pos[1]] + corr_input[pulse_pos[2]] +
832            corr_input[pulse_pos[3]];
833       p0 = p1_ir_buf - pulse_pos[0];
834       if (sign[pulse_pos[0]] < 0.0) {
835         p0 = ir_sign_inv - pulse_pos[0];
836       }
837       p1 = p1_ir_buf - pulse_pos[1];
838       if (sign[pulse_pos[1]] < 0.0) {
839         p1 = ir_sign_inv - pulse_pos[1];
840       }
841       p2 = p1_ir_buf - pulse_pos[2];
842       if (sign[pulse_pos[2]] < 0.0) {
843         p2 = ir_sign_inv - pulse_pos[2];
844       }
845       p3 = p1_ir_buf - pulse_pos[3];
846       if (sign[pulse_pos[3]] < 0.0) {
847         p3 = ir_sign_inv - pulse_pos[3];
848       }
849       vec[0] = p0[0] + p1[0] + p2[0] + p3[0];
850       for (i = 1; i < LEN_SUBFR; i += 3) {
851         vec[i] = p0[i] + p1[i] + p2[i] + p3[i];
852         vec[i + 1] = p0[i + 1] + p1[i + 1] + p2[i + 1] + p3[i + 1];
853         vec[i + 2] = p0[i + 2] + p1[i + 2] + p2[i + 2] + p3[i + 2];
854       }
855       alp = 0.0F;
856       alp += vec[0] * vec[0] + vec[1] * vec[1];
857       alp += vec[2] * vec[2] + vec[3] * vec[3];
858       for (i = 4; i < LEN_SUBFR; i += 6) {
859         alp += vec[i] * vec[i];
860         alp += vec[i + 1] * vec[i + 1];
861         alp += vec[i + 2] * vec[i + 2];
862         alp += vec[i + 3] * vec[i + 3];
863         alp += vec[i + 4] * vec[i + 4];
864         alp += vec[i + 5] * vec[i + 5];
865       }
866       alp *= 0.5F;
867       if (num_bits_cb == 72) {
868         ipos[16] = 0;
869         ipos[17] = 1;
870       }
871     }
872 
873     for (j = pos, st = 0; j < num_pulses; j += 2, st++) {
874       if ((num_pulses - j) >= 2) {
875         iusace_acelp_ir_vec_corr1(p1_ir_buf, vec, ipos[j], sign, corr_ir, corr_x, dn2_pos,
876                                   num_pulse_position[st]);
877         iusace_acelp_ir_vec_corr2(p1_ir_buf, vec, ipos[j + 1], sign, corr_ir, corr_y);
878 
879         iusace_acelp_get_2p_pos(num_pulse_position[st], ipos[j], ipos[j + 1], &ps, &alp,
880                                 &pulse_pos[j], &pulse_pos[j + 1], corr_input, dn2_pos, corr_x,
881                                 corr_y, corr_p1p2);
882       } else {
883         iusace_acelp_ir_vec_corr2(p1_ir_buf, vec, ipos[j], sign, corr_ir, corr_x);
884         iusace_acelp_ir_vec_corr2(p1_ir_buf, vec, ipos[j + 1], sign, corr_ir, corr_y);
885         iusace_acelp_get_1p_pos(ipos[j], ipos[j + 1], &ps, &alp, &pulse_pos[j], corr_input,
886                                 corr_x, corr_y);
887       }
888       if (j < (num_pulses - 2)) {
889         p0 = p1_ir_buf - pulse_pos[j];
890         if (sign[pulse_pos[j]] < 0.0) {
891           p0 = ir_sign_inv - pulse_pos[j];
892         }
893         p1 = p1_ir_buf - pulse_pos[j + 1];
894         if (sign[pulse_pos[j + 1]] < 0.0) {
895           p1 = ir_sign_inv - pulse_pos[j + 1];
896         }
897         vec[0] += p0[0] + p1[0];
898         vec[1] += p0[1] + p1[1];
899         vec[2] += p0[2] + p1[2];
900         vec[3] += p0[3] + p1[3];
901         for (i = 4; i < LEN_SUBFR; i += 6) {
902           vec[i] += p0[i] + p1[i];
903           vec[i + 1] += p0[i + 1] + p1[i + 1];
904           vec[i + 2] += p0[i + 2] + p1[i + 2];
905           vec[i + 3] += p0[i + 3] + p1[i + 3];
906           vec[i + 4] += p0[i + 4] + p1[i + 4];
907           vec[i + 5] += p0[i + 5] + p1[i + 5];
908         }
909       }
910     }
911     ps = ps * ps;
912     s = (alpk * ps) - (psk * alp);
913     if (s > 0.0F) {
914       psk = ps;
915       alpk = alp;
916       memcpy(codvec, pulse_pos, num_pulses * sizeof(WORD32));
917     }
918   }
919 
920   memset(alg_cb_exc_out, 0, LEN_SUBFR * sizeof(WORD16));
921   memset(filt_cb_exc, 0, LEN_SUBFR * sizeof(FLOAT32));
922   memset(pulse_pos, 0xffffffff, NPMAXPT * 4 * sizeof(WORD32));
923   for (k = 0; k < num_pulses; k++) {
924     i = codvec[k];
925     val = sign[i];
926     index = i / 4;
927     track = i % 4;
928     if (val > 0) {
929       alg_cb_exc_out[i] += 512;
930       codvec[k] += (2 * LEN_SUBFR);
931     } else {
932       alg_cb_exc_out[i] -= 512;
933       index += 16;
934     }
935     i = track * NPMAXPT;
936     while (pulse_pos[i] >= 0) {
937       i++;
938     }
939     pulse_pos[i] = index;
940     p0 = ir_sign_inv - codvec[k];
941     filt_cb_exc[0] += p0[0];
942     for (i = 1; i < LEN_SUBFR; i += 3) {
943       filt_cb_exc[i] += p0[i];
944       filt_cb_exc[i + 1] += p0[i + 1];
945       filt_cb_exc[i + 2] += p0[i + 2];
946     }
947   }
948 
949   if (num_bits_cb == ACELP_NUM_BITS_20) {
950     for (track = 0; track < 4; track++) {
951       k = track * NPMAXPT;
952       acelp_param_out[track] = iusace_acelp_quant_1p_n1bits(pulse_pos[k], 4);
953     }
954   } else if (num_bits_cb == ACELP_NUM_BITS_28) {
955     for (track = 0; track < (4 - 2); track++) {
956       k = track * NPMAXPT;
957       acelp_param_out[track] = iusace_acelp_quant_2p_2n1bits(pulse_pos[k], pulse_pos[k + 1], 4);
958     }
959     for (track = 2; track < 4; track++) {
960       k = track * NPMAXPT;
961       acelp_param_out[track] = iusace_acelp_quant_1p_n1bits(pulse_pos[k], 4);
962     }
963   } else if (num_bits_cb == ACELP_NUM_BITS_36) {
964     for (track = 0; track < 4; track++) {
965       k = track * NPMAXPT;
966       acelp_param_out[track] = iusace_acelp_quant_2p_2n1bits(pulse_pos[k], pulse_pos[k + 1], 4);
967     }
968   } else if (num_bits_cb == ACELP_NUM_BITS_44) {
969     for (track = 0; track < (4 - 2); track++) {
970       k = track * NPMAXPT;
971       acelp_param_out[track] =
972           iusace_acelp_quant_3p_3n1bits(pulse_pos[k], pulse_pos[k + 1], pulse_pos[k + 2], 4);
973     }
974     for (track = 2; track < 4; track++) {
975       k = track * NPMAXPT;
976       acelp_param_out[track] = iusace_acelp_quant_2p_2n1bits(pulse_pos[k], pulse_pos[k + 1], 4);
977     }
978   } else if (num_bits_cb == ACELP_NUM_BITS_52) {
979     for (track = 0; track < 4; track++) {
980       k = track * NPMAXPT;
981       acelp_param_out[track] =
982           iusace_acelp_quant_3p_3n1bits(pulse_pos[k], pulse_pos[k + 1], pulse_pos[k + 2], 4);
983     }
984   } else if (num_bits_cb == ACELP_NUM_BITS_64) {
985     for (track = 0; track < 4; track++) {
986       k = track * NPMAXPT;
987       l_index = iusace_acelp_quant_4p_4nbits(&pulse_pos[k], 4);
988       acelp_param_out[track] = ((l_index >> 14) & 3);
989       acelp_param_out[track + 4] = (l_index & 0x3FFF);
990     }
991   } else if (num_bits_cb == ACELP_NUM_BITS_72) {
992     for (track = 0; track < (4 - 2); track++) {
993       k = track * NPMAXPT;
994       l_index = iusace_acelp_quant_5p_5nbits(&pulse_pos[k], 4);
995       acelp_param_out[track] = ((l_index >> 10) & 0x03FF);
996       acelp_param_out[track + 4] = (l_index & 0x03FF);
997     }
998     for (track = 2; track < 4; track++) {
999       k = track * NPMAXPT;
1000       l_index = iusace_acelp_quant_4p_4nbits(&pulse_pos[k], 4);
1001       acelp_param_out[track] = ((l_index >> 14) & 3);
1002       acelp_param_out[track + 4] = (l_index & 0x3FFF);
1003     }
1004   } else if (num_bits_cb == ACELP_NUM_BITS_88) {
1005     for (track = 0; track < 4; track++) {
1006       k = track * NPMAXPT;
1007       l_index = iusace_acelp_quant_6p_6n_2bits(&pulse_pos[k], 4);
1008       acelp_param_out[track] = ((l_index >> 11) & 0x07FF);
1009       acelp_param_out[track + 4] = (l_index & 0x07FF);
1010     }
1011   }
1012   return;
1013 }
1014 
iusace_acelp_ltpred_cb_exc(FLOAT32 * exc,WORD32 t0,WORD32 t0_frac,WORD32 len_subfrm)1015 VOID iusace_acelp_ltpred_cb_exc(FLOAT32 *exc, WORD32 t0, WORD32 t0_frac, WORD32 len_subfrm) {
1016   WORD32 i, j;
1017   FLOAT32 s, *x0, *x1, *x2;
1018   const FLOAT32 *c1, *c2;
1019 
1020   x0 = &exc[-t0];
1021   t0_frac = -t0_frac;
1022   if (t0_frac < 0) {
1023     t0_frac += T_UP_SAMP;
1024     x0--;
1025   }
1026   for (j = 0; j < len_subfrm; j++) {
1027     x1 = x0++;
1028     x2 = x1 + 1;
1029     c1 = &iusace_res_interp_filter1_4[t0_frac];
1030     c2 = &iusace_res_interp_filter1_4[T_UP_SAMP - t0_frac];
1031     s = 0.0;
1032     for (i = 0; i < INTER_LP_FIL_ORDER; i++, c1 += T_UP_SAMP, c2 += T_UP_SAMP) {
1033       s += (*x1--) * (*c1) + (*x2++) * (*c2);
1034     }
1035     exc[j] = s;
1036   }
1037 }
1038 
iusace_acelp_quant_gain(FLOAT32 * code,FLOAT32 * pitch_gain,FLOAT32 * code_gain,FLOAT32 * tgt_cb_corr_data,FLOAT32 mean_energy,WORD32 * qunt_idx)1039 VOID iusace_acelp_quant_gain(FLOAT32 *code, FLOAT32 *pitch_gain, FLOAT32 *code_gain,
1040                              FLOAT32 *tgt_cb_corr_data, FLOAT32 mean_energy, WORD32 *qunt_idx) {
1041   WORD32 i, indice = 0, min_pitch_idx;
1042   FLOAT32 ener_code, pred_code_gain;
1043   FLOAT32 dist, dist_min, g_pitch, g_code;
1044   const FLOAT32 *p1_qua_gain_table, *p2_qua_gain_table;
1045 
1046   p1_qua_gain_table = iusace_acelp_quant_gain_table;
1047   p2_qua_gain_table = (const FLOAT32 *)(iusace_acelp_quant_gain_table + ACELP_GAIN_TBL_OFFSET);
1048   min_pitch_idx = 0;
1049   g_pitch = *pitch_gain;
1050   for (i = 0; i < ACELP_RANGE_GAIN_PT_IDX_SEARCH; i++, p2_qua_gain_table += 2) {
1051     if (g_pitch > *p2_qua_gain_table) {
1052       continue;
1053     }
1054   }
1055   ener_code = 0.01F;
1056 
1057   for (i = 0; i < LEN_SUBFR; i++) {
1058     ener_code += code[i] * code[i];
1059   }
1060 
1061   ener_code = (FLOAT32)(10.0 * log10(ener_code / (FLOAT32)LEN_SUBFR));
1062   pred_code_gain = mean_energy - ener_code;
1063   pred_code_gain = (FLOAT32)pow(10.0, pred_code_gain / 20.0);
1064 
1065   dist_min = MAX_FLT_VAL;
1066   p2_qua_gain_table = (const FLOAT32 *)(p1_qua_gain_table + min_pitch_idx * 2);
1067   for (i = 0; i < ACELP_SEARCH_RANGE_QUANTIZER_IDX; i++) {
1068     g_pitch = *p2_qua_gain_table++;
1069     g_code = pred_code_gain * *p2_qua_gain_table++;
1070     dist = g_pitch * g_pitch * tgt_cb_corr_data[0] + g_pitch * tgt_cb_corr_data[1] +
1071            g_code * g_code * tgt_cb_corr_data[2] + g_code * tgt_cb_corr_data[3] +
1072            g_pitch * g_code * tgt_cb_corr_data[4];
1073     if (dist < dist_min) {
1074       dist_min = dist;
1075       indice = i;
1076     }
1077   }
1078   indice += min_pitch_idx;
1079   *pitch_gain = p1_qua_gain_table[indice * 2];
1080   *code_gain = p1_qua_gain_table[indice * 2 + 1] * pred_code_gain;
1081   *qunt_idx = indice;
1082 }
1083