decoder excitation now in 16-bit precision (was 32), which saves quite a bit
[speexdsp.git] / libspeex / sb_celp.c
1 /* Copyright (C) 2002 Jean-Marc Valin 
2    File: sb_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 "sb_celp.h"
38 #include "stdlib.h"
39 #include "filters.h"
40 #include "lpc.h"
41 #include "lsp.h"
42 #include "stack_alloc.h"
43 #include "cb_search.h"
44 #include "quant_lsp.h"
45 #include "vq.h"
46 #include "ltp.h"
47 #include "misc.h"
48
49 /* Default size for the encoder and decoder stack (can be changed at compile time).
50    This does not apply when using variable-size arrays or alloca. */
51 #ifndef SB_ENC_STACK
52 #define SB_ENC_STACK (10000*sizeof(spx_sig_t))
53 #endif
54
55 #ifndef SB_DEC_STACK
56 #define SB_DEC_STACK (6000*sizeof(spx_sig_t))
57 #endif
58
59
60 #ifdef DISABLE_WIDEBAND
61 void *sb_encoder_init(const SpeexMode *m)
62 {
63    speex_error("Wideband and Ultra-wideband are disabled");
64    return NULL;
65 }
66 void sb_encoder_destroy(void *state)
67 {
68    speex_error("Wideband and Ultra-wideband are disabled");
69 }
70 int sb_encode(void *state, void *vin, SpeexBits *bits)
71 {
72    speex_error("Wideband and Ultra-wideband are disabled");
73    return -2;
74 }
75 void *sb_decoder_init(const SpeexMode *m)
76 {
77    speex_error("Wideband and Ultra-wideband are disabled");
78    return NULL;
79 }
80 void sb_decoder_destroy(void *state)
81 {
82    speex_error("Wideband and Ultra-wideband are disabled");
83 }
84 int sb_decode(void *state, SpeexBits *bits, void *vout)
85 {
86    speex_error("Wideband and Ultra-wideband are disabled");
87    return -2;
88 }
89 int sb_encoder_ctl(void *state, int request, void *ptr)
90 {
91    speex_error("Wideband and Ultra-wideband are disabled");
92    return -2;
93 }
94 int sb_decoder_ctl(void *state, int request, void *ptr)
95 {
96    speex_error("Wideband and Ultra-wideband are disabled");
97    return -2;
98 }
99 #else
100
101
102 #ifndef M_PI
103 #define M_PI           3.14159265358979323846  /* pi */
104 #endif
105
106 #define sqr(x) ((x)*(x))
107
108 #define SUBMODE(x) st->submodes[st->submodeID]->x
109
110 #ifdef FIXED_POINT
111 static const spx_word16_t gc_quant_bound[16] = {125, 164, 215, 282, 370, 484, 635, 832, 1090, 1428, 1871, 2452, 3213, 4210, 5516, 7228};
112 #define LSP_MARGIN 410
113 #define LSP_DELTA1 6553
114 #define LSP_DELTA2 1638
115
116 #else
117
118 #define LSP_MARGIN .05
119 #define LSP_DELTA1 .2
120 #define LSP_DELTA2 .05
121
122 #endif
123
124 #define QMF_ORDER 64
125
126 #ifdef FIXED_POINT
127 static const spx_word16_t h0[64] = {2, -7, -7, 18, 15, -39, -25, 75, 35, -130, -41, 212, 38, -327, -17, 483, -32, -689, 124, 956, -283, -1307, 543, 1780, -973, -2467, 1733, 3633, -3339, -6409, 9059, 30153, 30153, 9059, -6409, -3339, 3633, 1733, -2467, -973, 1780, 543, -1307, -283, 956, 124, -689, -32, 483, -17, -327, 38, 212, -41, -130, 35, 75, -25, -39, 15, 18, -7, -7, 2};
128
129 static const spx_word16_t h1[64] = {2, 7, -7, -18, 15, 39, -25, -75, 35, 130, -41, -212, 38, 327, -17, -483, -32, 689, 124, -956, -283, 1307, 543, -1780, -973, 2467, 1733, -3633, -3339, 6409, 9059, -30153, 30153, -9059, -6409, 3339, 3633, -1733, -2467, 973, 1780, -543, -1307, 283, 956, -124, -689, 32, 483, 17, -327, -38, 212, 41, -130, -35, 75, 25, -39, -15, 18, 7, -7, -2};
130
131
132 #else
133 static const float h0[64] = {
134    3.596189e-05, -0.0001123515,
135    -0.0001104587, 0.0002790277,
136    0.0002298438, -0.0005953563,
137    -0.0003823631, 0.00113826,
138    0.0005308539, -0.001986177,
139    -0.0006243724, 0.003235877,
140    0.0005743159, -0.004989147,
141    -0.0002584767, 0.007367171,
142    -0.0004857935, -0.01050689,
143    0.001894714, 0.01459396,
144    -0.004313674, -0.01994365,
145    0.00828756, 0.02716055,
146    -0.01485397, -0.03764973,
147    0.026447, 0.05543245,
148    -0.05095487, -0.09779096,
149    0.1382363, 0.4600981,
150    0.4600981, 0.1382363,
151    -0.09779096, -0.05095487,
152    0.05543245, 0.026447,
153    -0.03764973, -0.01485397,
154    0.02716055, 0.00828756,
155    -0.01994365, -0.004313674,
156    0.01459396, 0.001894714,
157    -0.01050689, -0.0004857935,
158    0.007367171, -0.0002584767,
159    -0.004989147, 0.0005743159,
160    0.003235877, -0.0006243724,
161    -0.001986177, 0.0005308539,
162    0.00113826, -0.0003823631,
163    -0.0005953563, 0.0002298438,
164    0.0002790277, -0.0001104587,
165    -0.0001123515, 3.596189e-05
166 };
167
168 static const float h1[64] = {
169    3.596189e-05, 0.0001123515,
170    -0.0001104587, -0.0002790277,
171    0.0002298438, 0.0005953563,
172    -0.0003823631, -0.00113826,
173    0.0005308539, 0.001986177,
174    -0.0006243724, -0.003235877,
175    0.0005743159, 0.004989147,
176    -0.0002584767, -0.007367171,
177    -0.0004857935, 0.01050689,
178    0.001894714, -0.01459396,
179    -0.004313674, 0.01994365,
180    0.00828756, -0.02716055,
181    -0.01485397, 0.03764973,
182    0.026447, -0.05543245,
183    -0.05095487, 0.09779096,
184    0.1382363, -0.4600981,
185    0.4600981, -0.1382363,
186    -0.09779096, 0.05095487,
187    0.05543245, -0.026447,
188    -0.03764973, 0.01485397,
189    0.02716055, -0.00828756,
190    -0.01994365, 0.004313674,
191    0.01459396, -0.001894714,
192    -0.01050689, 0.0004857935,
193    0.007367171, 0.0002584767,
194    -0.004989147, -0.0005743159,
195    0.003235877, 0.0006243724,
196    -0.001986177, -0.0005308539,
197    0.00113826, 0.0003823631,
198    -0.0005953563, -0.0002298438,
199    0.0002790277, 0.0001104587,
200    -0.0001123515, -3.596189e-05
201 };
202 #endif
203
204 extern const spx_word16_t lpc_window[];
205
206 static void mix_and_saturate(spx_word32_t *x0, spx_word32_t *x1, spx_word16_t *out, int len)
207 {
208    int i;
209    for (i=0;i<len;i++)
210    {
211       spx_word32_t tmp;
212 #ifdef FIXED_POINT
213       tmp=PSHR(x0[i]-x1[i],SIG_SHIFT-1);
214 #else
215       tmp=2*(x0[i]-x1[i]);
216 #endif
217       if (tmp>32767)
218          out[i] = 32767;
219       else if (tmp<-32767)
220          out[i] = -32767;
221       else
222          out[i] = tmp;
223    }
224 }
225
226 void *sb_encoder_init(const SpeexMode *m)
227 {
228    int i;
229    SBEncState *st;
230    const SpeexSBMode *mode;
231
232    st = (SBEncState*)speex_alloc(sizeof(SBEncState));
233    if (!st)
234       return NULL;
235 #if defined(VAR_ARRAYS) || defined (USE_ALLOCA)
236    st->stack = NULL;
237 #else
238    st->stack = (char*)speex_alloc_scratch(SB_ENC_STACK);
239 #endif
240    st->mode = m;
241    mode = (const SpeexSBMode*)m->mode;
242
243
244    st->st_low = speex_encoder_init(mode->nb_mode);
245    st->full_frame_size = 2*mode->frameSize;
246    st->frame_size = mode->frameSize;
247    st->subframeSize = mode->subframeSize;
248    st->nbSubframes = mode->frameSize/mode->subframeSize;
249    st->windowSize = st->frame_size+st->subframeSize;
250    st->lpcSize=mode->lpcSize;
251    st->bufSize=mode->bufSize;
252
253    st->encode_submode = 1;
254    st->submodes=mode->submodes;
255    st->submodeSelect = st->submodeID=mode->defaultSubmode;
256    
257    i=9;
258    speex_encoder_ctl(st->st_low, SPEEX_SET_QUALITY, &i);
259
260    st->lag_factor = mode->lag_factor;
261    st->lpc_floor = mode->lpc_floor;
262    st->gamma1=mode->gamma1;
263    st->gamma2=mode->gamma2;
264    st->first=1;
265
266    st->x0d=speex_alloc((st->frame_size)*sizeof(spx_sig_t));
267    st->x1d=speex_alloc((st->frame_size)*sizeof(spx_sig_t));
268    st->high=speex_alloc((st->full_frame_size)*sizeof(spx_sig_t));
269    st->y0=speex_alloc((st->full_frame_size)*sizeof(spx_sig_t));
270    st->y1=speex_alloc((st->full_frame_size)*sizeof(spx_sig_t));
271
272    st->h0_mem=speex_alloc((QMF_ORDER)*sizeof(spx_word16_t));
273    st->h1_mem=speex_alloc((QMF_ORDER)*sizeof(spx_word16_t));
274    st->g0_mem=speex_alloc((QMF_ORDER)*sizeof(spx_word32_t));
275    st->g1_mem=speex_alloc((QMF_ORDER)*sizeof(spx_word32_t));
276
277    st->excBuf=speex_alloc((st->bufSize)*sizeof(spx_sig_t));
278    st->exc = st->excBuf + st->bufSize - st->windowSize;
279
280    st->res=speex_alloc((st->frame_size)*sizeof(spx_sig_t));
281    st->sw=speex_alloc((st->frame_size)*sizeof(spx_sig_t));
282    st->target=speex_alloc((st->frame_size)*sizeof(spx_sig_t));
283    st->window= lpc_window;
284
285    st->lagWindow = speex_alloc((st->lpcSize+1)*sizeof(spx_word16_t));
286    for (i=0;i<st->lpcSize+1;i++)
287       st->lagWindow[i]=16384*exp(-.5*sqr(2*M_PI*st->lag_factor*i));
288
289    st->autocorr = speex_alloc((st->lpcSize+1)*sizeof(spx_word16_t));
290    st->lpc = speex_alloc(st->lpcSize*sizeof(spx_coef_t));
291    st->bw_lpc1 = speex_alloc(st->lpcSize*sizeof(spx_coef_t));
292    st->bw_lpc2 = speex_alloc(st->lpcSize*sizeof(spx_coef_t));
293    st->lsp = speex_alloc(st->lpcSize*sizeof(spx_lsp_t));
294    st->qlsp = speex_alloc(st->lpcSize*sizeof(spx_lsp_t));
295    st->old_lsp = speex_alloc(st->lpcSize*sizeof(spx_lsp_t));
296    st->old_qlsp = speex_alloc(st->lpcSize*sizeof(spx_lsp_t));
297    st->interp_lsp = speex_alloc(st->lpcSize*sizeof(spx_lsp_t));
298    st->interp_qlsp = speex_alloc(st->lpcSize*sizeof(spx_lsp_t));
299    st->interp_lpc = speex_alloc(st->lpcSize*sizeof(spx_coef_t));
300    st->interp_qlpc = speex_alloc(st->lpcSize*sizeof(spx_coef_t));
301    st->pi_gain = speex_alloc((st->nbSubframes)*sizeof(spx_word32_t));
302    st->low_innov = speex_alloc((st->frame_size)*sizeof(spx_word32_t));
303    speex_encoder_ctl(st->st_low, SPEEX_SET_INNOVATION_SAVE, st->low_innov);
304    st->innov_save = NULL;
305    
306    st->mem_sp = speex_alloc((st->lpcSize)*sizeof(spx_mem_t));
307    st->mem_sp2 = speex_alloc((st->lpcSize)*sizeof(spx_mem_t));
308    st->mem_sw = speex_alloc((st->lpcSize)*sizeof(spx_mem_t));
309
310    st->vbr_quality = 8;
311    st->vbr_enabled = 0;
312    st->vad_enabled = 0;
313    st->abr_enabled = 0;
314    st->relative_quality=0;
315
316    st->complexity=2;
317    speex_encoder_ctl(st->st_low, SPEEX_GET_SAMPLING_RATE, &st->sampling_rate);
318    st->sampling_rate*=2;
319 #ifdef ENABLE_VALGRIND
320    VALGRIND_MAKE_READABLE(st, (st->stack-(char*)st));
321 #endif
322    return st;
323 }
324
325 void sb_encoder_destroy(void *state)
326 {
327    SBEncState *st=(SBEncState*)state;
328
329    speex_encoder_destroy(st->st_low);
330 #if !(defined(VAR_ARRAYS) || defined (USE_ALLOCA))
331    speex_free_scratch(st->stack);
332 #endif
333
334    speex_free(st->x0d);
335    speex_free(st->x1d);
336    speex_free(st->high);
337    speex_free(st->y0);
338    speex_free(st->y1);
339
340    speex_free(st->h0_mem);
341    speex_free(st->h1_mem);
342    speex_free(st->g0_mem);
343    speex_free(st->g1_mem);
344
345    speex_free(st->excBuf);
346    speex_free(st->res);
347    speex_free(st->sw);
348    speex_free(st->target);
349    speex_free(st->lagWindow);
350
351    speex_free(st->autocorr);
352    speex_free(st->lpc);
353    speex_free(st->bw_lpc1);
354    speex_free(st->bw_lpc2);
355    speex_free(st->lsp);
356    speex_free(st->qlsp);
357    speex_free(st->old_lsp);
358    speex_free(st->old_qlsp);
359    speex_free(st->interp_lsp);
360    speex_free(st->interp_qlsp);
361    speex_free(st->interp_lpc);
362    speex_free(st->interp_qlpc);
363    speex_free(st->pi_gain);
364
365    speex_free(st->mem_sp);
366    speex_free(st->mem_sp2);
367    speex_free(st->mem_sw);
368
369    
370    speex_free(st);
371 }
372
373
374 int sb_encode(void *state, void *vin, SpeexBits *bits)
375 {
376    SBEncState *st;
377    int i, roots, sub;
378    char *stack;
379    VARDECL(spx_mem_t *mem);
380    VARDECL(spx_sig_t *innov);
381    VARDECL(spx_word16_t *syn_resp);
382    VARDECL(spx_word32_t *low_pi_gain);
383    VARDECL(spx_word16_t *low_exc);
384    const SpeexSBMode *mode;
385    int dtx;
386    spx_word16_t *in = vin;
387
388    st = (SBEncState*)state;
389    stack=st->stack;
390    mode = (const SpeexSBMode*)(st->mode->mode);
391
392    {
393       VARDECL(spx_word16_t *low);
394       ALLOC(low, st->frame_size, spx_word16_t);
395
396       /* Compute the two sub-bands by filtering with h0 and h1*/
397       qmf_decomp(in, h0, st->x0d, st->x1d, st->full_frame_size, QMF_ORDER, st->h0_mem, stack);
398       
399       for (i=0;i<st->frame_size;i++)
400          low[i] = SATURATE(PSHR(st->x0d[i],SIG_SHIFT),32767);
401       
402       /* Encode the narrowband part*/
403       speex_encode_native(st->st_low, low, bits);
404
405       for (i=0;i<st->frame_size;i++)
406          st->x0d[i] = SHL(low[i],SIG_SHIFT);
407    }
408    /* High-band buffering / sync with low band */
409    for (i=0;i<st->windowSize-st->frame_size;i++)
410       st->high[i] = st->high[st->frame_size+i];
411    for (i=0;i<st->frame_size;i++)
412       st->high[st->windowSize-st->frame_size+i]=SATURATE(st->x1d[i],536854528);
413
414    speex_move(st->excBuf, st->excBuf+st->frame_size, (st->bufSize-st->frame_size)*sizeof(spx_sig_t));
415
416
417    ALLOC(low_pi_gain, st->nbSubframes, spx_word32_t);
418    ALLOC(low_exc, st->frame_size, spx_word16_t);
419    speex_encoder_ctl(st->st_low, SPEEX_GET_PI_GAIN, low_pi_gain);
420    speex_encoder_ctl(st->st_low, SPEEX_GET_EXC, low_exc);
421    
422    speex_encoder_ctl(st->st_low, SPEEX_GET_LOW_MODE, &dtx);
423
424    if (dtx==0)
425       dtx=1;
426    else
427       dtx=0;
428
429    {
430       VARDECL(spx_word16_t *w_sig);
431       ALLOC(w_sig, st->windowSize, spx_word16_t);
432       /* Window for analysis */
433       for (i=0;i<st->windowSize;i++)
434          w_sig[i] = SHR(MULT16_16(SHR((spx_word32_t)(st->high[i]),SIG_SHIFT),st->window[i]),SIG_SHIFT);
435
436       /* Compute auto-correlation */
437       _spx_autocorr(w_sig, st->autocorr, st->lpcSize+1, st->windowSize);
438    }
439    st->autocorr[0] = ADD16(st->autocorr[0],MULT16_16_Q15(st->autocorr[0],st->lpc_floor)); /* Noise floor in auto-correlation domain */
440
441    /* Lag windowing: equivalent to filtering in the power-spectrum domain */
442    for (i=0;i<st->lpcSize+1;i++)
443       st->autocorr[i] = MULT16_16_Q14(st->autocorr[i],st->lagWindow[i]);
444
445    /* Levinson-Durbin */
446    _spx_lpc(st->lpc, st->autocorr, st->lpcSize);
447
448    /* LPC to LSPs (x-domain) transform */
449    roots=lpc_to_lsp (st->lpc, st->lpcSize, st->lsp, 15, LSP_DELTA1, stack);
450    if (roots!=st->lpcSize)
451    {
452       roots = lpc_to_lsp (st->lpc, st->lpcSize, st->lsp, 11, LSP_DELTA2, stack);
453       if (roots!=st->lpcSize) {
454          /*If we can't find all LSP's, do some damage control and use a flat filter*/
455          for (i=0;i<st->lpcSize;i++)
456          {
457             st->lsp[i]=LSP_SCALING*M_PI*((float)(i+1))/(st->lpcSize+1);
458          }
459       }
460    }
461
462    /* VBR code */
463    if ((st->vbr_enabled || st->vad_enabled) && !dtx)
464    {
465       float e_low=0, e_high=0;
466       float ratio;
467       if (st->abr_enabled)
468       {
469          float qual_change=0;
470          if (st->abr_drift2 * st->abr_drift > 0)
471          {
472             /* Only adapt if long-term and short-term drift are the same sign */
473             qual_change = -.00001*st->abr_drift/(1+st->abr_count);
474             if (qual_change>.1)
475                qual_change=.1;
476             if (qual_change<-.1)
477                qual_change=-.1;
478          }
479          st->vbr_quality += qual_change;
480          if (st->vbr_quality>10)
481             st->vbr_quality=10;
482          if (st->vbr_quality<0)
483             st->vbr_quality=0;
484       }
485
486
487       /*FIXME: Are the two signals (low, high) in sync? */
488       e_low = compute_rms(st->x0d, st->frame_size);
489       e_high = compute_rms(st->high, st->frame_size);
490       ratio = 2*log((1+e_high)/(1+e_low));
491       
492       speex_encoder_ctl(st->st_low, SPEEX_GET_RELATIVE_QUALITY, &st->relative_quality);
493       if (ratio<-4)
494          ratio=-4;
495       if (ratio>2)
496          ratio=2;
497       /*if (ratio>-2)*/
498       if (st->vbr_enabled) 
499       {
500          int modeid;
501          modeid = mode->nb_modes-1;
502          st->relative_quality+=1.0*(ratio+2);
503          if (st->relative_quality<-1)
504             st->relative_quality=-1;
505          while (modeid)
506          {
507             int v1;
508             float thresh;
509             v1=(int)floor(st->vbr_quality);
510             if (v1==10)
511                thresh = mode->vbr_thresh[modeid][v1];
512             else
513                thresh = (st->vbr_quality-v1)   * mode->vbr_thresh[modeid][v1+1] + 
514                         (1+v1-st->vbr_quality) * mode->vbr_thresh[modeid][v1];
515             if (st->relative_quality >= thresh)
516                break;
517             modeid--;
518          }
519          speex_encoder_ctl(state, SPEEX_SET_HIGH_MODE, &modeid);
520          if (st->abr_enabled)
521          {
522             int bitrate;
523             speex_encoder_ctl(state, SPEEX_GET_BITRATE, &bitrate);
524             st->abr_drift+=(bitrate-st->abr_enabled);
525             st->abr_drift2 = .95*st->abr_drift2 + .05*(bitrate-st->abr_enabled);
526             st->abr_count += 1.0;
527          }
528
529       } else {
530          /* VAD only */
531          int modeid;
532          if (st->relative_quality<2.0)
533             modeid=1;
534          else
535             modeid=st->submodeSelect;
536          /*speex_encoder_ctl(state, SPEEX_SET_MODE, &mode);*/
537          st->submodeID=modeid;
538
539       }
540       /*fprintf (stderr, "%f %f\n", ratio, low_qual);*/
541    }
542
543    if (st->encode_submode)
544    {
545       speex_bits_pack(bits, 1, 1);
546       if (dtx)
547          speex_bits_pack(bits, 0, SB_SUBMODE_BITS);
548       else
549          speex_bits_pack(bits, st->submodeID, SB_SUBMODE_BITS);
550    }
551
552    /* If null mode (no transmission), just set a couple things to zero*/
553    if (dtx || st->submodes[st->submodeID] == NULL)
554    {
555       for (i=0;i<st->frame_size;i++)
556          st->exc[i]=st->sw[i]=VERY_SMALL;
557
558       for (i=0;i<st->lpcSize;i++)
559          st->mem_sw[i]=0;
560       st->first=1;
561
562       /* Final signal synthesis from excitation */
563       iir_mem2(st->exc, st->interp_qlpc, st->high, st->frame_size, st->lpcSize, st->mem_sp);
564
565 #ifdef RESYNTH
566       /* Reconstruct the original */
567       fir_mem_up(st->x0d, h0, st->y0, st->full_frame_size, QMF_ORDER, st->g0_mem, stack);
568       fir_mem_up(st->high, h1, st->y1, st->full_frame_size, QMF_ORDER, st->g1_mem, stack);
569
570       for (i=0;i<st->full_frame_size;i++)
571          in[i]=SHR(st->y0[i]-st->y1[i], SIG_SHIFT-1);
572 #endif
573
574       if (dtx)
575          return 0;
576       else
577          return 1;
578    }
579
580
581    /* LSP quantization */
582    SUBMODE(lsp_quant)(st->lsp, st->qlsp, st->lpcSize, bits);   
583
584    if (st->first)
585    {
586       for (i=0;i<st->lpcSize;i++)
587          st->old_lsp[i] = st->lsp[i];
588       for (i=0;i<st->lpcSize;i++)
589          st->old_qlsp[i] = st->qlsp[i];
590    }
591    
592    ALLOC(mem, st->lpcSize, spx_mem_t);
593    ALLOC(syn_resp, st->subframeSize, spx_word16_t);
594    ALLOC(innov, st->subframeSize, spx_sig_t);
595
596    for (sub=0;sub<st->nbSubframes;sub++)
597    {
598       spx_sig_t *exc, *sp, *res, *target, *sw, *innov_save=NULL;
599       spx_word16_t filter_ratio;
600       int offset;
601       spx_word32_t rl, rh;
602       spx_word16_t eh=0;
603
604       offset = st->subframeSize*sub;
605       sp=st->high+offset;
606       exc=st->exc+offset;
607       res=st->res+offset;
608       target=st->target+offset;
609       sw=st->sw+offset;
610       /* Pointer for saving innovation */
611       if (st->innov_save)
612       {
613          innov_save = st->innov_save+2*offset;
614          for (i=0;i<2*st->subframeSize;i++)
615             innov_save[i]=0;
616       }
617       
618       /* LSP interpolation (quantized and unquantized) */
619       lsp_interpolate(st->old_lsp, st->lsp, st->interp_lsp, st->lpcSize, sub, st->nbSubframes);
620       lsp_interpolate(st->old_qlsp, st->qlsp, st->interp_qlsp, st->lpcSize, sub, st->nbSubframes);
621
622       lsp_enforce_margin(st->interp_lsp, st->lpcSize, LSP_MARGIN);
623       lsp_enforce_margin(st->interp_qlsp, st->lpcSize, LSP_MARGIN);
624
625       lsp_to_lpc(st->interp_lsp, st->interp_lpc, st->lpcSize,stack);
626       lsp_to_lpc(st->interp_qlsp, st->interp_qlpc, st->lpcSize, stack);
627
628       bw_lpc(st->gamma1, st->interp_lpc, st->bw_lpc1, st->lpcSize);
629       bw_lpc(st->gamma2, st->interp_lpc, st->bw_lpc2, st->lpcSize);
630
631       /* Compute mid-band (4000 Hz for wideband) response of low-band and high-band
632          filters */
633       st->pi_gain[sub]=LPC_SCALING;
634       rh = LPC_SCALING;
635       for (i=0;i<st->lpcSize;i+=2)
636       {
637          rh += st->interp_qlpc[i+1] - st->interp_qlpc[i];
638          st->pi_gain[sub] += st->interp_qlpc[i] + st->interp_qlpc[i+1];
639       }
640       
641       rl = low_pi_gain[sub];
642 #ifdef FIXED_POINT
643       filter_ratio=PDIV32_16(SHL(rl+82,2),SHR(82+rh,5));
644 #else
645       filter_ratio=(rl+.01)/(rh+.01);
646 #endif
647       
648       /* Compute "real excitation" */
649       fir_mem2(sp, st->interp_qlpc, exc, st->subframeSize, st->lpcSize, st->mem_sp2);
650       /* Compute energy of low-band and high-band excitation */
651
652       eh = compute_rms(exc, st->subframeSize);
653
654       if (!SUBMODE(innovation_quant)) {/* 1 for spectral folding excitation, 0 for stochastic */
655          float g;
656          spx_word16_t el;
657          el = compute_rms(st->low_innov+offset, st->subframeSize);
658
659          /* Gain to use if we want to use the low-band excitation for high-band */
660          g=eh/(1.+el);
661          
662 #if 0
663          {
664             char *tmp_stack=stack;
665             float *tmp_sig;
666             float g2;
667             ALLOC(tmp_sig, st->subframeSize, spx_sig_t);
668             for (i=0;i<st->lpcSize;i++)
669                mem[i]=st->mem_sp[i];
670             iir_mem2(st->low_innov+offset, st->interp_qlpc, tmp_sig, st->subframeSize, st->lpcSize, mem);
671             g2 = compute_rms(sp, st->subframeSize)/(.01+compute_rms(tmp_sig, st->subframeSize));
672             /*fprintf (stderr, "gains: %f %f\n", g, g2);*/
673             g = g2;
674             stack = tmp_stack;
675          }
676 #endif
677
678 #ifdef FIXED_POINT
679          g *= filter_ratio/128.;
680 #else
681          g *= filter_ratio;
682 #endif
683          /*print_vec(&g, 1, "gain factor");*/
684          /* Gain quantization */
685          {
686             int quant = (int) floor(.5 + 10 + 8.0 * log((g+.0001)));
687             /*speex_warning_int("tata", quant);*/
688             if (quant<0)
689                quant=0;
690             if (quant>31)
691                quant=31;
692             speex_bits_pack(bits, quant, 5);
693          }
694
695       } else {
696          spx_word16_t gc;
697          spx_word32_t scale;
698          spx_word16_t el;
699          el = compute_rms16(low_exc+offset, st->subframeSize);
700
701          gc = PDIV32_16(MULT16_16(filter_ratio,1+eh),1+el);
702
703          /* This is a kludge that cleans up a historical bug */
704          if (st->subframeSize==80)
705             gc *= 0.70711;
706          /*printf ("%f %f %f %f\n", el, eh, filter_ratio, gc);*/
707 #ifdef FIXED_POINT
708          {
709             int qgc = scal_quant(gc, gc_quant_bound, 16);
710             speex_bits_pack(bits, qgc, 4);
711             gc = MULT16_32_Q15(28626,gc_quant_bound[qgc]);
712          }
713 #else
714          {
715             int qgc = (int)floor(.5+3.7*(log(gc)+0.15556));
716             if (qgc<0)
717                qgc=0;
718             if (qgc>15)
719                qgc=15;
720             speex_bits_pack(bits, qgc, 4);
721             gc = exp((1/3.7)*qgc-0.15556);
722          }         
723 #endif
724          if (st->subframeSize==80)
725             gc *= 1.4142;
726
727          scale = SHL(MULT16_16(PDIV32_16(SHL(gc,SIG_SHIFT-4),filter_ratio),(1+el)),4);
728
729          compute_impulse_response(st->interp_qlpc, st->bw_lpc1, st->bw_lpc2, syn_resp, st->subframeSize, st->lpcSize, stack);
730
731          
732          /* Reset excitation */
733          for (i=0;i<st->subframeSize;i++)
734             exc[i]=VERY_SMALL;
735          
736          /* Compute zero response (ringing) of A(z/g1) / ( A(z/g2) * Aq(z) ) */
737          for (i=0;i<st->lpcSize;i++)
738             mem[i]=st->mem_sp[i];
739          iir_mem2(exc, st->interp_qlpc, exc, st->subframeSize, st->lpcSize, mem);
740
741          for (i=0;i<st->lpcSize;i++)
742             mem[i]=st->mem_sw[i];
743          filter_mem2(exc, st->bw_lpc1, st->bw_lpc2, res, st->subframeSize, st->lpcSize, mem);
744
745          /* Compute weighted signal */
746          for (i=0;i<st->lpcSize;i++)
747             mem[i]=st->mem_sw[i];
748          filter_mem2(sp, st->bw_lpc1, st->bw_lpc2, sw, st->subframeSize, st->lpcSize, mem);
749
750          /* Compute target signal */
751          for (i=0;i<st->subframeSize;i++)
752             target[i]=sw[i]-res[i];
753
754          for (i=0;i<st->subframeSize;i++)
755            exc[i]=0;
756
757          signal_div(target, target, scale, st->subframeSize);
758
759          /* Reset excitation */
760          for (i=0;i<st->subframeSize;i++)
761             innov[i]=0;
762
763          /*print_vec(target, st->subframeSize, "\ntarget");*/
764          SUBMODE(innovation_quant)(target, st->interp_qlpc, st->bw_lpc1, st->bw_lpc2, 
765                                    SUBMODE(innovation_params), st->lpcSize, st->subframeSize, 
766                                    innov, syn_resp, bits, stack, (st->complexity+1)>>1, SUBMODE(double_codebook));
767          /*print_vec(target, st->subframeSize, "after");*/
768
769          signal_mul(innov, innov, scale, st->subframeSize);
770
771          for (i=0;i<st->subframeSize;i++)
772             exc[i] = ADD32(exc[i], innov[i]);
773
774          if (st->innov_save)
775          {
776             for (i=0;i<st->subframeSize;i++)
777                innov_save[2*i]=innov[i];
778          }
779          
780          if (SUBMODE(double_codebook)) {
781             char *tmp_stack=stack;
782             VARDECL(spx_sig_t *innov2);
783             ALLOC(innov2, st->subframeSize, spx_sig_t);
784             for (i=0;i<st->subframeSize;i++)
785                innov2[i]=0;
786             for (i=0;i<st->subframeSize;i++)
787                target[i]*=2.5;
788             SUBMODE(innovation_quant)(target, st->interp_qlpc, st->bw_lpc1, st->bw_lpc2, 
789                                       SUBMODE(innovation_params), st->lpcSize, st->subframeSize, 
790                                       innov2, syn_resp, bits, stack, (st->complexity+1)>>1, 0);
791             for (i=0;i<st->subframeSize;i++)
792                innov2[i]*=scale*(1/2.5)/SIG_SCALING;
793             for (i=0;i<st->subframeSize;i++)
794                exc[i] = ADD32(exc[i],innov2[i]);
795             stack = tmp_stack;
796          }
797
798       }
799
800       /*Keep the previous memory*/
801       for (i=0;i<st->lpcSize;i++)
802          mem[i]=st->mem_sp[i];
803       /* Final signal synthesis from excitation */
804       iir_mem2(exc, st->interp_qlpc, sp, st->subframeSize, st->lpcSize, st->mem_sp);
805       
806       /* Compute weighted signal again, from synthesized speech (not sure it's the right thing) */
807       filter_mem2(sp, st->bw_lpc1, st->bw_lpc2, sw, st->subframeSize, st->lpcSize, st->mem_sw);
808    }
809
810
811 #ifdef RESYNTH
812    /* Reconstruct the original */
813    fir_mem_up(st->x0d, h0, st->y0, st->full_frame_size, QMF_ORDER, st->g0_mem, stack);
814    fir_mem_up(st->high, h1, st->y1, st->full_frame_size, QMF_ORDER, st->g1_mem, stack);
815
816    for (i=0;i<st->full_frame_size;i++)
817       in[i]=SHR(st->y0[i]-st->y1[i], SIG_SHIFT-1);
818 #endif
819    for (i=0;i<st->lpcSize;i++)
820       st->old_lsp[i] = st->lsp[i];
821    for (i=0;i<st->lpcSize;i++)
822       st->old_qlsp[i] = st->qlsp[i];
823
824    st->first=0;
825
826    return 1;
827 }
828
829
830
831
832
833 void *sb_decoder_init(const SpeexMode *m)
834 {
835    SBDecState *st;
836    const SpeexSBMode *mode;
837    st = (SBDecState*)speex_alloc(sizeof(SBDecState));
838    if (!st)
839       return NULL;
840 #if defined(VAR_ARRAYS) || defined (USE_ALLOCA)
841    st->stack = NULL;
842 #else
843    st->stack = (char*)speex_alloc_scratch(SB_DEC_STACK);
844 #endif
845    st->mode = m;
846    mode=(const SpeexSBMode*)m->mode;
847
848    st->encode_submode = 1;
849
850
851
852
853    st->st_low = speex_decoder_init(mode->nb_mode);
854    st->full_frame_size = 2*mode->frameSize;
855    st->frame_size = mode->frameSize;
856    st->subframeSize = mode->subframeSize;
857    st->nbSubframes = mode->frameSize/mode->subframeSize;
858    st->lpcSize=mode->lpcSize;
859    speex_decoder_ctl(st->st_low, SPEEX_GET_SAMPLING_RATE, &st->sampling_rate);
860    st->sampling_rate*=2;
861
862    st->submodes=mode->submodes;
863    st->submodeID=mode->defaultSubmode;
864
865    st->first=1;
866
867
868    st->x0d=speex_alloc((st->frame_size)*sizeof(spx_sig_t));
869    st->x1d=speex_alloc((st->frame_size)*sizeof(spx_sig_t));
870    st->high=speex_alloc((st->full_frame_size)*sizeof(spx_sig_t));
871    st->y0=speex_alloc((st->full_frame_size)*sizeof(spx_sig_t));
872    st->y1=speex_alloc((st->full_frame_size)*sizeof(spx_sig_t));
873
874    st->g0_mem=speex_alloc((QMF_ORDER)*sizeof(spx_word32_t));
875    st->g1_mem=speex_alloc((QMF_ORDER)*sizeof(spx_word32_t));
876
877    st->exc=speex_alloc((st->frame_size)*sizeof(spx_sig_t));
878    st->excBuf=speex_alloc((st->subframeSize)*sizeof(spx_sig_t));
879
880    st->qlsp = speex_alloc((st->lpcSize)*sizeof(spx_lsp_t));
881    st->old_qlsp = speex_alloc((st->lpcSize)*sizeof(spx_lsp_t));
882    st->interp_qlsp = speex_alloc(st->lpcSize*sizeof(spx_lsp_t));
883    st->interp_qlpc = speex_alloc(st->lpcSize*sizeof(spx_coef_t));
884
885    st->pi_gain = speex_alloc((st->nbSubframes)*sizeof(spx_word32_t));
886    st->mem_sp = speex_alloc((2*st->lpcSize)*sizeof(spx_mem_t));
887    
888    st->low_innov = speex_alloc((st->frame_size)*sizeof(spx_word32_t));
889    speex_decoder_ctl(st->st_low, SPEEX_SET_INNOVATION_SAVE, st->low_innov);
890    st->innov_save = NULL;
891
892
893    st->lpc_enh_enabled=0;
894    st->seed = 1000;
895
896 #ifdef ENABLE_VALGRIND
897    VALGRIND_MAKE_READABLE(st, (st->stack-(char*)st));
898 #endif
899    return st;
900 }
901
902 void sb_decoder_destroy(void *state)
903 {
904    SBDecState *st;
905    st = (SBDecState*)state;
906    speex_decoder_destroy(st->st_low);
907 #if !(defined(VAR_ARRAYS) || defined (USE_ALLOCA))
908    speex_free_scratch(st->stack);
909 #endif
910
911    speex_free(st->x0d);
912    speex_free(st->x1d);
913    speex_free(st->high);
914    speex_free(st->y0);
915    speex_free(st->y1);
916    speex_free(st->g0_mem);
917    speex_free(st->g1_mem);
918    speex_free(st->exc);
919    speex_free(st->excBuf);
920    speex_free(st->qlsp);
921    speex_free(st->old_qlsp);
922    speex_free(st->interp_qlsp);
923    speex_free(st->interp_qlpc);
924    speex_free(st->pi_gain);
925    speex_free(st->mem_sp);
926
927    speex_free(state);
928 }
929
930 static void sb_decode_lost(SBDecState *st, spx_word16_t *out, int dtx, char *stack)
931 {
932    int i;
933    int saved_modeid=0;
934
935    if (dtx)
936    {
937       saved_modeid=st->submodeID;
938       st->submodeID=1;
939    } else {
940       bw_lpc(GAMMA_SCALING*0.99, st->interp_qlpc, st->interp_qlpc, st->lpcSize);
941    }
942
943    st->first=1;
944    
945    
946    /* Final signal synthesis from excitation */
947    if (!dtx)
948    {
949       spx_word16_t low_ener;
950       low_ener = .9*compute_rms(st->exc, st->frame_size);
951       for (i=0;i<st->frame_size;i++)
952          st->exc[i] = speex_rand(low_ener, &st->seed);
953    }
954
955    for (i=0;i<st->frame_size;i++)
956       st->high[i]=st->exc[i];
957
958    iir_mem2(st->high, st->interp_qlpc, st->high, st->frame_size, st->lpcSize, 
959             st->mem_sp);
960    
961    
962    /* Reconstruct the original */
963    fir_mem_up(st->x0d, h0, st->y0, st->full_frame_size, QMF_ORDER, st->g0_mem, stack);
964    fir_mem_up(st->high, h1, st->y1, st->full_frame_size, QMF_ORDER, st->g1_mem, stack);
965
966    mix_and_saturate(st->y0, st->y1, out, st->full_frame_size);
967
968    if (dtx)
969    {
970       st->submodeID=saved_modeid;
971    }
972
973    return;
974 }
975
976 int sb_decode(void *state, SpeexBits *bits, void *vout)
977 {
978    int i, sub;
979    SBDecState *st;
980    int wideband;
981    int ret;
982    char *stack;
983    VARDECL(spx_word32_t *low_pi_gain);
984    VARDECL(spx_word16_t *low_exc);
985    VARDECL(spx_coef_t *ak);
986    int dtx;
987    const SpeexSBMode *mode;
988    spx_word16_t *out = vout;
989    
990    st = (SBDecState*)state;
991    stack=st->stack;
992    mode = (const SpeexSBMode*)(st->mode->mode);
993
994    {
995       VARDECL(spx_word16_t *low);
996       ALLOC(low, st->frame_size, spx_word16_t);
997       
998       /* Decode the low-band */
999       ret = speex_decode_native(st->st_low, bits, low);
1000       
1001       for (i=0;i<st->frame_size;i++)
1002          st->x0d[i] = SHL((spx_sig_t)low[i], SIG_SHIFT);
1003    }
1004
1005    speex_decoder_ctl(st->st_low, SPEEX_GET_DTX_STATUS, &dtx);
1006
1007    /* If error decoding the narrowband part, propagate error */
1008    if (ret!=0)
1009    {
1010       return ret;
1011    }
1012
1013    if (!bits)
1014    {
1015       sb_decode_lost(st, out, dtx, stack);
1016       return 0;
1017    }
1018
1019    if (st->encode_submode)
1020    {
1021
1022       /*Check "wideband bit"*/
1023       if (speex_bits_remaining(bits)>0)
1024          wideband = speex_bits_peek(bits);
1025       else
1026          wideband = 0;
1027       if (wideband)
1028       {
1029          /*Regular wideband frame, read the submode*/
1030          wideband = speex_bits_unpack_unsigned(bits, 1);
1031          st->submodeID = speex_bits_unpack_unsigned(bits, SB_SUBMODE_BITS);
1032       } else
1033       {
1034          /*Was a narrowband frame, set "null submode"*/
1035          st->submodeID = 0;
1036       }
1037       if (st->submodeID != 0 && st->submodes[st->submodeID] == NULL)
1038       {
1039          speex_warning("Invalid mode encountered: corrupted stream?");
1040          return -2;
1041       }
1042    }
1043
1044    /* If null mode (no transmission), just set a couple things to zero*/
1045    if (st->submodes[st->submodeID] == NULL)
1046    {
1047       if (dtx)
1048       {
1049          sb_decode_lost(st, out, 1, stack);
1050          return 0;
1051       }
1052
1053       for (i=0;i<st->frame_size;i++)
1054          st->exc[i]=VERY_SMALL;
1055
1056       st->first=1;
1057
1058       /* Final signal synthesis from excitation */
1059       iir_mem2(st->exc, st->interp_qlpc, st->high, st->frame_size, st->lpcSize, st->mem_sp);
1060
1061       fir_mem_up(st->x0d, h0, st->y0, st->full_frame_size, QMF_ORDER, st->g0_mem, stack);
1062       fir_mem_up(st->high, h1, st->y1, st->full_frame_size, QMF_ORDER, st->g1_mem, stack);
1063
1064       mix_and_saturate(st->y0, st->y1, out, st->full_frame_size);
1065
1066       return 0;
1067
1068    }
1069
1070    for (i=0;i<st->frame_size;i++)
1071       st->exc[i]=0;
1072
1073    ALLOC(low_pi_gain, st->nbSubframes, spx_word32_t);
1074    ALLOC(low_exc, st->frame_size, spx_word16_t);
1075    speex_decoder_ctl(st->st_low, SPEEX_GET_PI_GAIN, low_pi_gain);
1076    speex_decoder_ctl(st->st_low, SPEEX_GET_EXC, low_exc);
1077
1078    SUBMODE(lsp_unquant)(st->qlsp, st->lpcSize, bits);
1079    
1080    if (st->first)
1081    {
1082       for (i=0;i<st->lpcSize;i++)
1083          st->old_qlsp[i] = st->qlsp[i];
1084    }
1085    
1086    ALLOC(ak, st->lpcSize, spx_coef_t);
1087
1088    for (sub=0;sub<st->nbSubframes;sub++)
1089    {
1090       spx_sig_t *exc, *sp, *innov_save=NULL;
1091       spx_word16_t filter_ratio;
1092       spx_word16_t el=0;
1093       int offset;
1094       spx_word32_t rl=0,rh=0;
1095       
1096       offset = st->subframeSize*sub;
1097       sp=st->high+offset;
1098       exc=st->exc+offset;
1099       /* Pointer for saving innovation */
1100       if (st->innov_save)
1101       {
1102          innov_save = st->innov_save+2*offset;
1103          for (i=0;i<2*st->subframeSize;i++)
1104             innov_save[i]=0;
1105       }
1106       
1107       /* LSP interpolation */
1108       lsp_interpolate(st->old_qlsp, st->qlsp, st->interp_qlsp, st->lpcSize, sub, st->nbSubframes);
1109
1110       lsp_enforce_margin(st->interp_qlsp, st->lpcSize, LSP_MARGIN);
1111
1112       /* LSP to LPC */
1113       lsp_to_lpc(st->interp_qlsp, ak, st->lpcSize, stack);
1114
1115 #ifdef OLD_ENHANCER
1116       for (i=0;i<st->lpcSize;i++)
1117          st->interp_qlpc[i] = ak[i];
1118 #endif
1119
1120       /* Calculate reponse ratio between the low and high filter in the middle
1121          of the band (4000 Hz) */
1122       
1123          st->pi_gain[sub]=LPC_SCALING;
1124          rh = LPC_SCALING;
1125          for (i=0;i<st->lpcSize;i+=2)
1126          {
1127             rh += st->interp_qlpc[i+1] - st->interp_qlpc[i];
1128             st->pi_gain[sub] += st->interp_qlpc[i] + st->interp_qlpc[i+1];
1129          }
1130
1131          rl = low_pi_gain[sub];
1132 #ifdef FIXED_POINT
1133          filter_ratio=PDIV32_16(SHL(rl+82,2),SHR(82+rh,5));
1134 #else
1135          filter_ratio=(rl+.01)/(rh+.01);
1136 #endif
1137       
1138       for (i=0;i<st->subframeSize;i++)
1139          exc[i]=0;
1140       if (!SUBMODE(innovation_unquant))
1141       {
1142          float g;
1143          int quant;
1144
1145          quant = speex_bits_unpack_unsigned(bits, 5);
1146          g= exp(((float)quant-10)/8.0);
1147          
1148 #ifdef FIXED_POINT
1149          g /= filter_ratio/128.;
1150 #else
1151          g /= filter_ratio;
1152 #endif
1153          /* High-band excitation using the low-band excitation and a gain */
1154          
1155 #if 0
1156          for (i=0;i<st->subframeSize;i++)
1157             exc[i]=mode->folding_gain*g*st->low_innov[offset+i];
1158 #else
1159          {
1160             float tmp=1;
1161             /*static tmp1=0,tmp2=0;
1162             static int seed=1;
1163             el = compute_rms(low_innov+offset, st->subframeSize);*/
1164             for (i=0;i<st->subframeSize;i++)
1165             {
1166                float e=tmp*g*mode->folding_gain*st->low_innov[offset+i];
1167                tmp *= -1;
1168                exc[i] = e;
1169                /*float r = speex_rand(g*el,&seed);
1170                exc[i] = .5*(r+tmp2 + e-tmp1);
1171                tmp1 = e;
1172                tmp2 = r;*/               
1173             }
1174             
1175          }
1176          
1177 #endif    
1178       } else {
1179          spx_word16_t gc;
1180          spx_word32_t scale;
1181          int qgc = speex_bits_unpack_unsigned(bits, 4);
1182
1183          el = compute_rms16(low_exc+offset, st->subframeSize);
1184
1185 #ifdef FIXED_POINT
1186          gc = MULT16_32_Q15(28626,gc_quant_bound[qgc]);
1187 #else
1188          gc = exp((1/3.7)*qgc-0.15556);
1189 #endif
1190
1191          if (st->subframeSize==80)
1192             gc *= 1.4142;
1193
1194          scale = SHL(MULT16_16(PDIV32_16(SHL(gc,SIG_SHIFT-4),filter_ratio),(1+el)),4);
1195
1196          SUBMODE(innovation_unquant)(exc, SUBMODE(innovation_params), st->subframeSize, 
1197                                 bits, stack);
1198
1199          signal_mul(exc,exc,scale,st->subframeSize);
1200
1201          if (SUBMODE(double_codebook)) {
1202             char *tmp_stack=stack;
1203             VARDECL(spx_sig_t *innov2);
1204             ALLOC(innov2, st->subframeSize, spx_sig_t);
1205             for (i=0;i<st->subframeSize;i++)
1206                innov2[i]=0;
1207             SUBMODE(innovation_unquant)(innov2, SUBMODE(innovation_params), st->subframeSize, 
1208                                 bits, stack);
1209             for (i=0;i<st->subframeSize;i++)
1210                innov2[i]*=scale/(float)SIG_SCALING*(1/2.5);
1211             for (i=0;i<st->subframeSize;i++)
1212                exc[i] = ADD32(exc[i],innov2[i]);
1213             stack = tmp_stack;
1214          }
1215
1216       }
1217       
1218       if (st->innov_save)
1219       {
1220          for (i=0;i<st->subframeSize;i++)
1221             innov_save[2*i]=exc[i];
1222       }
1223       
1224 #ifdef OLD_ENHANCER
1225       for (i=0;i<st->subframeSize;i++)
1226          st->excBuf[i]=exc[i];
1227 #endif
1228       for (i=0;i<st->subframeSize;i++)
1229          sp[i]=st->excBuf[i];
1230       iir_mem2(sp, st->interp_qlpc, sp, st->subframeSize, st->lpcSize, 
1231                st->mem_sp);
1232 #ifndef OLD_ENHANCER
1233       for (i=0;i<st->subframeSize;i++)
1234          st->excBuf[i]=exc[i];
1235       for (i=0;i<st->lpcSize;i++)
1236          st->interp_qlpc[i] = ak[i];
1237 #endif
1238
1239    }
1240
1241    fir_mem_up(st->x0d, h0, st->y0, st->full_frame_size, QMF_ORDER, st->g0_mem, stack);
1242    fir_mem_up(st->high, h1, st->y1, st->full_frame_size, QMF_ORDER, st->g1_mem, stack);
1243
1244    mix_and_saturate(st->y0, st->y1, out, st->full_frame_size);
1245
1246    for (i=0;i<st->lpcSize;i++)
1247       st->old_qlsp[i] = st->qlsp[i];
1248
1249    st->first=0;
1250
1251    return 0;
1252 }
1253
1254
1255 int sb_encoder_ctl(void *state, int request, void *ptr)
1256 {
1257    SBEncState *st;
1258    st=(SBEncState*)state;
1259    switch(request)
1260    {
1261    case SPEEX_GET_FRAME_SIZE:
1262       (*(int*)ptr) = st->full_frame_size;
1263       break;
1264    case SPEEX_SET_HIGH_MODE:
1265       st->submodeSelect = st->submodeID = (*(int*)ptr);
1266       break;
1267    case SPEEX_SET_LOW_MODE:
1268       speex_encoder_ctl(st->st_low, SPEEX_SET_LOW_MODE, ptr);
1269       break;
1270    case SPEEX_SET_DTX:
1271       speex_encoder_ctl(st->st_low, SPEEX_SET_DTX, ptr);
1272       break;
1273    case SPEEX_GET_DTX:
1274       speex_encoder_ctl(st->st_low, SPEEX_GET_DTX, ptr);
1275       break;
1276    case SPEEX_GET_LOW_MODE:
1277       speex_encoder_ctl(st->st_low, SPEEX_GET_LOW_MODE, ptr);
1278       break;
1279    case SPEEX_SET_MODE:
1280       speex_encoder_ctl(st, SPEEX_SET_QUALITY, ptr);
1281       break;
1282    case SPEEX_SET_VBR:
1283       st->vbr_enabled = (*(int*)ptr);
1284       speex_encoder_ctl(st->st_low, SPEEX_SET_VBR, ptr);
1285       break;
1286    case SPEEX_GET_VBR:
1287       (*(int*)ptr) = st->vbr_enabled;
1288       break;
1289    case SPEEX_SET_VAD:
1290       st->vad_enabled = (*(int*)ptr);
1291       speex_encoder_ctl(st->st_low, SPEEX_SET_VAD, ptr);
1292       break;
1293    case SPEEX_GET_VAD:
1294       (*(int*)ptr) = st->vad_enabled;
1295       break;
1296    case SPEEX_SET_VBR_QUALITY:
1297       {
1298          int q;
1299          float qual = (*(float*)ptr)+.6;
1300          st->vbr_quality = (*(float*)ptr);
1301          if (qual>10)
1302             qual=10;
1303          q=(int)floor(.5+*(float*)ptr);
1304          if (q>10)
1305             q=10;
1306          speex_encoder_ctl(st->st_low, SPEEX_SET_VBR_QUALITY, &qual);
1307          speex_encoder_ctl(state, SPEEX_SET_QUALITY, &q);
1308          break;
1309       }
1310    case SPEEX_GET_VBR_QUALITY:
1311       (*(float*)ptr) = st->vbr_quality;
1312       break;
1313    case SPEEX_SET_ABR:
1314       st->abr_enabled = (*(int*)ptr);
1315       st->vbr_enabled = 1;
1316       speex_encoder_ctl(st->st_low, SPEEX_SET_VBR, &st->vbr_enabled);
1317       {
1318          int i=10, rate, target;
1319          float vbr_qual;
1320          target = (*(int*)ptr);
1321          while (i>=0)
1322          {
1323             speex_encoder_ctl(st, SPEEX_SET_QUALITY, &i);
1324             speex_encoder_ctl(st, SPEEX_GET_BITRATE, &rate);
1325             if (rate <= target)
1326                break;
1327             i--;
1328          }
1329          vbr_qual=i;
1330          if (vbr_qual<0)
1331             vbr_qual=0;
1332          speex_encoder_ctl(st, SPEEX_SET_VBR_QUALITY, &vbr_qual);
1333          st->abr_count=0;
1334          st->abr_drift=0;
1335          st->abr_drift2=0;
1336       }
1337       
1338       break;
1339    case SPEEX_GET_ABR:
1340       (*(int*)ptr) = st->abr_enabled;
1341       break;
1342    case SPEEX_SET_QUALITY:
1343       {
1344          int nb_qual;
1345          int quality = (*(int*)ptr);
1346          if (quality < 0)
1347             quality = 0;
1348          if (quality > 10)
1349             quality = 10;
1350          st->submodeSelect = st->submodeID = ((const SpeexSBMode*)(st->mode->mode))->quality_map[quality];
1351          nb_qual = ((const SpeexSBMode*)(st->mode->mode))->low_quality_map[quality];
1352          speex_encoder_ctl(st->st_low, SPEEX_SET_MODE, &nb_qual);
1353       }
1354       break;
1355    case SPEEX_SET_COMPLEXITY:
1356       speex_encoder_ctl(st->st_low, SPEEX_SET_COMPLEXITY, ptr);
1357       st->complexity = (*(int*)ptr);
1358       if (st->complexity<1)
1359          st->complexity=1;
1360       break;
1361    case SPEEX_GET_COMPLEXITY:
1362       (*(int*)ptr) = st->complexity;
1363       break;
1364    case SPEEX_SET_BITRATE:
1365       {
1366          int i=10, rate, target;
1367          target = (*(int*)ptr);
1368          while (i>=0)
1369          {
1370             speex_encoder_ctl(st, SPEEX_SET_QUALITY, &i);
1371             speex_encoder_ctl(st, SPEEX_GET_BITRATE, &rate);
1372             if (rate <= target)
1373                break;
1374             i--;
1375          }
1376       }
1377       break;
1378    case SPEEX_GET_BITRATE:
1379       speex_encoder_ctl(st->st_low, request, ptr);
1380       /*fprintf (stderr, "before: %d\n", (*(int*)ptr));*/
1381       if (st->submodes[st->submodeID])
1382          (*(int*)ptr) += st->sampling_rate*SUBMODE(bits_per_frame)/st->full_frame_size;
1383       else
1384          (*(int*)ptr) += st->sampling_rate*(SB_SUBMODE_BITS+1)/st->full_frame_size;
1385       /*fprintf (stderr, "after: %d\n", (*(int*)ptr));*/
1386       break;
1387    case SPEEX_SET_SAMPLING_RATE:
1388       {
1389          int tmp=(*(int*)ptr);
1390          st->sampling_rate = tmp;
1391          tmp>>=1;
1392          speex_encoder_ctl(st->st_low, SPEEX_SET_SAMPLING_RATE, &tmp);
1393       }
1394       break;
1395    case SPEEX_GET_SAMPLING_RATE:
1396       (*(int*)ptr)=st->sampling_rate;
1397       break;
1398    case SPEEX_RESET_STATE:
1399       {
1400          int i;
1401          st->first = 1;
1402          for (i=0;i<st->lpcSize;i++)
1403             st->lsp[i]=(M_PI*((float)(i+1)))/(st->lpcSize+1);
1404          for (i=0;i<st->lpcSize;i++)
1405             st->mem_sw[i]=st->mem_sp[i]=st->mem_sp2[i]=0;
1406          for (i=0;i<st->bufSize;i++)
1407             st->excBuf[i]=0;
1408          for (i=0;i<QMF_ORDER;i++)
1409             st->h0_mem[i]=st->h1_mem[i]=st->g0_mem[i]=st->g1_mem[i]=0;
1410       }
1411       break;
1412    case SPEEX_SET_SUBMODE_ENCODING:
1413       st->encode_submode = (*(int*)ptr);
1414       speex_encoder_ctl(st->st_low, SPEEX_SET_SUBMODE_ENCODING, &ptr);
1415       break;
1416    case SPEEX_GET_SUBMODE_ENCODING:
1417       (*(int*)ptr) = st->encode_submode;
1418       break;
1419    case SPEEX_GET_LOOKAHEAD:
1420       speex_encoder_ctl(st->st_low, SPEEX_GET_LOOKAHEAD, ptr);
1421       (*(int*)ptr) = 2*(*(int*)ptr) + QMF_ORDER - 1;
1422       break;
1423    case SPEEX_GET_PI_GAIN:
1424       {
1425          int i;
1426          spx_word32_t *g = (spx_word32_t*)ptr;
1427          for (i=0;i<st->nbSubframes;i++)
1428             g[i]=st->pi_gain[i];
1429       }
1430       break;
1431    case SPEEX_GET_EXC:
1432       {
1433          int i;
1434          spx_sig_t *e = (spx_sig_t*)ptr;
1435          for (i=0;i<st->full_frame_size;i++)
1436             e[i]=0;
1437          for (i=0;i<st->frame_size;i++)
1438             e[2*i]=2*st->exc[i];
1439       }
1440       break;
1441    case SPEEX_GET_INNOV:
1442       {
1443          int i;
1444          spx_sig_t *e = (spx_sig_t*)ptr;
1445          for (i=0;i<st->full_frame_size;i++)
1446             e[i]=0;
1447          for (i=0;i<st->frame_size;i++)
1448             e[2*i]=2*st->exc[i];
1449       }
1450       break;
1451    case SPEEX_GET_RELATIVE_QUALITY:
1452       (*(float*)ptr)=st->relative_quality;
1453       break;
1454    case SPEEX_SET_INNOVATION_SAVE:
1455       st->innov_save = ptr;
1456       break;
1457    default:
1458       speex_warning_int("Unknown nb_ctl request: ", request);
1459       return -1;
1460    }
1461    return 0;
1462 }
1463
1464 int sb_decoder_ctl(void *state, int request, void *ptr)
1465 {
1466    SBDecState *st;
1467    st=(SBDecState*)state;
1468    switch(request)
1469    {
1470    case SPEEX_SET_HIGH_MODE:
1471       st->submodeID = (*(int*)ptr);
1472       break;
1473    case SPEEX_SET_LOW_MODE:
1474       speex_decoder_ctl(st->st_low, SPEEX_SET_LOW_MODE, ptr);
1475       break;
1476    case SPEEX_GET_LOW_MODE:
1477       speex_decoder_ctl(st->st_low, SPEEX_GET_LOW_MODE, ptr);
1478       break;
1479    case SPEEX_GET_FRAME_SIZE:
1480       (*(int*)ptr) = st->full_frame_size;
1481       break;
1482    case SPEEX_SET_ENH:
1483       speex_decoder_ctl(st->st_low, request, ptr);
1484       st->lpc_enh_enabled = *((int*)ptr);
1485       break;
1486    case SPEEX_GET_ENH:
1487       *((int*)ptr) = st->lpc_enh_enabled;
1488       break;
1489    case SPEEX_SET_MODE:
1490    case SPEEX_SET_QUALITY:
1491       {
1492          int nb_qual;
1493          int quality = (*(int*)ptr);
1494          if (quality < 0)
1495             quality = 0;
1496          if (quality > 10)
1497             quality = 10;
1498          st->submodeID = ((const SpeexSBMode*)(st->mode->mode))->quality_map[quality];
1499          nb_qual = ((const SpeexSBMode*)(st->mode->mode))->low_quality_map[quality];
1500          speex_decoder_ctl(st->st_low, SPEEX_SET_MODE, &nb_qual);
1501       }
1502       break;
1503    case SPEEX_GET_BITRATE:
1504       speex_decoder_ctl(st->st_low, request, ptr);
1505       if (st->submodes[st->submodeID])
1506          (*(int*)ptr) += st->sampling_rate*SUBMODE(bits_per_frame)/st->full_frame_size;
1507       else
1508          (*(int*)ptr) += st->sampling_rate*(SB_SUBMODE_BITS+1)/st->full_frame_size;
1509       break;
1510    case SPEEX_SET_SAMPLING_RATE:
1511       {
1512          int tmp=(*(int*)ptr);
1513          st->sampling_rate = tmp;
1514          tmp>>=1;
1515          speex_decoder_ctl(st->st_low, SPEEX_SET_SAMPLING_RATE, &tmp);
1516       }
1517       break;
1518    case SPEEX_GET_SAMPLING_RATE:
1519       (*(int*)ptr)=st->sampling_rate;
1520       break;
1521    case SPEEX_SET_HANDLER:
1522       speex_decoder_ctl(st->st_low, SPEEX_SET_HANDLER, ptr);
1523       break;
1524    case SPEEX_SET_USER_HANDLER:
1525       speex_decoder_ctl(st->st_low, SPEEX_SET_USER_HANDLER, ptr);
1526       break;
1527    case SPEEX_RESET_STATE:
1528       {
1529          int i;
1530          for (i=0;i<2*st->lpcSize;i++)
1531             st->mem_sp[i]=0;
1532          for (i=0;i<QMF_ORDER;i++)
1533             st->g0_mem[i]=st->g1_mem[i]=0;
1534       }
1535       break;
1536    case SPEEX_SET_SUBMODE_ENCODING:
1537       st->encode_submode = (*(int*)ptr);
1538       speex_decoder_ctl(st->st_low, SPEEX_SET_SUBMODE_ENCODING, &ptr);
1539       break;
1540    case SPEEX_GET_SUBMODE_ENCODING:
1541       (*(int*)ptr) = st->encode_submode;
1542       break;
1543    case SPEEX_GET_LOOKAHEAD:
1544       speex_decoder_ctl(st->st_low, SPEEX_GET_LOOKAHEAD, ptr);
1545       (*(int*)ptr) = 2*(*(int*)ptr);
1546       break;
1547    case SPEEX_GET_PI_GAIN:
1548       {
1549          int i;
1550          spx_word32_t *g = (spx_word32_t*)ptr;
1551          for (i=0;i<st->nbSubframes;i++)
1552             g[i]=st->pi_gain[i];
1553       }
1554       break;
1555    case SPEEX_GET_EXC:
1556       {
1557          int i;
1558          spx_sig_t *e = (spx_sig_t*)ptr;
1559          for (i=0;i<st->full_frame_size;i++)
1560             e[i]=0;
1561          for (i=0;i<st->frame_size;i++)
1562             e[2*i]=2*st->exc[i];
1563       }
1564       break;
1565    case SPEEX_GET_INNOV:
1566       {
1567          int i;
1568          spx_sig_t *e = (spx_sig_t*)ptr;
1569          for (i=0;i<st->full_frame_size;i++)
1570             e[i]=0;
1571          for (i=0;i<st->frame_size;i++)
1572             e[2*i]=2*st->exc[i];
1573       }
1574       break;
1575    case SPEEX_GET_DTX_STATUS:
1576       speex_decoder_ctl(st->st_low, SPEEX_GET_DTX_STATUS, ptr);
1577       break;
1578    case SPEEX_SET_INNOVATION_SAVE:
1579       st->innov_save = ptr;
1580       break;
1581    default:
1582       speex_warning_int("Unknown nb_ctl request: ", request);
1583       return -1;
1584    }
1585    return 0;
1586 }
1587
1588 #endif
1589