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 809166 : UWord32 intLimCDivPos_fx(
12 : UWord32 NUM, /* Qx */
13 : Word16 DEN /* Q0 */
14 : )
15 : {
16 : UWord32 UL_ru, UL_rl;
17 809166 : Mpy_32_32_uu( UL_lshl( NUM, 1 ), intLimCDivInvDQ31[DEN], &UL_ru, &UL_rl ); /* Qx */
18 809166 : return UL_ru;
19 : }
20 :
21 : /*! r: Approximate integer division for signed input using lookup table */
22 564382 : static Word32 intLimCDivSigned_fx(
23 : Word32 NUM, /* Qx */
24 : Word16 DEN /* Q0 */
25 : )
26 : {
27 : Word32 L_tmp;
28 :
29 564382 : L_tmp = intLimCDivPos_fx( L_abs( NUM ), DEN ); /* Qx */
30 564382 : if ( NUM < 0 )
31 : {
32 75212 : L_tmp = L_negate( L_tmp ); /* one op */
33 : }
34 564382 : return L_tmp;
35 : }
36 :
37 : /*! r: Approximate integer division for negative input */
38 623533 : Word16 shrtCDivSignedApprox(
39 : const Word16 num, /* Q0 */
40 : const Word16 den /* Q0 */
41 : )
42 : {
43 : Word16 pool_part;
44 :
45 623533 : 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 623533 : if ( num < 0 )
48 : {
49 12497 : pool_part = negate( pool_part ); /* make negative, one op */
50 : }
51 623533 : return pool_part;
52 : }
53 :
54 122254 : 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 122254 : move16();
61 122254 : move16();
62 122254 : move16();
63 122254 : move16();
64 : Word32 b;
65 : UWord16 lsb;
66 :
67 122254 : b = L_deposit_l( a[0] );
68 122254 : b = L_shl( L_deposit_l( add( a[1], extract_h( L_mult0( extract_l( b ), x ) ) ) ), 1 ); /* Q15 */
69 122254 : Mpy_32_16_ss( b, x, &b, &lsb ); /* Q15 */
70 122254 : b = L_add( L_deposit_l( a[2] ), b ); /* Q15 */
71 122254 : Mpy_32_16_ss( b, x, &b, &lsb ); /* Q15 */
72 122254 : b = L_add( L_deposit_l( a[3] ), b ); /* Q15 */
73 122254 : *result = extract_l( b ); /* Q15 */
74 122254 : move16();
75 122254 : return;
76 : }
77 :
78 : /*-------------------------------------------------------------------*
79 : * obtainEnergyQuantizerDensity_fx()
80 : *
81 : *
82 : *-------------------------------------------------------------------*/
83 61847 : 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 61847 : den = sub( shl( L, 1 ), 1 );
92 61847 : IF( LE_16( den, 67 ) )
93 : {
94 60193 : Rnrg = extract_l( intLimCDivPos_fx( L_deposit_l( R ), den ) ); /* Q0 */
95 : }
96 : ELSE
97 : {
98 1654 : n = norm_s( den );
99 1654 : move16();
100 1654 : Rnrg = shr( div_s( R, shl( den, n ) ), sub( 15, n ) ); /* n */
101 : }
102 61847 : Rnrg = add( Rnrg, 28 ); /* Q0 */
103 :
104 61847 : Rnrg = s_min( Rnrg, 56 ); /* Q0 */
105 61847 : Rnrg = s_min( Rnrg, sub( R, 96 ) ); /* Q0 */
106 :
107 61847 : Rnrg = s_max( Rnrg, 3 );
108 61847 : *Density = obtainEnergyQuantizerDensity_f[Rnrg]; /* Q0 */
109 61847 : move16();
110 61847 : return;
111 : }
112 :
113 :
114 : /*-------------------------------------------------------------------*
115 : * dsDirac2Dirac_fx()
116 : *
117 : *
118 : *-------------------------------------------------------------------*/
119 623533 : void dsDirac2Dirac_fx(
120 : const Word16 dsDiracIndex, /* i : input index Q0*/
121 : Word16 *diracs /* o : number of diracs Q0*/
122 : )
123 : {
124 623533 : *diracs = dsDiracsTab[dsDiracIndex]; /* Q0 */
125 623533 : move16();
126 623533 : return;
127 : }
128 :
129 685340 : 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 685340 : sv = frQuanta[td]; /* Q0 */
144 685340 : nsv = sv[0];
145 685340 : move16();
146 685340 : t_quanta_o = sub( t_quanta, QUANTAQ3OFFSET ); /* Q0 */
147 :
148 685340 : IF( GE_16( t_quanta_o, sv[nsv] ) )
149 : {
150 36731 : *DsIdx = nsv; /* Q0 */
151 36731 : move16();
152 36731 : return;
153 : }
154 :
155 648609 : IF( LE_16( t_quanta_o, sv[1] ) )
156 : {
157 85221 : *DsIdx = 1; /* Q0 */
158 85221 : move16();
159 85221 : return;
160 : }
161 :
162 :
163 563388 : dsIndex = shl( 1, frQuanta[0][td] ); /* Q0 */
164 563388 : IF( GT_16( t_quanta_o, sv[nsv >> 1] ) )
165 : {
166 186276 : dsIndex = sub( nsv, dsIndex ); /* Q0 */
167 : }
168 2009547 : FOR( i = frQuanta[0][td] - 1; i >= 0; i-- )
169 : {
170 1446159 : dsIndex = add( dsIndex, shl( sub( shl( lshr( sub( sv[dsIndex], t_quanta_o ), 15 ), 1 ), 1 ), i ) ); /* Q0 */
171 : }
172 :
173 563388 : dsIndex = add( dsIndex, lshr( sub( sv[dsIndex], t_quanta_o ), 15 ) ); /* Q0 */
174 563388 : dsIndex = sub( dsIndex, lshr( sub( 1, dsIndex ), 15 ) ); /* Q0 */
175 :
176 563388 : if ( EQ_16( dsm, PVQ_CONS ) )
177 : {
178 222982 : *DsIdx = dsIndex; /* Q0 */
179 222982 : move16();
180 222982 : return;
181 : }
182 340406 : *DsIdx = add( dsIndex, lshr( sub( add( sv[dsIndex + 1], sv[dsIndex] ), shl( t_quanta_o, 1 ) ), 15 ) ); /* Q0 */
183 340406 : move16();
184 340406 : return;
185 : }
186 :
187 63451 : 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 63451 : *Quanta = dimFrQuanta[td][dsDiracIndex];
195 63451 : move16();
196 63451 : if ( dsDiracIndex == 0 )
197 : {
198 0 : *Quanta = -1; /* single op */
199 0 : move16();
200 : }
201 63451 : *Quanta = add( *Quanta, QUANTAQ3OFFSET ); /* Q0 */
202 63451 : move16();
203 63451 : return;
204 : }
205 :
206 623533 : 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 623533 : frQuantaL = hBitsN[L];
225 :
226 623533 : *Qreservplus = add( Qreserv, sub( Qvec, QUANTAQ3OFFSET ) ); /* Q0 */
227 623533 : move16();
228 623533 : dsDiracPerQuanta_fx( L, Qvec, Fcons, hBitsN, &Minit );
229 :
230 623533 : Mprime = Minit;
231 623533 : move16();
232 : DO
233 : {
234 686533 : Qtestminus = (Word16) frQuantaL[Mprime];
235 686533 : move16();
236 686533 : *Qspare = sub( Qavail, Qtestminus ); /* Q0 */
237 686533 : move16();
238 686533 : Mprime = sub( Mprime, 1 ); /* Q0 */
239 : }
240 686533 : WHILE( ( Mprime >= 0 ) && LT_16( *Qspare, QUANTAQ3OFFSET ) );
241 :
242 623533 : if ( Mprime < 0 )
243 : {
244 57247 : *Qspare = add( Qavail, QUANTAQ3OFFSET ); /* single op Q0*/
245 57247 : move16();
246 : }
247 623533 : dsDirac2Dirac_fx( add( Mprime, 1 ), Dvec );
248 :
249 623533 : *Dspecplus = add( Dspec, *Dvec ); /* Q0 */
250 623533 : move16();
251 623533 : *Qreservplus = sub( *Qreservplus, (Word16) frQuantaL[Minit] ); /* Q0 */
252 623533 : move16();
253 623533 : *Qspare = sub( *Qspare, QUANTAQ3OFFSET ); /* Q0 */
254 623533 : move16();
255 :
256 623533 : return;
257 : }
258 :
259 :
260 561686 : 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 561686 : rangeCoderFinalizationFBits_fx( Brc, INTrc, &Bff );
279 :
280 561686 : IF( LT_16( D, Nbands ) )
281 : {
282 502575 : L_tmp = L_deposit_l( sub( Breserv, Bff ) ); /* Q0 */
283 502575 : Btemp = extract_l( intLimCDivSigned_fx( L_tmp, s_min( D, 3 ) ) ); /* result always fits in Word16 Q0*/
284 502575 : *Breservplus = add( Bband, Breserv ); /* Q0 */
285 502575 : move16();
286 : }
287 : ELSE
288 : {
289 59111 : Btemp = 0;
290 59111 : move16();
291 59111 : *Breservplus = add( Bband, Bff ); /* Q0 */
292 59111 : move16();
293 : }
294 561686 : *Bband_adj = s_min( extract_l( L_mult( L, 40 ) ), Bband ); /* Q3 */
295 561686 : move16();
296 561686 : *Brem = sub( Bavail, Bff ); /* Q0 */
297 561686 : move16();
298 561686 : *Bband_adj = s_min( *Brem, add( *Bband_adj, Btemp ) ); /* Q0 */
299 561686 : move16();
300 561686 : *Bband_adj = s_max( 0, *Bband_adj ); /* Q0 */
301 561686 : move16();
302 561686 : return;
303 : }
304 :
305 :
306 61127 : 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 61127 : ns = norm_s( opp ); /* exponent */
316 61127 : nc = norm_s( near ); /* exponent */
317 :
318 61127 : ms = shl( opp, ns ); /* mantissa */
319 61127 : mc = shl( near, nc ); /* mantissa */
320 :
321 61127 : acc = L_mac( 538500224L, mc, -2776 ); /* a0*mc + a1, acc(Q27), a0(Q11), a1(Q27) */
322 61127 : z = mac_r( acc, ms, -2776 ); /* z in Q11, a0 in Q11 */
323 61127 : d = sub( ms, mc ); /* d in Q15 */
324 61127 : z = mult_r( z, d ); /* z in Q11 */
325 :
326 61127 : result = add( z, shl( sub( nc, ns ), 11 ) ); /* Q11 */
327 :
328 61127 : return result;
329 : }
330 :
331 61127 : 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 61127 : tmp = add( 1 << 7, Ratio_base2Q11_fx( opp, near ) ); /* Q11 */
340 61127 : *result = shr( tmp, 8 ); /* Q3 */
341 61127 : move16();
342 61127 : return;
343 : }
344 :
345 :
346 61847 : 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 61847 : phiQ14q = (Word16) intLimCDivPos_fx( (UWord32) L_shl( L_deposit_l( indexphi ), 13 ), shr( D, 1 ) ); /* Q14 */
358 61847 : move16();
359 61847 : if ( indexphi < 0 )
360 : {
361 337 : phiQ14q = 1 << 13; /* one op */
362 337 : move16();
363 : }
364 :
365 61847 : oppTail = shr( sub( 16320, phiQ14q ), 15 );
366 61847 : nearTail = shr( sub( phiQ14q, 64 ), 15 );
367 :
368 61847 : IF( s_or( oppTail, nearTail ) < 0 )
369 : {
370 720 : *oppQ15 = s_and( oppTail, ( 1 << 15 ) - 1 ); /* Q15 */
371 720 : move16();
372 720 : *nearQ15 = s_and( nearTail, ( 1 << 15 ) - 1 ); /* Q15 */
373 720 : move16();
374 720 : *oppRatioQ3 = shl( add( 1, shl( nearTail, 1 ) ), 14 ); /* Q3 */
375 720 : move16();
376 : }
377 : ELSE
378 : {
379 61127 : nearProjQ15_fx( shl( sub( 1 << 14, phiQ14q ), 1 ), oppQ15 );
380 61127 : nearProjQ15_fx( shl( phiQ14q, 1 ), nearQ15 );
381 61127 : Ratio_rQ3_fx( *oppQ15, *nearQ15, oppRatioQ3 );
382 : }
383 :
384 61847 : return;
385 : }
386 :
387 31562 : 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 31562 : *indexphi = mult_r( shl( D, 1 ), phiQ14uq ); /* Q0 */
397 31562 : move16();
398 31562 : if ( s_and( D, 1 ) > 0 )
399 : {
400 180 : *indexphi = -1; /* one op */
401 180 : move16();
402 : }
403 31562 : densityAngle2RmsProjDec_fx( D, *indexphi, oppQ15, nearQ15, oppRatioQ3 );
404 :
405 31562 : return;
406 : }
407 :
408 61847 : 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 61847 : Flag Overflow = 0;
432 61847 : move32();
433 : #endif
434 :
435 61847 : rangeCoderFinalizationFBits_fx( Qac, INTac, &qac );
436 61847 : qboth = sub( qband, sub( qac, qzero ) ); /* Q0 */
437 : /* skew calc code */
438 61847 : qskew = 0;
439 61847 : move16();
440 61847 : IF( GT_16( Nhead, 1 ) )
441 : {
442 61807 : qavg = extract_h( L_shl( intLimCDivSigned_fx( L_deposit_l( qboth ), Np ), 16 ) ); /* qboth may be negative Q0*/
443 61807 : dsDiracPerQuanta_fx( Ntail, qavg, FlagCons, hBitsN, &Midx );
444 61807 : QuantaPerDsDirac_fx( Nhead, Midx, hBitsN, &qmin );
445 61807 : qskew = sub( qavg, qmin ); /* Q0 */
446 61807 : qskew = s_max( 0, qskew ); /* Q0 */
447 : } /* end of skew calc code*/
448 :
449 61847 : QIa = add( extract_l( intLimCDivPos_fx( (UWord32) L_deposit_l( Nopp ), Nnear ) ), 1 ); /* always positive Word16 out Q0*/
450 61847 : L_qnum = L_sub( L_deposit_l( sub( sub( add( qband, qzero ), qac ), qskew ) ), L_mult0( Nopp, oppRQ3 ) ); /* Q0 */
451 :
452 61847 : L_QIb = L_deposit_l( 0 );
453 61847 : IF( L_qnum > 0 )
454 : {
455 60874 : L_QIb = (Word32) intLimCDivPos_fx( L_qnum, QIa ); /* Q0 */
456 : }
457 61847 : *qnear = qboth; /* Q3 */
458 61847 : move16();
459 61847 : QIb = extract_h( L_shl_o( L_QIb, 16, &Overflow ) ); /* may saturate Q0*/
460 61847 : if ( LE_16( QIb, qboth ) )
461 : {
462 61001 : *qnear = QIb; /* Q0 */
463 61001 : move16();
464 : }
465 61847 : *qopp = sub( qboth, *qnear ); /* Q0 */
466 61847 : move16();
467 61847 : *qglobalupd = sub( qglobal, sub( qac, qzero ) ); /* Q0 */
468 61847 : move16();
469 :
470 61847 : return;
471 : }
472 :
473 :
474 : /*--------------------------------------------------------------------------*
475 : * apply_gain()
476 : *
477 : * Apply gain
478 : *--------------------------------------------------------------------------*/
479 :
480 57659 : 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 900818 : FOR( band = 0; band < num_sfm; band++ )
493 : {
494 843159 : g = gains[ord[band]];
495 843159 : move16();
496 15510735 : FOR( i = band_start[band]; i < band_end[band]; i++ )
497 : {
498 : /*xq[i] *= g; */
499 14667576 : xq[i] = mult_r( g, xq[i] );
500 14667576 : move16(); /*12+15+1-16=12 */
501 : }
502 : }
503 :
504 57659 : return;
505 : }
506 :
507 : /*--------------------------------------------------------------------------*
508 : * fine_gain_quant_fx()
509 : *
510 : * Fine gain quantization
511 : *--------------------------------------------------------------------------*/
512 29371 : 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 29371 : Flag Overflow = 0;
532 29371 : move32();
533 : #endif
534 :
535 462958 : FOR( band = 0; band < num_sfm; band++ )
536 : {
537 433587 : gbits = gain_bits[ord[band]]; /* Q0 */
538 433587 : move16();
539 433587 : test();
540 433587 : IF( fg_pred[band] != 0 && gbits > 0 )
541 : {
542 34427 : exp1 = norm_s( gopt[band] );
543 34427 : exp1 = sub( exp1, 1 );
544 34427 : tmp1 = shl( gopt[band], exp1 );
545 34427 : exp2 = norm_s( fg_pred[band] );
546 34427 : tmp2 = shl( fg_pred[band], exp2 ); /* Q12 + exp2 */
547 34427 : exp1 = add( 15, sub( exp1, exp2 ) );
548 34427 : err = div_s( tmp1, tmp2 ); /* Q15 */
549 34427 : tmp1 = norm_s( err );
550 34427 : exp2 = Log2_norm_lc( L_deposit_h( shl( err, tmp1 ) ) );
551 34427 : tmp1 = sub( 14, tmp1 );
552 34427 : tmp1 = sub( tmp1, exp1 );
553 34427 : L_tmp = L_Comp( tmp1, exp2 );
554 34427 : Mpy_32_16_ss( L_tmp, 24660, &L_tmp, &lsb ); /* 24660 = 20*log10(2) in Q12 */ /*16+12-15=13 */
555 34427 : gain_db = round_fx_sat( L_shl_o( L_tmp, 17, &Overflow ) );
556 :
557 34427 : idx = squant_fx( gain_db, &gain_dbq, finegain_fx[gbits - 1], gain_cb_size[gbits - 1] );
558 34427 : push_indice( hBstr, IND_PVQ_FINE_GAIN, idx, gbits );
559 :
560 34427 : L_tmp = L_mult0( gain_dbq, 21771 ); /* 21771=0.05*log2(10) */ /* 14+17=31 */
561 34427 : L_tmp = L_shr( L_tmp, 15 ); /* Q16 */
562 34427 : tmp1 = L_Extract_lc( L_tmp, &exp1 );
563 34427 : tmp1 = abs_s( tmp1 );
564 34427 : tmp1 = extract_l( Pow2( 14, tmp1 ) );
565 34427 : exp1 = sub( 14, exp1 );
566 :
567 34427 : L_tmp = L_mult0( fg_pred[band], tmp1 ); /*12+exp1 */
568 34427 : fg_pred[band] = round_fx_sat( L_shl_sat( L_tmp, sub( 16, exp1 ) ) ); /*12+exp1+16-exp1-16=12 */
569 34427 : move16();
570 : }
571 : }
572 :
573 29371 : return;
574 : }
575 :
576 : /*-------------------------------------------------------------------*
577 : * srt_vec_ind()
578 : *
579 : * sort vector and save sorting indeces
580 : *-------------------------------------------------------------------*/
581 :
582 604923 : 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 1538049 : FOR( pos = 0; pos < length; pos++ )
595 : {
596 933126 : I[pos] = pos; /* Q0 */
597 933126 : move16();
598 : }
599 :
600 604923 : Copy( linear, srt, length ); /* Q3 */
601 :
602 : /* now iterate */
603 933126 : FOR( pos = 0; pos < ( length - 1 ); pos++ )
604 : {
605 1442281 : FOR( npos = ( pos + 1 ); npos < length; npos++ )
606 : {
607 1114078 : IF( LT_16( srt[npos], srt[pos] ) )
608 : {
609 517414 : idxMem = I[pos]; /* Q0 */
610 517414 : move16();
611 517414 : I[pos] = I[npos]; /* Q0 */
612 517414 : move16();
613 517414 : I[npos] = idxMem; /* Q0 */
614 517414 : move16();
615 :
616 517414 : valMem = srt[pos]; /* Q0 */
617 517414 : move16();
618 517414 : srt[pos] = srt[npos]; /* Q0 */
619 517414 : move16();
620 517414 : srt[npos] = valMem; /* Q0 */
621 517414 : move16();
622 : }
623 : }
624 : }
625 :
626 604923 : 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 6838242 : 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 6838242 : IF( x == 0 )
650 : {
651 54808 : return 25736; /* PI/2 in Q14 */
652 : }
653 6783434 : man = ratio( y, x, &expo ); /* man in Q14 */
654 6783434 : expo = sub( expo, ( 15 - 14 ) ); /* Now, man is considered in Q15 */
655 6783434 : arg = L_shr( (Word32) man, expo );
656 :
657 6783434 : 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 401832 : expo = norm_l( arg );
666 401832 : man = extract_h( L_shl( arg, expo ) );
667 401832 : reciprocal = div_s( 0x3fff, man );
668 401832 : expo = sub( 15 + 1, expo );
669 401832 : reciprocal = shr( reciprocal, expo ); /* Q14 */
670 401832 : 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 401832 : if ( LT_32( L_shr( arg, 15 ), 10L ) )
676 : {
677 83847 : angle = add( angle, 8 ); /* Add tiny correction term. */
678 : }
679 : }
680 6381602 : 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 261254 : w = extract_l( L_shr( arg, 3 ) ); /* Q15 y = x/8 */
696 261254 : acc = L_add( 533625337L, 0 ); /* Q31 c1 = a1*8^2 */
697 261254 : z = mac_r( acc, w, -2161 ); /* Q15 c0 = a0*8^3 */
698 261254 : acc = L_add( -797517542L, 0 ); /* Q31 c2 = a2*8 */
699 261254 : z = mac_r( acc, w, z ); /* Q15 */
700 261254 : acc = L_add( 592675551L, 0 ); /* Q31 c3 = a3 */
701 261254 : z = mac_r( acc, w, z ); /* z (in:Q15, out:Q12) */
702 261254 : acc = L_add( 201114012L, 0 ); /* Q28 c4 = a4 */
703 261254 : acc = L_mac( acc, w, z ); /* Q28 */
704 261254 : angle = extract_l( L_shr( acc, ( 28 - 14 ) ) ); /* Q14 result of atan(x), where 4 <= x < 8 */
705 : }
706 6120348 : 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 413180 : w = extract_l( L_shr( arg, 2 ) ); /* Q15 y = x/4 */
723 413180 : acc = L_add( 702602883L, 0 ); /* Q31 c1 = a1*8 */
724 413180 : z = mac_r( acc, w, -2751 ); /* Q15 c0 = a0*32 */
725 413180 : acc = L_add( -1100849465L, 0 ); /* Q31 c2 = a2*2 */
726 413180 : z = mac_r( acc, w, z ); /* z (in:Q15, out:Q14) */
727 413180 : acc = L_add( 877095185L, 0 ); /* Q30 c3 = a3 */
728 413180 : z = mac_r( acc, w, z ); /* z (in:Q14, out:Q12) */
729 413180 : acc = L_add( 57332634L, 0 ); /* Q28 c4 = a4 */
730 413180 : acc = L_mac( acc, w, z ); /* Q28 */
731 413180 : angle = extract_l( L_shr( acc, ( 28 - 14 ) ) ); /* Q14 result of atan(x) where 2 <= x < 4 */
732 : }
733 5707168 : 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 1063466 : w = extract_l( L_shr( arg, 1 ) ); /* Q15 y= x/2 */
750 1063466 : acc = L_add( 655887249L, 0 ); /* Q31 c1 = a1*2 */
751 1063466 : z = mac_r( acc, w, -2106 ); /* Q15 c0 = a0*4 */
752 1063466 : acc = L_add( -1314948992L, 0 ); /* Q31 c2 = a2 */
753 1063466 : z = mac_r( acc, w, z );
754 1063466 : acc = L_add( 1428924557L, 0 ); /* Q31 c3 = a3/2 */
755 1063466 : z = mac_r( acc, w, z ); /* z (in:Q15, out:Q13) */
756 1063466 : acc = L_add( -37424701L, 0 ); /* Q29 c4 = a4 */
757 1063466 : acc = L_mac( acc, w, z ); /* Q29 */
758 1063466 : 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 4643702 : w = extract_l( arg ); /* Q15 */
775 4643702 : acc = L_add( 1188376431L, 0 ); /* Q31 c1 = a1*2 */
776 4643702 : z = mac_r( acc, w, -3547 ); /* Q15 c0 = a0*2 */
777 4643702 : acc = L_add( -995054571L, 0 ); /* Q31 c2 = a2 */
778 4643702 : z = extract_h( L_mac0( acc, w, z ) ); /* Q15 non-fractional mode multiply */
779 4643702 : acc = L_add( 61673254L, 0 ); /* Q31 c3 = a3 */
780 4643702 : z = mac_r( acc, w, z );
781 4643702 : acc = L_add( 2141982059L, 0 ); /* Q31 c4 = a4 */
782 4643702 : z = mac_r( acc, w, z );
783 4643702 : acc = L_add( 115139L, 0 ); /* Q31 c5 = a5 */
784 4643702 : acc = L_mac( acc, w, z ); /* Q31 */
785 4643702 : angle = extract_l( L_shr( acc, 31 - 14 ) ); /* Q14 result of atan(x), where 0 <= x < 1 */
786 : }
787 :
788 6783434 : return angle; /* Q14 between 0 and PI/2 radian. */
789 : }
|
