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