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