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 2033966 : 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 2033966 : dn2 = &dn2_pos[( track * 8 )]; /*Q0*/
70 2033966 : p0 = rrixix[track]; /*Q9*/
71 :
72 15409334 : FOR( i = 0; i < nb_pulse; i++ )
73 : {
74 13375368 : dn = dn2[i]; /*Q0*/
75 13375368 : move16();
76 13375368 : L_sum = L_deposit_l( 0 );
77 13375368 : p1 = h; /*Qx*/
78 13375368 : p2 = &vec[dn]; /*Qx*/
79 467259960 : FOR( j = dn; j < L_SUBFR - 1; j++ )
80 : {
81 453884592 : L_sum = L_mac_sat( L_sum, *p1++, *p2++ ); /*2*Qx+1*/
82 : }
83 :
84 13375368 : corr = mac_r_sat( L_sum, *p1++, *p2++ ); /*Q9*/
85 :
86 : /*cor[dn >> 2] = sign[dn] * s + p0[dn >> 2];*/
87 13375368 : j = shr( dn, 2 );
88 13375368 : if ( sign[dn] > 0 )
89 : {
90 6615623 : corr = add_sat( p0[j], corr ); /*Q9*/
91 : }
92 13375368 : if ( sign[dn] < 0 )
93 : {
94 6759745 : corr = sub_sat( p0[j], corr ); /*Q9*/
95 : }
96 :
97 13375368 : cor[j] = corr; /*Q9*/
98 13375368 : move16();
99 : }
100 :
101 2033966 : return;
102 : }
103 :
104 :
105 2946902 : 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 2946902 : p0 = rrixix[track]; /*Q9*/
118 :
119 2946902 : pos = track; /*Q0*/
120 2946902 : move16();
121 50097334 : FOR( i = 0; i < 16; i++ )
122 : {
123 47150432 : L_sum = L_deposit_l( 0 );
124 47150432 : p1 = h; /*Qx*/
125 47150432 : p2 = &vec[pos]; /*Qx*/
126 1535022352 : FOR( j = pos; j < L_SUBFR - 1; j++ )
127 : {
128 1487871920 : L_sum = L_mac_sat( L_sum, *p1++, *p2++ ); /* 2*Qx+1 */
129 : }
130 47150432 : corr = mac_r_sat( L_sum, *p1++, *p2++ ); /*Q9*/
131 :
132 : /*cor[i] = s * sign[track] + p0[i];*/
133 :
134 47150432 : if ( sign[pos] > 0 )
135 : {
136 23379751 : corr = add_sat( *p0++, corr ); /*Q9*/
137 : }
138 47150432 : if ( sign[pos] < 0 )
139 : {
140 23770681 : corr = sub_sat( *p0++, corr ); /*Q9*/
141 : }
142 47150432 : cor[i] = corr; /*Q9*/
143 47150432 : move16();
144 :
145 47150432 : pos = add( pos, 4 );
146 : }
147 :
148 2946902 : 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 2033966 : 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 2033966 : 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 2033966 : pos_x = &dn2[( track_x * 8 )]; /*Qdn*/
200 2033966 : move16();
201 :
202 : /* save these to limit memory searches */
203 2033966 : alp0 = L_deposit_h( *alp ); /*Qx-16*/
204 2033966 : ps0 = *ps; /*Qdn*/
205 2033966 : move16();
206 :
207 2033966 : alpk[0] = 1;
208 2033966 : move16();
209 2033966 : sqk[0] = -1;
210 2033966 : move16();
211 2033966 : x2 = shr( pos_x[0], 2 ); /*Qdn*/
212 2033966 : 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 2033966 : ik = 0;
218 2033966 : move16();
219 2033966 : xy_save = L_mac0( L_deposit_l( track_y ), track_x, L_SUBFR ); /*Q0*/
220 :
221 : /* loop track 1 */
222 15409334 : FOR( i = 0; i < nb_pos_ix; i++ )
223 : {
224 13375368 : x = pos_x[i]; /*Qdn*/
225 13375368 : move16();
226 13375368 : x2 = shr( x, 2 );
227 : /* dn[x] has only nb_pos_ix positions saved */
228 : /*ps1 = ps0 + dn[x];*/
229 13375368 : ps1 = add( ps0, dn[x] ); /*Qdn*/
230 :
231 : /*alp1 = alp0 + cor_x[x2];*/
232 13375368 : alp1 = L_mac_sat( alp0, cor_x[x2], _1_ ); /*Q22*/
233 :
234 13375368 : p1 = cor_y; /*Qx*/
235 13375368 : p2 = &rrixiy[track_x][( x2 * 16 )]; /*Q9*/
236 :
237 227381256 : FOR( y = track_y; y < L_SUBFR; y += 4 )
238 : {
239 : /*ps2 = ps1 + dn[y];*/
240 214005888 : ps2 = add( ps1, dn[y] ); /*Qdn*/
241 214005888 : move16();
242 :
243 : /*alp2 = alp1 + (*p1++) + (*p2++);*/
244 214005888 : alp2 = L_mac_sat( alp1, *p1++, _1_ ); /*Qx+12+1*/
245 214005888 : alp2_16 = mac_r_sat( alp2, *p2++, _1_ ); /*Q6*/
246 214005888 : alpk[1 - ik] = alp2_16; /*Q6*/
247 214005888 : move16();
248 :
249 : /*sq = ps2 * ps2;*/
250 214005888 : sq = mult( ps2, ps2 ); /*2*Qdn+1*/
251 214005888 : sqk[1 - ik] = sq; /*2*Qdn+1*/
252 214005888 : move16();
253 :
254 : /*s = (alpk[ik] * sq) - (sqk[0] * alp2);*/
255 214005888 : s = L_msu( L_mult( alpk[ik], sq ), sqk[ik], alp2_16 ); /*Q16*/
256 :
257 214005888 : if ( s > 0 )
258 : {
259 9868400 : ik = sub( 1, ik );
260 9868400 : move16();
261 9868400 : check = 1; /* debug code not instrumented */
262 9868400 : move16();
263 : }
264 214005888 : if ( s > 0 )
265 : {
266 9868400 : xy_save = L_mac0( y, x, L_SUBFR ); /*Qdn*/
267 : }
268 : }
269 : }
270 :
271 2033966 : assert( check ); /* debug code not instrumented */
272 :
273 2033966 : ps2 = extract_l( xy_save ); /*Qdn*/
274 2033966 : *iy = s_and( ps2, L_SUBFR - 1 ); /*Q0*/
275 2033966 : move16();
276 2033966 : *ix = lshr( ps2, 6 ); /*Q0*/
277 2033966 : move16();
278 :
279 : /**ps = ps0 + dn[*ix] + dn[*iy];*/
280 2033966 : *ps = add( ps0, add( dn[*ix], dn[*iy] ) ); /*Qdn*/
281 2033966 : move16();
282 :
283 2033966 : *alp = alpk[ik]; /*Q6*/
284 2033966 : move16();
285 :
286 2033966 : 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 456468 : 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 456468 : 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 456468 : Word16 check = 0; /* debug code not instrumented */
326 :
327 : /* save these to limit memory searches */
328 456468 : alp0 = L_deposit_h( *alp );
329 456468 : ps0 = *ps; /*Qdn*/
330 456468 : move16();
331 :
332 456468 : alpk[0] = 1;
333 456468 : move16();
334 456468 : sqk[0] = -1;
335 456468 : move16();
336 456468 : if ( mac_r_sat( alp0, cor_x[( tracks[0] / 4 )], _1_ ) < 0 )
337 : {
338 0 : sqk[0] = 1;
339 0 : move16();
340 : }
341 456468 : ik = 0;
342 456468 : move16();
343 :
344 456468 : ntracks = 1;
345 456468 : move16();
346 456468 : if ( NE_16( tracks[1], tracks[0] ) )
347 : {
348 273975 : ntracks = 2;
349 273975 : move16();
350 : }
351 1186911 : FOR( t = 0; t < ntracks; ++t )
352 : {
353 730443 : if ( t != 0 )
354 : {
355 273975 : cor_x = cor_y;
356 273975 : move16();
357 : }
358 12417531 : FOR( x = tracks[t]; x < L_SUBFR; x += 4 )
359 : {
360 : /*ps1 = ps0 + dn[x]; ADD(1);*/
361 11687088 : ps1 = add( ps0, dn[x] ); /*Qdn*/
362 :
363 : /*alp1 = alp0 + cor_x[x>>2]; SHIFT(1);ADD(1);*/
364 11687088 : alp1 = mac_r_sat( alp0, cor_x[( x / 4 )], _1_ ); /*Q6*/
365 11687088 : alpk[1 - ik] = alp1; /*Q6*/
366 11687088 : move16();
367 :
368 : /*sq = ps1 * ps1; MULT(1);*/
369 11687088 : sq = mult( ps1, ps1 ); /*2*Qdn+1*/
370 11687088 : sqk[1 - ik] = sq; /*2*Qdn+1*/
371 11687088 : move16();
372 :
373 : /*s = (alpk * sq) - (sqk * alp1); MULT(1);MAC(1); */
374 11687088 : s = L_msu( L_mult( alpk[ik], sq ), sqk[ik], alp1 ); /*Q16*/
375 :
376 11687088 : if ( s > 0 )
377 : {
378 1909279 : ik = sub( 1, ik );
379 1909279 : check = 1; /* debug code not instrumented */
380 : }
381 11687088 : if ( s > 0 )
382 : {
383 1909279 : x_save = x;
384 1909279 : move16();
385 : }
386 : }
387 :
388 730443 : assert( check ); /* debug code not instrumented */
389 : }
390 456468 : *ps = add( ps0, dn[x_save] ); /*2*Qdn+1*/
391 456468 : move16();
392 456468 : *alp = alpk[ik]; /*Q6*/
393 456468 : move16();
394 456468 : *ix = x_save; /*Q0*/
395 456468 : move16();
396 :
397 456468 : 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 524760 : Word16 E_ACELP_hh_corr(
477 : Word16 *x /*Q11*/,
478 : Word16 *y /*Qy*/,
479 : Word16 L_subfr /*Q0*/,
480 : Word16 bits /*Q0*/ )
481 : {
482 524760 : Word16 i, j, k = 0; /* initialize just to avoid compiler warning */
483 : Word32 L_tmp, L_sum;
484 :
485 33584640 : FOR( i = 0; i < L_subfr - 1; i++ )
486 : {
487 : Word64 L_tmp_64;
488 : Word64 L_sum_64;
489 :
490 33059880 : L_tmp_64 = W_mult0_16_16( x[i], x[0] ); /*Q22*/
491 553621800 : FOR( j = i + 2; j < L_subfr; j += 2 )
492 : {
493 520561920 : L_tmp_64 = W_mac0_16_16( L_tmp_64, x[j], x[j - i] ); /*Q22*/
494 : }
495 33059880 : L_sum_64 = L_tmp_64;
496 33059880 : move64();
497 :
498 33059880 : L_tmp_64 = W_mult0_16_16( x[i + 1], x[1] ); /*Q22*/
499 537354240 : FOR( j = i + 3; j < L_subfr; j += 2 )
500 : {
501 504294360 : L_tmp_64 = W_mac0_16_16( L_tmp_64, x[j], x[j - i] ); /*Q22*/
502 : }
503 33059880 : L_sum_64 = W_add_nosat( W_shr( L_sum_64, 1 ), W_shr( L_tmp_64, 1 ) );
504 33059880 : L_sum = W_sat_l( L_sum_64 ); /*Q22*/
505 : /* L_sum = L_shr( L_sum, 1 ); */
506 33059880 : if ( i == 0 )
507 : {
508 524760 : k = norm_l( L_sum );
509 : }
510 33059880 : if ( i == 0 )
511 : {
512 524760 : k = sub( k, bits );
513 : }
514 :
515 33059880 : y[i] = round_fx( L_shl( L_sum, k ) ); /*Q22+k-16*/
516 : }
517 :
518 524760 : L_tmp = L_mult0( x[i], x[0] ); /*Q22*/
519 524760 : L_sum = L_shr( L_tmp, 1 );
520 524760 : y[i] = round_fx( L_shl( L_sum, k ) ); /*Qy = Q22+k-16*/
521 :
522 524760 : k = add( 1, k );
523 :
524 524760 : 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 8820 : 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 8820 : L_off = L_shr( 10737418l /*0.01f/2.0f Q31*/, s_min( add( exp_xn, exp_xn ), 31 ) );
559 8820 : L_off = L_max( 1, L_off ); /* ensure at least a '1' */
560 :
561 : /* Compute scalar product t1: <y1[] * y1[]> */
562 8820 : 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 8820 : 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 8820 : Q_xn = shl( sub( 15, exp_xn ), 1 );
569 8820 : g_corr->y1y1 = yy;
570 8820 : move16();
571 8820 : g_corr->y1y1_e = sub( exp_yy, Q_xn );
572 8820 : move16();
573 8820 : g_corr->xy1 = xy;
574 8820 : move16();
575 8820 : g_corr->xy1_e = sub( exp_xy, Q_xn );
576 8820 : move16();
577 :
578 : /* If (xy < 0) gain = 0 */
579 8820 : IF( xy < 0 )
580 : {
581 98 : move16();
582 98 : gain = 0;
583 98 : GOTO bail;
584 : }
585 :
586 : /* compute gain = xy/yy */
587 :
588 8722 : xy = mult_r( xy, 0x4000 ); /* Be sure xy < yy Q15 - exp_xy*/
589 8722 : gain = div_s( xy, yy ); /*Q14*/
590 :
591 8722 : i = add( exp_xy, 1 - 1 ); /* -1 -> gain in Q14 */
592 8722 : i = sub( i, exp_yy );
593 : BASOP_SATURATE_WARNING_OFF_EVS
594 8722 : 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 8722 : gain = s_min( 19661 /*1.2f Q14*/ /* 19661 */, gain ); /*Q14*/
601 :
602 : /*Limit the energy of pitch contribution*/
603 8722 : IF( norm_flag )
604 : {
605 : Word16 tmp, exp_tmp, exp_div;
606 :
607 : /* Compute scalar product <xn[],xn[]> */
608 8722 : 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 8722 : tmp = BASOP_Util_Divide1616_Scale( tmp, yy, &exp_div );
611 8722 : exp_tmp = add( sub( exp_tmp, exp_yy ), exp_div );
612 :
613 8722 : tmp = Sqrt16( tmp, &exp_tmp ); /*Q15 - exp_tmp*/
614 :
615 : /* Note: shl works as shl or shr. */
616 8722 : exp_tmp = sub( exp_tmp, 1 );
617 : BASOP_SATURATE_WARNING_OFF_EVS
618 8722 : 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 8722 : gain = s_min( gain, tmp ); /*Q14*/
622 : }
623 :
624 0 : bail:
625 :
626 8820 : 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 16952 : 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 16952 : 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 16952 : L_off = L_shr( 10737418l /*0.01f/2.0f Q31*/, sub( 30 - 9, exp_xn ) );
666 :
667 : /* Compute scalar product <xn[],y2[]> */
668 16952 : 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 16952 : 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 16952 : L_off = L_shr( 21474836l /*0.01f Q31*/, s_min( 31, sub( 30, shl( exp_xn, 1 ) ) ) );
675 16952 : xx = extract_h( Dot_product12_offs( xn, xn, L_subfr, &exp_xx, L_off ) ); /*Q15-exp_xx*/
676 :
677 :
678 16952 : g_corr->y2y2 = y2y2;
679 16952 : move16();
680 16952 : g_corr->y2y2_e = exp_y2y2;
681 16952 : move16();
682 16952 : g_corr->xy2 = xny2;
683 16952 : move16();
684 16952 : g_corr->xy2_e = exp_xny2;
685 16952 : move16();
686 16952 : g_corr->y1y2 = y1y2;
687 16952 : move16();
688 16952 : g_corr->y1y2_e = exp_y1y2;
689 16952 : move16();
690 16952 : g_corr->xx = xx;
691 16952 : move16();
692 16952 : g_corr->xx_e = exp_xx;
693 16952 : move16();
694 :
695 :
696 : BASOP_SATURATE_ERROR_OFF_EVS;
697 16952 : 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 14940 : 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 14940 : assert( gain >= 0 );
726 :
727 14940 : Q15_flag = 0;
728 14940 : move16();
729 14940 : if ( LT_16( gain, 1 << 14 ) )
730 : {
731 12842 : Q15_flag = 1;
732 12842 : move16();
733 : }
734 14940 : gain = shl( gain, Q15_flag ); /*Q14*/
735 :
736 971100 : FOR( i = 0; i < L_subfr; i++ )
737 : {
738 956160 : L_tmp = L_deposit_h( x[i] ); /*Q_xn+16*/
739 956160 : if ( Q15_flag == 0 )
740 : {
741 134272 : L_tmp = L_msu_sat( L_tmp, y[i], gain ); /*Q_xn+15*/
742 : }
743 956160 : x2[i] = msu_r_sat( L_tmp, y[i], gain ); /*Q_xn*/
744 956160 : move16();
745 : }
746 :
747 14940 : 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 722143 : 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 722143 : Lval = L_mac0( 1, cn[0], cn[0] ); /*2*Q_xn*/
790 722143 : Lcor = L_mac0( 1, dn[0], dn[0] ); /*2*Qdn*/
791 :
792 46217152 : FOR( i = 1; i < L_subfr; i++ )
793 : {
794 45495009 : Lval = L_mac0_sat( Lval, cn[i], cn[i] ); /*2*Q_xn*/
795 45495009 : Lcor = L_mac0( Lcor, dn[i], dn[i] ); /*2*Qdn*/
796 : }
797 :
798 722143 : e_dn = 31;
799 722143 : move16();
800 722143 : e_cn = 31;
801 722143 : move16();
802 :
803 722143 : Lval = Sqrt32( Lval, &e_dn ); /*Q31 - e_dn*/
804 722143 : Lcor = Sqrt32( Lcor, &e_cn ); /*Q31 - e_cn*/
805 722143 : i = sub( e_dn, e_cn );
806 722143 : if ( i < 0 )
807 319542 : Lval = L_shl( Lval, i );
808 722143 : if ( i > 0 )
809 271579 : Lcor = L_shr( Lcor, i );
810 :
811 722143 : k_dn = round_fx_sat( Lval ); /*Q15 - e_dn*/
812 722143 : k_cn = round_fx_sat( Lcor ); /*Q15 - e_cn*/
813 :
814 722143 : k_cn = mult_r( 0x2000, k_cn ); /* 1 in Q13 */
815 722143 : k_dn = mult_r( alp, k_dn ); /* alp in Q13 */
816 :
817 722143 : sign_neg = negate( sign_val ); /*Q15*/
818 :
819 722143 : signs[0] = sign_neg; /*Q15*/
820 722143 : move16();
821 722143 : signs[1] = sign_val; /*Q15*/
822 722143 : move16();
823 722143 : signs[2] = sign_neg; /*Q15*/
824 722143 : move16();
825 722143 : ptr16 = &signs[1]; /*Q15*/
826 :
827 46939295 : FOR( i = 0; i < L_subfr; i++ )
828 : {
829 : /*cor = (s * cn[i]) + (alp * dn[i]); MULT(1);MAC(1);*/
830 46217152 : Lcor = L_mult( cn[i], k_cn ); /*Q_xn + Q15 - e_cn + 1*/
831 46217152 : Lcor = L_mac( Lcor, dn[i], k_dn ); /*Qdn + Q15 - e_dn + 1*/
832 46217152 : val = round_fx_sat( L_shl_sat( Lcor, 4 ) ); /*shifting by 4 may overflow but improves accuracy Qdn + 4 - e_dn*/
833 :
834 46217152 : index = shr( val, 15 );
835 46217152 : sign[i] = ptr16[index]; /*Q15*/
836 46217152 : move16(); /* yields -1 (when ps < 0) or 0 (when ps >= 0) */
837 46217152 : vec[i] = ptr16[index + 1]; /*Q15*/
838 46217152 : move16();
839 :
840 46217152 : if ( val < 0 )
841 : {
842 23182321 : dn[i] = negate( dn[i] ); /*Qdn*/
843 23182321 : move16();
844 : }
845 46217152 : dn2[i] = abs_s( val ); /*Qdn2 = Qdn + 4 - e_dn*/
846 46217152 : move16(); /* dn2[] = mix of dn[] and cn[] */
847 : }
848 722143 : }
849 :
850 :
851 722143 : 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 3610715 : FOR( i = 0; i < 4; i++ )
860 : {
861 2888572 : i8 = shl( i, 3 );
862 25997148 : FOR( k = i8; k < i8 + 8; k++ )
863 : {
864 23108576 : ps_ptr = &dn2[i]; /*Qx*/
865 :
866 369737216 : FOR( j = i + 4; j < L_SUBFR; j += 4 )
867 : {
868 346628640 : if ( GT_16( dn2[j], *ps_ptr ) )
869 : {
870 54460822 : ps_ptr = &dn2[j]; /*Qx*/
871 54460822 : move16();
872 : }
873 : }
874 :
875 23108576 : *ps_ptr = -1; /* dn2 < 0 when position is selected */
876 23108576 : move16();
877 23108576 : dn2_pos[k] = (Word16) ( ps_ptr - dn2 );
878 23108576 : move16();
879 : }
880 2888572 : pos_max[i] = dn2_pos[i8]; /*Q0*/
881 2888572 : move16();
882 : }
883 722143 : }
884 :
885 :
886 13454592 : static void E_ACELP_apply_sign(
887 : Word16 *p0 /*Qx*/,
888 : Word16 *psign0 /*Q15*/ )
889 : {
890 13454592 : p0[0] = mult_r( p0[0], psign0[0] ); /*Qx*/
891 13454592 : move16();
892 13454592 : p0[1] = mult_r( p0[1], psign0[4] );
893 13454592 : move16();
894 13454592 : p0[2] = mult_r( p0[2], psign0[8] );
895 13454592 : move16();
896 13454592 : p0[3] = mult_r( p0[3], psign0[12] );
897 13454592 : move16();
898 13454592 : p0[4] = mult_r( p0[4], psign0[16] );
899 13454592 : move16();
900 13454592 : p0[5] = mult_r( p0[5], psign0[20] );
901 13454592 : move16();
902 13454592 : p0[6] = mult_r( p0[6], psign0[24] );
903 13454592 : move16();
904 13454592 : p0[7] = mult_r( p0[7], psign0[28] );
905 13454592 : move16();
906 13454592 : p0[8] = mult_r( p0[8], psign0[32] );
907 13454592 : move16();
908 13454592 : p0[9] = mult_r( p0[9], psign0[36] );
909 13454592 : move16();
910 13454592 : p0[10] = mult_r( p0[10], psign0[40] );
911 13454592 : move16();
912 13454592 : p0[11] = mult_r( p0[11], psign0[44] );
913 13454592 : move16();
914 13454592 : p0[12] = mult_r( p0[12], psign0[48] );
915 13454592 : move16();
916 13454592 : p0[13] = mult_r( p0[13], psign0[52] );
917 13454592 : move16();
918 13454592 : p0[14] = mult_r( p0[14], psign0[56] );
919 13454592 : move16();
920 13454592 : p0[15] = mult_r( p0[15], psign0[60] );
921 13454592 : move16();
922 :
923 13454592 : return;
924 : }
925 :
926 210228 : 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 13664820 : FOR( i = 0; i < L_subfr; i++ )
934 : {
935 13454592 : h_inv[i] = negate( h[i] );
936 13454592 : move16();
937 : }
938 :
939 210228 : return;
940 : }
941 :
942 :
943 210228 : 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 210228 : p0 = &rrixix[0][16 - 1]; /* Q9 */
964 210228 : p1 = &rrixix[1][16 - 1];
965 210228 : p2 = &rrixix[2][16 - 1];
966 210228 : p3 = &rrixix[3][16 - 1];
967 :
968 210228 : ptr_h1 = h; /*Q12*/
969 210228 : cor = L_deposit_l( 0 );
970 3573876 : FOR( i = 0; i < 16; i++ )
971 : {
972 3363648 : cor = L_mac( cor, *ptr_h1, *ptr_h1 ); /*Q25*/
973 3363648 : ptr_h1++;
974 3363648 : *p3-- = round_fx( L_shr( cor, 1 ) ); /* Q9 */
975 3363648 : cor = L_mac( cor, *ptr_h1, *ptr_h1 ); /*Q25*/
976 3363648 : ptr_h1++;
977 3363648 : *p2-- = round_fx( L_shr( cor, 1 ) ); /* Q9 */
978 3363648 : cor = L_mac( cor, *ptr_h1, *ptr_h1 ); /*Q25*/
979 3363648 : ptr_h1++;
980 3363648 : *p1-- = round_fx( L_shr( cor, 1 ) ); /* Q9 */
981 3363648 : cor = L_mac( cor, *ptr_h1, *ptr_h1 ); /*Q25*/
982 3363648 : ptr_h1++;
983 3363648 : *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 210228 : pos = 256 - 1;
993 210228 : move16();
994 210228 : ptr_hf = h + 1; /*Q12*/
995 3573876 : FOR( k = 0; k < 16; k++ )
996 : {
997 3363648 : p3 = &rrixiy[2][pos]; /* Q9 */
998 3363648 : p2 = &rrixiy[1][pos];
999 3363648 : p1 = &rrixiy[0][pos];
1000 3363648 : p0 = &rrixiy[3][pos - 16];
1001 :
1002 3363648 : cor = L_deposit_h( 0 );
1003 3363648 : ptr_h1 = h; /*Q12*/
1004 3363648 : ptr_h2 = ptr_hf; /*Q12*/
1005 :
1006 28591008 : FOR( i = k; i < 16 - 1; i++ )
1007 : {
1008 25227360 : cor = L_mac( cor, *ptr_h1++, *ptr_h2++ ); /*Q25*/
1009 25227360 : *p3 = round_fx( cor ); /*Q9*/
1010 25227360 : cor = L_mac( cor, *ptr_h1++, *ptr_h2++ ); /*Q25*/
1011 25227360 : *p2 = round_fx( cor ); /*Q9*/
1012 25227360 : cor = L_mac( cor, *ptr_h1++, *ptr_h2++ ); /*Q25*/
1013 25227360 : *p1 = round_fx( cor ); /*Q9*/
1014 25227360 : cor = L_mac( cor, *ptr_h1++, *ptr_h2++ ); /*Q25*/
1015 25227360 : *p0 = round_fx( cor ); /*Q9*/
1016 :
1017 25227360 : p3 -= ( 16 + 1 );
1018 25227360 : p2 -= ( 16 + 1 );
1019 25227360 : p1 -= ( 16 + 1 );
1020 25227360 : p0 -= ( 16 + 1 );
1021 : }
1022 3363648 : cor = L_mac( cor, *ptr_h1++, *ptr_h2++ ); /*Q25*/
1023 3363648 : *p3 = round_fx( cor ); /*Q9*/
1024 3363648 : cor = L_mac( cor, *ptr_h1++, *ptr_h2++ ); /*Q25*/
1025 3363648 : *p2 = round_fx( cor ); /*Q9*/
1026 3363648 : cor = L_mac( cor, *ptr_h1++, *ptr_h2++ ); /*Q25*/
1027 3363648 : *p1 = round_fx( cor ); /*Q9*/
1028 3363648 : move16();
1029 3363648 : move16();
1030 3363648 : move16();
1031 :
1032 3363648 : pos -= 16;
1033 3363648 : move16();
1034 3363648 : ptr_hf += 4;
1035 : }
1036 :
1037 : /* storage order --> i3i0, i2i3, i1i2, i0i1 */
1038 :
1039 210228 : pos = 256 - 1;
1040 210228 : move16();
1041 210228 : ptr_hf = h + 3; /*Q12*/
1042 3573876 : FOR( k = 0; k < 16; k++ )
1043 : {
1044 3363648 : p3 = &rrixiy[3][pos]; /*Q9*/
1045 3363648 : p2 = &rrixiy[2][pos - 1];
1046 3363648 : p1 = &rrixiy[1][pos - 1];
1047 3363648 : p0 = &rrixiy[0][pos - 1];
1048 :
1049 3363648 : cor = L_deposit_h( 0 );
1050 3363648 : ptr_h1 = h; /*Q12*/
1051 3363648 : ptr_h2 = ptr_hf; /*Q12*/
1052 28591008 : FOR( i = k; i < 16 - 1; i++ )
1053 : {
1054 25227360 : cor = L_mac( cor, *ptr_h1++, *ptr_h2++ ); /*Q25*/
1055 25227360 : *p3 = round_fx( cor ); /*Q9*/
1056 25227360 : cor = L_mac( cor, *ptr_h1++, *ptr_h2++ ); /*Q25*/
1057 25227360 : *p2 = round_fx( cor ); /*Q9*/
1058 25227360 : cor = L_mac( cor, *ptr_h1++, *ptr_h2++ ); /*Q25*/
1059 25227360 : *p1 = round_fx( cor ); /*Q9*/
1060 25227360 : cor = L_mac( cor, *ptr_h1++, *ptr_h2++ ); /*Q25*/
1061 25227360 : *p0 = round_fx( cor ); /*Q9*/
1062 25227360 : move16();
1063 25227360 : move16();
1064 25227360 : move16();
1065 25227360 : move16();
1066 :
1067 25227360 : p3 -= ( 16 + 1 );
1068 25227360 : p2 -= ( 16 + 1 );
1069 25227360 : p1 -= ( 16 + 1 );
1070 25227360 : p0 -= ( 16 + 1 );
1071 : }
1072 3363648 : cor = L_mac( cor, *ptr_h1++, *ptr_h2++ ); /*Q25*/
1073 3363648 : *p3 = round_fx( cor ); /*Q9*/
1074 3363648 : move16();
1075 :
1076 3363648 : pos--;
1077 3363648 : ptr_hf += 4;
1078 : }
1079 :
1080 : /*
1081 : * Modification of rrixiy[][] to take signs into account.
1082 : */
1083 :
1084 210228 : p0 = &rrixiy[0][0]; /*Q9*/
1085 :
1086 : /* speed-up: 11% */
1087 210228 : p1 = &rrixiy[1][0];
1088 210228 : p2 = &rrixiy[2][0];
1089 210228 : p3 = &rrixiy[3][0];
1090 :
1091 3573876 : FOR( i = 0; i < L_SUBFR; i += 4 )
1092 : {
1093 :
1094 3363648 : psign0 = &vec[1]; /*Q15*/
1095 3363648 : if ( sign[i + 0] > 0 )
1096 1666740 : psign0 = &sign[1];
1097 :
1098 3363648 : psign1 = &vec[2]; /*Q15*/
1099 3363648 : if ( sign[i + 1] > 0 )
1100 1668771 : psign1 = &sign[2];
1101 :
1102 3363648 : psign2 = &vec[3]; /*Q15*/
1103 3363648 : if ( sign[i + 2] > 0 )
1104 1668705 : psign2 = &sign[3];
1105 :
1106 3363648 : psign3 = &vec[0]; /*Q15*/
1107 3363648 : if ( sign[i + 3] > 0 )
1108 1665558 : psign3 = &sign[0];
1109 :
1110 3363648 : E_ACELP_apply_sign( p0, psign0 );
1111 3363648 : p0 += 16;
1112 :
1113 3363648 : E_ACELP_apply_sign( p1, psign1 );
1114 3363648 : p1 += 16;
1115 :
1116 3363648 : E_ACELP_apply_sign( p2, psign2 );
1117 3363648 : p2 += 16;
1118 :
1119 3363648 : E_ACELP_apply_sign( p3, psign3 );
1120 3363648 : p3 += 16;
1121 : }
1122 :
1123 210228 : return;
1124 : }
1125 :
1126 210228 : 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 210228 : 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 210228 : Word16 check = 0; /* debug code not instrumented */
1153 :
1154 210228 : alp = config->alp; /* Q13 */ /* initial value for energy of all fixed pulses */
1155 210228 : move16();
1156 210228 : nb_pulse = config->nb_pulse;
1157 210228 : move16();
1158 210228 : nb_pulse_m2 = sub( nb_pulse, 2 );
1159 :
1160 210228 : set16_fx( codvec, 0, nb_pulse );
1161 :
1162 : /*
1163 : * Find sign for each pulse position.
1164 : */
1165 :
1166 210228 : 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 210228 : E_ACELP_findcandidates( dn2, dn2_pos, pos_max );
1172 :
1173 : /*
1174 : * Compute h_inv[i].
1175 : */
1176 210228 : set16_fx( h_buf, 0, L_SUBFR );
1177 :
1178 210228 : set16_fx( h_buf + ( 2 * L_SUBFR ), 0, L_SUBFR );
1179 :
1180 210228 : h = h_buf + L_SUBFR;
1181 210228 : h_inv = h_buf + ( 3 * L_SUBFR );
1182 :
1183 : /*Check the energy if it is too high then scale to prevent an overflow*/
1184 210228 : scale = 0;
1185 210228 : move16();
1186 210228 : L_tmp = L_deposit_l( 0 );
1187 : BASOP_SATURATE_WARNING_OFF_EVS
1188 13664820 : FOR( i = 0; i < L_SUBFR; i++ )
1189 : {
1190 13454592 : L_tmp = L_mac_sat( L_tmp, H[i], H[i] ); /*Q25*/
1191 : }
1192 210228 : val = extract_h( L_tmp ); /*Q9*/
1193 : BASOP_SATURATE_WARNING_ON_EVS
1194 :
1195 210228 : if ( GT_16( val, 0x2000 ) )
1196 : {
1197 20990 : scale = -1;
1198 20990 : move16();
1199 : }
1200 210228 : if ( GT_16( val, 0x7000 ) )
1201 : {
1202 2385 : scale = -2;
1203 2385 : move16();
1204 : }
1205 210228 : test();
1206 210228 : if ( EQ_16( val, 32767 ) && element_mode > EVS_MONO )
1207 : {
1208 1819 : scale = -3;
1209 1819 : move16();
1210 : }
1211 210228 : Copy_Scale_sig( H, h, L_SUBFR, scale ); /*Q12+scale*/
1212 :
1213 210228 : E_ACELP_vec_neg_fx( h, h_inv, L_SUBFR );
1214 :
1215 : /*
1216 : * Compute correlation matrices needed for the codebook search.
1217 : */
1218 210228 : 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 210228 : psk = -1;
1231 210228 : move16();
1232 210228 : alpk = 1;
1233 210228 : move16();
1234 :
1235 : /*Number of iterations*/
1236 1086798 : FOR( k = 0; k < config->nbiter; k++ )
1237 : {
1238 876570 : E_ACELP_setup_pulse_search_pos( config, k, ipos );
1239 :
1240 : /* format of alp changes to Q(15-ALP2_E) */
1241 :
1242 876570 : pos = config->fixedpulses;
1243 876570 : move16();
1244 :
1245 876570 : IF( config->fixedpulses == 0 ) /* 1100, 11, 1110, 1111, 2211 */
1246 : {
1247 826912 : ps = 0;
1248 826912 : move16();
1249 826912 : alp = 0;
1250 826912 : move16();
1251 826912 : set16_fx( vec, 0, L_SUBFR );
1252 : }
1253 49658 : ELSE IF( EQ_16( config->fixedpulses, 2 ) ) /* 2222 and 3322 */
1254 : {
1255 : /* first stage: fix 2 pulses */
1256 49653 : ind[0] = pos_max[ipos[0]];
1257 49653 : move16();
1258 49653 : ind[1] = pos_max[ipos[1]];
1259 49653 : move16();
1260 49653 : 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 49653 : i = shr( ind[0], 2 );
1266 49653 : j = shr( ind[1], 2 );
1267 49653 : l = add( shl( i, 4 ), j );
1268 49653 : s = L_mult( rrixix[ipos[0]][i], _1_ ); /* Q9+Q12+1 */
1269 49653 : s = L_mac( s, rrixix[ipos[1]][j], _1_ ); /* Q9+Q12+1 */
1270 49653 : alp = mac_r( s, rrixiy[ipos[0]][l], _1_ ); /* Q9+Q12+1-16 */
1271 :
1272 49653 : p0 = h - ind[0]; /*Q12+scale*/
1273 49653 : if ( sign[ind[0]] < 0 )
1274 : {
1275 24410 : p0 = h_inv - ind[0]; /*Q12+scale*/
1276 : }
1277 :
1278 49653 : p1 = h - ind[1]; /*Q12+scale*/
1279 49653 : if ( sign[ind[1]] < 0 )
1280 : {
1281 24399 : p1 = h_inv - ind[1]; /*Q12+scale*/
1282 : }
1283 :
1284 3227445 : FOR( i = 0; i < L_SUBFR; i++ )
1285 : {
1286 3177792 : vec[i] = add( *p0++, *p1++ ); /*Q12+scale*/
1287 3177792 : 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 5 : 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 5 : 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 876570 : st = 0;
1346 876570 : move16();
1347 3367004 : FOR( j = pos; j < nb_pulse; j += 2 )
1348 : {
1349 2490434 : 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 2033966 : E_ACELP_h_vec_corr1_fx( h, vec, ipos[j], sign, rrixix, cor_x, dn2_pos, config->nbpos[st] );
1357 :
1358 2033966 : 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 2033966 : E_ACELP_2pulse_search( config->nbpos[st], ipos[j], ipos[j + 1], &ps, &alp,
1364 2033966 : &ind[j], &ind[j + 1], dn, dn2_pos, cor_x, cor_y, rrixiy );
1365 : }
1366 : ELSE /* single pulse search */
1367 : {
1368 456468 : E_ACELP_h_vec_corr2_fx( h, vec, ipos[j], sign, rrixix, cor_x );
1369 :
1370 456468 : E_ACELP_h_vec_corr2_fx( h, vec, ipos[j + 1], sign, rrixix, cor_y );
1371 :
1372 456468 : E_ACELP_1pulse_search( &ipos[j], &ps, &alp,
1373 456468 : &ind[j], dn, cor_x, cor_y );
1374 : }
1375 :
1376 2490434 : IF( GT_16( nb_pulse_m2, j ) )
1377 : {
1378 1613864 : p0 = h - ind[j]; /*Q12+scale*/
1379 1613864 : if ( sign[ind[j]] < 0 )
1380 : {
1381 812081 : p0 = h_inv - ind[j]; /*Q12+scale*/
1382 : }
1383 :
1384 1613864 : p1 = h - ind[j + 1]; /*Q12+scale*/
1385 1613864 : if ( sign[ind[j + 1]] < 0 )
1386 : {
1387 812161 : p1 = h_inv - ind[j + 1]; /*Q12+scale*/
1388 : }
1389 :
1390 :
1391 104901160 : FOR( i = 0; i < L_SUBFR; i++ )
1392 : {
1393 103287296 : tmp = add( *p0++, *p1++ );
1394 103287296 : vec[i] = add_sat( vec[i], tmp ); /* can saturate here. */
1395 103287296 : move16();
1396 : }
1397 : }
1398 2490434 : st = add( st, 1 );
1399 : }
1400 :
1401 : /* memorise the best codevector */
1402 :
1403 : /*ps = ps * ps; MULT(1);*/
1404 876570 : ps = mult( ps, ps );
1405 : /*s = (alpk * ps) - (psk * alp); MULT(2);ADD(1);*/
1406 876570 : s = L_msu( L_mult( alpk, ps ), psk, alp ); /*Q9+Q6+1=Q16*/
1407 :
1408 876570 : if ( psk < 0 )
1409 : {
1410 210228 : s = 1;
1411 210228 : move32();
1412 : }
1413 876570 : IF( s > 0 )
1414 : {
1415 415088 : psk = ps;
1416 415088 : move16();
1417 415088 : alpk = alp;
1418 415088 : move16();
1419 415088 : Copy( ind, codvec, nb_pulse ); /*Q0*/
1420 415088 : check = 1; /* debug code not instrumented */
1421 : }
1422 : }
1423 :
1424 210228 : 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 210228 : E_ACELP_build_code( nb_pulse, codvec, sign, code, ind );
1430 :
1431 210228 : set16_fx( y, 0, L_SUBFR );
1432 1355004 : FOR( k = 0; k < nb_pulse; ++k )
1433 : {
1434 1144776 : i = codvec[k]; /*Q0*/
1435 1144776 : move16();
1436 1144776 : p0 = h_inv - i; /*Q12+scale*/
1437 1144776 : if ( sign[i] > 0 )
1438 : {
1439 568698 : p0 -= 2 * L_SUBFR;
1440 : }
1441 74410440 : FOR( i = 0; i < L_SUBFR; i++ )
1442 : {
1443 73265664 : y[i] = add_sat( y[i], *p0++ ); /*Q12+scale*/
1444 73265664 : move16();
1445 : }
1446 : }
1447 210228 : 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 629999 : 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 629999 : move16();
1508 629999 : move16();
1509 629999 : move16();
1510 629999 : move16();
1511 629999 : move16();
1512 629999 : move16();
1513 629999 : move16();
1514 629999 : move16();
1515 629999 : move16();
1516 629999 : move16();
1517 629999 : move16();
1518 629999 : move16();
1519 629999 : move16();
1520 629999 : move16();
1521 629999 : move16();
1522 629999 : move16();
1523 629999 : memcpy( &config, &PulseConfTable[cdk_index], sizeof( PulseConfTable[cdk_index] ) );
1524 :
1525 629999 : if ( cmpl_flag > 0 )
1526 : {
1527 11359 : config.nbiter = cmpl_flag;
1528 11359 : move16();
1529 : }
1530 629999 : test();
1531 629999 : test();
1532 629999 : IF( NE_16( L_frame, last_L_frame ) && EQ_32( total_brate, ACELP_24k40 ) && LT_32( i_subfr, 5 * L_SUBFR ) )
1533 : {
1534 5 : config.nbiter = sub( config.nbiter, 1 );
1535 5 : config.nbiter = s_max( config.nbiter, 1 );
1536 : }
1537 :
1538 629999 : IF( acelpautoc )
1539 : {
1540 426043 : E_ACELP_4tsearchx_fx( dn, cn, R, code, &config, ind, element_mode );
1541 : }
1542 : ELSE
1543 : {
1544 203956 : E_ACELP_4tsearch_fx( dn, cn, H, code, &config, ind, y, element_mode );
1545 : }
1546 629999 : E_ACELP_indexing_fx( code, &config, NB_TRACK_FCB_4T, _index );
1547 :
1548 629999 : return;
1549 : }
1550 :
1551 :
1552 722143 : 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 722143 : assert( shift_bits <= 16 );
1563 :
1564 722143 : right_shift = sub( 16, shift_bits );
1565 :
1566 2334661 : FOR( i = sub( wordcnt, 1 ); i > 0; --i )
1567 : {
1568 1612518 : dst[i] = s_or( lshl( src[i], shift_bits ), lshr( src[i - 1], right_shift ) ); /*Q0+shift_bits*/
1569 1612518 : move16();
1570 : }
1571 722143 : dst[i] = s_or( lshl( src[i], shift_bits ), lsb_bits ); /*Q(shift_bits)*/
1572 722143 : move16();
1573 :
1574 722143 : return;
1575 : }
1576 :
1577 :
1578 : #define MAX_IDX_LEN 9
1579 :
1580 722143 : 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 722143 : assert( num_tracks == NB_TRACK_FCB_4T );
1593 :
1594 722143 : saved_bits = 0;
1595 722143 : move16();
1596 :
1597 : /*
1598 : * Code state of pulses of all tracks
1599 : * */
1600 722143 : wordcnt = shr( add( config->bits, 15 ), 4 ); /* ceil(bits/16) */
1601 :
1602 722143 : set16_fx( (Word16 *) idx, 0, wordcnt );
1603 :
1604 722143 : IF( EQ_16( config->bits, 43 ) ) /* EVS pulse indexing */
1605 : {
1606 39225 : saved_bits = E_ACELP_code43bit_fx( code, s, p, idx );
1607 : }
1608 : ELSE
1609 : {
1610 3414590 : FOR( track = 0; track < num_tracks; track++ )
1611 : {
1612 : /* Code track of length 2^4 where step between tracks is 4. */
1613 2731672 : E_ACELP_codearithp_fx( code + track, &n[track], &s[track], &p[track] );
1614 : }
1615 682918 : fcb_pulse_track_joint_fx( idx, wordcnt, s, p, num_tracks );
1616 : }
1617 :
1618 : /* check if we need to code track positions */
1619 722143 : track = 0;
1620 722143 : move16();
1621 722143 : shift_bits = 0;
1622 722143 : move16();
1623 722143 : SWITCH( config->codetrackpos )
1624 : {
1625 84805 : case TRACKPOS_FIXED_TWO:
1626 : /* Code position of consecutive tracks with single extra pulses */
1627 : /* Find track with one pulse less. */
1628 84805 : if ( NE_16( p[0], p[1] ) )
1629 : {
1630 : /* Either 0110 or 1001 */
1631 42400 : track = 1;
1632 42400 : move16();
1633 : }
1634 84805 : if ( GT_16( p[3], p[1] ) )
1635 : {
1636 42346 : track = add( track, 2 );
1637 42346 : move16();
1638 : }
1639 84805 : shift_bits = 2;
1640 84805 : move16();
1641 84805 : BREAK;
1642 :
1643 77971 : case TRACKPOS_FREE_THREE:
1644 : /* Code position of track with one pulse less than others */
1645 : /* Find track with one pulse less. */
1646 77971 : if ( LT_16( p[1], p[0] ) )
1647 : {
1648 19557 : track = 1;
1649 19557 : move16();
1650 : }
1651 77971 : if ( LT_16( p[2], p[0] ) )
1652 : {
1653 19293 : track = 2;
1654 19293 : move16();
1655 : }
1656 77971 : if ( LT_16( p[3], p[0] ) )
1657 : {
1658 20138 : track = 3;
1659 20138 : move16();
1660 : }
1661 77971 : shift_bits = 2;
1662 77971 : move16();
1663 77971 : BREAK;
1664 :
1665 29715 : case TRACKPOS_FREE_ONE:
1666 : /* Code position of track with one pulse less than others */
1667 : /* Find track with one pulse less. */
1668 29715 : if ( GT_16( p[1], p[0] ) )
1669 : {
1670 7585 : track = 1;
1671 7585 : move16();
1672 : }
1673 29715 : if ( GT_16( p[2], p[0] ) )
1674 : {
1675 7067 : track = 2;
1676 7067 : move16();
1677 : }
1678 29715 : if ( GT_16( p[3], p[0] ) )
1679 : {
1680 7697 : track = 3;
1681 7697 : move16();
1682 : }
1683 29715 : shift_bits = 2;
1684 29715 : move16();
1685 29715 : BREAK;
1686 :
1687 529652 : case TRACKPOS_FIXED_EVEN:
1688 : case TRACKPOS_FIXED_FIRST:
1689 529652 : 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 722143 : E_ACELP_indexing_shift( wordcnt, shift_bits, track, idx, (UWord16 *) prm );
1698 :
1699 722143 : return saved_bits;
1700 : }
1701 :
1702 :
1703 : /*--------------------------------------------------------------------------*
1704 : * E_ACELP_adaptive_codebook
1705 : *
1706 : * Find adaptive codebook.
1707 : *--------------------------------------------------------------------------*/
1708 :
1709 6120 : 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 6120 : Word16 gain1 = 0, gain2 = 0, fac_m, fac_n;
1734 6120 : move16();
1735 6120 : 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 6120 : Word16 use_prev_sf_pit_gain = 0;
1741 6120 : move16();
1742 :
1743 6120 : test();
1744 6120 : 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 6120 : L_ener = L_deposit_l( 0 );
1753 :
1754 : /* find pitch excitation */
1755 : /*for &exc[i_subfr]*/
1756 6120 : IF( EQ_16( T0_res, shr( T0_res_max, 1 ) ) )
1757 : {
1758 5896 : T0_frac = shl( T0_frac, 1 ); /*Q0*/
1759 : }
1760 :
1761 6120 : test();
1762 6120 : IF( EQ_16( T0_res_max, 6 ) && rf_mode == 0 )
1763 : {
1764 6120 : pitch_inter = pitch_inter6_2; /*Q14*/
1765 6120 : pit_L_interpol = PIT_L_INTERPOL6_2;
1766 6120 : move16();
1767 6120 : pit_up_samp = PIT_UP_SAMP6;
1768 6120 : 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 6120 : pred_lt4( &exc[i_subfr], &exc[i_subfr], T0, T0_frac, L_SUBFR + 1, pitch_inter, pit_L_interpol, pit_up_samp );
1780 :
1781 6120 : test();
1782 6120 : IF( EQ_16( mode, NORMAL_OPERATION ) || ( EQ_16( mode, FULL_BAND ) ) )
1783 : {
1784 2700 : E_UTIL_f_convolve( &exc[i_subfr], h1, y1, L_subfr );
1785 :
1786 2700 : IF( use_prev_sf_pit_gain == 0 )
1787 : {
1788 2700 : 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 2700 : test();
1792 2700 : IF( clip_gain && GT_16( gain1, 15565 /*0.95 Q14*/ ) )
1793 : {
1794 0 : gain1 = 15565 /*0.95f Q14*/;
1795 0 : move16();
1796 : }
1797 2700 : *pitch_gain = gain1; /*Q14*/
1798 2700 : move16();
1799 : }
1800 :
1801 : /* find energy of new target xn2[] */
1802 2700 : E_ACELP_codebook_target_update_fx( xn, xn2, y1, gain1, L_subfr );
1803 2700 : L_ener = Dot_product12_offs( xn2, xn2, L_subfr, &exp_ener, 0 ); /*Q31-exp_ener*/
1804 2700 : 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 6120 : test();
1814 6120 : IF( EQ_16( mode, NORMAL_OPERATION ) || EQ_16( mode, LOW_PASS ) )
1815 : {
1816 : /* find pitch excitation with lp filter */
1817 6120 : fac_m = 20972 /*0.64f Q15*/;
1818 6120 : move16();
1819 6120 : if ( EQ_16( L_frame, L_FRAME16k ) )
1820 : {
1821 6120 : fac_m = 19005 /*0.58f Q15*/;
1822 6120 : move16();
1823 : }
1824 : /* fac_n = 0.5*(1.0-fac_m); */
1825 6120 : fac_n = mult_r( sub( 0x7FFF, fac_m ), 0x4000 ); /*Q15*/
1826 397800 : FOR( i = 0; i < L_subfr; i++ )
1827 : {
1828 391680 : L_tmp = L_mult( fac_n, exc[i - 1 + i_subfr] ); /*Q_new+Q16*/
1829 391680 : L_tmp = L_mac( L_tmp, fac_m, exc[i + 0 + i_subfr] ); /*Q_new+Q16*/
1830 391680 : code[i] = mac_r( L_tmp, fac_n, exc[i + 1 + i_subfr] ); /*Q_new*/
1831 391680 : move16();
1832 : }
1833 6120 : E_UTIL_f_convolve( code, h1, y2, L_subfr );
1834 6120 : 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 6120 : test();
1838 6120 : 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 6120 : E_ACELP_codebook_target_update_fx( xn, xn2, y2, gain2, L_subfr );
1846 6120 : L_tmp = Dot_product12_offs( xn2, xn2, L_subfr, &exp_ener, 0 ); /*Q31 - exp_ener*/
1847 6120 : L_tmp = L_shr( L_tmp, sub( 31, exp_ener ) ); /*Q0*/
1848 :
1849 : /*-----------------------------------------------------------------*
1850 : * use the best prediction (minimise quadratic error). *
1851 : *-----------------------------------------------------------------*/
1852 :
1853 6120 : test();
1854 6120 : IF( EQ_16( mode, LOW_PASS ) || LT_32( L_tmp, L_ener ) )
1855 : {
1856 : /* use the lp filter for pitch excitation prediction */
1857 5292 : select = LOW_PASS;
1858 5292 : move16();
1859 5292 : Copy( code, &exc[i_subfr], L_subfr ); /*Q_new*/
1860 5292 : Copy( y2, y1, L_subfr ); /*Q_xn*/
1861 5292 : *pitch_gain = gain2; /*Q14*/
1862 5292 : move16();
1863 5292 : g_corr->y1y1 = g_corr2.y1y1;
1864 5292 : move16();
1865 5292 : g_corr->xy1 = g_corr2.xy1;
1866 5292 : move16();
1867 5292 : g_corr->y1y1_e = g_corr2.y1y1_e;
1868 5292 : move16();
1869 5292 : g_corr->xy1_e = g_corr2.xy1_e;
1870 5292 : move16();
1871 : }
1872 : ELSE
1873 : {
1874 : /* no filter used for pitch excitation prediction */
1875 828 : select = FULL_BAND;
1876 828 : move16();
1877 828 : *pitch_gain = gain1; /*Q14*/
1878 828 : move16();
1879 : }
1880 :
1881 6120 : IF( EQ_16( mode, NORMAL_OPERATION ) )
1882 : {
1883 2700 : **pt_indice = select;
1884 2700 : ( *pt_indice )++;
1885 2700 : move16();
1886 : }
1887 : }
1888 : ELSE
1889 : {
1890 : /* no filter used for pitch excitation prediction */
1891 0 : select = FULL_BAND;
1892 0 : move16();
1893 : }
1894 6120 : *lp_select = select;
1895 6120 : move16();
1896 :
1897 : BASOP_SATURATE_ERROR_OFF_EVS;
1898 6120 : }
1899 :
1900 :
1901 : /*--------------------------------------------------------------------------*
1902 : * E_ACELP_innovative_codebook_fx
1903 : *
1904 : * Find innovative codebook.
1905 : *--------------------------------------------------------------------------*/
1906 :
1907 6120 : 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 6120 : pitch = T0;
1936 6120 : move16();
1937 6120 : idx = shr( i_subfr, 6 );
1938 6120 : if ( GT_16( T0_frac, shr( T0_res, 1 ) ) )
1939 : {
1940 1458 : pitch = add( pitch, 1 );
1941 : }
1942 :
1943 : BASOP_SATURATE_ERROR_ON_EVS;
1944 :
1945 : /* Update target vector for ACELP codebook search */
1946 6120 : 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 6120 : Copy_Scale_sig( h1, h2, L_SUBFR, sub( -3, shift ) ); /*h2 1Q14+shift -> 4Q11, 1bit of headroom for Residu and xh_corr*/
1950 :
1951 6120 : cb_shape_fx( acelp_cfg->pre_emphasis, acelp_cfg->pitch_sharpening, acelp_cfg->phase_scrambling, acelp_cfg->formant_enh, acelp_cfg->formant_tilt,
1952 6120 : 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 6120 : IF( acelpautoc )
1956 : {
1957 : /* h2: 4Q11, Rw2: (Rw2_e)Q */
1958 6120 : /* Rw2_e = */ E_ACELP_hh_corr( h2, Rw2, L_SUBFR, 3 );
1959 :
1960 6120 : E_ACELP_conv( xn2, h2, cn2 );
1961 :
1962 : /* dn_e -> Rw2_e*Q_xn */
1963 6120 : /*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 6120 : assert( acelp_cfg->fixed_cdk_index[idx] < ACELP_FIXED_CDK_NB );
1976 :
1977 6120 : 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 6120 : *pt_indice += 8;
1979 :
1980 : /* Generate weighted code */
1981 6120 : E_ACELP_weighted_code( code, h2, 11, y2 );
1982 :
1983 : /*-------------------------------------------------------*
1984 : * - Add the fixed-gain pitch contribution to code[]. *
1985 : *-------------------------------------------------------*/
1986 :
1987 6120 : cb_shape_fx( acelp_cfg->pre_emphasis, acelp_cfg->pitch_sharpening, acelp_cfg->phase_scrambling, acelp_cfg->formant_enh, acelp_cfg->formant_tilt,
1988 6120 : 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 6120 : 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 2731672 : 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 2731672 : posno = 0;
2017 2731672 : move16();
2018 2731672 : t = 0;
2019 2731672 : move16();
2020 46438424 : FOR( k = 0; k < L_SUBFR; k += 4 )
2021 : {
2022 43706752 : if ( v[k] != 0 )
2023 : {
2024 6688930 : pos[posno++] = t;
2025 6688930 : move16();
2026 : }
2027 43706752 : t = add( t, 1 );
2028 43706752 : move16();
2029 43706752 : if ( sub( posno, NPMAXPT ) >= 0 )
2030 : {
2031 0 : BREAK;
2032 : }
2033 : }
2034 :
2035 : /* Iterate over the different pulse positions */
2036 2731672 : W_s = 0;
2037 2731672 : t = 0;
2038 2731672 : move16();
2039 2731672 : nb_pulse = 0;
2040 2731672 : move16();
2041 9420602 : FOR( k = 0; k < posno; ++k )
2042 : {
2043 6688930 : sign = shr( v[( pos[k] * 4 )], 9 ); /* sign with multiplicity Q0*/
2044 6688930 : m = abs_s( sign ); /* multiplicity */
2045 6688930 : nb_pulse = add( nb_pulse, m );
2046 : /* Code m-1 pulses */
2047 7120435 : FOR( i = 1; i < m; ++i )
2048 : {
2049 431505 : W_s = W_add( W_s, pulsestostates[pos[k]][t] );
2050 431505 : t = add( t, 1 );
2051 431505 : 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 6688930 : W_s = W_lshl( W_s, 1 );
2061 6688930 : if ( sign < 0 )
2062 : {
2063 3385149 : W_s = W_add( W_s, 1 );
2064 : }
2065 :
2066 : /* Code last pulse */
2067 6688930 : W_s = W_add( W_s, pulsestostates[pos[k]][t] );
2068 6688930 : t = add( t, 1 );
2069 : }
2070 :
2071 2731672 : *ps = (UWord32) W_s; /*Q0*/
2072 2731672 : move32();
2073 2731672 : *n = L_deposit_l( 0 );
2074 2731672 : if ( nb_pulse )
2075 : {
2076 2706157 : *n = pulsestostates[NB_POS_FCB_4T][nb_pulse - 1]; /*Q0*/
2077 2706157 : move32();
2078 : }
2079 :
2080 2731672 : *p = nb_pulse; /*Q0*/
2081 2731672 : move16();
2082 :
2083 2731672 : return;
2084 : }
2085 :
2086 :
2087 682918 : 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 682918 : Copy( hi_to_low_tmpl, hi_to_low, 10 ); /*Q0*/
2102 :
2103 682918 : indx_flag = 0;
2104 682918 : move16();
2105 682918 : indx_flag_1 = 0;
2106 682918 : move16();
2107 682918 : indx_flag_2 = 0;
2108 682918 : move16();
2109 :
2110 3414590 : FOR( track = 0; track < track_num; track++ )
2111 : {
2112 2731672 : indx_flag = add( indx_flag, shr( pulse_num[track], 2 ) );
2113 2731672 : indx_flag_1 = add( indx_flag_1, shr( pulse_num[track], 1 ) );
2114 2731672 : indx_flag_2 = add( indx_flag_2, shr( pulse_num[track], 3 ) );
2115 : }
2116 :
2117 682918 : IF( GE_16( indx_flag_2, 1 ) )
2118 : {
2119 57 : hi_to_low[7] = 9;
2120 57 : move16();
2121 57 : index_mask = 0xFFFFFF; /*Q0*/
2122 57 : move32();
2123 : }
2124 : ELSE
2125 : {
2126 682861 : if ( LT_16( indx_flag, track_num ) )
2127 : {
2128 576059 : hi_to_low[4] = 1;
2129 576059 : move16();
2130 : }
2131 682861 : index_mask = L_shr( 0xFFFF, sub( 9, hi_to_low[4] ) ); /*Q0*/
2132 : }
2133 :
2134 682918 : IF( GE_16( indx_flag_1, track_num ) )
2135 : {
2136 452050 : indx_tmp = L_deposit_l( 0 );
2137 452050 : index = L_shr( index_n[0], low_len[pulse_num[0]] ); /*Q0*/
2138 1808200 : FOR( track = 1; track < track_num; track++ )
2139 : {
2140 1356150 : pulse_num0 = pulse_num[track - 1]; /*Q0*/
2141 1356150 : move16();
2142 1356150 : pulse_num1 = pulse_num[track]; /*Q0*/
2143 1356150 : move16();
2144 1356150 : indx_tmp = L_lshr( index_n[track], low_len[pulse_num1] ); /*Q0*/
2145 : /* index = index * indx_fact[pulse_num1] + indx_tmp; */
2146 1356150 : index = UL_Mpy_32_32( index, UL_deposit_l( indx_fact[pulse_num1] ) ); /*Q0*/
2147 1356150 : index = UL_addNsD( index, indx_tmp );
2148 1356150 : index_n[track - 1] = L_add( L_and( index_n[track - 1], low_mask[pulse_num0] ),
2149 1356150 : L_and( L_lshl( index, low_len[pulse_num0] ), index_mask ) ); /*Q0*/
2150 :
2151 1356150 : index = L_lshr( index, hi_to_low[pulse_num0] );
2152 : }
2153 452050 : track_num1 = sub( track_num, 1 );
2154 452050 : move16();
2155 452050 : pulse_num1 = pulse_num[track_num1]; /*Q0*/
2156 452050 : move16();
2157 452050 : index_n[track_num1] = L_and( L_add( L_and( index_n[track_num1], low_mask[pulse_num1] ),
2158 452050 : L_lshl( index, low_len[pulse_num1] ) ),
2159 : index_mask ); /*Q0*/
2160 452050 : index = L_lshr( index, hi_to_low[pulse_num1] );
2161 452050 : IF( GE_16( indx_flag, track_num ) )
2162 : {
2163 106859 : IF( GE_16( indx_flag_2, 1 ) )
2164 : {
2165 57 : idxs[0] = extract_l( index_n[0] );
2166 57 : idxs[1] = extract_l( L_add( L_lshl( index_n[1], 8 ), L_lshr( index_n[0], 16 ) ) );
2167 57 : idxs[2] = extract_l( L_lshr( index_n[1], 8 ) );
2168 57 : idxs[3] = extract_l( index_n[2] );
2169 57 : idxs[4] = extract_l( L_add( L_lshl( index_n[3], 8 ), L_lshr( index_n[2], 16 ) ) );
2170 57 : idxs[5] = extract_l( L_lshr( index_n[3], 8 ) );
2171 57 : track = 6;
2172 57 : move16();
2173 : }
2174 : ELSE
2175 : {
2176 534010 : FOR( track = 0; track < track_num; track++ )
2177 : {
2178 427208 : idxs[track] = extract_l( index_n[track] );
2179 : }
2180 : }
2181 : }
2182 : ELSE
2183 : {
2184 345191 : idxs[0] = extract_l( L_add( L_lshl( index_n[0], 8 ), index_n[1] ) );
2185 345191 : idxs[1] = extract_l( L_add( L_lshl( index_n[2], 8 ), index_n[3] ) );
2186 345191 : track = 2;
2187 345191 : move16();
2188 : }
2189 : }
2190 : ELSE
2191 : {
2192 230868 : index = index_n[0]; /*Q0*/
2193 230868 : move32();
2194 923472 : FOR( track = 1; track < 4; track++ )
2195 : {
2196 692604 : pulse_num1 = pulse_num[track]; /*Q0*/
2197 692604 : index = L_add( L_lshl( index, index_len[pulse_num1] ), index_n[track] );
2198 : }
2199 230868 : track = 0;
2200 230868 : move16();
2201 : }
2202 1781972 : FOR( ; track < wordcnt; track++ )
2203 : {
2204 1099054 : idxs[track] = extract_l( index );
2205 1099054 : index = L_lshr( index, 16 );
2206 : }
2207 :
2208 682918 : return;
2209 : }
|
