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 "cnst.h" /* Common constants */
7 : #include "rom_com_fx.h" /* Static table prototypes */
8 : #include "rom_com.h" /* Static table prototypes */
9 : #include "rom_enc.h" /* Static table prototypes */
10 : #include "prot_fx.h" /* Function prototypes */
11 : #include "prot_fx_enc.h" /* Function prototypes */
12 : //#include "basop_mpy.h"
13 :
14 : /*-------------------------------------------------------------------*
15 : * Local constants
16 : *-------------------------------------------------------------------*/
17 :
18 : #define NMAX 8 /* Control of the routine's complexity */
19 : /* #define FAC_DELTA 16.0f */
20 : #define SFAC_DELTA 11
21 :
22 : /*-------------------------------------------------------------------*
23 : * Local functions
24 : *-------------------------------------------------------------------*/
25 :
26 : static Word16 cod_2pos_fx( const Word16 ind1, const Word16 ind2, const Word16 sign1, const Word16 sign2, const Word16 n );
27 :
28 : static void gauss2v_fx( BSTR_ENC_HANDLE hBstr, const Word16 h[], const Word16 xn[], const Word16 dn[], Word16 code[], Word16 y1[], Word32 *gain, const Word16 lg, const Word16 shift, const Word16 Q_new, const Word16 nb_bits );
29 :
30 : static void gauss2v_ivas_fx( BSTR_ENC_HANDLE hBstr, const Word16 h[], const Word16 xn[], const Word16 dn[], Word16 code[], Word16 y1[], Word32 *gain, const Word16 lg, const Word16 shift, const Word16 Q_new, const Word16 nb_bits );
31 :
32 : /*-------------------------------------------------------------------*
33 : * Gaus_encode
34 : *
35 : * Encoder UnVoiced excitation coding using Gaussian codebooks
36 : * - ACELP quantized Gaussian excitation
37 : * - gain quantization
38 : * - Total excitation for UnVoiced coders
39 : * - Updates
40 : *-------------------------------------------------------------------*/
41 0 : Word16 gaus_encode_fx(
42 : Encoder_State *st_fx, /* i/o: encoder state structure */
43 : const Word16 i_subfr, /* i : subframe index Q0*/
44 : const Word16 *h1, /* i : weighted filter input response Q14*/
45 : const Word16 *xn, /* i : target vector Q12*/
46 : Word16 *exc, /* o : pointer to excitation signal frame Q_new*/
47 : Word16 *mem_w0, /* o : weighting filter denominator memory Q_new*/
48 : Word16 *clip_gain, /* o : memory of gain of pitch clipping algorithm [2.56x,Q14,Q8,Q0,Q14,Q14]*/
49 : Word16 *tilt_code, /* o : synthesis excitation spectrum tilt Q15*/
50 : Word16 *code, /* o : algebraic excitation Q9*/
51 : Word32 *gain_code, /* o : Code gain. Q16*/
52 : Word16 *y2, /* o : zero-memory filtered adaptive excitation Q9*/
53 : Word16 *gain_inov, /* o : innovation gain Q12*/
54 : Word16 *voice_fac, /* o : voicing factor Q15*/
55 : Word16 *gain_pit, /* o : adaptive excitation gain Q14*/
56 : const Word16 Q_new, /* i : scaling factor */
57 : const Word16 shift, /* i : scaling factor */
58 : Word32 *norm_gain_code /* o : normalized innovative cb. gain Q16*/
59 : )
60 : {
61 : Word16 nb_bits, idx;
62 0 : Word16 i = 0;
63 : Word32 Ltmp;
64 : Word16 dn[L_SUBFR], exp_code, gcode; /* Correlation between xn and h1 */
65 : Word16 exp, tmp, tmp_idx;
66 : #ifdef BASOP_NOGLOB_DECLARE_LOCAL
67 0 : Flag Overflow = 0;
68 0 : move32();
69 : #endif
70 :
71 : /*----------------------------------------------------------------*
72 : * Encode gaussian excitation
73 : *----------------------------------------------------------------*/
74 :
75 : /* Correlation between target xn2[] and impulse response h1[] */
76 0 : corr_xh_fx( xn, dn, h1 );
77 :
78 0 : tmp_idx = shr( i_subfr, 6 );
79 0 : nb_bits = st_fx->acelp_cfg.fixed_cdk_index[tmp_idx]; /* Q0 */
80 0 : move16();
81 :
82 0 : gauss2v_fx( st_fx->hBstr, h1, xn, dn, code, y2, gain_code, L_SUBFR, shift, Q_new, shr( nb_bits, 1 ) );
83 :
84 : /*----------------------------------------------------------------*
85 : * Encode gaussian gain
86 : *----------------------------------------------------------------*/
87 :
88 : /* codeword energy computation */
89 0 : Ltmp = Dot_product12( code, code, L_SUBFR, &exp_code );
90 :
91 0 : exp_code = sub( exp_code, 18 + 6 ); /* exp: -18 (code in Q9), -6 (L_subfr = 64) */
92 0 : Ltmp = Isqrt_lc( Ltmp, &exp_code );
93 0 : *gain_inov = extract_h( L_shl_sat( Ltmp, sub( exp_code, 3 ) ) ); /* g_code_inov in Q12 */
94 :
95 0 : nb_bits = st_fx->acelp_cfg.gains_mode[tmp_idx]; /* Q0 */
96 0 : move16();
97 : /* low bound = -30; stepSize = 1.71875; inv_stepSize = 0.5818181 */
98 0 : idx = gain_enc_gaus_fx( gain_code, nb_bits, -7680, 28160, 19065 ); /* Q0 */
99 0 : push_indice( st_fx->hBstr, IND_GAIN, idx, nb_bits );
100 :
101 : /*----------------------------------------------------------------*
102 : * Total excitation for Unvoiced coders
103 : *----------------------------------------------------------------*/
104 0 : gcode = round_fx_o( L_shl_o( *gain_code, Q_new, &Overflow ), &Overflow ); /* scaled gain_code with Qnew */
105 0 : FOR( i = 0; i < L_SUBFR; i++ )
106 : {
107 0 : exc[i + i_subfr] = round_fx( L_shl( L_mult( gcode, code[i] ), 15 - 9 ) ); /* Q_new */
108 : }
109 :
110 : /*----------------------------------------------------------------*
111 : * Updates: last value of new target is stored in mem_w0
112 : *----------------------------------------------------------------*/
113 :
114 0 : Ltmp = L_mult( gcode, y2[L_SUBFR - 1] ); /* Q_new + 10 */
115 0 : Ltmp = L_shl( Ltmp, add( 5, shift ) ); /* Q_new + 15 + shift */
116 0 : Ltmp = L_negate( Ltmp );
117 0 : Ltmp = L_mac( Ltmp, xn[L_SUBFR - 1], 16384 ); /* Q_new + 15 + shift */
118 0 : Ltmp = L_shl_sat( Ltmp, sub( 1, shift ) ); /* Q_new + 16 */
119 0 : *mem_w0 = round_fx_sat( Ltmp ); /* Q_new */
120 0 : move16();
121 0 : init_gp_clip_fx( clip_gain ); /* reset pitch clipping parameters */
122 :
123 0 : *gain_pit = 0;
124 0 : *tilt_code = 0;
125 0 : move16(); /* purely unvoiced */
126 0 : *voice_fac = -32768; /* -1 in Q31 */
127 0 : move16(); /* purely unvoiced */
128 0 : exp = sub( norm_s( *gain_inov ), 1 );
129 0 : exp = s_max( exp, 0 );
130 :
131 0 : tmp = div_s( shr( 8192, exp ), *gain_inov );
132 0 : *norm_gain_code = L_shr( Mult_32_16( *gain_code, tmp ), sub( 1, exp ) ); /* Q16 */
133 0 : move16();
134 :
135 0 : return ( L_SUBFR << 6 );
136 : }
137 :
138 0 : Word16 gaus_encode_ivas_fx(
139 : Encoder_State *st_fx, /* i/o: encoder state structure */
140 : const Word16 i_subfr, /* i : subframe index Q0*/
141 : const Word16 *h1, /* i : weighted filter input response Q14*/
142 : const Word16 *xn, /* i : target vector Q12*/
143 : Word16 *exc, /* o : pointer to excitation signal frame Q_new*/
144 : Word16 *mem_w0, /* o : weighting filter denominator memory Q_new*/
145 : Word16 *clip_gain, /* o : memory of gain of pitch clipping algorithm [2.56x,Q14,Q8,Q0,Q14,Q14]*/
146 : Word16 *tilt_code, /* o : synthesis excitation spectrum tilt Q15*/
147 : Word16 *code, /* o : algebraic excitation Q9*/
148 : Word32 *gain_code, /* o : Code gain. Q16*/
149 : Word16 *y2, /* o : zero-memory filtered adaptive excitation Q9*/
150 : Word16 *gain_inov, /* o : innovation gain Q12*/
151 : Word16 *voice_fac, /* o : voicing factor Q15*/
152 : Word16 *gain_pit, /* o : adaptive excitation gain Q14*/
153 : const Word16 Q_new, /* i : scaling factor */
154 : const Word16 shift, /* i : scaling factor */
155 : Word32 *norm_gain_code /* o : normalized innovative cb. gain Q16*/
156 : )
157 : {
158 : Word16 nb_bits, idx;
159 0 : Word16 i = 0;
160 0 : move16();
161 : Word32 Ltmp;
162 : Word16 dn[L_SUBFR], exp_code, gcode; /* Correlation between xn and h1 */
163 : Word16 exp, tmp, tmp_idx;
164 : #ifdef BASOP_NOGLOB_DECLARE_LOCAL
165 0 : Flag Overflow = 0;
166 0 : move32();
167 : #endif
168 :
169 : /*----------------------------------------------------------------*
170 : * Encode gaussian excitation
171 : *----------------------------------------------------------------*/
172 :
173 : /* Correlation between target xn2[] and impulse response h1[] */
174 0 : corr_xh_fx( xn, dn, h1 );
175 :
176 0 : tmp_idx = shr( i_subfr, 6 );
177 0 : nb_bits = st_fx->acelp_cfg.fixed_cdk_index[tmp_idx]; /* Q0 */
178 0 : move16();
179 :
180 0 : gauss2v_ivas_fx( st_fx->hBstr, h1, xn, dn, code, y2, gain_code, L_SUBFR, shift, Q_new, shr( nb_bits, 1 ) );
181 :
182 : /*----------------------------------------------------------------*
183 : * Encode gaussian gain
184 : *----------------------------------------------------------------*/
185 :
186 : /* codeword energy computation */
187 0 : Ltmp = Dot_product12( code, code, L_SUBFR, &exp_code );
188 :
189 0 : exp_code = sub( exp_code, 18 + 6 ); /* exp: -18 (code in Q9), -6 (L_subfr = 64) */
190 0 : Ltmp = Isqrt_lc( Ltmp, &exp_code );
191 0 : *gain_inov = extract_h( L_shl( Ltmp, sub( exp_code, 3 ) ) ); /* g_code_inov in Q12 */
192 :
193 0 : nb_bits = st_fx->acelp_cfg.gains_mode[tmp_idx]; /* Q0 */
194 0 : move16();
195 : /* low bound = -30; stepSize = 1.71875; inv_stepSize = 0.5818181 */
196 0 : idx = gain_enc_gaus_fx( gain_code, nb_bits, -7680, 28160, 19065 ); /* Q0 */
197 0 : push_indice( st_fx->hBstr, IND_GAIN, idx, nb_bits );
198 :
199 : /*----------------------------------------------------------------*
200 : * Total excitation for Unvoiced coders
201 : *----------------------------------------------------------------*/
202 0 : gcode = round_fx_o( L_shl_o( *gain_code, Q_new, &Overflow ), &Overflow ); /* scaled gain_code with Qnew */
203 0 : FOR( i = 0; i < L_SUBFR; i++ )
204 : {
205 0 : exc[i + i_subfr] = round_fx( L_shl( L_mult( gcode, code[i] ), 15 - 9 ) ); /* Q_new */
206 : }
207 :
208 : /*----------------------------------------------------------------*
209 : * Updates: last value of new target is stored in mem_w0
210 : *----------------------------------------------------------------*/
211 :
212 0 : Ltmp = L_mult( gcode, y2[L_SUBFR - 1] ); /* Q_new + 10 */
213 0 : Ltmp = L_shl( Ltmp, add( 5, shift ) ); /* Q_new + 15 + shift */
214 0 : Ltmp = L_negate( Ltmp );
215 0 : Ltmp = L_mac( Ltmp, xn[L_SUBFR - 1], 16384 ); /* Q_new + 15 + shift */
216 0 : Ltmp = L_shl_sat( Ltmp, sub( 1, shift ) ); /* Q_new + 16 */
217 0 : *mem_w0 = round_fx_sat( Ltmp ); /* Q_new */
218 0 : move16();
219 0 : init_gp_clip_fx( clip_gain ); /* reset pitch clipping parameters */
220 :
221 0 : *gain_pit = 0;
222 0 : *tilt_code = 0;
223 0 : move16(); /* purely unvoiced */
224 0 : *voice_fac = -32768; /* -1 in Q15 */
225 0 : move16(); /* purely unvoiced */
226 0 : exp = sub( norm_s( *gain_inov ), 1 );
227 0 : exp = s_max( exp, 0 );
228 :
229 0 : tmp = div_s( shr( 8192, exp ), *gain_inov );
230 0 : *norm_gain_code = L_shr( Mult_32_16( *gain_code, tmp ), sub( 1, exp ) ); /* Q16 */
231 0 : move16();
232 :
233 0 : return ( L_SUBFR << 6 );
234 : }
235 : /*-------------------------------------------------------------------*
236 : * gauss2v()
237 : *
238 : * encoder of Gaussian Codebook for unvoiced
239 : * consisting of addition of 2 Gaussian vectors
240 : *
241 : * One Gaussian vector of 192 values vectors delayed by 2
242 : *-------------------------------------------------------------------*/
243 0 : void gauss2v_fx(
244 : BSTR_ENC_HANDLE hBstr, /* i/o: encoder state structure */
245 : const Word16 h[], /* i : weighted LP filter impulse response Q15 */
246 : const Word16 xn[], /* i : target signal Q12 */
247 : const Word16 dn[], /* i : backward filtered target Q12 */
248 : Word16 code[], /* o : gaussian excitation Q9 */
249 : Word16 y1[], /* o : zero-memory filtered gauss. excitation Q8 */
250 : Word32 *gain, /* o : excitation gain. 32-bit number in Q16 */
251 : const Word16 lg, /* i : subframe size Q0 */
252 : const Word16 shift, /* i : Scaling factor Q0 */
253 : const Word16 Q_new, /* i : Scaling factor Q0 */
254 : const Word16 nb_bits /* i : nb ob bits per track (max 6) Q0 */
255 : )
256 : {
257 : Word16 i, j, ind1, ind2;
258 : Word16 nvec, step;
259 : Word32 cor, cora, dotprod;
260 : Word16 enerw;
261 : Word32 eneri, cor2;
262 : Word32 enerw32, cor2w32;
263 : Word16 *cpt1;
264 : Word16 *pt1, *pt2;
265 : Word32 max_val[NMAX + 1];
266 : Word16 *pos[NMAX + 1];
267 : Word32 sign[NMAX + 1];
268 : Word32 ener[NMAX + 1], corr[NMAX + 1], ener1;
269 : Word16 dico2[L_SUBFR * NMAX];
270 : Word16 exp_num;
271 : Word16 exp_den;
272 : Word16 Num;
273 : Word16 Den;
274 : Word32 GainPortion1;
275 : Word32 GainPortion2;
276 : Word32 cor_abs;
277 : Word16 cor_neg;
278 : Word16 div_result;
279 : Word32 ener_sqrt;
280 : Word32 Portion;
281 : Word16 sign1, sign2;
282 : Word16 enerw_norm, enerw_mantissa;
283 : Word16 cor2w_norm, cor2w_mantissa;
284 : Word16 eneri_norm, eneri_mantissa;
285 : Word16 cor2_norm, cor2_mantissa;
286 : Word16 difference_norm;
287 : Word32 cor32; /* 32-bit intermediate value*/
288 : Word16 hi1, lo1;
289 : Word16 update_best;
290 : Word16 idx;
291 : Word32 Lc0, Lc1, Lnum, Lden;
292 : Word16 gxx, gcc, index_delta, delta, m_sign, inv_delta;
293 : Word16 hg[190], Gaus_dico2[190];
294 : Word16 shiftP3;
295 : #ifdef BASOP_NOGLOB_DECLARE_LOCAL
296 0 : Flag Overflow = 0;
297 0 : move32();
298 : #endif
299 :
300 : /*----------------------------------------------------------------*
301 : * Encode the tilt of gaussian excitation
302 : *----------------------------------------------------------------*/
303 :
304 : /* Compute spectral tilt of target */
305 0 : Lc0 = L_mult( xn[1], xn[1] ); /* Q25 */
306 0 : Lc1 = L_mult( xn[1], xn[0] ); /* Q25 */
307 0 : FOR( i = 2; i < L_SUBFR; i++ )
308 : {
309 : /* fc0 += xn[i]*xn[i] */
310 : /* fc1 += xn[i]*xn[i-1] */
311 0 : Lc0 = L_mac_sat( Lc0, xn[i], xn[i] ); /* Q25 */
312 0 : Lc1 = L_mac_sat( Lc1, xn[i], xn[i - 1] ); /* Q25 */
313 : }
314 : /* fgxx = fc1/fc0 */
315 0 : exp_num = sub( norm_l( Lc1 ), 1 );
316 0 : Num = extract_h( L_shl( Lc1, exp_num ) );
317 0 : m_sign = s_or( shr( Num, 16 ), 1 ); /* Remove sign */
318 0 : Num = abs_s( Num );
319 0 : Lc0 = L_max( Lc0, 1 );
320 0 : exp_den = norm_l( Lc0 );
321 0 : Den = extract_h( L_shl( Lc0, exp_den ) ); /* Q9 + exp_den */
322 0 : gxx = shr( div_s( Num, Den ), sub( exp_num, sub( exp_den, 2 ) ) ); /* Q13 */
323 0 : gxx = i_mult2( gxx, m_sign ); /* Apply sign */
324 :
325 0 : set16_fx( hg, 0, 190 ); /* Compute spectral tilt of filtered codebook */
326 0 : Copy( h, hg, L_SUBFR ); // Q15
327 0 : conv_fx( gaus_dico_fx, hg, Gaus_dico2, 190 ); // Q12
328 :
329 0 : Lc0 = L_mult( Gaus_dico2[1], Gaus_dico2[1] ); /* Q25 */
330 0 : Lc1 = L_mult( Gaus_dico2[1], Gaus_dico2[0] ); /* Q25 */
331 0 : FOR( i = 2; i < 190; i++ )
332 : {
333 : /* fc0 += fgaus_dico2[i]*fgaus_dico2[i] */
334 : /* fc1 += fgaus_dico2[i]*fgaus_dico2[i-1] */
335 0 : Lc0 = L_mac( Lc0, Gaus_dico2[i], Gaus_dico2[i] ); /* Q25 */
336 0 : Lc1 = L_mac( Lc1, Gaus_dico2[i], Gaus_dico2[i - 1] ); /* Q25 */
337 : }
338 :
339 : /* fgcc = fc1/fc0 */
340 :
341 0 : exp_num = sub( norm_l( Lc1 ), 1 );
342 0 : Num = extract_h( L_shl( Lc1, exp_num ) );
343 0 : m_sign = s_or( shr( Num, 16 ), 1 ); /* Remove sign */
344 0 : Num = abs_s( Num );
345 :
346 0 : Lc0 = L_max( Lc0, 1 );
347 0 : exp_den = norm_l( Lc0 );
348 0 : Den = extract_h( L_shl( Lc0, exp_den ) );
349 0 : gcc = shr( div_s( Num, Den ), sub( exp_num, sub( exp_den, 2 ) ) ); /* Q13 */
350 0 : gcc = i_mult2( gcc, m_sign ); /* Apply sign */
351 :
352 : /* fdelta = (1-fgcc*fgxx) / (2*fgcc+fgxx) Compute and quantize spectral tilt modification factor */
353 0 : Lnum = L_sub( 134217728L, L_mult( gcc, gxx ) ); /* Q30 */
354 0 : Lden = L_mac( L_mult( gxx, 8192 ), gcc, 16384 ); /* Q30 */
355 :
356 0 : exp_num = sub( norm_l( Lnum ), 1 );
357 0 : Num = extract_h( L_shl( Lnum, exp_num ) ); /* Q14 + exp_den */
358 0 : m_sign = s_or( shr( Num, 16 ), 1 ); /* Remove sign */
359 0 : Num = abs_s( Num );
360 :
361 0 : Lden = L_max( Lden, 1 );
362 0 : exp_den = norm_l( Lden );
363 0 : Den = extract_h( L_shl( Lden, exp_den ) );
364 :
365 0 : delta = shr_o( div_s( Num, Den ), sub( exp_num, exp_den ), &Overflow ); /* Q15 */
366 0 : delta = i_mult2( delta, m_sign ); /* Apply sign Q0*/
367 : /* index_delta = (short)(FAC_DELTA * fdelta) */
368 0 : index_delta = shr( delta, SFAC_DELTA );
369 :
370 : /* index_delta [0,7] */
371 0 : index_delta = s_max( index_delta, 0 );
372 0 : index_delta = s_min( index_delta, 7 );
373 :
374 : /* fdelta = STEP_DELTA * (float)index_delta */
375 0 : delta = shl( index_delta, 11 ); /* delta in Q15 */
376 :
377 0 : IF( delta > 0 ) /* Adapt spectral tilt of initial codebook */
378 : {
379 : /* Computation of 1 / (1+fdelta*fdelta) */
380 0 : inv_delta = inv_delta_tab[sub( index_delta, 1 )];
381 0 : move16(); /* Q15 */
382 :
383 : /* fgaus_dico2[0] = gaus_dico[0] */
384 0 : Gaus_dico2[0] = gaus_dico_fx[0]; /* Q12 */
385 0 : move16();
386 0 : FOR( i = 1; i < 190; i++ )
387 : {
388 : /* fgaus_dico2[i] = (gaus_dico[i] - fdelta*gaus_dico[i-1]) / (1 + fdelta*fdelta) */
389 0 : Lnum = L_msu( L_deposit_h( gaus_dico_fx[i] ), delta, gaus_dico_fx[i - 1] );
390 0 : Gaus_dico2[i] = round_fx( Mpy_32_16_1( Lnum, inv_delta ) );
391 : }
392 : }
393 : ELSE
394 : {
395 0 : FOR( i = 0; i < 190; i++ )
396 : {
397 : /* fgaus_dico2[i] = gaus_dico[i] */
398 0 : Gaus_dico2[i] = gaus_dico_fx[i]; /* Q12 */
399 0 : move16();
400 : }
401 : }
402 :
403 : /*----------------------------------------------------------------*
404 : * Initializations
405 : *----------------------------------------------------------------*/
406 :
407 0 : ind1 = 0;
408 0 : move16();
409 0 : ind2 = 0;
410 0 : move16();
411 :
412 0 : nvec = shl( 1, nb_bits );
413 0 : step = shr( 0x80, nb_bits );
414 :
415 : /*----------------------------------------------------------------*
416 : * dot product between dn and gaussian codevectors,
417 : * keep NMAX best vectors
418 : *----------------------------------------------------------------*/
419 :
420 0 : set32_fx( max_val, 0, NMAX + 1 );
421 0 : set32_fx( sign, 0, NMAX + 1 );
422 :
423 0 : FOR( i = 0; i < NMAX + 1; i++ )
424 : {
425 0 : pos[i] = (Word16 *) Gaus_dico2;
426 : }
427 :
428 0 : cpt1 = Gaus_dico2;
429 0 : move16();
430 :
431 0 : FOR( i = 0; i < nvec; i++ )
432 : {
433 : /* Dot product without normalization, because values are compared with each other afterwards. */
434 0 : cor = Dot_product( cpt1, dn, lg ); /* Q12 * Q12 * length of 64 + 1 left shift ==> Q31*/
435 0 : cora = L_abs( cor );
436 0 : j = NMAX - 1;
437 0 : move16();
438 :
439 : DO
440 : {
441 0 : IF( GE_32( cora, max_val[j] ) )
442 : {
443 0 : max_val[j + 1] = max_val[j];
444 0 : move32(); /*Q31*/
445 0 : pos[j + 1] = pos[j];
446 0 : move16(); /*Pointer*/
447 0 : sign[j + 1] = sign[j];
448 0 : move32(); /*Q31*/
449 0 : max_val[j] = cora;
450 0 : move32(); /*Q31*/
451 0 : pos[j] = cpt1;
452 0 : move16(); /*Pointer*/
453 0 : sign[j] = cor;
454 0 : move32(); /*Q31*/
455 : }
456 0 : j--;
457 : }
458 0 : WHILE( j >= 0 );
459 0 : cpt1 += step;
460 : }
461 :
462 : /*----------------------------------------------------------------*
463 : * filter selected vectors
464 : * put sign
465 : * compute energy
466 : *----------------------------------------------------------------*/
467 :
468 0 : pt1 = dico2;
469 0 : move16();
470 0 : FOR( i = 0; i < NMAX; i++ )
471 : {
472 : /* Input vector (pos) Q12, filter coefs in Q15, result in same format as input vector (Q12) */
473 0 : conv_fx( pos[i], h, pt1, lg );
474 :
475 : /* put sign and compute energy */
476 0 : IF( sign[i] < 0 )
477 : {
478 0 : FOR( j = 0; j < lg; j++ )
479 : {
480 0 : pt1[j] = negate( pt1[j] );
481 0 : move16(); /*Store into dico2*/
482 : }
483 : }
484 0 : ener[i] = Dot_product( pt1, pt1, lg ); /* pt1 points to filtered vector in dico2, in Q12 */
485 0 : move32(); /* Result is for Q12 * Q12 with length of 64 (6 bits) + 1 left shift => Q31 */
486 0 : corr[i] = Dot_product( pt1, xn, lg ); /* must be equal to sign[i] !! */
487 0 : move32(); /* pt1 points into dico2, in Q12. xn is in Q12 */
488 : /* Result is for Q12 * Q12 with length of 64 (6 bits) + 1 left shift => Q31 */
489 0 : pt1 += L_SUBFR;
490 : }
491 :
492 : /*------------------------------------------------------------------------*
493 : * try all combinations of NMAX best vectors
494 : *------------------------------------------------------------------------*/
495 :
496 0 : pt1 = dico2;
497 0 : move16();
498 :
499 : /* Initial values for search algorithm */
500 0 : enerw32 = L_deposit_h( 0x80 );
501 0 : cor2w32 = L_deposit_l( -2 );
502 0 : enerw_norm = norm_l( enerw32 );
503 0 : cor2w_norm = norm_l( cor2w32 );
504 0 : cor2w_mantissa = round_fx( L_shl( cor2w32, cor2w_norm ) );
505 0 : enerw_mantissa = round_fx( L_shl( enerw32, enerw_norm ) );
506 :
507 0 : FOR( i = 0; i < NMAX; i++ )
508 : {
509 0 : pt2 = pt1;
510 0 : move16();
511 0 : FOR( j = i; j < NMAX; j++ )
512 : {
513 0 : cor32 = L_add( corr[i], corr[j] ); /* Q31 */
514 :
515 0 : dotprod = Dot_product( pt1, pt2, lg ); /* Q12 * Q12 * length of 64 + 1 left shift ==> Q31 */
516 :
517 : /* eneri = round_fx(ener[i]) + round_fx(ener[j]) + 2*round_fx(dotprod) */
518 : /* Use ScalingShift to stay aligned with ener[] */
519 0 : eneri = L_shl( dotprod, 1 ); /* One left shift added for factor of 2 */
520 0 : eneri = L_add( ener[i], eneri );
521 0 : eneri = L_add( ener[j], eneri ); /* Q31 */
522 :
523 0 : lo1 = L_Extract_lc( cor32, &hi1 );
524 0 : cor2 = Sad_32( 0, hi1, lo1 ); /* Square + Add */
525 :
526 0 : cor2_norm = norm_l( cor2 );
527 0 : eneri_norm = norm_l( eneri );
528 0 : cor2_mantissa = round_fx_o( L_shl_o( cor2, cor2_norm, &Overflow ), &Overflow );
529 0 : eneri_mantissa = round_fx_o( L_shl_o( eneri, eneri_norm, &Overflow ), &Overflow );
530 0 : difference_norm = sub( add( cor2_norm, enerw_norm ), add( cor2w_norm, eneri_norm ) );
531 :
532 0 : update_best = 0;
533 0 : move16();
534 :
535 0 : IF( difference_norm > 0 )
536 : {
537 0 : if ( GT_32( L_shr( L_mult( cor2_mantissa, enerw_mantissa ), difference_norm ),
538 : L_mult( cor2w_mantissa, eneri_mantissa ) ) )
539 : {
540 0 : update_best = 1;
541 0 : move16();
542 : }
543 : }
544 : ELSE
545 : {
546 0 : if ( L_msu_sat( L_shl( L_mult( cor2w_mantissa, eneri_mantissa ), difference_norm ), cor2_mantissa, enerw_mantissa ) < 0 ) // Saturation to be revisited
547 : {
548 0 : update_best = 1;
549 0 : move16();
550 : }
551 : }
552 0 : IF( update_best != 0 )
553 : {
554 0 : cor2w_mantissa = cor2_mantissa;
555 0 : move16();
556 0 : cor2w_norm = cor2_norm;
557 0 : move16();
558 0 : enerw_mantissa = eneri_mantissa;
559 0 : move16();
560 0 : enerw_norm = eneri_norm;
561 0 : move16();
562 0 : ind1 = i;
563 0 : move16();
564 0 : ind2 = j;
565 0 : move16();
566 : }
567 0 : pt2 += L_SUBFR;
568 : }
569 0 : pt1 += L_SUBFR;
570 : }
571 :
572 0 : enerw = round_fx( L_shr( L_deposit_h( enerw_mantissa ), enerw_norm ) );
573 :
574 : /*----------------------------------------------------------------*
575 : * Compute zero-memory filtered gauss. excitation y
576 : *----------------------------------------------------------------*/
577 :
578 0 : pt1 = dico2 + ind1 * L_SUBFR;
579 0 : move16(); /*Pointer arithmetic*/
580 0 : pt2 = dico2 + ind2 * L_SUBFR;
581 0 : move16();
582 :
583 0 : shiftP3 = add( shift, 3 );
584 0 : FOR( i = 0; i < lg; i++ )
585 : {
586 : /* Sum of 2 Q12 values, must give a Q1.8 */
587 0 : y1[i] = shr( add( pt1[i], pt2[i] ), shiftP3 );
588 0 : move16(); /* Compensate for "shift" */
589 : }
590 :
591 : /*----------------------------------------------------------------*
592 : * signs of vectors
593 : *----------------------------------------------------------------*/
594 :
595 0 : sign1 = ( -32768 );
596 0 : move16();
597 0 : if ( sign[ind1] >= 0 )
598 : {
599 0 : sign1 = 32767;
600 0 : move16();
601 : }
602 :
603 0 : sign2 = ( -32768 );
604 0 : move16();
605 0 : if ( sign[ind2] >= 0 )
606 : {
607 0 : sign2 = 32767;
608 0 : move16();
609 : }
610 :
611 : /*----------------------------------------------------------------*
612 : * Compute code
613 : *----------------------------------------------------------------*/
614 :
615 0 : pt1 = pos[ind1];
616 0 : move16(); /* Points to gaussian vector (gaus_dico_fx) in Q12 */
617 0 : pt2 = pos[ind2];
618 0 : move16(); /* Points to gaussian vector (gaus_dico_fx) in Q12 */
619 :
620 : /* sign[ind1] and sign[ind2] */
621 0 : FOR( i = 0; i < lg; i++ )
622 : {
623 : /* code[i]=(pt1[i]*sign1 + pt2[i]*sign2) /8 */
624 : /* Division by 8 (shift by 3) is for scaling (Q12 to Q0.9 output) */
625 0 : code[i] = shr( add( mult( pt1[i], sign1 ), mult( pt2[i], sign2 ) ), 3 );
626 0 : move16();
627 : }
628 :
629 0 : cor = L_add( corr[ind1], corr[ind2] );
630 :
631 : /*----------------------------------------------------------------*
632 : * Compute index
633 : *----------------------------------------------------------------*/
634 :
635 0 : i = (Word16) ( ( pos[ind1] - Gaus_dico2 ) / step ); /* Division by step can be replaced by shift. Pointer arithmetic */
636 0 : j = (Word16) ( ( pos[ind2] - Gaus_dico2 ) / step ); /* Division by step can be replaced by shift. Pointer arithmetic */
637 :
638 0 : idx = cod_2pos_fx( i, j, sign1, sign2, nvec ); /* Q0 */
639 0 : move16();
640 :
641 0 : push_indice( hBstr, IND_GAUS_CDBK_INDEX, idx, 2 * nb_bits + 1 );
642 0 : push_indice( hBstr, IND_TILT_FACTOR, index_delta, 3 );
643 :
644 : /*----------------------------------------------------------------*
645 : * Find quantized gain
646 : *----------------------------------------------------------------*/
647 :
648 : /* Divide cor/enerw: intermediate result stored into GainPortion1 */
649 0 : cor_neg = 0;
650 0 : move16();
651 0 : if ( cor < 0 ) /* Make Num positive. */
652 : {
653 0 : cor_neg = 1;
654 0 : move16();
655 : }
656 0 : cor_abs = L_abs( cor );
657 :
658 0 : exp_num = sub( norm_l( cor_abs ), 1 );
659 0 : exp_den = norm_s( enerw );
660 0 : Num = round_fx( L_shl( cor_abs, exp_num ) );
661 0 : Den = shl( enerw, exp_den );
662 :
663 0 : GainPortion1 = L_deposit_l( 0 ); /* Unexpected division by zero. Eliminate this gain contribution */
664 0 : IF( Den != 0 ) /* Protection against division by zero */
665 : {
666 0 : div_result = div_s( Num, Den ); /* Q15 */
667 0 : IF( cor_neg != 0 )
668 : {
669 0 : div_result = negate( div_result ); /* Retrieve sign */
670 : }
671 : /* Re-scale to compensate for normalization*/
672 0 : GainPortion1 = L_shr( L_deposit_l( div_result ), sub( exp_num, exp_den ) );
673 : }
674 :
675 0 : ener1 = Dot_product( xn, xn, lg ); /* Q12 * Q12 * length of 64 + 1 left shift ==> Q31 */
676 :
677 0 : exp_num = sub( norm_s( enerw ), 1 );
678 0 : exp_den = norm_l( ener1 );
679 0 : Num = shl( enerw, exp_num );
680 0 : Den = round_fx_sat( L_shl_sat( ener1, exp_den ) );
681 0 : GainPortion2 = L_deposit_l( 0 ); /* Unexpected division by zero. Eliminate this gain contribution */
682 0 : IF( Den != 0 ) /* Protection against division by zero */
683 : {
684 0 : div_result = div_s( Num, Den ); /* Q15 */
685 :
686 : /* Re-scale to compensate for normalization*/
687 0 : GainPortion2 = L_shr_sat( L_deposit_l( div_result ), sub( exp_num, exp_den ) );
688 : }
689 :
690 0 : ener_sqrt = Isqrt( L_shl_sat( GainPortion2, 1 ) ); /* Make value a Q16 prior to division (align on power of 4) */
691 0 : ener_sqrt = L_shr( ener_sqrt, 8 ); /* Left-shift Q23 result to make a Q15 result */
692 :
693 0 : Portion = Mult_32_16( GainPortion1, 19661 ); /* Performs GainPortion1*.6 */
694 0 : Portion = Madd_32_16( Portion, ener_sqrt, 13107 ); /* Performs ener_sqrt*.4 */
695 :
696 : /* Gain must be output in a 32-bit variable as a Q16 */
697 : /* Compensate for Q_new */
698 0 : *gain = L_shl_o( Portion, sub( 13, Q_new ), &Overflow ); /* Q16 */
699 0 : move32();
700 :
701 0 : return;
702 : }
703 :
704 0 : void gauss2v_ivas_fx(
705 : BSTR_ENC_HANDLE hBstr, /* i/o: encoder state structure */
706 : const Word16 h[], /* i : weighted LP filter impulse response Q15 */
707 : const Word16 xn[], /* i : target signal Q12 */
708 : const Word16 dn[], /* i : backward filtered target Q12 */
709 : Word16 code[], /* o : gaussian excitation Q9 */
710 : Word16 y1[], /* o : zero-memory filtered gauss. excitation Q8 */
711 : Word32 *gain, /* o : excitation gain. 32-bit number in Q16 */
712 : const Word16 lg, /* i : subframe size Q0 */
713 : const Word16 shift, /* i : Scaling factor Q0 */
714 : const Word16 Q_new, /* i : Scaling factor Q0 */
715 : const Word16 nb_bits /* i : nb ob bits per track (max 6) */
716 : )
717 : {
718 : Word16 i, j, ind1, ind2;
719 : Word16 nvec, step;
720 : Word32 cor, cora, dotprod;
721 : Word16 enerw;
722 : Word32 eneri, cor2;
723 : Word32 enerw32, cor2w32;
724 : Word16 *cpt1;
725 : Word16 *pt1, *pt2;
726 : Word32 max_val[NMAX + 1];
727 : Word16 *pos[NMAX + 1];
728 : Word32 sign[NMAX + 1];
729 : Word32 ener[NMAX + 1], corr[NMAX + 1], ener1;
730 : Word16 dico2[L_SUBFR * NMAX];
731 : Word16 exp_num;
732 : Word16 exp_den;
733 : Word16 Num;
734 : Word16 Den;
735 : Word32 GainPortion1;
736 : Word32 GainPortion2;
737 : Word32 cor_abs;
738 : Word16 cor_neg;
739 : Word16 div_result;
740 : Word32 ener_sqrt;
741 : Word32 Portion;
742 : Word16 sign1, sign2;
743 : Word16 enerw_norm, enerw_mantissa;
744 : Word16 cor2w_norm, cor2w_mantissa;
745 : Word16 eneri_norm, eneri_mantissa;
746 : Word16 cor2_norm, cor2_mantissa;
747 : Word16 difference_norm;
748 : Word32 cor32; /* 32-bit intermediate value*/
749 : Word16 hi1, lo1;
750 : Word16 update_best;
751 : Word16 idx;
752 : Word32 Lc0, Lc1, Lnum, Lden;
753 : Word16 gxx, gcc, index_delta, delta, m_sign, inv_delta;
754 : Word16 hg[190], Gaus_dico2[190];
755 : Word16 shiftP3;
756 : #ifdef BASOP_NOGLOB_DECLARE_LOCAL
757 0 : Flag Overflow = 0;
758 0 : move32();
759 : #endif
760 :
761 : /*----------------------------------------------------------------*
762 : * Encode the tilt of gaussian excitation
763 : *----------------------------------------------------------------*/
764 :
765 : /* Compute spectral tilt of target */
766 0 : Lc0 = L_mult( xn[1], xn[1] );
767 0 : Lc1 = L_mult( xn[1], xn[0] );
768 0 : FOR( i = 2; i < L_SUBFR; i++ )
769 : {
770 : /* fc0 += xn[i]*xn[i] */
771 : /* fc1 += xn[i]*xn[i-1] */
772 0 : Lc0 = L_mac_sat( Lc0, xn[i], xn[i] );
773 0 : Lc1 = L_mac_sat( Lc1, xn[i], xn[i - 1] );
774 : }
775 : /* fgxx = fc1/fc0 */
776 0 : exp_num = sub( norm_l( Lc1 ), 1 );
777 0 : Num = extract_h( L_shl( Lc1, exp_num ) );
778 0 : m_sign = s_or( shr( Num, 16 ), 1 ); /* Remove sign */
779 0 : Num = abs_s( Num );
780 0 : Lc0 = L_max( Lc0, 1 );
781 0 : exp_den = norm_l( Lc0 );
782 0 : Den = extract_h( L_shl( Lc0, exp_den ) );
783 0 : gxx = shr( div_s( Num, Den ), sub( exp_num, sub( exp_den, 2 ) ) ); /* Q13 */
784 0 : gxx = i_mult2( gxx, m_sign ); /* Apply sign */
785 :
786 0 : set16_fx( hg, 0, 190 ); /* Compute spectral tilt of filtered codebook */
787 0 : Copy( h, hg, L_SUBFR );
788 0 : conv_fx( gaus_dico_fx, hg, Gaus_dico2, 190 );
789 :
790 0 : Lc0 = L_mult( Gaus_dico2[1], Gaus_dico2[1] );
791 0 : Lc1 = L_mult( Gaus_dico2[1], Gaus_dico2[0] );
792 0 : FOR( i = 2; i < 190; i++ )
793 : {
794 : /* fc0 += fgaus_dico2[i]*fgaus_dico2[i] */
795 : /* fc1 += fgaus_dico2[i]*fgaus_dico2[i-1] */
796 0 : Lc0 = L_mac( Lc0, Gaus_dico2[i], Gaus_dico2[i] );
797 0 : Lc1 = L_mac( Lc1, Gaus_dico2[i], Gaus_dico2[i - 1] );
798 : }
799 :
800 : /* fgcc = fc1/fc0 */
801 :
802 0 : exp_num = sub( norm_l( Lc1 ), 1 );
803 0 : Num = extract_h( L_shl( Lc1, exp_num ) );
804 0 : m_sign = s_or( shr( Num, 16 ), 1 ); /* Remove sign */
805 0 : Num = abs_s( Num );
806 :
807 0 : Lc0 = L_max( Lc0, 1 );
808 0 : exp_den = norm_l( Lc0 );
809 0 : Den = extract_h( L_shl( Lc0, exp_den ) );
810 0 : gcc = shr( div_s( Num, Den ), sub( exp_num, sub( exp_den, 2 ) ) ); /* Q13 */
811 0 : gcc = i_mult2( gcc, m_sign ); /* Apply sign */
812 :
813 : /* fdelta = (1-fgcc*fgxx) / (2*fgcc+fgxx) Compute and quantize spectral tilt modification factor */
814 0 : Lnum = L_sub( 134217728L, L_mult( gcc, gxx ) ); /* Q30 */
815 0 : Lden = L_mac( L_mult( gxx, 8192 ), gcc, 16384 ); /* Q30 */
816 :
817 0 : exp_num = sub( norm_l( Lnum ), 1 );
818 0 : Num = extract_h( L_shl( Lnum, exp_num ) );
819 0 : m_sign = s_or( shr( Num, 16 ), 1 ); /* Remove sign */
820 0 : Num = abs_s( Num );
821 :
822 0 : Lden = L_max( Lden, 1 );
823 0 : exp_den = norm_l( Lden );
824 0 : Den = extract_h( L_shl( Lden, exp_den ) );
825 :
826 0 : delta = shr_o( div_s( Num, Den ), sub( exp_num, exp_den ), &Overflow ); /* Q15 */
827 0 : delta = i_mult2( delta, m_sign ); /* Apply sign */
828 : /* index_delta = (short)(FAC_DELTA * fdelta) */
829 0 : index_delta = shr( delta, SFAC_DELTA );
830 :
831 : /* index_delta [0,7] */
832 0 : index_delta = s_max( index_delta, 0 );
833 0 : index_delta = s_min( index_delta, 7 );
834 :
835 : /* fdelta = STEP_DELTA * (float)index_delta */
836 0 : delta = shl( index_delta, 11 ); /* delta in Q15 */
837 :
838 0 : IF( delta > 0 ) /* Adapt spectral tilt of initial codebook */
839 : {
840 : /* Computation of 1 / (1+fdelta*fdelta) */
841 0 : inv_delta = inv_delta_tab[sub( index_delta, 1 )];
842 0 : move16(); /* Q15 */
843 :
844 : /* fgaus_dico2[0] = gaus_dico[0] */
845 0 : Gaus_dico2[0] = gaus_dico_fx[0];
846 0 : move16();
847 0 : FOR( i = 1; i < 190; i++ )
848 : {
849 : /* fgaus_dico2[i] = (gaus_dico[i] - fdelta*gaus_dico[i-1]) / (1 + fdelta*fdelta) */
850 0 : Lnum = L_msu( L_deposit_h( gaus_dico_fx[i] ), delta, gaus_dico_fx[i - 1] );
851 0 : Gaus_dico2[i] = round_fx( Mpy_32_16_1( Lnum, inv_delta ) );
852 0 : move16();
853 : }
854 : }
855 : ELSE
856 : {
857 0 : FOR( i = 0; i < 190; i++ )
858 : {
859 : /* fgaus_dico2[i] = gaus_dico[i] */
860 0 : Gaus_dico2[i] = gaus_dico_fx[i];
861 0 : move16();
862 : }
863 : }
864 :
865 : /*----------------------------------------------------------------*
866 : * Initializations
867 : *----------------------------------------------------------------*/
868 :
869 0 : ind1 = 0;
870 0 : move16();
871 0 : ind2 = 0;
872 0 : move16();
873 :
874 0 : nvec = shl( 1, nb_bits );
875 0 : step = shr( 0x80, nb_bits );
876 :
877 : /*----------------------------------------------------------------*
878 : * dot product between dn and gaussian codevectors,
879 : * keep NMAX best vectors
880 : *----------------------------------------------------------------*/
881 :
882 0 : set32_fx( max_val, 0, NMAX + 1 );
883 0 : set32_fx( sign, 0, NMAX + 1 );
884 :
885 0 : FOR( i = 0; i < NMAX + 1; i++ )
886 : {
887 0 : pos[i] = (Word16 *) Gaus_dico2;
888 : }
889 :
890 0 : cpt1 = Gaus_dico2;
891 0 : move16();
892 :
893 0 : FOR( i = 0; i < nvec; i++ )
894 : {
895 : /* Dot product without normalization, because values are compared with each other afterwards. */
896 0 : cor = Dot_product( cpt1, dn, lg ); /* Q12 * Q12 * length of 64 + 1 left shift ==> Q31*/
897 0 : cora = L_abs( cor );
898 0 : j = NMAX - 1;
899 0 : move16();
900 :
901 : DO
902 : {
903 0 : IF( GE_32( cora, max_val[j] ) )
904 : {
905 0 : max_val[j + 1] = max_val[j];
906 0 : move32(); /*Q31*/
907 0 : pos[j + 1] = pos[j];
908 0 : move16(); /*Pointer*/
909 0 : sign[j + 1] = sign[j];
910 0 : move32(); /*Q31*/
911 0 : max_val[j] = cora;
912 0 : move32(); /*Q31*/
913 0 : pos[j] = cpt1;
914 0 : move16(); /*Pointer*/
915 0 : sign[j] = cor;
916 0 : move32(); /*Q31*/
917 : }
918 0 : j--;
919 : }
920 0 : WHILE( j >= 0 );
921 0 : cpt1 += step;
922 : }
923 :
924 : /*----------------------------------------------------------------*
925 : * filter selected vectors
926 : * put sign
927 : * compute energy
928 : *----------------------------------------------------------------*/
929 :
930 0 : pt1 = dico2;
931 0 : move16();
932 0 : FOR( i = 0; i < NMAX; i++ )
933 : {
934 : /* Input vector (pos) Q12, filter coefs in Q15, result in same format as input vector (Q12) */
935 0 : conv_fx( pos[i], h, pt1, lg );
936 :
937 : /* put sign and compute energy */
938 0 : IF( sign[i] < 0 )
939 : {
940 0 : FOR( j = 0; j < lg; j++ )
941 : {
942 0 : pt1[j] = negate( pt1[j] );
943 0 : move16(); /*Store into dico2*/
944 : }
945 : }
946 0 : ener[i] = Dot_product( pt1, pt1, lg ); /* pt1 points to filtered vector in dico2, in Q12 */
947 0 : move32(); /* Result is for Q12 * Q12 with length of 64 (6 bits) + 1 left shift => Q31 */
948 0 : corr[i] = Dot_product( pt1, xn, lg ); /* must be equal to sign[i] !! */
949 0 : move32(); /* pt1 points into dico2, in Q12. xn is in Q12 */
950 : /* Result is for Q12 * Q12 with length of 64 (6 bits) + 1 left shift => Q31 */
951 0 : pt1 += L_SUBFR;
952 : }
953 :
954 : /*------------------------------------------------------------------------*
955 : * try all combinations of NMAX best vectors
956 : *------------------------------------------------------------------------*/
957 :
958 0 : pt1 = dico2;
959 0 : move16();
960 :
961 : /* Initial values for search algorithm */
962 0 : enerw32 = L_deposit_h( 0x80 );
963 0 : cor2w32 = L_deposit_l( -2 );
964 0 : enerw_norm = norm_l( enerw32 );
965 0 : cor2w_norm = norm_l( cor2w32 );
966 0 : cor2w_mantissa = round_fx( L_shl( cor2w32, cor2w_norm ) );
967 0 : enerw_mantissa = round_fx( L_shl( enerw32, enerw_norm ) );
968 :
969 0 : FOR( i = 0; i < NMAX; i++ )
970 : {
971 0 : pt2 = pt1;
972 0 : move16();
973 0 : FOR( j = i; j < NMAX; j++ )
974 : {
975 0 : cor32 = L_add( corr[i], corr[j] ); /* Q31 */
976 :
977 0 : dotprod = Dot_product( pt1, pt2, lg ); /* Q12 * Q12 * length of 64 + 1 left shift ==> Q31 */
978 :
979 : /* eneri = round_fx(ener[i]) + round_fx(ener[j]) + 2*round_fx(dotprod) */
980 : /* Use ScalingShift to stay aligned with ener[] */
981 0 : eneri = L_shl( dotprod, 1 ); /* One left shift added for factor of 2 */
982 0 : eneri = L_add_sat( ener[i], eneri );
983 0 : eneri = L_add_sat( ener[j], eneri ); /* Q31 */
984 0 : lo1 = L_Extract_lc( cor32, &hi1 );
985 0 : cor2 = Sad_32( 0, hi1, lo1 ); /* Square + Add */
986 :
987 0 : cor2_norm = norm_l( cor2 );
988 0 : eneri_norm = norm_l( eneri );
989 0 : cor2_mantissa = round_fx_o( L_shl_o( cor2, cor2_norm, &Overflow ), &Overflow );
990 0 : eneri_mantissa = round_fx_o( L_shl_o( eneri, eneri_norm, &Overflow ), &Overflow );
991 0 : difference_norm = sub( add( cor2_norm, enerw_norm ), add( cor2w_norm, eneri_norm ) );
992 :
993 0 : update_best = 0;
994 0 : move16();
995 :
996 0 : IF( difference_norm > 0 )
997 : {
998 0 : if ( GT_32( L_shr( L_mult( cor2_mantissa, enerw_mantissa ), difference_norm ),
999 : L_mult( cor2w_mantissa, eneri_mantissa ) ) )
1000 : {
1001 0 : update_best = 1;
1002 0 : move16();
1003 : }
1004 : }
1005 : ELSE
1006 : {
1007 0 : if ( L_msu_sat( L_shl( L_mult( cor2w_mantissa, eneri_mantissa ), difference_norm ), cor2_mantissa, enerw_mantissa ) < 0 ) // Saturation to be revisited
1008 : {
1009 0 : update_best = 1;
1010 0 : move16();
1011 : }
1012 : }
1013 0 : IF( update_best != 0 )
1014 : {
1015 0 : cor2w_mantissa = cor2_mantissa;
1016 0 : move16();
1017 0 : cor2w_norm = cor2_norm;
1018 0 : move16();
1019 0 : enerw_mantissa = eneri_mantissa;
1020 0 : move16();
1021 0 : enerw_norm = eneri_norm;
1022 0 : move16();
1023 0 : ind1 = i;
1024 0 : move16();
1025 0 : ind2 = j;
1026 0 : move16();
1027 : }
1028 0 : pt2 += L_SUBFR;
1029 : }
1030 0 : pt1 += L_SUBFR;
1031 : }
1032 :
1033 0 : enerw = round_fx( L_shr( L_deposit_h( enerw_mantissa ), enerw_norm ) );
1034 :
1035 : /*----------------------------------------------------------------*
1036 : * Compute zero-memory filtered gauss. excitation y
1037 : *----------------------------------------------------------------*/
1038 :
1039 0 : pt1 = dico2 + ind1 * L_SUBFR;
1040 0 : move16(); /*Pointer arithmetic*/
1041 0 : pt2 = dico2 + ind2 * L_SUBFR;
1042 0 : move16();
1043 :
1044 0 : shiftP3 = add( shift, 3 );
1045 0 : FOR( i = 0; i < lg; i++ )
1046 : {
1047 : /* Sum of 2 Q12 values, must give a Q1.8 */
1048 0 : y1[i] = shr( add( pt1[i], pt2[i] ), shiftP3 );
1049 0 : move16(); /* Compensate for "shift" */
1050 : }
1051 :
1052 : /*----------------------------------------------------------------*
1053 : * signs of vectors
1054 : *----------------------------------------------------------------*/
1055 :
1056 0 : sign1 = ( -32768 );
1057 0 : move16();
1058 0 : if ( sign[ind1] >= 0 )
1059 : {
1060 0 : sign1 = 32767;
1061 0 : move16();
1062 : }
1063 :
1064 0 : sign2 = ( -32768 );
1065 0 : move16();
1066 0 : if ( sign[ind2] >= 0 )
1067 : {
1068 0 : sign2 = 32767;
1069 0 : move16();
1070 : }
1071 :
1072 : /*----------------------------------------------------------------*
1073 : * Compute code
1074 : *----------------------------------------------------------------*/
1075 :
1076 0 : pt1 = pos[ind1];
1077 0 : move16(); /* Points to gaussian vector (gaus_dico_fx) in Q12 */
1078 0 : pt2 = pos[ind2];
1079 0 : move16(); /* Points to gaussian vector (gaus_dico_fx) in Q12 */
1080 :
1081 : /* sign[ind1] and sign[ind2] */
1082 0 : FOR( i = 0; i < lg; i++ )
1083 : {
1084 : /* code[i]=(pt1[i]*sign1 + pt2[i]*sign2) /8 */
1085 : /* Division by 8 (shift by 3) is for scaling (Q12 to Q0.9 output) */
1086 0 : code[i] = shr( add( mult( pt1[i], sign1 ), mult( pt2[i], sign2 ) ), 3 );
1087 0 : move16();
1088 : }
1089 :
1090 0 : cor = L_add( corr[ind1], corr[ind2] );
1091 :
1092 : /*----------------------------------------------------------------*
1093 : * Compute index
1094 : *----------------------------------------------------------------*/
1095 :
1096 0 : i = (Word16) ( ( pos[ind1] - Gaus_dico2 ) / step ); /* Division by step can be replaced by shift. Pointer arithmetic */
1097 0 : j = (Word16) ( ( pos[ind2] - Gaus_dico2 ) / step ); /* Division by step can be replaced by shift. Pointer arithmetic */
1098 :
1099 0 : idx = cod_2pos_fx( i, j, sign1, sign2, nvec );
1100 0 : move16();
1101 :
1102 0 : push_indice( hBstr, IND_GAUS_CDBK_INDEX, idx, 2 * nb_bits + 1 );
1103 0 : push_indice( hBstr, IND_TILT_FACTOR, index_delta, 3 );
1104 :
1105 : /*----------------------------------------------------------------*
1106 : * Find quantized gain
1107 : *----------------------------------------------------------------*/
1108 :
1109 : /* Divide cor/enerw: intermediate result stored into GainPortion1 */
1110 0 : cor_neg = 0;
1111 0 : move16();
1112 0 : if ( cor < 0 ) /* Make Num positive. */
1113 : {
1114 0 : cor_neg = 1;
1115 0 : move16();
1116 : }
1117 0 : cor_abs = L_abs( cor );
1118 :
1119 0 : exp_num = sub( norm_l( cor_abs ), 1 );
1120 0 : exp_den = norm_s( enerw );
1121 0 : Num = round_fx( L_shl( cor_abs, exp_num ) );
1122 0 : Den = shl( enerw, exp_den );
1123 :
1124 0 : GainPortion1 = L_deposit_l( 0 ); /* Unexpected division by zero. Eliminate this gain contribution */
1125 0 : IF( Den != 0 ) /* Protection against division by zero */
1126 : {
1127 0 : div_result = div_s( Num, Den ); /* Q15 */
1128 0 : IF( cor_neg != 0 )
1129 : {
1130 0 : div_result = negate( div_result ); /* Retrieve sign */
1131 : }
1132 : /* Re-scale to compensate for normalization*/
1133 0 : GainPortion1 = L_shr( L_deposit_l( div_result ), sub( exp_num, exp_den ) );
1134 : }
1135 :
1136 0 : ener1 = Dot_product( xn, xn, lg ); /* Q12 * Q12 * length of 64 + 1 left shift ==> Q31 */
1137 :
1138 0 : exp_num = sub( norm_s( enerw ), 1 );
1139 0 : exp_den = norm_l( ener1 );
1140 0 : Num = shl( enerw, exp_num );
1141 0 : Den = round_fx_sat( L_shl_sat( ener1, exp_den ) );
1142 0 : GainPortion2 = L_deposit_l( 0 ); /* Unexpected division by zero. Eliminate this gain contribution */
1143 0 : IF( Den != 0 ) /* Protection against division by zero */
1144 : {
1145 0 : div_result = div_s( Num, Den ); /* Q15 */
1146 :
1147 : /* Re-scale to compensate for normalization*/
1148 0 : GainPortion2 = L_shr_sat( L_deposit_l( div_result ), sub( exp_num, exp_den ) );
1149 : }
1150 :
1151 0 : ener_sqrt = Isqrt( L_shl_sat( GainPortion2, 1 ) ); /* Make value a Q16 prior to division (align on power of 4) */
1152 0 : ener_sqrt = L_shr( ener_sqrt, 8 ); /* Left-shift Q23 result to make a Q15 result */
1153 :
1154 0 : Portion = Mult_32_16( GainPortion1, 19661 ); /* Performs GainPortion1*.6 */
1155 0 : Portion = Madd_32_16( Portion, ener_sqrt, 13107 ); /* Performs ener_sqrt*.4 */
1156 :
1157 : /* Gain must be output in a 32-bit variable as a Q16 */
1158 : /* Compensate for Q_new */
1159 0 : *gain = L_shl_o( Portion, sub( 13, Q_new ), &Overflow );
1160 0 : move32();
1161 :
1162 0 : return;
1163 : }
1164 :
1165 : /*---------------------------------------------------------------------*
1166 : * Put selected codevector positions and signs into quantization index
1167 : *---------------------------------------------------------------------*/
1168 0 : static Word16 cod_2pos_fx( /* o : codebook quantization index */
1169 : const Word16 ind1, /* i : index of 1st gaussian vector Q0*/
1170 : const Word16 ind2, /* i : index of 2nd gaussian vector Q0*/
1171 : const Word16 sign1, /* i : sign of 1st gaussian vector Qx*/
1172 : const Word16 sign2, /* i : sign of 2nd gaussian vector Qx*/
1173 : const Word16 n /* i : nb. of codebook vectors Q0*/
1174 : )
1175 : {
1176 : Word16 i1, i2, index, s1, s2;
1177 0 : s1 = 1;
1178 0 : move16();
1179 :
1180 0 : if ( sign1 > 0 )
1181 : {
1182 0 : s1 = 0;
1183 0 : move16();
1184 : }
1185 0 : s2 = 1;
1186 0 : move16();
1187 0 : if ( sign2 > 0 )
1188 : {
1189 0 : s2 = 0;
1190 0 : move16();
1191 : }
1192 :
1193 0 : IF( EQ_16( s1, s2 ) )
1194 : {
1195 0 : IF( LE_16( ind1, ind2 ) )
1196 : {
1197 0 : i1 = ind1; /* Q0 */
1198 0 : move16();
1199 0 : i2 = ind2; /* Q0 */
1200 0 : move16();
1201 : }
1202 : ELSE
1203 : {
1204 0 : i1 = ind2; /* Q0 */
1205 0 : move16();
1206 0 : i2 = ind1; /* Q0 */
1207 0 : move16();
1208 : }
1209 : }
1210 : ELSE
1211 : {
1212 0 : IF( GT_16( ind1, ind2 ) )
1213 : {
1214 0 : i1 = ind1; /* Q0 */
1215 0 : move16();
1216 0 : i2 = ind2; /* Q0 */
1217 0 : move16();
1218 : }
1219 : ELSE
1220 : {
1221 0 : i1 = ind2; /* Q0 */
1222 0 : move16();
1223 0 : i2 = ind1; /* Q0 */
1224 0 : move16();
1225 0 : s1 = s2;
1226 0 : move16();
1227 : }
1228 : }
1229 :
1230 0 : index = extract_l( L_mult( i1, n ) );
1231 0 : index = add( index, shl( i2, 1 ) );
1232 0 : index = add( index, s1 );
1233 :
1234 0 : return index;
1235 : }
|
