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 810127 : UWord32 intLimCDivPos_fx(
12 : UWord32 NUM, /* Qx */
13 : Word16 DEN /* Q0 */
14 : )
15 : {
16 : UWord32 UL_ru, UL_rl;
17 810127 : Mpy_32_32_uu( UL_lshl( NUM, 1 ), intLimCDivInvDQ31[DEN], &UL_ru, &UL_rl ); /* Qx */
18 810127 : return UL_ru;
19 : }
20 :
21 : /*! r: Approximate integer division for signed input using lookup table */
22 565328 : static Word32 intLimCDivSigned_fx(
23 : Word32 NUM, /* Qx */
24 : Word16 DEN /* Q0 */
25 : )
26 : {
27 : Word32 L_tmp;
28 :
29 565328 : L_tmp = intLimCDivPos_fx( L_abs( NUM ), DEN ); /* Qx */
30 565328 : if ( NUM < 0 )
31 : {
32 75756 : L_tmp = L_negate( L_tmp ); /* one op */
33 : }
34 565328 : return L_tmp;
35 : }
36 :
37 : /*! r: Approximate integer division for negative input */
38 624530 : Word16 shrtCDivSignedApprox(
39 : const Word16 num, /* Q0 */
40 : const Word16 den /* Q0 */
41 : )
42 : {
43 : Word16 pool_part;
44 :
45 624530 : 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 624530 : if ( num < 0 )
48 : {
49 12518 : pool_part = negate( pool_part ); /* make negative, one op */
50 : }
51 624530 : return pool_part;
52 : }
53 :
54 122264 : 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 122264 : move16();
61 122264 : move16();
62 122264 : move16();
63 122264 : move16();
64 : Word32 b;
65 : UWord16 lsb;
66 :
67 122264 : b = L_deposit_l( a[0] );
68 122264 : b = L_shl( L_deposit_l( add( a[1], extract_h( L_mult0( extract_l( b ), x ) ) ) ), 1 ); /* Q15 */
69 122264 : Mpy_32_16_ss( b, x, &b, &lsb ); /* Q15 */
70 122264 : b = L_add( L_deposit_l( a[2] ), b ); /* Q15 */
71 122264 : Mpy_32_16_ss( b, x, &b, &lsb ); /* Q15 */
72 122264 : b = L_add( L_deposit_l( a[3] ), b ); /* Q15 */
73 122264 : *result = extract_l( b ); /* Q15 */
74 122264 : move16();
75 122264 : return;
76 : }
77 :
78 : /*-------------------------------------------------------------------*
79 : * obtainEnergyQuantizerDensity_fx()
80 : *
81 : *
82 : *-------------------------------------------------------------------*/
83 61853 : 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 61853 : den = sub( shl( L, 1 ), 1 );
92 61853 : IF( LE_16( den, 67 ) )
93 : {
94 60191 : Rnrg = extract_l( intLimCDivPos_fx( L_deposit_l( R ), den ) ); /* Q0 */
95 : }
96 : ELSE
97 : {
98 1662 : n = norm_s( den );
99 1662 : move16();
100 1662 : Rnrg = shr( div_s( R, shl( den, n ) ), sub( 15, n ) ); /* n */
101 : }
102 61853 : Rnrg = add( Rnrg, 28 ); /* Q0 */
103 :
104 61853 : Rnrg = s_min( Rnrg, 56 ); /* Q0 */
105 61853 : Rnrg = s_min( Rnrg, sub( R, 96 ) ); /* Q0 */
106 :
107 61853 : Rnrg = s_max( Rnrg, 3 );
108 61853 : *Density = obtainEnergyQuantizerDensity_f[Rnrg]; /* Q0 */
109 61853 : move16();
110 61853 : return;
111 : }
112 :
113 :
114 : /*-------------------------------------------------------------------*
115 : * dsDirac2Dirac_fx()
116 : *
117 : *
118 : *-------------------------------------------------------------------*/
119 624530 : void dsDirac2Dirac_fx(
120 : const Word16 dsDiracIndex, /* i : input index Q0*/
121 : Word16 *diracs /* o : number of diracs Q0*/
122 : )
123 : {
124 624530 : *diracs = dsDiracsTab[dsDiracIndex]; /* Q0 */
125 624530 : move16();
126 624530 : return;
127 : }
128 :
129 686343 : 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 686343 : sv = frQuanta[td]; /* Q0 */
144 686343 : nsv = sv[0];
145 686343 : move16();
146 686343 : t_quanta_o = sub( t_quanta, QUANTAQ3OFFSET ); /* Q0 */
147 :
148 686343 : IF( GE_16( t_quanta_o, sv[nsv] ) )
149 : {
150 36822 : *DsIdx = nsv; /* Q0 */
151 36822 : move16();
152 36822 : return;
153 : }
154 :
155 649521 : IF( LE_16( t_quanta_o, sv[1] ) )
156 : {
157 85484 : *DsIdx = 1; /* Q0 */
158 85484 : move16();
159 85484 : return;
160 : }
161 :
162 :
163 564037 : dsIndex = shl( 1, frQuanta[0][td] ); /* Q0 */
164 564037 : IF( GT_16( t_quanta_o, sv[nsv >> 1] ) )
165 : {
166 186640 : dsIndex = sub( nsv, dsIndex ); /* Q0 */
167 : }
168 2012273 : FOR( i = frQuanta[0][td] - 1; i >= 0; i-- )
169 : {
170 1448236 : dsIndex = add( dsIndex, shl( sub( shl( lshr( sub( sv[dsIndex], t_quanta_o ), 15 ), 1 ), 1 ), i ) ); /* Q0 */
171 : }
172 :
173 564037 : dsIndex = add( dsIndex, lshr( sub( sv[dsIndex], t_quanta_o ), 15 ) ); /* Q0 */
174 564037 : dsIndex = sub( dsIndex, lshr( sub( 1, dsIndex ), 15 ) ); /* Q0 */
175 :
176 564037 : if ( EQ_16( dsm, PVQ_CONS ) )
177 : {
178 222789 : *DsIdx = dsIndex; /* Q0 */
179 222789 : move16();
180 222789 : return;
181 : }
182 341248 : *DsIdx = add( dsIndex, lshr( sub( add( sv[dsIndex + 1], sv[dsIndex] ), shl( t_quanta_o, 1 ) ), 15 ) ); /* Q0 */
183 341248 : move16();
184 341248 : return;
185 : }
186 :
187 63453 : 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 63453 : *Quanta = dimFrQuanta[td][dsDiracIndex];
195 63453 : move16();
196 63453 : if ( dsDiracIndex == 0 )
197 : {
198 0 : *Quanta = -1; /* single op */
199 0 : move16();
200 : }
201 63453 : *Quanta = add( *Quanta, QUANTAQ3OFFSET ); /* Q0 */
202 63453 : move16();
203 63453 : return;
204 : }
205 :
206 624530 : 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 624530 : frQuantaL = hBitsN[L];
225 :
226 624530 : *Qreservplus = add( Qreserv, sub( Qvec, QUANTAQ3OFFSET ) ); /* Q0 */
227 624530 : move16();
228 624530 : dsDiracPerQuanta_fx( L, Qvec, Fcons, hBitsN, &Minit );
229 :
230 624530 : Mprime = Minit;
231 624530 : move16();
232 : DO
233 : {
234 687491 : Qtestminus = (Word16) frQuantaL[Mprime];
235 687491 : move16();
236 687491 : *Qspare = sub( Qavail, Qtestminus ); /* Q0 */
237 687491 : move16();
238 687491 : Mprime = sub( Mprime, 1 ); /* Q0 */
239 : }
240 687491 : WHILE( ( Mprime >= 0 ) && LT_16( *Qspare, QUANTAQ3OFFSET ) );
241 :
242 624530 : if ( Mprime < 0 )
243 : {
244 57172 : *Qspare = add( Qavail, QUANTAQ3OFFSET ); /* single op Q0*/
245 57172 : move16();
246 : }
247 624530 : dsDirac2Dirac_fx( add( Mprime, 1 ), Dvec );
248 :
249 624530 : *Dspecplus = add( Dspec, *Dvec ); /* Q0 */
250 624530 : move16();
251 624530 : *Qreservplus = sub( *Qreservplus, (Word16) frQuantaL[Minit] ); /* Q0 */
252 624530 : move16();
253 624530 : *Qspare = sub( *Qspare, QUANTAQ3OFFSET ); /* Q0 */
254 624530 : move16();
255 :
256 624530 : return;
257 : }
258 :
259 :
260 562677 : 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 562677 : rangeCoderFinalizationFBits_fx( Brc, INTrc, &Bff );
279 :
280 562677 : IF( LT_16( D, Nbands ) )
281 : {
282 503515 : L_tmp = L_deposit_l( sub( Breserv, Bff ) ); /* Q0 */
283 503515 : Btemp = extract_l( intLimCDivSigned_fx( L_tmp, s_min( D, 3 ) ) ); /* result always fits in Word16 Q0*/
284 503515 : *Breservplus = add( Bband, Breserv ); /* Q0 */
285 503515 : move16();
286 : }
287 : ELSE
288 : {
289 59162 : Btemp = 0;
290 59162 : move16();
291 59162 : *Breservplus = add( Bband, Bff ); /* Q0 */
292 59162 : move16();
293 : }
294 562677 : *Bband_adj = s_min( extract_l( L_mult( L, 40 ) ), Bband ); /* Q3 */
295 562677 : move16();
296 562677 : *Brem = sub( Bavail, Bff ); /* Q0 */
297 562677 : move16();
298 562677 : *Bband_adj = s_min( *Brem, add( *Bband_adj, Btemp ) ); /* Q0 */
299 562677 : move16();
300 562677 : *Bband_adj = s_max( 0, *Bband_adj ); /* Q0 */
301 562677 : move16();
302 562677 : return;
303 : }
304 :
305 :
306 61132 : 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 61132 : ns = norm_s( opp ); /* exponent */
316 61132 : nc = norm_s( near ); /* exponent */
317 :
318 61132 : ms = shl( opp, ns ); /* mantissa */
319 61132 : mc = shl( near, nc ); /* mantissa */
320 :
321 61132 : acc = L_mac( 538500224L, mc, -2776 ); /* a0*mc + a1, acc(Q27), a0(Q11), a1(Q27) */
322 61132 : z = mac_r( acc, ms, -2776 ); /* z in Q11, a0 in Q11 */
323 61132 : d = sub( ms, mc ); /* d in Q15 */
324 61132 : z = mult_r( z, d ); /* z in Q11 */
325 :
326 61132 : result = add( z, shl( sub( nc, ns ), 11 ) ); /* Q11 */
327 :
328 61132 : return result;
329 : }
330 :
331 61132 : 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 61132 : tmp = add( 1 << 7, Ratio_base2Q11_fx( opp, near ) ); /* Q11 */
340 61132 : *result = shr( tmp, 8 ); /* Q3 */
341 61132 : move16();
342 61132 : return;
343 : }
344 :
345 :
346 61853 : 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 61853 : phiQ14q = (Word16) intLimCDivPos_fx( (UWord32) L_shl( L_deposit_l( indexphi ), 13 ), shr( D, 1 ) ); /* Q14 */
358 61853 : move16();
359 61853 : if ( indexphi < 0 )
360 : {
361 337 : phiQ14q = 1 << 13; /* one op */
362 337 : move16();
363 : }
364 :
365 61853 : oppTail = shr( sub( 16320, phiQ14q ), 15 );
366 61853 : nearTail = shr( sub( phiQ14q, 64 ), 15 );
367 :
368 61853 : IF( s_or( oppTail, nearTail ) < 0 )
369 : {
370 721 : *oppQ15 = s_and( oppTail, ( 1 << 15 ) - 1 ); /* Q15 */
371 721 : move16();
372 721 : *nearQ15 = s_and( nearTail, ( 1 << 15 ) - 1 ); /* Q15 */
373 721 : move16();
374 721 : *oppRatioQ3 = shl( add( 1, shl( nearTail, 1 ) ), 14 ); /* Q3 */
375 721 : move16();
376 : }
377 : ELSE
378 : {
379 61132 : nearProjQ15_fx( shl( sub( 1 << 14, phiQ14q ), 1 ), oppQ15 );
380 61132 : nearProjQ15_fx( shl( phiQ14q, 1 ), nearQ15 );
381 61132 : Ratio_rQ3_fx( *oppQ15, *nearQ15, oppRatioQ3 );
382 : }
383 :
384 61853 : return;
385 : }
386 :
387 31568 : 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 31568 : *indexphi = mult_r( shl( D, 1 ), phiQ14uq ); /* Q0 */
397 31568 : move16();
398 31568 : if ( s_and( D, 1 ) > 0 )
399 : {
400 180 : *indexphi = -1; /* one op */
401 180 : move16();
402 : }
403 31568 : densityAngle2RmsProjDec_fx( D, *indexphi, oppQ15, nearQ15, oppRatioQ3 );
404 :
405 31568 : return;
406 : }
407 :
408 61853 : 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 : #ifdef BASOP_NOGLOB_DECLARE_LOCAL
431 61853 : Flag Overflow = 0;
432 61853 : move32();
433 : #endif
434 :
435 61853 : rangeCoderFinalizationFBits_fx( Qac, INTac, &qac );
436 61853 : qboth = sub( qband, sub( qac, qzero ) ); /* Q0 */
437 : /* skew calc code */
438 61853 : qskew = 0;
439 61853 : move16();
440 61853 : IF( GT_16( Nhead, 1 ) )
441 : {
442 61813 : qavg = extract_h( L_shl( intLimCDivSigned_fx( L_deposit_l( qboth ), Np ), 16 ) ); /* qboth may be negative Q0*/
443 61813 : dsDiracPerQuanta_fx( Ntail, qavg, FlagCons, hBitsN, &Midx );
444 61813 : QuantaPerDsDirac_fx( Nhead, Midx, hBitsN, &qmin );
445 61813 : qskew = sub( qavg, qmin ); /* Q0 */
446 61813 : qskew = s_max( 0, qskew ); /* Q0 */
447 : } /* end of skew calc code*/
448 :
449 61853 : QIa = add( extract_l( intLimCDivPos_fx( (UWord32) L_deposit_l( Nopp ), Nnear ) ), 1 ); /* always positive Word16 out Q0*/
450 61853 : L_qnum = L_sub( L_deposit_l( sub( sub( add( qband, qzero ), qac ), qskew ) ), L_mult0( Nopp, oppRQ3 ) ); /* Q0 */
451 :
452 61853 : L_QIb = L_deposit_l( 0 );
453 61853 : IF( L_qnum > 0 )
454 : {
455 60879 : L_QIb = (Word32) intLimCDivPos_fx( L_qnum, QIa ); /* Q0 */
456 : }
457 61853 : *qnear = qboth; /* Q3 */
458 61853 : move16();
459 61853 : QIb = extract_h( L_shl_o( L_QIb, 16, &Overflow ) ); /* may saturate Q0*/
460 61853 : if ( LE_16( QIb, qboth ) )
461 : {
462 61006 : *qnear = QIb; /* Q0 */
463 61006 : move16();
464 : }
465 61853 : *qopp = sub( qboth, *qnear ); /* Q0 */
466 61853 : move16();
467 61853 : *qglobalupd = sub( qglobal, sub( qac, qzero ) ); /* Q0 */
468 61853 : move16();
469 :
470 61853 : return;
471 : }
472 :
473 :
474 : /*--------------------------------------------------------------------------*
475 : * apply_gain()
476 : *
477 : * Apply gain
478 : *--------------------------------------------------------------------------*/
479 :
480 57713 : void apply_gain_fx(
481 : const Word16 *ord, /* i : Indices for energy order Q0 */
482 : const Word16 *band_start, /* i : Sub band start indices Q0 */
483 : const Word16 *band_end, /* i : Sub band end indices Q0 */
484 : const Word16 num_sfm, /* i : Number of bands Q0 */
485 : const Word16 *gains, /* i : Band gain vector Qx */
486 : Word16 *xq /* i/o: Float synthesis / Gain adjusted synth Q15/Qx */
487 : )
488 : {
489 : Word16 band, i;
490 : Word16 g; /* Q12 */
491 :
492 901946 : FOR( band = 0; band < num_sfm; band++ )
493 : {
494 844233 : g = gains[ord[band]];
495 844233 : move16();
496 15530897 : FOR( i = band_start[band]; i < band_end[band]; i++ )
497 : {
498 : /*xq[i] *= g; */
499 14686664 : xq[i] = mult_r( g, xq[i] );
500 14686664 : move16(); /*12+15+1-16=12 */
501 : }
502 : }
503 :
504 57713 : return;
505 : }
506 :
507 : /*--------------------------------------------------------------------------*
508 : * fine_gain_quant_fx()
509 : *
510 : * Fine gain quantization
511 : *--------------------------------------------------------------------------*/
512 29425 : void fine_gain_quant_fx(
513 : BSTR_ENC_HANDLE hBstr, /* i/o: encoder bitstream handle */
514 : const Word16 *ord, /* i : Indices for energy order Q0 */
515 : const Word16 num_sfm, /* i : Number of bands Q0 */
516 : const Word16 *gain_bits, /* i : Gain adjustment bits per sub band Q0 */
517 : Word16 *fg_pred, /* i/o: Predicted gains / Corrected gains Q12 */
518 : const Word16 *gopt /* i : Optimal gains Q12 */
519 : )
520 : {
521 : Word16 band;
522 : Word16 gbits;
523 : Word16 idx;
524 : Word16 gain_db, gain_dbq;
525 : Word16 err;
526 :
527 : Word16 tmp1, tmp2, exp1, exp2;
528 : Word32 L_tmp;
529 : UWord16 lsb;
530 : #ifdef BASOP_NOGLOB_DECLARE_LOCAL
531 29425 : Flag Overflow = 0;
532 29425 : move32();
533 : #endif
534 :
535 464086 : FOR( band = 0; band < num_sfm; band++ )
536 : {
537 434661 : gbits = gain_bits[ord[band]]; /* Q0 */
538 434661 : move16();
539 434661 : test();
540 434661 : IF( fg_pred[band] != 0 && gbits > 0 )
541 : {
542 34836 : exp1 = norm_s( gopt[band] );
543 34836 : exp1 = sub( exp1, 1 );
544 34836 : tmp1 = shl( gopt[band], exp1 );
545 34836 : exp2 = norm_s( fg_pred[band] );
546 34836 : tmp2 = shl( fg_pred[band], exp2 ); /* Q12 + exp2 */
547 34836 : exp1 = add( 15, sub( exp1, exp2 ) );
548 34836 : err = div_s( tmp1, tmp2 ); /* Q15 */
549 34836 : tmp1 = norm_s( err );
550 34836 : exp2 = Log2_norm_lc( L_deposit_h( shl( err, tmp1 ) ) );
551 34836 : tmp1 = sub( 14, tmp1 );
552 34836 : tmp1 = sub( tmp1, exp1 );
553 34836 : L_tmp = L_Comp( tmp1, exp2 );
554 34836 : Mpy_32_16_ss( L_tmp, 24660, &L_tmp, &lsb ); /* 24660 = 20*log10(2) in Q12 */ /*16+12-15=13 */
555 34836 : gain_db = round_fx_sat( L_shl_o( L_tmp, 17, &Overflow ) );
556 :
557 34836 : idx = squant_fx( gain_db, &gain_dbq, finegain_fx[gbits - 1], gain_cb_size[gbits - 1] );
558 34836 : push_indice( hBstr, IND_PVQ_FINE_GAIN, idx, gbits );
559 :
560 34836 : L_tmp = L_mult0( gain_dbq, 21771 ); /* 21771=0.05*log2(10) */ /* 14+17=31 */
561 34836 : L_tmp = L_shr( L_tmp, 15 ); /* Q16 */
562 34836 : tmp1 = L_Extract_lc( L_tmp, &exp1 );
563 34836 : tmp1 = abs_s( tmp1 );
564 34836 : tmp1 = extract_l( Pow2( 14, tmp1 ) );
565 34836 : exp1 = sub( 14, exp1 );
566 :
567 34836 : L_tmp = L_mult0( fg_pred[band], tmp1 ); /*12+exp1 */
568 34836 : fg_pred[band] = round_fx_sat( L_shl_sat( L_tmp, sub( 16, exp1 ) ) ); /*12+exp1+16-exp1-16=12 */
569 34836 : move16();
570 : }
571 : }
572 :
573 29425 : return;
574 : }
575 :
576 : /*-------------------------------------------------------------------*
577 : * srt_vec_ind()
578 : *
579 : * sort vector and save sorting indeces
580 : *-------------------------------------------------------------------*/
581 :
582 605940 : void srt_vec_ind16_fx(
583 : const Word16 *linear, /* linear input Q3*/
584 : Word16 *srt, /* sorted output Q3*/
585 : Word16 *I, /* index for sorted output Q0*/
586 : Word16 length /* Q0 */
587 : )
588 : {
589 : Word16 pos, npos;
590 : Word16 idxMem;
591 : Word16 valMem;
592 :
593 : /*initilize */
594 1540151 : FOR( pos = 0; pos < length; pos++ )
595 : {
596 934211 : I[pos] = pos; /* Q0 */
597 934211 : move16();
598 : }
599 :
600 605940 : Copy( linear, srt, length ); /* Q3 */
601 :
602 : /* now iterate */
603 934211 : FOR( pos = 0; pos < ( length - 1 ); pos++ )
604 : {
605 1442244 : FOR( npos = ( pos + 1 ); npos < length; npos++ )
606 : {
607 1113973 : IF( LT_16( srt[npos], srt[pos] ) )
608 : {
609 517806 : idxMem = I[pos]; /* Q0 */
610 517806 : move16();
611 517806 : I[pos] = I[npos]; /* Q0 */
612 517806 : move16();
613 517806 : I[npos] = idxMem; /* Q0 */
614 517806 : move16();
615 :
616 517806 : valMem = srt[pos]; /* Q0 */
617 517806 : move16();
618 517806 : srt[pos] = srt[npos]; /* Q0 */
619 517806 : move16();
620 517806 : srt[npos] = valMem; /* Q0 */
621 517806 : move16();
622 : }
623 : }
624 : }
625 :
626 605940 : return;
627 : }
628 :
629 : /*-----------------------------------------------------------------------------
630 : * atan2_fx():
631 : *
632 : * Approximates arctan piecewise with various 4th to 5th order least square fit
633 : * polynomials for input in 5 segments:
634 : * - 0.0 to 1.0
635 : * - 1.0 to 2.0
636 : * - 2.0 to 4.0
637 : * - 4.0 to 8.0
638 : * - 8.0 to infinity
639 : *---------------------------------------------------------------------------*/
640 6839398 : Word16 atan2_fx( /* o: Angle between 0 and PI/2 radian (Q14) */
641 : const Word32 y, /* i : near side (Argument must be positive) (Q15) */
642 : const Word32 x /* i : opposite side (Q15) */
643 : )
644 : {
645 : Word32 acc, arg;
646 : Word16 man, expo, reciprocal;
647 : Word16 angle, w, z;
648 :
649 6839398 : IF( x == 0 )
650 : {
651 54813 : return 25736; /* PI/2 in Q14 */
652 : }
653 6784585 : man = ratio( y, x, &expo ); /* man in Q14 */
654 6784585 : expo = sub( expo, ( 15 - 14 ) ); /* Now, man is considered in Q15 */
655 6784585 : arg = L_shr( (Word32) man, expo );
656 :
657 6784585 : IF( L_shr( arg, 3 + 15 ) != 0 )
658 : /*===============================*
659 : * 8.0 <= x < infinity *
660 : *===============================*/
661 : {
662 : /* atan(x) = PI/2 - 1/x + 1/(3x^3) - 1/(5x^5) + ...
663 : * ~ PI/2 - 1/x, for x >= 8.
664 : */
665 401885 : expo = norm_l( arg );
666 401885 : man = extract_h( L_shl( arg, expo ) );
667 401885 : reciprocal = div_s( 0x3fff, man );
668 401885 : expo = sub( 15 + 1, expo );
669 401885 : reciprocal = shr( reciprocal, expo ); /* Q14 */
670 401885 : angle = sub( 25736, reciprocal ); /* Q14 (PI/2 - 1/x) */
671 :
672 : /* For 8.0 <= x < 10.0, 1/(5x^5) is not completely negligible.
673 : * For more accurate result, add very small correction term.
674 : */
675 401885 : if ( LT_32( L_shr( arg, 15 ), 10L ) )
676 : {
677 84006 : angle = add( angle, 8 ); /* Add tiny correction term. */
678 : }
679 : }
680 6382700 : ELSE IF( L_shr( arg, 2 + 15 ) != 0 )
681 : /*==========================*
682 : * 4.0 <= x < 8.0 *
683 : *==========================*/
684 : {
685 : /* interval: [3.999, 8.001]
686 : * atan(x) ~ (((a0*x + a1)*x + a2)*x + a3)*x + a4
687 : * = (((a0*8*y + a1)*8*y + a2)*8*y + a3)*8*y + a4 Substitute 8*y -> x
688 : * = (((a0*8^3*y + a1*8^2)*y + a2*8)*y + a3)*8*y + a4
689 : * = ((( c0*y + c1)*y + c2)*y + c3)*8*y + c4,
690 : * where y = x/8
691 : * and a0 = -1.28820869667651e-04, a1 = 3.88263533346295e-03,
692 : * a2 = -4.64216306484597e-02, a3 = 2.75986060068931e-01,
693 : * a4 = 7.49208077809799e-01.
694 : */
695 261521 : w = extract_l( L_shr( arg, 3 ) ); /* Q15 y = x/8 */
696 261521 : acc = L_add( 533625337L, 0 ); /* Q31 c1 = a1*8^2 */
697 261521 : z = mac_r( acc, w, -2161 ); /* Q15 c0 = a0*8^3 */
698 261521 : acc = L_add( -797517542L, 0 ); /* Q31 c2 = a2*8 */
699 261521 : z = mac_r( acc, w, z ); /* Q15 */
700 261521 : acc = L_add( 592675551L, 0 ); /* Q31 c3 = a3 */
701 261521 : z = mac_r( acc, w, z ); /* z (in:Q15, out:Q12) */
702 261521 : acc = L_add( 201114012L, 0 ); /* Q28 c4 = a4 */
703 261521 : acc = L_mac( acc, w, z ); /* Q28 */
704 261521 : angle = extract_l( L_shr( acc, ( 28 - 14 ) ) ); /* Q14 result of atan(x), where 4 <= x < 8 */
705 : }
706 6121179 : ELSE IF( L_shr( arg, 1 + 15 ) != 0 )
707 : /*==========================*
708 : * 2.0 <= x < 4.0 *
709 : *==========================*/
710 : {
711 : /* interval: [1.999, 4.001]
712 : * atan(x) ~ (((a0*x + a1)*x + a2)*x + a3)*x + a4
713 : * = (((a0*4*y + a1)*4*y + a2)*4*y + a3)*4*y + a4 Substitute 4*y -> x
714 : * = (((a0*16*y + a1*4)*y + a2)*4*y + a3)*4*y + a4
715 : * = (((a0*32*y + a1*8)*y + a2*2)*2*y + a3)*4*y + a4
716 : * = ((( c0*y + c1)*y + c2)*2*y + c3)*4*y + c4,
717 : * where y = x/4
718 : * and a0 = -0.00262378195660943, a1 = 0.04089687039888652,
719 : * a2 = -0.25631148958325911, a3 = 0.81685854627399479,
720 : * a4 = 0.21358070563097167
721 : * */
722 413088 : w = extract_l( L_shr( arg, 2 ) ); /* Q15 y = x/4 */
723 413088 : acc = L_add( 702602883L, 0 ); /* Q31 c1 = a1*8 */
724 413088 : z = mac_r( acc, w, -2751 ); /* Q15 c0 = a0*32 */
725 413088 : acc = L_add( -1100849465L, 0 ); /* Q31 c2 = a2*2 */
726 413088 : z = mac_r( acc, w, z ); /* z (in:Q15, out:Q14) */
727 413088 : acc = L_add( 877095185L, 0 ); /* Q30 c3 = a3 */
728 413088 : z = mac_r( acc, w, z ); /* z (in:Q14, out:Q12) */
729 413088 : acc = L_add( 57332634L, 0 ); /* Q28 c4 = a4 */
730 413088 : acc = L_mac( acc, w, z ); /* Q28 */
731 413088 : angle = extract_l( L_shr( acc, ( 28 - 14 ) ) ); /* Q14 result of atan(x) where 2 <= x < 4 */
732 : }
733 5708091 : ELSE IF( L_shr( arg, 15 ) != 0 )
734 : /*==========================*
735 : * 1.0 <= x < 2.0 *
736 : *==========================*/
737 : {
738 : /* interval: [0.999, 2.001]
739 : * atan(x) ~ (((a0*x + 1)*x + a2)*x + a3)*x + a4
740 : * = (((a0*2*y + a1)*2*y + a2)*2*y + a3)*2*y + a4 Substitute 2*y -> x
741 : * = (((a0*4*y + a1*2)*y + a2)*2*y + a3)*2*y + a4
742 : * = (((a0*4*y + a1*2)*y + a2)*y + a3/2)*4*y + a4
743 : * = ((( c0*y + c1)*y + c2)*y + c3)*4*y + c4,
744 : * where y = x/2
745 : * and a0 = -0.0160706457245251, a1 = 0.1527106504065224,
746 : * a2 = -0.6123208404800871, a3 = 1.3307896976322915,
747 : * a4 = -0.0697089375247448
748 : */
749 1063623 : w = extract_l( L_shr( arg, 1 ) ); /* Q15 y= x/2 */
750 1063623 : acc = L_add( 655887249L, 0 ); /* Q31 c1 = a1*2 */
751 1063623 : z = mac_r( acc, w, -2106 ); /* Q15 c0 = a0*4 */
752 1063623 : acc = L_add( -1314948992L, 0 ); /* Q31 c2 = a2 */
753 1063623 : z = mac_r( acc, w, z );
754 1063623 : acc = L_add( 1428924557L, 0 ); /* Q31 c3 = a3/2 */
755 1063623 : z = mac_r( acc, w, z ); /* z (in:Q15, out:Q13) */
756 1063623 : acc = L_add( -37424701L, 0 ); /* Q29 c4 = a4 */
757 1063623 : acc = L_mac( acc, w, z ); /* Q29 */
758 1063623 : angle = extract_l( L_shr( acc, ( 29 - 14 ) ) ); /* Q14 result of atan(x) where 1 <= x < 2 */
759 : }
760 : ELSE
761 : /*==========================*
762 : * 0.0 <= x < 1.0 *
763 : *==========================*/
764 : {
765 : /* interval: [-0.001, 1.001]
766 : * atan(x) ~ ((((a0*x + a1)*x + a2)*x + a3)*x + a4)*x + a5
767 : * = ((((a0*2*x + a1*2)*x/2 + a2)*x + a3)*x + a4)*x + a5
768 : * = (((( c0*x + c1)*x/2 + c2)*x + c3)*x + c4)*x + c5
769 : * where
770 : * a0 = -5.41182677118661e-02, a1 = 2.76690449232515e-01,
771 : * a2 = -4.63358392562492e-01, a3 = 2.87188466598566e-02,
772 : * a4 = 9.97438122814383e-01, a5 = 5.36158556179092e-05.
773 : */
774 4644468 : w = extract_l( arg ); /* Q15 */
775 4644468 : acc = L_add( 1188376431L, 0 ); /* Q31 c1 = a1*2 */
776 4644468 : z = mac_r( acc, w, -3547 ); /* Q15 c0 = a0*2 */
777 4644468 : acc = L_add( -995054571L, 0 ); /* Q31 c2 = a2 */
778 4644468 : z = extract_h( L_mac0( acc, w, z ) ); /* Q15 non-fractional mode multiply */
779 4644468 : acc = L_add( 61673254L, 0 ); /* Q31 c3 = a3 */
780 4644468 : z = mac_r( acc, w, z );
781 4644468 : acc = L_add( 2141982059L, 0 ); /* Q31 c4 = a4 */
782 4644468 : z = mac_r( acc, w, z );
783 4644468 : acc = L_add( 115139L, 0 ); /* Q31 c5 = a5 */
784 4644468 : acc = L_mac( acc, w, z ); /* Q31 */
785 4644468 : angle = extract_l( L_shr( acc, 31 - 14 ) ); /* Q14 result of atan(x), where 0 <= x < 1 */
786 : }
787 :
788 6784585 : return angle; /* Q14 between 0 and PI/2 radian. */
789 : }
|
