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