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