Line data Source code
1 : /*====================================================================================
2 : EVS Codec 3GPP TS26.452 Aug 12, 2021. Version 16.3.0
3 : ====================================================================================*/
4 : #include <stdint.h>
5 : #include "options.h" /* Compilation switches */
6 : #include "prot_fx.h" /* Function prototypes */
7 : #include "cnst.h" /* Common constants */
8 : #include "rom_com.h" /* Static table prototypes */
9 :
10 : /*! r: Approximate integer division for positive input using lookup table */
11 803459 : UWord32 intLimCDivPos_fx(
12 : UWord32 NUM, /* Qx */
13 : Word16 DEN /* Q0 */
14 : )
15 : {
16 : UWord32 UL_ru, UL_rl;
17 803459 : Mpy_32_32_uu( UL_lshl( NUM, 1 ), intLimCDivInvDQ31[DEN], &UL_ru, &UL_rl ); /* Qx */
18 803459 : return UL_ru;
19 : }
20 :
21 : /*! r: Approximate integer division for signed input using lookup table */
22 560227 : static Word32 intLimCDivSigned_fx(
23 : Word32 NUM, /* Qx */
24 : Word16 DEN /* Q0 */
25 : )
26 : {
27 : Word32 L_tmp;
28 :
29 560227 : L_tmp = intLimCDivPos_fx( L_abs( NUM ), DEN ); /* Qx */
30 560227 : if ( NUM < 0 )
31 : {
32 74459 : L_tmp = L_negate( L_tmp ); /* one op */
33 : }
34 560227 : return L_tmp;
35 : }
36 :
37 : /*! r: Approximate integer division for negative input */
38 618992 : Word16 shrtCDivSignedApprox(
39 : const Word16 num, /* Q0 */
40 : const Word16 den /* Q0 */
41 : )
42 : {
43 : Word16 pool_part;
44 :
45 618992 : pool_part = extract_h( L_mult( negate( abs_s( num ) ), lim_neg_inv_tbl_fx[den] ) ); /* Q0 */
46 : /* neg_in always, positive out always, so that positive truncation(rounding) is used */
47 618992 : if ( num < 0 )
48 : {
49 12304 : pool_part = negate( pool_part ); /* make negative, one op */
50 : }
51 618992 : return pool_part;
52 : }
53 :
54 121482 : static void nearProjQ15_fx(
55 : const Word16 x, /* i : input coefficient Q15*/
56 : Word16 *result /* o : projection Q15*/
57 : )
58 : {
59 : const Word16 a[4] = { 14967, -25518, 3415, 32351 }; /* Q15 */
60 121482 : move16();
61 121482 : move16();
62 121482 : move16();
63 121482 : move16();
64 : Word32 b;
65 : UWord16 lsb;
66 :
67 121482 : b = L_deposit_l( a[0] );
68 121482 : b = L_shl( L_deposit_l( add( a[1], extract_h( L_mult0( extract_l( b ), x ) ) ) ), 1 ); /* Q15 */
69 121482 : Mpy_32_16_ss( b, x, &b, &lsb ); /* Q15 */
70 121482 : b = L_add( L_deposit_l( a[2] ), b ); /* Q15 */
71 121482 : Mpy_32_16_ss( b, x, &b, &lsb ); /* Q15 */
72 121482 : b = L_add( L_deposit_l( a[3] ), b ); /* Q15 */
73 121482 : *result = extract_l( b ); /* Q15 */
74 121482 : move16();
75 121482 : return;
76 : }
77 :
78 : /*-------------------------------------------------------------------*
79 : * obtainEnergyQuantizerDensity_fx()
80 : *
81 : *
82 : *-------------------------------------------------------------------*/
83 61454 : void obtainEnergyQuantizerDensity_fx(
84 : const Word16 L, /* i : left vector energy Q0*/
85 : const Word16 R, /* i : right vector energy Q0*/
86 : Word16 *Density /* o : quantizer density Q0*/
87 : )
88 : {
89 : Word16 Rnrg, den, n;
90 :
91 61454 : den = sub( shl( L, 1 ), 1 );
92 61454 : IF( LE_16( den, 67 ) )
93 : {
94 59812 : Rnrg = extract_l( intLimCDivPos_fx( L_deposit_l( R ), den ) ); /* Q0 */
95 : }
96 : ELSE
97 : {
98 1642 : n = norm_s( den );
99 1642 : move16();
100 1642 : Rnrg = shr( div_s( R, shl( den, n ) ), sub( 15, n ) ); /* n */
101 : }
102 61454 : Rnrg = add( Rnrg, 28 ); /* Q0 */
103 :
104 61454 : Rnrg = s_min( Rnrg, 56 ); /* Q0 */
105 61454 : Rnrg = s_min( Rnrg, sub( R, 96 ) ); /* Q0 */
106 :
107 61454 : Rnrg = s_max( Rnrg, 3 );
108 61454 : *Density = obtainEnergyQuantizerDensity_f[Rnrg]; /* Q0 */
109 61454 : move16();
110 61454 : return;
111 : }
112 :
113 :
114 : /*-------------------------------------------------------------------*
115 : * dsDirac2Dirac_fx()
116 : *
117 : *
118 : *-------------------------------------------------------------------*/
119 618992 : void dsDirac2Dirac_fx(
120 : const Word16 dsDiracIndex, /* i : input index Q0*/
121 : Word16 *diracs /* o : number of diracs Q0*/
122 : )
123 : {
124 618992 : *diracs = dsDiracsTab[dsDiracIndex]; /* Q0 */
125 618992 : move16();
126 618992 : return;
127 : }
128 :
129 680446 : void dsDiracPerQuanta_fx(
130 : const Word16 td, /* i : Length of vector segment Q0*/
131 : const Word16 t_quanta, /* i : Assigned number of quanta Q0*/
132 : const Word16 dsm, /* i : Conservative rounding flag Q0*/
133 : const unsigned char *const *frQuanta, /* i : Quanta lookup table */
134 : Word16 *DsIdx /* o : Lookup table index Q0*/
135 : )
136 : {
137 : const unsigned char *sv;
138 : Word16 nsv;
139 : Word16 t_quanta_o;
140 : Word16 dsIndex;
141 : Word16 i;
142 :
143 680446 : sv = frQuanta[td]; /* Q0 */
144 680446 : nsv = sv[0];
145 680446 : move16();
146 680446 : t_quanta_o = sub( t_quanta, QUANTAQ3OFFSET ); /* Q0 */
147 :
148 680446 : IF( GE_16( t_quanta_o, sv[nsv] ) )
149 : {
150 36748 : *DsIdx = nsv; /* Q0 */
151 36748 : move16();
152 36748 : return;
153 : }
154 :
155 643698 : IF( LE_16( t_quanta_o, sv[1] ) )
156 : {
157 84406 : *DsIdx = 1; /* Q0 */
158 84406 : move16();
159 84406 : return;
160 : }
161 :
162 :
163 559292 : dsIndex = shl( 1, frQuanta[0][td] ); /* Q0 */
164 559292 : IF( GT_16( t_quanta_o, sv[nsv >> 1] ) )
165 : {
166 185477 : dsIndex = sub( nsv, dsIndex ); /* Q0 */
167 : }
168 1993367 : FOR( i = frQuanta[0][td] - 1; i >= 0; i-- )
169 : {
170 1434075 : dsIndex = add( dsIndex, shl( sub( shl( lshr( sub( sv[dsIndex], t_quanta_o ), 15 ), 1 ), 1 ), i ) ); /* Q0 */
171 : }
172 :
173 559292 : dsIndex = add( dsIndex, lshr( sub( sv[dsIndex], t_quanta_o ), 15 ) ); /* Q0 */
174 559292 : dsIndex = sub( dsIndex, lshr( sub( 1, dsIndex ), 15 ) ); /* Q0 */
175 :
176 559292 : if ( EQ_16( dsm, PVQ_CONS ) )
177 : {
178 222839 : *DsIdx = dsIndex; /* Q0 */
179 222839 : move16();
180 222839 : return;
181 : }
182 336453 : *DsIdx = add( dsIndex, lshr( sub( add( sv[dsIndex + 1], sv[dsIndex] ), shl( t_quanta_o, 1 ) ), 15 ) ); /* Q0 */
183 336453 : move16();
184 336453 : return;
185 : }
186 :
187 63155 : void QuantaPerDsDirac_fx(
188 : const Word16 td, /* i : Length of vector segment Q0*/
189 : const Word16 dsDiracIndex, /* i : Quanta table index Q0*/
190 : const unsigned char *const *dimFrQuanta, /* i : Quanta lookup table */
191 : Word16 *Quanta /* i : Quanta Q0*/
192 : )
193 : {
194 63155 : *Quanta = dimFrQuanta[td][dsDiracIndex];
195 63155 : move16();
196 63155 : if ( dsDiracIndex == 0 )
197 : {
198 0 : *Quanta = -1; /* single op */
199 0 : move16();
200 : }
201 63155 : *Quanta = add( *Quanta, QUANTAQ3OFFSET ); /* Q0 */
202 63155 : move16();
203 63155 : return;
204 : }
205 :
206 618992 : void conservativeL1Norm_fx(
207 : const Word16 L, /* i : Length of vector segment Q0*/
208 : const Word16 Qvec, /* i : Assigned number of quanta Q0*/
209 : const Word16 Fcons, /* i : Conservative rounding flag Q0*/
210 : const Word16 Qavail, /* i : Input quanta remaining Q0*/
211 : const Word16 Qreserv, /* i : Input quanta in reservoir Q0*/
212 : const Word16 Dspec, /* i : assigned diracs from bitalloc Q0*/
213 : Word16 *Dvec, /* o : actual number of diracs Q0*/
214 : Word16 *Qspare, /* o : Output quanta remaining Q0*/
215 : Word16 *Qreservplus, /* o : Output quanta in reservoir Q0*/
216 : Word16 *Dspecplus /* o : Output number of diracs Q0*/
217 : )
218 : {
219 :
220 : Word16 Minit, Mprime;
221 : Word16 Qtestminus;
222 : const unsigned char *frQuantaL;
223 :
224 618992 : frQuantaL = hBitsN[L];
225 :
226 618992 : *Qreservplus = add( Qreserv, sub( Qvec, QUANTAQ3OFFSET ) ); /* Q0 */
227 618992 : move16();
228 618992 : dsDiracPerQuanta_fx( L, Qvec, Fcons, hBitsN, &Minit );
229 :
230 618992 : Mprime = Minit;
231 618992 : move16();
232 : DO
233 : {
234 681865 : Qtestminus = (Word16) frQuantaL[Mprime];
235 681865 : move16();
236 681865 : *Qspare = sub( Qavail, Qtestminus ); /* Q0 */
237 681865 : move16();
238 681865 : Mprime = sub( Mprime, 1 ); /* Q0 */
239 : }
240 681865 : WHILE( ( Mprime >= 0 ) && LT_16( *Qspare, QUANTAQ3OFFSET ) );
241 :
242 618992 : if ( Mprime < 0 )
243 : {
244 57095 : *Qspare = add( Qavail, QUANTAQ3OFFSET ); /* single op Q0*/
245 57095 : move16();
246 : }
247 618992 : dsDirac2Dirac_fx( add( Mprime, 1 ), Dvec );
248 :
249 618992 : *Dspecplus = add( Dspec, *Dvec ); /* Q0 */
250 618992 : move16();
251 618992 : *Qreservplus = sub( *Qreservplus, (Word16) frQuantaL[Minit] ); /* Q0 */
252 618992 : move16();
253 618992 : *Qspare = sub( *Qspare, QUANTAQ3OFFSET ); /* Q0 */
254 618992 : move16();
255 :
256 618992 : return;
257 : }
258 :
259 :
260 557538 : void bandBitsAdjustment_fx(
261 : const Word16 Brc, /* i : Current number of read quanta in range coder Q0*/
262 : const UWord32 INTrc, /* i : Range coder state Q0*/
263 : const Word16 Bavail, /* i : Available number of quanta Q3*/
264 : const Word16 Nbands, /* i : Number of bands Q0*/
265 : const Word16 D, /* i : Remaining number of bands to encode Q0*/
266 : const Word16 L, /* i : Size of current band Q0*/
267 : const Word16 Bband, /* i : Quanta allocation for current band Q0*/
268 : const Word16 Breserv, /* i : Quanta reservoir Q0*/
269 : Word16 *Bband_adj, /* o : Actual used number of quanta Q0*/
270 : Word16 *Brem, /* o : Quanta remaining Q0*/
271 : Word16 *Breservplus /* o : Quanta pool size Q0*/
272 : )
273 : {
274 : Word16 Btemp;
275 : Word16 Bff;
276 : Word32 L_tmp;
277 :
278 557538 : rangeCoderFinalizationFBits_fx( Brc, INTrc, &Bff );
279 :
280 557538 : IF( LT_16( D, Nbands ) )
281 : {
282 498773 : L_tmp = L_deposit_l( sub( Breserv, Bff ) ); /* Q0 */
283 498773 : Btemp = extract_l( intLimCDivSigned_fx( L_tmp, s_min( D, 3 ) ) ); /* result always fits in Word16 Q0*/
284 498773 : *Breservplus = add( Bband, Breserv ); /* Q0 */
285 498773 : move16();
286 : }
287 : ELSE
288 : {
289 58765 : Btemp = 0;
290 58765 : move16();
291 58765 : *Breservplus = add( Bband, Bff ); /* Q0 */
292 58765 : move16();
293 : }
294 557538 : *Bband_adj = s_min( extract_l( L_mult( L, 40 ) ), Bband ); /* Q3 */
295 557538 : move16();
296 557538 : *Brem = sub( Bavail, Bff ); /* Q0 */
297 557538 : move16();
298 557538 : *Bband_adj = s_min( *Brem, add( *Bband_adj, Btemp ) ); /* Q0 */
299 557538 : move16();
300 557538 : *Bband_adj = s_max( 0, *Bband_adj ); /* Q0 */
301 557538 : move16();
302 557538 : return;
303 : }
304 :
305 :
306 60741 : static Word16 Ratio_base2Q11_fx( /* o : Q11 */
307 : const Word16 opp, /* i : opposite Q15 */
308 : const Word16 near /* i : near Q15 */
309 : )
310 : {
311 : Word16 mc, nc, ms, ns, d, z;
312 : Word16 result;
313 : Word32 acc;
314 :
315 60741 : ns = norm_s( opp ); /* exponent */
316 60741 : nc = norm_s( near ); /* exponent */
317 :
318 60741 : ms = shl( opp, ns ); /* mantissa */
319 60741 : mc = shl( near, nc ); /* mantissa */
320 :
321 60741 : acc = L_mac( 538500224L, mc, -2776 ); /* a0*mc + a1, acc(Q27), a0(Q11), a1(Q27) */
322 60741 : z = mac_r( acc, ms, -2776 ); /* z in Q11, a0 in Q11 */
323 60741 : d = sub( ms, mc ); /* d in Q15 */
324 60741 : z = mult_r( z, d ); /* z in Q11 */
325 :
326 60741 : result = add( z, shl( sub( nc, ns ), 11 ) ); /* Q11 */
327 :
328 60741 : return result;
329 : }
330 :
331 60741 : static void Ratio_rQ3_fx(
332 : Word16 opp, /* i : opposite Q15*/
333 : Word16 near, /* i : near Q15*/
334 : Word16 *result /* o : ratio Q3*/
335 : )
336 : {
337 : Word16 tmp;
338 :
339 60741 : tmp = add( 1 << 7, Ratio_base2Q11_fx( opp, near ) ); /* Q11 */
340 60741 : *result = shr( tmp, 8 ); /* Q3 */
341 60741 : move16();
342 60741 : return;
343 : }
344 :
345 :
346 61454 : void densityAngle2RmsProjDec_fx(
347 : const Word16 D, /* i : density Q0*/
348 : const Word16 indexphi, /* i : decoded index from AR dec Q0*/
349 : Word16 *oppQ15, /* o : opposite Q15*/
350 : Word16 *nearQ15, /* o : near Q15*/
351 : Word16 *oppRatioQ3 /* o : ratio Q3*/
352 : )
353 : {
354 : Word16 phiQ14q;
355 : Word16 oppTail, nearTail;
356 :
357 61454 : phiQ14q = (Word16) intLimCDivPos_fx( (UWord32) L_shl( L_deposit_l( indexphi ), 13 ), shr( D, 1 ) ); /* Q14 */
358 61454 : move16();
359 61454 : if ( indexphi < 0 )
360 : {
361 333 : phiQ14q = 1 << 13; /* one op */
362 333 : move16();
363 : }
364 :
365 61454 : oppTail = shr( sub( 16320, phiQ14q ), 15 );
366 61454 : nearTail = shr( sub( phiQ14q, 64 ), 15 );
367 :
368 61454 : IF( s_or( oppTail, nearTail ) < 0 )
369 : {
370 713 : *oppQ15 = s_and( oppTail, ( 1 << 15 ) - 1 ); /* Q15 */
371 713 : move16();
372 713 : *nearQ15 = s_and( nearTail, ( 1 << 15 ) - 1 ); /* Q15 */
373 713 : move16();
374 713 : *oppRatioQ3 = shl( add( 1, shl( nearTail, 1 ) ), 14 ); /* Q3 */
375 713 : move16();
376 : }
377 : ELSE
378 : {
379 60741 : nearProjQ15_fx( shl( sub( 1 << 14, phiQ14q ), 1 ), oppQ15 );
380 60741 : nearProjQ15_fx( shl( phiQ14q, 1 ), nearQ15 );
381 60741 : Ratio_rQ3_fx( *oppQ15, *nearQ15, oppRatioQ3 );
382 : }
383 :
384 61454 : return;
385 : }
386 :
387 31613 : void densityAngle2RmsProjEnc_fx(
388 : const Word16 D, /* i : density Q0*/
389 : const Word16 phiQ14uq, /* i : angle Q14*/
390 : Word16 *indexphi, /* o : index Q0*/
391 : Word16 *oppQ15, /* o : opposite Q15*/
392 : Word16 *nearQ15, /* o : near Q15*/
393 : Word16 *oppRatioQ3 /* o : ratio Q3*/
394 : )
395 : {
396 31613 : *indexphi = mult_r( shl( D, 1 ), phiQ14uq ); /* Q0 */
397 31613 : move16();
398 31613 : if ( s_and( D, 1 ) > 0 )
399 : {
400 180 : *indexphi = -1; /* one op */
401 180 : move16();
402 : }
403 31613 : densityAngle2RmsProjDec_fx( D, *indexphi, oppQ15, nearQ15, oppRatioQ3 );
404 :
405 31613 : return;
406 : }
407 :
408 61454 : void NearOppSplitAdjustment_fx(
409 : const Word16 qband, /* i : quanta for current band Q0*/
410 : const Word16 qzero, /* i : range coder finalization quanta Q0*/
411 : const Word16 Qac, /* i : range coder current quanta Q0*/
412 : const UWord32 INTac, /* i : range coder state Q0*/
413 : const Word16 qglobal, /* i : quanta input Q0*/
414 : const Word16 FlagCons, /* i : conservative rounding flag Q0*/
415 : const Word16 Np, /* i : number of parts Q0*/
416 : const Word16 Nhead, /* i : first part Q0*/
417 : const Word16 Ntail, /* i : remaining parts Q0*/
418 : const Word16 Nnear, /* i : length of near component Q0*/
419 : const Word16 Nopp, /* i : length of opposite component Q0*/
420 : Word16 oppRQ3, /* i : ratio Q0*/
421 : Word16 *qnear, /* o : quantized near Q0*/
422 : Word16 *qopp, /* o : quantized opposite Q0*/
423 : Word16 *qglobalupd /* o : quanta remaining Q0*/
424 : )
425 : {
426 :
427 : Word16 qac, qboth, qskew, qavg, qmin, Midx;
428 : Word32 L_QIb, L_qnum;
429 : Word16 QIb, QIa;
430 :
431 61454 : rangeCoderFinalizationFBits_fx( Qac, INTac, &qac );
432 61454 : qboth = sub( qband, sub( qac, qzero ) ); /* Q0 */
433 : /* skew calc code */
434 61454 : qskew = 0;
435 61454 : move16();
436 61454 : IF( GT_16( Nhead, 1 ) )
437 : {
438 61454 : qavg = extract_h( L_shl( intLimCDivSigned_fx( L_deposit_l( qboth ), Np ), 16 ) ); /* qboth may be negative Q0*/
439 61454 : dsDiracPerQuanta_fx( Ntail, qavg, FlagCons, hBitsN, &Midx );
440 61454 : QuantaPerDsDirac_fx( Nhead, Midx, hBitsN, &qmin );
441 61454 : qskew = sub( qavg, qmin ); /* Q0 */
442 61454 : qskew = s_max( 0, qskew ); /* Q0 */
443 : } /* end of skew calc code*/
444 :
445 61454 : QIa = add( extract_l( intLimCDivPos_fx( (UWord32) L_deposit_l( Nopp ), Nnear ) ), 1 ); /* always positive Word16 out Q0*/
446 61454 : L_qnum = L_sub( L_deposit_l( sub( sub( add( qband, qzero ), qac ), qskew ) ), L_mult0( Nopp, oppRQ3 ) ); /* Q0 */
447 :
448 61454 : L_QIb = L_deposit_l( 0 );
449 61454 : IF( L_qnum > 0 )
450 : {
451 60489 : L_QIb = (Word32) intLimCDivPos_fx( L_qnum, QIa ); /* Q0 */
452 : }
453 61454 : *qnear = qboth; /* Q3 */
454 61454 : move16();
455 61454 : QIb = extract_h( L_shl_sat( L_QIb, 16 ) ); /* may saturate Q0*/
456 61454 : if ( LE_16( QIb, qboth ) )
457 : {
458 60604 : *qnear = QIb; /* Q0 */
459 60604 : move16();
460 : }
461 61454 : *qopp = sub( qboth, *qnear ); /* Q0 */
462 61454 : move16();
463 61454 : *qglobalupd = sub( qglobal, sub( qac, qzero ) ); /* Q0 */
464 61454 : move16();
465 :
466 61454 : return;
467 : }
468 :
469 :
470 : /*--------------------------------------------------------------------------*
471 : * apply_gain()
472 : *
473 : * Apply gain
474 : *--------------------------------------------------------------------------*/
475 :
476 57312 : void apply_gain_fx(
477 : const Word16 *ord, /* i : Indices for energy order Q0 */
478 : const Word16 *band_start, /* i : Sub band start indices Q0 */
479 : const Word16 *band_end, /* i : Sub band end indices Q0 */
480 : const Word16 num_sfm, /* i : Number of bands Q0 */
481 : const Word16 *gains, /* i : Band gain vector Qx */
482 : Word16 *xq /* i/o: Float synthesis / Gain adjusted synth Q15/Qx */
483 : )
484 : {
485 : Word16 band, i;
486 : Word16 g; /* Q12 */
487 :
488 890321 : FOR( band = 0; band < num_sfm; band++ )
489 : {
490 833009 : g = gains[ord[band]];
491 833009 : move16();
492 15351593 : FOR( i = band_start[band]; i < band_end[band]; i++ )
493 : {
494 : /*xq[i] *= g; */
495 14518584 : xq[i] = mult_r( g, xq[i] );
496 14518584 : move16(); /*12+15+1-16=12 */
497 : }
498 : }
499 :
500 57312 : return;
501 : }
502 :
503 : /*--------------------------------------------------------------------------*
504 : * fine_gain_quant_fx()
505 : *
506 : * Fine gain quantization
507 : *--------------------------------------------------------------------------*/
508 29384 : void fine_gain_quant_fx(
509 : BSTR_ENC_HANDLE hBstr, /* i/o: encoder bitstream handle */
510 : const Word16 *ord, /* i : Indices for energy order Q0 */
511 : const Word16 num_sfm, /* i : Number of bands Q0 */
512 : const Word16 *gain_bits, /* i : Gain adjustment bits per sub band Q0 */
513 : Word16 *fg_pred, /* i/o: Predicted gains / Corrected gains Q12 */
514 : const Word16 *gopt /* i : Optimal gains Q12 */
515 : )
516 : {
517 : Word16 band;
518 : Word16 gbits;
519 : Word16 idx;
520 : Word16 gain_db, gain_dbq;
521 : Word16 err;
522 :
523 : Word16 tmp1, tmp2, exp1, exp2;
524 : Word32 L_tmp;
525 : UWord16 lsb;
526 :
527 464055 : FOR( band = 0; band < num_sfm; band++ )
528 : {
529 434671 : gbits = gain_bits[ord[band]]; /* Q0 */
530 434671 : move16();
531 434671 : test();
532 434671 : IF( fg_pred[band] != 0 && gbits > 0 )
533 : {
534 34472 : exp1 = norm_s( gopt[band] );
535 34472 : exp1 = sub( exp1, 1 );
536 34472 : tmp1 = shl( gopt[band], exp1 );
537 34472 : exp2 = norm_s( fg_pred[band] );
538 34472 : tmp2 = shl( fg_pred[band], exp2 ); /* Q12 + exp2 */
539 34472 : exp1 = add( 15, sub( exp1, exp2 ) );
540 34472 : err = div_s( tmp1, tmp2 ); /* Q15 */
541 34472 : tmp1 = norm_s( err );
542 34472 : exp2 = Log2_norm_lc( L_deposit_h( shl( err, tmp1 ) ) );
543 34472 : tmp1 = sub( 14, tmp1 );
544 34472 : tmp1 = sub( tmp1, exp1 );
545 34472 : L_tmp = L_Comp( tmp1, exp2 );
546 34472 : Mpy_32_16_ss( L_tmp, 24660, &L_tmp, &lsb ); /* 24660 = 20*log10(2) in Q12 */ /*16+12-15=13 */
547 34472 : gain_db = round_fx_sat( L_shl_sat( L_tmp, 17 ) );
548 :
549 34472 : idx = squant_fx( gain_db, &gain_dbq, finegain_fx[gbits - 1], gain_cb_size[gbits - 1] );
550 34472 : push_indice( hBstr, IND_PVQ_FINE_GAIN, idx, gbits );
551 :
552 34472 : L_tmp = L_mult0( gain_dbq, 21771 ); /* 21771=0.05*log2(10) */ /* 14+17=31 */
553 34472 : L_tmp = L_shr( L_tmp, 15 ); /* Q16 */
554 34472 : tmp1 = L_Extract_lc( L_tmp, &exp1 );
555 34472 : tmp1 = abs_s( tmp1 );
556 34472 : tmp1 = extract_l( Pow2( 14, tmp1 ) );
557 34472 : exp1 = sub( 14, exp1 );
558 :
559 34472 : L_tmp = L_mult0( fg_pred[band], tmp1 ); /*12+exp1 */
560 34472 : fg_pred[band] = round_fx_sat( L_shl_sat( L_tmp, sub( 16, exp1 ) ) ); /*12+exp1+16-exp1-16=12 */
561 34472 : move16();
562 : }
563 : }
564 :
565 29384 : return;
566 : }
567 :
568 : /*-------------------------------------------------------------------*
569 : * srt_vec_ind()
570 : *
571 : * sort vector and save sorting indeces
572 : *-------------------------------------------------------------------*/
573 :
574 600552 : void srt_vec_ind16_fx(
575 : const Word16 *linear, /* linear input Q3*/
576 : Word16 *srt, /* sorted output Q3*/
577 : Word16 *I, /* index for sorted output Q0*/
578 : Word16 length /* Q0 */
579 : )
580 : {
581 : Word16 pos, npos;
582 : Word16 idxMem;
583 : Word16 valMem;
584 :
585 : /*initilize */
586 1527737 : FOR( pos = 0; pos < length; pos++ )
587 : {
588 927185 : I[pos] = pos; /* Q0 */
589 927185 : move16();
590 : }
591 :
592 600552 : Copy( linear, srt, length ); /* Q3 */
593 :
594 : /* now iterate */
595 927185 : FOR( pos = 0; pos < ( length - 1 ); pos++ )
596 : {
597 1435475 : FOR( npos = ( pos + 1 ); npos < length; npos++ )
598 : {
599 1108842 : IF( LT_16( srt[npos], srt[pos] ) )
600 : {
601 516994 : idxMem = I[pos]; /* Q0 */
602 516994 : move16();
603 516994 : I[pos] = I[npos]; /* Q0 */
604 516994 : move16();
605 516994 : I[npos] = idxMem; /* Q0 */
606 516994 : move16();
607 :
608 516994 : valMem = srt[pos]; /* Q0 */
609 516994 : move16();
610 516994 : srt[pos] = srt[npos]; /* Q0 */
611 516994 : move16();
612 516994 : srt[npos] = valMem; /* Q0 */
613 516994 : move16();
614 : }
615 : }
616 : }
617 :
618 600552 : return;
619 : }
620 :
621 : /*-----------------------------------------------------------------------------
622 : * atan2_fx():
623 : *
624 : * Approximates arctan piecewise with various 4th to 5th order least square fit
625 : * polynomials for input in 5 segments:
626 : * - 0.0 to 1.0
627 : * - 1.0 to 2.0
628 : * - 2.0 to 4.0
629 : * - 4.0 to 8.0
630 : * - 8.0 to infinity
631 : *---------------------------------------------------------------------------*/
632 6908777 : Word16 atan2_fx( /* o: Angle between 0 and PI/2 radian (Q14) */
633 : const Word32 y, /* i : near side (Argument must be positive) (Q15) */
634 : const Word32 x /* i : opposite side (Q15) */
635 : )
636 : {
637 : Word32 acc, arg;
638 : Word16 man, expo, reciprocal;
639 : Word16 angle, w, z;
640 :
641 6908777 : IF( x == 0 )
642 : {
643 54791 : return 25736; /* PI/2 in Q14 */
644 : }
645 6853986 : man = ratio( y, x, &expo ); /* man in Q14 */
646 6853986 : expo = sub( expo, ( 15 - 14 ) ); /* Now, man is considered in Q15 */
647 6853986 : arg = L_shr( (Word32) man, expo );
648 :
649 6853986 : IF( L_shr( arg, 3 + 15 ) != 0 )
650 : /*===============================*
651 : * 8.0 <= x < infinity *
652 : *===============================*/
653 : {
654 : /* atan(x) = PI/2 - 1/x + 1/(3x^3) - 1/(5x^5) + ...
655 : * ~ PI/2 - 1/x, for x >= 8.
656 : */
657 403346 : expo = norm_l( arg );
658 403346 : man = extract_h( L_shl( arg, expo ) );
659 403346 : reciprocal = div_s( 0x3fff, man );
660 403346 : expo = sub( 15 + 1, expo );
661 403346 : reciprocal = shr( reciprocal, expo ); /* Q14 */
662 403346 : angle = sub( 25736, reciprocal ); /* Q14 (PI/2 - 1/x) */
663 :
664 : /* For 8.0 <= x < 10.0, 1/(5x^5) is not completely negligible.
665 : * For more accurate result, add very small correction term.
666 : */
667 403346 : if ( LT_32( L_shr( arg, 15 ), 10L ) )
668 : {
669 84737 : angle = add( angle, 8 ); /* Add tiny correction term. */
670 : }
671 : }
672 6450640 : ELSE IF( L_shr( arg, 2 + 15 ) != 0 )
673 : /*==========================*
674 : * 4.0 <= x < 8.0 *
675 : *==========================*/
676 : {
677 : /* interval: [3.999, 8.001]
678 : * atan(x) ~ (((a0*x + a1)*x + a2)*x + a3)*x + a4
679 : * = (((a0*8*y + a1)*8*y + a2)*8*y + a3)*8*y + a4 Substitute 8*y -> x
680 : * = (((a0*8^3*y + a1*8^2)*y + a2*8)*y + a3)*8*y + a4
681 : * = ((( c0*y + c1)*y + c2)*y + c3)*8*y + c4,
682 : * where y = x/8
683 : * and a0 = -1.28820869667651e-04, a1 = 3.88263533346295e-03,
684 : * a2 = -4.64216306484597e-02, a3 = 2.75986060068931e-01,
685 : * a4 = 7.49208077809799e-01.
686 : */
687 264589 : w = extract_l( L_shr( arg, 3 ) ); /* Q15 y = x/8 */
688 264589 : acc = L_add( 533625337L, 0 ); /* Q31 c1 = a1*8^2 */
689 264589 : z = mac_r( acc, w, -2161 ); /* Q15 c0 = a0*8^3 */
690 264589 : acc = L_add( -797517542L, 0 ); /* Q31 c2 = a2*8 */
691 264589 : z = mac_r( acc, w, z ); /* Q15 */
692 264589 : acc = L_add( 592675551L, 0 ); /* Q31 c3 = a3 */
693 264589 : z = mac_r( acc, w, z ); /* z (in:Q15, out:Q12) */
694 264589 : acc = L_add( 201114012L, 0 ); /* Q28 c4 = a4 */
695 264589 : acc = L_mac( acc, w, z ); /* Q28 */
696 264589 : angle = extract_l( L_shr( acc, ( 28 - 14 ) ) ); /* Q14 result of atan(x), where 4 <= x < 8 */
697 : }
698 6186051 : ELSE IF( L_shr( arg, 1 + 15 ) != 0 )
699 : /*==========================*
700 : * 2.0 <= x < 4.0 *
701 : *==========================*/
702 : {
703 : /* interval: [1.999, 4.001]
704 : * atan(x) ~ (((a0*x + a1)*x + a2)*x + a3)*x + a4
705 : * = (((a0*4*y + a1)*4*y + a2)*4*y + a3)*4*y + a4 Substitute 4*y -> x
706 : * = (((a0*16*y + a1*4)*y + a2)*4*y + a3)*4*y + a4
707 : * = (((a0*32*y + a1*8)*y + a2*2)*2*y + a3)*4*y + a4
708 : * = ((( c0*y + c1)*y + c2)*2*y + c3)*4*y + c4,
709 : * where y = x/4
710 : * and a0 = -0.00262378195660943, a1 = 0.04089687039888652,
711 : * a2 = -0.25631148958325911, a3 = 0.81685854627399479,
712 : * a4 = 0.21358070563097167
713 : * */
714 422173 : w = extract_l( L_shr( arg, 2 ) ); /* Q15 y = x/4 */
715 422173 : acc = L_add( 702602883L, 0 ); /* Q31 c1 = a1*8 */
716 422173 : z = mac_r( acc, w, -2751 ); /* Q15 c0 = a0*32 */
717 422173 : acc = L_add( -1100849465L, 0 ); /* Q31 c2 = a2*2 */
718 422173 : z = mac_r( acc, w, z ); /* z (in:Q15, out:Q14) */
719 422173 : acc = L_add( 877095185L, 0 ); /* Q30 c3 = a3 */
720 422173 : z = mac_r( acc, w, z ); /* z (in:Q14, out:Q12) */
721 422173 : acc = L_add( 57332634L, 0 ); /* Q28 c4 = a4 */
722 422173 : acc = L_mac( acc, w, z ); /* Q28 */
723 422173 : angle = extract_l( L_shr( acc, ( 28 - 14 ) ) ); /* Q14 result of atan(x) where 2 <= x < 4 */
724 : }
725 5763878 : ELSE IF( L_shr( arg, 15 ) != 0 )
726 : /*==========================*
727 : * 1.0 <= x < 2.0 *
728 : *==========================*/
729 : {
730 : /* interval: [0.999, 2.001]
731 : * atan(x) ~ (((a0*x + 1)*x + a2)*x + a3)*x + a4
732 : * = (((a0*2*y + a1)*2*y + a2)*2*y + a3)*2*y + a4 Substitute 2*y -> x
733 : * = (((a0*4*y + a1*2)*y + a2)*2*y + a3)*2*y + a4
734 : * = (((a0*4*y + a1*2)*y + a2)*y + a3/2)*4*y + a4
735 : * = ((( c0*y + c1)*y + c2)*y + c3)*4*y + c4,
736 : * where y = x/2
737 : * and a0 = -0.0160706457245251, a1 = 0.1527106504065224,
738 : * a2 = -0.6123208404800871, a3 = 1.3307896976322915,
739 : * a4 = -0.0697089375247448
740 : */
741 1078224 : w = extract_l( L_shr( arg, 1 ) ); /* Q15 y= x/2 */
742 1078224 : acc = L_add( 655887249L, 0 ); /* Q31 c1 = a1*2 */
743 1078224 : z = mac_r( acc, w, -2106 ); /* Q15 c0 = a0*4 */
744 1078224 : acc = L_add( -1314948992L, 0 ); /* Q31 c2 = a2 */
745 1078224 : z = mac_r( acc, w, z );
746 1078224 : acc = L_add( 1428924557L, 0 ); /* Q31 c3 = a3/2 */
747 1078224 : z = mac_r( acc, w, z ); /* z (in:Q15, out:Q13) */
748 1078224 : acc = L_add( -37424701L, 0 ); /* Q29 c4 = a4 */
749 1078224 : acc = L_mac( acc, w, z ); /* Q29 */
750 1078224 : angle = extract_l( L_shr( acc, ( 29 - 14 ) ) ); /* Q14 result of atan(x) where 1 <= x < 2 */
751 : }
752 : ELSE
753 : /*==========================*
754 : * 0.0 <= x < 1.0 *
755 : *==========================*/
756 : {
757 : /* interval: [-0.001, 1.001]
758 : * atan(x) ~ ((((a0*x + a1)*x + a2)*x + a3)*x + a4)*x + a5
759 : * = ((((a0*2*x + a1*2)*x/2 + a2)*x + a3)*x + a4)*x + a5
760 : * = (((( c0*x + c1)*x/2 + c2)*x + c3)*x + c4)*x + c5
761 : * where
762 : * a0 = -5.41182677118661e-02, a1 = 2.76690449232515e-01,
763 : * a2 = -4.63358392562492e-01, a3 = 2.87188466598566e-02,
764 : * a4 = 9.97438122814383e-01, a5 = 5.36158556179092e-05.
765 : */
766 4685654 : w = extract_l( arg ); /* Q15 */
767 4685654 : acc = L_add( 1188376431L, 0 ); /* Q31 c1 = a1*2 */
768 4685654 : z = mac_r( acc, w, -3547 ); /* Q15 c0 = a0*2 */
769 4685654 : acc = L_add( -995054571L, 0 ); /* Q31 c2 = a2 */
770 4685654 : z = extract_h( L_mac0( acc, w, z ) ); /* Q15 non-fractional mode multiply */
771 4685654 : acc = L_add( 61673254L, 0 ); /* Q31 c3 = a3 */
772 4685654 : z = mac_r( acc, w, z );
773 4685654 : acc = L_add( 2141982059L, 0 ); /* Q31 c4 = a4 */
774 4685654 : z = mac_r( acc, w, z );
775 4685654 : acc = L_add( 115139L, 0 ); /* Q31 c5 = a5 */
776 4685654 : acc = L_mac( acc, w, z ); /* Q31 */
777 4685654 : angle = extract_l( L_shr( acc, 31 - 14 ) ); /* Q14 result of atan(x), where 0 <= x < 1 */
778 : }
779 :
780 6853986 : return angle; /* Q14 between 0 and PI/2 radian. */
781 : }
|
