1 /* Copyright (C) 2002-2006 Jean-Marc Valin
2 File: nb_celp.c
3
4 Redistribution and use in source and binary forms, with or without
5 modification, are permitted provided that the following conditions
6 are met:
7
8 - Redistributions of source code must retain the above copyright
9 notice, this list of conditions and the following disclaimer.
10
11 - Redistributions in binary form must reproduce the above copyright
12 notice, this list of conditions and the following disclaimer in the
13 documentation and/or other materials provided with the distribution.
14
15 - Neither the name of the Xiph.org Foundation nor the names of its
16 contributors may be used to endorse or promote products derived from
17 this software without specific prior written permission.
18
19 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
20 ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
21 LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
22 A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE FOUNDATION OR
23 CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
24 EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
25 PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
26 PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
27 LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
28 NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
29 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
30 */
31
32 #ifdef HAVE_CONFIG_H
33 #include "config.h"
34 #endif
35
36 #include <math.h>
37 #include "nb_celp.h"
38 #include "lpc.h"
39 #include "lsp.h"
40 #include "ltp.h"
41 #include "quant_lsp.h"
42 #include "cb_search.h"
43 #include "filters.h"
44 #include "stack_alloc.h"
45 #include "vq.h"
46 #include "vbr.h"
47 #include "arch.h"
48 #include "math_approx.h"
49 #include "os_support.h"
50
51 #ifdef VORBIS_PSYCHO
52 #include "vorbis_psy.h"
53 #endif
54
55 #ifndef NULL
56 #define NULL 0
57 #endif
58
59 #define SUBMODE(x) st->submodes[st->submodeID]->x
60
61 /* Default size for the encoder and decoder stack (can be changed at compile time).
62 This does not apply when using variable-size arrays or alloca. */
63 #ifndef NB_ENC_STACK
64 #define NB_ENC_STACK (8000*sizeof(spx_sig_t))
65 #endif
66
67 #ifndef NB_DEC_STACK
68 #define NB_DEC_STACK (4000*sizeof(spx_sig_t))
69 #endif
70
71
72 #ifdef FIXED_POINT
73 static const spx_word32_t ol_gain_table[32]={18900, 25150, 33468, 44536, 59265, 78865, 104946, 139653, 185838, 247297, 329081, 437913, 582736, 775454, 1031906, 1373169, 1827293, 2431601, 3235761, 4305867, 5729870, 7624808, 10146425, 13501971, 17967238, 23909222, 31816294, 42338330, 56340132, 74972501, 99766822, 132760927};
74 static const spx_word16_t exc_gain_quant_scal3_bound[7]={1841, 3883, 6051, 8062, 10444, 13580, 18560};
75 static const spx_word16_t exc_gain_quant_scal3[8]={1002, 2680, 5086, 7016, 9108, 11781, 15380, 21740};
76 static const spx_word16_t exc_gain_quant_scal1_bound[1]={14385};
77 static const spx_word16_t exc_gain_quant_scal1[2]={11546, 17224};
78
79 #define LSP_MARGIN 16
80 #define LSP_DELTA1 6553
81 #define LSP_DELTA2 1638
82
83 #else
84
85 static const float exc_gain_quant_scal3_bound[7]={0.112338f, 0.236980f, 0.369316f, 0.492054f, 0.637471f, 0.828874f, 1.132784f};
86 static const float exc_gain_quant_scal3[8]={0.061130f, 0.163546f, 0.310413f, 0.428220f, 0.555887f, 0.719055f, 0.938694f, 1.326874f};
87 static const float exc_gain_quant_scal1_bound[1]={0.87798f};
88 static const float exc_gain_quant_scal1[2]={0.70469f, 1.05127f};
89
90 #define LSP_MARGIN .002f
91 #define LSP_DELTA1 .2f
92 #define LSP_DELTA2 .05f
93
94 #endif
95
96 #ifdef VORBIS_PSYCHO
97 #define EXTRA_BUFFER 100
98 #else
99 #define EXTRA_BUFFER 0
100 #endif
101
102
103 extern const spx_word16_t lag_window[];
104 extern const spx_word16_t lpc_window[];
105
106 #ifndef DISABLE_ENCODER
nb_encoder_init(const SpeexMode * m)107 void *nb_encoder_init(const SpeexMode *m)
108 {
109 EncState *st;
110 const SpeexNBMode *mode;
111 int i;
112
113 mode=(const SpeexNBMode *)m->mode;
114 st = (EncState*)speex_alloc(sizeof(EncState));
115 if (!st)
116 return NULL;
117 #if defined(VAR_ARRAYS) || defined (USE_ALLOCA)
118 st->stack = NULL;
119 #else
120 st->stack = (char*)speex_alloc_scratch(NB_ENC_STACK);
121 #endif
122
123 st->mode=m;
124
125 st->gamma1=mode->gamma1;
126 st->gamma2=mode->gamma2;
127 st->lpc_floor = mode->lpc_floor;
128
129 st->submodes=mode->submodes;
130 st->submodeID=st->submodeSelect=mode->defaultSubmode;
131 st->bounded_pitch = 1;
132
133 st->encode_submode = 1;
134
135 #ifdef VORBIS_PSYCHO
136 st->psy = vorbis_psy_init(8000, 256);
137 st->curve = (float*)speex_alloc(128*sizeof(float));
138 st->old_curve = (float*)speex_alloc(128*sizeof(float));
139 st->psy_window = (float*)speex_alloc(256*sizeof(float));
140 #endif
141
142 st->cumul_gain = 1024;
143
144 st->window= lpc_window;
145
146 /* Create the window for autocorrelation (lag-windowing) */
147 st->lagWindow = lag_window;
148
149 st->first = 1;
150 for (i=0;i<NB_ORDER;i++)
151 st->old_lsp[i]= DIV32(MULT16_16(QCONST16(3.1415927f, LSP_SHIFT), i+1), NB_ORDER+1);
152
153 st->innov_rms_save = NULL;
154
155 #ifndef DISABLE_VBR
156 vbr_init(&st->vbr);
157 st->vbr_quality = 8;
158 st->vbr_enabled = 0;
159 st->vbr_max = 0;
160 st->vad_enabled = 0;
161 st->dtx_enabled = 0;
162 st->dtx_count=0;
163 st->abr_enabled = 0;
164 st->abr_drift = 0;
165 st->abr_drift2 = 0;
166 #endif /* #ifndef DISABLE_VBR */
167
168 st->plc_tuning = 2;
169 st->complexity=2;
170 st->sampling_rate=8000;
171 st->isWideband = 0;
172 st->highpass_enabled = 1;
173
174 #ifdef ENABLE_VALGRIND
175 VALGRIND_MAKE_READABLE(st, NB_ENC_STACK);
176 #endif
177 return st;
178 }
179
nb_encoder_destroy(void * state)180 void nb_encoder_destroy(void *state)
181 {
182 EncState *st=(EncState *)state;
183 /* Free all allocated memory */
184 #if !(defined(VAR_ARRAYS) || defined (USE_ALLOCA))
185 speex_free_scratch(st->stack);
186 #endif
187
188 #ifndef DISABLE_VBR
189 vbr_destroy(&st->vbr);
190 #endif /* #ifndef DISABLE_VBR */
191
192 #ifdef VORBIS_PSYCHO
193 vorbis_psy_destroy(st->psy);
194 speex_free (st->curve);
195 speex_free (st->old_curve);
196 speex_free (st->psy_window);
197 #endif
198
199 /*Free state memory... should be last*/
200 speex_free(st);
201 }
202
203
nb_encoder_ctl(void * state,int request,void * ptr)204 int nb_encoder_ctl(void *state, int request, void *ptr)
205 {
206 EncState *st;
207 st=(EncState*)state;
208 switch(request)
209 {
210 case SPEEX_GET_FRAME_SIZE:
211 (*(spx_int32_t*)ptr) = NB_FRAME_SIZE;
212 break;
213 case SPEEX_SET_LOW_MODE:
214 case SPEEX_SET_MODE:
215 st->submodeSelect = st->submodeID = (*(spx_int32_t*)ptr);
216 break;
217 case SPEEX_GET_LOW_MODE:
218 case SPEEX_GET_MODE:
219 (*(spx_int32_t*)ptr) = st->submodeID;
220 break;
221 #ifndef DISABLE_VBR
222 case SPEEX_SET_VBR:
223 st->vbr_enabled = (*(spx_int32_t*)ptr);
224 break;
225 case SPEEX_GET_VBR:
226 (*(spx_int32_t*)ptr) = st->vbr_enabled;
227 break;
228 case SPEEX_SET_VAD:
229 st->vad_enabled = (*(spx_int32_t*)ptr);
230 break;
231 case SPEEX_GET_VAD:
232 (*(spx_int32_t*)ptr) = st->vad_enabled;
233 break;
234 case SPEEX_SET_DTX:
235 st->dtx_enabled = (*(spx_int32_t*)ptr);
236 break;
237 case SPEEX_GET_DTX:
238 (*(spx_int32_t*)ptr) = st->dtx_enabled;
239 break;
240 case SPEEX_SET_ABR:
241 st->abr_enabled = (*(spx_int32_t*)ptr);
242 st->vbr_enabled = st->abr_enabled!=0;
243 if (st->vbr_enabled)
244 {
245 spx_int32_t i=10;
246 spx_int32_t rate, target;
247 float vbr_qual;
248 target = (*(spx_int32_t*)ptr);
249 while (i>=0)
250 {
251 speex_encoder_ctl(st, SPEEX_SET_QUALITY, &i);
252 speex_encoder_ctl(st, SPEEX_GET_BITRATE, &rate);
253 if (rate <= target)
254 break;
255 i--;
256 }
257 vbr_qual=i;
258 if (vbr_qual<0)
259 vbr_qual=0;
260 speex_encoder_ctl(st, SPEEX_SET_VBR_QUALITY, &vbr_qual);
261 st->abr_count=0;
262 st->abr_drift=0;
263 st->abr_drift2=0;
264 }
265
266 break;
267 case SPEEX_GET_ABR:
268 (*(spx_int32_t*)ptr) = st->abr_enabled;
269 break;
270 #endif /* #ifndef DISABLE_VBR */
271 #if !defined(DISABLE_VBR) && !defined(DISABLE_FLOAT_API)
272 case SPEEX_SET_VBR_QUALITY:
273 st->vbr_quality = (*(float*)ptr);
274 break;
275 case SPEEX_GET_VBR_QUALITY:
276 (*(float*)ptr) = st->vbr_quality;
277 break;
278 #endif /* !defined(DISABLE_VBR) && !defined(DISABLE_FLOAT_API) */
279 case SPEEX_SET_QUALITY:
280 {
281 int quality = (*(spx_int32_t*)ptr);
282 if (quality < 0)
283 quality = 0;
284 if (quality > 10)
285 quality = 10;
286 st->submodeSelect = st->submodeID = ((const SpeexNBMode*)(st->mode->mode))->quality_map[quality];
287 }
288 break;
289 case SPEEX_SET_COMPLEXITY:
290 st->complexity = (*(spx_int32_t*)ptr);
291 if (st->complexity<0)
292 st->complexity=0;
293 break;
294 case SPEEX_GET_COMPLEXITY:
295 (*(spx_int32_t*)ptr) = st->complexity;
296 break;
297 case SPEEX_SET_BITRATE:
298 {
299 spx_int32_t i=10;
300 spx_int32_t rate, target;
301 target = (*(spx_int32_t*)ptr);
302 while (i>=0)
303 {
304 speex_encoder_ctl(st, SPEEX_SET_QUALITY, &i);
305 speex_encoder_ctl(st, SPEEX_GET_BITRATE, &rate);
306 if (rate <= target)
307 break;
308 i--;
309 }
310 }
311 break;
312 case SPEEX_GET_BITRATE:
313 if (st->submodes[st->submodeID])
314 (*(spx_int32_t*)ptr) = st->sampling_rate*SUBMODE(bits_per_frame)/NB_FRAME_SIZE;
315 else
316 (*(spx_int32_t*)ptr) = st->sampling_rate*(NB_SUBMODE_BITS+1)/NB_FRAME_SIZE;
317 break;
318 case SPEEX_SET_SAMPLING_RATE:
319 st->sampling_rate = (*(spx_int32_t*)ptr);
320 break;
321 case SPEEX_GET_SAMPLING_RATE:
322 (*(spx_int32_t*)ptr)=st->sampling_rate;
323 break;
324 case SPEEX_RESET_STATE:
325 {
326 int i;
327 st->bounded_pitch = 1;
328 st->first = 1;
329 for (i=0;i<NB_ORDER;i++)
330 st->old_lsp[i]= DIV32(MULT16_16(QCONST16(3.1415927f, LSP_SHIFT), i+1), NB_ORDER+1);
331 for (i=0;i<NB_ORDER;i++)
332 st->mem_sw[i]=st->mem_sw_whole[i]=st->mem_sp[i]=st->mem_exc[i]=0;
333 for (i=0;i<NB_FRAME_SIZE+NB_PITCH_END+1;i++)
334 st->excBuf[i]=st->swBuf[i]=0;
335 for (i=0;i<NB_WINDOW_SIZE-NB_FRAME_SIZE;i++)
336 st->winBuf[i]=0;
337 }
338 break;
339 case SPEEX_SET_SUBMODE_ENCODING:
340 st->encode_submode = (*(spx_int32_t*)ptr);
341 break;
342 case SPEEX_GET_SUBMODE_ENCODING:
343 (*(spx_int32_t*)ptr) = st->encode_submode;
344 break;
345 case SPEEX_GET_LOOKAHEAD:
346 (*(spx_int32_t*)ptr)=(NB_WINDOW_SIZE-NB_FRAME_SIZE);
347 break;
348 case SPEEX_SET_PLC_TUNING:
349 st->plc_tuning = (*(spx_int32_t*)ptr);
350 if (st->plc_tuning>100)
351 st->plc_tuning=100;
352 break;
353 case SPEEX_GET_PLC_TUNING:
354 (*(spx_int32_t*)ptr)=(st->plc_tuning);
355 break;
356 #ifndef DISABLE_VBR
357 case SPEEX_SET_VBR_MAX_BITRATE:
358 st->vbr_max = (*(spx_int32_t*)ptr);
359 break;
360 case SPEEX_GET_VBR_MAX_BITRATE:
361 (*(spx_int32_t*)ptr) = st->vbr_max;
362 break;
363 #endif /* #ifndef DISABLE_VBR */
364 case SPEEX_SET_HIGHPASS:
365 st->highpass_enabled = (*(spx_int32_t*)ptr);
366 break;
367 case SPEEX_GET_HIGHPASS:
368 (*(spx_int32_t*)ptr) = st->highpass_enabled;
369 break;
370
371 /* This is all internal stuff past this point */
372 case SPEEX_GET_PI_GAIN:
373 {
374 int i;
375 spx_word32_t *g = (spx_word32_t*)ptr;
376 for (i=0;i<NB_NB_SUBFRAMES;i++)
377 g[i]=st->pi_gain[i];
378 }
379 break;
380 case SPEEX_GET_EXC:
381 {
382 int i;
383 for (i=0;i<NB_NB_SUBFRAMES;i++)
384 ((spx_word16_t*)ptr)[i] = compute_rms16(st->exc+i*NB_SUBFRAME_SIZE, NB_SUBFRAME_SIZE);
385 }
386 break;
387 #ifndef DISABLE_VBR
388 case SPEEX_GET_RELATIVE_QUALITY:
389 (*(float*)ptr)=st->relative_quality;
390 break;
391 #endif /* #ifndef DISABLE_VBR */
392 case SPEEX_SET_INNOVATION_SAVE:
393 st->innov_rms_save = (spx_word16_t*)ptr;
394 break;
395 case SPEEX_SET_WIDEBAND:
396 st->isWideband = *((spx_int32_t*)ptr);
397 break;
398 case SPEEX_GET_STACK:
399 *((char**)ptr) = st->stack;
400 break;
401 default:
402 speex_warning_int("Unknown nb_ctl request: ", request);
403 return -1;
404 }
405 return 0;
406 }
407
408
nb_encode(void * state,void * vin,SpeexBits * bits)409 int nb_encode(void *state, void *vin, SpeexBits *bits)
410 {
411 EncState *st;
412 int i, sub, roots;
413 int ol_pitch;
414 spx_word16_t ol_pitch_coef;
415 spx_word32_t ol_gain;
416 VARDECL(spx_word16_t *target);
417 VARDECL(spx_sig_t *innov);
418 VARDECL(spx_word32_t *exc32);
419 VARDECL(spx_mem_t *mem);
420 VARDECL(spx_coef_t *bw_lpc1);
421 VARDECL(spx_coef_t *bw_lpc2);
422 VARDECL(spx_coef_t *lpc);
423 VARDECL(spx_lsp_t *lsp);
424 VARDECL(spx_lsp_t *qlsp);
425 VARDECL(spx_lsp_t *interp_lsp);
426 VARDECL(spx_lsp_t *interp_qlsp);
427 VARDECL(spx_coef_t *interp_lpc);
428 VARDECL(spx_coef_t *interp_qlpc);
429 char *stack;
430 VARDECL(spx_word16_t *syn_resp);
431
432 spx_word32_t ener=0;
433 spx_word16_t fine_gain;
434 spx_word16_t *in = (spx_word16_t*)vin;
435
436 st=(EncState *)state;
437 stack=st->stack;
438
439 ALLOC(lpc, NB_ORDER, spx_coef_t);
440 ALLOC(bw_lpc1, NB_ORDER, spx_coef_t);
441 ALLOC(bw_lpc2, NB_ORDER, spx_coef_t);
442 ALLOC(lsp, NB_ORDER, spx_lsp_t);
443 ALLOC(qlsp, NB_ORDER, spx_lsp_t);
444 ALLOC(interp_lsp, NB_ORDER, spx_lsp_t);
445 ALLOC(interp_qlsp, NB_ORDER, spx_lsp_t);
446 ALLOC(interp_lpc, NB_ORDER, spx_coef_t);
447 ALLOC(interp_qlpc, NB_ORDER, spx_coef_t);
448
449 st->exc = st->excBuf + NB_PITCH_END + 2;
450 st->sw = st->swBuf + NB_PITCH_END + 2;
451 /* Move signals 1 frame towards the past */
452 SPEEX_MOVE(st->excBuf, st->excBuf+NB_FRAME_SIZE, NB_PITCH_END+2);
453 SPEEX_MOVE(st->swBuf, st->swBuf+NB_FRAME_SIZE, NB_PITCH_END+2);
454
455 if (st->highpass_enabled)
456 highpass(in, in, NB_FRAME_SIZE, (st->isWideband?HIGHPASS_WIDEBAND:HIGHPASS_NARROWBAND)|HIGHPASS_INPUT, st->mem_hp);
457
458 {
459 VARDECL(spx_word16_t *w_sig);
460 VARDECL(spx_word16_t *autocorr);
461 ALLOC(w_sig, NB_WINDOW_SIZE, spx_word16_t);
462 ALLOC(autocorr, NB_ORDER+1, spx_word16_t);
463 /* Window for analysis */
464 for (i=0;i<NB_WINDOW_SIZE-NB_FRAME_SIZE;i++)
465 w_sig[i] = MULT16_16_Q15(st->winBuf[i],st->window[i]);
466 for (;i<NB_WINDOW_SIZE;i++)
467 w_sig[i] = MULT16_16_Q15(in[i-NB_WINDOW_SIZE+NB_FRAME_SIZE],st->window[i]);
468 /* Compute auto-correlation */
469 _spx_autocorr(w_sig, autocorr, NB_ORDER+1, NB_WINDOW_SIZE);
470 autocorr[0] = ADD16(autocorr[0],MULT16_16_Q15(autocorr[0],st->lpc_floor)); /* Noise floor in auto-correlation domain */
471
472 /* Lag windowing: equivalent to filtering in the power-spectrum domain */
473 for (i=0;i<NB_ORDER+1;i++)
474 autocorr[i] = MULT16_16_Q15(autocorr[i],st->lagWindow[i]);
475 autocorr[0] = ADD16(autocorr[0],1);
476
477 /* Levinson-Durbin */
478 _spx_lpc(lpc, autocorr, NB_ORDER);
479 /* LPC to LSPs (x-domain) transform */
480 roots=lpc_to_lsp (lpc, NB_ORDER, lsp, 10, LSP_DELTA1, stack);
481 /* Check if we found all the roots */
482 if (roots!=NB_ORDER)
483 {
484 /*If we can't find all LSP's, do some damage control and use previous filter*/
485 for (i=0;i<NB_ORDER;i++)
486 {
487 lsp[i]=st->old_lsp[i];
488 }
489 }
490 }
491
492
493
494
495 /* Whole frame analysis (open-loop estimation of pitch and excitation gain) */
496 {
497 int diff = NB_WINDOW_SIZE-NB_FRAME_SIZE;
498 if (st->first)
499 for (i=0;i<NB_ORDER;i++)
500 interp_lsp[i] = lsp[i];
501 else
502 lsp_interpolate(st->old_lsp, lsp, interp_lsp, NB_ORDER, NB_NB_SUBFRAMES, NB_NB_SUBFRAMES<<1, LSP_MARGIN);
503
504 /* Compute interpolated LPCs (unquantized) for whole frame*/
505 lsp_to_lpc(interp_lsp, interp_lpc, NB_ORDER,stack);
506
507
508 /*Open-loop pitch*/
509 if (!st->submodes[st->submodeID] || (st->complexity>2 && SUBMODE(have_subframe_gain)<3) || SUBMODE(forced_pitch_gain) || SUBMODE(lbr_pitch) != -1
510 #ifndef DISABLE_VBR
511 || st->vbr_enabled || st->vad_enabled
512 #endif
513 )
514 {
515 int nol_pitch[6];
516 spx_word16_t nol_pitch_coef[6];
517
518 bw_lpc(0.9, interp_lpc, bw_lpc1, NB_ORDER);
519 bw_lpc(0.55, interp_lpc, bw_lpc2, NB_ORDER);
520
521 SPEEX_COPY(st->sw, st->winBuf, diff);
522 SPEEX_COPY(st->sw+diff, in, NB_FRAME_SIZE-diff);
523 filter10(st->sw, bw_lpc1, bw_lpc2, st->sw, NB_FRAME_SIZE, st->mem_sw_whole, stack);
524
525 open_loop_nbest_pitch(st->sw, NB_PITCH_START, NB_PITCH_END, NB_FRAME_SIZE,
526 nol_pitch, nol_pitch_coef, 6, stack);
527 ol_pitch=nol_pitch[0];
528 ol_pitch_coef = nol_pitch_coef[0];
529 /*Try to remove pitch multiples*/
530 for (i=1;i<6;i++)
531 {
532 #ifdef FIXED_POINT
533 if ((nol_pitch_coef[i]>MULT16_16_Q15(nol_pitch_coef[0],27853)) &&
534 #else
535 if ((nol_pitch_coef[i]>.85*nol_pitch_coef[0]) &&
536 #endif
537 (ABS(2*nol_pitch[i]-ol_pitch)<=2 || ABS(3*nol_pitch[i]-ol_pitch)<=3 ||
538 ABS(4*nol_pitch[i]-ol_pitch)<=4 || ABS(5*nol_pitch[i]-ol_pitch)<=5))
539 {
540 /*ol_pitch_coef=nol_pitch_coef[i];*/
541 ol_pitch = nol_pitch[i];
542 }
543 }
544 /*if (ol_pitch>50)
545 ol_pitch/=2;*/
546 /*ol_pitch_coef = sqrt(ol_pitch_coef);*/
547
548 } else {
549 ol_pitch=0;
550 ol_pitch_coef=0;
551 }
552
553 /*Compute "real" excitation*/
554 /*SPEEX_COPY(st->exc, st->winBuf, diff);
555 SPEEX_COPY(st->exc+diff, in, NB_FRAME_SIZE-diff);*/
556 fir_mem16(st->winBuf, interp_lpc, st->exc, diff, NB_ORDER, st->mem_exc, stack);
557 fir_mem16(in, interp_lpc, st->exc+diff, NB_FRAME_SIZE-diff, NB_ORDER, st->mem_exc, stack);
558
559 /* Compute open-loop excitation gain */
560 {
561 spx_word16_t g = compute_rms16(st->exc, NB_FRAME_SIZE);
562 if (st->submodeID!=1 && ol_pitch>0)
563 ol_gain = MULT16_16(g, MULT16_16_Q14(QCONST16(1.1,14),
564 spx_sqrt(QCONST32(1.,28)-MULT16_32_Q15(QCONST16(.8,15),SHL32(MULT16_16(ol_pitch_coef,ol_pitch_coef),16)))));
565 else
566 ol_gain = SHL32(EXTEND32(g),SIG_SHIFT);
567 }
568 }
569
570 #ifdef VORBIS_PSYCHO
571 SPEEX_MOVE(st->psy_window, st->psy_window+NB_FRAME_SIZE, 256-NB_FRAME_SIZE);
572 SPEEX_COPY(&st->psy_window[256-NB_FRAME_SIZE], in, NB_FRAME_SIZE);
573 compute_curve(st->psy, st->psy_window, st->curve);
574 /*print_vec(st->curve, 128, "curve");*/
575 if (st->first)
576 SPEEX_COPY(st->old_curve, st->curve, 128);
577 #endif
578
579 /*VBR stuff*/
580 #ifndef DISABLE_VBR
581 if (st->vbr_enabled||st->vad_enabled)
582 {
583 float lsp_dist=0;
584 for (i=0;i<NB_ORDER;i++)
585 lsp_dist += (st->old_lsp[i] - lsp[i])*(st->old_lsp[i] - lsp[i]);
586 lsp_dist /= LSP_SCALING*LSP_SCALING;
587
588 if (st->abr_enabled)
589 {
590 float qual_change=0;
591 if (st->abr_drift2 * st->abr_drift > 0)
592 {
593 /* Only adapt if long-term and short-term drift are the same sign */
594 qual_change = -.00001*st->abr_drift/(1+st->abr_count);
595 if (qual_change>.05)
596 qual_change=.05;
597 if (qual_change<-.05)
598 qual_change=-.05;
599 }
600 st->vbr_quality += qual_change;
601 if (st->vbr_quality>10)
602 st->vbr_quality=10;
603 if (st->vbr_quality<0)
604 st->vbr_quality=0;
605 }
606
607 st->relative_quality = vbr_analysis(&st->vbr, in, NB_FRAME_SIZE, ol_pitch, GAIN_SCALING_1*ol_pitch_coef);
608 /*if (delta_qual<0)*/
609 /* delta_qual*=.1*(3+st->vbr_quality);*/
610 if (st->vbr_enabled)
611 {
612 spx_int32_t mode;
613 int choice=0;
614 float min_diff=100;
615 mode = 8;
616 while (mode)
617 {
618 int v1;
619 float thresh;
620 v1=(int)floor(st->vbr_quality);
621 if (v1==10)
622 thresh = vbr_nb_thresh[mode][v1];
623 else
624 thresh = (st->vbr_quality-v1)*vbr_nb_thresh[mode][v1+1] + (1+v1-st->vbr_quality)*vbr_nb_thresh[mode][v1];
625 if (st->relative_quality > thresh &&
626 st->relative_quality-thresh<min_diff)
627 {
628 choice = mode;
629 min_diff = st->relative_quality-thresh;
630 }
631 mode--;
632 }
633 mode=choice;
634 if (mode==0)
635 {
636 if (st->dtx_count==0 || lsp_dist>.05 || !st->dtx_enabled || st->dtx_count>20)
637 {
638 mode=1;
639 st->dtx_count=1;
640 } else {
641 mode=0;
642 st->dtx_count++;
643 }
644 } else {
645 st->dtx_count=0;
646 }
647
648 speex_encoder_ctl(state, SPEEX_SET_MODE, &mode);
649 if (st->vbr_max>0)
650 {
651 spx_int32_t rate;
652 speex_encoder_ctl(state, SPEEX_GET_BITRATE, &rate);
653 if (rate > st->vbr_max)
654 {
655 rate = st->vbr_max;
656 speex_encoder_ctl(state, SPEEX_SET_BITRATE, &rate);
657 }
658 }
659
660 if (st->abr_enabled)
661 {
662 spx_int32_t bitrate;
663 speex_encoder_ctl(state, SPEEX_GET_BITRATE, &bitrate);
664 st->abr_drift+=(bitrate-st->abr_enabled);
665 st->abr_drift2 = .95*st->abr_drift2 + .05*(bitrate-st->abr_enabled);
666 st->abr_count += 1.0;
667 }
668
669 } else {
670 /*VAD only case*/
671 int mode;
672 if (st->relative_quality<2)
673 {
674 if (st->dtx_count==0 || lsp_dist>.05 || !st->dtx_enabled || st->dtx_count>20)
675 {
676 st->dtx_count=1;
677 mode=1;
678 } else {
679 mode=0;
680 st->dtx_count++;
681 }
682 } else {
683 st->dtx_count = 0;
684 mode=st->submodeSelect;
685 }
686 /*speex_encoder_ctl(state, SPEEX_SET_MODE, &mode);*/
687 st->submodeID=mode;
688 }
689 } else {
690 st->relative_quality = -1;
691 }
692 #endif /* #ifndef DISABLE_VBR */
693
694 if (st->encode_submode)
695 {
696 /* First, transmit a zero for narrowband */
697 speex_bits_pack(bits, 0, 1);
698
699 /* Transmit the sub-mode we use for this frame */
700 speex_bits_pack(bits, st->submodeID, NB_SUBMODE_BITS);
701
702 }
703
704 /* If null mode (no transmission), just set a couple things to zero*/
705 if (st->submodes[st->submodeID] == NULL)
706 {
707 for (i=0;i<NB_FRAME_SIZE;i++)
708 st->exc[i]=st->sw[i]=VERY_SMALL;
709
710 for (i=0;i<NB_ORDER;i++)
711 st->mem_sw[i]=0;
712 st->first=1;
713 st->bounded_pitch = 1;
714
715 SPEEX_COPY(st->winBuf, in+2*NB_FRAME_SIZE-NB_WINDOW_SIZE, NB_WINDOW_SIZE-NB_FRAME_SIZE);
716
717 /* Clear memory (no need to really compute it) */
718 for (i=0;i<NB_ORDER;i++)
719 st->mem_sp[i] = 0;
720 return 0;
721
722 }
723
724 /* LSP Quantization */
725 if (st->first)
726 {
727 for (i=0;i<NB_ORDER;i++)
728 st->old_lsp[i] = lsp[i];
729 }
730
731
732 /*Quantize LSPs*/
733 #if 1 /*0 for unquantized*/
734 SUBMODE(lsp_quant)(lsp, qlsp, NB_ORDER, bits);
735 #else
736 for (i=0;i<NB_ORDER;i++)
737 qlsp[i]=lsp[i];
738 #endif
739
740 /*If we use low bit-rate pitch mode, transmit open-loop pitch*/
741 if (SUBMODE(lbr_pitch)!=-1)
742 {
743 speex_bits_pack(bits, ol_pitch-NB_PITCH_START, 7);
744 }
745
746 if (SUBMODE(forced_pitch_gain))
747 {
748 int quant;
749 /* This just damps the pitch a bit, because it tends to be too aggressive when forced */
750 ol_pitch_coef = MULT16_16_Q15(QCONST16(.9,15), ol_pitch_coef);
751 #ifdef FIXED_POINT
752 quant = PSHR16(MULT16_16_16(15, ol_pitch_coef),GAIN_SHIFT);
753 #else
754 quant = (int)floor(.5+15*ol_pitch_coef*GAIN_SCALING_1);
755 #endif
756 if (quant>15)
757 quant=15;
758 if (quant<0)
759 quant=0;
760 speex_bits_pack(bits, quant, 4);
761 ol_pitch_coef=MULT16_16_P15(QCONST16(0.066667,15),SHL16(quant,GAIN_SHIFT));
762 }
763
764
765 /*Quantize and transmit open-loop excitation gain*/
766 #ifdef FIXED_POINT
767 {
768 int qe = scal_quant32(ol_gain, ol_gain_table, 32);
769 /*ol_gain = exp(qe/3.5)*SIG_SCALING;*/
770 ol_gain = MULT16_32_Q15(28406,ol_gain_table[qe]);
771 speex_bits_pack(bits, qe, 5);
772 }
773 #else
774 {
775 int qe = (int)(floor(.5+3.5*log(ol_gain*1.0/SIG_SCALING)));
776 if (qe<0)
777 qe=0;
778 if (qe>31)
779 qe=31;
780 ol_gain = exp(qe/3.5)*SIG_SCALING;
781 speex_bits_pack(bits, qe, 5);
782 }
783 #endif
784
785
786
787 /* Special case for first frame */
788 if (st->first)
789 {
790 for (i=0;i<NB_ORDER;i++)
791 st->old_qlsp[i] = qlsp[i];
792 }
793
794 /* Target signal */
795 ALLOC(target, NB_SUBFRAME_SIZE, spx_word16_t);
796 ALLOC(innov, NB_SUBFRAME_SIZE, spx_sig_t);
797 ALLOC(exc32, NB_SUBFRAME_SIZE, spx_word32_t);
798 ALLOC(syn_resp, NB_SUBFRAME_SIZE, spx_word16_t);
799 ALLOC(mem, NB_ORDER, spx_mem_t);
800
801 /* Loop on sub-frames */
802 for (sub=0;sub<NB_NB_SUBFRAMES;sub++)
803 {
804 int offset;
805 spx_word16_t *sw;
806 spx_word16_t *exc, *inBuf;
807 int pitch;
808 int response_bound = NB_SUBFRAME_SIZE;
809
810 /* Offset relative to start of frame */
811 offset = NB_SUBFRAME_SIZE*sub;
812 /* Excitation */
813 exc=st->exc+offset;
814 /* Weighted signal */
815 sw=st->sw+offset;
816
817 /* LSP interpolation (quantized and unquantized) */
818 lsp_interpolate(st->old_lsp, lsp, interp_lsp, NB_ORDER, sub, NB_NB_SUBFRAMES, LSP_MARGIN);
819 lsp_interpolate(st->old_qlsp, qlsp, interp_qlsp, NB_ORDER, sub, NB_NB_SUBFRAMES, LSP_MARGIN);
820
821 /* Compute interpolated LPCs (quantized and unquantized) */
822 lsp_to_lpc(interp_lsp, interp_lpc, NB_ORDER,stack);
823
824 lsp_to_lpc(interp_qlsp, interp_qlpc, NB_ORDER, stack);
825
826 /* Compute analysis filter gain at w=pi (for use in SB-CELP) */
827 {
828 spx_word32_t pi_g=LPC_SCALING;
829 for (i=0;i<NB_ORDER;i+=2)
830 {
831 /*pi_g += -st->interp_qlpc[i] + st->interp_qlpc[i+1];*/
832 pi_g = ADD32(pi_g, SUB32(EXTEND32(interp_qlpc[i+1]),EXTEND32(interp_qlpc[i])));
833 }
834 st->pi_gain[sub] = pi_g;
835 }
836
837 #ifdef VORBIS_PSYCHO
838 {
839 float curr_curve[128];
840 float fact = ((float)sub+1.0f)/NB_NB_SUBFRAMES;
841 for (i=0;i<128;i++)
842 curr_curve[i] = (1.0f-fact)*st->old_curve[i] + fact*st->curve[i];
843 curve_to_lpc(st->psy, curr_curve, bw_lpc1, bw_lpc2, 10);
844 }
845 #else
846 /* Compute bandwidth-expanded (unquantized) LPCs for perceptual weighting */
847 bw_lpc(st->gamma1, interp_lpc, bw_lpc1, NB_ORDER);
848 bw_lpc(st->gamma2, interp_lpc, bw_lpc2, NB_ORDER);
849 /*print_vec(st->bw_lpc1, 10, "bw_lpc");*/
850 #endif
851
852 /*FIXME: This will break if we change the window size */
853 speex_assert(NB_WINDOW_SIZE-NB_FRAME_SIZE == NB_SUBFRAME_SIZE);
854 if (sub==0)
855 inBuf = st->winBuf;
856 else
857 inBuf = &in[((sub-1)*NB_SUBFRAME_SIZE)];
858 for (i=0;i<NB_SUBFRAME_SIZE;i++)
859 sw[i] = inBuf[i];
860
861 if (st->complexity==0)
862 response_bound >>= 1;
863 compute_impulse_response(interp_qlpc, bw_lpc1, bw_lpc2, syn_resp, response_bound, NB_ORDER, stack);
864 for (i=response_bound;i<NB_SUBFRAME_SIZE;i++)
865 syn_resp[i]=VERY_SMALL;
866
867 /* Compute zero response of A(z/g1) / ( A(z/g2) * A(z) ) */
868 for (i=0;i<NB_ORDER;i++)
869 mem[i]=SHL32(st->mem_sp[i],1);
870 for (i=0;i<NB_SUBFRAME_SIZE;i++)
871 exc[i] = VERY_SMALL;
872 #ifdef SHORTCUTS2
873 iir_mem16(exc, interp_qlpc, exc, response_bound, NB_ORDER, mem, stack);
874 for (i=0;i<NB_ORDER;i++)
875 mem[i]=SHL32(st->mem_sw[i],1);
876 filter10(exc, st->bw_lpc1, st->bw_lpc2, exc, response_bound, mem, stack);
877 SPEEX_MEMSET(&exc[response_bound], 0, NB_SUBFRAME_SIZE-response_bound);
878 #else
879 iir_mem16(exc, interp_qlpc, exc, NB_SUBFRAME_SIZE, NB_ORDER, mem, stack);
880 for (i=0;i<NB_ORDER;i++)
881 mem[i]=SHL32(st->mem_sw[i],1);
882 filter10(exc, bw_lpc1, bw_lpc2, exc, NB_SUBFRAME_SIZE, mem, stack);
883 #endif
884
885 /* Compute weighted signal */
886 for (i=0;i<NB_ORDER;i++)
887 mem[i]=st->mem_sw[i];
888 filter10(sw, bw_lpc1, bw_lpc2, sw, NB_SUBFRAME_SIZE, mem, stack);
889
890 if (st->complexity==0)
891 for (i=0;i<NB_ORDER;i++)
892 st->mem_sw[i]=mem[i];
893
894 /* Compute target signal (saturation prevents overflows on clipped input speech) */
895 for (i=0;i<NB_SUBFRAME_SIZE;i++)
896 target[i]=EXTRACT16(SATURATE(SUB32(sw[i],PSHR32(exc[i],1)),32767));
897
898 for (i=0;i<NB_SUBFRAME_SIZE;i++)
899 exc[i] = inBuf[i];
900 fir_mem16(exc, interp_qlpc, exc, NB_SUBFRAME_SIZE, NB_ORDER, st->mem_exc2, stack);
901 /* If we have a long-term predictor (otherwise, something's wrong) */
902 speex_assert (SUBMODE(ltp_quant));
903 {
904 int pit_min, pit_max;
905 /* Long-term prediction */
906 if (SUBMODE(lbr_pitch) != -1)
907 {
908 /* Low bit-rate pitch handling */
909 int margin;
910 margin = SUBMODE(lbr_pitch);
911 if (margin)
912 {
913 if (ol_pitch < NB_PITCH_START+margin-1)
914 ol_pitch=NB_PITCH_START+margin-1;
915 if (ol_pitch > NB_PITCH_END-margin)
916 ol_pitch=NB_PITCH_END-margin;
917 pit_min = ol_pitch-margin+1;
918 pit_max = ol_pitch+margin;
919 } else {
920 pit_min=pit_max=ol_pitch;
921 }
922 } else {
923 pit_min = NB_PITCH_START;
924 pit_max = NB_PITCH_END;
925 }
926
927 /* Force pitch to use only the current frame if needed */
928 if (st->bounded_pitch && pit_max>offset)
929 pit_max=offset;
930
931 /* Perform pitch search */
932 pitch = SUBMODE(ltp_quant)(target, sw, interp_qlpc, bw_lpc1, bw_lpc2,
933 exc32, SUBMODE(ltp_params), pit_min, pit_max, ol_pitch_coef,
934 NB_ORDER, NB_SUBFRAME_SIZE, bits, stack,
935 exc, syn_resp, st->complexity, 0, st->plc_tuning, &st->cumul_gain);
936
937 st->pitch[sub]=pitch;
938 }
939 /* Quantization of innovation */
940 SPEEX_MEMSET(innov, 0, NB_SUBFRAME_SIZE);
941
942 /* FIXME: Make sure this is safe from overflows (so far so good) */
943 for (i=0;i<NB_SUBFRAME_SIZE;i++)
944 exc[i] = EXTRACT16(SUB32(EXTEND32(exc[i]), PSHR32(exc32[i],SIG_SHIFT-1)));
945
946 ener = SHL32(EXTEND32(compute_rms16(exc, NB_SUBFRAME_SIZE)),SIG_SHIFT);
947
948 /*FIXME: Should use DIV32_16 and make sure result fits in 16 bits */
949 #ifdef FIXED_POINT
950 {
951 spx_word32_t f = PDIV32(ener,PSHR32(ol_gain,SIG_SHIFT));
952 if (f<=32767)
953 fine_gain = f;
954 else
955 fine_gain = 32767;
956 }
957 #else
958 fine_gain = PDIV32_16(ener,PSHR32(ol_gain,SIG_SHIFT));
959 #endif
960 /* Calculate gain correction for the sub-frame (if any) */
961 if (SUBMODE(have_subframe_gain))
962 {
963 int qe;
964 if (SUBMODE(have_subframe_gain)==3)
965 {
966 qe = scal_quant(fine_gain, exc_gain_quant_scal3_bound, 8);
967 speex_bits_pack(bits, qe, 3);
968 ener=MULT16_32_Q14(exc_gain_quant_scal3[qe],ol_gain);
969 } else {
970 qe = scal_quant(fine_gain, exc_gain_quant_scal1_bound, 2);
971 speex_bits_pack(bits, qe, 1);
972 ener=MULT16_32_Q14(exc_gain_quant_scal1[qe],ol_gain);
973 }
974 } else {
975 ener=ol_gain;
976 }
977
978 /*printf ("%f %f\n", ener, ol_gain);*/
979
980 /* Normalize innovation */
981 signal_div(target, target, ener, NB_SUBFRAME_SIZE);
982
983 /* Quantize innovation */
984 speex_assert (SUBMODE(innovation_quant));
985 {
986 /* Codebook search */
987 SUBMODE(innovation_quant)(target, interp_qlpc, bw_lpc1, bw_lpc2,
988 SUBMODE(innovation_params), NB_ORDER, NB_SUBFRAME_SIZE,
989 innov, syn_resp, bits, stack, st->complexity, SUBMODE(double_codebook));
990
991 /* De-normalize innovation and update excitation */
992 signal_mul(innov, innov, ener, NB_SUBFRAME_SIZE);
993
994 /* In some (rare) modes, we do a second search (more bits) to reduce noise even more */
995 if (SUBMODE(double_codebook)) {
996 char *tmp_stack=stack;
997 VARDECL(spx_sig_t *innov2);
998 ALLOC(innov2, NB_SUBFRAME_SIZE, spx_sig_t);
999 SPEEX_MEMSET(innov2, 0, NB_SUBFRAME_SIZE);
1000 for (i=0;i<NB_SUBFRAME_SIZE;i++)
1001 target[i]=MULT16_16_P13(QCONST16(2.2f,13), target[i]);
1002 SUBMODE(innovation_quant)(target, interp_qlpc, bw_lpc1, bw_lpc2,
1003 SUBMODE(innovation_params), NB_ORDER, NB_SUBFRAME_SIZE,
1004 innov2, syn_resp, bits, stack, st->complexity, 0);
1005 signal_mul(innov2, innov2, MULT16_32_Q15(QCONST16(0.454545f,15),ener), NB_SUBFRAME_SIZE);
1006 for (i=0;i<NB_SUBFRAME_SIZE;i++)
1007 innov[i] = ADD32(innov[i],innov2[i]);
1008 stack = tmp_stack;
1009 }
1010 for (i=0;i<NB_SUBFRAME_SIZE;i++)
1011 exc[i] = EXTRACT16(SATURATE32(PSHR32(ADD32(SHL32(exc32[i],1),innov[i]),SIG_SHIFT),32767));
1012 if (st->innov_rms_save)
1013 st->innov_rms_save[sub] = compute_rms(innov, NB_SUBFRAME_SIZE);
1014 }
1015
1016 /* Final signal synthesis from excitation */
1017 iir_mem16(exc, interp_qlpc, sw, NB_SUBFRAME_SIZE, NB_ORDER, st->mem_sp, stack);
1018
1019 /* Compute weighted signal again, from synthesized speech (not sure it's the right thing) */
1020 if (st->complexity!=0)
1021 filter10(sw, bw_lpc1, bw_lpc2, sw, NB_SUBFRAME_SIZE, st->mem_sw, stack);
1022
1023 }
1024
1025 /* Store the LSPs for interpolation in the next frame */
1026 if (st->submodeID>=1)
1027 {
1028 for (i=0;i<NB_ORDER;i++)
1029 st->old_lsp[i] = lsp[i];
1030 for (i=0;i<NB_ORDER;i++)
1031 st->old_qlsp[i] = qlsp[i];
1032 }
1033
1034 #ifdef VORBIS_PSYCHO
1035 if (st->submodeID>=1)
1036 SPEEX_COPY(st->old_curve, st->curve, 128);
1037 #endif
1038
1039 if (st->submodeID==1)
1040 {
1041 #ifndef DISABLE_VBR
1042 if (st->dtx_count)
1043 speex_bits_pack(bits, 15, 4);
1044 else
1045 #endif
1046 speex_bits_pack(bits, 0, 4);
1047 }
1048
1049 /* The next frame will not be the first (Duh!) */
1050 st->first = 0;
1051 SPEEX_COPY(st->winBuf, in+2*NB_FRAME_SIZE-NB_WINDOW_SIZE, NB_WINDOW_SIZE-NB_FRAME_SIZE);
1052
1053 if (SUBMODE(innovation_quant) == noise_codebook_quant || st->submodeID==0)
1054 st->bounded_pitch = 1;
1055 else
1056 st->bounded_pitch = 0;
1057
1058 return 1;
1059 }
1060 #endif /* DISABLE_ENCODER */
1061
1062
1063 #ifndef DISABLE_DECODER
nb_decoder_init(const SpeexMode * m)1064 void *nb_decoder_init(const SpeexMode *m)
1065 {
1066 DecState *st;
1067 const SpeexNBMode *mode;
1068 int i;
1069
1070 mode=(const SpeexNBMode*)m->mode;
1071 st = (DecState *)speex_alloc(sizeof(DecState));
1072 if (!st)
1073 return NULL;
1074 #if defined(VAR_ARRAYS) || defined (USE_ALLOCA)
1075 st->stack = NULL;
1076 #else
1077 st->stack = (char*)speex_alloc_scratch(NB_DEC_STACK);
1078 #endif
1079
1080 st->mode=m;
1081
1082
1083 st->encode_submode = 1;
1084
1085 st->first=1;
1086 /* Codec parameters, should eventually have several "modes"*/
1087
1088 st->submodes=mode->submodes;
1089 st->submodeID=mode->defaultSubmode;
1090
1091 st->lpc_enh_enabled=1;
1092
1093 SPEEX_MEMSET(st->excBuf, 0, NB_FRAME_SIZE + NB_PITCH_END);
1094
1095 st->last_pitch = 40;
1096 st->count_lost=0;
1097 st->pitch_gain_buf[0] = st->pitch_gain_buf[1] = st->pitch_gain_buf[2] = 0;
1098 st->pitch_gain_buf_idx = 0;
1099 st->seed = 1000;
1100
1101 st->sampling_rate=8000;
1102 st->last_ol_gain = 0;
1103
1104 st->user_callback.func = &speex_default_user_handler;
1105 st->user_callback.data = NULL;
1106 for (i=0;i<16;i++)
1107 st->speex_callbacks[i].func = NULL;
1108
1109 st->voc_m1=st->voc_m2=st->voc_mean=0;
1110 st->voc_offset=0;
1111 st->dtx_enabled=0;
1112 st->isWideband = 0;
1113 st->highpass_enabled = 1;
1114
1115 #ifdef ENABLE_VALGRIND
1116 VALGRIND_MAKE_READABLE(st, NB_DEC_STACK);
1117 #endif
1118 return st;
1119 }
1120
nb_decoder_destroy(void * state)1121 void nb_decoder_destroy(void *state)
1122 {
1123 #if !(defined(VAR_ARRAYS) || defined (USE_ALLOCA))
1124 DecState *st;
1125 st=(DecState*)state;
1126
1127 speex_free_scratch(st->stack);
1128 #endif
1129
1130 speex_free(state);
1131 }
1132
nb_decoder_ctl(void * state,int request,void * ptr)1133 int nb_decoder_ctl(void *state, int request, void *ptr)
1134 {
1135 DecState *st;
1136 st=(DecState*)state;
1137 switch(request)
1138 {
1139 case SPEEX_SET_LOW_MODE:
1140 case SPEEX_SET_MODE:
1141 st->submodeID = (*(spx_int32_t*)ptr);
1142 break;
1143 case SPEEX_GET_LOW_MODE:
1144 case SPEEX_GET_MODE:
1145 (*(spx_int32_t*)ptr) = st->submodeID;
1146 break;
1147 case SPEEX_SET_ENH:
1148 st->lpc_enh_enabled = *((spx_int32_t*)ptr);
1149 break;
1150 case SPEEX_GET_ENH:
1151 *((spx_int32_t*)ptr) = st->lpc_enh_enabled;
1152 break;
1153 case SPEEX_GET_FRAME_SIZE:
1154 (*(spx_int32_t*)ptr) = NB_FRAME_SIZE;
1155 break;
1156 case SPEEX_GET_BITRATE:
1157 if (st->submodes[st->submodeID])
1158 (*(spx_int32_t*)ptr) = st->sampling_rate*SUBMODE(bits_per_frame)/NB_FRAME_SIZE;
1159 else
1160 (*(spx_int32_t*)ptr) = st->sampling_rate*(NB_SUBMODE_BITS+1)/NB_FRAME_SIZE;
1161 break;
1162 case SPEEX_SET_SAMPLING_RATE:
1163 st->sampling_rate = (*(spx_int32_t*)ptr);
1164 break;
1165 case SPEEX_GET_SAMPLING_RATE:
1166 (*(spx_int32_t*)ptr)=st->sampling_rate;
1167 break;
1168 case SPEEX_SET_HANDLER:
1169 {
1170 SpeexCallback *c = (SpeexCallback*)ptr;
1171 st->speex_callbacks[c->callback_id].func=c->func;
1172 st->speex_callbacks[c->callback_id].data=c->data;
1173 st->speex_callbacks[c->callback_id].callback_id=c->callback_id;
1174 }
1175 break;
1176 case SPEEX_SET_USER_HANDLER:
1177 {
1178 SpeexCallback *c = (SpeexCallback*)ptr;
1179 st->user_callback.func=c->func;
1180 st->user_callback.data=c->data;
1181 st->user_callback.callback_id=c->callback_id;
1182 }
1183 break;
1184 case SPEEX_RESET_STATE:
1185 {
1186 int i;
1187 for (i=0;i<NB_ORDER;i++)
1188 st->mem_sp[i]=0;
1189 for (i=0;i<NB_FRAME_SIZE + NB_PITCH_END + 1;i++)
1190 st->excBuf[i]=0;
1191 }
1192 break;
1193 case SPEEX_SET_SUBMODE_ENCODING:
1194 st->encode_submode = (*(spx_int32_t*)ptr);
1195 break;
1196 case SPEEX_GET_SUBMODE_ENCODING:
1197 (*(spx_int32_t*)ptr) = st->encode_submode;
1198 break;
1199 case SPEEX_GET_LOOKAHEAD:
1200 (*(spx_int32_t*)ptr)=NB_SUBFRAME_SIZE;
1201 break;
1202 case SPEEX_SET_HIGHPASS:
1203 st->highpass_enabled = (*(spx_int32_t*)ptr);
1204 break;
1205 case SPEEX_GET_HIGHPASS:
1206 (*(spx_int32_t*)ptr) = st->highpass_enabled;
1207 break;
1208 /* FIXME: Convert to fixed-point and re-enable even when float API is disabled */
1209 #ifndef DISABLE_FLOAT_API
1210 case SPEEX_GET_ACTIVITY:
1211 {
1212 float ret;
1213 ret = log(st->level/st->min_level)/log(st->max_level/st->min_level);
1214 if (ret>1)
1215 ret = 1;
1216 /* Done in a strange way to catch NaNs as well */
1217 if (!(ret > 0))
1218 ret = 0;
1219 /*printf ("%f %f %f %f\n", st->level, st->min_level, st->max_level, ret);*/
1220 (*(spx_int32_t*)ptr) = (int)(100*ret);
1221 }
1222 break;
1223 #endif
1224 case SPEEX_GET_PI_GAIN:
1225 {
1226 int i;
1227 spx_word32_t *g = (spx_word32_t*)ptr;
1228 for (i=0;i<NB_NB_SUBFRAMES;i++)
1229 g[i]=st->pi_gain[i];
1230 }
1231 break;
1232 case SPEEX_GET_EXC:
1233 {
1234 int i;
1235 for (i=0;i<NB_NB_SUBFRAMES;i++)
1236 ((spx_word16_t*)ptr)[i] = compute_rms16(st->exc+i*NB_SUBFRAME_SIZE, NB_SUBFRAME_SIZE);
1237 }
1238 break;
1239 case SPEEX_GET_DTX_STATUS:
1240 *((spx_int32_t*)ptr) = st->dtx_enabled;
1241 break;
1242 case SPEEX_SET_INNOVATION_SAVE:
1243 st->innov_save = (spx_word16_t*)ptr;
1244 break;
1245 case SPEEX_SET_WIDEBAND:
1246 st->isWideband = *((spx_int32_t*)ptr);
1247 break;
1248 case SPEEX_GET_STACK:
1249 *((char**)ptr) = st->stack;
1250 break;
1251 default:
1252 speex_warning_int("Unknown nb_ctl request: ", request);
1253 return -1;
1254 }
1255 return 0;
1256 }
1257
1258
1259 #define median3(a, b, c) ((a) < (b) ? ((b) < (c) ? (b) : ((a) < (c) ? (c) : (a))) : ((c) < (b) ? (b) : ((c) < (a) ? (c) : (a))))
1260
1261 #ifdef FIXED_POINT
1262 const spx_word16_t attenuation[10] = {32767, 31483, 27923, 22861, 17278, 12055, 7764, 4616, 2533, 1283};
1263 #else
1264 const spx_word16_t attenuation[10] = {1., 0.961, 0.852, 0.698, 0.527, 0.368, 0.237, 0.141, 0.077, 0.039};
1265
1266 #endif
1267
nb_decode_lost(DecState * st,spx_word16_t * out,char * stack)1268 static void nb_decode_lost(DecState *st, spx_word16_t *out, char *stack)
1269 {
1270 int i;
1271 int pitch_val;
1272 spx_word16_t pitch_gain;
1273 spx_word16_t fact;
1274 spx_word16_t gain_med;
1275 spx_word16_t innov_gain;
1276 spx_word16_t noise_gain;
1277
1278 st->exc = st->excBuf + 2*NB_PITCH_END + NB_SUBFRAME_SIZE + 6;
1279
1280 if (st->count_lost<10)
1281 fact = attenuation[st->count_lost];
1282 else
1283 fact = 0;
1284
1285 gain_med = median3(st->pitch_gain_buf[0], st->pitch_gain_buf[1], st->pitch_gain_buf[2]);
1286 if (gain_med < st->last_pitch_gain)
1287 st->last_pitch_gain = gain_med;
1288
1289 #ifdef FIXED_POINT
1290 pitch_gain = st->last_pitch_gain;
1291 if (pitch_gain>54)
1292 pitch_gain = 54;
1293 pitch_gain = SHL16(pitch_gain, 9);
1294 #else
1295 pitch_gain = GAIN_SCALING_1*st->last_pitch_gain;
1296 if (pitch_gain>.85)
1297 pitch_gain=.85;
1298 #endif
1299 pitch_gain = MULT16_16_Q15(fact,pitch_gain) + VERY_SMALL;
1300 /* FIXME: This was rms of innovation (not exc) */
1301 innov_gain = compute_rms16(st->exc, NB_FRAME_SIZE);
1302 noise_gain = MULT16_16_Q15(innov_gain, MULT16_16_Q15(fact, SUB16(Q15ONE,MULT16_16_Q15(pitch_gain,pitch_gain))));
1303 /* Shift all buffers by one frame */
1304 SPEEX_MOVE(st->excBuf, st->excBuf+NB_FRAME_SIZE, 2*NB_PITCH_END + NB_SUBFRAME_SIZE + 12);
1305
1306
1307 pitch_val = st->last_pitch + SHR32((spx_int32_t)speex_rand(1+st->count_lost, &st->seed),SIG_SHIFT);
1308 if (pitch_val > NB_PITCH_END)
1309 pitch_val = NB_PITCH_END;
1310 if (pitch_val < NB_PITCH_START)
1311 pitch_val = NB_PITCH_START;
1312 for (i=0;i<NB_FRAME_SIZE;i++)
1313 {
1314 st->exc[i]= MULT16_16_Q15(pitch_gain, (st->exc[i-pitch_val]+VERY_SMALL)) +
1315 speex_rand(noise_gain, &st->seed);
1316 }
1317
1318 bw_lpc(QCONST16(.98,15), st->interp_qlpc, st->interp_qlpc, NB_ORDER);
1319 iir_mem16(&st->exc[-NB_SUBFRAME_SIZE], st->interp_qlpc, out, NB_FRAME_SIZE,
1320 NB_ORDER, st->mem_sp, stack);
1321 highpass(out, out, NB_FRAME_SIZE, HIGHPASS_NARROWBAND|HIGHPASS_OUTPUT, st->mem_hp);
1322
1323 st->first = 0;
1324 st->count_lost++;
1325 st->pitch_gain_buf[st->pitch_gain_buf_idx++] = PSHR16(pitch_gain,9);
1326 if (st->pitch_gain_buf_idx > 2) /* rollover */
1327 st->pitch_gain_buf_idx = 0;
1328 }
1329
1330 /* Just so we don't need to carry the complete wideband mode information */
1331 static const int wb_skip_table[8] = {0, 36, 112, 192, 352, 0, 0, 0};
1332
nb_decode(void * state,SpeexBits * bits,void * vout)1333 int nb_decode(void *state, SpeexBits *bits, void *vout)
1334 {
1335 DecState *st;
1336 int i, sub;
1337 int pitch;
1338 spx_word16_t pitch_gain[3];
1339 spx_word32_t ol_gain=0;
1340 int ol_pitch=0;
1341 spx_word16_t ol_pitch_coef=0;
1342 int best_pitch=40;
1343 spx_word16_t best_pitch_gain=0;
1344 int wideband;
1345 int m;
1346 char *stack;
1347 VARDECL(spx_sig_t *innov);
1348 VARDECL(spx_word32_t *exc32);
1349 VARDECL(spx_coef_t *ak);
1350 VARDECL(spx_lsp_t *qlsp);
1351 spx_word16_t pitch_average=0;
1352
1353 spx_word16_t *out = (spx_word16_t*)vout;
1354 VARDECL(spx_lsp_t *interp_qlsp);
1355
1356 st=(DecState*)state;
1357 stack=st->stack;
1358
1359 st->exc = st->excBuf + 2*NB_PITCH_END + NB_SUBFRAME_SIZE + 6;
1360
1361 /* Check if we're in DTX mode*/
1362 if (!bits && st->dtx_enabled)
1363 {
1364 st->submodeID=0;
1365 } else
1366 {
1367 /* If bits is NULL, consider the packet to be lost (what could we do anyway) */
1368 if (!bits)
1369 {
1370 nb_decode_lost(st, out, stack);
1371 return 0;
1372 }
1373
1374 if (st->encode_submode)
1375 {
1376
1377 /* Search for next narrowband block (handle requests, skip wideband blocks) */
1378 do {
1379 if (speex_bits_remaining(bits)<5)
1380 return -1;
1381 wideband = speex_bits_unpack_unsigned(bits, 1);
1382 if (wideband) /* Skip wideband block (for compatibility) */
1383 {
1384 int submode;
1385 int advance;
1386 advance = submode = speex_bits_unpack_unsigned(bits, SB_SUBMODE_BITS);
1387 /*speex_mode_query(&speex_wb_mode, SPEEX_SUBMODE_BITS_PER_FRAME, &advance);*/
1388 advance = wb_skip_table[submode];
1389 if (advance < 0)
1390 {
1391 speex_notify("Invalid mode encountered. The stream is corrupted.");
1392 return -2;
1393 }
1394 advance -= (SB_SUBMODE_BITS+1);
1395 speex_bits_advance(bits, advance);
1396
1397 if (speex_bits_remaining(bits)<5)
1398 return -1;
1399 wideband = speex_bits_unpack_unsigned(bits, 1);
1400 if (wideband)
1401 {
1402 advance = submode = speex_bits_unpack_unsigned(bits, SB_SUBMODE_BITS);
1403 /*speex_mode_query(&speex_wb_mode, SPEEX_SUBMODE_BITS_PER_FRAME, &advance);*/
1404 advance = wb_skip_table[submode];
1405 if (advance < 0)
1406 {
1407 speex_notify("Invalid mode encountered. The stream is corrupted.");
1408 return -2;
1409 }
1410 advance -= (SB_SUBMODE_BITS+1);
1411 speex_bits_advance(bits, advance);
1412 wideband = speex_bits_unpack_unsigned(bits, 1);
1413 if (wideband)
1414 {
1415 speex_notify("More than two wideband layers found. The stream is corrupted.");
1416 return -2;
1417 }
1418
1419 }
1420 }
1421 if (speex_bits_remaining(bits)<4)
1422 return -1;
1423 /* FIXME: Check for overflow */
1424 m = speex_bits_unpack_unsigned(bits, 4);
1425 if (m==15) /* We found a terminator */
1426 {
1427 return -1;
1428 } else if (m==14) /* Speex in-band request */
1429 {
1430 int ret = speex_inband_handler(bits, st->speex_callbacks, state);
1431 if (ret)
1432 return ret;
1433 } else if (m==13) /* User in-band request */
1434 {
1435 int ret = st->user_callback.func(bits, state, st->user_callback.data);
1436 if (ret)
1437 return ret;
1438 } else if (m>8) /* Invalid mode */
1439 {
1440 speex_notify("Invalid mode encountered. The stream is corrupted.");
1441 return -2;
1442 }
1443
1444 } while (m>8);
1445
1446 /* Get the sub-mode that was used */
1447 st->submodeID = m;
1448 }
1449
1450 }
1451
1452 /* Shift all buffers by one frame */
1453 SPEEX_MOVE(st->excBuf, st->excBuf+NB_FRAME_SIZE, 2*NB_PITCH_END + NB_SUBFRAME_SIZE + 12);
1454
1455 /* If null mode (no transmission), just set a couple things to zero*/
1456 if (st->submodes[st->submodeID] == NULL)
1457 {
1458 VARDECL(spx_coef_t *lpc);
1459 ALLOC(lpc, NB_ORDER, spx_coef_t);
1460 bw_lpc(QCONST16(0.93f,15), st->interp_qlpc, lpc, NB_ORDER);
1461 {
1462 spx_word16_t innov_gain=0;
1463 /* FIXME: This was innov, not exc */
1464 innov_gain = compute_rms16(st->exc, NB_FRAME_SIZE);
1465 for (i=0;i<NB_FRAME_SIZE;i++)
1466 st->exc[i]=speex_rand(innov_gain, &st->seed);
1467 }
1468
1469
1470 st->first=1;
1471
1472 /* Final signal synthesis from excitation */
1473 iir_mem16(st->exc, lpc, out, NB_FRAME_SIZE, NB_ORDER, st->mem_sp, stack);
1474
1475 st->count_lost=0;
1476 return 0;
1477 }
1478
1479 ALLOC(qlsp, NB_ORDER, spx_lsp_t);
1480
1481 /* Unquantize LSPs */
1482 SUBMODE(lsp_unquant)(qlsp, NB_ORDER, bits);
1483
1484 /*Damp memory if a frame was lost and the LSP changed too much*/
1485 if (st->count_lost)
1486 {
1487 spx_word16_t fact;
1488 spx_word32_t lsp_dist=0;
1489 for (i=0;i<NB_ORDER;i++)
1490 lsp_dist = ADD32(lsp_dist, EXTEND32(ABS(st->old_qlsp[i] - qlsp[i])));
1491 #ifdef FIXED_POINT
1492 fact = SHR16(19661,SHR32(lsp_dist,LSP_SHIFT+2));
1493 #else
1494 fact = .6*exp(-.2*lsp_dist);
1495 #endif
1496 for (i=0;i<NB_ORDER;i++)
1497 st->mem_sp[i] = MULT16_32_Q15(fact,st->mem_sp[i]);
1498 }
1499
1500
1501 /* Handle first frame and lost-packet case */
1502 if (st->first || st->count_lost)
1503 {
1504 for (i=0;i<NB_ORDER;i++)
1505 st->old_qlsp[i] = qlsp[i];
1506 }
1507
1508 /* Get open-loop pitch estimation for low bit-rate pitch coding */
1509 if (SUBMODE(lbr_pitch)!=-1)
1510 {
1511 ol_pitch = NB_PITCH_START+speex_bits_unpack_unsigned(bits, 7);
1512 }
1513
1514 if (SUBMODE(forced_pitch_gain))
1515 {
1516 int quant;
1517 quant = speex_bits_unpack_unsigned(bits, 4);
1518 ol_pitch_coef=MULT16_16_P15(QCONST16(0.066667,15),SHL16(quant,GAIN_SHIFT));
1519 }
1520
1521 /* Get global excitation gain */
1522 {
1523 int qe;
1524 qe = speex_bits_unpack_unsigned(bits, 5);
1525 #ifdef FIXED_POINT
1526 /* FIXME: Perhaps we could slightly lower the gain here when the output is going to saturate? */
1527 ol_gain = MULT16_32_Q15(28406,ol_gain_table[qe]);
1528 #else
1529 ol_gain = SIG_SCALING*exp(qe/3.5);
1530 #endif
1531 }
1532
1533 ALLOC(ak, NB_ORDER, spx_coef_t);
1534 ALLOC(innov, NB_SUBFRAME_SIZE, spx_sig_t);
1535 ALLOC(exc32, NB_SUBFRAME_SIZE, spx_word32_t);
1536
1537 if (st->submodeID==1)
1538 {
1539 int extra;
1540 extra = speex_bits_unpack_unsigned(bits, 4);
1541
1542 if (extra==15)
1543 st->dtx_enabled=1;
1544 else
1545 st->dtx_enabled=0;
1546 }
1547 if (st->submodeID>1)
1548 st->dtx_enabled=0;
1549
1550 /*Loop on subframes */
1551 for (sub=0;sub<NB_NB_SUBFRAMES;sub++)
1552 {
1553 int offset;
1554 spx_word16_t *exc;
1555 spx_word16_t *innov_save = NULL;
1556 spx_word16_t tmp;
1557
1558 /* Offset relative to start of frame */
1559 offset = NB_SUBFRAME_SIZE*sub;
1560 /* Excitation */
1561 exc=st->exc+offset;
1562 /* Original signal */
1563 if (st->innov_save)
1564 innov_save = st->innov_save+offset;
1565
1566
1567 /* Reset excitation */
1568 SPEEX_MEMSET(exc, 0, NB_SUBFRAME_SIZE);
1569
1570 /*Adaptive codebook contribution*/
1571 speex_assert (SUBMODE(ltp_unquant));
1572 {
1573 int pit_min, pit_max;
1574 /* Handle pitch constraints if any */
1575 if (SUBMODE(lbr_pitch) != -1)
1576 {
1577 int margin;
1578 margin = SUBMODE(lbr_pitch);
1579 if (margin)
1580 {
1581 /* GT - need optimization?
1582 if (ol_pitch < NB_PITCH_START+margin-1)
1583 ol_pitch=NB_PITCH_START+margin-1;
1584 if (ol_pitch > NB_PITCH_END-margin)
1585 ol_pitch=NB_PITCH_END-margin;
1586 pit_min = ol_pitch-margin+1;
1587 pit_max = ol_pitch+margin;
1588 */
1589 pit_min = ol_pitch-margin+1;
1590 if (pit_min < NB_PITCH_START)
1591 pit_min = NB_PITCH_START;
1592 pit_max = ol_pitch+margin;
1593 if (pit_max > NB_PITCH_END)
1594 pit_max = NB_PITCH_END;
1595 } else {
1596 pit_min = pit_max = ol_pitch;
1597 }
1598 } else {
1599 pit_min = NB_PITCH_START;
1600 pit_max = NB_PITCH_END;
1601 }
1602
1603
1604
1605 SUBMODE(ltp_unquant)(exc, exc32, pit_min, pit_max, ol_pitch_coef, SUBMODE(ltp_params),
1606 NB_SUBFRAME_SIZE, &pitch, &pitch_gain[0], bits, stack,
1607 st->count_lost, offset, st->last_pitch_gain, 0);
1608
1609 /* Ensuring that things aren't blowing up as would happen if e.g. an encoder is
1610 crafting packets to make us produce NaNs and slow down the decoder (vague DoS threat).
1611 We can probably be even more aggressive and limit to 15000 or so. */
1612 sanitize_values32(exc32, NEG32(QCONST32(32000,SIG_SHIFT-1)), QCONST32(32000,SIG_SHIFT-1), NB_SUBFRAME_SIZE);
1613
1614 tmp = gain_3tap_to_1tap(pitch_gain);
1615
1616 pitch_average += tmp;
1617 if ((tmp>best_pitch_gain&&ABS(2*best_pitch-pitch)>=3&&ABS(3*best_pitch-pitch)>=4&&ABS(4*best_pitch-pitch)>=5)
1618 || (tmp>MULT16_16_Q15(QCONST16(.6,15),best_pitch_gain)&&(ABS(best_pitch-2*pitch)<3||ABS(best_pitch-3*pitch)<4||ABS(best_pitch-4*pitch)<5))
1619 || (MULT16_16_Q15(QCONST16(.67,15),tmp)>best_pitch_gain&&(ABS(2*best_pitch-pitch)<3||ABS(3*best_pitch-pitch)<4||ABS(4*best_pitch-pitch)<5)) )
1620 {
1621 best_pitch = pitch;
1622 if (tmp > best_pitch_gain)
1623 best_pitch_gain = tmp;
1624 }
1625 }
1626
1627 /* Unquantize the innovation */
1628 {
1629 int q_energy;
1630 spx_word32_t ener;
1631
1632 SPEEX_MEMSET(innov, 0, NB_SUBFRAME_SIZE);
1633
1634 /* Decode sub-frame gain correction */
1635 if (SUBMODE(have_subframe_gain)==3)
1636 {
1637 q_energy = speex_bits_unpack_unsigned(bits, 3);
1638 ener = MULT16_32_Q14(exc_gain_quant_scal3[q_energy],ol_gain);
1639 } else if (SUBMODE(have_subframe_gain)==1)
1640 {
1641 q_energy = speex_bits_unpack_unsigned(bits, 1);
1642 ener = MULT16_32_Q14(exc_gain_quant_scal1[q_energy],ol_gain);
1643 } else {
1644 ener = ol_gain;
1645 }
1646
1647 speex_assert (SUBMODE(innovation_unquant));
1648 {
1649 /*Fixed codebook contribution*/
1650 SUBMODE(innovation_unquant)(innov, SUBMODE(innovation_params), NB_SUBFRAME_SIZE, bits, stack, &st->seed);
1651 /* De-normalize innovation and update excitation */
1652
1653 signal_mul(innov, innov, ener, NB_SUBFRAME_SIZE);
1654
1655 /* Decode second codebook (only for some modes) */
1656 if (SUBMODE(double_codebook))
1657 {
1658 char *tmp_stack=stack;
1659 VARDECL(spx_sig_t *innov2);
1660 ALLOC(innov2, NB_SUBFRAME_SIZE, spx_sig_t);
1661 SPEEX_MEMSET(innov2, 0, NB_SUBFRAME_SIZE);
1662 SUBMODE(innovation_unquant)(innov2, SUBMODE(innovation_params), NB_SUBFRAME_SIZE, bits, stack, &st->seed);
1663 signal_mul(innov2, innov2, MULT16_32_Q15(QCONST16(0.454545f,15),ener), NB_SUBFRAME_SIZE);
1664 for (i=0;i<NB_SUBFRAME_SIZE;i++)
1665 innov[i] = ADD32(innov[i], innov2[i]);
1666 stack = tmp_stack;
1667 }
1668 for (i=0;i<NB_SUBFRAME_SIZE;i++)
1669 exc[i]=EXTRACT16(SATURATE32(PSHR32(ADD32(SHL32(exc32[i],1),innov[i]),SIG_SHIFT),32767));
1670 /*print_vec(exc, 40, "innov");*/
1671 if (innov_save)
1672 {
1673 for (i=0;i<NB_SUBFRAME_SIZE;i++)
1674 innov_save[i] = EXTRACT16(PSHR32(innov[i], SIG_SHIFT));
1675 }
1676 }
1677
1678 /*Vocoder mode*/
1679 if (st->submodeID==1)
1680 {
1681 spx_word16_t g=ol_pitch_coef;
1682 g=MULT16_16_P14(QCONST16(1.5f,14),(g-QCONST16(.2f,6)));
1683 if (g<0)
1684 g=0;
1685 if (g>GAIN_SCALING)
1686 g=GAIN_SCALING;
1687
1688 SPEEX_MEMSET(exc, 0, NB_SUBFRAME_SIZE);
1689 while (st->voc_offset<NB_SUBFRAME_SIZE)
1690 {
1691 /* exc[st->voc_offset]= g*sqrt(2*ol_pitch)*ol_gain;
1692 Not quite sure why we need the factor of two in the sqrt */
1693 if (st->voc_offset>=0)
1694 exc[st->voc_offset]=MULT16_16(spx_sqrt(MULT16_16_16(2,ol_pitch)),EXTRACT16(PSHR32(MULT16_16(g,PSHR32(ol_gain,SIG_SHIFT)),6)));
1695 st->voc_offset+=ol_pitch;
1696 }
1697 st->voc_offset -= NB_SUBFRAME_SIZE;
1698
1699 for (i=0;i<NB_SUBFRAME_SIZE;i++)
1700 {
1701 spx_word16_t exci=exc[i];
1702 exc[i]= ADD16(ADD16(MULT16_16_Q15(QCONST16(.7f,15),exc[i]) , MULT16_16_Q15(QCONST16(.3f,15),st->voc_m1)),
1703 SUB16(MULT16_16_Q15(Q15_ONE-MULT16_16_16(QCONST16(.85f,9),g),EXTRACT16(PSHR32(innov[i],SIG_SHIFT))),
1704 MULT16_16_Q15(MULT16_16_16(QCONST16(.15f,9),g),EXTRACT16(PSHR32(st->voc_m2,SIG_SHIFT)))
1705 ));
1706 st->voc_m1 = exci;
1707 st->voc_m2=innov[i];
1708 st->voc_mean = EXTRACT16(PSHR32(ADD32(MULT16_16(QCONST16(.8f,15),st->voc_mean), MULT16_16(QCONST16(.2f,15),exc[i])), 15));
1709 exc[i]-=st->voc_mean;
1710 }
1711 }
1712
1713 }
1714 }
1715
1716 ALLOC(interp_qlsp, NB_ORDER, spx_lsp_t);
1717
1718 if (st->lpc_enh_enabled && SUBMODE(comb_gain)>0 && !st->count_lost)
1719 {
1720 multicomb(st->exc-NB_SUBFRAME_SIZE, out, st->interp_qlpc, NB_ORDER, 2*NB_SUBFRAME_SIZE, best_pitch, 40, SUBMODE(comb_gain), stack);
1721 multicomb(st->exc+NB_SUBFRAME_SIZE, out+2*NB_SUBFRAME_SIZE, st->interp_qlpc, NB_ORDER, 2*NB_SUBFRAME_SIZE, best_pitch, 40, SUBMODE(comb_gain), stack);
1722 } else {
1723 SPEEX_COPY(out, &st->exc[-NB_SUBFRAME_SIZE], NB_FRAME_SIZE);
1724 }
1725
1726 /* If the last packet was lost, re-scale the excitation to obtain the same energy as encoded in ol_gain */
1727 if (st->count_lost)
1728 {
1729 spx_word16_t exc_ener;
1730 spx_word32_t gain32;
1731 spx_word16_t gain;
1732 exc_ener = compute_rms16 (st->exc, NB_FRAME_SIZE);
1733 gain32 = PDIV32(ol_gain, ADD16(exc_ener,1));
1734 #ifdef FIXED_POINT
1735 if (gain32 > 32767)
1736 gain32 = 32767;
1737 gain = EXTRACT16(gain32);
1738 #else
1739 if (gain32 > 2)
1740 gain32=2;
1741 gain = gain32;
1742 #endif
1743 for (i=0;i<NB_FRAME_SIZE;i++)
1744 {
1745 st->exc[i] = MULT16_16_Q14(gain, st->exc[i]);
1746 out[i]=st->exc[i-NB_SUBFRAME_SIZE];
1747 }
1748 }
1749
1750 /*Loop on subframes */
1751 for (sub=0;sub<NB_NB_SUBFRAMES;sub++)
1752 {
1753 int offset;
1754 spx_word16_t *sp;
1755
1756 /* Offset relative to start of frame */
1757 offset = NB_SUBFRAME_SIZE*sub;
1758 /* Original signal */
1759 sp=out+offset;
1760
1761 /* LSP interpolation (quantized and unquantized) */
1762 lsp_interpolate(st->old_qlsp, qlsp, interp_qlsp, NB_ORDER, sub, NB_NB_SUBFRAMES, LSP_MARGIN);
1763
1764 /* Compute interpolated LPCs (unquantized) */
1765 lsp_to_lpc(interp_qlsp, ak, NB_ORDER, stack);
1766
1767 /* Compute analysis filter at w=pi */
1768 {
1769 spx_word32_t pi_g=LPC_SCALING;
1770 for (i=0;i<NB_ORDER;i+=2)
1771 {
1772 /*pi_g += -st->interp_qlpc[i] + st->interp_qlpc[i+1];*/
1773 pi_g = ADD32(pi_g, SUB32(EXTEND32(ak[i+1]),EXTEND32(ak[i])));
1774 }
1775 st->pi_gain[sub] = pi_g;
1776 }
1777
1778 iir_mem16(sp, st->interp_qlpc, sp, NB_SUBFRAME_SIZE, NB_ORDER,
1779 st->mem_sp, stack);
1780
1781 for (i=0;i<NB_ORDER;i++)
1782 st->interp_qlpc[i] = ak[i];
1783
1784 }
1785
1786 if (st->highpass_enabled)
1787 highpass(out, out, NB_FRAME_SIZE, (st->isWideband?HIGHPASS_WIDEBAND:HIGHPASS_NARROWBAND)|HIGHPASS_OUTPUT, st->mem_hp);
1788 /*for (i=0;i<NB_FRAME_SIZE;i++)
1789 printf ("%d\n", (int)st->frame[i]);*/
1790
1791 /* Tracking output level */
1792 st->level = 1+PSHR32(ol_gain,SIG_SHIFT);
1793 st->max_level = MAX16(MULT16_16_Q15(QCONST16(.99f,15), st->max_level), st->level);
1794 st->min_level = MIN16(ADD16(1,MULT16_16_Q14(QCONST16(1.01f,14), st->min_level)), st->level);
1795 if (st->max_level < st->min_level+1)
1796 st->max_level = st->min_level+1;
1797 /*printf ("%f %f %f %d\n", og, st->min_level, st->max_level, update);*/
1798
1799 /* Store the LSPs for interpolation in the next frame */
1800 for (i=0;i<NB_ORDER;i++)
1801 st->old_qlsp[i] = qlsp[i];
1802
1803 /* The next frame will not be the first (Duh!) */
1804 st->first = 0;
1805 st->count_lost=0;
1806 st->last_pitch = best_pitch;
1807 #ifdef FIXED_POINT
1808 st->last_pitch_gain = PSHR16(pitch_average,2);
1809 #else
1810 st->last_pitch_gain = .25*pitch_average;
1811 #endif
1812 st->pitch_gain_buf[st->pitch_gain_buf_idx++] = st->last_pitch_gain;
1813 if (st->pitch_gain_buf_idx > 2) /* rollover */
1814 st->pitch_gain_buf_idx = 0;
1815
1816 st->last_ol_gain = ol_gain;
1817
1818 return 0;
1819 }
1820 #endif /* DISABLE_DECODER */
1821
1822