Line data Source code
1 : /*====================================================================================
2 : EVS Codec 3GPP TS26.452 Aug 12, 2021. Version 16.3.0
3 : ====================================================================================*/
4 :
5 : #include <assert.h>
6 : #include "typedef.h"
7 : #include <stdint.h>
8 : #include <string.h>
9 : #include "options.h"
10 : #include "cnst.h"
11 : #include "basop_util.h"
12 : #include "rom_com.h"
13 : #include "rom_enc.h"
14 : #include "prot_fx.h" /* Function prototypes */
15 : #include "prot_fx_enc.h" /* Function prototypes */
16 :
17 :
18 : #define _2_ 0x4000 /*Q12*/
19 : #define _1_ 0x2000 /*Q12*/
20 : #define _1_Q9 0x200
21 :
22 : static void E_ACELP_codearithp_fx( const Word16 v[] /*Q9*/, UWord32 *n /*Q0*/, UWord32 *ps /*Q0*/, Word16 *p /*Q0*/ );
23 :
24 : void E_ACELP_h_vec_corr1_fx( Word16 h[] /*Qx*/, Word16 vec[] /*Qx*/, UWord8 track /*Q0*/, Word16 sign[] /*Q15*/, Word16 ( *rrixix )[16] /*Q9*/, Word16 cor[] /*Q9*/, Word16 dn2_pos[] /*Q0*/, Word16 nb_pulse /*Q0*/ );
25 :
26 : void E_ACELP_h_vec_corr2_fx( Word16 h[] /*Qx*/, Word16 vec[] /*Qx*/, UWord8 track /*Q0*/, Word16 sign[] /*Q15*/, Word16 ( *rrixix )[16] /*Q9*/, Word16 cor[] /*Q9*/ );
27 :
28 : Word16 E_ACELP_xy1_corr_fx( Word16 xn[] /*Q15-exp_xn*/, Word16 y1[] /*Q15-exp_xn*/, ACELP_CbkCorr *g_corr, Word16 norm_flag /*Q0*/, Word16 L_subfr /*Q0*/, Word16 exp_xn );
29 :
30 : void E_ACELP_codebook_target_update_fx( Word16 *x /*Q_xn*/, Word16 *x2 /*Q_xn*/, Word16 *y /*Q_xn*/, Word16 gain /*Q14*/, Word16 L_subfr /*Q0*/ );
31 :
32 : void E_ACELP_vec_neg_fx( Word16 h[] /*Qx*/, Word16 h_inv[] /*Qx*/, Word16 L_subfr /*Q0*/ );
33 :
34 : void E_ACELP_corrmatrix_fx( Word16 h[] /*Q12*/, Word16 sign[] /*Q0*/, Word16 vec[] /*Q15*/, Word16 rrixix[4][16] /*Q9*/, Word16 rrixiy[4][256] /*Q9*/ );
35 :
36 : /*
37 : * E_ACELP_h_vec_corrx
38 : *
39 : * Parameters:
40 : * h I: scaled impulse response
41 : * vec I: vector to correlate with h[]
42 : * track I: track to use
43 : * sign I: sign vector
44 : * rrixix I: correlation of h[x] with h[x]
45 : * cor O: result of correlation (16 elements)
46 : *
47 : * Function:
48 : * Calculate the correlations of h[] with vec[] for the specified track
49 : *
50 : * Returns:
51 : * void
52 : */
53 1779211 : void E_ACELP_h_vec_corr1_fx(
54 : Word16 h[] /*Qx*/,
55 : Word16 vec[] /*Qx*/,
56 : UWord8 track /*Q0*/,
57 : Word16 sign[] /*Q15*/,
58 : Word16 ( *rrixix )[16] /*Q9*/,
59 : Word16 cor[] /*Q9*/,
60 : Word16 dn2_pos[] /*Q0*/,
61 : Word16 nb_pulse /*Q0*/ )
62 : {
63 : Word16 i, j;
64 : Word16 dn, corr;
65 : Word16 *dn2;
66 : Word16 *p0, *p1, *p2;
67 : Word32 L_sum;
68 :
69 1779211 : dn2 = &dn2_pos[( track * 8 )]; /*Q0*/
70 1779211 : p0 = rrixix[track]; /*Q9*/
71 :
72 13495811 : FOR( i = 0; i < nb_pulse; i++ )
73 : {
74 11716600 : dn = dn2[i]; /*Q0*/
75 11716600 : move16();
76 11716600 : L_sum = L_deposit_l( 0 );
77 11716600 : p1 = h; /*Qx*/
78 11716600 : p2 = &vec[dn]; /*Qx*/
79 409230624 : FOR( j = dn; j < L_SUBFR - 1; j++ )
80 : {
81 397514024 : L_sum = L_mac_sat( L_sum, *p1++, *p2++ ); /*2*Qx+1*/
82 : }
83 :
84 11716600 : corr = mac_r_sat( L_sum, *p1++, *p2++ ); /*Q9*/
85 :
86 : /*cor[dn >> 2] = sign[dn] * s + p0[dn >> 2];*/
87 11716600 : j = shr( dn, 2 );
88 11716600 : if ( sign[dn] > 0 )
89 : {
90 5765294 : corr = add_sat( p0[j], corr ); /*Q9*/
91 : }
92 11716600 : if ( sign[dn] < 0 )
93 : {
94 5951306 : corr = sub_sat( p0[j], corr ); /*Q9*/
95 : }
96 :
97 11716600 : cor[j] = corr; /*Q9*/
98 11716600 : move16();
99 : }
100 :
101 1779211 : return;
102 : }
103 :
104 :
105 2588657 : void E_ACELP_h_vec_corr2_fx(
106 : Word16 h[] /*Qx*/,
107 : Word16 vec[] /*Qx*/,
108 : UWord8 track /*Q0*/,
109 : Word16 sign[] /*Q15*/,
110 : Word16 ( *rrixix )[16] /*Q9*/,
111 : Word16 cor[] /*Q9*/ )
112 : {
113 : Word16 i, j, pos, corr;
114 : Word16 *p0, *p1, *p2;
115 : Word32 L_sum;
116 :
117 2588657 : p0 = rrixix[track]; /*Q9*/
118 :
119 2588657 : pos = track; /*Q0*/
120 2588657 : move16();
121 44007169 : FOR( i = 0; i < 16; i++ )
122 : {
123 41418512 : L_sum = L_deposit_l( 0 );
124 41418512 : p1 = h; /*Qx*/
125 41418512 : p2 = &vec[pos]; /*Qx*/
126 1348085520 : FOR( j = pos; j < L_SUBFR - 1; j++ )
127 : {
128 1306667008 : L_sum = L_mac_sat( L_sum, *p1++, *p2++ ); /* 2*Qx+1 */
129 : }
130 41418512 : corr = mac_r_sat( L_sum, *p1++, *p2++ ); /*Q9*/
131 :
132 : /*cor[i] = s * sign[track] + p0[i];*/
133 :
134 41418512 : if ( sign[pos] > 0 )
135 : {
136 20455335 : corr = add_sat( *p0++, corr ); /*Q9*/
137 : }
138 41418512 : if ( sign[pos] < 0 )
139 : {
140 20963177 : corr = sub_sat( *p0++, corr ); /*Q9*/
141 : }
142 41418512 : cor[i] = corr; /*Q9*/
143 41418512 : move16();
144 :
145 41418512 : pos = add( pos, 4 );
146 : }
147 :
148 2588657 : return;
149 : }
150 :
151 :
152 : /*
153 : * E_ACELP_2pulse_search
154 : *
155 : * Parameters:
156 : * nb_pos_ix I: nb of pos for pulse 1 (1..8)
157 : * track_x I: track of pulse 1
158 : * track_y I: track of pulse 2
159 : * ps I/O: correlation of all fixed pulses
160 : * alp I/O: energy of all fixed pulses
161 : * ix O: position of pulse 1
162 : * iy O: position of pulse 2
163 : * dn I: corr. between target and h[]
164 : * dn2 I: vector of selected positions
165 : * cor_x I: corr. of pulse 1 with fixed pulses
166 : * cor_y I: corr. of pulse 2 with fixed pulses
167 : * rrixiy I: corr. of pulse 1 with pulse 2
168 : *
169 : * Function:
170 : * Find the best positions of 2 pulses in a subframe
171 : *
172 : * Returns:
173 : * void
174 : */
175 1779211 : static void E_ACELP_2pulse_search(
176 : Word16 nb_pos_ix /*Q0*/,
177 : UWord8 track_x /*Q0*/,
178 : UWord8 track_y /*Q0*/,
179 : Word16 *ps /*Qdn*/,
180 : Word16 *alp /*Qx*/,
181 : Word16 *ix /*Q0*/,
182 : Word16 *iy /*Q0*/,
183 : Word16 dn[] /*Qdn*/,
184 : Word16 *dn2 /*Q0*/,
185 : Word16 cor_x[] /*Qx*/,
186 : Word16 cor_y[] /*Qx*/,
187 : Word16 ( *rrixiy )[256] /*Q9*/ )
188 : {
189 : Word16 x, x2, y, i, *pos_x;
190 : Word16 ps0, ps1, alp2_16, ps2, sq;
191 : Word32 alp0, alp1, alp2, s;
192 : Word16 *p1, *p2;
193 : Word16 sqk[2], alpk[2], ik;
194 : Word32 xy_save;
195 1779211 : Word16 check = 0; /* debug code not instrumented */
196 :
197 : /* eight dn2 max positions per track */
198 : /*pos_x = &dn2[track_x << 3]; SHIFT(1); PTR_INIT(1);*/
199 1779211 : pos_x = &dn2[( track_x * 8 )]; /*Qdn*/
200 1779211 : move16();
201 :
202 : /* save these to limit memory searches */
203 1779211 : alp0 = L_deposit_h( *alp ); /*Qx-16*/
204 1779211 : ps0 = *ps; /*Qdn*/
205 1779211 : move16();
206 :
207 1779211 : alpk[0] = 1;
208 1779211 : move16();
209 1779211 : sqk[0] = -1;
210 1779211 : move16();
211 1779211 : x2 = shr( pos_x[0], 2 ); /*Qdn*/
212 1779211 : if ( mac_r_sat( L_mac_sat( L_mac_sat( alp0, cor_x[x2], _1_ ), cor_y[0], _1_ ), rrixiy[track_x][( x2 * 16 )], _1_ ) < 0 )
213 : {
214 0 : sqk[0] = 1;
215 0 : move16();
216 : }
217 1779211 : ik = 0;
218 1779211 : move16();
219 1779211 : xy_save = L_mac0( L_deposit_l( track_y ), track_x, L_SUBFR ); /*Q0*/
220 :
221 : /* loop track 1 */
222 13495811 : FOR( i = 0; i < nb_pos_ix; i++ )
223 : {
224 11716600 : x = pos_x[i]; /*Qdn*/
225 11716600 : move16();
226 11716600 : x2 = shr( x, 2 );
227 : /* dn[x] has only nb_pos_ix positions saved */
228 : /*ps1 = ps0 + dn[x];*/
229 11716600 : ps1 = add( ps0, dn[x] ); /*Qdn*/
230 :
231 : /*alp1 = alp0 + cor_x[x2];*/
232 11716600 : alp1 = L_mac_sat( alp0, cor_x[x2], _1_ ); /*Q22*/
233 :
234 11716600 : p1 = cor_y; /*Qx*/
235 11716600 : p2 = &rrixiy[track_x][( x2 * 16 )]; /*Q9*/
236 :
237 199182200 : FOR( y = track_y; y < L_SUBFR; y += 4 )
238 : {
239 : /*ps2 = ps1 + dn[y];*/
240 187465600 : ps2 = add( ps1, dn[y] ); /*Qdn*/
241 187465600 : move16();
242 :
243 : /*alp2 = alp1 + (*p1++) + (*p2++);*/
244 187465600 : alp2 = L_mac_sat( alp1, *p1++, _1_ ); /*Qx+12+1*/
245 187465600 : alp2_16 = mac_r_sat( alp2, *p2++, _1_ ); /*Q6*/
246 187465600 : alpk[1 - ik] = alp2_16; /*Q6*/
247 187465600 : move16();
248 :
249 : /*sq = ps2 * ps2;*/
250 187465600 : sq = mult( ps2, ps2 ); /*2*Qdn+1*/
251 187465600 : sqk[1 - ik] = sq; /*2*Qdn+1*/
252 187465600 : move16();
253 :
254 : /*s = (alpk[ik] * sq) - (sqk[0] * alp2);*/
255 187465600 : s = L_msu( L_mult( alpk[ik], sq ), sqk[ik], alp2_16 ); /*Q16*/
256 :
257 187465600 : if ( s > 0 )
258 : {
259 8646628 : ik = sub( 1, ik );
260 8646628 : move16();
261 8646628 : check = 1; /* debug code not instrumented */
262 8646628 : move16();
263 : }
264 187465600 : if ( s > 0 )
265 : {
266 8646628 : xy_save = L_mac0( y, x, L_SUBFR ); /*Qdn*/
267 : }
268 : }
269 : }
270 :
271 1779211 : assert( check ); /* debug code not instrumented */
272 :
273 1779211 : ps2 = extract_l( xy_save ); /*Qdn*/
274 1779211 : *iy = s_and( ps2, L_SUBFR - 1 ); /*Q0*/
275 1779211 : move16();
276 1779211 : *ix = lshr( ps2, 6 ); /*Q0*/
277 1779211 : move16();
278 :
279 : /**ps = ps0 + dn[*ix] + dn[*iy];*/
280 1779211 : *ps = add( ps0, add( dn[*ix], dn[*iy] ) ); /*Qdn*/
281 1779211 : move16();
282 :
283 1779211 : *alp = alpk[ik]; /*Q6*/
284 1779211 : move16();
285 :
286 1779211 : return;
287 : }
288 :
289 :
290 : /*
291 : * E_ACELP_1pulse_search
292 : *
293 : * Parameters:
294 : * track_x I: track of pulse 1
295 : * track_y I: track of pulse 2
296 : * ps I/O: correlation of all fixed pulses
297 : * alp I/O: energy of all fixed pulses
298 : * ix O: position of pulse 1
299 : * dn I: corr. between target and h[]
300 : * cor_x I: corr. of pulse 1 with fixed pulses
301 : * cor_y I: corr. of pulse 2 with fixed pulses
302 : *
303 : * Function:
304 : * Find the best positions of 1 pulse in a subframe
305 : *
306 : * Returns:
307 : * void
308 : */
309 404723 : static void E_ACELP_1pulse_search(
310 : UWord8 tracks[2],
311 : Word16 *ps, /*Qdn*/
312 : Word16 *alp, /*Qx*/
313 : Word16 *ix, /*Q0*/
314 : Word16 dn[], /*Qdn*/
315 : Word16 cor_x[], /*Q6*/
316 : Word16 cor_y[] /*Q6*/ )
317 : {
318 404723 : Word16 x, x_save = 0;
319 : Word16 ps0;
320 : Word16 ps1, sq;
321 : Word16 alp1;
322 : Word32 s, alp0;
323 : Word16 sqk[2], alpk[2], ik;
324 : Word16 ntracks, t;
325 404723 : Word16 check = 0; /* debug code not instrumented */
326 :
327 : /* save these to limit memory searches */
328 404723 : alp0 = L_deposit_h( *alp );
329 404723 : ps0 = *ps; /*Qdn*/
330 404723 : move16();
331 :
332 404723 : alpk[0] = 1;
333 404723 : move16();
334 404723 : sqk[0] = -1;
335 404723 : move16();
336 404723 : if ( mac_r_sat( alp0, cor_x[( tracks[0] / 4 )], _1_ ) < 0 )
337 : {
338 0 : sqk[0] = 1;
339 0 : move16();
340 : }
341 404723 : ik = 0;
342 404723 : move16();
343 :
344 404723 : ntracks = 1;
345 404723 : move16();
346 404723 : if ( NE_16( tracks[1], tracks[0] ) )
347 : {
348 243512 : ntracks = 2;
349 243512 : move16();
350 : }
351 1052958 : FOR( t = 0; t < ntracks; ++t )
352 : {
353 648235 : if ( t != 0 )
354 : {
355 243512 : cor_x = cor_y;
356 243512 : move16();
357 : }
358 11019995 : FOR( x = tracks[t]; x < L_SUBFR; x += 4 )
359 : {
360 : /*ps1 = ps0 + dn[x]; ADD(1);*/
361 10371760 : ps1 = add( ps0, dn[x] ); /*Qdn*/
362 :
363 : /*alp1 = alp0 + cor_x[x>>2]; SHIFT(1);ADD(1);*/
364 10371760 : alp1 = mac_r_sat( alp0, cor_x[( x / 4 )], _1_ ); /*Q6*/
365 10371760 : alpk[1 - ik] = alp1; /*Q6*/
366 10371760 : move16();
367 :
368 : /*sq = ps1 * ps1; MULT(1);*/
369 10371760 : sq = mult( ps1, ps1 ); /*2*Qdn+1*/
370 10371760 : sqk[1 - ik] = sq; /*2*Qdn+1*/
371 10371760 : move16();
372 :
373 : /*s = (alpk * sq) - (sqk * alp1); MULT(1);MAC(1); */
374 10371760 : s = L_msu( L_mult( alpk[ik], sq ), sqk[ik], alp1 ); /*Q16*/
375 :
376 10371760 : if ( s > 0 )
377 : {
378 1696658 : ik = sub( 1, ik );
379 1696658 : check = 1; /* debug code not instrumented */
380 : }
381 10371760 : if ( s > 0 )
382 : {
383 1696658 : x_save = x;
384 1696658 : move16();
385 : }
386 : }
387 :
388 648235 : assert( check ); /* debug code not instrumented */
389 : }
390 404723 : *ps = add( ps0, dn[x_save] ); /*2*Qdn+1*/
391 404723 : move16();
392 404723 : *alp = alpk[ik]; /*Q6*/
393 404723 : move16();
394 404723 : *ix = x_save; /*Q0*/
395 404723 : move16();
396 :
397 404723 : return;
398 : }
399 :
400 :
401 : /*
402 : * E_ACELP_xh_corr
403 : *
404 : * Parameters:
405 : * h I: impulse response (of weighted synthesis filter) (Q12)
406 : * x I: target signal (Q0)
407 : * y O: correlation between x[] and h[] <12b
408 : *
409 : * Function:
410 : * Compute the correlation between the target signal and the impulse
411 : * response of the weighted synthesis filter.
412 : *
413 : * y[i]=sum(j=i,l-1) x[j]*h[j-i], i=0,l-1
414 : *
415 : * Vector size is L_SUBFR
416 : *
417 : * Returns:
418 : * void
419 : */
420 0 : static void E_ACELP_xh_corr(
421 : Word16 *x /*Qx*/,
422 : Word16 *y /*Qy*/,
423 : Word16 *h /*Q12*/,
424 : Word16 L_subfr /*Q0*/ )
425 : {
426 : Word16 i, j, k;
427 : Word32 L_tmp, y32[L_SUBFR16k], L_maxloc, L_tot;
428 :
429 0 : assert( L_subfr <= L_SUBFR16k );
430 :
431 : /* first keep the result on 32 bits and find absolute maximum */
432 0 : L_tot = L_deposit_l( 1 );
433 :
434 0 : FOR( k = 0; k < 4; k++ )
435 : {
436 0 : L_maxloc = L_deposit_l( 0 );
437 0 : FOR( i = k; i < L_subfr; i += 4 )
438 : {
439 0 : L_tmp = L_mac0( 1L, x[i], h[0] ); /* 1 -> to avoid null dn[] Qx+Q12*/
440 0 : FOR( j = i; j < L_subfr - 1; j++ )
441 : {
442 0 : L_tmp = L_mac0( L_tmp, x[j + 1], h[j + 1 - i] ); /*Qx+Q12*/
443 : }
444 :
445 0 : y32[i] = L_tmp; /*Qx+Q12*/
446 0 : move32();
447 0 : L_tmp = L_abs( L_tmp ); /*Qx+Q12*/
448 0 : L_maxloc = L_max( L_tmp, L_maxloc ); /*Qx+Q12*/
449 : }
450 : /* tot += 3*max / 8 */
451 0 : L_maxloc = L_shr( L_maxloc, 2 );
452 : /* Do not warn saturation of L_tot, since its for headroom estimation. */
453 : BASOP_SATURATE_WARNING_OFF_EVS
454 0 : L_tot = L_add_sat( L_tot, L_maxloc ); /* +max/4 */
455 0 : L_tot = L_add_sat( L_tot, L_shr( L_maxloc, 1 ) ); /* +max/8 */
456 : BASOP_SATURATE_WARNING_ON_EVS
457 : }
458 :
459 : /* Find the number of right shifts to do on y32[] so that */
460 : /* 6.0 x sumation of max of dn[] in each track not saturate. */
461 :
462 0 : j = sub( norm_l( L_tot ), 4 + 16 ); /* 4 -> 16 x tot */
463 :
464 0 : Copy_Scale_sig_32_16( y32, y, L_subfr, j ); /*Qy = Qx+Q12+j*/
465 0 : return;
466 : }
467 :
468 : /**
469 : * \brief calculate autocorrelation of vector x
470 : * \param x input vector 4Q11
471 : * \param y output (autocorrelation coefficients)
472 : * \param L_subfr length of x (and y)
473 : * \param bits amount of target headroom bits for y
474 : * \return exponent of y
475 : */
476 407667 : Word16 E_ACELP_hh_corr(
477 : Word16 *x /*Q11*/,
478 : Word16 *y /*Qy*/,
479 : Word16 L_subfr /*Q0*/,
480 : Word16 bits /*Q0*/ )
481 : {
482 407667 : Word16 i, j, k = 0; /* initialize just to avoid compiler warning */
483 : Word32 L_tmp, L_sum;
484 :
485 26090688 : FOR( i = 0; i < L_subfr - 1; i++ )
486 : {
487 : Word64 L_tmp_64;
488 : Word64 L_sum_64;
489 :
490 25683021 : L_tmp_64 = W_mult0_16_16( x[i], x[0] ); /*Q22*/
491 430088685 : FOR( j = i + 2; j < L_subfr; j += 2 )
492 : {
493 404405664 : L_tmp_64 = W_mac0_16_16( L_tmp_64, x[j], x[j - i] ); /*Q22*/
494 : }
495 25683021 : L_sum_64 = L_tmp_64;
496 25683021 : move64();
497 :
498 25683021 : L_tmp_64 = W_mult0_16_16( x[i + 1], x[1] ); /*Q22*/
499 417451008 : FOR( j = i + 3; j < L_subfr; j += 2 )
500 : {
501 391767987 : L_tmp_64 = W_mac0_16_16( L_tmp_64, x[j], x[j - i] ); /*Q22*/
502 : }
503 25683021 : L_sum_64 = W_add_nosat( W_shr( L_sum_64, 1 ), W_shr( L_tmp_64, 1 ) );
504 25683021 : L_sum = W_sat_l( L_sum_64 ); /*Q22*/
505 : /* L_sum = L_shr( L_sum, 1 ); */
506 25683021 : if ( i == 0 )
507 : {
508 407667 : k = norm_l( L_sum );
509 : }
510 25683021 : if ( i == 0 )
511 : {
512 407667 : k = sub( k, bits );
513 : }
514 :
515 25683021 : y[i] = round_fx( L_shl( L_sum, k ) ); /*Q22+k-16*/
516 : }
517 :
518 407667 : L_tmp = L_mult0( x[i], x[0] ); /*Q22*/
519 407667 : L_sum = L_shr( L_tmp, 1 );
520 407667 : y[i] = round_fx( L_shl( L_sum, k ) ); /*Qy = Q22+k-16*/
521 :
522 407667 : k = add( 1, k );
523 :
524 407667 : return k;
525 : }
526 :
527 : /*
528 : * E_ACELP_xy1_corr
529 : *
530 : * Parameters:
531 : * xn I: target signal
532 : * y1 I: filtered adaptive codebook excitation
533 : * g_coeff O: correlations <y1,y1> and -2<xn,y1>
534 : * norm_flag I: flag to trigger normalization of the result
535 : * L_subfr I: length of data
536 : * exp_xn I: common exponent of xn[] and y1[]
537 : *
538 : * Function:
539 : * Find the correlations between the target xn[] and the filtered adaptive
540 : * codebook excitation y1[]. ( <y1,y1> and -2<xn,y1> )
541 : * Subframe size = L_SUBFR
542 : *
543 : * Returns:
544 : * pitch gain (0 ... 1.2F) (Q14)
545 : */
546 4410 : Word16 E_ACELP_xy1_corr_fx(
547 : Word16 xn[] /*Q15-exp_xn*/,
548 : Word16 y1[] /*Q15-exp_xn*/,
549 : ACELP_CbkCorr *g_corr,
550 : Word16 norm_flag /*Q0*/,
551 : Word16 L_subfr /*Q0*/,
552 : Word16 exp_xn )
553 : {
554 : Word16 i, Q_xn;
555 : Word16 xy, yy, exp_xy, exp_yy, gain;
556 : Word32 L_off;
557 :
558 4410 : L_off = L_shr( 10737418l /*0.01f/2.0f Q31*/, s_min( add( exp_xn, exp_xn ), 31 ) );
559 4410 : L_off = L_max( 1, L_off ); /* ensure at least a '1' */
560 :
561 : /* Compute scalar product t1: <y1[] * y1[]> */
562 4410 : yy = round_fx_sat( Dot_product15_offs( y1, y1, L_subfr, &exp_yy, L_off ) ); /*Q15 - exp_yy*/
563 :
564 : /* Compute scalar product t0: <xn[] * y1[]> */
565 4410 : xy = round_fx_sat( Dot_product12_offs( xn, y1, L_subfr, &exp_xy, L_off ) ); /*Q15 - exp_xy*/
566 :
567 : /* Compute doubled format out of the exponent */
568 4410 : Q_xn = shl( sub( 15, exp_xn ), 1 );
569 4410 : g_corr->y1y1 = yy;
570 4410 : move16();
571 4410 : g_corr->y1y1_e = sub( exp_yy, Q_xn );
572 4410 : move16();
573 4410 : g_corr->xy1 = xy;
574 4410 : move16();
575 4410 : g_corr->xy1_e = sub( exp_xy, Q_xn );
576 4410 : move16();
577 :
578 : /* If (xy < 0) gain = 0 */
579 4410 : IF( xy < 0 )
580 : {
581 49 : move16();
582 49 : gain = 0;
583 49 : GOTO bail;
584 : }
585 :
586 : /* compute gain = xy/yy */
587 :
588 4361 : xy = mult_r( xy, 0x4000 ); /* Be sure xy < yy Q15 - exp_xy*/
589 4361 : gain = div_s( xy, yy ); /*Q14*/
590 :
591 4361 : i = add( exp_xy, 1 - 1 ); /* -1 -> gain in Q14 */
592 4361 : i = sub( i, exp_yy );
593 : BASOP_SATURATE_WARNING_OFF_EVS
594 4361 : gain = shl_sat( gain, i ); /* saturation can occur here */
595 : BASOP_SATURATE_WARNING_ON_EVS
596 : /* gain = s_max(0, gain); */ /* see above xy < 0. */
597 :
598 : /* if (gain > 1.2) gain = 1.2 in Q14 */
599 :
600 4361 : gain = s_min( 19661 /*1.2f Q14*/ /* 19661 */, gain ); /*Q14*/
601 :
602 : /*Limit the energy of pitch contribution*/
603 4361 : IF( norm_flag )
604 : {
605 : Word16 tmp, exp_tmp, exp_div;
606 :
607 : /* Compute scalar product <xn[],xn[]> */
608 4361 : tmp = round_fx_sat( Dot_product12_offs( xn, xn, L_subfr, &exp_tmp, 1 ) ); /*Q15 - exp_tmp*/
609 : /* gain_p_snr = sqrt(<xn,xn>/<y1,y1>) */
610 4361 : tmp = BASOP_Util_Divide1616_Scale( tmp, yy, &exp_div );
611 4361 : exp_tmp = add( sub( exp_tmp, exp_yy ), exp_div );
612 :
613 4361 : tmp = Sqrt16( tmp, &exp_tmp ); /*Q15 - exp_tmp*/
614 :
615 : /* Note: shl works as shl or shr. */
616 4361 : exp_tmp = sub( exp_tmp, 1 );
617 : BASOP_SATURATE_WARNING_OFF_EVS
618 4361 : tmp = round_fx_sat( L_shl_sat( Mpy_32_16_1( 1717986944l /*ACELP_GAINS_CONST Q31*/, tmp ), exp_tmp ) ); /*Q14*/
619 : BASOP_SATURATE_WARNING_ON_EVS
620 :
621 4361 : gain = s_min( gain, tmp ); /*Q14*/
622 : }
623 :
624 0 : bail:
625 :
626 4410 : return ( gain );
627 : }
628 :
629 : /*
630 : * E_ACELP_xy2_corr
631 : *
632 : * Parameters:
633 : * xn I: target signal in Q_xn
634 : * y1 I: filtered adaptive codebook excitation in Q_xn
635 : * y2 I: filtered fixed codebook excitation in Q9
636 : * g_corr O: correlations <y2,y2>, -2<xn,y2>, 2<y1,y2>
637 : * L_subfr I: subframe size
638 : *
639 : * Function:
640 : * Find the correlations between the target xn[], the filtered adaptive
641 : * codebook exc. y1[], and the filtered fixed codebook innovation y2[].
642 : * ( <y2,y2> , -2<xn,y2> and 2<y1,y2> )
643 : * Subrame size = L_SUBFR
644 : *
645 : * Returns:
646 : * pitch gain (0 ... 1.2F)
647 : */
648 11120 : void E_ACELP_xy2_corr(
649 : Word16 xn[] /*Q_xn*/,
650 : Word16 y1[] /*Q_xn*/,
651 : Word16 y2[] /*Q9*/,
652 : ACELP_CbkCorr *g_corr,
653 : Word16 L_subfr /*Q0*/,
654 : Word16 exp_xn )
655 : {
656 : Word16 xny2, y2y2, y1y2, xx, exp_xny2, exp_y2y2, exp_y1y2, exp_xx;
657 : Word32 L_off;
658 :
659 : BASOP_SATURATE_ERROR_ON_EVS;
660 :
661 : /* Compute scalar product <y2[],y2[]> */
662 11120 : y2y2 = extract_h( Dot_product15_offs( y2, y2, L_subfr, &exp_y2y2, 5243l /*0.01f Q19*/ ) ); /*Q15-exp_y2y2*/
663 :
664 : /* L_off = 1L; */
665 11120 : L_off = L_shr( 10737418l /*0.01f/2.0f Q31*/, sub( 30 - 9, exp_xn ) );
666 :
667 : /* Compute scalar product <xn[],y2[]> */
668 11120 : xny2 = extract_h( Dot_product12_offs( xn, y2, L_subfr, &exp_xny2, L_off ) ); /*Q15-exp_xny2*/
669 :
670 : /* Compute scalar product <y1[],y2[]> */
671 11120 : y1y2 = extract_h( Dot_product12_offs( y1, y2, L_subfr, &exp_y1y2, L_off ) ); /*Q15-exp_y1y2*/
672 :
673 : /* Compute scalar product <xn[],xn[]> */
674 11120 : L_off = L_shr( 21474836l /*0.01f Q31*/, s_min( 31, sub( 30, shl( exp_xn, 1 ) ) ) );
675 11120 : xx = extract_h( Dot_product12_offs( xn, xn, L_subfr, &exp_xx, L_off ) ); /*Q15-exp_xx*/
676 :
677 :
678 11120 : g_corr->y2y2 = y2y2;
679 11120 : move16();
680 11120 : g_corr->y2y2_e = exp_y2y2;
681 11120 : move16();
682 11120 : g_corr->xy2 = xny2;
683 11120 : move16();
684 11120 : g_corr->xy2_e = exp_xny2;
685 11120 : move16();
686 11120 : g_corr->y1y2 = y1y2;
687 11120 : move16();
688 11120 : g_corr->y1y2_e = exp_y1y2;
689 11120 : move16();
690 11120 : g_corr->xx = xx;
691 11120 : move16();
692 11120 : g_corr->xx_e = exp_xx;
693 11120 : move16();
694 :
695 :
696 : BASOP_SATURATE_ERROR_OFF_EVS;
697 11120 : return;
698 : }
699 :
700 :
701 : /*
702 : * E_ACELP_codebook_target_update
703 : *
704 : * Parameters:
705 : * x I: old target (for pitch search) (Q_xn)
706 : * x2 O: new target (for codebook search) (Q_xn)
707 : * y I: filtered adaptive codebook vector (Q_xn)
708 : * gain I: adaptive codebook gain (Q14)
709 : *
710 : * Function:
711 : * Update the target vector for codebook search.
712 : * Subframe size = L_SUBFR
713 : * Returns:
714 : * void
715 : */
716 7470 : void E_ACELP_codebook_target_update_fx(
717 : Word16 *x /*Q_xn*/,
718 : Word16 *x2 /*Q_xn*/,
719 : Word16 *y /*Q_xn*/,
720 : Word16 gain /*Q14*/,
721 : Word16 L_subfr /*Q0*/ )
722 : {
723 : Word16 i, Q15_flag;
724 : Word32 L_tmp;
725 7470 : assert( gain >= 0 );
726 :
727 7470 : Q15_flag = 0;
728 7470 : move16();
729 7470 : if ( LT_16( gain, 1 << 14 ) )
730 : {
731 6421 : Q15_flag = 1;
732 6421 : move16();
733 : }
734 7470 : gain = shl( gain, Q15_flag ); /*Q14*/
735 :
736 485550 : FOR( i = 0; i < L_subfr; i++ )
737 : {
738 478080 : L_tmp = L_deposit_h( x[i] ); /*Q_xn+16*/
739 478080 : if ( Q15_flag == 0 )
740 : {
741 67136 : L_tmp = L_msu_sat( L_tmp, y[i], gain ); /*Q_xn+15*/
742 : }
743 478080 : x2[i] = msu_r_sat( L_tmp, y[i], gain ); /*Q_xn*/
744 478080 : move16();
745 : }
746 :
747 7470 : return;
748 : }
749 :
750 :
751 : /*
752 : * E_ACELP_pulsesign
753 : *
754 : * Parameters:
755 : * cn I: residual after long term prediction <12b
756 : * dn I: corr. between target and h[]. <12b
757 : * dn2 I/O: dn2[] = mix of dn[] and cn[]
758 : * sign O: sign of pulse 0 or -1
759 : * vec O: negative sign of pulse
760 : * alp I: energy of all fixed pulses Q13
761 : * sign_val I: value for signs
762 : * L_subfr I: subframe length
763 : *
764 : * Function:
765 : * Determine sign of each pulse position, store them in "sign"
766 : * and change dn to all positive.
767 : * Subframe size = L_SUBFR
768 : * Returns:
769 : * void
770 : */
771 578767 : void E_ACELP_pulsesign(
772 : const Word16 cn[] /*Q_xn*/,
773 : Word16 dn[] /*Qdn*/,
774 : Word16 dn2[] /*Qdn2*/,
775 : Word16 sign[] /*Q13*/,
776 : Word16 vec[] /*Q15*/,
777 : const Word16 alp /*Q13*/,
778 : const Word16 sign_val /*Q15*/,
779 : const Word16 L_subfr /*Q0*/ )
780 : {
781 : Word16 i;
782 : Word32 Lval, Lcor;
783 : Word16 k_cn, k_dn, sign_neg, e_dn, e_cn;
784 : Word16 signs[3];
785 : Word16 *ptr16;
786 : Word16 val, index;
787 :
788 : /* calculate energy for normalization of cn[] and dn[] */
789 578767 : Lval = L_mac0( 1, cn[0], cn[0] ); /*2*Q_xn*/
790 578767 : Lcor = L_mac0( 1, dn[0], dn[0] ); /*2*Qdn*/
791 :
792 37041088 : FOR( i = 1; i < L_subfr; i++ )
793 : {
794 36462321 : Lval = L_mac0_sat( Lval, cn[i], cn[i] ); /*2*Q_xn*/
795 36462321 : Lcor = L_mac0( Lcor, dn[i], dn[i] ); /*2*Qdn*/
796 : }
797 :
798 578767 : e_dn = 31;
799 578767 : move16();
800 578767 : e_cn = 31;
801 578767 : move16();
802 :
803 578767 : Lval = Sqrt32( Lval, &e_dn ); /*Q31 - e_dn*/
804 578767 : Lcor = Sqrt32( Lcor, &e_cn ); /*Q31 - e_cn*/
805 578767 : i = sub( e_dn, e_cn );
806 578767 : if ( i < 0 )
807 343196 : Lval = L_shl( Lval, i );
808 578767 : if ( i > 0 )
809 130953 : Lcor = L_shr( Lcor, i );
810 :
811 578767 : k_dn = round_fx_sat( Lval ); /*Q15 - e_dn*/
812 578767 : k_cn = round_fx_sat( Lcor ); /*Q15 - e_cn*/
813 :
814 578767 : k_cn = mult_r( 0x2000, k_cn ); /* 1 in Q13 */
815 578767 : k_dn = mult_r( alp, k_dn ); /* alp in Q13 */
816 :
817 578767 : sign_neg = negate( sign_val ); /*Q15*/
818 :
819 578767 : signs[0] = sign_neg; /*Q15*/
820 578767 : move16();
821 578767 : signs[1] = sign_val; /*Q15*/
822 578767 : move16();
823 578767 : signs[2] = sign_neg; /*Q15*/
824 578767 : move16();
825 578767 : ptr16 = &signs[1]; /*Q15*/
826 :
827 37619855 : FOR( i = 0; i < L_subfr; i++ )
828 : {
829 : /*cor = (s * cn[i]) + (alp * dn[i]); MULT(1);MAC(1);*/
830 37041088 : Lcor = L_mult( cn[i], k_cn ); /*Q_xn + Q15 - e_cn + 1*/
831 37041088 : Lcor = L_mac( Lcor, dn[i], k_dn ); /*Qdn + Q15 - e_dn + 1*/
832 37041088 : val = round_fx_sat( L_shl_sat( Lcor, 4 ) ); /*shifting by 4 may overflow but improves accuracy Qdn + 4 - e_dn*/
833 :
834 37041088 : index = shr( val, 15 );
835 37041088 : sign[i] = ptr16[index]; /*Q15*/
836 37041088 : move16(); /* yields -1 (when ps < 0) or 0 (when ps >= 0) */
837 37041088 : vec[i] = ptr16[index + 1]; /*Q15*/
838 37041088 : move16();
839 :
840 37041088 : if ( val < 0 )
841 : {
842 18666368 : dn[i] = negate( dn[i] ); /*Qdn*/
843 18666368 : move16();
844 : }
845 37041088 : dn2[i] = abs_s( val ); /*Qdn2 = Qdn + 4 - e_dn*/
846 37041088 : move16(); /* dn2[] = mix of dn[] and cn[] */
847 : }
848 578767 : }
849 :
850 :
851 578767 : void E_ACELP_findcandidates(
852 : Word16 dn2[] /*Qx*/,
853 : Word16 dn2_pos[] /*Q0*/,
854 : Word16 pos_max[] /*Q0*/ )
855 : {
856 : Word16 i, k, j, i8;
857 : Word16 *ps_ptr;
858 :
859 2893835 : FOR( i = 0; i < 4; i++ )
860 : {
861 2315068 : i8 = shl( i, 3 );
862 20835612 : FOR( k = i8; k < i8 + 8; k++ )
863 : {
864 18520544 : ps_ptr = &dn2[i]; /*Qx*/
865 :
866 296328704 : FOR( j = i + 4; j < L_SUBFR; j += 4 )
867 : {
868 277808160 : if ( GT_16( dn2[j], *ps_ptr ) )
869 : {
870 43633733 : ps_ptr = &dn2[j]; /*Qx*/
871 43633733 : move16();
872 : }
873 : }
874 :
875 18520544 : *ps_ptr = -1; /* dn2 < 0 when position is selected */
876 18520544 : move16();
877 18520544 : dn2_pos[k] = (Word16) ( ps_ptr - dn2 );
878 18520544 : move16();
879 : }
880 2315068 : pos_max[i] = dn2_pos[i8]; /*Q0*/
881 2315068 : move16();
882 : }
883 578767 : }
884 :
885 :
886 11784320 : static void E_ACELP_apply_sign(
887 : Word16 *p0 /*Qx*/,
888 : Word16 *psign0 /*Q15*/ )
889 : {
890 11784320 : p0[0] = mult_r( p0[0], psign0[0] ); /*Qx*/
891 11784320 : move16();
892 11784320 : p0[1] = mult_r( p0[1], psign0[4] );
893 11784320 : move16();
894 11784320 : p0[2] = mult_r( p0[2], psign0[8] );
895 11784320 : move16();
896 11784320 : p0[3] = mult_r( p0[3], psign0[12] );
897 11784320 : move16();
898 11784320 : p0[4] = mult_r( p0[4], psign0[16] );
899 11784320 : move16();
900 11784320 : p0[5] = mult_r( p0[5], psign0[20] );
901 11784320 : move16();
902 11784320 : p0[6] = mult_r( p0[6], psign0[24] );
903 11784320 : move16();
904 11784320 : p0[7] = mult_r( p0[7], psign0[28] );
905 11784320 : move16();
906 11784320 : p0[8] = mult_r( p0[8], psign0[32] );
907 11784320 : move16();
908 11784320 : p0[9] = mult_r( p0[9], psign0[36] );
909 11784320 : move16();
910 11784320 : p0[10] = mult_r( p0[10], psign0[40] );
911 11784320 : move16();
912 11784320 : p0[11] = mult_r( p0[11], psign0[44] );
913 11784320 : move16();
914 11784320 : p0[12] = mult_r( p0[12], psign0[48] );
915 11784320 : move16();
916 11784320 : p0[13] = mult_r( p0[13], psign0[52] );
917 11784320 : move16();
918 11784320 : p0[14] = mult_r( p0[14], psign0[56] );
919 11784320 : move16();
920 11784320 : p0[15] = mult_r( p0[15], psign0[60] );
921 11784320 : move16();
922 :
923 11784320 : return;
924 : }
925 :
926 184130 : void E_ACELP_vec_neg_fx(
927 : Word16 h[] /*Qx*/,
928 : Word16 h_inv[] /*Qx*/,
929 : Word16 L_subfr /*Q0*/ )
930 : {
931 : Word16 i;
932 :
933 11968450 : FOR( i = 0; i < L_subfr; i++ )
934 : {
935 11784320 : h_inv[i] = negate( h[i] );
936 11784320 : move16();
937 : }
938 :
939 184130 : return;
940 : }
941 :
942 :
943 184130 : void E_ACELP_corrmatrix_fx(
944 : Word16 h[] /*Q12*/,
945 : Word16 sign[] /*Q0*/,
946 : Word16 vec[] /*Q15*/,
947 : Word16 rrixix[4][16] /*Q9*/,
948 : Word16 rrixiy[4][256] /*Q9*/ )
949 : {
950 :
951 : Word16 *p0, *p1, *p2, *p3, *psign0, *psign1, *psign2, *psign3;
952 : Word16 *ptr_h1, *ptr_h2, *ptr_hf;
953 : Word32 cor;
954 : Word16 i, /* j, */ k, pos;
955 :
956 : /*
957 : * Compute rrixix[][] needed for the codebook search.
958 : */
959 :
960 : /* storage order --> i3i3, i2i2, i1i1, i0i0 */
961 :
962 : /* Init pointers to last position of rrixix[] */
963 184130 : p0 = &rrixix[0][16 - 1]; /* Q9 */
964 184130 : p1 = &rrixix[1][16 - 1];
965 184130 : p2 = &rrixix[2][16 - 1];
966 184130 : p3 = &rrixix[3][16 - 1];
967 :
968 184130 : ptr_h1 = h; /*Q12*/
969 184130 : cor = L_deposit_l( 0 );
970 3130210 : FOR( i = 0; i < 16; i++ )
971 : {
972 2946080 : cor = L_mac( cor, *ptr_h1, *ptr_h1 ); /*Q25*/
973 2946080 : ptr_h1++;
974 2946080 : *p3-- = round_fx( L_shr( cor, 1 ) ); /* Q9 */
975 2946080 : cor = L_mac( cor, *ptr_h1, *ptr_h1 ); /*Q25*/
976 2946080 : ptr_h1++;
977 2946080 : *p2-- = round_fx( L_shr( cor, 1 ) ); /* Q9 */
978 2946080 : cor = L_mac( cor, *ptr_h1, *ptr_h1 ); /*Q25*/
979 2946080 : ptr_h1++;
980 2946080 : *p1-- = round_fx( L_shr( cor, 1 ) ); /* Q9 */
981 2946080 : cor = L_mac( cor, *ptr_h1, *ptr_h1 ); /*Q25*/
982 2946080 : ptr_h1++;
983 2946080 : *p0-- = round_fx( L_shr( cor, 1 ) ); /* Q9 */
984 : }
985 :
986 : /*
987 : * Compute rrixiy[][] needed for the codebook search.
988 : */
989 :
990 : /* storage order --> i2i3, i1i2, i0i1, i3i0 */
991 :
992 184130 : pos = 256 - 1;
993 184130 : move16();
994 184130 : ptr_hf = h + 1; /*Q12*/
995 3130210 : FOR( k = 0; k < 16; k++ )
996 : {
997 2946080 : p3 = &rrixiy[2][pos]; /* Q9 */
998 2946080 : p2 = &rrixiy[1][pos];
999 2946080 : p1 = &rrixiy[0][pos];
1000 2946080 : p0 = &rrixiy[3][pos - 16];
1001 :
1002 2946080 : cor = L_deposit_h( 0 );
1003 2946080 : ptr_h1 = h; /*Q12*/
1004 2946080 : ptr_h2 = ptr_hf; /*Q12*/
1005 :
1006 25041680 : FOR( i = k; i < 16 - 1; i++ )
1007 : {
1008 22095600 : cor = L_mac( cor, *ptr_h1++, *ptr_h2++ ); /*Q25*/
1009 22095600 : *p3 = round_fx( cor ); /*Q9*/
1010 22095600 : cor = L_mac( cor, *ptr_h1++, *ptr_h2++ ); /*Q25*/
1011 22095600 : *p2 = round_fx( cor ); /*Q9*/
1012 22095600 : cor = L_mac( cor, *ptr_h1++, *ptr_h2++ ); /*Q25*/
1013 22095600 : *p1 = round_fx( cor ); /*Q9*/
1014 22095600 : cor = L_mac( cor, *ptr_h1++, *ptr_h2++ ); /*Q25*/
1015 22095600 : *p0 = round_fx( cor ); /*Q9*/
1016 :
1017 22095600 : p3 -= ( 16 + 1 );
1018 22095600 : p2 -= ( 16 + 1 );
1019 22095600 : p1 -= ( 16 + 1 );
1020 22095600 : p0 -= ( 16 + 1 );
1021 : }
1022 2946080 : cor = L_mac( cor, *ptr_h1++, *ptr_h2++ ); /*Q25*/
1023 2946080 : *p3 = round_fx( cor ); /*Q9*/
1024 2946080 : cor = L_mac( cor, *ptr_h1++, *ptr_h2++ ); /*Q25*/
1025 2946080 : *p2 = round_fx( cor ); /*Q9*/
1026 2946080 : cor = L_mac( cor, *ptr_h1++, *ptr_h2++ ); /*Q25*/
1027 2946080 : *p1 = round_fx( cor ); /*Q9*/
1028 2946080 : move16();
1029 2946080 : move16();
1030 2946080 : move16();
1031 :
1032 2946080 : pos -= 16;
1033 2946080 : move16();
1034 2946080 : ptr_hf += 4;
1035 : }
1036 :
1037 : /* storage order --> i3i0, i2i3, i1i2, i0i1 */
1038 :
1039 184130 : pos = 256 - 1;
1040 184130 : move16();
1041 184130 : ptr_hf = h + 3; /*Q12*/
1042 3130210 : FOR( k = 0; k < 16; k++ )
1043 : {
1044 2946080 : p3 = &rrixiy[3][pos]; /*Q9*/
1045 2946080 : p2 = &rrixiy[2][pos - 1];
1046 2946080 : p1 = &rrixiy[1][pos - 1];
1047 2946080 : p0 = &rrixiy[0][pos - 1];
1048 :
1049 2946080 : cor = L_deposit_h( 0 );
1050 2946080 : ptr_h1 = h; /*Q12*/
1051 2946080 : ptr_h2 = ptr_hf; /*Q12*/
1052 25041680 : FOR( i = k; i < 16 - 1; i++ )
1053 : {
1054 22095600 : cor = L_mac( cor, *ptr_h1++, *ptr_h2++ ); /*Q25*/
1055 22095600 : *p3 = round_fx( cor ); /*Q9*/
1056 22095600 : cor = L_mac( cor, *ptr_h1++, *ptr_h2++ ); /*Q25*/
1057 22095600 : *p2 = round_fx( cor ); /*Q9*/
1058 22095600 : cor = L_mac( cor, *ptr_h1++, *ptr_h2++ ); /*Q25*/
1059 22095600 : *p1 = round_fx( cor ); /*Q9*/
1060 22095600 : cor = L_mac( cor, *ptr_h1++, *ptr_h2++ ); /*Q25*/
1061 22095600 : *p0 = round_fx( cor ); /*Q9*/
1062 22095600 : move16();
1063 22095600 : move16();
1064 22095600 : move16();
1065 22095600 : move16();
1066 :
1067 22095600 : p3 -= ( 16 + 1 );
1068 22095600 : p2 -= ( 16 + 1 );
1069 22095600 : p1 -= ( 16 + 1 );
1070 22095600 : p0 -= ( 16 + 1 );
1071 : }
1072 2946080 : cor = L_mac( cor, *ptr_h1++, *ptr_h2++ ); /*Q25*/
1073 2946080 : *p3 = round_fx( cor ); /*Q9*/
1074 2946080 : move16();
1075 :
1076 2946080 : pos--;
1077 2946080 : ptr_hf += 4;
1078 : }
1079 :
1080 : /*
1081 : * Modification of rrixiy[][] to take signs into account.
1082 : */
1083 :
1084 184130 : p0 = &rrixiy[0][0]; /*Q9*/
1085 :
1086 : /* speed-up: 11% */
1087 184130 : p1 = &rrixiy[1][0];
1088 184130 : p2 = &rrixiy[2][0];
1089 184130 : p3 = &rrixiy[3][0];
1090 :
1091 3130210 : FOR( i = 0; i < L_SUBFR; i += 4 )
1092 : {
1093 :
1094 2946080 : psign0 = &vec[1]; /*Q15*/
1095 2946080 : if ( sign[i + 0] > 0 )
1096 1455157 : psign0 = &sign[1];
1097 :
1098 2946080 : psign1 = &vec[2]; /*Q15*/
1099 2946080 : if ( sign[i + 1] > 0 )
1100 1456474 : psign1 = &sign[2];
1101 :
1102 2946080 : psign2 = &vec[3]; /*Q15*/
1103 2946080 : if ( sign[i + 2] > 0 )
1104 1455997 : psign2 = &sign[3];
1105 :
1106 2946080 : psign3 = &vec[0]; /*Q15*/
1107 2946080 : if ( sign[i + 3] > 0 )
1108 1453523 : psign3 = &sign[0];
1109 :
1110 2946080 : E_ACELP_apply_sign( p0, psign0 );
1111 2946080 : p0 += 16;
1112 :
1113 2946080 : E_ACELP_apply_sign( p1, psign1 );
1114 2946080 : p1 += 16;
1115 :
1116 2946080 : E_ACELP_apply_sign( p2, psign2 );
1117 2946080 : p2 += 16;
1118 :
1119 2946080 : E_ACELP_apply_sign( p3, psign3 );
1120 2946080 : p3 += 16;
1121 : }
1122 :
1123 184130 : return;
1124 : }
1125 :
1126 184130 : void E_ACELP_4tsearch_fx(
1127 : Word16 dn[] /*Qdn*/,
1128 : const Word16 cn[] /*Q_xn*/,
1129 : const Word16 H[] /*Q12*/,
1130 : Word16 code[] /*Q9*/,
1131 : const PulseConfig *config,
1132 : Word16 ind[] /*Q0*/,
1133 : Word16 y[] /*Qy*/,
1134 : const Word16 element_mode )
1135 : {
1136 : Word16 sign[L_SUBFR], vec[L_SUBFR];
1137 : Word16 cor_x[16], cor_y[16], h_buf[4 * L_SUBFR];
1138 : Word16 rrixix[4][16];
1139 : Word16 rrixiy[4][256];
1140 : Word16 dn2[L_SUBFR];
1141 184130 : Word16 psk, ps, alpk, alp = 0;
1142 : Word16 codvec[NB_PULSE_MAX];
1143 : Word16 pos_max[4];
1144 : Word16 dn2_pos[8 * 4];
1145 : UWord8 ipos[NB_PULSE_MAX];
1146 : Word16 *p0, *p1, *p2, *p3;
1147 : Word16 *h, *h_inv;
1148 : Word16 i, j, k, l, st, pos;
1149 : Word16 val, tmp, scale;
1150 : Word32 s, L_tmp;
1151 : Word16 nb_pulse, nb_pulse_m2;
1152 184130 : Word16 check = 0; /* debug code not instrumented */
1153 :
1154 184130 : alp = config->alp; /* Q13 */ /* initial value for energy of all fixed pulses */
1155 184130 : move16();
1156 184130 : nb_pulse = config->nb_pulse;
1157 184130 : move16();
1158 184130 : nb_pulse_m2 = sub( nb_pulse, 2 );
1159 :
1160 184130 : set16_fx( codvec, 0, nb_pulse );
1161 :
1162 : /*
1163 : * Find sign for each pulse position.
1164 : */
1165 :
1166 184130 : E_ACELP_pulsesign( cn, dn, dn2, sign, vec, alp, 0x7fff, L_SUBFR );
1167 :
1168 : /*
1169 : * Select the most important 8 position per track according to dn2[].
1170 : */
1171 184130 : E_ACELP_findcandidates( dn2, dn2_pos, pos_max );
1172 :
1173 : /*
1174 : * Compute h_inv[i].
1175 : */
1176 184130 : set16_fx( h_buf, 0, L_SUBFR );
1177 :
1178 184130 : set16_fx( h_buf + ( 2 * L_SUBFR ), 0, L_SUBFR );
1179 :
1180 184130 : h = h_buf + L_SUBFR;
1181 184130 : h_inv = h_buf + ( 3 * L_SUBFR );
1182 :
1183 : /*Check the energy if it is too high then scale to prevent an overflow*/
1184 184130 : scale = 0;
1185 184130 : move16();
1186 184130 : L_tmp = L_deposit_l( 0 );
1187 : BASOP_SATURATE_WARNING_OFF_EVS
1188 11968450 : FOR( i = 0; i < L_SUBFR; i++ )
1189 : {
1190 11784320 : L_tmp = L_mac_sat( L_tmp, H[i], H[i] ); /*Q25*/
1191 : }
1192 184130 : val = extract_h( L_tmp ); /*Q9*/
1193 : BASOP_SATURATE_WARNING_ON_EVS
1194 :
1195 184130 : if ( GT_16( val, 0x2000 ) )
1196 : {
1197 18475 : scale = -1;
1198 18475 : move16();
1199 : }
1200 184130 : if ( GT_16( val, 0x7000 ) )
1201 : {
1202 2225 : scale = -2;
1203 2225 : move16();
1204 : }
1205 184130 : test();
1206 184130 : if ( EQ_16( val, 32767 ) && element_mode > EVS_MONO )
1207 : {
1208 1687 : scale = -3;
1209 1687 : move16();
1210 : }
1211 184130 : Copy_Scale_sig( H, h, L_SUBFR, scale ); /*Q12+scale*/
1212 :
1213 184130 : E_ACELP_vec_neg_fx( h, h_inv, L_SUBFR );
1214 :
1215 : /*
1216 : * Compute correlation matrices needed for the codebook search.
1217 : */
1218 184130 : E_ACELP_corrmatrix_fx( h, sign, vec, rrixix, rrixiy );
1219 :
1220 : /*
1221 : * Deep first search:
1222 : * ------------------
1223 : * 20 bits (4p): 4 iter x ((4x16)+(8x16)) = 768 tests
1224 : * 36 bits (8p): 4 iter x ((1x1)+(4x16)+(8x16)+(8x16)) = 1280 tests
1225 : * 52 bits (12p): 3 iter x ((1x1)+(1x1)+(4x16)+(6x16)
1226 : * +(8x16)+(8x16)) = 1248 tests
1227 : * 64 bits (16p): 2 iter x ((1x1)+(1x1)+(4x16)+(6x16)
1228 : * +(6x16)+(8x16)+(8x16)+(8x16)) = 1280 tests
1229 : */
1230 184130 : psk = -1;
1231 184130 : move16();
1232 184130 : alpk = 1;
1233 184130 : move16();
1234 :
1235 : /*Number of iterations*/
1236 954617 : FOR( k = 0; k < config->nbiter; k++ )
1237 : {
1238 770487 : E_ACELP_setup_pulse_search_pos( config, k, ipos );
1239 :
1240 : /* format of alp changes to Q(15-ALP2_E) */
1241 :
1242 770487 : pos = config->fixedpulses;
1243 770487 : move16();
1244 :
1245 770487 : IF( config->fixedpulses == 0 ) /* 1100, 11, 1110, 1111, 2211 */
1246 : {
1247 731247 : ps = 0;
1248 731247 : move16();
1249 731247 : alp = 0;
1250 731247 : move16();
1251 731247 : set16_fx( vec, 0, L_SUBFR );
1252 : }
1253 39240 : ELSE IF( EQ_16( config->fixedpulses, 2 ) ) /* 2222 and 3322 */
1254 : {
1255 : /* first stage: fix 2 pulses */
1256 39235 : ind[0] = pos_max[ipos[0]];
1257 39235 : move16();
1258 39235 : ind[1] = pos_max[ipos[1]];
1259 39235 : move16();
1260 39235 : ps = add( dn[ind[0]], dn[ind[1]] );
1261 :
1262 : /*alp = rrixix[ipos[0]][ind[0] >> 2] + rrixix[ipos[1]][ind[1] >> 2] +
1263 : rrixiy[ipos[0]][((ind[0] >> 2) << 4) + (ind[1] >> 2)];*/
1264 :
1265 39235 : i = shr( ind[0], 2 );
1266 39235 : j = shr( ind[1], 2 );
1267 39235 : l = add( shl( i, 4 ), j );
1268 39235 : s = L_mult( rrixix[ipos[0]][i], _1_ ); /* Q9+Q12+1 */
1269 39235 : s = L_mac( s, rrixix[ipos[1]][j], _1_ ); /* Q9+Q12+1 */
1270 39235 : alp = mac_r( s, rrixiy[ipos[0]][l], _1_ ); /* Q9+Q12+1-16 */
1271 :
1272 39235 : p0 = h - ind[0]; /*Q12+scale*/
1273 39235 : if ( sign[ind[0]] < 0 )
1274 : {
1275 19143 : p0 = h_inv - ind[0]; /*Q12+scale*/
1276 : }
1277 :
1278 39235 : p1 = h - ind[1]; /*Q12+scale*/
1279 39235 : if ( sign[ind[1]] < 0 )
1280 : {
1281 19156 : p1 = h_inv - ind[1]; /*Q12+scale*/
1282 : }
1283 :
1284 2550275 : FOR( i = 0; i < L_SUBFR; i++ )
1285 : {
1286 2511040 : vec[i] = add( *p0++, *p1++ ); /*Q12+scale*/
1287 2511040 : move16();
1288 : }
1289 : }
1290 : ELSE /* 3333 and above */
1291 : {
1292 : /* first stage: fix 4 pulses */
1293 :
1294 5 : ind[0] = pos_max[ipos[0]]; /*Q0*/
1295 5 : move16();
1296 5 : ind[1] = pos_max[ipos[1]]; /*Q0*/
1297 5 : move16();
1298 5 : ind[2] = pos_max[ipos[2]]; /*Q0*/
1299 5 : move16();
1300 5 : ind[3] = pos_max[ipos[3]]; /*Q0*/
1301 5 : move16();
1302 :
1303 : /*ps = dn[ind[0]] + dn[ind[1]] + dn[ind[2]] + dn[ind[3]];*/
1304 5 : ps = add( add( add( dn[ind[0]], dn[ind[1]] ), dn[ind[2]] ), dn[ind[3]] );
1305 :
1306 5 : p0 = h - ind[0]; /*Q12+scale*/
1307 5 : if ( sign[ind[0]] < 0 )
1308 : {
1309 0 : p0 = h_inv - ind[0]; /*Q12+scale*/
1310 : }
1311 :
1312 5 : p1 = h - ind[1]; /*Q12+scale*/
1313 5 : if ( sign[ind[1]] < 0 )
1314 : {
1315 0 : p1 = h_inv - ind[1]; /*Q12+scale*/
1316 : }
1317 :
1318 5 : p2 = h - ind[2]; /*Q12+scale*/
1319 5 : if ( sign[ind[2]] < 0 )
1320 : {
1321 5 : p2 = h_inv - ind[2]; /*Q12+scale*/
1322 : }
1323 :
1324 5 : p3 = h - ind[3]; /*Q12+scale*/
1325 5 : if ( sign[ind[3]] < 0 )
1326 : {
1327 5 : p3 = h_inv - ind[3]; /*Q12+scale*/
1328 : }
1329 :
1330 325 : FOR( i = 0; i < L_SUBFR; i++ )
1331 : {
1332 320 : vec[i] = add( add( add( *p0++, *p1++ ), *p2++ ), *p3++ ); /*Q12+scale*/
1333 320 : move16();
1334 : }
1335 :
1336 5 : L_tmp = L_mult( vec[0], vec[0] ); /*Q25+2*scale*/
1337 320 : FOR( i = 1; i < L_SUBFR; i++ )
1338 315 : L_tmp = L_mac_sat( L_tmp, vec[i], vec[i] ); /*Q25+2*scale*/
1339 5 : alp = round_fx( L_shr( L_tmp, 3 ) ); /*Q6+2*scale*/
1340 :
1341 : /*alp *= 0.5F; */
1342 : }
1343 :
1344 : /* other stages of 2 pulses */
1345 770487 : st = 0;
1346 770487 : move16();
1347 2954421 : FOR( j = pos; j < nb_pulse; j += 2 )
1348 : {
1349 2183934 : IF( GE_16( nb_pulse_m2, j ) ) /* pair-wise search */
1350 : {
1351 : /*
1352 : * Calculate correlation of all possible positions
1353 : * of the next 2 pulses with previous fixed pulses.
1354 : * Each pulse can have 16 possible positions.
1355 : */
1356 1779211 : E_ACELP_h_vec_corr1_fx( h, vec, ipos[j], sign, rrixix, cor_x, dn2_pos, config->nbpos[st] );
1357 :
1358 1779211 : E_ACELP_h_vec_corr2_fx( h, vec, ipos[j + 1], sign, rrixix, cor_y );
1359 :
1360 : /*
1361 : * Find best positions of 2 pulses.
1362 : */
1363 1779211 : E_ACELP_2pulse_search( config->nbpos[st], ipos[j], ipos[j + 1], &ps, &alp,
1364 1779211 : &ind[j], &ind[j + 1], dn, dn2_pos, cor_x, cor_y, rrixiy );
1365 : }
1366 : ELSE /* single pulse search */
1367 : {
1368 404723 : E_ACELP_h_vec_corr2_fx( h, vec, ipos[j], sign, rrixix, cor_x );
1369 :
1370 404723 : E_ACELP_h_vec_corr2_fx( h, vec, ipos[j + 1], sign, rrixix, cor_y );
1371 :
1372 404723 : E_ACELP_1pulse_search( &ipos[j], &ps, &alp,
1373 404723 : &ind[j], dn, cor_x, cor_y );
1374 : }
1375 :
1376 2183934 : IF( GT_16( nb_pulse_m2, j ) )
1377 : {
1378 1413447 : p0 = h - ind[j]; /*Q12+scale*/
1379 1413447 : if ( sign[ind[j]] < 0 )
1380 : {
1381 715478 : p0 = h_inv - ind[j]; /*Q12+scale*/
1382 : }
1383 :
1384 1413447 : p1 = h - ind[j + 1]; /*Q12+scale*/
1385 1413447 : if ( sign[ind[j + 1]] < 0 )
1386 : {
1387 713929 : p1 = h_inv - ind[j + 1]; /*Q12+scale*/
1388 : }
1389 :
1390 :
1391 91874055 : FOR( i = 0; i < L_SUBFR; i++ )
1392 : {
1393 90460608 : tmp = add( *p0++, *p1++ );
1394 90460608 : vec[i] = add_sat( vec[i], tmp ); /* can saturate here. */
1395 90460608 : move16();
1396 : }
1397 : }
1398 2183934 : st = add( st, 1 );
1399 : }
1400 :
1401 : /* memorise the best codevector */
1402 :
1403 : /*ps = ps * ps; MULT(1);*/
1404 770487 : ps = mult( ps, ps );
1405 : /*s = (alpk * ps) - (psk * alp); MULT(2);ADD(1);*/
1406 770487 : s = L_msu( L_mult( alpk, ps ), psk, alp ); /*Q9+Q6+1=Q16*/
1407 :
1408 770487 : if ( psk < 0 )
1409 : {
1410 184130 : s = 1;
1411 184130 : move32();
1412 : }
1413 770487 : IF( s > 0 )
1414 : {
1415 366127 : psk = ps;
1416 366127 : move16();
1417 366127 : alpk = alp;
1418 366127 : move16();
1419 366127 : Copy( ind, codvec, nb_pulse ); /*Q0*/
1420 366127 : check = 1; /* debug code not instrumented */
1421 : }
1422 : }
1423 :
1424 184130 : assert( check ); /* debug code not instrumented */
1425 : /*
1426 : * Build the codeword, the filtered codeword and index of codevector, as well as store weighted correlations.
1427 : */
1428 :
1429 184130 : E_ACELP_build_code( nb_pulse, codvec, sign, code, ind );
1430 :
1431 184130 : set16_fx( y, 0, L_SUBFR );
1432 1182199 : FOR( k = 0; k < nb_pulse; ++k )
1433 : {
1434 998069 : i = codvec[k]; /*Q0*/
1435 998069 : move16();
1436 998069 : p0 = h_inv - i; /*Q12+scale*/
1437 998069 : if ( sign[i] > 0 )
1438 : {
1439 493726 : p0 -= 2 * L_SUBFR;
1440 : }
1441 64874485 : FOR( i = 0; i < L_SUBFR; i++ )
1442 : {
1443 63876416 : y[i] = add_sat( y[i], *p0++ ); /*Q12+scale*/
1444 63876416 : move16();
1445 : }
1446 : }
1447 184130 : return;
1448 : }
1449 :
1450 :
1451 : /*
1452 : * E_ACELP_4t_fx
1453 : *
1454 : * Parameters:
1455 : * dn I: corr. between target and h[].
1456 : * cn I: residual after Word32 term prediction
1457 : * H I: impulse response of weighted synthesis filter (Q12)
1458 : * code O: algebraic (fixed) codebook excitation (Q9)
1459 : * y O: filtered fixed codebook excitation (Q9)
1460 : * nbbits I: 20, 36, 44, 52, 64, 72 or 88 bits
1461 : * mode I: speech mode
1462 : * _index O: index
1463 : *
1464 : * Function:
1465 : * 20, 36, 44, 52, 64, 72, 88 bits algebraic codebook.
1466 : * 4 tracks x 16 positions per track = 64 samples.
1467 : *
1468 : * 20 bits 5 + 5 + 5 + 5 --> 4 pulses in a frame of 64 samples.
1469 : * 36 bits 9 + 9 + 9 + 9 --> 8 pulses in a frame of 64 samples.
1470 : * 44 bits 13 + 9 + 13 + 9 --> 10 pulses in a frame of 64 samples.
1471 : * 52 bits 13 + 13 + 13 + 13 --> 12 pulses in a frame of 64 samples.
1472 : * 64 bits 2 + 2 + 2 + 2 + 14 + 14 + 14 + 14 -->
1473 : * 16 pulses in a frame of 64 samples.
1474 : * 72 bits 10 + 2 + 10 + 2 + 10 + 14 + 10 + 14 -->
1475 : * 18 pulses in a frame of 64 samples.
1476 : * 88 bits 11 + 11 + 11 + 11 + 11 + 11 + 11 + 11 -->
1477 : * 24 pulses in a frame of 64 samples.
1478 : *
1479 : * All pulses can have two (2) possible amplitudes: +1 or -1.
1480 : * Each pulse can sixteen (16) possible positions.
1481 : *
1482 : * Returns:
1483 : * void
1484 : */
1485 :
1486 514114 : void E_ACELP_4t_fx(
1487 : Word16 dn[], /*Qdn*/
1488 : Word16 cn[] /* Q_xn */,
1489 : Word16 H[], /*Q12*/
1490 : Word16 R[], /*Qx*/
1491 : Word8 acelpautoc, /*Q0*/
1492 : Word16 code[], /*Q9*/
1493 : Word16 cdk_index, /*Q0*/
1494 : Word16 _index[], /*Q0*/
1495 : const Word16 L_frame, /*Q0*/
1496 : const Word16 last_L_frame, /*Q0*/
1497 : const Word32 total_brate, /*Q0*/
1498 : const Word16 i_subfr, /*Q0*/
1499 : const Word16 cmpl_flag, /*Q0*/
1500 : const Word16 element_mode /*Q0*/
1501 : )
1502 : {
1503 : PulseConfig config;
1504 : Word16 ind[NPMAXPT * 4];
1505 : Word16 y[L_SUBFR];
1506 :
1507 514114 : move16();
1508 514114 : move16();
1509 514114 : move16();
1510 514114 : move16();
1511 514114 : move16();
1512 514114 : move16();
1513 514114 : move16();
1514 514114 : move16();
1515 514114 : move16();
1516 514114 : move16();
1517 514114 : move16();
1518 514114 : move16();
1519 514114 : move16();
1520 514114 : move16();
1521 514114 : move16();
1522 514114 : move16();
1523 514114 : memcpy( &config, &PulseConfTable[cdk_index], sizeof( PulseConfTable[cdk_index] ) );
1524 :
1525 514114 : if ( cmpl_flag > 0 )
1526 : {
1527 10365 : config.nbiter = cmpl_flag;
1528 10365 : move16();
1529 : }
1530 514114 : test();
1531 514114 : test();
1532 514114 : IF( NE_16( L_frame, last_L_frame ) && EQ_32( total_brate, ACELP_24k40 ) && LT_32( i_subfr, 5 * L_SUBFR ) )
1533 : {
1534 0 : config.nbiter = sub( config.nbiter, 1 );
1535 0 : config.nbiter = s_max( config.nbiter, 1 );
1536 : }
1537 :
1538 514114 : IF( acelpautoc )
1539 : {
1540 333636 : E_ACELP_4tsearchx_fx( dn, cn, R, code, &config, ind, element_mode );
1541 : }
1542 : ELSE
1543 : {
1544 180478 : E_ACELP_4tsearch_fx( dn, cn, H, code, &config, ind, y, element_mode );
1545 : }
1546 514114 : E_ACELP_indexing_fx( code, &config, NB_TRACK_FCB_4T, _index );
1547 :
1548 514114 : return;
1549 : }
1550 :
1551 :
1552 578767 : static void E_ACELP_indexing_shift(
1553 : Word16 wordcnt, /* i: 16-bit word count including the newly shifted-in bits Q0*/
1554 : Word16 shift_bits, /* i: number of bits to shift in from the lsb Q0*/
1555 : UWord16 lsb_bits, /* i: bits to shift in from the lsb Q0*/
1556 : const UWord16 src[], /* i: source buffer Q0*/
1557 : UWord16 dst[] /* o: destination buffer Q(shift_bits)*/
1558 : )
1559 : {
1560 : Word16 right_shift, i;
1561 :
1562 578767 : assert( shift_bits <= 16 );
1563 :
1564 578767 : right_shift = sub( 16, shift_bits );
1565 :
1566 1852536 : FOR( i = sub( wordcnt, 1 ); i > 0; --i )
1567 : {
1568 1273769 : dst[i] = s_or( lshl( src[i], shift_bits ), lshr( src[i - 1], right_shift ) ); /*Q0+shift_bits*/
1569 1273769 : move16();
1570 : }
1571 578767 : dst[i] = s_or( lshl( src[i], shift_bits ), lsb_bits ); /*Q(shift_bits)*/
1572 578767 : move16();
1573 :
1574 578767 : return;
1575 : }
1576 :
1577 :
1578 : #define MAX_IDX_LEN 9
1579 :
1580 578767 : Word16 E_ACELP_indexing_fx(
1581 : const Word16 code[], /*Q9*/
1582 : const PulseConfig *config,
1583 : Word16 num_tracks, /*Q0*/
1584 : Word16 prm[] /*Q(shift_bits)*/ )
1585 : {
1586 : Word16 track, shift_bits;
1587 : Word16 p[NB_TRACK_FCB_4T], wordcnt;
1588 : UWord32 s[NB_TRACK_FCB_4T], n[NB_TRACK_FCB_4T];
1589 : UWord16 idx[MAX_IDX_LEN];
1590 : Word16 saved_bits;
1591 :
1592 578767 : assert( num_tracks == NB_TRACK_FCB_4T );
1593 :
1594 578767 : saved_bits = 0;
1595 578767 : move16();
1596 :
1597 : /*
1598 : * Code state of pulses of all tracks
1599 : * */
1600 578767 : wordcnt = shr( add( config->bits, 15 ), 4 ); /* ceil(bits/16) */
1601 :
1602 578767 : set16_fx( (Word16 *) idx, 0, wordcnt );
1603 :
1604 578767 : IF( EQ_16( config->bits, 43 ) ) /* EVS pulse indexing */
1605 : {
1606 33706 : saved_bits = E_ACELP_code43bit_fx( code, s, p, idx );
1607 : }
1608 : ELSE
1609 : {
1610 2725305 : FOR( track = 0; track < num_tracks; track++ )
1611 : {
1612 : /* Code track of length 2^4 where step between tracks is 4. */
1613 2180244 : E_ACELP_codearithp_fx( code + track, &n[track], &s[track], &p[track] );
1614 : }
1615 545061 : fcb_pulse_track_joint_fx( idx, wordcnt, s, p, num_tracks );
1616 : }
1617 :
1618 : /* check if we need to code track positions */
1619 578767 : track = 0;
1620 578767 : move16();
1621 578767 : shift_bits = 0;
1622 578767 : move16();
1623 578767 : SWITCH( config->codetrackpos )
1624 : {
1625 74762 : case TRACKPOS_FIXED_TWO:
1626 : /* Code position of consecutive tracks with single extra pulses */
1627 : /* Find track with one pulse less. */
1628 74762 : if ( NE_16( p[0], p[1] ) )
1629 : {
1630 : /* Either 0110 or 1001 */
1631 37288 : track = 1;
1632 37288 : move16();
1633 : }
1634 74762 : if ( GT_16( p[3], p[1] ) )
1635 : {
1636 37575 : track = add( track, 2 );
1637 37575 : move16();
1638 : }
1639 74762 : shift_bits = 2;
1640 74762 : move16();
1641 74762 : BREAK;
1642 :
1643 67347 : case TRACKPOS_FREE_THREE:
1644 : /* Code position of track with one pulse less than others */
1645 : /* Find track with one pulse less. */
1646 67347 : if ( LT_16( p[1], p[0] ) )
1647 : {
1648 16491 : track = 1;
1649 16491 : move16();
1650 : }
1651 67347 : if ( LT_16( p[2], p[0] ) )
1652 : {
1653 17013 : track = 2;
1654 17013 : move16();
1655 : }
1656 67347 : if ( LT_16( p[3], p[0] ) )
1657 : {
1658 17271 : track = 3;
1659 17271 : move16();
1660 : }
1661 67347 : shift_bits = 2;
1662 67347 : move16();
1663 67347 : BREAK;
1664 :
1665 22461 : case TRACKPOS_FREE_ONE:
1666 : /* Code position of track with one pulse less than others */
1667 : /* Find track with one pulse less. */
1668 22461 : if ( GT_16( p[1], p[0] ) )
1669 : {
1670 5898 : track = 1;
1671 5898 : move16();
1672 : }
1673 22461 : if ( GT_16( p[2], p[0] ) )
1674 : {
1675 5474 : track = 2;
1676 5474 : move16();
1677 : }
1678 22461 : if ( GT_16( p[3], p[0] ) )
1679 : {
1680 5772 : track = 3;
1681 5772 : move16();
1682 : }
1683 22461 : shift_bits = 2;
1684 22461 : move16();
1685 22461 : BREAK;
1686 :
1687 414197 : case TRACKPOS_FIXED_EVEN:
1688 : case TRACKPOS_FIXED_FIRST:
1689 414197 : BREAK;
1690 :
1691 0 : default:
1692 0 : printf( "Codebook mode not implemented.\n" );
1693 0 : assert( 0 ); /* mode not yet implemented*/
1694 : BREAK;
1695 : }
1696 :
1697 578767 : E_ACELP_indexing_shift( wordcnt, shift_bits, track, idx, (UWord16 *) prm );
1698 :
1699 578767 : return saved_bits;
1700 : }
1701 :
1702 :
1703 : /*--------------------------------------------------------------------------*
1704 : * E_ACELP_adaptive_codebook
1705 : *
1706 : * Find adaptive codebook.
1707 : *--------------------------------------------------------------------------*/
1708 :
1709 3060 : void E_ACELP_adaptive_codebook(
1710 : Word16 *exc, /* i/o: pointer to the excitation frame Q_new */
1711 : Word16 T0, /* i : integer pitch lag Q0 */
1712 : Word16 T0_frac, /* i : fraction of lag Q0 */
1713 : Word16 T0_res, /* i : pitch resolution Q0 */
1714 : Word16 T0_res_max, /* i : maximum pitch resolution Q0 */
1715 : Word16 mode, /* i : filtering mode (0: no, 1: yes, 2: adaptive) Q0 */
1716 : Word16 i_subfr, /* i : subframe index */
1717 : Word16 L_subfr, /* i : subframe length Q0 */
1718 : Word16 L_frame, /* i : subframe length Q0 */
1719 : Word16 *h1, /* i : impulse response of weighted synthesis filter 1Q14+shift */
1720 : Word16 clip_gain, /* i : flag to indicate ??? Q14 */
1721 : Word16 *xn, /* i : Close-loop Pitch search target vector Q_xn */
1722 : Word16 *y1, /* o : zero-memory filtered adaptive excitation Q_xn */
1723 : ACELP_CbkCorr *g_corr, /* o : ACELP correlation values */
1724 : Word16 **pt_indice, /* i/o: quantization indices pointer */
1725 : Word16 *pitch_gain, /* o : adaptive codebook gain 1Q14 */
1726 : Word16 exp_xn, /* i : exponent of xn (Q_xn-15) */
1727 : Word16 rf_mode,
1728 : Word16 rf_coder_type,
1729 : Word16 *lp_select )
1730 : {
1731 : Word16 y2[L_SUBFR], xn2[L_SUBFR], code[L_SUBFR];
1732 : ACELP_CbkCorr g_corr2;
1733 3060 : Word16 gain1 = 0, gain2 = 0, fac_m, fac_n;
1734 3060 : move16();
1735 3060 : move16();
1736 : Word16 i, select, exp_ener;
1737 : Word32 L_tmp, L_ener;
1738 : const Word16 *pitch_inter;
1739 : Word16 pit_L_interpol, pit_up_samp;
1740 3060 : Word16 use_prev_sf_pit_gain = 0;
1741 3060 : move16();
1742 :
1743 3060 : test();
1744 3060 : IF( EQ_16( rf_mode, 1 ) && EQ_16( rf_coder_type, 100 ) )
1745 : {
1746 0 : use_prev_sf_pit_gain = 1;
1747 0 : move16();
1748 : }
1749 :
1750 : BASOP_SATURATE_ERROR_ON_EVS;
1751 :
1752 3060 : L_ener = L_deposit_l( 0 );
1753 :
1754 : /* find pitch excitation */
1755 : /*for &exc[i_subfr]*/
1756 3060 : IF( EQ_16( T0_res, shr( T0_res_max, 1 ) ) )
1757 : {
1758 2948 : T0_frac = shl( T0_frac, 1 ); /*Q0*/
1759 : }
1760 :
1761 3060 : test();
1762 3060 : IF( EQ_16( T0_res_max, 6 ) && rf_mode == 0 )
1763 : {
1764 3060 : pitch_inter = pitch_inter6_2; /*Q14*/
1765 3060 : pit_L_interpol = PIT_L_INTERPOL6_2;
1766 3060 : move16();
1767 3060 : pit_up_samp = PIT_UP_SAMP6;
1768 3060 : move16();
1769 : }
1770 : ELSE
1771 : {
1772 0 : pitch_inter = pitch_inter4_2; /*Q14*/
1773 0 : pit_L_interpol = L_INTERPOL2;
1774 0 : move16();
1775 0 : pit_up_samp = PIT_UP_SAMP;
1776 0 : move16();
1777 : }
1778 :
1779 3060 : pred_lt4( &exc[i_subfr], &exc[i_subfr], T0, T0_frac, L_SUBFR + 1, pitch_inter, pit_L_interpol, pit_up_samp );
1780 :
1781 3060 : test();
1782 3060 : IF( EQ_16( mode, NORMAL_OPERATION ) || ( EQ_16( mode, FULL_BAND ) ) )
1783 : {
1784 1350 : E_UTIL_f_convolve( &exc[i_subfr], h1, y1, L_subfr );
1785 :
1786 1350 : IF( use_prev_sf_pit_gain == 0 )
1787 : {
1788 1350 : gain1 = E_ACELP_xy1_corr_fx( xn, y1, g_corr, 1, L_subfr, exp_xn ); /*Q14*/
1789 :
1790 : /* clip gain if necessary to avoid problem at decoder */
1791 1350 : test();
1792 1350 : IF( clip_gain && GT_16( gain1, 15565 /*0.95 Q14*/ ) )
1793 : {
1794 0 : gain1 = 15565 /*0.95f Q14*/;
1795 0 : move16();
1796 : }
1797 1350 : *pitch_gain = gain1; /*Q14*/
1798 1350 : move16();
1799 : }
1800 :
1801 : /* find energy of new target xn2[] */
1802 1350 : E_ACELP_codebook_target_update_fx( xn, xn2, y1, gain1, L_subfr );
1803 1350 : L_ener = Dot_product12_offs( xn2, xn2, L_subfr, &exp_ener, 0 ); /*Q31-exp_ener*/
1804 1350 : L_ener = L_shr( L_ener, sub( 31, exp_ener ) ); /*Q0*/
1805 : }
1806 :
1807 : /*-----------------------------------------------------------------*
1808 : * - find pitch excitation filtered by 1st order LP filter. *
1809 : * - find filtered pitch exc. y2[]=exc[] convolved with h1[]) *
1810 : * - compute pitch gain2 *
1811 : *-----------------------------------------------------------------*/
1812 :
1813 3060 : test();
1814 3060 : IF( EQ_16( mode, NORMAL_OPERATION ) || EQ_16( mode, LOW_PASS ) )
1815 : {
1816 : /* find pitch excitation with lp filter */
1817 3060 : fac_m = 20972 /*0.64f Q15*/;
1818 3060 : move16();
1819 3060 : if ( EQ_16( L_frame, L_FRAME16k ) )
1820 : {
1821 3060 : fac_m = 19005 /*0.58f Q15*/;
1822 3060 : move16();
1823 : }
1824 : /* fac_n = 0.5*(1.0-fac_m); */
1825 3060 : fac_n = mult_r( sub( 0x7FFF, fac_m ), 0x4000 ); /*Q15*/
1826 198900 : FOR( i = 0; i < L_subfr; i++ )
1827 : {
1828 195840 : L_tmp = L_mult( fac_n, exc[i - 1 + i_subfr] ); /*Q_new+Q16*/
1829 195840 : L_tmp = L_mac( L_tmp, fac_m, exc[i + 0 + i_subfr] ); /*Q_new+Q16*/
1830 195840 : code[i] = mac_r( L_tmp, fac_n, exc[i + 1 + i_subfr] ); /*Q_new*/
1831 195840 : move16();
1832 : }
1833 3060 : E_UTIL_f_convolve( code, h1, y2, L_subfr );
1834 3060 : gain2 = E_ACELP_xy1_corr_fx( xn, y2, &g_corr2, 1, L_subfr, exp_xn ); /*Q14*/
1835 :
1836 : /* clip gain if necessary to avoid problem at decoder */
1837 3060 : test();
1838 3060 : IF( clip_gain && GT_16( gain2, 15565 /*0.95 Q14*/ ) )
1839 : {
1840 0 : gain2 = 15565 /*0.95f Q14*/;
1841 0 : move16();
1842 : }
1843 :
1844 : /* find energy of new target xn2[] */
1845 3060 : E_ACELP_codebook_target_update_fx( xn, xn2, y2, gain2, L_subfr );
1846 3060 : L_tmp = Dot_product12_offs( xn2, xn2, L_subfr, &exp_ener, 0 ); /*Q31 - exp_ener*/
1847 3060 : L_tmp = L_shr( L_tmp, sub( 31, exp_ener ) ); /*Q0*/
1848 :
1849 : /*-----------------------------------------------------------------*
1850 : * use the best prediction (minimise quadratic error). *
1851 : *-----------------------------------------------------------------*/
1852 :
1853 3060 : test();
1854 3060 : IF( EQ_16( mode, LOW_PASS ) || LT_32( L_tmp, L_ener ) )
1855 : {
1856 : /* use the lp filter for pitch excitation prediction */
1857 2646 : select = LOW_PASS;
1858 2646 : move16();
1859 2646 : Copy( code, &exc[i_subfr], L_subfr ); /*Q_new*/
1860 2646 : Copy( y2, y1, L_subfr ); /*Q_xn*/
1861 2646 : *pitch_gain = gain2; /*Q14*/
1862 2646 : move16();
1863 2646 : g_corr->y1y1 = g_corr2.y1y1;
1864 2646 : move16();
1865 2646 : g_corr->xy1 = g_corr2.xy1;
1866 2646 : move16();
1867 2646 : g_corr->y1y1_e = g_corr2.y1y1_e;
1868 2646 : move16();
1869 2646 : g_corr->xy1_e = g_corr2.xy1_e;
1870 2646 : move16();
1871 : }
1872 : ELSE
1873 : {
1874 : /* no filter used for pitch excitation prediction */
1875 414 : select = FULL_BAND;
1876 414 : move16();
1877 414 : *pitch_gain = gain1; /*Q14*/
1878 414 : move16();
1879 : }
1880 :
1881 3060 : IF( EQ_16( mode, NORMAL_OPERATION ) )
1882 : {
1883 1350 : **pt_indice = select;
1884 1350 : ( *pt_indice )++;
1885 1350 : move16();
1886 : }
1887 : }
1888 : ELSE
1889 : {
1890 : /* no filter used for pitch excitation prediction */
1891 0 : select = FULL_BAND;
1892 0 : move16();
1893 : }
1894 3060 : *lp_select = select;
1895 3060 : move16();
1896 :
1897 : BASOP_SATURATE_ERROR_OFF_EVS;
1898 3060 : }
1899 :
1900 :
1901 : /*--------------------------------------------------------------------------*
1902 : * E_ACELP_innovative_codebook_fx
1903 : *
1904 : * Find innovative codebook.
1905 : *--------------------------------------------------------------------------*/
1906 :
1907 3060 : void E_ACELP_innovative_codebook_fx(
1908 : Word16 *exc, /* i : pointer to the excitation frame Q_new */
1909 : Word16 T0, /* i : integer pitch lag Q0 */
1910 : Word16 T0_frac, /* i : fraction of lag Q0 */
1911 : Word16 T0_res, /* i : pitch resolution Q0 */
1912 : Word16 pitch_gain, /* i : adaptive codebook gain 1Q14 */
1913 : Word16 tilt_code, /* i : tilt factor Q15 */
1914 : ACELP_config *acelp_cfg, /* i/o: configuration of the ACELP */
1915 : Word16 i_subfr, /* i : subframe index */
1916 : const Word16 *Aq, /* i : quantized LPC coefficients 3Q12 */
1917 : Word16 *h1, /* i : impulse response of weighted synthesis filter 1Q14+shift */
1918 : Word16 *xn, /* i : Close-loop Pitch search target vector Q_xn */
1919 : Word16 *cn, /* i : Innovative codebook search target vector Q_new */
1920 : Word16 *y1, /* i : zero-memory filtered adaptive excitation Q_xn */
1921 : Word16 *y2, /* o : zero-memory filtered algebraic excitation Q9 */
1922 : Word8 acelpautoc, /* i : autocorrelation mode enabled */
1923 : Word16 **pt_indice, /* i/o: quantization indices pointer */
1924 : Word16 *code, /* o : innovative codebook Q9 Q9 */
1925 : Word16 shift, /* i : Scaling to get 12 bits */
1926 : const Word16 L_frame, /* i : length of the frame Q0*/
1927 : const Word16 last_L_frame, /* i : length of the last frame Q0*/
1928 : const Word32 total_brate /* i : ttoal bit-rate Q0*/
1929 : )
1930 : {
1931 : Word16 xn2[L_SUBFR] /* Q_xn */, cn2[L_SUBFR] /* Q_xn */, dn[L_SUBFR] /* Rw2*cn2 */, h2[L_SUBFR] /* 4Q11 */;
1932 : Word16 Rw2[L_SUBFR];
1933 : Word16 pitch, idx;
1934 :
1935 3060 : pitch = T0;
1936 3060 : move16();
1937 3060 : idx = shr( i_subfr, 6 );
1938 3060 : if ( GT_16( T0_frac, shr( T0_res, 1 ) ) )
1939 : {
1940 729 : pitch = add( pitch, 1 );
1941 : }
1942 :
1943 : BASOP_SATURATE_ERROR_ON_EVS;
1944 :
1945 : /* Update target vector for ACELP codebook search */
1946 3060 : E_ACELP_codebook_target_update_fx( xn, xn2, y1, pitch_gain, L_SUBFR );
1947 :
1948 : /* Include fixed-gain pitch contribution into impulse resp. h1[] */
1949 3060 : Copy_Scale_sig( h1, h2, L_SUBFR, sub( -3, shift ) ); /*h2 1Q14+shift -> 4Q11, 1bit of headroom for Residu and xh_corr*/
1950 :
1951 3060 : cb_shape_fx( acelp_cfg->pre_emphasis, acelp_cfg->pitch_sharpening, acelp_cfg->phase_scrambling, acelp_cfg->formant_enh, acelp_cfg->formant_tilt,
1952 3060 : acelp_cfg->formant_enh_num, acelp_cfg->formant_enh_den, Aq, h2, tilt_code, pitch, 1, L_SUBFR );
1953 :
1954 : /* Correlation between target xn2[] and impulse response h1[] */
1955 3060 : IF( acelpautoc )
1956 : {
1957 : /* h2: 4Q11, Rw2: (Rw2_e)Q */
1958 3060 : /* Rw2_e = */ E_ACELP_hh_corr( h2, Rw2, L_SUBFR, 3 );
1959 :
1960 3060 : E_ACELP_conv( xn2, h2, cn2 );
1961 :
1962 : /* dn_e -> Rw2_e*Q_xn */
1963 3060 : /*dn_e = */ E_ACELP_toeplitz_mul_fx( Rw2, cn2, dn, L_SUBFR, sub( (Word16) PulseConfTable[acelp_cfg->fixed_cdk_index[idx]].nb_pulse, 24 ) > 0 );
1964 : }
1965 : ELSE
1966 : {
1967 : BASOP_SATURATE_WARNING_OFF_EVS;
1968 0 : E_ACELP_codebook_target_update_fx( cn, cn2, &exc[i_subfr], pitch_gain, L_SUBFR );
1969 : BASOP_SATURATE_WARNING_ON_EVS;
1970 0 : Scale_sig( cn2, L_SUBFR, shift );
1971 0 : E_ACELP_xh_corr( xn2, dn, h2, L_SUBFR );
1972 : }
1973 :
1974 : /* Innovative codebook search */
1975 3060 : assert( acelp_cfg->fixed_cdk_index[idx] < ACELP_FIXED_CDK_NB );
1976 :
1977 3060 : E_ACELP_4t_fx( dn, cn2, h2, Rw2, acelpautoc, code, acelp_cfg->fixed_cdk_index[idx], *pt_indice, L_frame, last_L_frame, total_brate, i_subfr, 0, 0 );
1978 3060 : *pt_indice += 8;
1979 :
1980 : /* Generate weighted code */
1981 3060 : E_ACELP_weighted_code( code, h2, 11, y2 );
1982 :
1983 : /*-------------------------------------------------------*
1984 : * - Add the fixed-gain pitch contribution to code[]. *
1985 : *-------------------------------------------------------*/
1986 :
1987 3060 : cb_shape_fx( acelp_cfg->pre_emphasis, acelp_cfg->pitch_sharpening, acelp_cfg->phase_scrambling, acelp_cfg->formant_enh, acelp_cfg->formant_tilt,
1988 3060 : acelp_cfg->formant_enh_num, acelp_cfg->formant_enh_den, Aq, code, tilt_code, pitch, 1, L_SUBFR );
1989 :
1990 : BASOP_SATURATE_ERROR_OFF_EVS;
1991 3060 : return;
1992 : }
1993 :
1994 :
1995 : /*--------------------------------------------------------------------------*
1996 : * E_ACELP_codearithp_fx
1997 : *
1998 : * Fixed bit-length arithmetic coding of pulses
1999 : * v - (input) pulse vector
2000 : * ps - (output) encoded state
2001 : * n - (output) range of possible states (0...n-1)
2002 : * p - (output) number of pulses found
2003 : *--------------------------------------------------------------------------*/
2004 :
2005 2180244 : static void E_ACELP_codearithp_fx(
2006 : const Word16 v[] /*Q9*/,
2007 : UWord32 *n /*Q0*/,
2008 : UWord32 *ps /*Q0*/,
2009 : Word16 *p /*Q0*/ )
2010 : {
2011 : Word16 k, nb_pulse, i, t, pos[NPMAXPT], posno;
2012 : Word16 sign, m;
2013 : UWord64 W_s;
2014 :
2015 : /* Collect different pulse positions to pos[], number of them to posno */
2016 2180244 : posno = 0;
2017 2180244 : move16();
2018 2180244 : t = 0;
2019 2180244 : move16();
2020 37064148 : FOR( k = 0; k < L_SUBFR; k += 4 )
2021 : {
2022 34883904 : if ( v[k] != 0 )
2023 : {
2024 5305374 : pos[posno++] = t;
2025 5305374 : move16();
2026 : }
2027 34883904 : t = add( t, 1 );
2028 34883904 : move16();
2029 34883904 : if ( sub( posno, NPMAXPT ) >= 0 )
2030 : {
2031 0 : BREAK;
2032 : }
2033 : }
2034 :
2035 : /* Iterate over the different pulse positions */
2036 2180244 : W_s = 0;
2037 2180244 : t = 0;
2038 2180244 : move16();
2039 2180244 : nb_pulse = 0;
2040 2180244 : move16();
2041 7485618 : FOR( k = 0; k < posno; ++k )
2042 : {
2043 5305374 : sign = shr( v[( pos[k] * 4 )], 9 ); /* sign with multiplicity Q0*/
2044 5305374 : m = abs_s( sign ); /* multiplicity */
2045 5305374 : nb_pulse = add( nb_pulse, m );
2046 : /* Code m-1 pulses */
2047 5625432 : FOR( i = 1; i < m; ++i )
2048 : {
2049 320058 : W_s = W_add( W_s, pulsestostates[pos[k]][t] );
2050 320058 : t = add( t, 1 );
2051 320058 : if ( sub( t, NPMAXPT ) > 0 )
2052 : {
2053 0 : BREAK;
2054 : }
2055 : }
2056 :
2057 : /* Code sign */
2058 : /* We use L_add_c since we want to work with unsigned UWord32 */
2059 : /* Therefore, we have to clear carry */
2060 5305374 : W_s = W_lshl( W_s, 1 );
2061 5305374 : if ( sign < 0 )
2062 : {
2063 2685058 : W_s = W_add( W_s, 1 );
2064 : }
2065 :
2066 : /* Code last pulse */
2067 5305374 : W_s = W_add( W_s, pulsestostates[pos[k]][t] );
2068 5305374 : t = add( t, 1 );
2069 : }
2070 :
2071 2180244 : *ps = (UWord32) W_s; /*Q0*/
2072 2180244 : move32();
2073 2180244 : *n = L_deposit_l( 0 );
2074 2180244 : if ( nb_pulse )
2075 : {
2076 2156620 : *n = pulsestostates[NB_POS_FCB_4T][nb_pulse - 1]; /*Q0*/
2077 2156620 : move32();
2078 : }
2079 :
2080 2180244 : *p = nb_pulse; /*Q0*/
2081 2180244 : move16();
2082 :
2083 2180244 : return;
2084 : }
2085 :
2086 :
2087 545061 : void fcb_pulse_track_joint_fx(
2088 : UWord16 *idxs /*Q0*/,
2089 : Word16 wordcnt /*Q0*/,
2090 : UWord32 *index_n /*Q0*/,
2091 : Word16 *pulse_num /*Q0*/,
2092 : Word16 track_num /*Q0*/ )
2093 : {
2094 : Word16 hi_to_low[10];
2095 : UWord32 index, index_mask;
2096 : Word32 indx_tmp;
2097 : Word16 indx_flag, indx_flag_1;
2098 : Word16 track, track_num1, pulse_num0, pulse_num1;
2099 : Word16 indx_flag_2;
2100 :
2101 545061 : Copy( hi_to_low_tmpl, hi_to_low, 10 ); /*Q0*/
2102 :
2103 545061 : indx_flag = 0;
2104 545061 : move16();
2105 545061 : indx_flag_1 = 0;
2106 545061 : move16();
2107 545061 : indx_flag_2 = 0;
2108 545061 : move16();
2109 :
2110 2725305 : FOR( track = 0; track < track_num; track++ )
2111 : {
2112 2180244 : indx_flag = add( indx_flag, shr( pulse_num[track], 2 ) );
2113 2180244 : indx_flag_1 = add( indx_flag_1, shr( pulse_num[track], 1 ) );
2114 2180244 : indx_flag_2 = add( indx_flag_2, shr( pulse_num[track], 3 ) );
2115 : }
2116 :
2117 545061 : IF( GE_16( indx_flag_2, 1 ) )
2118 : {
2119 46 : hi_to_low[7] = 9;
2120 46 : move16();
2121 46 : index_mask = 0xFFFFFF; /*Q0*/
2122 46 : move32();
2123 : }
2124 : ELSE
2125 : {
2126 545015 : if ( LT_16( indx_flag, track_num ) )
2127 : {
2128 462413 : hi_to_low[4] = 1;
2129 462413 : move16();
2130 : }
2131 545015 : index_mask = L_shr( 0xFFFF, sub( 9, hi_to_low[4] ) ); /*Q0*/
2132 : }
2133 :
2134 545061 : IF( GE_16( indx_flag_1, track_num ) )
2135 : {
2136 345201 : indx_tmp = L_deposit_l( 0 );
2137 345201 : index = L_shr( index_n[0], low_len[pulse_num[0]] ); /*Q0*/
2138 1380804 : FOR( track = 1; track < track_num; track++ )
2139 : {
2140 1035603 : pulse_num0 = pulse_num[track - 1]; /*Q0*/
2141 1035603 : move16();
2142 1035603 : pulse_num1 = pulse_num[track]; /*Q0*/
2143 1035603 : move16();
2144 1035603 : indx_tmp = L_lshr( index_n[track], low_len[pulse_num1] ); /*Q0*/
2145 : /* index = index * indx_fact[pulse_num1] + indx_tmp; */
2146 1035603 : index = UL_Mpy_32_32( index, UL_deposit_l( indx_fact[pulse_num1] ) ); /*Q0*/
2147 1035603 : index = UL_addNsD( index, indx_tmp );
2148 1035603 : index_n[track - 1] = L_add( L_and( index_n[track - 1], low_mask[pulse_num0] ),
2149 1035603 : L_and( L_lshl( index, low_len[pulse_num0] ), index_mask ) ); /*Q0*/
2150 :
2151 1035603 : index = L_lshr( index, hi_to_low[pulse_num0] );
2152 : }
2153 345201 : track_num1 = sub( track_num, 1 );
2154 345201 : move16();
2155 345201 : pulse_num1 = pulse_num[track_num1]; /*Q0*/
2156 345201 : move16();
2157 345201 : index_n[track_num1] = L_and( L_add( L_and( index_n[track_num1], low_mask[pulse_num1] ),
2158 345201 : L_lshl( index, low_len[pulse_num1] ) ),
2159 : index_mask ); /*Q0*/
2160 345201 : index = L_lshr( index, hi_to_low[pulse_num1] );
2161 345201 : IF( GE_16( indx_flag, track_num ) )
2162 : {
2163 82648 : IF( GE_16( indx_flag_2, 1 ) )
2164 : {
2165 46 : idxs[0] = extract_l( index_n[0] );
2166 46 : idxs[1] = extract_l( L_add( L_lshl( index_n[1], 8 ), L_lshr( index_n[0], 16 ) ) );
2167 46 : idxs[2] = extract_l( L_lshr( index_n[1], 8 ) );
2168 46 : idxs[3] = extract_l( index_n[2] );
2169 46 : idxs[4] = extract_l( L_add( L_lshl( index_n[3], 8 ), L_lshr( index_n[2], 16 ) ) );
2170 46 : idxs[5] = extract_l( L_lshr( index_n[3], 8 ) );
2171 46 : track = 6;
2172 46 : move16();
2173 : }
2174 : ELSE
2175 : {
2176 413010 : FOR( track = 0; track < track_num; track++ )
2177 : {
2178 330408 : idxs[track] = extract_l( index_n[track] );
2179 : }
2180 : }
2181 : }
2182 : ELSE
2183 : {
2184 262553 : idxs[0] = extract_l( L_add( L_lshl( index_n[0], 8 ), index_n[1] ) );
2185 262553 : idxs[1] = extract_l( L_add( L_lshl( index_n[2], 8 ), index_n[3] ) );
2186 262553 : track = 2;
2187 262553 : move16();
2188 : }
2189 : }
2190 : ELSE
2191 : {
2192 199860 : index = index_n[0]; /*Q0*/
2193 199860 : move32();
2194 799440 : FOR( track = 1; track < 4; track++ )
2195 : {
2196 599580 : pulse_num1 = pulse_num[track]; /*Q0*/
2197 599580 : index = L_add( L_lshl( index, index_len[pulse_num1] ), index_n[track] );
2198 : }
2199 199860 : track = 0;
2200 199860 : move16();
2201 : }
2202 1440689 : FOR( ; track < wordcnt; track++ )
2203 : {
2204 895628 : idxs[track] = extract_l( index );
2205 895628 : index = L_lshr( index, 16 );
2206 : }
2207 :
2208 545061 : return;
2209 : }
|
