Line data Source code
1 : /*====================================================================================
2 : EVS Codec 3GPP TS26.452 Aug 12, 2021. Version 16.3.0
3 : ====================================================================================*/
4 :
5 : #include <stdint.h>
6 : #include "options.h" /* Compilation switches */
7 : #include "rom_enc.h"
8 : #include "rom_com.h" /* Static table prototypes */
9 : #include "prot_fx.h" /* Function prototypes */
10 : #include "prot_fx_enc.h" /* Function prototypes */
11 : #include "ivas_prot_fx.h" /* Function prototypes */
12 :
13 :
14 : /*-----------------------------------------------------------------*
15 : * Local constants
16 : *-----------------------------------------------------------------*/
17 :
18 : #define MIN_LOG_FX 0
19 : #define MIN_LOG_VAL_FX -15360 /* -60.0f in Q8 */
20 : #define MSVQ_MAXCNT 3000
21 : #define MAXINT32 MAX_32
22 :
23 : /*---------------------------------------------------------------------*
24 : * Local function prototypes
25 : *---------------------------------------------------------------------*/
26 :
27 : static void lsfq_CNG_fx( BSTR_ENC_HANDLE hBstr, const Word16 *lsf, const Word16 *wghts, Word16 *qlsf, Word32 *p_offset_scale1, Word32 *p_offset_scale2, Word16 *p_no_scales );
28 :
29 : static Word32 vq_lvq_lsf_enc( Word16 pred_flag, Word16 mode, Word16 u[], Word16 *levels, Word16 stages, Word16 w[], Word16 Idx[], const Word16 *lsf, const Word16 *pred, Word32 p_offset_scale1[][MAX_NO_SCALES + 1], Word32 p_offset_scale2[][MAX_NO_SCALES + 1], Word16 p_no_scales[][2], Word16 *resq, Word16 *lsfq );
30 :
31 : static Word32 vq_lvq_lsf_enc_ivas_fx( Word16 pred_flag, Word16 mode, Word16 u[], Word16 *levels, Word16 stages, Word16 w[], Word16 Idx[], const Word16 *lsf, const Word16 *pred, Word16 *resq, Word16 *lsfq );
32 :
33 : static void lsf_mid_enc_fx( BSTR_ENC_HANDLE hBstr, Word16 nb_bits, const Word16 int_fs, const Word16 qlsp0[], const Word16 qlsp1[], Word16 lsp[], const Word16 coder_type, const Word16 bwidth, Word32 Bin_Ener_old[], Word32 Bin_Ener[], Word16 Q_ener, Word16 ppp_mode, Word16 nelp_mode );
34 :
35 : static void lsf_mid_enc_ivas_fx( BSTR_ENC_HANDLE hBstr, Word16 nb_bits, const Word32 int_fs, const Word16 qlsp0[], const Word16 qlsp1[], Word16 lsp[], const Word16 coder_type, const Word16 bwidth, Word32 Bin_Ener_old[], Word16 Q_ener, Word16 ppp_mode, Word16 nelp_mode );
36 :
37 :
38 : /*===========================================================================*/
39 : /* FUNCTION : lsf_enc_fx() */
40 : /*---------------------------------------------------------------------------*/
41 : /* PURPOSE : Quantization of LSF vector */
42 : /*---------------------------------------------------------------------------*/
43 : /* INPUT ARGUMENTS : */
44 : /* _ (Word16) L_frame : length of the frame */
45 : /* _ (Word16) coder_type : coding type */
46 : /*---------------------------------------------------------------------------*/
47 : /* OUTPUT ARGUMENTS : */
48 : /* _ (Word16*) Aq : LP filter coefficient Q12 */
49 : /* _ (Word16*) lsf_ne w : LP filter coefficient Q(x2.56) */
50 : /* _ (Word16) st_fx->stab_fac_fx : LSF stability factor Q15 */
51 : /*---------------------------------------------------------------------------*/
52 : /* INPUT/OUTPUT ARGUMENTS : */
53 : /* _ (Struct) st_fx : state structure */
54 : /* _ (Word16*) lsp_new : LP filter coefficient Q15 */
55 : /* _ (Word16*) lsp_mid : LP filter coefficient Q15 */
56 : /* _ (Word16[]) st_fx->mem_AR_fx : AR memory of LSF quantizer */
57 : /* (past quantized LSFs without mean) x2.56 */
58 : /* _ (Word16[]) st_fx->clip_var_fx : pitch gain clipping memory x2.56*/
59 : /*---------------------------------------------------------------------------*/
60 :
61 : /*---------------------------------------------------------------------------*/
62 : /* RETURN ARGUMENTS : */
63 : /* _ None */
64 : /*===========================================================================*/
65 1423 : void lsf_enc_fx(
66 : Encoder_State *st_fx, /* i/o: state structure */
67 : Word16 *lsf_new, /* o : quantized LSF vector */
68 : Word16 *lsp_new, /* i/o: LSP vector to quantize/quantized */
69 : Word16 *lsp_mid, /* i/o : mid-frame LSP vector */
70 : Word16 *Aq, /* o : quantized A(z) for 4 subframes */
71 : const Word16 Nb_ACELP_frames,
72 : const Word16 tdm_low_rate_mode, /* i : secondary channel low rate mode flag */
73 : const Word16 GSC_IVAS_mode, /* i : GSC IVAS mode */
74 : const Word16 Q_new )
75 : {
76 1423 : Word16 nBits = 0;
77 1423 : move16();
78 : Word16 int_fs;
79 1423 : Word16 force_sf = 0;
80 1423 : move16();
81 : Word16 fec_lsf[M], stab, i;
82 : Word32 L_tmp;
83 : Word16 coder_type, ppp_mode, nelp_mode;
84 :
85 1423 : test();
86 1423 : IF( EQ_32( st_fx->core_brate, SID_2k40 ) || EQ_32( st_fx->core_brate, SID_1k75 ) )
87 : {
88 0 : coder_type = INACTIVE;
89 0 : move16();
90 : }
91 : ELSE
92 : {
93 1423 : coder_type = st_fx->coder_type;
94 1423 : move16();
95 : }
96 :
97 1423 : test();
98 1423 : if ( EQ_16( coder_type, AUDIO ) && GSC_IVAS_mode > 0 )
99 : {
100 0 : coder_type = GENERIC;
101 0 : move16();
102 : }
103 :
104 1423 : move16();
105 :
106 1423 : IF( st_fx->Opt_SC_VBR )
107 : {
108 0 : ppp_mode = st_fx->hSC_VBR->ppp_mode;
109 0 : nelp_mode = st_fx->hSC_VBR->nelp_mode;
110 0 : move16();
111 0 : move16();
112 : }
113 : ELSE
114 : {
115 1423 : ppp_mode = 0;
116 1423 : nelp_mode = 0;
117 1423 : move16();
118 1423 : move16();
119 : }
120 :
121 : /* initialize */
122 1423 : assert( st_fx->sr_core <= 32000 );
123 1423 : int_fs = extract_l( st_fx->sr_core );
124 1423 : move32();
125 : /* convert LSPs to LSFs */
126 1423 : lsp2lsf_fx( lsp_new, lsf_new, M, int_fs );
127 :
128 : /* check resonance for pitch clipping algorithm */
129 1423 : gp_clip_test_lsf_fx( st_fx->element_mode, lsf_new, st_fx->clip_var_fx, 0 );
130 :
131 : /* Find the number of bits for LSF quantization */
132 1423 : nBits = 0;
133 1423 : move16();
134 1423 : IF( EQ_32( st_fx->core_brate, SID_2k40 ) )
135 : {
136 0 : nBits = LSF_BITS_CNG;
137 0 : move16();
138 : }
139 : ELSE
140 : {
141 1423 : test();
142 1423 : IF( ( nelp_mode == 0 ) && ( ppp_mode == 0 ) )
143 : {
144 1423 : nBits = st_fx->acelp_cfg.lsf_bits;
145 1423 : move16();
146 : }
147 0 : ELSE IF( EQ_16( nelp_mode, 1 ) )
148 : {
149 0 : nBits = 30;
150 0 : move16();
151 :
152 0 : if ( EQ_16( st_fx->bwidth, NB ) )
153 : {
154 0 : nBits = 32;
155 0 : move16();
156 : }
157 : }
158 0 : ELSE IF( EQ_16( ppp_mode, 1 ) )
159 : {
160 0 : nBits = 26;
161 0 : move16();
162 : }
163 : }
164 1423 : force_sf = 0;
165 1423 : move16();
166 : /* first three ACELP frames after an HQ frame shall be processed only with safety-net quantizer */
167 1423 : test();
168 1423 : if ( LT_16( Nb_ACELP_frames, 3 ) && NE_32( st_fx->core_brate, SID_2k40 ) )
169 : {
170 65 : force_sf = 1;
171 65 : move16();
172 : }
173 :
174 : /* in case of unstable filter in decoder FEC, choose safety-net to help FEC */
175 1423 : IF( EQ_16( st_fx->next_force_safety_net, 1 ) )
176 : {
177 120 : force_sf = 1;
178 120 : move16();
179 120 : st_fx->next_force_safety_net = 0;
180 120 : move16();
181 : }
182 :
183 : /*-------------------------------------------------------------------------------------*
184 : * Frame end LSF quantization
185 : *-------------------------------------------------------------------------------------*/
186 :
187 1423 : lsf_end_enc_fx( st_fx, lsf_new, lsf_new, nBits, coder_type, Q_new + QSCALE - 2,
188 1423 : force_sf, NULL, NULL, NULL, st_fx->coder_type_raw );
189 :
190 : /* convert quantized LSFs back to LSPs */
191 1423 : lsf2lsp_fx( lsf_new, lsp_new, M, int_fs );
192 :
193 1423 : test();
194 1423 : IF( EQ_16( st_fx->last_core, HQ_CORE ) && ( st_fx->core == ACELP_CORE ) )
195 : {
196 : /* don't use old LSF values if this is the first ACELP frame after HQ frames */
197 32 : Copy( lsf_new, st_fx->lsf_old_fx, M );
198 : }
199 : /* set seed_acelp used in UC mode */
200 1423 : IF( EQ_32( st_fx->core_brate, SID_2k40 ) )
201 : {
202 : /* return if SID frame (conversion to A(z) done in the calling function) */
203 0 : return;
204 : }
205 :
206 : /*-------------------------------------------------------------------------------------*
207 : * FEC - enforce safety-net in the next frame in case of unstable filter
208 : *-------------------------------------------------------------------------------------*/
209 :
210 1423 : IF( NE_16( st_fx->last_L_frame, st_fx->L_frame ) )
211 : {
212 : /* FEC - in case of core switching, use old LSFs */
213 0 : Copy( st_fx->lsf_old_fx, st_fx->lsfoldbfi1_fx, M );
214 0 : Copy( st_fx->lsf_old_fx, st_fx->lsfoldbfi0_fx, M );
215 0 : Copy( st_fx->lsf_old_fx, st_fx->lsf_adaptive_mean_fx, M );
216 : }
217 :
218 1423 : FEC_lsf_estim_enc_fx( st_fx, fec_lsf );
219 :
220 : /* in case of FEC in decoder - calculate LSF stability */
221 1423 : stab = lsf_stab_fx( lsf_new, fec_lsf, 0, st_fx->L_frame ); // Q15
222 :
223 1423 : test();
224 1423 : test();
225 1423 : test();
226 : /* If decoder FEC frame may be unstable force safety-net usage */
227 1423 : IF( ( EQ_16( st_fx->L_frame, L_FRAME16k ) ) && ( LT_16( stab, STAB_FAC_LIMIT_FX ) ) && ( EQ_16( coder_type, GENERIC ) ) )
228 : {
229 90 : st_fx->next_force_safety_net = 1;
230 90 : move16();
231 : }
232 1333 : ELSE IF( ( LT_16( stab, STAB_FAC_LIMIT_FX ) ) && ( EQ_16( st_fx->clas, VOICED_CLAS ) || ( LT_16( st_fx->clas, VOICED_CLAS ) && EQ_16( coder_type, AUDIO ) ) ) )
233 : {
234 31 : st_fx->next_force_safety_net = 1;
235 31 : move16();
236 : }
237 :
238 :
239 : /* FEC - update adaptive LSF mean vector */
240 24191 : FOR( i = 0; i < M; i++ )
241 : {
242 22768 : L_tmp = L_mult( lsf_new[i], 10922 ); /*Q(x2.56+16)*/
243 22768 : L_tmp = L_mac( L_tmp, st_fx->lsfoldbfi1_fx[i], 10922 ); /*Q(x2.56+16)*/
244 22768 : L_tmp = L_mac( L_tmp, st_fx->lsfoldbfi0_fx[i], 10922 ); /*Q(x2.56+16)*/
245 22768 : st_fx->lsf_adaptive_mean_fx[i] = round_fx( L_tmp ); /*Q(x2.56)*/
246 : }
247 :
248 : /* FEC - update LSF memories */
249 1423 : Copy( st_fx->lsfoldbfi0_fx, st_fx->lsfoldbfi1_fx, M );
250 1423 : Copy( lsf_new, st_fx->lsfoldbfi0_fx, M );
251 :
252 :
253 : /*-------------------------------------------------------------------------------------*
254 : * Mid-frame LSF encoding
255 : * LSP interpolation and conversion of LSPs to A(z)
256 : *-------------------------------------------------------------------------------------*/
257 1423 : if ( st_fx->rate_switching_reset )
258 : {
259 : /*extrapolation in case of unstable LSF convert*/
260 0 : Copy( lsp_new, st_fx->lsp_old_fx, M ); // Q15
261 0 : Copy( lsf_new, st_fx->lsf_old_fx, M ); // Q15
262 : }
263 : /* Mid-frame LSF encoding */
264 1423 : lsf_mid_enc_fx( st_fx->hBstr, st_fx->acelp_cfg.mid_lsf_bits, int_fs, st_fx->lsp_old_fx, lsp_new, lsp_mid, coder_type, st_fx->bwidth, st_fx->Bin_E_old_fx, st_fx->Bin_E_fx, Q_new + QSCALE - 2, ppp_mode, nelp_mode );
265 :
266 1423 : test();
267 1423 : IF( EQ_16( st_fx->last_core, HQ_CORE ) && EQ_16( st_fx->core, ACELP_CORE ) )
268 : {
269 : /* don't use old LSP/LSF values if this is the first ACELP frame after HQ frames */
270 32 : Copy( lsp_mid, st_fx->lsp_old_fx, M ); // Q15
271 32 : lsp2lsf_fx( lsp_mid, st_fx->lsf_old_fx, M, int_fs );
272 : }
273 :
274 : /* LSP interpolation and conversion of LSPs to A(z) */
275 1423 : test();
276 1423 : IF( EQ_16( tdm_low_rate_mode, 1 ) && GT_16( coder_type, UNVOICED ) )
277 : {
278 0 : IF( EQ_16( st_fx->active_cnt, 1 ) )
279 : {
280 0 : Copy( lsp_mid, st_fx->lsp_old_fx, M );
281 0 : lsp2lsf_fx( lsp_mid, st_fx->lsf_old_fx, M, int_fs );
282 0 : Copy( lsp_new, lsp_mid, M );
283 : }
284 :
285 : /* LSP interpolation and conversion of LSPs to A(z) - two-subframe mode */
286 0 : int_lsp4_fx( st_fx->L_frame, st_fx->lsp_old_fx, lsp_mid, lsp_new, Aq, M, -2 );
287 : }
288 : ELSE
289 : {
290 1423 : int_lsp4_fx( st_fx->L_frame, st_fx->lsp_old_fx, lsp_mid, lsp_new, Aq, M, 0 );
291 : }
292 :
293 : /*------------------------------------------------------------------*
294 : * Check LSF stability (distance between old LSFs and current LSFs)
295 : *------------------------------------------------------------------*/
296 :
297 1423 : IF( NE_32( st_fx->core_brate, SID_2k40 ) )
298 : {
299 1423 : st_fx->stab_fac_fx = lsf_stab_fx( lsf_new, st_fx->lsf_old_fx, 0, st_fx->L_frame );
300 : }
301 :
302 1423 : return;
303 : }
304 :
305 157016 : void lsf_enc_ivas_fx(
306 : Encoder_State *st, /* i/o: state structure */
307 : Word16 *lsf_new, /* o : quantized LSF vector Q(x2.56)*/
308 : Word16 *lsp_new, /* i/o: LSP vector to quantize/quantized Q15*/
309 : Word16 *lsp_mid, /* i/o : mid-frame LSP vector Q15*/
310 : Word16 *Aq, /* o : quantized A(z) for 4 subframes Q12*/
311 : const Word16 tdm_low_rate_mode, /* i : secondary channel low rate mode flag */
312 : const Word16 GSC_IVAS_mode, /* i : GSC IVAS mode */
313 : const Word16 tdm_lsfQ_PCh[M], /* i : Q LSFs for primary channel */
314 : const Word16 Q_new )
315 : {
316 : Word16 nBits;
317 : Word16 force_sf;
318 : Word16 fec_lsf[M], stab, i;
319 : Word16 no_param_lpc;
320 :
321 : Word16 param_lpc[NPRM_LPC_NEW];
322 : Word32 L_tmp;
323 : Word16 coder_type, ppp_mode, nelp_mode;
324 :
325 157016 : nBits = 0;
326 157016 : force_sf = 0;
327 157016 : move16();
328 157016 : move16();
329 :
330 157016 : test();
331 157016 : IF( EQ_32( st->core_brate, SID_2k40 ) || EQ_32( st->core_brate, SID_1k75 ) )
332 : {
333 447 : coder_type = INACTIVE;
334 447 : move16();
335 : }
336 : ELSE
337 : {
338 156569 : coder_type = st->coder_type;
339 156569 : move16();
340 : }
341 :
342 157016 : test();
343 157016 : if ( EQ_16( coder_type, AUDIO ) && GSC_IVAS_mode > 0 )
344 : {
345 4717 : coder_type = GENERIC;
346 4717 : move16();
347 : }
348 :
349 157016 : no_param_lpc = 0;
350 157016 : move16();
351 :
352 157016 : IF( st->Opt_SC_VBR )
353 : {
354 0 : ppp_mode = st->hSC_VBR->ppp_mode;
355 0 : nelp_mode = st->hSC_VBR->nelp_mode;
356 0 : move16();
357 0 : move16();
358 : }
359 : ELSE
360 : {
361 157016 : ppp_mode = 0;
362 157016 : nelp_mode = 0;
363 157016 : move16();
364 157016 : move16();
365 : }
366 :
367 : /* convert LSPs to LSFs */
368 157016 : lsp2lsf_fx( lsp_new, lsf_new, M, st->sr_core );
369 :
370 : /* check resonance for pitch clipping algorithm */
371 157016 : gp_clip_test_lsf_ivas_fx( st->element_mode, st->core_brate, lsf_new, st->clip_var_fx, 0 );
372 :
373 : /* Find the number of bits for LSF quantization */
374 157016 : nBits = 0;
375 157016 : move16();
376 157016 : IF( EQ_32( st->core_brate, SID_2k40 ) )
377 : {
378 447 : nBits = LSF_BITS_CNG;
379 447 : move16();
380 : }
381 : ELSE
382 : {
383 156569 : test();
384 156569 : IF( ( nelp_mode == 0 ) && ( ppp_mode == 0 ) )
385 : {
386 156569 : nBits = st->acelp_cfg.lsf_bits;
387 156569 : move16();
388 : }
389 0 : ELSE IF( EQ_16( nelp_mode, 1 ) )
390 : {
391 0 : IF( st->bwidth == NB )
392 : {
393 0 : nBits = 32;
394 0 : move16();
395 : }
396 0 : ELSE IF( EQ_16( st->bwidth, WB ) )
397 : {
398 0 : nBits = 30;
399 0 : move16();
400 : }
401 : }
402 0 : ELSE IF( EQ_16( ppp_mode, 1 ) )
403 : {
404 0 : nBits = 26;
405 0 : move16();
406 : }
407 : }
408 157016 : force_sf = 0;
409 157016 : move16();
410 : /* first three ACELP frames after an HQ frame shall be processed only with safety-net quantizer */
411 157016 : test();
412 157016 : if ( LT_16( st->Nb_ACELP_frames, 3 ) && NE_32( st->core_brate, SID_2k40 ) )
413 : {
414 18697 : force_sf = 1;
415 18697 : move16();
416 : }
417 :
418 : /* in case of unstable filter in decoder FEC, choose safety-net to help FEC */
419 157016 : IF( EQ_16( st->next_force_safety_net, 1 ) )
420 : {
421 14021 : force_sf = 1;
422 14021 : move16();
423 14021 : st->next_force_safety_net = 0;
424 14021 : move16();
425 : }
426 :
427 : /*-------------------------------------------------------------------------------------*
428 : * Frame end LSF quantization
429 : *-------------------------------------------------------------------------------------*/
430 157016 : lsf_end_enc_ivas_fx( st, lsf_new, lsf_new, nBits, coder_type, Q_new + QSCALE - 2,
431 157016 : force_sf, param_lpc, &no_param_lpc, NULL, st->coder_type_raw, tdm_lsfQ_PCh );
432 :
433 : /* convert quantized LSFs back to LSPs */
434 157016 : lsf2lsp_fx( lsf_new, lsp_new, M, st->sr_core );
435 :
436 157016 : test();
437 157016 : IF( EQ_16( st->last_core, HQ_CORE ) && ( st->core == ACELP_CORE ) )
438 : {
439 : /* don't use old LSF values if this is the first ACELP frame after HQ frames */
440 219 : Copy( lsf_new, st->lsf_old_fx, M );
441 : }
442 : /* set seed_acelp used in UC mode */
443 :
444 157016 : test();
445 157016 : IF( EQ_16( coder_type, UNVOICED ) && ( st->element_mode > EVS_MONO ) )
446 : {
447 2041 : st->seed_acelp = 0;
448 2041 : move16();
449 2041 : FOR( i = no_param_lpc - 1; i >= 0; i-- )
450 : {
451 : /* rightshift before *seed_acelp+param_lpc[i] to avoid overflows*/
452 0 : st->seed_acelp = add( i_mult( add( shr( st->seed_acelp, 1 ), param_lpc[i] ), 31821 ), 13849 );
453 0 : move16();
454 : }
455 : }
456 :
457 157016 : IF( EQ_32( st->core_brate, SID_2k40 ) )
458 : {
459 : /* return if SID frame (conversion to A(z) done in the calling function) */
460 447 : return;
461 : }
462 :
463 : /*-------------------------------------------------------------------------------------*
464 : * FEC - enforce safety-net in the next frame in case of unstable filter
465 : *-------------------------------------------------------------------------------------*/
466 :
467 156569 : IF( NE_16( st->last_L_frame, st->L_frame ) )
468 : {
469 : /* FEC - in case of core switching, use old LSFs */
470 1772 : Copy( st->lsf_old_fx, st->lsfoldbfi1_fx, M ); // Q15
471 1772 : Copy( st->lsf_old_fx, st->lsfoldbfi0_fx, M ); // Q15
472 1772 : Copy( st->lsf_old_fx, st->lsf_adaptive_mean_fx, M ); // Q15
473 : }
474 :
475 156569 : FEC_lsf_estim_enc_fx( st, fec_lsf );
476 :
477 : /* in case of FEC in decoder - calculate LSF stability */
478 156569 : stab = lsf_stab_ivas_fx( lsf_new, fec_lsf, 0, st->L_frame ); // Q15
479 :
480 156569 : test();
481 156569 : test();
482 156569 : test();
483 : /* If decoder FEC frame may be unstable force safety-net usage */
484 156569 : IF( ( EQ_16( st->L_frame, L_FRAME16k ) ) && ( LT_16( stab, STAB_FAC_LIMIT_FX ) ) && ( EQ_16( coder_type, GENERIC ) ) )
485 : {
486 9668 : st->next_force_safety_net = 1;
487 9668 : move16();
488 : }
489 146901 : ELSE IF( ( LT_16( stab, STAB_FAC_LIMIT_FX ) ) && ( EQ_16( st->clas, VOICED_CLAS ) || ( LT_16( st->clas, VOICED_CLAS ) && EQ_16( coder_type, AUDIO ) ) ) )
490 : {
491 4643 : st->next_force_safety_net = 1;
492 4643 : move16();
493 : }
494 :
495 :
496 : /* FEC - update adaptive LSF mean vector */
497 2661673 : FOR( i = 0; i < M; i++ )
498 : {
499 2505104 : L_tmp = L_mult( lsf_new[i], 10922 ); /*Q(x2.56+16)*/
500 2505104 : L_tmp = L_mac( L_tmp, st->lsfoldbfi1_fx[i], 10922 ); /*Q(x2.56+16)*/
501 2505104 : L_tmp = L_mac( L_tmp, st->lsfoldbfi0_fx[i], 10922 ); /*Q(x2.56+16)*/
502 2505104 : st->lsf_adaptive_mean_fx[i] = extract_h( L_tmp ); /*Q(x2.56)*/
503 : }
504 :
505 : /* FEC - update LSF memories */
506 156569 : Copy( st->lsfoldbfi0_fx, st->lsfoldbfi1_fx, M );
507 156569 : Copy( lsf_new, st->lsfoldbfi0_fx, M );
508 :
509 :
510 : /*-------------------------------------------------------------------------------------*
511 : * Mid-frame LSF encoding
512 : * LSP interpolation and conversion of LSPs to A(z)
513 : *-------------------------------------------------------------------------------------*/
514 156569 : IF( st->rate_switching_reset )
515 : {
516 : /*extrapolation in case of unstable LSF convert*/
517 0 : Copy( lsp_new, st->lsp_old_fx, M ); // Q15
518 0 : Copy( lsf_new, st->lsf_old_fx, M ); // Q15
519 : }
520 : /* Mid-frame LSF encoding */
521 156569 : lsf_mid_enc_ivas_fx( st->hBstr, st->acelp_cfg.mid_lsf_bits, st->sr_core, st->lsp_old_fx, lsp_new, lsp_mid, coder_type, st->bwidth, st->Bin_E_old_fx, Q_new + QSCALE - 2, ppp_mode, nelp_mode );
522 :
523 156569 : test();
524 156569 : IF( EQ_16( st->last_core, HQ_CORE ) && EQ_16( st->core, ACELP_CORE ) )
525 : {
526 : /* don't use old LSP/LSF values if this is the first ACELP frame after HQ frames */
527 219 : Copy( lsp_mid, st->lsp_old_fx, M );
528 219 : lsp2lsf_fx( lsp_mid, st->lsf_old_fx, M, st->sr_core );
529 : }
530 :
531 : /* LSP interpolation and conversion of LSPs to A(z) */
532 156569 : test();
533 156569 : IF( EQ_16( tdm_low_rate_mode, 1 ) && GT_16( coder_type, UNVOICED ) )
534 : {
535 5 : IF( EQ_16( st->active_cnt, 1 ) )
536 : {
537 0 : Copy( lsp_mid, st->lsp_old_fx, M ); // Q15
538 0 : lsp2lsf_fx( lsp_mid, st->lsf_old_fx, M, st->sr_core );
539 0 : Copy( lsp_new, lsp_mid, M ); // Q15
540 : }
541 :
542 : /* LSP interpolation and conversion of LSPs to A(z) - two-subframe mode */
543 5 : int_lsp4_ivas_fx( st->L_frame, st->lsp_old_fx, lsp_mid, lsp_new, Aq, M, -2 );
544 : }
545 : ELSE
546 : {
547 156564 : int_lsp4_ivas_fx( st->L_frame, st->lsp_old_fx, lsp_mid, lsp_new, Aq, M, 0 );
548 : }
549 : /*------------------------------------------------------------------*
550 : * Check LSF stability (distance between old LSFs and current LSFs)
551 : *------------------------------------------------------------------*/
552 156569 : IF( NE_32( st->core_brate, SID_2k40 ) )
553 : {
554 156569 : st->stab_fac_fx = lsf_stab_ivas_fx( lsf_new, st->lsf_old_fx, 0, st->L_frame ); // Q15
555 : }
556 156569 : return;
557 : }
558 : /*-------------------------------------------------------------------*
559 : * lsfq_CNG_fx()
560 : *
561 : * LSF quantizer for SID frames (uses 29 bits, 4 for VQ, 25 for LVQ)
562 : * Note:
563 : * The sampling frequency of the LP-CNG frame can be determined by checking the value of the highest order LSF
564 : * coefficient (last coefficient of lsf). If the last LSF coefficient (lsf[M-1]) is larger than 6350
565 : * the decoded frame is WB2 with sampling rate of 16 kHz, otherwise it is sampled at 12.8kHz and contains
566 : * either NB or WB LSF data.
567 : *-------------------------------------------------------------------*/
568 :
569 0 : static void lsfq_CNG_fx(
570 : BSTR_ENC_HANDLE hBstr, /* i/o: encoder bitstream handle */
571 : const Word16 *lsf, /*x2.56 unquantized LSF vector */
572 : const Word16 *wghts, /*Q10 LSF weights */
573 : Word16 *qlsf, /*x2.56 quantized LSF vecotor */
574 : Word32 *p_offset_scale1,
575 : Word32 *p_offset_scale2,
576 : Word16 *p_no_scales )
577 : {
578 : Word16 i, j, idx_cv, idx_lvq[3];
579 : Word32 min_dist, dist;
580 : Word16 dd[M], ddq[M];
581 : const Word16 *p_cb;
582 : Word16 first_cb, last_cb;
583 : Word16 idx_lead_cng[2], idx_scale_cng[2];
584 : Word16 tmp;
585 :
586 0 : idx_cv = 0;
587 0 : move16();
588 :
589 : /* quantize first stage with 4 bits
590 : The sampling frequency of the LP-CNG frame can be determined by checking the value of the highest order LSF
591 : coefficient (last coefficient of lsf). If the last LSF coefficient (lsf[M-1]) is larger than 6350
592 : the decoded frame is WB2 with sampling rate of 16 kHz, otherwise it is sampled at 12.8kHz and contains
593 : either NB or WB LSF data. */
594 0 : IF( GT_16( lsf[M - 1], WB_LIMIT_LSF_FX ) ) /* 16kHz sampled LSF vector*/
595 : {
596 0 : p_cb = &CNG_SN1_fx[0];
597 0 : move16();
598 0 : first_cb = 0;
599 0 : move16();
600 0 : last_cb = 6;
601 0 : move16();
602 : }
603 : ELSE /* 12.8kHz sampled LSF vector*/
604 : {
605 0 : p_cb = &CNG_SN1_fx[6 * M];
606 0 : move16();
607 0 : first_cb = 6;
608 0 : move16();
609 0 : last_cb = M;
610 0 : move16();
611 : }
612 :
613 :
614 0 : min_dist = L_add( MAXINT32, 0 );
615 0 : FOR( i = first_cb; i < last_cb; i++ )
616 : {
617 0 : tmp = sub( *p_cb, shl( lsf[0], 1 ) ); /*x2.56 */
618 0 : dist = Mult_32_16( L_mult0( wghts[0], *p_cb ), tmp ); /*Q8 + x2.56 -Q15 + x2.56 = Q-7 + x2.56+x.256 */
619 0 : p_cb++;
620 0 : FOR( j = 1; j < M; j++ )
621 : {
622 0 : tmp = sub( *p_cb, lsf[j] );
623 0 : tmp = sub( tmp, lsf[j] );
624 :
625 0 : dist = L_add( dist, Mult_32_16( L_mult0( wghts[j], *p_cb ), tmp ) );
626 0 : p_cb++;
627 : }
628 0 : IF( LT_32( dist, min_dist ) )
629 : {
630 0 : min_dist = dist;
631 0 : move16(); /*Q-4 */
632 0 : idx_cv = i;
633 0 : move16();
634 : }
635 : }
636 :
637 : /* calculate difference */
638 0 : FOR( i = 0; i < M; i++ )
639 : {
640 0 : dd[i] = sub( lsf[i], CNG_SN1_fx[idx_cv * M + i] ); /*x2.56 */
641 0 : move16();
642 : }
643 :
644 : /* quantize the difference with LVQ */
645 : /* MSVQ_ROM to be updated */
646 0 : mslvq_cng_fx( idx_cv, dd, qlsf, ddq, idx_lead_cng, idx_scale_cng, wghts, p_no_scales );
647 :
648 0 : index_lvq_fx( ddq, idx_lead_cng, idx_scale_cng, START_CNG + idx_cv, idx_lvq,
649 : p_offset_scale1, p_offset_scale2, p_no_scales );
650 0 : Vr_add( qlsf, &CNG_SN1_fx[idx_cv * M], qlsf, M );
651 :
652 : /* write the VQ index to the bitstream */
653 0 : push_indice( hBstr, IND_ISF_0_0, idx_cv, 4 );
654 :
655 : /* write the LVQ index to the bitstream */
656 0 : push_indice( hBstr, IND_ISF_0_1, idx_lvq[0], LEN_INDICE );
657 0 : push_indice( hBstr, IND_ISF_0_1, idx_lvq[1], LSF_BITS_CNG - 4 - LEN_INDICE );
658 :
659 0 : return;
660 : }
661 :
662 447 : static void lsfq_CNG_ivas_fx(
663 : BSTR_ENC_HANDLE hBstr, /* i/o: encoder bitstream handle */
664 : const Word16 *lsf, /*x2.56 unquantized LSF vector */
665 : const Word16 *wghts, /*Q10 LSF weights */
666 : Word16 *qlsf /*x2.56 quantized LSF vecotor */
667 : )
668 : {
669 : Word16 i, j, idx_cv, idx_lvq[3];
670 : Word32 min_dist, dist;
671 : Word16 dd[M], ddq[M];
672 : const Word16 *p_cb;
673 : Word16 first_cb, last_cb;
674 : Word16 idx_lead_cng[2], idx_scale_cng[2];
675 : Word16 tmp;
676 :
677 447 : idx_cv = 0;
678 447 : move16();
679 :
680 : /* quantize first stage with 4 bits
681 : The sampling frequency of the LP-CNG frame can be determined by checking the value of the highest order LSF
682 : coefficient (last coefficient of lsf). If the last LSF coefficient (lsf[M-1]) is larger than 6350
683 : the decoded frame is WB2 with sampling rate of 16 kHz, otherwise it is sampled at 12.8kHz and contains
684 : either NB or WB LSF data. */
685 447 : IF( GT_16( lsf[M - 1], WB_LIMIT_LSF_FX ) ) /* 16kHz sampled LSF vector*/
686 : {
687 277 : p_cb = &CNG_SN1_fx[0];
688 277 : move16();
689 277 : first_cb = 0;
690 277 : move16();
691 277 : last_cb = 6;
692 277 : move16();
693 : }
694 : ELSE /* 12.8kHz sampled LSF vector*/
695 : {
696 170 : p_cb = &CNG_SN1_fx[6 * M];
697 170 : move16();
698 170 : first_cb = 6;
699 170 : move16();
700 170 : last_cb = M;
701 170 : move16();
702 : }
703 :
704 :
705 447 : min_dist = L_add( MAXINT32, 0 );
706 3809 : FOR( i = first_cb; i < last_cb; i++ )
707 : {
708 3362 : tmp = sub( *p_cb, shl( lsf[0], 1 ) ); /*x2.56 */
709 3362 : dist = Mult_32_16( L_mult0( wghts[0], *p_cb ), tmp ); /*Q8 + x2.56 -Q15 + x2.56 = Q-7 + x2.56+x.256 */
710 3362 : p_cb++;
711 53792 : FOR( j = 1; j < M; j++ )
712 : {
713 50430 : tmp = sub( *p_cb, lsf[j] );
714 50430 : tmp = sub( tmp, lsf[j] );
715 :
716 50430 : dist = L_add( dist, Mult_32_16( L_mult0( wghts[j], *p_cb ), tmp ) );
717 50430 : p_cb++;
718 : }
719 3362 : IF( LT_32( dist, min_dist ) )
720 : {
721 1177 : min_dist = dist;
722 1177 : move16(); /*Q-4 */
723 1177 : idx_cv = i;
724 1177 : move16();
725 : }
726 : }
727 :
728 : /* calculate difference */
729 7599 : FOR( i = 0; i < M; i++ )
730 : {
731 7152 : dd[i] = sub( lsf[i], CNG_SN1_fx[idx_cv * M + i] ); /*x2.56 */
732 7152 : move16();
733 : }
734 :
735 : /* quantize the difference with LVQ */
736 : /* MSVQ_ROM to be updated */
737 447 : mslvq_cng_ivas_fx( idx_cv, dd, qlsf, ddq, idx_lead_cng, idx_scale_cng, wghts );
738 :
739 447 : index_lvq_ivas_fx( ddq, idx_lead_cng, idx_scale_cng, START_CNG_IVAS + idx_cv, idx_lvq, 0 );
740 447 : Vr_add( qlsf, &CNG_SN1_fx[idx_cv * M], qlsf, M );
741 :
742 : /* write the VQ index to the bitstream */
743 447 : push_indice( hBstr, IND_ISF_0_0, idx_cv, 4 );
744 :
745 : /* write the LVQ index to the bitstream */
746 447 : push_indice( hBstr, IND_ISF_0_1, idx_lvq[0], LEN_INDICE );
747 447 : push_indice( hBstr, IND_ISF_0_1, idx_lvq[1], LSF_BITS_CNG - 4 - LEN_INDICE );
748 :
749 447 : return;
750 : }
751 : /*-------------------------------------------------------------------*
752 : * qlsf_Mode_Select_fx()
753 : *
754 : * Mode selection for LSF quantizer
755 : *-------------------------------------------------------------------*/
756 :
757 :
758 629 : static Word16 qlsf_Mode_Select_fx(
759 : const Word16 *w, /* i : weighting vector Q8 */
760 : const Word16 *pred1, /* i : prediction vector x2.56 */
761 : const Word16 streaklimit, /* i : predictive streak limit Q15 */
762 : const Word32 op_loop_thr /* i : Open-loop Threshold */
763 : )
764 : {
765 : Word16 pred_pow2[M];
766 629 : Word32 temp32, En = 0;
767 : Word16 safety_net;
768 : Word16 i, cs, cl;
769 :
770 : /* calculate the prediction residual */
771 629 : cl = 0;
772 629 : move16();
773 10693 : FOR( i = 0; i < M; i++ )
774 : {
775 10064 : cl = s_max( cl, abs_s( pred1[i] ) );
776 : }
777 629 : cs = norm_s( cl );
778 629 : En = 0;
779 629 : move16();
780 10693 : FOR( i = 0; i < M; i++ )
781 : {
782 10064 : pred_pow2[i] = shl( pred1[i], cs );
783 10064 : move16();
784 10064 : En = L_mac_sat( En, mult( pred_pow2[i], shl_sat( w[i], 2 ) ), pred_pow2[i] ); /* 2.56*2.56 at Q-4 */
785 : }
786 :
787 629 : cs = shl( cs, 1 );
788 629 : En = L_shr( En, cs );
789 629 : temp32 = Mult_32_16( op_loop_thr, streaklimit );
790 :
791 : /* choose the mode */
792 629 : IF( GT_32( En, temp32 ) )
793 : {
794 : /* Safety-net */
795 56 : safety_net = 1;
796 56 : move16();
797 : }
798 : ELSE
799 : {
800 : /* Predictive */
801 573 : safety_net = 0;
802 573 : move16();
803 : }
804 629 : return safety_net;
805 : }
806 :
807 :
808 : /*========================================================================*/
809 : /* FUNCTION : lsf_end_enc_fx() */
810 : /*------------------------------------------------------------------------*/
811 : /* PURPOSE : Quantization of LSF parameters */
812 : /*------------------------------------------------------------------------*/
813 : /* INPUT ARGUMENTS : */
814 : /* _ (Word16*) lsf : LSF in the frequency domain (0..6400) x2.56 */
815 : /* _ (Word16) coder_type : coding type */
816 : /* _ (Word16) bwidth : input signal bandwidth */
817 : /* _ (Word16) nBits : number of bits used for ISF quantization */
818 : /* _ (Word16*) stable_isp : most recent stable ISP (can be */
819 : /* removed after passing to LSF) Q15 */
820 : /* _ (Word16*) stable_lsp : most recent stable LSP Q15 */
821 : /* _ (Word32*) grid : Table of 100 grid points for evaluating */
822 : /* Chebyshev polynomials Q31 */
823 : /* _ (Word16) int_fs : sampling frequency */
824 : /* _ (Word32) core_brate : Coding Bit Rate */
825 : /* _ (Word16) force_sf : Force safety-net usage if possible */
826 : /* _ (Word32*) Bin_Ener : FFT Bin energy 128 *2 sets Q_ener */
827 : /* _ (Word16) Q_ener : Q format of Bin_Ener */
828 : /* _ (Word32*) offset_scale1: offsets for LSF LVQ structure 1st */
829 : /* 8-dim subvector Q0 */
830 : /* _ (Word32*) offset_scale2: offsets for LSF LVQ structure 2nd */
831 : /* 8-dim subvector Q0 */
832 : /* _ (Word32*) offset_scale1_p: offsets for LSF LVQ structure, pred. */
833 : /* case, 1st 8-dim subvector Q0 */
834 : /* _ (Word32*) offset_scale2_p: offsets for LSF LVQ structure, */
835 : /* pred. case, 2nd 8-dim subvector Q0 */
836 : /* _ (Word16*) no_scales : LSF LVQ structure Q0 */
837 : /* _ (Word16*) no_scales_p : LSF LVQ structure Q0 */
838 : /*------------------------------------------------------------------------*/
839 : /* INPUT/OUTPUT ARGUMENTS : */
840 : /* _ (Word16*) mem_AR : quantizer memory for AR model x2.56 */
841 : /*------------------------------------------------------------------------*/
842 : /* OUTPUT ARGUMENTS : */
843 : /* _ (Word16*) qlsf : quantized LSFs in the cosine domain x2.56 */
844 : /*------------------------------------------------------------------------*/
845 :
846 : /*------------------------------------------------------------------------*/
847 : /* RETURN ARGUMENTS : */
848 : /* _ None */
849 : /*========================================================================*/
850 2669 : void lsf_end_enc_fx(
851 : Encoder_State *st, /* i/o: encoder state structure */
852 : const Word16 *lsf, /* i : LSF in the frequency domain (0..6400) x2.56*/
853 : Word16 *qlsf, /* o : quantized LSF x2.56*/
854 : const Word16 nBits_in, /* i : number of bits to spend on ISF quantization */
855 : const Word16 coder_type_org, /* i : coding type */
856 : Word16 Q_ener, /* i : Q valuen for Bin_Ener */
857 : Word16 force_sf, /* i : Force safety-net usage if coding type supports */
858 : Word16 *lpc_param,
859 : Word16 *no_indices,
860 : Word16 *bits_param_lpc,
861 : Word16 coder_type_raw /* i : Coder type (LSF coder_type have some special cases)*/
862 : )
863 : {
864 : Word16 i;
865 : Word16 Idx0[MAX_VQ_STAGES + 3]; /* Optimal codebook indices for safety-net quantizer */
866 : Word16 Idx1[MAX_VQ_STAGES + 3]; /* Optimal codebook indices for predictive quantizer */
867 : Word16 indice[MAX_VQ_STAGES + 3]; /* Temp. array of indice for vector de-quantizer */
868 2669 : Word16 mode_lvq = 0, mode_lvq_p = 0; /* LVQ mode and predictive LVQ mode */
869 : Word16 bits0[MAX_VQ_STAGES], bits1[MAX_VQ_STAGES];
870 2669 : const Word16 *Bit_alloc1 = NULL;
871 : Word32 Err[2]; /* Quantization error for safety-net(0) and predictive(1) quantizers */
872 : Word16 Tmp[M]; /* Temporary target vector (mean and prediction removed) */
873 : Word16 pred0[M]; /* Prediction for the safety-net quantizer (usually mean) */
874 : Word16 pred1[M]; /* Prediction for the predictive quantizer */
875 : Word16 pred2[M]; /* Prediction for the predictive quantizer */
876 : Word16 wghts[M]; /* Weighting used for quantizer (currently GSM based) */
877 : Word16 stages0; /* Amount of stages used by safety-net quantizer */
878 : Word16 stages1; /* Amount of stages used by predictive quantizer */
879 : Word16 levels0[MAX_VQ_STAGES]; /* Sizes of different codebook stages for safety-net quantizer */
880 : Word16 levels1[MAX_VQ_STAGES]; /* Sizes of different codebook stages for predictive quantizer */
881 : Word16 predmode; /* 0: safety-net only, 1: predictive only, 2: best of the two */
882 : Word16 safety_net, cumleft, num_bits;
883 : Word16 *Idx, stages, *bits;
884 : Word16 Tmp2[M], Tmp1[M];
885 : Word32 abs_threshold; /* Absolute threshold depending on signal bandwidth, that indicates
886 : very good perceptual LSF quantization performance */
887 : Word16 lsfq[M * 2], resq[M * 2];
888 : Word16 coder_type; /* coder type (from LSF quantizer point of view) */
889 : Word16 nBits; /* Number of bits */
890 : Word16 TCQIdx0[M + 2]; /* Optimal codebook indices for VQ-TCQ quantizer */
891 : Word16 *TCQIdx;
892 : Word16 tmp;
893 : Word16 flag_1bit_gran;
894 2669 : BSTR_ENC_HANDLE hBstr = st->hBstr;
895 :
896 2669 : flag_1bit_gran = (Word16) GT_16( st->element_mode, EVS_MONO );
897 :
898 2669 : nBits = nBits_in;
899 2669 : move16();
900 : /* Update LSF coder_type for LSF quantizer for some special cases */
901 2669 : test();
902 2669 : test();
903 2669 : test();
904 2669 : IF( EQ_16( coder_type_org, GENERIC ) && EQ_32( st->sr_core, INT_FS_16k ) && EQ_16( st->codec_mode, MODE1 ) )
905 : {
906 624 : IF( EQ_16( coder_type_raw, VOICED ) )
907 : {
908 359 : coder_type = VOICED;
909 359 : move16(); /* Reflect Inactive mode */
910 359 : if ( EQ_16( flag_1bit_gran, 1 ) )
911 : {
912 0 : nBits = sub( nBits, 1 ); /* This is for real Generic*/
913 : }
914 : }
915 : ELSE
916 : {
917 265 : nBits = sub( nBits, 1 ); /* This is for real Generic*/
918 265 : coder_type = coder_type_org;
919 265 : move16();
920 : }
921 : }
922 : ELSE
923 : {
924 2045 : coder_type = coder_type_org;
925 2045 : move16();
926 : }
927 :
928 : /*----------------------------------------------------------------------------------- -*
929 : * Calculate the number of stages and levels for each stage based on allowed bit budget
930 : * Set absolute threshold for codebook-type decision logic depending on signal bandwidth
931 : *------------------------------------------------------------------------------------ -*/
932 2669 : IF( EQ_16( st->bwidth, NB ) )
933 : {
934 0 : abs_threshold = L_add( SFNETLOWLIMIT_NB, 0 );
935 : }
936 : ELSE
937 : {
938 2669 : abs_threshold = L_add( SFNETLOWLIMIT_WB, 0 );
939 : }
940 : /* Calculate LSF weighting coefficients */
941 2669 : Unified_weighting_fx( &st->Bin_E_fx[L_FFT / 2], Q_ener, lsf, wghts, (Word16) EQ_16( st->bwidth, NB ), (Word16) EQ_16( coder_type, UNVOICED ), st->sr_core, M );
942 :
943 : /*--------------------------------------------------------------------------------*
944 : * LSF quantization of SID frames
945 : *--------------------------------------------------------------------------------*/
946 2669 : IF( EQ_32( st->core_brate, SID_2k40 ) )
947 : {
948 0 : lsfq_CNG_fx( hBstr, lsf, wghts, qlsf, &st->offset_scale1_fx[0][0], &st->offset_scale2_fx[0][0], &st->no_scales_fx[0][0] );
949 0 : sort_fx( qlsf, 0, M - 1 );
950 0 : reorder_lsf_fx( qlsf, MODE1_LSF_GAP_FX, M, st->sr_core );
951 :
952 0 : return;
953 : }
954 : /* Find allowed predictor mode for current coder_type. (SN only (0), SN/AR switched (2) or MA predictive (1) */
955 2669 : find_pred_mode( &predmode, coder_type, st->bwidth, st->sr_core, &mode_lvq, &mode_lvq_p, st->total_brate );
956 :
957 : /*----------------------------------------------------------------*
958 : * Calculate number of stages and levels for each stage based on the allowed bit allocation
959 : * (subtract one bit for LSF predictor selection)
960 : *----------------------------------------------------------------*/
961 2669 : lsf_allocate_fx( sub( nBits, shr( predmode, 1 ) ), mode_lvq, mode_lvq_p, &stages0, &stages1, levels0, levels1, bits0, bits1 );
962 :
963 :
964 : /*--------------------------------------------------------------------------------*
965 : * LSF quantization of all other frames but SID frames
966 : * Select safety-net or predictive mode
967 : *--------------------------------------------------------------------------------*/
968 :
969 2669 : Err[0] = MAXINT32;
970 2669 : move32();
971 2669 : Err[1] = MAXINT32;
972 2669 : move32();
973 : /* for mem_MA update */
974 45373 : FOR( i = 0; i < M; i++ )
975 : {
976 42704 : pred1[i] = add( ModeMeans_fx[mode_lvq][i], mult_r( MU_MA_FX, st->mem_MA_fx[i] ) );
977 42704 : move16();
978 : }
979 :
980 2669 : IF( predmode == 0 )
981 : {
982 : /* Subtract only mean */
983 124 : Copy( ModeMeans_fx[mode_lvq], pred0, M );
984 124 : Vr_subt( lsf, pred0, Tmp, M );
985 :
986 : /* LVQ quantization (safety-net only) */
987 248 : Err[0] = vq_lvq_lsf_enc( 0, mode_lvq, Tmp, levels0, stages0, wghts, Idx0, lsf, pred0,
988 124 : st->offset_scale1_fx, st->offset_scale2_fx, st->no_scales_fx, resq, lsfq );
989 124 : safety_net = 1;
990 124 : move16();
991 124 : st->pstreaklen = 0;
992 124 : move16(); /* predictive LSF quantizer streak is ended with safety-net */
993 : }
994 2545 : ELSE IF( EQ_16( predmode, 1 ) ) /* only MA prediction */
995 : {
996 1442 : Vr_subt( lsf, pred1, Tmp1, M );
997 2884 : Err[1] = vq_lvq_lsf_enc( 2, mode_lvq_p, Tmp1, levels1, stages1, wghts, Idx1, lsf, pred1,
998 1442 : st->offset_scale1_p_fx, st->offset_scale2_p_fx, st->no_scales_p_fx, resq, lsfq );
999 :
1000 1442 : safety_net = 0;
1001 1442 : move16();
1002 : }
1003 : ELSE
1004 : {
1005 : /* Adaptive scaling factor (multiplier) is updated in order to reduce the amount of consecutive predictive frames in
1006 : case of possible frame erasure. AR-predictive usage for VOICED mode is allowed to be higher than other modes. */
1007 1103 : test();
1008 1103 : test();
1009 1103 : test();
1010 1103 : IF( ( ( GT_16( st->pstreaklen, ( STREAKLEN + 3 ) ) ) && ( EQ_16( coder_type, VOICED ) ) ) || ( ( GT_16( st->pstreaklen, ( STREAKLEN ) ) ) && ( NE_16( coder_type, VOICED ) ) ) )
1011 : {
1012 : /* update the adaptive scaling factor to become smaller with increasing number of concecutive predictive frames. */
1013 213 : st->streaklimit_fx = mult( st->streaklimit_fx, STREAKMULT_FX ); // Q15
1014 213 : move16();
1015 : }
1016 :
1017 1103 : IF( st->pstreaklen == 0 )
1018 : {
1019 : /* reset the consecutive AR-predictor multiplier */
1020 326 : st->streaklimit_fx = 32767; /*1.0 in Q15 */
1021 326 : move16();
1022 : }
1023 :
1024 : /* VOICED_WB@16kHz */
1025 1103 : test();
1026 1103 : IF( EQ_32( st->sr_core, INT_FS_16k ) && EQ_16( coder_type, VOICED ) && flag_1bit_gran == 0 )
1027 : {
1028 : /* Subtract mean and AR prediction */
1029 629 : Copy( ModeMeans_fx[mode_lvq], pred0, M );
1030 : /* subtract only mean */
1031 629 : Vr_subt( lsf, pred0, Tmp, M );
1032 :
1033 10693 : FOR( i = 0; i < M; i++ )
1034 : {
1035 : /* subtract mean and AR prediction */
1036 10064 : pred2[i] = mult( Predictors_fx[mode_lvq_p][i], sub( st->mem_AR_fx[i], pred0[i] ) );
1037 10064 : Tmp2[i] = sub( Tmp[i], pred2[i] );
1038 10064 : pred2[i] = add( pred2[i], pred0[i] );
1039 : }
1040 :
1041 : /* select safety_net or predictive */
1042 629 : safety_net = qlsf_Mode_Select_fx( wghts, Tmp2, st->streaklimit_fx, OP_LOOP_THR_HVO );
1043 629 : IF( EQ_16( force_sf, 1 ) )
1044 : {
1045 57 : safety_net = 1;
1046 57 : move16();
1047 : }
1048 :
1049 629 : IF( safety_net )
1050 : {
1051 : /* Safety-net - BC-TCQ quantization : SN */
1052 104 : Err[0] = qlsf_ARSN_tcvq_Enc_16k_fx( Tmp, lsfq, TCQIdx0, wghts, sub( nBits, 1 ), safety_net );
1053 104 : st->pstreaklen = 0;
1054 104 : move16();
1055 : }
1056 : ELSE
1057 : {
1058 : /* predictive - BC-TCQ quantization : AR */
1059 525 : Err[1] = qlsf_ARSN_tcvq_Enc_16k_fx( Tmp2, lsfq, TCQIdx0, wghts, sub( nBits, 1 ), safety_net );
1060 525 : st->pstreaklen = add( st->pstreaklen, 1 );
1061 : }
1062 : }
1063 : /* all other frames (not VOICED@16kHz) */
1064 : ELSE
1065 : {
1066 : /* Subtract mean and AR prediction */
1067 474 : Copy( ModeMeans_fx[mode_lvq], pred0, M );
1068 : /* subtract only mean */
1069 474 : Vr_subt( lsf, pred0, Tmp, M );
1070 :
1071 8058 : FOR( i = 0; i < M; i++ )
1072 : {
1073 : /* subtract mean and AR prediction */
1074 7584 : pred2[i] = add( pred0[i], mult( Predictors_fx[mode_lvq_p][i], sub( st->mem_AR_fx[i], pred0[i] ) ) );
1075 7584 : Tmp2[i] = sub( lsf[i], pred2[i] );
1076 : }
1077 :
1078 : /* safety-net */
1079 474 : Err[0] = vq_lvq_lsf_enc( 0, mode_lvq, Tmp, levels0, stages0, wghts, Idx0, lsf, pred0, st->offset_scale1_fx, st->offset_scale2_fx, st->no_scales_fx, resq, lsfq );
1080 : /* Predictive quantizer is calculated only if it can be selected */
1081 474 : test();
1082 474 : IF( !force_sf || GT_32( Err[0], abs_threshold ) )
1083 : {
1084 468 : Err[1] = vq_lvq_lsf_enc( 2, mode_lvq_p, Tmp2, levels1, stages1, wghts, Idx1, lsf, pred2,
1085 468 : st->offset_scale1_p_fx, st->offset_scale2_p_fx, st->no_scales_p_fx, &resq[M], &lsfq[M] );
1086 : }
1087 474 : test();
1088 474 : test();
1089 : /* Select whether to use safety-net (non-predictive) or predictive LSF quantizer. The decision is based on following:
1090 : if the non-predictive (safety-net) quantization error (Err[0]) is low enough (spectral distortion is low) it is selected
1091 : or if the predictively quantized error (Err[1]) is by at least adaptive margin smaller than non-predictive quantizer.
1092 : or if the in case of frame erasure the resulting concealed predictive LSF would be unstable safety-net is selected */
1093 474 : IF( force_sf || LT_32( Mult_32_16( Err[0], ( st->streaklimit_fx ) ), L_add( Err[1], Mult_32_16( Err[1], PREFERSFNET_FX ) ) ) || LT_32( Err[0], abs_threshold ) )
1094 : {
1095 166 : safety_net = 1;
1096 166 : move16();
1097 166 : st->pstreaklen = 0;
1098 166 : move16(); /* Reset the consecutive predictive frame counter */
1099 : }
1100 : ELSE
1101 : {
1102 308 : safety_net = 0;
1103 308 : move16(); /* Increase the consecutive predictive frame counter by one */
1104 308 : st->pstreaklen = add( st->pstreaklen, 1 );
1105 : }
1106 : }
1107 : }
1108 :
1109 : /*--------------------------------------------------------------------------* \
1110 : * Write indices to array \
1111 : *--------------------------------------------------------------------------*/
1112 :
1113 2669 : IF( EQ_16( st->codec_mode, MODE1 ) && EQ_16( st->core, ACELP_CORE ) )
1114 : {
1115 : /* write coder_type bit for VOICED@16kHz or GENERIC@16kHz */
1116 1423 : test();
1117 1423 : IF( EQ_16( coder_type_org, GENERIC ) && EQ_32( st->sr_core, INT_FS_16k ) )
1118 : {
1119 : /* VOICED =2 and GENERIC=3, so "coder_type-2" means VOICED =0 and GENERIC=1*/
1120 624 : push_indice( hBstr, IND_LSF_PREDICTOR_SELECT_BIT, sub( coder_type, 2 ), 1 );
1121 : }
1122 :
1123 : /* write predictor selection bit */
1124 1423 : IF( EQ_16( predmode, 2 ) )
1125 : {
1126 571 : push_indice( st->hBstr, IND_LSF_PREDICTOR_SELECT_BIT, safety_net, 1 );
1127 : }
1128 :
1129 1423 : test();
1130 1423 : IF( EQ_16( coder_type, VOICED ) && EQ_32( st->sr_core, INT_FS_16k ) && flag_1bit_gran == 0 )
1131 : {
1132 : /* BC-TCVQ (only for VOICED@16kHz) */
1133 359 : TCQIdx = &TCQIdx0[1];
1134 359 : Bit_alloc1 = &BC_TCVQ_BIT_ALLOC_40B[1];
1135 4308 : FOR( i = 0; i < ( M / 2 ) + 3; i++ )
1136 : {
1137 3949 : push_indice( hBstr, IND_LSF, TCQIdx[i], Bit_alloc1[i] );
1138 : }
1139 : }
1140 : ELSE
1141 : {
1142 1064 : cumleft = nBits;
1143 1064 : move16();
1144 1064 : IF( EQ_16( predmode, 2 ) )
1145 : {
1146 : /* subtract predictor selection bit */
1147 212 : cumleft = sub( nBits, 1 );
1148 : }
1149 :
1150 1064 : IF( safety_net )
1151 : {
1152 205 : stages = stages0;
1153 205 : move16();
1154 205 : Idx = Idx0;
1155 205 : move16();
1156 205 : bits = bits0;
1157 205 : move16();
1158 : }
1159 : ELSE
1160 : {
1161 859 : stages = stages1;
1162 859 : move16();
1163 859 : Idx = Idx1;
1164 859 : move16();
1165 859 : bits = bits1;
1166 859 : move16();
1167 : }
1168 :
1169 1064 : tmp = sub( stages, 1 );
1170 2464 : FOR( i = 0; i < tmp; i++ )
1171 : {
1172 1400 : indice[i] = Idx[i];
1173 1400 : move16();
1174 1400 : num_bits = bits[i];
1175 1400 : move16();
1176 1400 : cumleft -= num_bits;
1177 1400 : move16();
1178 1400 : push_indice( hBstr, IND_LSF, indice[i], num_bits );
1179 : }
1180 :
1181 4036 : WHILE( cumleft > 0 )
1182 : {
1183 2972 : indice[i] = Idx[i];
1184 2972 : move16();
1185 :
1186 2972 : IF( GT_16( cumleft, LEN_INDICE ) )
1187 : {
1188 1908 : num_bits = LEN_INDICE;
1189 1908 : move16();
1190 : }
1191 : ELSE
1192 : {
1193 1064 : num_bits = cumleft;
1194 1064 : move16();
1195 : }
1196 :
1197 2972 : cumleft = sub( cumleft, num_bits );
1198 2972 : push_indice( hBstr, IND_LSF, indice[i], num_bits );
1199 2972 : i = add( i, 1 );
1200 : }
1201 : }
1202 : }
1203 : ELSE
1204 : {
1205 1246 : test();
1206 1246 : IF( EQ_16( coder_type, VOICED ) && EQ_32( st->sr_core, INT_FS_16k ) )
1207 : {
1208 : /* BC-TCVQ (only for VOICED@16kHz) */
1209 : /* Number of quantization indices */
1210 270 : *no_indices = 10;
1211 270 : move16();
1212 2970 : FOR( i = 0; i < *no_indices; i++ )
1213 : {
1214 2700 : lpc_param[i] = TCQIdx0[i];
1215 2700 : move16();
1216 2700 : bits_param_lpc[i] = BC_TCVQ_BIT_ALLOC_40B[i]; // Q0
1217 2700 : move16();
1218 : }
1219 : }
1220 : ELSE
1221 : {
1222 : /* Number of quantization indices */
1223 :
1224 : /* there are 31 bits */
1225 976 : IF( EQ_16( safety_net, 1 ) )
1226 : {
1227 85 : Idx = Idx0;
1228 85 : move16();
1229 85 : *no_indices = add( stages0, 1 );
1230 255 : FOR( i = 0; i < stages0; i++ )
1231 : {
1232 170 : lpc_param[i] = Idx[i];
1233 170 : move16();
1234 170 : indice[i] = Idx[i];
1235 170 : move16();
1236 170 : bits_param_lpc[i] = bits0[i];
1237 170 : move16();
1238 : }
1239 85 : lpc_param[stages0] = Idx[stages0];
1240 85 : move16();
1241 85 : indice[stages0] = Idx[stages0];
1242 85 : move16();
1243 85 : tmp = sub( stages0, 1 );
1244 85 : bits_param_lpc[tmp] = LEN_INDICE;
1245 85 : move16();
1246 85 : bits_param_lpc[stages0] = sub( bits0[tmp], LEN_INDICE );
1247 : }
1248 : ELSE
1249 : {
1250 891 : *no_indices = add( stages1, 1 );
1251 891 : Idx = Idx1;
1252 891 : move16();
1253 2496 : FOR( i = 0; i < stages1; i++ )
1254 : {
1255 1605 : lpc_param[i] = ( Idx[i] );
1256 1605 : move16();
1257 1605 : indice[i] = Idx[i];
1258 1605 : move16();
1259 1605 : bits_param_lpc[i] = bits1[i];
1260 1605 : move16();
1261 : }
1262 891 : lpc_param[stages1] = ( Idx[stages1] );
1263 891 : move16();
1264 891 : indice[stages1] = Idx[stages1];
1265 891 : move16();
1266 891 : tmp = sub( stages1, 1 );
1267 891 : bits_param_lpc[tmp] = LEN_INDICE;
1268 891 : move16();
1269 891 : bits_param_lpc[stages1] = sub( bits1[tmp], LEN_INDICE );
1270 : }
1271 976 : IF( EQ_16( predmode, 2 ) )
1272 : {
1273 871 : FOR( i = *no_indices; i > 0; i-- )
1274 : {
1275 609 : tmp = sub( i, 1 );
1276 609 : lpc_param[i] = lpc_param[tmp];
1277 609 : move16();
1278 609 : bits_param_lpc[i] = bits_param_lpc[tmp];
1279 609 : move16();
1280 : }
1281 262 : lpc_param[0] = safety_net;
1282 262 : move16(); /* put the safety net info on the last param */
1283 262 : bits_param_lpc[0] = 1;
1284 262 : move16();
1285 262 : *no_indices = add( *no_indices, 1 );
1286 : }
1287 : }
1288 : }
1289 :
1290 :
1291 : /*--------------------------------------------------------------------------*
1292 : * De-quantize encoded LSF vector
1293 : *--------------------------------------------------------------------------*/
1294 :
1295 2669 : IF( safety_net )
1296 : {
1297 : /* Safety-net */
1298 394 : test();
1299 394 : IF( EQ_16( coder_type, VOICED ) && EQ_32( st->sr_core, INT_FS_16k ) && flag_1bit_gran == 0 )
1300 : {
1301 : /* BC-TCQ */
1302 104 : Copy( lsfq, st->mem_MA_fx, M );
1303 104 : Vr_add( lsfq, pred0, qlsf, M );
1304 : }
1305 : ELSE
1306 : {
1307 : {
1308 290 : vq_dec_lvq_fx( 1, qlsf, &indice[0], stages0, M, mode_lvq, levels0[stages0 - 1],
1309 : &st->offset_scale1_fx[0][0], &st->offset_scale2_fx[0][0], &st->offset_scale1_p_fx[0][0], &st->offset_scale2_p_fx[0][0],
1310 : &st->no_scales_fx[0][0], &st->no_scales_p_fx[0][0] );
1311 290 : Vr_add( qlsf, pred0, qlsf, M );
1312 290 : Vr_subt( qlsf, pred1, st->mem_MA_fx, M );
1313 : }
1314 : }
1315 : }
1316 : ELSE
1317 : {
1318 2275 : test();
1319 2275 : IF( EQ_16( coder_type, VOICED ) && EQ_32( st->sr_core, INT_FS_16k ) && flag_1bit_gran == 0 )
1320 : {
1321 : /* BC-TCVQ */
1322 525 : Copy( lsfq, st->mem_MA_fx, M );
1323 525 : Vr_add( lsfq, pred2, qlsf, M );
1324 : }
1325 : ELSE
1326 : {
1327 : /* LVQ */
1328 1750 : vq_dec_lvq_fx( 0, qlsf, &indice[0], stages1, M, mode_lvq_p, levels1[stages1 - 1],
1329 : &st->offset_scale1_fx[0][0], &st->offset_scale2_fx[0][0], &st->offset_scale1_p_fx[0][0],
1330 : &st->offset_scale2_p_fx[0][0], &st->no_scales_fx[0][0], &st->no_scales_p_fx[0][0] );
1331 1750 : IF( EQ_16( predmode, 1 ) )
1332 : {
1333 1442 : Copy( qlsf, st->mem_MA_fx, M );
1334 1442 : Vr_add( qlsf, pred1, qlsf, M );
1335 : }
1336 : ELSE
1337 : {
1338 308 : Vr_add( qlsf, pred2, qlsf, M );
1339 308 : Vr_subt( qlsf, pred1, st->mem_MA_fx, M );
1340 : }
1341 : }
1342 : }
1343 :
1344 : /* Sort the quantized vector to ascending order */
1345 2669 : sort_fx( qlsf, 0, M - 1 );
1346 :
1347 : /* Verify stability by adding minimum separation */
1348 2669 : reorder_lsf_fx( qlsf, MODE1_LSF_GAP_FX, M, st->sr_core );
1349 :
1350 : /* Update AR-predictor memories */
1351 2669 : Copy( qlsf, st->mem_AR_fx, M );
1352 :
1353 2669 : return;
1354 : }
1355 :
1356 :
1357 269145 : void lsf_end_enc_ivas_fx(
1358 : Encoder_State *st, /* i/o: encoder state structure */
1359 : const Word16 *lsf, /* i : LSF in the frequency domain (0..6400) x2.56*/
1360 : Word16 *qlsf, /* o : quantized LSF x2.56*/
1361 : const Word16 nBits_in, /* i : number of bits to spend on ISF quantization */
1362 : const Word16 coder_type_org, /* i : coding type */
1363 : Word16 Q_ener, /* i : Q valuen for Bin_Ener */
1364 : Word16 force_sf, /* i : Force safety-net usage if coding type supports */
1365 : Word16 *lpc_param,
1366 : Word16 *no_indices,
1367 : Word16 *bits_param_lpc,
1368 : Word16 coder_type_raw, /* i : Coder type (LSF coder_type have some special cases)*/
1369 : const Word16 tdm_lsfQ_PCh[M] /* i : Q LSFs for primary channel */
1370 : )
1371 : {
1372 : Word16 i;
1373 : Word16 Idx0[MAX_VQ_STAGES + 3]; /* Optimal codebook indices for safety-net quantizer */
1374 : Word16 Idx1[MAX_VQ_STAGES + 3]; /* Optimal codebook indices for predictive quantizer */
1375 : Word16 indice[MAX_VQ_STAGES + 3]; /* Temp. array of indice for vector de-quantizer */
1376 269145 : Word16 mode_lvq = 0, mode_lvq_p = 0; /* LVQ mode and predictive LVQ mode */
1377 : Word16 bits0[MAX_VQ_STAGES], bits1[MAX_VQ_STAGES];
1378 269145 : const Word16 *Bit_alloc1 = NULL;
1379 : Word32 Err[2]; /* Quantization error for safety-net(0) and predictive(1) quantizers */
1380 : Word16 Tmp[M]; /* Temporary target vector (mean and prediction removed) */
1381 : Word16 pred0[M]; /* Prediction for the safety-net quantizer (usually mean) */
1382 : Word16 pred1[M]; /* Prediction for the predictive quantizer */
1383 : Word16 pred2[M]; /* Prediction for the predictive quantizer */
1384 : Word16 wghts[M]; /* Weighting used for quantizer (currently GSM based) */
1385 : Word16 stages0; /* Amount of stages used by safety-net quantizer */
1386 : Word16 stages1; /* Amount of stages used by predictive quantizer */
1387 : Word16 levels0[MAX_VQ_STAGES]; /* Sizes of different codebook stages for safety-net quantizer */
1388 : Word16 levels1[MAX_VQ_STAGES]; /* Sizes of different codebook stages for predictive quantizer */
1389 : Word16 predmode; /* 0: safety-net only, 1: predictive only, 2: best of the two */
1390 : Word16 safety_net, cumleft, num_bits;
1391 : Word16 *Idx, stages, *bits;
1392 : Word16 Tmp2[M], Tmp1[M];
1393 : Word32 abs_threshold; /* Absolute threshold depending on signal bandwidth, that indicates
1394 : very good perceptual LSF quantization performance */
1395 : Word16 lsfq[M * 2], resq[M * 2];
1396 : Word16 coder_type; /* coder type (from LSF quantizer point of view) */
1397 : Word16 nBits; /* Number of bits */
1398 : Word16 TCQIdx0[M + 2]; /* Optimal codebook indices for VQ-TCQ quantizer */
1399 : Word16 *TCQIdx;
1400 : Word16 tmp;
1401 : Word16 flag_1bit_gran;
1402 269145 : BSTR_ENC_HANDLE hBstr = st->hBstr;
1403 : Word16 pred3[M];
1404 : Word16 dummy, dummy_v[5];
1405 :
1406 269145 : flag_1bit_gran = (Word16) GT_16( st->element_mode, EVS_MONO );
1407 :
1408 269145 : nBits = nBits_in;
1409 269145 : move16();
1410 : /* Update LSF coder_type for LSF quantizer for some special cases */
1411 269145 : test();
1412 269145 : test();
1413 269145 : test();
1414 :
1415 269145 : IF( EQ_16( coder_type_org, GENERIC ) && EQ_32( st->sr_core, INT_FS_16k ) && EQ_16( st->codec_mode, MODE1 ) && ( st->idchan == 0 ) ) /* this bit is used only for primary channel or mono */
1416 : {
1417 61287 : IF( EQ_16( coder_type_raw, VOICED ) )
1418 : {
1419 18788 : coder_type = VOICED;
1420 18788 : move16(); /* Reflect Inactive mode */
1421 18788 : if ( EQ_16( flag_1bit_gran, 1 ) )
1422 : {
1423 18788 : nBits = sub( nBits, 1 ); /* This is for real Generic*/
1424 : }
1425 : }
1426 : ELSE
1427 : {
1428 42499 : nBits = sub( nBits, 1 ); /* This is for real Generic*/
1429 42499 : coder_type = coder_type_org;
1430 42499 : move16();
1431 : }
1432 : }
1433 : ELSE
1434 : {
1435 207858 : coder_type = coder_type_org;
1436 207858 : move16();
1437 : }
1438 :
1439 : /*------------------------------------------------------------------------------------*
1440 : * Calculate the number of stages and levels for each stage based on allowed bit budget
1441 : * Set absolute threshold for codebook-type decision logic depending on signal bandwidth
1442 : *-------------------------------------------------------------------------------------*/
1443 :
1444 269145 : IF( st->bwidth == NB )
1445 : {
1446 0 : abs_threshold = SFNETLOWLIMIT_NB / 2;
1447 0 : move32();
1448 : }
1449 : ELSE
1450 : {
1451 269145 : abs_threshold = SFNETLOWLIMIT_WB / 2;
1452 269145 : move32();
1453 : }
1454 : /* Calculate LSF weighting coefficients */
1455 269145 : Unified_weighting_fx( &st->Bin_E_fx[L_FFT / 2], Q_ener, lsf, wghts, (Word16) EQ_16( st->bwidth, NB ), (Word16) EQ_16( coder_type, UNVOICED ), st->sr_core, M );
1456 :
1457 : /*--------------------------------------------------------------------------------*
1458 : * LSF quantization of SID frames
1459 : *--------------------------------------------------------------------------------*/
1460 :
1461 269145 : IF( EQ_32( st->core_brate, SID_2k40 ) )
1462 : {
1463 447 : lsfq_CNG_ivas_fx( hBstr, lsf, wghts, qlsf );
1464 447 : sort_fx( qlsf, 0, M - 1 );
1465 447 : reorder_lsf_fx( qlsf, MODE1_LSF_GAP_FX, M, st->sr_core );
1466 :
1467 447 : return;
1468 : }
1469 : /* Find allowed predictor mode for current coder_type. (SN only (0), SN/AR switched (2) or MA predictive (1) */
1470 268698 : find_pred_mode( &predmode, coder_type, st->bwidth, st->sr_core, &mode_lvq, &mode_lvq_p, st->total_brate );
1471 :
1472 : /*----------------------------------------------------------------*
1473 : * Calculate number of stages and levels for each stage based on the allowed bit allocation
1474 : * (subtract one bit for LSF predictor selection)
1475 : *----------------------------------------------------------------*/
1476 :
1477 268698 : lsf_allocate_fx( sub( nBits, shr( predmode, 1 ) ), mode_lvq, mode_lvq_p, &stages0, &stages1, levels0, levels1, bits0, bits1 );
1478 :
1479 : /*--------------------------------------------------------------------------------*
1480 : * LSF quantization of all other frames but SID frames
1481 : * Select safety-net or predictive mode
1482 : *--------------------------------------------------------------------------------*/
1483 :
1484 268698 : Err[0] = MAXINT32;
1485 268698 : move32();
1486 268698 : Err[1] = MAXINT32;
1487 268698 : move32();
1488 : /* for mem_MA update */
1489 4567866 : FOR( i = 0; i < M; i++ )
1490 : {
1491 4299168 : pred1[i] = add( ModeMeans_fx[mode_lvq][i], mult_r( MU_MA_FX, st->mem_MA_fx[i] ) );
1492 4299168 : move16();
1493 : }
1494 :
1495 268698 : test();
1496 268698 : test();
1497 : /* TD stereo SCh: perform intra-frame prediction with pulling-to-mean */
1498 268698 : IF( st->tdm_LRTD_flag == 0 && st->idchan == 1 && tdm_lsfQ_PCh != NULL )
1499 : {
1500 : /* if secondary channel predmode is set to be > 2 */
1501 120 : predmode = add( predmode, 3 );
1502 :
1503 120 : tdm_SCh_LSF_intra_pred_fx( st->element_brate, tdm_lsfQ_PCh, pred3 );
1504 : }
1505 :
1506 268698 : IF( predmode == 0 )
1507 : {
1508 : /* Subtract only mean */
1509 14253 : Copy( ModeMeans_fx[mode_lvq], pred0, M );
1510 14253 : Vr_subt( lsf, pred0, Tmp, M );
1511 :
1512 : /* LVQ quantization (safety-net only) */
1513 14253 : Err[0] = vq_lvq_lsf_enc_ivas_fx( 0, mode_lvq, Tmp, levels0, stages0, wghts, Idx0, lsf, pred0, resq, lsfq );
1514 14253 : safety_net = 1;
1515 14253 : move16();
1516 14253 : st->pstreaklen = 0;
1517 14253 : move16(); /* predictive LSF quantizer streak is ended with safety-net */
1518 : }
1519 254445 : ELSE IF( EQ_16( predmode, 1 ) ) /* only MA prediction */
1520 : {
1521 177570 : Vr_subt( lsf, pred1, Tmp1, M );
1522 177570 : Err[1] = vq_lvq_lsf_enc_ivas_fx( 2, mode_lvq_p, Tmp1, levels1, stages1, wghts, Idx1, lsf, pred1, resq, lsfq );
1523 :
1524 177570 : safety_net = 0;
1525 177570 : move16();
1526 : }
1527 : ELSE
1528 : {
1529 76875 : IF( EQ_16( predmode, 2 ) )
1530 : {
1531 : /* Subtract mean and AR prediction */
1532 76755 : Copy( ModeMeans_fx[mode_lvq], pred0, M );
1533 :
1534 : /* subtract only mean */
1535 76755 : Vr_subt( lsf, pred0, Tmp, M );
1536 :
1537 1304835 : FOR( i = 0; i < M; i++ )
1538 : {
1539 : /* subtract mean and AR prediction */
1540 1228080 : pred2[i] = add( pred0[i], mult( Predictors_fx[mode_lvq_p][i], sub( st->mem_AR_fx[i], pred0[i] ) ) );
1541 1228080 : Tmp2[i] = sub( lsf[i], pred2[i] );
1542 : }
1543 :
1544 : /* Adaptive scaling factor (multiplier) is updated in order to reduce the amount of consecutive predictive frames in
1545 : case of possible frame erasure. AR-predictive usage for VOICED mode is allowed to be higher than other modes. */
1546 76755 : test();
1547 76755 : test();
1548 76755 : test();
1549 76755 : if ( ( ( GT_16( st->pstreaklen, ( STREAKLEN + 3 ) ) ) && ( EQ_16( coder_type, VOICED ) ) ) || ( ( GT_16( st->pstreaklen, ( STREAKLEN ) ) ) && ( NE_16( coder_type, VOICED ) ) ) )
1550 : {
1551 : /* update the adaptive scaling factor to become smaller with increasing number of concecutive predictive frames. */
1552 12001 : st->streaklimit_fx = mult( st->streaklimit_fx, STREAKMULT_FX );
1553 12001 : move16();
1554 : }
1555 :
1556 76755 : if ( st->pstreaklen == 0 )
1557 : {
1558 : /* reset the consecutive AR-predictor multiplier */
1559 28260 : st->streaklimit_fx = 32767; /*1.0 in Q15 */
1560 28260 : move16();
1561 : }
1562 :
1563 : /* VOICED_WB@16kHz */
1564 76755 : test();
1565 76755 : test();
1566 76755 : IF( EQ_32( st->sr_core, INT_FS_16k ) && EQ_16( coder_type, VOICED ) && flag_1bit_gran == 0 )
1567 : {
1568 : /* select safety_net or predictive */
1569 0 : safety_net = qlsf_Mode_Select_fx( wghts, Tmp2, st->streaklimit_fx, OP_LOOP_THR_HVO );
1570 0 : if ( EQ_16( force_sf, 1 ) )
1571 : {
1572 0 : safety_net = 1;
1573 0 : move16();
1574 : }
1575 :
1576 0 : IF( safety_net )
1577 : {
1578 : /* Safety-net - BC-TCQ quantization : SN */
1579 0 : Err[0] = qlsf_ARSN_tcvq_Enc_16k_fx( Tmp, lsfq, TCQIdx0, wghts, sub( nBits, 1 ), safety_net );
1580 0 : st->pstreaklen = 0;
1581 0 : move16();
1582 : }
1583 : ELSE
1584 : {
1585 : /* predictive - BC-TCQ quantization : AR */
1586 0 : Err[1] = qlsf_ARSN_tcvq_Enc_16k_fx( Tmp2, lsfq, TCQIdx0, wghts, sub( nBits, 1 ), safety_net );
1587 0 : st->pstreaklen = add( st->pstreaklen, 1 );
1588 : }
1589 : }
1590 : /* all other frames (not VOICED@16kHz) */
1591 : ELSE
1592 : {
1593 : /* safety-net */
1594 :
1595 76755 : Err[0] = vq_lvq_lsf_enc_ivas_fx( 0, mode_lvq, Tmp, levels0, stages0, wghts, Idx0, lsf, pred0, resq, lsfq );
1596 : /* Predictive quantizer is calculated only if it can be selected */
1597 76755 : test();
1598 76755 : IF( !force_sf || GT_32( Err[0], abs_threshold ) )
1599 : {
1600 76719 : Err[1] = vq_lvq_lsf_enc_ivas_fx( 2, mode_lvq_p, Tmp2, levels1, stages1, wghts, Idx1, lsf, pred2, &resq[M], &lsfq[M] );
1601 : }
1602 76755 : test();
1603 76755 : test();
1604 : /* Select whether to use safety-net (non-predictive) or predictive LSF quantizer. The decision is based on following:
1605 : if the non-predictive (safety-net) quantization error (Err[0]) is low enough (spectral distortion is low) it is selected
1606 : or if the predictively quantized error (Err[1]) is by at least adaptive margin smaller than non-predictive quantizer.
1607 : or if the in case of frame erasure the resulting concealed predictive LSF would be unstable safety-net is selected */
1608 76755 : IF( force_sf || LT_32( Mult_32_16( Err[0], ( st->streaklimit_fx ) ), L_add( Err[1], Mult_32_16( Err[1], PREFERSFNET_FX ) ) ) || LT_32( Err[0], abs_threshold ) )
1609 : {
1610 23793 : safety_net = 1;
1611 23793 : move16();
1612 23793 : st->pstreaklen = 0;
1613 23793 : move16(); /* Reset the consecutive predictive frame counter */
1614 : }
1615 : ELSE
1616 : {
1617 52962 : safety_net = 0;
1618 52962 : move16(); /* Increase the consecutive predictive frame counter by one */
1619 52962 : st->pstreaklen = add( st->pstreaklen, 1 );
1620 : }
1621 : }
1622 : }
1623 : ELSE /* of "if (predmode==2)" */
1624 : {
1625 120 : Copy( ModeMeans_fx[mode_lvq], pred0, M );
1626 :
1627 120 : if ( EQ_16( predmode, 4 ) )
1628 : {
1629 99 : mode_lvq_p = 9; /* force to Generic WB with AR*/
1630 99 : move16();
1631 : }
1632 :
1633 2040 : FOR( i = 0; i < M; i++ )
1634 : {
1635 : /* subtract mean and AR prediction */
1636 1920 : pred2[i] = add( pred0[i], mult( Predictors_fx[mode_lvq_p][i], sub( st->mem_AR_fx[i], pred0[i] ) ) );
1637 1920 : Tmp[i] = sub( lsf[i], pred2[i] );
1638 1920 : Tmp2[i] = sub( lsf[i], pred3[i] );
1639 : }
1640 :
1641 : /* Adaptive scaling factor (multiplier) is updated in order to reduce the amount of consecutive predictive frames in
1642 : case of possible frame erasure. AR-predictive usage for VOICED mode is allowed to be higher than other modes. */
1643 120 : IF( GT_16( st->pstreaklen, ( STREAKLEN ) ) )
1644 : {
1645 : /* update the adaptive scaling factor to become smaller with increasing number of concecutive predictive frames. */
1646 1 : st->streaklimit_fx = mult( st->streaklimit_fx, STREAKMULT_FX );
1647 1 : move16();
1648 : }
1649 :
1650 120 : if ( st->pstreaklen == 0 )
1651 : {
1652 : /* reset the adaptive scaling factor */
1653 74 : st->streaklimit_fx = 32767;
1654 74 : move16();
1655 : }
1656 :
1657 : /* intra pred */
1658 : /* use G AR pred for the intra mode (as a safety mode, this is why the indexes 0/1 are interchanged)*/
1659 :
1660 120 : lsf_allocate_fx( sub( nBits, 1 ), mode_lvq, 9, &stages1, &stages0, levels1, levels0, bits1, bits0 );
1661 120 : Err[0] = vq_lvq_lsf_enc_ivas_fx( 2, 9, Tmp2, levels0, stages0, wghts, Idx0, lsf, pred3, &resq[M], &lsfq[M] );
1662 120 : IF( force_sf )
1663 : {
1664 0 : safety_net = 1; /* intra-frame prediction */
1665 0 : st->pstreaklen = 0; /* Reset the consecutive predictive frame counter */
1666 : }
1667 : ELSE
1668 : { /* try also the inter frame prediction */
1669 :
1670 : /* AR inter-frame prediction */
1671 120 : lsf_allocate_fx( nBits - 1, mode_lvq, mode_lvq_p, &dummy, &stages1, dummy_v, levels1, dummy_v, bits1 );
1672 120 : Err[1] = vq_lvq_lsf_enc_ivas_fx( 2, mode_lvq_p, Tmp, levels1, stages1, wghts, Idx1, lsf, pred2, resq, lsfq );
1673 120 : IF( LT_32( Mult_32_16( Err[0], ( st->streaklimit_fx ) ), L_add( Err[1], Mult_32_16( Err[1], PREFERSFNET_FX ) ) ) )
1674 : {
1675 66 : safety_net = 1;
1676 66 : move16();
1677 66 : st->pstreaklen = 0; /* Reset the consecutive predictive frame counter */
1678 66 : move16();
1679 : }
1680 : ELSE
1681 : {
1682 54 : safety_net = 0;
1683 54 : move16();
1684 54 : st->pstreaklen = add( st->pstreaklen, 1 ); /* Increase the consecutive predictive frame counter by one */
1685 54 : move16();
1686 : }
1687 : }
1688 : }
1689 : }
1690 :
1691 : /*--------------------------------------------------------------------------*
1692 : * Write indices to array
1693 : *--------------------------------------------------------------------------*/
1694 :
1695 268698 : test();
1696 268698 : IF( EQ_16( st->codec_mode, MODE1 ) && EQ_16( st->core, ACELP_CORE ) )
1697 : {
1698 : /* write coder_type bit for VOICED@16kHz or GENERIC@16kHz */
1699 156569 : test();
1700 156569 : test();
1701 156569 : IF( EQ_16( coder_type_org, GENERIC ) && EQ_32( st->sr_core, INT_FS_16k ) && ( st->idchan == 0 ) )
1702 : {
1703 : /* VOICED =2 and GENERIC=3, so "coder_type-2" means VOICED =0 and GENERIC=1*/
1704 61287 : push_indice( hBstr, IND_LSF_PREDICTOR_SELECT_BIT, sub( coder_type, 2 ), 1 );
1705 : }
1706 :
1707 : /* write predictor selection bit */
1708 156569 : IF( GE_16( predmode, 2 ) )
1709 : {
1710 39723 : push_indice( st->hBstr, IND_LSF_PREDICTOR_SELECT_BIT, safety_net, 1 );
1711 : }
1712 :
1713 156569 : test();
1714 156569 : test();
1715 156569 : IF( EQ_16( coder_type, VOICED ) && EQ_32( st->sr_core, INT_FS_16k ) && flag_1bit_gran == 0 )
1716 : {
1717 : /* BC-TCVQ (only for VOICED@16kHz) */
1718 0 : TCQIdx = &TCQIdx0[1];
1719 0 : Bit_alloc1 = &BC_TCVQ_BIT_ALLOC_40B[1];
1720 0 : FOR( i = 0; i < ( M / 2 ) + 3; i++ )
1721 : {
1722 0 : push_indice( hBstr, IND_LSF, TCQIdx[i], Bit_alloc1[i] );
1723 : }
1724 : }
1725 : ELSE
1726 : {
1727 156569 : cumleft = nBits;
1728 156569 : move16();
1729 156569 : IF( GE_16( predmode, 2 ) )
1730 : {
1731 : /* subtract predictor selection bit */
1732 39723 : cumleft = sub( nBits, 1 );
1733 : }
1734 :
1735 156569 : IF( safety_net )
1736 : {
1737 25543 : stages = stages0;
1738 25543 : move16();
1739 25543 : Idx = Idx0;
1740 25543 : move16();
1741 25543 : bits = bits0;
1742 25543 : move16();
1743 : }
1744 : ELSE
1745 : {
1746 131026 : stages = stages1;
1747 131026 : move16();
1748 131026 : Idx = Idx1;
1749 131026 : move16();
1750 131026 : bits = bits1;
1751 131026 : move16();
1752 : }
1753 :
1754 156569 : tmp = sub( stages, 1 );
1755 361262 : FOR( i = 0; i < tmp; i++ )
1756 : {
1757 204693 : indice[i] = Idx[i];
1758 204693 : move16();
1759 204693 : num_bits = bits[i];
1760 204693 : move16();
1761 204693 : cumleft -= num_bits;
1762 204693 : move16();
1763 204693 : push_indice( hBstr, IND_LSF, indice[i], num_bits );
1764 : }
1765 :
1766 589474 : WHILE( cumleft > 0 )
1767 : {
1768 432905 : indice[i] = Idx[i];
1769 432905 : move16();
1770 :
1771 432905 : IF( GT_16( cumleft, LEN_INDICE ) )
1772 : {
1773 276336 : num_bits = LEN_INDICE;
1774 276336 : move16();
1775 : }
1776 : ELSE
1777 : {
1778 156569 : num_bits = cumleft;
1779 156569 : move16();
1780 : }
1781 :
1782 432905 : cumleft = sub( cumleft, num_bits );
1783 432905 : push_indice( hBstr, IND_LSF, indice[i], num_bits );
1784 432905 : i = add( i, 1 );
1785 : }
1786 : }
1787 : }
1788 : ELSE
1789 : {
1790 112129 : test();
1791 112129 : test();
1792 112129 : IF( EQ_16( coder_type, VOICED ) && EQ_32( st->sr_core, INT_FS_16k ) && flag_1bit_gran == 0 )
1793 : {
1794 : /* BC-TCVQ (only for VOICED@16kHz) */
1795 : /* Number of quantization indices */
1796 0 : *no_indices = 10;
1797 0 : move16();
1798 0 : FOR( i = 0; i < *no_indices; i++ )
1799 : {
1800 0 : lpc_param[i] = TCQIdx0[i];
1801 0 : move16();
1802 0 : bits_param_lpc[i] = BC_TCVQ_BIT_ALLOC_40B[i];
1803 0 : move16();
1804 : }
1805 : }
1806 : ELSE
1807 : {
1808 : /* Number of quantization indices */
1809 :
1810 : /* there are 31 bits */
1811 112129 : IF( EQ_16( safety_net, 1 ) )
1812 : {
1813 12569 : Idx = Idx0;
1814 12569 : move16();
1815 12569 : *no_indices = add( stages0, 1 );
1816 37707 : FOR( i = 0; i < stages0; i++ )
1817 : {
1818 25138 : lpc_param[i] = Idx[i];
1819 25138 : move16();
1820 25138 : indice[i] = Idx[i];
1821 25138 : move16();
1822 25138 : bits_param_lpc[i] = bits0[i];
1823 25138 : move16();
1824 : }
1825 12569 : lpc_param[stages0] = Idx[stages0];
1826 12569 : move16();
1827 12569 : indice[stages0] = Idx[stages0];
1828 12569 : move16();
1829 12569 : tmp = sub( stages0, 1 );
1830 12569 : bits_param_lpc[tmp] = LEN_INDICE;
1831 12569 : move16();
1832 12569 : bits_param_lpc[stages0] = sub( bits0[tmp], LEN_INDICE );
1833 : }
1834 : ELSE
1835 : {
1836 99560 : *no_indices = add( stages1, 1 );
1837 99560 : Idx = Idx1;
1838 99560 : move16();
1839 274097 : FOR( i = 0; i < stages1; i++ )
1840 : {
1841 174537 : lpc_param[i] = ( Idx[i] );
1842 174537 : move16();
1843 174537 : indice[i] = Idx[i];
1844 174537 : move16();
1845 174537 : bits_param_lpc[i] = bits1[i];
1846 174537 : move16();
1847 : }
1848 99560 : lpc_param[stages1] = ( Idx[stages1] );
1849 99560 : move16();
1850 99560 : indice[stages1] = Idx[stages1];
1851 99560 : move16();
1852 99560 : tmp = sub( stages1, 1 );
1853 99560 : bits_param_lpc[tmp] = LEN_INDICE;
1854 99560 : move16();
1855 99560 : bits_param_lpc[stages1] = sub( bits1[tmp], LEN_INDICE );
1856 : }
1857 112129 : IF( EQ_16( predmode, 2 ) )
1858 : {
1859 124025 : FOR( i = *no_indices; i > 0; i-- )
1860 : {
1861 86873 : tmp = sub( i, 1 );
1862 86873 : lpc_param[i] = lpc_param[tmp];
1863 86873 : move16();
1864 86873 : bits_param_lpc[i] = bits_param_lpc[tmp];
1865 86873 : move16();
1866 : }
1867 37152 : lpc_param[0] = safety_net;
1868 37152 : move16(); /* put the safety net info on the last param */
1869 37152 : bits_param_lpc[0] = 1;
1870 37152 : move16();
1871 37152 : *no_indices = add( *no_indices, 1 );
1872 : }
1873 : }
1874 : }
1875 :
1876 : /*--------------------------------------------------------------------------*
1877 : * De-quantize encoded LSF vector
1878 : *--------------------------------------------------------------------------*/
1879 :
1880 268698 : IF( safety_net )
1881 : {
1882 : /* Safety-net */
1883 38112 : test();
1884 38112 : test();
1885 38112 : IF( EQ_16( coder_type, VOICED ) && EQ_32( st->sr_core, INT_FS_16k ) && flag_1bit_gran == 0 )
1886 : {
1887 : /* BC-TCQ */
1888 0 : Copy( lsfq, st->mem_MA_fx, M );
1889 0 : Vr_add( lsfq, pred0, qlsf, M );
1890 : }
1891 : ELSE
1892 : {
1893 38112 : IF( EQ_16( st->tdm_LRTD_flag, 0 ) && EQ_16( st->idchan, 1 ) && tdm_lsfQ_PCh != NULL )
1894 : {
1895 : /* intra mode*/
1896 66 : vq_dec_lvq_ivas_fx( 0, qlsf, &indice[0], stages0, M, 9, levels0[stages0 - 1] );
1897 66 : Vr_add( qlsf, pred3, qlsf, M );
1898 66 : Vr_subt( qlsf, pred1, st->mem_MA_fx, M );
1899 : }
1900 : ELSE
1901 : {
1902 38046 : vq_dec_lvq_ivas_fx( 1, qlsf, &indice[0], stages0, M, mode_lvq, levels0[stages0 - 1] );
1903 38046 : Vr_add( qlsf, pred0, qlsf, M );
1904 38046 : Vr_subt( qlsf, pred1, st->mem_MA_fx, M );
1905 : }
1906 : }
1907 : }
1908 : ELSE
1909 : {
1910 230586 : test();
1911 230586 : IF( EQ_16( coder_type, VOICED ) && EQ_32( st->sr_core, INT_FS_16k ) && flag_1bit_gran == 0 )
1912 : {
1913 : /* BC-TCVQ */
1914 0 : Copy( lsfq, st->mem_MA_fx, M );
1915 0 : Vr_add( lsfq, pred2, qlsf, M );
1916 : }
1917 : ELSE
1918 : {
1919 : /* LVQ */
1920 230586 : vq_dec_lvq_ivas_fx( 0, qlsf, &indice[0], stages1, M, mode_lvq_p, levels1[stages1 - 1] );
1921 230586 : IF( EQ_16( predmode, 1 ) )
1922 : {
1923 177570 : Copy( qlsf, st->mem_MA_fx, M );
1924 177570 : Vr_add( qlsf, pred1, qlsf, M );
1925 : }
1926 : ELSE
1927 : {
1928 53016 : Vr_add( qlsf, pred2, qlsf, M );
1929 53016 : Vr_subt( qlsf, pred1, st->mem_MA_fx, M );
1930 : }
1931 : }
1932 : }
1933 :
1934 : /* Sort the quantized vector to ascending order */
1935 268698 : sort_fx( qlsf, 0, M - 1 );
1936 :
1937 : /* Verify stability by adding minimum separation */
1938 268698 : reorder_lsf_fx( qlsf, MODE1_LSF_GAP_FX, M, st->sr_core );
1939 :
1940 : /* Update AR-predictor memories */
1941 268698 : Copy( qlsf, st->mem_AR_fx, M );
1942 :
1943 268698 : return;
1944 : }
1945 :
1946 :
1947 : /*-------------------------------------------------------------------*
1948 : * first_VQstages()
1949 : *
1950 : *
1951 : *-------------------------------------------------------------------*/
1952 :
1953 2247 : static void first_VQstages(
1954 : const Word16 *const *cb,
1955 : Word16 u[], /* i : vector to be encoded (prediction and mean removed) x2.56*/
1956 : Word16 *levels, /* i : number of levels in each stage */
1957 : Word16 stagesVQ, /* i : number of stages */
1958 : Word16 w[], /* i : weights Q8*/
1959 : Word16 N, /* i : vector dimension */
1960 : Word16 max_inner, /* i : maximum number of swaps in inner loop */
1961 : Word16 indices_VQstage[] )
1962 : {
1963 : Word16 resid_buf[2 * LSFMBEST * M], *resid[2];
1964 : Word32 dist_buf[2 * LSFMBEST], *dist[2], en;
1965 : Word32 f_tmp, L_tmp, L_tmp1, *pTmp32;
1966 : Word16 Tmp[M], *pTmp, cs;
1967 2247 : Word16 *pTmp_short, idx_buf[2 * LSFMBEST * MAX_VQ_STAGES], parents[LSFMBEST], counter = 0, j,
1968 : m, s, c, c2, p_max, *indices[2];
1969 2247 : Word16 maxC = LSFMBEST;
1970 :
1971 : /*float dd[16];*/
1972 : const Word16 *cb_stage, *cbp;
1973 :
1974 : /* Set pointers to previous (parent) and current node (parent node is indexed [0], current node is indexed [1]) */
1975 2247 : indices[0] = idx_buf;
1976 2247 : move16();
1977 2247 : indices[1] = idx_buf + maxC * stagesVQ;
1978 2247 : move16();
1979 2247 : resid[0] = resid_buf;
1980 2247 : move16();
1981 2247 : resid[1] = resid_buf + maxC * N;
1982 2247 : move16();
1983 2247 : dist[0] = dist_buf;
1984 2247 : move16();
1985 2247 : dist[1] = dist_buf + maxC;
1986 2247 : move16();
1987 :
1988 2247 : set16_fx( idx_buf, 0, ( const Word16 )( 2 * stagesVQ * maxC ) );
1989 2247 : set16_fx( parents, 0, maxC );
1990 :
1991 : /* Set up inital distance vector */
1992 2247 : L_tmp = L_deposit_l( 0 );
1993 38199 : FOR( j = 0; j < N; j++ )
1994 : {
1995 35952 : L_tmp1 = L_shl_sat( L_mult0( u[j], w[j] ), 7 ); /*x2.56 + Q8 + Q7 */
1996 35952 : L_tmp1 = Mult_32_16( L_tmp1, u[j] ); /*x2.56 + Q15 + x2.56 -Q15 */
1997 35952 : L_tmp = L_add( L_tmp, L_tmp1 ); /*Q0 + x2.56 +x2.56 */
1998 : }
1999 2247 : set32_fx( dist[1], L_tmp, maxC );
2000 :
2001 : /* Set up initial error (residual) vectors */
2002 2247 : pTmp = resid[1];
2003 2247 : move16();
2004 6741 : FOR( c = 0; c < maxC; c++ )
2005 : {
2006 4494 : Copy( u, pTmp, N );
2007 4494 : pTmp += N;
2008 : }
2009 :
2010 : /*----------------------------------------------------------------*
2011 : * LSF quantization
2012 : *----------------------------------------------------------------*/
2013 :
2014 : /* Loop over all stages */
2015 2247 : m = 1;
2016 2247 : move16();
2017 5037 : FOR( s = 0; s < stagesVQ; s++ )
2018 : {
2019 : /* set codebook pointer to point to first stage */
2020 2790 : cbp = cb[s];
2021 2790 : move16();
2022 :
2023 : /* save pointer to the beginning of the current stage */
2024 2790 : cb_stage = cbp;
2025 2790 : move16();
2026 :
2027 : /* swap pointers to parent and current nodes */
2028 2790 : pTmp_short = indices[0];
2029 2790 : indices[0] = indices[1];
2030 2790 : move16();
2031 2790 : indices[1] = pTmp_short;
2032 2790 : move16();
2033 :
2034 2790 : pTmp = resid[0];
2035 2790 : resid[0] = resid[1];
2036 2790 : move16();
2037 2790 : resid[1] = pTmp;
2038 2790 : move16();
2039 :
2040 2790 : pTmp32 = dist[0];
2041 2790 : dist[0] = dist[1];
2042 2790 : move32();
2043 2790 : dist[1] = pTmp32;
2044 2790 : move32();
2045 :
2046 : /* p_max points to maximum distortion node (worst of best) */
2047 2790 : p_max = 0;
2048 2790 : move16();
2049 :
2050 : /* set distortions to a large value */
2051 2790 : set32_fx( dist[1], MAXINT32, maxC );
2052 :
2053 68374 : FOR( j = 0; j < levels[s]; j++ )
2054 : {
2055 : /* compute weighted codebook element and its energy */
2056 1114928 : FOR( c2 = 0; c2 < N; c2++ )
2057 : {
2058 1049344 : Tmp[c2] = shl( mult( w[c2], cbp[c2] ), 2 ); /* Q8 + x2.56 -Q15 +Q2 */
2059 1049344 : move16();
2060 : }
2061 :
2062 65584 : en = L_mult( cbp[0], Tmp[0] );
2063 :
2064 1049344 : FOR( c2 = 1; c2 < N; c2++ )
2065 : {
2066 983760 : en = L_mac( en, cbp[c2], Tmp[c2] ); /*x2.56 + x2.56 + Q-5 +Q1 */
2067 : }
2068 65584 : cbp += N;
2069 65584 : move16();
2070 :
2071 : /* iterate over all parent nodes */
2072 138200 : FOR( c = 0; c < m; c++ )
2073 : {
2074 72616 : pTmp = &resid[0][c * N];
2075 72616 : move16();
2076 72616 : L_tmp = L_mult( pTmp[0], Tmp[0] );
2077 1161856 : FOR( c2 = 1; c2 < N; c2++ )
2078 : {
2079 1089240 : L_tmp = L_mac( L_tmp, pTmp[c2], Tmp[c2] ); /* */
2080 : }
2081 :
2082 72616 : L_tmp = L_add( dist[0][c], L_sub( en, L_shl( L_tmp, 1 ) ) );
2083 :
2084 72616 : IF( LE_32( L_tmp, dist[1][p_max] ) )
2085 : {
2086 : /* replace worst */
2087 20766 : dist[1][p_max] = L_tmp;
2088 20766 : move32();
2089 20766 : indices[1][p_max * stagesVQ + s] = j;
2090 20766 : move16();
2091 20766 : parents[p_max] = c;
2092 20766 : move16();
2093 :
2094 : /* limit number of times inner loop is entered */
2095 20766 : IF( LT_16( counter, max_inner ) )
2096 : {
2097 20766 : counter = add( counter, 1 );
2098 20766 : IF( LT_16( counter, max_inner ) )
2099 : {
2100 : /* find new worst */
2101 20766 : p_max = maximum_32_fx( dist[1], maxC, &f_tmp );
2102 : }
2103 : ELSE
2104 : {
2105 : /* find minimum distortion */
2106 0 : p_max = minimum_32_fx( dist[1], maxC, &f_tmp );
2107 : }
2108 : }
2109 : }
2110 : }
2111 : }
2112 :
2113 : /*------------------------------------------------------------*
2114 : * Compute error vectors for each node
2115 : *------------------------------------------------------------*/
2116 :
2117 2790 : cs = 0;
2118 2790 : move16();
2119 8370 : FOR( c = 0; c < maxC; c++ )
2120 : {
2121 : /* subtract codebook entry from the residual vector of the parent node */
2122 5580 : pTmp = resid[1] + c * N;
2123 5580 : move16();
2124 5580 : Copy( resid[0] + parents[c] * N, pTmp, N );
2125 5580 : Vr_subt( pTmp, cb_stage + ( indices[1][cs + s] ) * N, pTmp, N );
2126 :
2127 : /* get indices that were used for parent node */
2128 5580 : Copy( indices[0] + parents[c] * stagesVQ, indices[1] + cs, s );
2129 5580 : cs = add( cs, stagesVQ );
2130 : }
2131 :
2132 2790 : m = maxC;
2133 2790 : move16();
2134 : }
2135 :
2136 2247 : Copy( indices[1], indices_VQstage, maxC * stagesVQ );
2137 :
2138 2247 : return;
2139 : }
2140 :
2141 :
2142 302187 : static void first_VQstages_ivas_fx(
2143 : const Word16 *const *cb,
2144 : Word16 u[], /* i : vector to be encoded (prediction and mean removed) */
2145 : Word16 *levels, /* i : number of levels in each stage */
2146 : Word16 stagesVQ, /* i : number of stages */
2147 : Word16 w[], /* i : weights */
2148 : Word16 N, /* i : vector dimension */
2149 : Word16 max_inner, /* i : maximum number of swaps in inner loop */
2150 : Word16 indices_VQstage[] )
2151 : {
2152 : Word16 resid_buf[2 * LSFMBEST * M], *resid[2];
2153 : Word32 dist_buf[2 * LSFMBEST], *dist[2], en;
2154 : Word32 f_tmp, L_tmp, L_tmp1, *pTmp32;
2155 : Word16 Tmp[M], *pTmp, cs;
2156 302187 : Word16 *pTmp_short, idx_buf[2 * LSFMBEST * MAX_VQ_STAGES], parents[LSFMBEST], counter = 0, j,
2157 : m, s, c, c2, p_max, *indices[2];
2158 302187 : Word16 maxC = LSFMBEST;
2159 :
2160 : /*float dd[16];*/
2161 : const Word16 *cb_stage, *cbp;
2162 :
2163 : /* Set pointers to previous (parent) and current node (parent node is indexed [0], current node is indexed [1]) */
2164 302187 : indices[0] = idx_buf;
2165 302187 : move16();
2166 302187 : indices[1] = idx_buf + maxC * stagesVQ;
2167 302187 : move16();
2168 302187 : resid[0] = resid_buf;
2169 302187 : move16();
2170 302187 : resid[1] = resid_buf + maxC * N;
2171 302187 : move16();
2172 302187 : dist[0] = dist_buf;
2173 302187 : move16();
2174 302187 : dist[1] = dist_buf + maxC;
2175 302187 : move16();
2176 :
2177 302187 : set16_fx( idx_buf, 0, ( const Word16 )( 2 * stagesVQ * maxC ) );
2178 302187 : set16_fx( parents, 0, maxC );
2179 :
2180 : /* Set up inital distance vector */
2181 302187 : L_tmp = L_deposit_l( 0 );
2182 5137179 : FOR( j = 0; j < N; j++ )
2183 : {
2184 4834992 : L_tmp1 = L_shl_sat( L_mult0( u[j], w[j] ), 7 ); /*x2.56 + Q8 + Q7 */
2185 4834992 : L_tmp1 = Mult_32_16( L_tmp1, u[j] ); /*x2.56 + Q15 + x2.56 -Q15 */
2186 4834992 : L_tmp = L_add( L_tmp, L_tmp1 ); /*Q0 + x2.56 +x2.56 */
2187 : }
2188 302187 : set32_fx( dist[1], L_shr( L_tmp, 1 ), maxC ); /*Q-1 + x2.56 +x2.56 */
2189 :
2190 : /* Set up initial error (residual) vectors */
2191 302187 : pTmp = resid[1];
2192 302187 : move16();
2193 906561 : FOR( c = 0; c < maxC; c++ )
2194 : {
2195 604374 : Copy( u, pTmp, N );
2196 604374 : pTmp += N;
2197 : }
2198 :
2199 : /*----------------------------------------------------------------*
2200 : * LSF quantization
2201 : *----------------------------------------------------------------*/
2202 :
2203 : /* Loop over all stages */
2204 302187 : m = 1;
2205 302187 : move16();
2206 689678 : FOR( s = 0; s < stagesVQ; s++ )
2207 : {
2208 : /* set codebook pointer to point to first stage */
2209 387491 : cbp = cb[s];
2210 387491 : move16();
2211 :
2212 : /* save pointer to the beginning of the current stage */
2213 387491 : cb_stage = cbp;
2214 387491 : move16();
2215 :
2216 : /* swap pointers to parent and current nodes */
2217 387491 : pTmp_short = indices[0];
2218 387491 : indices[0] = indices[1];
2219 387491 : move16();
2220 387491 : indices[1] = pTmp_short;
2221 387491 : move16();
2222 :
2223 387491 : pTmp = resid[0];
2224 387491 : resid[0] = resid[1];
2225 387491 : move16();
2226 387491 : resid[1] = pTmp;
2227 387491 : move16();
2228 :
2229 387491 : pTmp32 = dist[0];
2230 387491 : dist[0] = dist[1];
2231 387491 : move32();
2232 387491 : dist[1] = pTmp32;
2233 387491 : move32();
2234 :
2235 : /* p_max points to maximum distortion node (worst of best) */
2236 387491 : p_max = 0;
2237 387491 : move16();
2238 :
2239 : /* set distortions to a large value */
2240 387491 : set32_fx( dist[1], MAXINT32, maxC );
2241 :
2242 9072259 : FOR( j = 0; j < levels[s]; j++ )
2243 : {
2244 : /* compute weighted codebook element and its energy */
2245 147641056 : FOR( c2 = 0; c2 < N; c2++ )
2246 : {
2247 138956288 : Tmp[c2] = extract_h( L_shl( L_mult0( w[c2], cbp[c2] ), 6 ) ); /* Q8 + x2.56 + q6 -q16 */
2248 138956288 : move16();
2249 : }
2250 :
2251 8684768 : en = L_mult( cbp[0], Tmp[0] ); /*x2.56 + x2.56 + Q-2 +Q1 */
2252 :
2253 138956288 : FOR( c2 = 1; c2 < N; c2++ )
2254 : {
2255 130271520 : en = L_mac( en, cbp[c2], Tmp[c2] ); /*x2.56 + x2.56 + Q-2 +Q1 */
2256 : }
2257 8684768 : cbp += N;
2258 8684768 : move16();
2259 :
2260 : /* iterate over all parent nodes */
2261 18465528 : FOR( c = 0; c < m; c++ )
2262 : {
2263 9780760 : pTmp = &resid[0][c * N]; /*x2.56*/
2264 9780760 : move16();
2265 9780760 : L_tmp = L_mult( pTmp[0], Tmp[0] ); /*x2.56 + x2.56 + Q-2 +Q1 */
2266 156492160 : FOR( c2 = 1; c2 < N; c2++ )
2267 : {
2268 146711400 : L_tmp = L_mac( L_tmp, pTmp[c2], Tmp[c2] ); /*x2.56 + x2.56 + Q-2 +Q1 */
2269 : }
2270 :
2271 9780760 : L_tmp = L_add( dist[0][c], L_sub( en, L_shl( L_tmp, 1 ) ) ); /*x2.56 + x2.56 -1*/
2272 :
2273 9780760 : IF( LE_32( L_tmp, dist[1][p_max] ) )
2274 : {
2275 : /* replace worst */
2276 2836769 : dist[1][p_max] = L_tmp;
2277 2836769 : move32();
2278 2836769 : indices[1][p_max * stagesVQ + s] = j;
2279 2836769 : move16();
2280 2836769 : parents[p_max] = c;
2281 2836769 : move16();
2282 :
2283 : /* limit number of times inner loop is entered */
2284 2836769 : IF( LT_16( counter, max_inner ) )
2285 : {
2286 2836769 : counter = add( counter, 1 );
2287 2836769 : IF( LT_16( counter, max_inner ) )
2288 : {
2289 : /* find new worst */
2290 2836769 : p_max = maximum_32_fx( dist[1], maxC, &f_tmp );
2291 : }
2292 : ELSE
2293 : {
2294 : /* find minimum distortion */
2295 0 : p_max = minimum_32_fx( dist[1], maxC, &f_tmp );
2296 : }
2297 : }
2298 : }
2299 : }
2300 : }
2301 :
2302 : /*------------------------------------------------------------*
2303 : * Compute error vectors for each node
2304 : *------------------------------------------------------------*/
2305 :
2306 387491 : cs = 0;
2307 387491 : move16();
2308 1162473 : FOR( c = 0; c < maxC; c++ )
2309 : {
2310 : /* subtract codebook entry from the residual vector of the parent node */
2311 774982 : pTmp = resid[1] + c * N;
2312 774982 : move16();
2313 774982 : Copy( resid[0] + parents[c] * N, pTmp, N );
2314 774982 : Vr_subt( pTmp, cb_stage + ( indices[1][cs + s] ) * N, pTmp, N );
2315 :
2316 : /* get indices that were used for parent node */
2317 774982 : Copy( indices[0] + parents[c] * stagesVQ, indices[1] + cs, s );
2318 774982 : cs = add( cs, stagesVQ );
2319 : }
2320 :
2321 387491 : m = maxC;
2322 387491 : move16();
2323 : }
2324 :
2325 302187 : Copy( indices[1], indices_VQstage, maxC * stagesVQ );
2326 :
2327 302187 : return;
2328 : }
2329 :
2330 :
2331 : /*---------------------------------------------------------------------------
2332 : * vq_enc_lsf_lvq()
2333 : *
2334 : * Multi-stage VQ encoder for LSF quantization. Trained codebooks are used in initial stages
2335 : * and lattice-VQ quantization is applied on residual vector in other stages.
2336 : *
2337 : * Note:
2338 : * Compared to normal multistage VQ resulting LSF vector is reordered to ascending order before
2339 : * weighted error calculation (spectral distortion) at the final stage.
2340 : *
2341 : * Returns:
2342 : * Weighted error
2343 : *--------------------------------------------------------------------------*/
2344 :
2345 2508 : static Word32 vq_lvq_lsf_enc(
2346 : Word16 pred_flag,
2347 : Word16 mode,
2348 : Word16 u[], // x2.56
2349 : Word16 *levels,
2350 : Word16 stages,
2351 : Word16 w[], // Q8
2352 : Word16 Idx[],
2353 : const Word16 *lsf,
2354 : const Word16 *pred,
2355 : Word32 p_offset_scale1[][MAX_NO_SCALES + 1],
2356 : Word32 p_offset_scale2[][MAX_NO_SCALES + 1],
2357 : Word16 p_no_scales[][2],
2358 : Word16 *resq,
2359 : Word16 *lsfq )
2360 : {
2361 : Word16 i;
2362 : const Word16 *const *cb, *cb_stage;
2363 : Word16 cand[LSFMBEST][M];
2364 2508 : Word16 maxC = LSFMBEST, stagesVQ;
2365 : Word16 mode_glb, j, indices_firstVQ[LSFMBEST * MAX_VQ_STAGES], c2;
2366 : Word32 e[LSFMBEST], L_tmp, L_ftmp;
2367 : Word16 quant[LSFMBEST][M], diff[M], dd[M];
2368 : Word16 lat_cv[LSFMBEST][M];
2369 : Word16 idx_lead[LSFMBEST][2], idx_scale[LSFMBEST][2];
2370 :
2371 2508 : stagesVQ = sub( stages, 1 );
2372 : /* Codebook selection */
2373 2508 : IF( pred_flag == 0 ) /* safety net*/
2374 : {
2375 598 : cb = &Quantizers_fx[CB_lsf[mode]];
2376 598 : move16();
2377 598 : mode_glb = add( offset_lvq_modes_SN_fx[mode], offset_in_lvq_mode_SN_fx[mode][( levels[stagesVQ] - min_lat_bits_SN_fx[mode] )] ); // Q0
2378 : }
2379 : ELSE /* predictive */
2380 : {
2381 1910 : cb = &Quantizers_p_fx[CB_p_lsf[mode]];
2382 1910 : move16();
2383 1910 : mode_glb = add( offset_lvq_modes_pred_fx[mode], offset_in_lvq_mode_pred_fx[mode][( levels[stagesVQ] - min_lat_bits_pred_fx[mode] )] ); // Q0
2384 : }
2385 2508 : IF( stagesVQ > 0 )
2386 : {
2387 : /* first VQ stages */
2388 2247 : first_VQstages( cb, u, levels, stagesVQ, w, M, MSVQ_MAXCNT, indices_firstVQ );
2389 : }
2390 :
2391 7524 : FOR( i = 0; i < maxC; i++ )
2392 : {
2393 5016 : Copy( pred, cand[i], M );
2394 10596 : FOR( j = 0; j < stagesVQ; j++ )
2395 : {
2396 5580 : Idx[j] = indices_firstVQ[i * stagesVQ + j];
2397 5580 : move16();
2398 : }
2399 :
2400 10596 : FOR( j = 0; j < stagesVQ; j++ )
2401 : {
2402 5580 : cb_stage = cb[j];
2403 5580 : move16();
2404 5580 : Vr_add( cand[i], cb_stage + Idx[j] * M, cand[i], M );
2405 : }
2406 :
2407 : /* LVQ quantization */
2408 5016 : Vr_subt( lsf, cand[i], dd, M );
2409 5016 : mslvq_fx( dd, quant[i], lat_cv[i], idx_lead[i], idx_scale[i], w, mode, mode_glb, pred_flag, p_no_scales );
2410 5016 : Vr_add( cand[i], quant[i], cand[i], M );
2411 :
2412 : /* arrange the LSF candidate vector prior to selection to an ascending order*/
2413 5016 : sort_fx( cand[i], 0, M - 1 );
2414 :
2415 : /* calculate the spectral distortion using weighted MSE of sorted LSF vector*/
2416 5016 : Vr_subt( cand[i], lsf, diff, M );
2417 85272 : FOR( j = 0; j < M; j++ )
2418 : {
2419 80256 : diff[j] = shl_sat( diff[j], 4 );
2420 80256 : move16();
2421 : }
2422 5016 : L_tmp = L_mult( mult( diff[0], shl_sat( w[0], 1 ) ), diff[0] ); /*(2.56+Q5+ Q10 -Q15) + 2.56+ Q5 + Q1 = 2.56 + 2.56 + Q6 */
2423 80256 : FOR( j = 1; j < M; j++ )
2424 : {
2425 75240 : L_tmp = L_mac( L_tmp, mult( diff[j], shl_sat( w[j], 1 ) ), diff[j] ); /*(2.56+Q5+ Q10 -Q15) + 2.56+ Q5 + Q1 = 2.56 + 2.56 + Q6 */
2426 : }
2427 5016 : e[i] = L_tmp; /*(2.56+Q5+ Q10 -Q15) + 2.56+ Q5 + Q1 = 2.56 + 2.56 + Q6 */
2428 5016 : move32();
2429 : }
2430 :
2431 : /* find the optimal candidate */
2432 2508 : c2 = minimum_32_fx( e, maxC, &L_ftmp );
2433 2508 : set16_fx( resq, 0, M );
2434 5298 : FOR( j = 0; j < stagesVQ; j++ )
2435 : {
2436 2790 : Idx[j] = indices_firstVQ[c2 * stagesVQ + j];
2437 2790 : move16();
2438 2790 : cb_stage = cb[j];
2439 2790 : move16();
2440 2790 : Vr_add( resq, cb_stage + Idx[j] * M, resq, M );
2441 : }
2442 2508 : Vr_add( resq, quant[c2], resq, M ); /* quantized prediction residual */
2443 2508 : Copy( cand[c2], lsfq, M );
2444 2508 : index_lvq_fx( lat_cv[c2], idx_lead[c2], idx_scale[c2], mode_glb, &Idx[stagesVQ],
2445 : &p_offset_scale1[0][0], &p_offset_scale2[0][0], &p_no_scales[0][0] );
2446 :
2447 2508 : return e[c2];
2448 : }
2449 :
2450 :
2451 345537 : static Word32 vq_lvq_lsf_enc_ivas_fx(
2452 : Word16 pred_flag,
2453 : Word16 mode,
2454 : Word16 u[], // x2.56
2455 : Word16 *levels,
2456 : Word16 stages,
2457 : Word16 w[], // Q8
2458 : Word16 Idx[],
2459 : const Word16 *lsf,
2460 : const Word16 *pred,
2461 : Word16 *resq,
2462 : Word16 *lsfq )
2463 : {
2464 : Word16 i;
2465 : const Word16 *const *cb, *cb_stage;
2466 : Word16 cand[LSFMBEST][M];
2467 345537 : Word16 maxC = LSFMBEST, stagesVQ;
2468 : Word16 mode_glb, j, indices_firstVQ[LSFMBEST * MAX_VQ_STAGES], c2;
2469 : Word32 e[LSFMBEST], L_tmp, L_ftmp;
2470 : Word16 quant[LSFMBEST][M], diff[M], dd[M];
2471 : Word16 lat_cv[LSFMBEST][M];
2472 : Word16 idx_lead[LSFMBEST][2], idx_scale[LSFMBEST][2];
2473 :
2474 345537 : stagesVQ = sub( stages, 1 );
2475 : /* Codebook selection */
2476 345537 : IF( pred_flag == 0 ) /* safety net*/
2477 : {
2478 91008 : cb = &Quantizers_fx[CB_lsf[mode]];
2479 91008 : IF( LT_16( mode, 6 ) )
2480 : {
2481 0 : move16();
2482 0 : mode_glb = add( offset_lvq_modes_SN[mode], offset_in_lvq_mode_SN[mode][( levels[stagesVQ] - min_lat_bits_SN[mode] )] );
2483 : }
2484 : ELSE
2485 : {
2486 91008 : move16();
2487 91008 : mode_glb = add( offset_lvq_modes_SN[mode], levels[stagesVQ] - min_lat_bits_SN[mode] );
2488 : }
2489 : }
2490 : ELSE /* predictive */
2491 : {
2492 254529 : cb = &Quantizers_p_fx[CB_p_lsf[mode]];
2493 254529 : IF( LT_16( mode, 6 ) || EQ_16( mode, 12 ) )
2494 : {
2495 5265 : move16();
2496 5265 : mode_glb = add( offset_lvq_modes_pred[mode], offset_in_lvq_mode_pred[mode][( levels[stagesVQ] - min_lat_bits_pred_fx[mode] )] );
2497 : }
2498 : ELSE
2499 : {
2500 249264 : move16();
2501 249264 : mode_glb = sub( add( offset_lvq_modes_pred[mode], levels[stagesVQ] ), min_lat_bits_pred_fx[mode] );
2502 : }
2503 : }
2504 :
2505 345537 : IF( stagesVQ > 0 )
2506 : {
2507 : /* first VQ stages */
2508 302187 : first_VQstages_ivas_fx( cb, u, levels, stagesVQ, w, M, MSVQ_MAXCNT, indices_firstVQ );
2509 : }
2510 :
2511 1036611 : FOR( i = 0; i < maxC; i++ )
2512 : {
2513 691074 : Copy( pred, cand[i], M );
2514 1466056 : FOR( j = 0; j < stagesVQ; j++ )
2515 : {
2516 774982 : Idx[j] = indices_firstVQ[i * stagesVQ + j];
2517 774982 : move16();
2518 : }
2519 :
2520 1466056 : FOR( j = 0; j < stagesVQ; j++ )
2521 : {
2522 774982 : cb_stage = cb[j];
2523 774982 : move16();
2524 774982 : Vr_add( cand[i], cb_stage + Idx[j] * M, cand[i], M );
2525 : }
2526 :
2527 : /* LVQ quantization */
2528 691074 : Vr_subt( lsf, cand[i], dd, M );
2529 691074 : mslvq_ivas_16( dd, quant[i], lat_cv[i], idx_lead[i], idx_scale[i], w, mode, mode_glb, pred_flag );
2530 691074 : Vr_add( cand[i], quant[i], cand[i], M );
2531 :
2532 : /* arrange the LSF candidate vector prior to selection to an ascending order*/
2533 691074 : sort_fx( cand[i], 0, M - 1 );
2534 :
2535 : /* calculate the spectral distortion using weighted MSE of sorted LSF vector*/
2536 691074 : Vr_subt( cand[i], lsf, diff, M );
2537 11748258 : FOR( j = 0; j < M; j++ )
2538 : {
2539 11057184 : diff[j] = shl_sat( diff[j], 4 );
2540 11057184 : move16();
2541 : }
2542 691074 : L_tmp = L_mult( mult( diff[0], shl_sat( w[0], 1 ) ), diff[0] ); /*(2.56+Q5+ Q10 -Q15) + 2.56+ Q5 + Q1 = 2.56 + 2.56 + Q6 */
2543 11057184 : FOR( j = 1; j < M; j++ )
2544 : {
2545 10366110 : L_tmp = L_mac_sat( L_tmp, mult( diff[j], shl_sat( w[j], 1 ) ), diff[j] ); /*(2.56+Q5+ Q10 -Q15) + 2.56+ Q5 + Q1 = 2.56 + 2.56 + Q6 */
2546 : }
2547 691074 : e[i] = L_tmp;
2548 691074 : move32();
2549 : }
2550 :
2551 : /* find the optimal candidate */
2552 345537 : c2 = minimum_32_fx( e, maxC, &L_ftmp );
2553 345537 : set16_fx( resq, 0, M );
2554 733028 : FOR( j = 0; j < stagesVQ; j++ )
2555 : {
2556 387491 : Idx[j] = indices_firstVQ[c2 * stagesVQ + j];
2557 387491 : move16();
2558 387491 : cb_stage = cb[j];
2559 387491 : move16();
2560 387491 : Vr_add( resq, cb_stage + Idx[j] * M, resq, M );
2561 : }
2562 345537 : Vr_add( resq, quant[c2], resq, M ); /* quantized prediction residual */
2563 345537 : Copy( cand[c2], lsfq, M );
2564 345537 : index_lvq_ivas_fx( lat_cv[c2], idx_lead[c2], idx_scale[c2], mode_glb, &Idx[stagesVQ], pred_flag );
2565 :
2566 345537 : return e[c2];
2567 : }
2568 :
2569 :
2570 629 : static void BcTcvq_1st_fx(
2571 : Word16 x_fx[][2], /*x2.56*/
2572 : const Word16 CB_fx[][128][2], /*x2.56*/
2573 : Word16 s[][16],
2574 : Word16 c[][16],
2575 : Word32 cDist_fx[][16], /*2.56*2.56*Q(-5 - 2)*/
2576 : Word16 Q_fx[][16][2],
2577 : Word16 W_fx[][2] /*Q8*/
2578 : )
2579 : {
2580 : Word16 state, prev_state;
2581 : Word16 index, bestCode;
2582 : Word32 dist_fx, minDist_fx;
2583 : Word16 temp16_fx;
2584 :
2585 5661 : FOR( state = 0; state < NUM_STATE; state += 2 )
2586 : {
2587 5032 : prev_state = NTRANS[0][state];
2588 5032 : move16();
2589 5032 : index = NTRANS[2][state];
2590 5032 : move16();
2591 5032 : temp16_fx = sub( x_fx[0][0], CB_fx[0][index][0] );
2592 5032 : minDist_fx = Mult_32_16( L_mult0( temp16_fx, temp16_fx ), W_fx[0][0] ); /* 2.56*2.56*Q(-5) */
2593 5032 : temp16_fx = sub( x_fx[0][1], CB_fx[0][index][1] );
2594 5032 : minDist_fx = L_add( minDist_fx, Mult_32_16( L_mult0( temp16_fx, temp16_fx ), W_fx[0][1] ) ); /* 2.56*2.56*Q(-5) */
2595 5032 : bestCode = index;
2596 5032 : move16();
2597 :
2598 80512 : FOR( index = index + 8; index < 128; index += 8 )
2599 : {
2600 75480 : temp16_fx = sub( x_fx[0][0], CB_fx[0][index][0] );
2601 75480 : dist_fx = Mult_32_16( L_mult0( temp16_fx, temp16_fx ), W_fx[0][0] ); /* 2.56*2.56*Q(-5) */
2602 75480 : temp16_fx = sub( x_fx[0][1], CB_fx[0][index][1] );
2603 75480 : dist_fx = L_add( dist_fx, Mult_32_16( L_mult0( temp16_fx, temp16_fx ), W_fx[0][1] ) ); /* 2.56*2.56*Q(-5) */
2604 :
2605 75480 : if ( LT_32( dist_fx, minDist_fx ) )
2606 : {
2607 16632 : bestCode = index;
2608 16632 : move16();
2609 : }
2610 75480 : minDist_fx = L_min( minDist_fx, dist_fx );
2611 : }
2612 :
2613 : /* Update */
2614 5032 : s[0][state] = prev_state;
2615 5032 : move16();
2616 5032 : c[0][state] = bestCode;
2617 5032 : move16();
2618 :
2619 5032 : cDist_fx[0][state] = L_shr( minDist_fx, 2 );
2620 5032 : move32(); /*2.56*2.56*Q(-5 - 2)*/
2621 5032 : Q_fx[0][state][0] = CB_fx[0][bestCode][0];
2622 5032 : move16();
2623 5032 : Q_fx[0][state][1] = CB_fx[0][bestCode][1];
2624 5032 : move16();
2625 : }
2626 :
2627 629 : return;
2628 : }
2629 :
2630 :
2631 629 : static void BcTcvq_2nd_fx(
2632 : Word16 x_fx[][2], /*x2.56*/
2633 : const Word16 CB_fx[][128][2], /*x2.56*/
2634 : Word16 s[][16],
2635 : Word16 c[][16],
2636 : Word32 cDist_fx[][16], /*2.56*2.56*Q(-5 - 2) */
2637 : Word16 Q_fx[][16][2], /*x2.56*/
2638 : Word16 W_fx[][2], /*Q8*/
2639 : const Word16 itc_fx[][2][2] /*Q15*/
2640 : )
2641 : {
2642 : Word16 state, prev_state;
2643 : Word16 index, bestCode;
2644 : Word32 dist_fx, minDist_fx;
2645 : Word16 pred_fx[N_DIM], target_fx[N_DIM];
2646 : Word16 temp16_fx;
2647 :
2648 10693 : FOR( state = 0; state < NUM_STATE; state++ )
2649 : {
2650 10064 : prev_state = NTRANS[0][state];
2651 10064 : move16();
2652 10064 : index = NTRANS[2][state];
2653 10064 : move16();
2654 :
2655 : /* Prediction */
2656 10064 : pred_fx[0] = add( mult_r( itc_fx[0][0][0], Q_fx[0][prev_state][0] ), mult_r( itc_fx[0][0][1], Q_fx[0][prev_state][1] ) );
2657 10064 : move16();
2658 10064 : pred_fx[1] = add( mult_r( itc_fx[0][1][0], Q_fx[0][prev_state][0] ), mult_r( itc_fx[0][1][1], Q_fx[0][prev_state][1] ) );
2659 10064 : move16();
2660 10064 : target_fx[0] = sub( x_fx[1][0], pred_fx[0] );
2661 10064 : move16(); /* x2.65 */
2662 10064 : target_fx[1] = sub( x_fx[1][1], pred_fx[1] );
2663 10064 : move16(); /* x2.65 */
2664 :
2665 10064 : temp16_fx = sub( target_fx[0], CB_fx[1][index][0] );
2666 10064 : minDist_fx = Mult_32_16( L_mult0( temp16_fx, temp16_fx ), W_fx[1][0] ); /* 2.65*2.65*Q(-5) */
2667 10064 : temp16_fx = sub( target_fx[1], CB_fx[1][index][1] );
2668 10064 : minDist_fx = L_add( minDist_fx, Mult_32_16( L_mult0( temp16_fx, temp16_fx ), W_fx[1][1] ) ); /* 2.65*2.65*Q(-5) */
2669 :
2670 10064 : bestCode = index;
2671 10064 : move16();
2672 :
2673 161024 : FOR( index = index + 8; index < 128; index += 8 )
2674 : {
2675 150960 : temp16_fx = sub( target_fx[0], CB_fx[1][index][0] );
2676 150960 : dist_fx = Mult_32_16( L_mult0( temp16_fx, temp16_fx ), W_fx[1][0] ); /* 2.65*2.65*Q(-5) */
2677 150960 : temp16_fx = sub( target_fx[1], CB_fx[1][index][1] );
2678 150960 : dist_fx = L_add( dist_fx, Mult_32_16( L_mult0( temp16_fx, temp16_fx ), W_fx[1][1] ) ); /* 2.65*2.65*Q(-5) */
2679 :
2680 150960 : if ( LT_32( dist_fx, minDist_fx ) )
2681 : {
2682 29286 : bestCode = index;
2683 29286 : move16();
2684 : }
2685 150960 : minDist_fx = L_min( minDist_fx, dist_fx );
2686 : }
2687 :
2688 : /* Update */
2689 10064 : s[1][state] = prev_state;
2690 10064 : move16();
2691 10064 : c[1][state] = bestCode;
2692 10064 : move16();
2693 :
2694 10064 : cDist_fx[1][state] = L_add( cDist_fx[0][prev_state], L_shr( minDist_fx, 2 ) );
2695 10064 : move32(); /* 2.56*2.56*Q(-5 - 2) */
2696 10064 : Q_fx[1][state][0] = add( CB_fx[1][bestCode][0], pred_fx[0] );
2697 10064 : move16();
2698 10064 : Q_fx[1][state][1] = add( CB_fx[1][bestCode][1], pred_fx[1] );
2699 10064 : move16();
2700 : }
2701 :
2702 629 : return;
2703 : }
2704 :
2705 1258 : static void BcTcvq_SubBlock_fx(
2706 : Word16 x_fx[][2], /*x2.56*/
2707 : const Word16 CB_fx[][64][2], /*x2.56*/
2708 : Word16 s[][16],
2709 : Word16 c[][16],
2710 : Word32 cDist_fx[][16], /*2.56*2.56*Q(-5 - 2)*/
2711 : Word16 Q_fx[][16][2],
2712 : Word16 stage,
2713 : Word16 W_fx[][2], /*Q10*/
2714 : const Word16 itc_fx[][2][2] /*Q15*/
2715 : )
2716 : {
2717 : Word16 stage1, stage2, state, prev_state, branch;
2718 : Word16 index, bestCode, brCode[N_DIM];
2719 : Word16 temp16_fx;
2720 : Word32 dist_fx, minDist_fx, brDist_fx[N_DIM];
2721 : Word16 pred_fx[N_DIM], target_fx[N_DIM], brQuant_fx[N_DIM][N_DIM];
2722 :
2723 1258 : stage1 = sub( stage, 1 );
2724 1258 : stage2 = sub( stage, 2 );
2725 :
2726 21386 : FOR( state = 0; state < NUM_STATE; state++ )
2727 : {
2728 : /* 1st branch search */
2729 20128 : prev_state = NTRANS[0][state];
2730 20128 : move16();
2731 20128 : index = NTRANS[2][state];
2732 20128 : move16();
2733 :
2734 : /* Prediction */
2735 20128 : pred_fx[0] = add( mult_r( itc_fx[stage1][0][0], Q_fx[stage1][prev_state][0] ), mult_r( itc_fx[stage1][0][1], Q_fx[stage1][prev_state][1] ) );
2736 20128 : move16();
2737 20128 : pred_fx[1] = add( mult_r( itc_fx[stage1][1][0], Q_fx[stage1][prev_state][0] ), mult_r( itc_fx[stage1][1][1], Q_fx[stage1][prev_state][1] ) );
2738 20128 : move16();
2739 20128 : target_fx[0] = sub( x_fx[stage][0], pred_fx[0] );
2740 20128 : move16();
2741 20128 : target_fx[1] = sub( x_fx[stage][1], pred_fx[1] );
2742 20128 : move16();
2743 :
2744 20128 : temp16_fx = sub( target_fx[0], CB_fx[stage2][index][0] );
2745 20128 : minDist_fx = Mult_32_16( L_mult0( temp16_fx, temp16_fx ), W_fx[stage][0] ); /* 2.65*2.65*Q(-5) */
2746 20128 : temp16_fx = sub( target_fx[1], CB_fx[stage2][index][1] );
2747 20128 : minDist_fx = L_add( minDist_fx, Mult_32_16( L_mult0( temp16_fx, temp16_fx ), W_fx[stage][1] ) ); /* 2.65*2.65*Q(-5) */
2748 :
2749 20128 : bestCode = index;
2750 20128 : move16();
2751 :
2752 161024 : FOR( index = index + 8; index < 64; index += 8 )
2753 : {
2754 140896 : temp16_fx = sub( target_fx[0], CB_fx[stage2][index][0] );
2755 140896 : dist_fx = Mult_32_16( L_mult0( temp16_fx, temp16_fx ), W_fx[stage][0] );
2756 140896 : temp16_fx = sub( target_fx[1], CB_fx[stage2][index][1] );
2757 140896 : dist_fx = L_add( dist_fx, Mult_32_16( L_mult0( temp16_fx, temp16_fx ), W_fx[stage][1] ) );
2758 :
2759 140896 : if ( LT_32( dist_fx, minDist_fx ) )
2760 : {
2761 39368 : bestCode = index;
2762 39368 : move16();
2763 : }
2764 140896 : minDist_fx = L_min( minDist_fx, dist_fx );
2765 : }
2766 :
2767 20128 : brCode[0] = bestCode;
2768 20128 : move16();
2769 :
2770 20128 : brDist_fx[0] = L_add( cDist_fx[stage1][prev_state], L_shr( minDist_fx, 2 ) );
2771 20128 : move32();
2772 20128 : brQuant_fx[0][0] = add( CB_fx[stage2][bestCode][0], pred_fx[0] );
2773 20128 : move16();
2774 20128 : brQuant_fx[0][1] = add( CB_fx[stage2][bestCode][1], pred_fx[1] );
2775 20128 : move16();
2776 :
2777 : /* 2nd branch search */
2778 20128 : prev_state = NTRANS[1][state];
2779 20128 : move16();
2780 20128 : index = NTRANS[3][state];
2781 20128 : move16();
2782 :
2783 : /* Prediction */
2784 20128 : pred_fx[0] = add( mult_r( itc_fx[stage1][0][0], Q_fx[stage1][prev_state][0] ), mult_r( itc_fx[stage1][0][1], Q_fx[stage1][prev_state][1] ) );
2785 20128 : move16();
2786 20128 : pred_fx[1] = add( mult_r( itc_fx[stage1][1][0], Q_fx[stage1][prev_state][0] ), mult_r( itc_fx[stage1][1][1], Q_fx[stage1][prev_state][1] ) );
2787 20128 : move16();
2788 20128 : target_fx[0] = sub( x_fx[stage][0], pred_fx[0] );
2789 20128 : move16();
2790 20128 : target_fx[1] = sub( x_fx[stage][1], pred_fx[1] );
2791 20128 : move16();
2792 :
2793 20128 : temp16_fx = sub( target_fx[0], CB_fx[stage2][index][0] );
2794 20128 : minDist_fx = Mult_32_16( L_mult0( temp16_fx, temp16_fx ), W_fx[stage][0] ); /* 2.65*2.65*Q(-5) */
2795 20128 : temp16_fx = sub( target_fx[1], CB_fx[stage2][index][1] );
2796 20128 : minDist_fx = L_add( minDist_fx, Mult_32_16( L_mult0( temp16_fx, temp16_fx ), W_fx[stage][1] ) ); /* 2.65*2.65*Q(-5) */
2797 :
2798 20128 : bestCode = index;
2799 20128 : move16();
2800 :
2801 161024 : FOR( index = index + 8; index < 64; index += 8 )
2802 : {
2803 140896 : temp16_fx = sub( target_fx[0], CB_fx[stage2][index][0] );
2804 140896 : dist_fx = Mult_32_16( L_mult0( temp16_fx, temp16_fx ), W_fx[stage][0] );
2805 140896 : temp16_fx = sub( target_fx[1], CB_fx[stage2][index][1] );
2806 140896 : dist_fx = L_add( dist_fx, Mult_32_16( L_mult0( temp16_fx, temp16_fx ), W_fx[stage][1] ) );
2807 140896 : if ( LT_32( dist_fx, minDist_fx ) )
2808 : {
2809 39821 : bestCode = index;
2810 39821 : move16();
2811 : }
2812 140896 : minDist_fx = L_min( minDist_fx, dist_fx );
2813 : }
2814 :
2815 20128 : brCode[1] = bestCode;
2816 20128 : move16();
2817 :
2818 20128 : brDist_fx[1] = L_add( cDist_fx[stage1][prev_state], L_shr( minDist_fx, 2 ) );
2819 20128 : move32();
2820 20128 : brQuant_fx[1][0] = add( CB_fx[stage2][bestCode][0], pred_fx[0] );
2821 20128 : move16();
2822 20128 : brQuant_fx[1][1] = add( CB_fx[stage2][bestCode][1], pred_fx[1] );
2823 20128 : move16();
2824 :
2825 : /* Select Best branch */
2826 20128 : branch = 1;
2827 20128 : move16();
2828 :
2829 20128 : if ( LE_32( brDist_fx[0], brDist_fx[1] ) )
2830 : {
2831 9511 : branch = 0;
2832 9511 : move16();
2833 : }
2834 :
2835 : /* Update */
2836 20128 : s[stage][state] = NTRANS[branch][state];
2837 20128 : move16();
2838 20128 : c[stage][state] = brCode[branch];
2839 20128 : move16();
2840 :
2841 20128 : cDist_fx[stage][state] = brDist_fx[branch];
2842 20128 : move32();
2843 20128 : Q_fx[stage][state][0] = brQuant_fx[branch][0];
2844 20128 : move16();
2845 20128 : Q_fx[stage][state][1] = brQuant_fx[branch][1];
2846 20128 : move16();
2847 : }
2848 :
2849 1258 : return;
2850 : }
2851 :
2852 161024 : static Word32 BcTcvq_FixSearch_fx(
2853 : Word16 x_fx[][2], /*x2.56*/
2854 : const Word16 CB_fx[][32][2], /*x2.56*/
2855 : Word16 c[][4],
2856 : Word16 Q_fx[][16][2],
2857 : const Word16 FixBranch[][4][4],
2858 : Word16 stage,
2859 : Word16 inis,
2860 : Word16 fins,
2861 : Word16 *prev_state,
2862 : Word16 W_fx[][2], /*Q10*/
2863 : const Word16 itc_fx[][2][2] /*Q15*/
2864 : )
2865 : {
2866 : Word16 stage1, stage4, branch;
2867 : Word16 index, bestCode;
2868 : Word32 dist_fx, minDist_fx;
2869 : Word16 pred_fx[N_DIM], target_fx[N_DIM];
2870 : Word16 temp16_fx;
2871 :
2872 161024 : stage1 = sub( stage, 1 );
2873 161024 : stage4 = sub( stage, 4 );
2874 :
2875 161024 : branch = FixBranch[shr( inis, 2 )][fins][stage4];
2876 161024 : move16();
2877 161024 : index = NTRANS2[add( branch, 2 )][*prev_state];
2878 161024 : move16();
2879 :
2880 : /* Prediction */
2881 161024 : pred_fx[0] = add( mult_r( itc_fx[stage1][0][0], Q_fx[stage1][*prev_state][0] ), mult_r( itc_fx[stage1][0][1], Q_fx[stage1][*prev_state][1] ) );
2882 161024 : move16();
2883 161024 : pred_fx[1] = add( mult_r( itc_fx[stage1][1][0], Q_fx[stage1][*prev_state][0] ), mult_r( itc_fx[stage1][1][1], Q_fx[stage1][*prev_state][1] ) );
2884 161024 : move16();
2885 161024 : target_fx[0] = sub( x_fx[stage][0], pred_fx[0] );
2886 161024 : move16();
2887 161024 : target_fx[1] = sub( x_fx[stage][1], pred_fx[1] );
2888 161024 : move16();
2889 :
2890 161024 : temp16_fx = sub( target_fx[0], CB_fx[stage4][index][0] );
2891 161024 : minDist_fx = Mult_32_16( L_mult0( temp16_fx, temp16_fx ), W_fx[stage][0] ); /* 2.65*2.65*Q(-5) */
2892 161024 : temp16_fx = sub( target_fx[1], CB_fx[stage4][index][1] );
2893 161024 : minDist_fx = L_add( minDist_fx, Mult_32_16( L_mult0( temp16_fx, temp16_fx ), W_fx[stage][1] ) ); /* 2.65*2.65*Q(-5) */
2894 :
2895 161024 : bestCode = index;
2896 161024 : move16();
2897 :
2898 644096 : FOR( index = index + 8; index < 32; index += 8 )
2899 : {
2900 483072 : temp16_fx = sub( target_fx[0], CB_fx[stage4][index][0] );
2901 483072 : dist_fx = Mult_32_16( L_mult0( temp16_fx, temp16_fx ), W_fx[stage][0] );
2902 483072 : temp16_fx = sub( target_fx[1], CB_fx[stage4][index][1] );
2903 483072 : dist_fx = L_add( dist_fx, Mult_32_16( L_mult0( temp16_fx, temp16_fx ), W_fx[stage][1] ) );
2904 :
2905 483072 : if ( LT_32( dist_fx, minDist_fx ) )
2906 : {
2907 197362 : bestCode = index;
2908 197362 : move16();
2909 : }
2910 483072 : minDist_fx = L_min( minDist_fx, dist_fx );
2911 : }
2912 :
2913 : /* Update */
2914 161024 : *prev_state = NTRANS2[branch][*prev_state];
2915 161024 : move16();
2916 161024 : c[fins][stage4] = bestCode;
2917 161024 : move16();
2918 :
2919 161024 : Q_fx[stage][*prev_state][0] = add( CB_fx[stage4][bestCode][0], pred_fx[0] );
2920 161024 : move16();
2921 161024 : Q_fx[stage][*prev_state][1] = add( CB_fx[stage4][bestCode][1], pred_fx[1] );
2922 161024 : move16();
2923 :
2924 161024 : minDist_fx = L_shr( minDist_fx, 2 ); /*2.56*2.56*Q(-5 - 2)*/
2925 :
2926 161024 : return minDist_fx;
2927 : }
2928 :
2929 :
2930 629 : static Word16 optimalPath_fx(
2931 : Word32 cDist_fx[][16], /*2.56*2.56*Q(-5 - 2)*/
2932 : Word32 blockDist_fx[], /*2.56*2.56*Q(-5 - 2)*/
2933 : Word16 blockCodeword[][4],
2934 : Word16 bestCodeword[],
2935 : Word16 codeWord[][16],
2936 : Word16 bestState[],
2937 : Word16 preState[][16] )
2938 : {
2939 : Word16 stage, state;
2940 : Word32 opDist_fx[NUM_STATE];
2941 : Word32 minDist_fx;
2942 : Word16 fBlock;
2943 : Word16 prev_state;
2944 :
2945 10693 : FOR( state = 0; state < NUM_STATE; state++ )
2946 : {
2947 10064 : opDist_fx[state] = L_add( L_shr( cDist_fx[3][state], 1 ), L_shr( blockDist_fx[state], 1 ) );
2948 10064 : move32();
2949 : }
2950 :
2951 629 : minDist_fx = L_add( opDist_fx[0], 0 );
2952 629 : fBlock = 0;
2953 629 : move16();
2954 :
2955 10064 : FOR( state = 1; state < NUM_STATE; state++ )
2956 : {
2957 9435 : if ( LT_32( opDist_fx[state], minDist_fx ) )
2958 : {
2959 1662 : fBlock = state;
2960 1662 : move16();
2961 : }
2962 9435 : minDist_fx = L_min( minDist_fx, opDist_fx[state] );
2963 : }
2964 :
2965 629 : prev_state = bestState[4] = fBlock;
2966 629 : move16();
2967 629 : move16();
2968 :
2969 3145 : FOR( stage = N_STAGE_VQ - 5; stage >= 0; stage-- )
2970 : {
2971 2516 : bestCodeword[stage] = codeWord[stage][prev_state];
2972 2516 : move16();
2973 2516 : bestState[stage] = preState[stage][prev_state];
2974 2516 : move16();
2975 2516 : prev_state = bestState[stage];
2976 2516 : move16();
2977 : }
2978 :
2979 3145 : FOR( stage = 0; stage < 4; stage++ )
2980 : {
2981 2516 : bestCodeword[stage + 4] = blockCodeword[fBlock][stage];
2982 2516 : move16();
2983 : }
2984 :
2985 629 : return fBlock;
2986 : }
2987 :
2988 629 : static void quantEnc_fx(
2989 : Word16 *y_fx,
2990 : Word16 c[],
2991 : const Word16 CB_SUB1_fx[][128][2], /*x2.56*/
2992 : const Word16 CB_SUB2_fx[][64][2], /*x2.56*/
2993 : const Word16 CB_SUB3_fx[][32][2], /*x2.56*/
2994 : const Word16 itc_fx[][2][2] // Q15
2995 : )
2996 : {
2997 : Word16 i, j;
2998 : Word16 stage;
2999 : Word16 pred_fx[N_DIM], Y_fx[8][2];
3000 :
3001 : /* stage #1 */
3002 629 : Y_fx[0][0] = CB_SUB1_fx[0][c[0]][0];
3003 629 : move16();
3004 629 : Y_fx[0][1] = CB_SUB1_fx[0][c[0]][1];
3005 629 : move16();
3006 :
3007 : /* stage #2 */
3008 629 : pred_fx[0] = add( mult_r( itc_fx[0][0][0], Y_fx[0][0] ), mult_r( itc_fx[0][0][1], Y_fx[0][1] ) );
3009 629 : move16();
3010 629 : pred_fx[1] = add( mult_r( itc_fx[0][1][0], Y_fx[0][0] ), mult_r( itc_fx[0][1][1], Y_fx[0][1] ) );
3011 629 : move16();
3012 629 : Y_fx[1][0] = add( CB_SUB1_fx[1][c[1]][0], pred_fx[0] );
3013 629 : move16();
3014 629 : Y_fx[1][1] = add( CB_SUB1_fx[1][c[1]][1], pred_fx[1] );
3015 629 : move16();
3016 :
3017 : /* stage #3 - #4 */
3018 1887 : FOR( stage = 2; stage < N_STAGE_VQ - 4; stage++ )
3019 : {
3020 1258 : pred_fx[0] = add( mult_r( itc_fx[stage - 1][0][0], Y_fx[stage - 1][0] ), mult_r( itc_fx[stage - 1][0][1], Y_fx[stage - 1][1] ) );
3021 1258 : move16();
3022 1258 : pred_fx[1] = add( mult_r( itc_fx[stage - 1][1][0], Y_fx[stage - 1][0] ), mult_r( itc_fx[stage - 1][1][1], Y_fx[stage - 1][1] ) );
3023 1258 : move16();
3024 :
3025 1258 : Y_fx[stage][0] = add( CB_SUB2_fx[stage - 2][c[stage]][0], pred_fx[0] );
3026 1258 : move16();
3027 1258 : Y_fx[stage][1] = add( CB_SUB2_fx[stage - 2][c[stage]][1], pred_fx[1] );
3028 1258 : move16();
3029 : }
3030 :
3031 : /* stage #5 - #8 */
3032 3145 : FOR( stage = N_STAGE_VQ - 4; stage < N_STAGE_VQ; stage++ )
3033 : {
3034 2516 : pred_fx[0] = add( mult_r( itc_fx[stage - 1][0][0], Y_fx[stage - 1][0] ), mult_r( itc_fx[stage - 1][0][1], Y_fx[stage - 1][1] ) );
3035 2516 : move16();
3036 2516 : pred_fx[1] = add( mult_r( itc_fx[stage - 1][1][0], Y_fx[stage - 1][0] ), mult_r( itc_fx[stage - 1][1][1], Y_fx[stage - 1][1] ) );
3037 2516 : move16();
3038 :
3039 2516 : Y_fx[stage][0] = add( CB_SUB3_fx[stage - 4][c[stage]][0], pred_fx[0] );
3040 2516 : move16();
3041 2516 : Y_fx[stage][1] = add( CB_SUB3_fx[stage - 4][c[stage]][1], pred_fx[1] );
3042 2516 : move16();
3043 : }
3044 :
3045 : /* Transform Vector to Scalar */
3046 5661 : FOR( i = 0; i < N_STAGE_VQ; i++ )
3047 : {
3048 15096 : FOR( j = 0; j < N_DIM; j++ )
3049 : {
3050 10064 : y_fx[i * N_DIM + j] = Y_fx[i][j];
3051 10064 : move16();
3052 : }
3053 : }
3054 :
3055 629 : return;
3056 : }
3057 :
3058 629 : static void buildCode_fx(
3059 : Word16 *ind,
3060 : Word16 fins,
3061 : Word16 c[],
3062 : Word16 s[] )
3063 : {
3064 : Word16 stage;
3065 : Word16 BrIndex[4];
3066 :
3067 629 : set16_fx( BrIndex, 0, ( N_STAGE_VQ - 4 ) );
3068 :
3069 3145 : FOR( stage = N_STAGE_VQ - 4; stage >= 1; stage-- )
3070 : {
3071 2516 : if ( GT_16( s[stage], 7 ) )
3072 : {
3073 1262 : BrIndex[stage - 1] = 1;
3074 1262 : move16();
3075 : }
3076 : }
3077 629 : ind[0] = fins;
3078 629 : move16();
3079 :
3080 : /* stage #1 - #2 */
3081 1887 : FOR( stage = 0; stage < 2; stage++ )
3082 : {
3083 1258 : ind[stage + 1] = shl( BrIndex[stage], 4 );
3084 1258 : move16();
3085 1258 : ind[stage + 1] = add( ind[stage + 1], shr( c[stage], 3 ) );
3086 1258 : move16();
3087 : }
3088 :
3089 : /* stage #3 - #4 */
3090 1887 : FOR( stage = 2; stage < N_STAGE_VQ - 4; stage++ )
3091 : {
3092 1258 : ind[stage + 1] = shl( BrIndex[stage], 3 );
3093 1258 : move16();
3094 1258 : ind[stage + 1] = add( ind[stage + 1], shr( c[stage], 3 ) );
3095 1258 : move16();
3096 : }
3097 :
3098 : /* Stage #5 - #8 */
3099 3145 : FOR( stage = N_STAGE_VQ - 4; stage < N_STAGE_VQ; stage++ )
3100 : {
3101 2516 : ind[stage + 1] = shr( c[stage], 3 );
3102 2516 : move16();
3103 : }
3104 :
3105 629 : return;
3106 : }
3107 :
3108 :
3109 629 : static void BcTcvq_fx(
3110 : Word16 snFlag,
3111 : const Word16 *x_fx, // x2.65
3112 : Word16 *y_fx, // x2.65
3113 : const Word16 *weight_fx, // Q8
3114 : Word16 *ind )
3115 : {
3116 : Word16 X_fx[N_STAGE_VQ][N_DIM], W_fx[N_STAGE_VQ][N_DIM];
3117 :
3118 : /* Count Variable */
3119 : Word16 i, j;
3120 :
3121 : /* TCVQ Structure */
3122 : Word16 stage, state, prev_state;
3123 : Word16 preState[N_STAGE_VQ][NUM_STATE];
3124 : Word16 codeWord[N_STAGE_VQ][NUM_STATE];
3125 : Word32 acumDist_fx[N_STAGE_VQ - 4][NUM_STATE];
3126 : Word16 inis, fins, ptr_fins;
3127 : Word16 fBlock;
3128 : Word16 fState[NUM_STATE];
3129 : Word16 fCodeword[4][4];
3130 : Word16 iniBlock[NUM_STATE];
3131 : Word16 blockCodeword[NUM_STATE][4];
3132 :
3133 : /* Prediction variable */
3134 : Word16 quant_fx[N_STAGE_VQ][NUM_STATE][N_DIM];
3135 :
3136 : /* Distortion variable */
3137 : Word32 minDist_fx;
3138 : Word32 fDist_fx;
3139 : Word32 blockDist_fx[NUM_STATE];
3140 :
3141 : /* Decoding variable */
3142 : Word16 bestCodeword[N_STAGE_VQ];
3143 : Word16 bestState[N_STAGE_VQ];
3144 :
3145 : /* Code Share variable */
3146 : const Word16( *TCVQ_CB_SUB1_fx )[128][2], ( *TCVQ_CB_SUB2_fx )[64][2], ( *TCVQ_CB_SUB3_fx )[32][2] /**/;
3147 : const Word16( *IntraCoeff_fx )[2][2];
3148 :
3149 : /* Memoryless Module */
3150 629 : IF( snFlag )
3151 : {
3152 104 : TCVQ_CB_SUB1_fx = SN_TCVQ_CB_SUB1_fx;
3153 104 : TCVQ_CB_SUB2_fx = SN_TCVQ_CB_SUB2_fx;
3154 104 : TCVQ_CB_SUB3_fx = SN_TCVQ_CB_SUB3_fx;
3155 104 : IntraCoeff_fx = SN_IntraCoeff_fx;
3156 : }
3157 : ELSE /* Memory Module */
3158 : {
3159 525 : TCVQ_CB_SUB1_fx = AR_TCVQ_CB_SUB1_fx;
3160 525 : TCVQ_CB_SUB2_fx = AR_TCVQ_CB_SUB2_fx;
3161 525 : TCVQ_CB_SUB3_fx = AR_TCVQ_CB_SUB3_fx;
3162 525 : IntraCoeff_fx = AR_IntraCoeff_fx;
3163 : }
3164 :
3165 : /* Transform Scalar to Vector */
3166 5661 : FOR( i = 0; i < N_STAGE_VQ; i++ )
3167 : {
3168 15096 : FOR( j = 0; j < N_DIM; j++ )
3169 : {
3170 10064 : X_fx[i][j] = x_fx[( N_DIM * i ) + j]; // x2.65
3171 10064 : move16();
3172 10064 : W_fx[i][j] = weight_fx[( N_DIM * i ) + j]; // x2.56
3173 10064 : move16();
3174 : }
3175 : }
3176 :
3177 : /* Initialzie */
3178 3145 : FOR( i = 0; i < N_STAGE_VQ - 4; i++ )
3179 : {
3180 42772 : FOR( j = 0; j < NUM_STATE; j++ )
3181 : {
3182 40256 : acumDist_fx[i][j] = L_deposit_l( 0 );
3183 : }
3184 : }
3185 :
3186 : /* BcTcvq Search */
3187 : /* stage #1 */
3188 629 : BcTcvq_1st_fx( X_fx, TCVQ_CB_SUB1_fx, preState, codeWord, acumDist_fx, quant_fx, W_fx );
3189 :
3190 : /* stage #2 */
3191 629 : BcTcvq_2nd_fx( X_fx, TCVQ_CB_SUB1_fx, preState, codeWord, acumDist_fx, quant_fx, W_fx, IntraCoeff_fx );
3192 :
3193 : /* stage #3 - #4 */
3194 1887 : FOR( stage = 2; stage < N_STAGE_VQ - 4; stage++ )
3195 : {
3196 1258 : BcTcvq_SubBlock_fx( X_fx, TCVQ_CB_SUB2_fx, preState, codeWord, acumDist_fx, quant_fx, stage, W_fx, IntraCoeff_fx );
3197 : }
3198 :
3199 : /* Search initial state at each block */
3200 10693 : FOR( state = 0; state < NUM_STATE; state++ )
3201 : {
3202 10064 : prev_state = state;
3203 10064 : move16();
3204 :
3205 50320 : FOR( stage = N_STAGE_VQ - 5; stage >= 0; stage-- )
3206 : {
3207 40256 : prev_state = preState[stage][prev_state];
3208 40256 : move16();
3209 : }
3210 10064 : iniBlock[state] = prev_state;
3211 10064 : move16();
3212 : }
3213 :
3214 : /* stage #5 - #8 */
3215 10693 : FOR( state = 0; state < NUM_STATE; state++ )
3216 : {
3217 10064 : inis = iniBlock[state];
3218 10064 : move16();
3219 10064 : ptr_fins = shr( inis, 2 );
3220 10064 : minDist_fx = L_add( MAX_32, 0 );
3221 :
3222 50320 : FOR( i = 0; i < 4; i++ )
3223 : {
3224 40256 : fins = add( shl( ptr_fins, 2 ), i );
3225 40256 : prev_state = state;
3226 40256 : move16();
3227 40256 : fDist_fx = BcTcvq_FixSearch_fx( X_fx, TCVQ_CB_SUB3_fx, fCodeword, quant_fx, FixBranch_tbl, N_STAGE_VQ - 4, inis, i, &prev_state, W_fx, IntraCoeff_fx );
3228 :
3229 161024 : FOR( stage = N_STAGE_VQ - 3; stage < N_STAGE_VQ; stage++ )
3230 : {
3231 120768 : fDist_fx = L_add( fDist_fx, BcTcvq_FixSearch_fx( X_fx, TCVQ_CB_SUB3_fx, fCodeword, quant_fx, FixBranch_tbl, stage, inis, i, &prev_state, W_fx, IntraCoeff_fx ) );
3232 : }
3233 40256 : IF( LT_32( fDist_fx, minDist_fx ) )
3234 : {
3235 21371 : minDist_fx = L_add( fDist_fx, 0 );
3236 21371 : blockDist_fx[state] = minDist_fx;
3237 21371 : move32();
3238 :
3239 21371 : fState[state] = fins;
3240 21371 : move16();
3241 :
3242 21371 : blockCodeword[state][0] = fCodeword[i][0];
3243 21371 : move16();
3244 21371 : blockCodeword[state][1] = fCodeword[i][1];
3245 21371 : move16();
3246 21371 : blockCodeword[state][2] = fCodeword[i][2];
3247 21371 : move16();
3248 21371 : blockCodeword[state][3] = fCodeword[i][3];
3249 21371 : move16();
3250 : }
3251 : }
3252 : }
3253 :
3254 : /* Select optimal path */
3255 629 : fBlock = optimalPath_fx( acumDist_fx, blockDist_fx, blockCodeword, bestCodeword, codeWord, bestState, preState );
3256 :
3257 : /* Select Quantized Value */
3258 629 : quantEnc_fx( y_fx, bestCodeword, TCVQ_CB_SUB1_fx, TCVQ_CB_SUB2_fx, TCVQ_CB_SUB3_fx, IntraCoeff_fx );
3259 :
3260 : /* Buid Code for Decoder */
3261 629 : buildCode_fx( ind, fState[fBlock], bestCodeword, bestState );
3262 :
3263 629 : return;
3264 : }
3265 :
3266 718 : static Word16 SVQ_2d_fx(
3267 : Word16 *x_fx, // x2.65
3268 : Word16 *y_fx, // x2.65
3269 : const Word16 *W_fx, // Q8
3270 : const Word16 CB_fx[][8], // x2.65
3271 : Word16 Size )
3272 : {
3273 : Word16 i, j;
3274 718 : Word16 index = 0;
3275 : Word32 distortion_fx;
3276 : Word32 temp_fx;
3277 : Word16 temp16_fx;
3278 :
3279 718 : temp_fx = L_add( MAX_32, 0 );
3280 :
3281 17950 : FOR( i = 0; i < Size; i++ )
3282 : {
3283 17232 : distortion_fx = L_deposit_l( 0 );
3284 155088 : FOR( j = 0; j < 8; j++ )
3285 : {
3286 137856 : temp16_fx = sub( x_fx[j], CB_fx[i][j] );
3287 137856 : distortion_fx = L_add( distortion_fx,
3288 137856 : L_shr( Mult_32_16( L_mult( temp16_fx, temp16_fx ), W_fx[j] ), 1 ) ); // (2*x2.65 + Q1 + Q8) - Q15 - Q1
3289 : }
3290 :
3291 17232 : IF( LT_32( distortion_fx, temp_fx ) )
3292 : {
3293 2778 : temp_fx = L_add( distortion_fx, 0 );
3294 2778 : index = i;
3295 2778 : move16();
3296 : }
3297 : }
3298 :
3299 6462 : FOR( i = 0; i < M / 2; i++ )
3300 : {
3301 5744 : y_fx[i] = CB_fx[index][i];
3302 5744 : move16();
3303 : }
3304 :
3305 718 : return index;
3306 : }
3307 :
3308 :
3309 629 : Word32 qlsf_ARSN_tcvq_Enc_16k_fx(
3310 : const Word16 *x_fx, /* i : Vector to be encoded x2.65 */
3311 : Word16 *y_fx, /* o : Quantized LSF vector x2.65 */
3312 : Word16 *indice, /* o : Indices */
3313 : const Word16 *w_fx, /* i : LSF Weights Q8 */
3314 : const Word16 nBits, /* i : number of bits */
3315 : Word16 safety_net /* i : safety_net flag */
3316 : )
3317 : {
3318 : Word16 i;
3319 : Word16 x_q_fx[M];
3320 : Word16 yy_fx[M];
3321 : Word16 error_svq_fx[M], error_svq_q_fx[M];
3322 : Word16 cl, cs;
3323 : Word32 temp_l;
3324 :
3325 629 : IF( EQ_16( safety_net, 1 ) )
3326 : {
3327 104 : indice[0] = 1;
3328 104 : move16();
3329 104 : BcTcvq_fx( 1, /*x, x_q, w, */ x_fx, x_q_fx, w_fx, &indice[1] );
3330 :
3331 104 : IF( GT_16( nBits, 30 ) )
3332 : {
3333 : /* SVQ */
3334 1003 : FOR( i = 0; i < M; i++ )
3335 : {
3336 944 : error_svq_fx[i] = mult_r( sub( x_fx[i], x_q_fx[i] ), scale_inv_ARSN_fx[i] );
3337 944 : move16();
3338 : }
3339 :
3340 : /* 5bits 1st Split VQ for Residual*/
3341 59 : indice[10] = SVQ_2d_fx( error_svq_fx, error_svq_q_fx, w_fx, AR_SVQ_CB1_fx, 32 );
3342 : /* 4bits 2nd Split VQ for Residual*/
3343 59 : indice[11] = SVQ_2d_fx( &error_svq_fx[8], &error_svq_q_fx[8], &w_fx[8], AR_SVQ_CB2_fx, 16 );
3344 :
3345 1003 : FOR( i = 0; i < M; i++ )
3346 : {
3347 944 : x_q_fx[i] = add( x_q_fx[i], extract_h( L_shl( L_mult0( error_svq_q_fx[i], scale_ARSN_fx[i] ), 2 ) ) );
3348 944 : move16();
3349 : }
3350 : }
3351 : }
3352 : ELSE
3353 : {
3354 525 : indice[0] = 0;
3355 525 : move16();
3356 525 : BcTcvq_fx( 0, /*x, x_q, w, */ x_fx, x_q_fx, w_fx, &indice[1] );
3357 :
3358 525 : IF( GT_16( nBits, 30 ) )
3359 : {
3360 : /* SVQ */
3361 5100 : FOR( i = 0; i < M; i++ )
3362 : {
3363 4800 : error_svq_fx[i] = sub( x_fx[i], x_q_fx[i] );
3364 4800 : move16();
3365 : }
3366 :
3367 : /* 5bits 1st Split VQ for Residual*/
3368 300 : indice[10] = SVQ_2d_fx( error_svq_fx, error_svq_q_fx, w_fx, AR_SVQ_CB1_fx, 32 );
3369 : /* 4bits 2nd Split VQ for Residual*/
3370 300 : indice[11] = SVQ_2d_fx( &error_svq_fx[8], &error_svq_q_fx[8], &w_fx[8], AR_SVQ_CB2_fx, 16 );
3371 :
3372 5100 : FOR( i = 0; i < M; i++ )
3373 : {
3374 4800 : x_q_fx[i] = add( x_q_fx[i], error_svq_q_fx[i] );
3375 4800 : move16();
3376 : }
3377 : }
3378 : }
3379 :
3380 629 : cl = 0;
3381 629 : move16();
3382 10693 : FOR( i = 0; i < M; i++ )
3383 : {
3384 10064 : yy_fx[i] = sub( x_fx[i], x_q_fx[i] );
3385 10064 : move16();
3386 10064 : cl = s_max( cl, abs_s( yy_fx[i] ) );
3387 : }
3388 629 : cs = norm_s( cl );
3389 629 : temp_l = 0;
3390 629 : move16();
3391 :
3392 10693 : FOR( i = 0; i < M; i++ )
3393 : {
3394 10064 : yy_fx[i] = shl( yy_fx[i], cs );
3395 10064 : move16();
3396 10064 : temp_l = L_mac_sat( temp_l, mult( yy_fx[i], shl_sat( w_fx[i], 2 ) ), yy_fx[i] );
3397 : }
3398 629 : cs = shl( cs, 1 );
3399 629 : temp_l = L_shr( temp_l, cs );
3400 629 : temp_l = Mult_32_16( temp_l, LSF_1_OVER_256SQ ); /* Q-4 */
3401 :
3402 : /* Recover the quantized LSF */
3403 629 : Copy( x_q_fx, y_fx, M );
3404 :
3405 629 : return temp_l;
3406 : }
3407 :
3408 1423 : static void FFT_Mid_Interpol_16k_fx(
3409 : Word32 Bin_Ener_old[], /* i/o: Old 2nd FFT Bin energy (128) Q_ener*/
3410 : Word32 Bin_Ener[], /* i : Current 2nd FFT Bin energy (128) Q_ener*/
3411 : Word32 Bin_Ener_mid[] /* o : LP weighting filter (numerator) Q_ener*/
3412 : )
3413 : {
3414 : Word16 i;
3415 :
3416 183567 : FOR( i = 0; i < L_FFT / 2; i++ )
3417 : {
3418 : /* Interpolation */
3419 182144 : Bin_Ener_mid[i] = L_shr( L_add_sat( Bin_Ener_old[i], Bin_Ener[i] ), 1 );
3420 :
3421 : /* Memory update */
3422 182144 : Bin_Ener_old[i] = Bin_Ener[i];
3423 182144 : move32();
3424 : }
3425 :
3426 1423 : return;
3427 : }
3428 :
3429 :
3430 : /*========================================================================*/
3431 : /* FUNCTION : lsf_mid_enc_fx() */
3432 : /*------------------------------------------------------------------------*/
3433 : /* PURPOSE : Mid-frame LSF quantization */
3434 : /*------------------------------------------------------------------------*/
3435 : /* INPUT ARGUMENTS : */
3436 : /* _ (Word16) coder_type : Coder type */
3437 : /* _ (Word16) bwidth : input signal bandwidth */
3438 : /* _ (Word16) int_fs : internal (ACELP) sampling frequency */
3439 : /* _ (Word32) core_brate : core bitrate */
3440 : /* _ (Word32) ppp_mode : PPP mode */
3441 : /* _ (Word32) nelp_mode : NELP mode */
3442 : /* _ (Word16[]) qlsp0 : quantized LSPs from frame beginning Q15 */
3443 : /* _ (Word16[]) qlsp1 : quantized LSPs from frame end Q15 */
3444 : /* _ (Word16[]) Bin_Ener : per bin log energy spectrum Q_ener */
3445 : /* _ (Word16) Q_ener : */
3446 : /* _ (Word16) ppp_mode : */
3447 : /* _ (Word16) nelp_mode : */
3448 : /*------------------------------------------------------------------------*/
3449 : /* INPUT/OUTPUT ARGUMENTS : */
3450 : /* _ (Word16[]) lsp : quantized LSPs Q15 */
3451 : /* _ (Word16[]) Bin_Ener_old : per bin old log energy spectrum Q_ener */
3452 : /*------------------------------------------------------------------------*/
3453 : /* OUTPUT ARGUMENTS : */
3454 : /*------------------------------------------------------------------------*/
3455 :
3456 : /*------------------------------------------------------------------------*/
3457 : /* RETURN ARGUMENTS : */
3458 : /* _ None */
3459 : /*========================================================================*/
3460 :
3461 :
3462 1423 : static void lsf_mid_enc_fx(
3463 : BSTR_ENC_HANDLE hBstr, /* i/o: encoder bitstream handle */
3464 : Word16 nb_bits, /* i : number of bits */
3465 : const Word16 int_fs, /* i : internal (ACELP) sampling frequency*/
3466 : const Word16 qlsp0[], /* i : quantized LSPs from frame beginning*/
3467 : const Word16 qlsp1[], /* i : quantized LSPs from frame end */
3468 : Word16 lsp[], /* i/o: mid-frame LSP */
3469 : const Word16 coder_type, /* i : coding type */
3470 : const Word16 bwidth, /* i : input signal bandwidth */
3471 : Word32 Bin_Ener_old[], /* i/o: per bin old log energy spectrum */
3472 : Word32 Bin_Ener[], /* i : per bin log energy spectrum */
3473 : Word16 Q_ener, /* i : Q value of Bin_ener */
3474 : Word16 ppp_mode,
3475 : Word16 nelp_mode )
3476 : {
3477 : Word16 lsf[M], qlsf[M], qlsf1[M], qlsf0[M], wghts[M];
3478 : Word32 err, err_min;
3479 1423 : Word16 j, k, idx, size = 0;
3480 : Word32 Bin_Ener_mid[L_FFT / 2];
3481 : Word32 L_tmp;
3482 : Word16 tmp, k1;
3483 1423 : const Word16 *ratio = NULL;
3484 :
3485 : /* convert LSPs to LSFs */
3486 1423 : lsp2lsf_fx( lsp, lsf, M, int_fs );
3487 1423 : lsp2lsf_fx( qlsp0, qlsf0, M, int_fs );
3488 1423 : lsp2lsf_fx( qlsp1, qlsf1, M, int_fs );
3489 :
3490 : /* calculate weights */
3491 1423 : FFT_Mid_Interpol_16k_fx( Bin_Ener_old, &Bin_Ener[L_FFT / 2], Bin_Ener_mid );
3492 :
3493 : /* LSF weighting */
3494 1423 : Unified_weighting_fx( Bin_Ener_mid, Q_ener, lsf, wghts, (Word16) EQ_16( bwidth, NB ), (Word16) EQ_16( coder_type, UNVOICED ), int_fs, M );
3495 1423 : move16();
3496 : /* codebook selection, number of bits, size of the codebook */
3497 1423 : test();
3498 1423 : IF( ppp_mode == 0 && nelp_mode == 0 )
3499 : {
3500 : /* codebook selection */
3501 1423 : IF( EQ_16( coder_type, VOICED ) )
3502 : {
3503 212 : SWITCH( nb_bits )
3504 : {
3505 0 : case 5:
3506 : {
3507 0 : ratio = tbl_mid_voi_wb_5b_fx;
3508 0 : move16();
3509 0 : BREAK;
3510 : }
3511 212 : case 4:
3512 : {
3513 212 : ratio = tbl_mid_voi_wb_4b_fx;
3514 212 : move16();
3515 212 : BREAK;
3516 : }
3517 : }
3518 212 : }
3519 1211 : ELSE IF( EQ_16( coder_type, UNVOICED ) )
3520 : {
3521 0 : ratio = tbl_mid_unv_wb_5b_fx;
3522 : }
3523 : ELSE
3524 : {
3525 : /* GENERIC, TRANSITION, AUDIO and INACTIVE */
3526 1211 : SWITCH( nb_bits )
3527 : {
3528 1203 : case 5:
3529 : {
3530 1203 : ratio = tbl_mid_gen_wb_5b_fx;
3531 1203 : move16();
3532 1203 : BREAK;
3533 : }
3534 8 : case 2:
3535 : {
3536 8 : ratio = tbl_mid_gen_wb_2b_fx;
3537 8 : move16();
3538 8 : BREAK;
3539 : }
3540 : }
3541 1423 : }
3542 :
3543 1423 : size = (Word16) pow2[nb_bits];
3544 1423 : move16();
3545 : }
3546 0 : ELSE IF( EQ_16( ppp_mode, 1 ) )
3547 : {
3548 0 : ratio = tbl_mid_voi_wb_1b_fx;
3549 0 : move16();
3550 0 : nb_bits = 1;
3551 0 : move16();
3552 0 : size = 2;
3553 0 : move16();
3554 : }
3555 0 : ELSE IF( EQ_16( nelp_mode, 1 ) )
3556 : {
3557 0 : ratio = tbl_mid_unv_wb_4b_fx;
3558 0 : move16();
3559 0 : nb_bits = 4;
3560 0 : move16();
3561 0 : size = 16;
3562 0 : move16();
3563 : }
3564 :
3565 : /* loop over codevectors */
3566 1423 : err_min = MAXINT32;
3567 1423 : move16();
3568 1423 : idx = 0;
3569 1423 : move16();
3570 1423 : k1 = 0;
3571 1423 : move16();
3572 43343 : FOR( k = 0; k < size; k++ )
3573 : {
3574 41920 : err = L_deposit_l( 0 );
3575 :
3576 712640 : FOR( j = 0; j < M; j++ )
3577 : {
3578 : /* qlsf[j] = (1.0f - ratio[k*M+j]) * qlsf0[j] + ratio[k*M+j] * qlsf1[j]; */
3579 670720 : L_tmp = L_mult( sub( 0x2000, ratio[k1 + j] ), qlsf0[j] );
3580 670720 : L_tmp = L_mac( L_tmp, ratio[k1 + j], qlsf1[j] );
3581 670720 : qlsf[j] = round_fx( L_shl( L_tmp, 2 ) );
3582 :
3583 670720 : test();
3584 670720 : test();
3585 670720 : IF( j > 0 && LT_16( j, M ) && LT_16( qlsf[j], add( qlsf[j - 1], LSF_GAP_MID_FX ) ) )
3586 : {
3587 6245 : qlsf[j] = add( qlsf[j - 1], LSF_GAP_MID_FX );
3588 6245 : move16();
3589 : }
3590 :
3591 670720 : tmp = sub( lsf[j], qlsf[j] );
3592 : /* err += wghts[j] * ftemp * ftemp; */
3593 : /* tmp is usually very small, we can have some extra precision with very rare saturation */
3594 670720 : tmp = shl_sat( tmp, 4 );
3595 670720 : tmp = mult_r_sat( tmp, tmp );
3596 670720 : err = L_mac_sat( err, tmp, shl_sat( wghts[j], 2 ) );
3597 : }
3598 : /* err = L_shl(err,Wscale); */
3599 41920 : err = Mult_32_16( err, LSF_1_OVER_256SQ );
3600 : /* err = Mult_32_16(err,Wmult); */
3601 :
3602 41920 : IF( LT_32( err, err_min ) )
3603 : {
3604 5468 : err_min = L_add( err, 0 );
3605 5468 : idx = k;
3606 5468 : move16();
3607 : }
3608 41920 : k1 += M;
3609 41920 : move16();
3610 : }
3611 :
3612 : /* calculate the quantized LSF vector */
3613 24191 : FOR( j = 0; j < M; j++ )
3614 : {
3615 : /* qlsf[j] = (1.0f - ratio[idx*M+j]) * qlsf0[j] + ratio[idx*M+j] * qlsf1[j]; */
3616 22768 : L_tmp = L_mult( sub( 0x2000, ratio[idx * M + j] ), qlsf0[j] );
3617 22768 : L_tmp = L_mac( L_tmp, ratio[idx * M + j], qlsf1[j] );
3618 22768 : qlsf[j] = round_fx( L_shl( L_tmp, 2 ) );
3619 :
3620 22768 : test();
3621 22768 : test();
3622 22768 : IF( j > 0 && LT_16( j, M ) && LT_16( qlsf[j], add( qlsf[j - 1], LSF_GAP_MID_FX ) ) )
3623 : {
3624 232 : qlsf[j] = add( qlsf[j - 1], LSF_GAP_MID_FX );
3625 232 : move16();
3626 : }
3627 : }
3628 :
3629 1423 : reorder_lsf_fx( qlsf, LSF_GAP_MID_FX, M, int_fs );
3630 :
3631 : /* convert LSFs back to LSPs */
3632 1423 : lsf2lsp_fx( qlsf, lsp, M, int_fs );
3633 1423 : push_indice( hBstr, IND_MID_FRAME_LSF_INDEX, idx, nb_bits );
3634 :
3635 1423 : return;
3636 : }
3637 :
3638 156569 : static void lsf_mid_enc_ivas_fx(
3639 : BSTR_ENC_HANDLE hBstr, /* i/o: encoder bitstream handle */
3640 : Word16 nb_bits, /* i : number of bits */
3641 : const Word32 int_fs, /* i : internal (ACELP) sampling frequency*/
3642 : const Word16 qlsp0[], /* i : quantized LSPs from frame beginning Q15*/
3643 : const Word16 qlsp1[], /* i : quantized LSPs from frame end Q15*/
3644 : Word16 lsp[], /* i/o: mid-frame LSP Q15*/
3645 : const Word16 coder_type, /* i : coding type */
3646 : const Word16 bwidth, /* i : input signal bandwidth */
3647 : Word32 Bin_Ener[], /* i : per bin log energy spectrum Q_ener*/
3648 : Word16 Q_ener, /* i : Q value of Bin_ener */
3649 : Word16 ppp_mode,
3650 : Word16 nelp_mode )
3651 : {
3652 : Word16 lsf[M], qlsf[M], qlsf1[M], qlsf0[M], wghts[M];
3653 : Word32 err, err_min;
3654 156569 : Word16 j, k, idx, size = 0;
3655 : Word32 L_tmp;
3656 : Word16 tmp, k1;
3657 156569 : const Word16 *ratio = NULL;
3658 :
3659 : /* convert LSPs to LSFs */
3660 156569 : lsp2lsf_fx( lsp, lsf, M, int_fs );
3661 156569 : lsp2lsf_fx( qlsp0, qlsf0, M, int_fs );
3662 156569 : lsp2lsf_fx( qlsp1, qlsf1, M, int_fs );
3663 :
3664 : /* LSF weighting */
3665 156569 : Unified_weighting_fx( Bin_Ener, Q_ener, lsf, wghts, (Word16) EQ_16( bwidth, NB ), (Word16) EQ_16( coder_type, UNVOICED ), int_fs, M );
3666 156569 : move16();
3667 : /* codebook selection, number of bits, size of the codebook */
3668 156569 : test();
3669 156569 : IF( ppp_mode == 0 && nelp_mode == 0 )
3670 : {
3671 : /* codebook selection */
3672 156569 : IF( EQ_16( coder_type, VOICED ) )
3673 : {
3674 8700 : SWITCH( nb_bits )
3675 : {
3676 1709 : case 5:
3677 : {
3678 1709 : ratio = tbl_mid_voi_wb_5b_fx; // Q13
3679 1709 : move16();
3680 1709 : BREAK;
3681 : }
3682 6991 : case 4:
3683 : {
3684 6991 : ratio = tbl_mid_voi_wb_4b_fx; // Q13
3685 6991 : move16();
3686 6991 : BREAK;
3687 : }
3688 0 : case 1:
3689 : {
3690 0 : ratio = tbl_mid_voi_wb_1b_fx; // Q13
3691 0 : move16();
3692 0 : BREAK;
3693 : }
3694 : }
3695 8700 : }
3696 147869 : ELSE IF( EQ_16( coder_type, UNVOICED ) )
3697 : {
3698 2041 : ratio = tbl_mid_unv_wb_5b_fx; // Q13
3699 : }
3700 : ELSE
3701 : {
3702 : /* GENERIC, TRANSITION, AUDIO and INACTIVE */
3703 145828 : SWITCH( nb_bits )
3704 : {
3705 136671 : case 5:
3706 : {
3707 136671 : ratio = tbl_mid_gen_wb_5b_fx; // Q13
3708 136671 : move16();
3709 136671 : BREAK;
3710 : }
3711 734 : case 4:
3712 : {
3713 734 : ratio = tbl_mid_gen_wb_4b_fx; // Q13
3714 734 : move16();
3715 734 : BREAK;
3716 : }
3717 8423 : case 2:
3718 : {
3719 8423 : ratio = tbl_mid_gen_wb_2b_fx; // Q13
3720 8423 : move16();
3721 8423 : BREAK;
3722 : }
3723 : }
3724 156569 : }
3725 :
3726 156569 : size = (Word16) pow2[nb_bits];
3727 156569 : move16();
3728 : }
3729 0 : ELSE IF( EQ_16( ppp_mode, 1 ) )
3730 : {
3731 0 : ratio = tbl_mid_voi_wb_1b_fx; // Q13
3732 0 : move16();
3733 0 : nb_bits = 1;
3734 0 : move16();
3735 0 : size = 2;
3736 0 : move16();
3737 : }
3738 0 : ELSE IF( EQ_16( nelp_mode, 1 ) )
3739 : {
3740 0 : ratio = tbl_mid_unv_wb_4b_fx; // Q13
3741 0 : move16();
3742 0 : nb_bits = 4;
3743 0 : move16();
3744 0 : size = 16;
3745 0 : move16();
3746 : }
3747 :
3748 : /* loop over codevectors */
3749 156569 : err_min = MAXINT32;
3750 156569 : move16();
3751 156569 : idx = 0;
3752 156569 : move16();
3753 156569 : k1 = 0;
3754 156569 : move16();
3755 4807333 : FOR( k = 0; k < size; k++ )
3756 : {
3757 4650764 : err = L_deposit_l( 0 );
3758 :
3759 79062988 : FOR( j = 0; j < M; j++ )
3760 : {
3761 : /* qlsf[j] = (1.0f - ratio[k*M+j]) * qlsf0[j] + ratio[k*M+j] * qlsf1[j]; */
3762 74412224 : L_tmp = L_mult( sub( 0x2000 /*1.Q13*/, ratio[k1 + j] ), qlsf0[j] );
3763 74412224 : L_tmp = L_mac( L_tmp, ratio[k1 + j], qlsf1[j] );
3764 74412224 : qlsf[j] = round_fx( L_shl( L_tmp, 2 ) );
3765 :
3766 74412224 : test();
3767 74412224 : test();
3768 74412224 : IF( j > 0 && LT_16( j, M ) && LT_16( qlsf[j], add( qlsf[j - 1], LSF_GAP_MID_FX ) ) )
3769 : {
3770 452277 : qlsf[j] = add( qlsf[j - 1], LSF_GAP_MID_FX );
3771 452277 : move16();
3772 : }
3773 :
3774 74412224 : tmp = sub( lsf[j], qlsf[j] );
3775 : /* err += wghts[j] * ftemp * ftemp; */
3776 : /* tmp is usually very small, we can have some extra precision with very rare saturation */
3777 74412224 : tmp = shl_sat( tmp, 4 );
3778 74412224 : tmp = mult_r_sat( tmp, tmp );
3779 74412224 : err = L_mac_sat( err, tmp, shl_sat( wghts[j], 2 ) );
3780 : }
3781 : /* err = L_shl(err,Wscale); */
3782 4650764 : err = Mult_32_16( err, LSF_1_OVER_256SQ );
3783 : /* err = Mult_32_16(err,Wmult); */
3784 :
3785 4650764 : IF( LT_32( err, err_min ) )
3786 : {
3787 610943 : err_min = L_add( err, 0 );
3788 610943 : idx = k;
3789 610943 : move16();
3790 : }
3791 4650764 : k1 += M;
3792 4650764 : move16();
3793 : }
3794 :
3795 : /* calculate the quantized LSF vector */
3796 2661673 : FOR( j = 0; j < M; j++ )
3797 : {
3798 : /* qlsf[j] = (1.0f - ratio[idx*M+j]) * qlsf0[j] + ratio[idx*M+j] * qlsf1[j]; */
3799 2505104 : L_tmp = L_mult( sub( 0x2000 /*1.Q13*/, ratio[idx * M + j] ), qlsf0[j] );
3800 2505104 : L_tmp = L_mac( L_tmp, ratio[idx * M + j], qlsf1[j] );
3801 2505104 : qlsf[j] = round_fx( L_shl( L_tmp, 2 ) );
3802 :
3803 2505104 : test();
3804 2505104 : test();
3805 2505104 : IF( j > 0 && LT_16( j, M ) && LT_16( qlsf[j], add( qlsf[j - 1], LSF_GAP_MID_FX ) ) )
3806 : {
3807 13942 : qlsf[j] = add( qlsf[j - 1], LSF_GAP_MID_FX );
3808 13942 : move16();
3809 : }
3810 : }
3811 :
3812 156569 : reorder_lsf_fx( qlsf, LSF_GAP_MID_FX, M, int_fs );
3813 :
3814 : /* convert LSFs back to LSPs */
3815 156569 : lsf2lsp_fx( qlsf, lsp, M, int_fs );
3816 156569 : push_indice( hBstr, IND_MID_FRAME_LSF_INDEX, idx, nb_bits );
3817 :
3818 156569 : return;
3819 : }
|
