summaryrefslogtreecommitdiff
path: root/audio_codec/libamr/dec_util.c (plain)
blob: 9d0f6c550ca390bba06b7098ece13a5d68eb4779
1/*
2 *===================================================================
3 * 3GPP AMR Wideband Floating-point Speech Codec
4 *===================================================================
5 */
6#include <stdio.h>
7#include <stdlib.h>
8#include <string.h>
9#include <fcntl.h>
10#include <math.h>
11//#include <memory.h>
12#include "typedef.h"
13#include "dec_main.h"
14#include "dec_lpc.h"
15
16#define MAX_16 (Word16)0x7FFF
17#define MIN_16 (Word16)0x8000
18#define L_SUBFR 64 /* Subframe size */
19#define L_SUBFR16k 80 /* Subframe size at 16kHz */
20#define M16k 20 /* Order of LP filter */
21#define PREEMPH_FAC 22282 /* preemphasis factor (0.68 in Q15) */
22#define FAC4 4
23#define FAC5 5
24#define UP_FAC 20480 /* 5/4 in Q14 */
25#define INV_FAC5 6554 /* 1/5 in Q15 */
26#define NB_COEF_UP 12
27#define L_FIR 31
28#define MODE_7k 0
29#define MODE_24k 8
30
31
32extern const Word16 D_ROM_pow2[];
33extern const Word16 D_ROM_isqrt[];
34extern const Word16 D_ROM_log2[];
35extern const Word16 D_ROM_fir_up[];
36extern const Word16 D_ROM_fir_6k_7k[];
37extern const Word16 D_ROM_fir_7k[];
38extern const Word16 D_ROM_hp_gain[];
39
40#ifdef WIN32
41#pragma warning( disable : 4310)
42#endif
43/*
44 * D_UTIL_random
45 *
46 * Parameters:
47 * seed I/O: seed for random number
48 *
49 * Function:
50 * Signed 16 bits random generator.
51 *
52 * Returns:
53 * random number
54 */
55Word16 D_UTIL_random(Word16 *seed)
56{
57 /*static Word16 seed = 21845;*/
58 *seed = (Word16)(*seed * 31821L + 13849L);
59 return(*seed);
60}
61
62
63/*
64 * D_UTIL_pow2
65 *
66 * Parameters:
67 * exponant I: (Q0) Integer part. (range: 0 <= val <= 30)
68 * fraction I: (Q15) Fractionnal part. (range: 0.0 <= val < 1.0)
69 *
70 * Function:
71 * L_x = pow(2.0, exponant.fraction) (exponant = interger part)
72 * = pow(2.0, 0.fraction) << exponant
73 *
74 * Algorithm:
75 *
76 * The function Pow2(L_x) is approximated by a table and linear
77 * interpolation.
78 *
79 * 1 - i = bit10 - b15 of fraction, 0 <= i <= 31
80 * 2 - a = bit0 - b9 of fraction
81 * 3 - L_x = table[i] << 16 - (table[i] - table[i + 1]) * a * 2
82 * 4 - L_x = L_x >> (30-exponant) (with rounding)
83 *
84 * Returns:
85 * range 0 <= val <= 0x7fffffff
86 */
87Word32 D_UTIL_pow2(Word16 exponant, Word16 fraction)
88{
89 Word32 L_x, tmp, i, exp;
90 Word16 a;
91
92 L_x = fraction * 32; /* L_x = fraction<<6 */
93 i = L_x >> 15; /* Extract b10-b16 of fraction */
94 a = (Word16)(L_x); /* Extract b0-b9 of fraction */
95 a = (Word16)(a & (Word16)0x7fff);
96 L_x = D_ROM_pow2[i] << 16; /* table[i] << 16 */
97 tmp = D_ROM_pow2[i] - D_ROM_pow2[i + 1]; /* table[i] - table[i+1] */
98 tmp = L_x - ((tmp * a) << 1); /* L_x -= tmp*a*2 */
99 exp = 30 - exponant;
100 if (exp <= 31) {
101 L_x = tmp >> exp;
102
103 if ((1 << (exp - 1)) & tmp) {
104 L_x++;
105 }
106 } else {
107 L_x = 0;
108 }
109
110 return(L_x);
111}
112
113
114/*
115 * D_UTIL_norm_l
116 *
117 * Parameters:
118 * L_var1 I: 32 bit Word32 signed integer (Word32) whose value
119 * falls in the range 0x8000 0000 <= var1 <= 0x7fff ffff.
120 *
121 * Function:
122 * Produces the number of left shifts needed to normalize the 32 bit
123 * variable L_var1 for positive values on the interval with minimum of
124 * 1073741824 and maximum of 2147483647, and for negative values on
125 * the interval with minimum of -2147483648 and maximum of -1073741824;
126 * in order to normalize the result, the following operation must be done :
127 * norm_L_var1 = L_shl(L_var1,norm_l(L_var1)).
128 *
129 * Returns:
130 * 16 bit Word16 signed integer (Word16) whose value falls in the range
131 * 0x0000 0000 <= var_out <= 0x0000 001f.
132 */
133Word16 D_UTIL_norm_l(Word32 L_var1)
134{
135 Word16 var_out;
136
137 if (L_var1 == 0) {
138 var_out = 0;
139 } else {
140 if (L_var1 == (Word32)0xffffffffL) {
141 var_out = 31;
142 } else {
143 if (L_var1 < 0) {
144 L_var1 = ~L_var1;
145 }
146
147 for (var_out = 0; L_var1 < (Word32)0x40000000L; var_out++) {
148 L_var1 <<= 1;
149 }
150 }
151 }
152
153 return(var_out);
154}
155
156
157/*
158 * D_UTIL_norm_s
159 *
160 * Parameters:
161 * L_var1 I: 32 bit Word32 signed integer (Word32) whose value
162 * falls in the range 0xffff 8000 <= var1 <= 0x0000 7fff.
163 *
164 * Function:
165 * Produces the number of left shift needed to normalize the 16 bit
166 * variable var1 for positive values on the interval with minimum
167 * of 16384 and maximum of 32767, and for negative values on
168 * the interval with minimum of -32768 and maximum of -16384.
169 *
170 * Returns:
171 * 16 bit Word16 signed integer (Word16) whose value falls in the range
172 * 0x0000 0000 <= var_out <= 0x0000 000f.
173 */
174Word16 D_UTIL_norm_s(Word16 var1)
175{
176 Word16 var_out;
177
178 if (var1 == 0) {
179 var_out = 0;
180 } else {
181 if (var1 == -1) {
182 var_out = 15;
183 } else {
184 if (var1 < 0) {
185 var1 = (Word16)~var1;
186 }
187
188 for (var_out = 0; var1 < 0x4000; var_out++) {
189 var1 <<= 1;
190 }
191 }
192 }
193 return(var_out);
194}
195
196
197/*
198 * D_UTIL_dot_product12
199 *
200 * Parameters:
201 * x I: 12bit x vector
202 * y I: 12bit y vector
203 * lg I: vector length
204 * exp O: exponent of result (0..+30)
205 *
206 * Function:
207 * Compute scalar product of <x[],y[]> using accumulator.
208 * The result is normalized (in Q31) with exponent (0..30).
209 *
210 * Returns:
211 * Q31 normalised result (1 < val <= -1)
212 */
213Word32 D_UTIL_dot_product12(Word16 x[], Word16 y[], Word16 lg, Word16 *exp)
214{
215 Word32 sum, i, sft;
216
217 sum = 0L;
218
219 for (i = 0; i < lg; i++) {
220 sum += x[i] * y[i];
221 }
222 sum = (sum << 1) + 1;
223
224 /* Normalize acc in Q31 */
225 sft = D_UTIL_norm_l(sum);
226 sum = sum << sft;
227 *exp = (Word16)(30 - sft); /* exponent = 0..30 */
228
229 return(sum);
230}
231
232
233/*
234 * D_UTIL_normalised_inverse_sqrt
235 *
236 * Parameters:
237 * frac I/O: (Q31) normalized value (1.0 < frac <= 0.5)
238 * exp I/O: exponent (value = frac x 2^exponent)
239 *
240 * Function:
241 * Compute 1/sqrt(value).
242 * If value is negative or zero, result is 1 (frac=7fffffff, exp=0).
243 *
244 * The function 1/sqrt(value) is approximated by a table and linear
245 * interpolation.
246 * 1. If exponant is odd then shift fraction right once.
247 * 2. exponant = -((exponant - 1) >> 1)
248 * 3. i = bit25 - b30 of fraction, 16 <= i <= 63 ->because of normalization.
249 * 4. a = bit10 - b24
250 * 5. i -= 16
251 * 6. fraction = table[i]<<16 - (table[i] - table[i+1]) * a * 2
252 *
253 * Returns:
254 * void
255 */
256void D_UTIL_normalised_inverse_sqrt(Word32 *frac, Word16 *exp)
257{
258 Word32 i, tmp;
259 Word16 a;
260
261 if (*frac <= (Word32)0) {
262 *exp = 0;
263 *frac = 0x7fffffffL;
264 return;
265 }
266
267 if ((*exp & 0x1) == 1) { /* If exponant odd -> shift right */
268 *frac = *frac >> 1;
269 }
270 *exp = (Word16)(-((*exp - 1) >> 1));
271 *frac = *frac >> 9;
272 i = *frac >> 16; /* Extract b25-b31 */
273 *frac = *frac >> 1;
274 a = (Word16)(*frac); /* Extract b10-b24 */
275 a = (Word16)(a & (Word16)0x7fff);
276 i = i - 16;
277 *frac = D_ROM_isqrt[i] << 16; /* table[i] << 16 */
278 tmp = D_ROM_isqrt[i] - D_ROM_isqrt[i + 1]; /* table[i] - table[i+1]) */
279 *frac = *frac - ((tmp * a) << 1); /* frac -= tmp*a*2 */
280
281 return;
282}
283
284
285/*
286 * D_UTIL_inverse_sqrt
287 *
288 * Parameters:
289 * L_x I/O: (Q0) input value (range: 0<=val<=7fffffff)
290 *
291 * Function:
292 * Compute 1/sqrt(L_x).
293 * If value is negative or zero, result is 1 (7fffffff).
294 *
295 * The function 1/sqrt(value) is approximated by a table and linear
296 * interpolation.
297 * 1. Normalization of L_x
298 * 2. call Normalised_Inverse_sqrt(L_x, exponant)
299 * 3. L_y = L_x << exponant
300 *
301 * Returns:
302 * (Q31) output value (range: 0 <= val < 1)
303 */
304Word32 D_UTIL_inverse_sqrt(Word32 L_x)
305{
306 Word32 L_y;
307 Word16 exp;
308
309 exp = D_UTIL_norm_l(L_x);
310 L_x = (L_x << exp); /* L_x is normalized */
311 exp = (Word16)(31 - exp);
312 D_UTIL_normalised_inverse_sqrt(&L_x, &exp);
313
314 if (exp < 0) {
315 L_y = (L_x >> -exp); /* denormalization */
316 } else {
317 L_y = (L_x << exp); /* denormalization */
318 }
319
320 return(L_y);
321}
322
323
324/*
325 * D_UTIL_normalised_log2
326 *
327 * Parameters:
328 * L_x I: input value (normalized)
329 * exp I: norm_l (L_x)
330 * exponent O: Integer part of Log2. (range: 0<=val<=30)
331 * fraction O: Fractional part of Log2. (range: 0<=val<1)
332 *
333 * Function:
334 * Computes log2(L_x, exp), where L_x is positive and
335 * normalized, and exp is the normalisation exponent
336 * If L_x is negative or zero, the result is 0.
337 *
338 * The function Log2(L_x) is approximated by a table and linear
339 * interpolation. The following steps are used to compute Log2(L_x)
340 *
341 * 1. exponent = 30 - norm_exponent
342 * 2. i = bit25 - b31 of L_x; 32 <= i <= 63 (because of normalization).
343 * 3. a = bit10 - b24
344 * 4. i -= 32
345 * 5. fraction = table[i] << 16 - (table[i] - table[i + 1]) * a * 2
346 *
347 *
348 * Returns:
349 * void
350 */
351static void D_UTIL_normalised_log2(Word32 L_x, Word16 exp, Word16 *exponent,
352 Word16 *fraction)
353{
354 Word32 i, a, tmp;
355 Word32 L_y;
356
357 if (L_x <= 0) {
358 *exponent = 0;
359 *fraction = 0;
360 return;
361 }
362
363 *exponent = (Word16)(30 - exp);
364
365 L_x = L_x >> 10;
366 i = L_x >> 15; /* Extract b25-b31 */
367 a = L_x; /* Extract b10-b24 of fraction */
368 a = a & 0x00007fff;
369 i = i - 32;
370 L_y = D_ROM_log2[i] << 16; /* table[i] << 16 */
371 tmp = D_ROM_log2[i] - D_ROM_log2[i + 1]; /* table[i] - table[i+1] */
372 L_y = L_y - ((tmp * a) << 1); /* L_y -= tmp*a*2 */
373 *fraction = (Word16)(L_y >> 16);
374
375 return;
376}
377
378
379/*
380 * D_UTIL_log2
381 *
382 * Parameters:
383 * L_x I: input value
384 * exponent O: Integer part of Log2. (range: 0<=val<=30)
385 * fraction O: Fractional part of Log2. (range: 0<=val<1)
386 *
387 * Function:
388 * Computes log2(L_x), where L_x is positive.
389 * If L_x is negative or zero, the result is 0.
390 *
391 * Returns:
392 * void
393 */
394void D_UTIL_log2(Word32 L_x, Word16 *exponent, Word16 *fraction)
395{
396 Word16 exp;
397
398 exp = D_UTIL_norm_l(L_x);
399 D_UTIL_normalised_log2((L_x << exp), exp, exponent, fraction);
400}
401
402
403/*
404 * D_UTIL_l_extract
405 *
406 * Parameters:
407 * L_32 I: 32 bit integer.
408 * hi O: b16 to b31 of L_32
409 * lo O: (L_32 - hi<<16)>>1
410 *
411 * Function:
412 * Extract from a 32 bit integer two 16 bit DPF.
413 *
414 * Returns:
415 * void
416 */
417void D_UTIL_l_extract(Word32 L_32, Word16 *hi, Word16 *lo)
418{
419 *hi = (Word16)(L_32 >> 16);
420 *lo = (Word16)((L_32 >> 1) - (*hi * 32768));
421
422 return;
423}
424
425
426/*
427 * D_UTIL_mpy_32_16
428 *
429 * Parameters:
430 * hi I: hi part of 32 bit number
431 * lo I: lo part of 32 bit number
432 * n I: 16 bit number
433 *
434 * Function:
435 * Multiply a 16 bit integer by a 32 bit (DPF). The result is divided
436 * by 2^15.
437 *
438 * L_32 = (hi1*lo2)<<1 + ((lo1*lo2)>>15)<<1
439 *
440 * Returns:
441 * 32 bit result
442 */
443Word32 D_UTIL_mpy_32_16(Word16 hi, Word16 lo, Word16 n)
444{
445 Word32 L_32;
446
447 L_32 = hi * n;
448 L_32 += (lo * n) >> 15;
449
450 return(L_32 << 1);
451}
452
453
454/*
455 * D_UTIL_mpy_32
456 *
457 * Parameters:
458 * hi1 I: hi part of first number
459 * lo1 I: lo part of first number
460 * hi2 I: hi part of second number
461 * lo2 I: lo part of second number
462 *
463 * Function:
464 * Multiply two 32 bit integers (DPF). The result is divided by 2^31
465 *
466 * L_32 = (hi1*lo2)<<1 + ((lo1*lo2)>>15)<<1
467 *
468 * Returns:
469 * 32 bit result
470 */
471Word32 D_UTIL_mpy_32(Word16 hi1, Word16 lo1, Word16 hi2, Word16 lo2)
472{
473 Word32 L_32;
474
475 L_32 = hi1 * hi2;
476 L_32 += (hi1 * lo2) >> 15;
477 L_32 += (lo1 * hi2) >> 15;
478
479 return(L_32 << 1);
480}
481
482/*
483 * D_UTIL_saturate
484 *
485 * Parameters:
486 * inp I: 32-bit number
487 *
488 * Function:
489 * Saturation to 16-bit number
490 *
491 * Returns:
492 * 16-bit number
493 */
494Word16 D_UTIL_saturate(Word32 inp)
495{
496 Word16 out;
497 if ((inp < MAX_16) & (inp > MIN_16)) {
498 out = (Word16)inp;
499 } else {
500 if (inp > 0) {
501 out = MAX_16;
502 } else {
503 out = MIN_16;
504 }
505 }
506
507 return(out);
508}
509
510/*
511 * D_UTIL_signal_up_scale
512 *
513 * Parameters:
514 * x I/O: signal to scale
515 * lg I: size of x[]
516 * exp I: exponent: x = round(x << exp)
517 *
518 * Function:
519 * Scale signal up to get maximum of dynamic.
520 *
521 * Returns:
522 * 32 bit result
523 */
524void D_UTIL_signal_up_scale(Word16 x[], Word16 lg, Word16 exp)
525{
526 Word32 i, tmp;
527
528 for (i = 0; i < lg; i++) {
529 tmp = x[i] << exp;
530 x[i] = D_UTIL_saturate(tmp);
531 }
532
533 return;
534}
535
536
537/*
538 * D_UTIL_signal_down_scale
539 *
540 * Parameters:
541 * x I/O: signal to scale
542 * lg I: size of x[]
543 * exp I: exponent: x = round(x << exp)
544 *
545 * Function:
546 * Scale signal up to get maximum of dynamic.
547 *
548 * Returns:
549 * 32 bit result
550 */
551void D_UTIL_signal_down_scale(Word16 x[], Word16 lg, Word16 exp)
552{
553 Word32 i, tmp;
554
555 for (i = 0; i < lg; i++) {
556 tmp = x[i] << 16;
557 tmp = tmp >> exp;
558 x[i] = (Word16)((tmp + 0x8000) >> 16);
559 }
560
561 return;
562}
563
564
565/*
566 * D_UTIL_deemph_32
567 *
568 * Parameters:
569 * x_hi I: input signal (bit31..16)
570 * x_lo I: input signal (bit15..4)
571 * y O: output signal (x16)
572 * mu I: (Q15) deemphasis factor
573 * L I: vector size
574 * mem I/O: memory (y[-1])
575 *
576 * Function:
577 * Filtering through 1/(1-mu z^-1)
578 *
579 * Returns:
580 * void
581 */
582static void D_UTIL_deemph_32(Word16 x_hi[], Word16 x_lo[], Word16 y[],
583 Word16 mu, Word16 L, Word16 *mem)
584{
585 Word32 i, fac;
586 Word32 tmp;
587
588 fac = mu >> 1; /* Q15 --> Q14 */
589
590 /* L_tmp = hi<<16 + lo<<4 */
591 tmp = (x_hi[0] << 12) + x_lo[0];
592 tmp = (tmp << 6) + (*mem * fac);
593 tmp = (tmp + 0x2000) >> 14;
594 y[0] = D_UTIL_saturate(tmp);
595
596 for (i = 1; i < L; i++) {
597 tmp = (x_hi[i] << 12) + x_lo[i];
598 tmp = (tmp << 6) + (y[i - 1] * fac);
599 tmp = (tmp + 0x2000) >> 14;
600 y[i] = D_UTIL_saturate(tmp);
601 }
602
603 *mem = y[L - 1];
604
605 return;
606}
607
608
609/*
610 * D_UTIL_synthesis_32
611 *
612 * Parameters:
613 * a I: LP filter coefficients
614 * m I: order of LP filter
615 * exc I: excitation
616 * Qnew I: exc scaling = 0(min) to 8(max)
617 * sig_hi O: synthesis high
618 * sig_lo O: synthesis low
619 * lg I: size of filtering
620 *
621 * Function:
622 * Perform the synthesis filtering 1/A(z).
623 *
624 * Returns:
625 * void
626 */
627static void D_UTIL_synthesis_32(Word16 a[], Word16 m, Word16 exc[],
628 Word16 Qnew, Word16 sig_hi[], Word16 sig_lo[],
629 Word16 lg)
630{
631 Word32 i, j, a0, s;
632 Word32 tmp, tmp2;
633
634 /* See if a[0] is scaled */
635 s = D_UTIL_norm_s((Word16)a[0]) - 2;
636
637 a0 = a[0] >> (4 + Qnew); /* input / 16 and >>Qnew */
638
639 /* Do the filtering. */
640 for (i = 0; i < lg; i++) {
641 tmp = 0;
642
643 for (j = 1; j <= m; j++) {
644 tmp -= sig_lo[i - j] * a[j];
645 }
646
647 tmp = tmp >> (15 - 4); /* -4 : sig_lo[i] << 4 */
648
649 tmp2 = exc[i] * a0;
650
651 for (j = 1; j <= m; j++) {
652 tmp2 -= sig_hi[i - j] * a[j];
653 }
654
655 tmp += tmp2 << 1;
656 tmp <<= s;
657
658 /* sig_hi = bit16 to bit31 of synthesis */
659 sig_hi[i] = (Word16)(tmp >> 13);
660
661 /* sig_lo = bit4 to bit15 of synthesis */
662 sig_lo[i] = (Word16)((tmp >> 1) - (sig_hi[i] * 4096));
663 }
664
665 return;
666}
667
668
669/*
670 * D_UTIL_hp50_12k8
671 *
672 * Parameters:
673 * signal I/O: signal
674 * lg I: lenght of signal
675 * mem I/O: filter memory [6]
676 *
677 * Function:
678 * 2nd order high pass filter with cut off frequency at 50 Hz.
679 *
680 * Algorithm:
681 *
682 * y[i] = b[0]*x[i] + b[1]*x[i-1] + b[2]*x[i-2]
683 * + a[1]*y[i-1] + a[2]*y[i-2];
684 *
685 * b[3] = {0.989501953f, -1.979003906f, 0.989501953f};
686 * a[3] = {1.000000000F, 1.978881836f,-0.966308594f};
687 *
688 *
689 * Returns:
690 * void
691 */
692static void D_UTIL_hp50_12k8(Word16 signal[], Word16 lg, Word16 mem[])
693{
694 Word32 i, L_tmp;
695 Word16 y2_hi, y2_lo, y1_hi, y1_lo, x0, x1, x2;
696
697 y2_hi = mem[0];
698 y2_lo = mem[1];
699 y1_hi = mem[2];
700 y1_lo = mem[3];
701 x0 = mem[4];
702 x1 = mem[5];
703
704 for (i = 0; i < lg; i++) {
705 x2 = x1;
706 x1 = x0;
707 x0 = signal[i];
708
709 /* y[i] = b[0]*x[i] + b[1]*x[i-1] + b140[2]*x[i-2] */
710 /* + a[1]*y[i-1] + a[2] * y[i-2]; */
711 L_tmp = 8192L; /* rounding to maximise precision */
712 L_tmp = L_tmp + (y1_lo * 16211);
713 L_tmp = L_tmp + (y2_lo * (-8021));
714 L_tmp = L_tmp >> 14;
715 L_tmp = L_tmp + (y1_hi * 32422);
716 L_tmp = L_tmp + (y2_hi * (-16042));
717 L_tmp = L_tmp + (x0 * 8106);
718 L_tmp = L_tmp + (x1 * (-16212));
719 L_tmp = L_tmp + (x2 * 8106);
720 L_tmp = L_tmp << 2; /* coeff Q11 --> Q14 */
721 y2_hi = y1_hi;
722 y2_lo = y1_lo;
723 D_UTIL_l_extract(L_tmp, &y1_hi, &y1_lo);
724 L_tmp = (L_tmp + 0x4000) >> 15; /* coeff Q14 --> Q15 with saturation */
725 signal[i] = D_UTIL_saturate(L_tmp);
726
727 }
728 mem[0] = y2_hi;
729 mem[1] = y2_lo;
730 mem[2] = y1_hi;
731 mem[3] = y1_lo;
732 mem[4] = x0;
733 mem[5] = x1;
734
735 return;
736}
737
738
739/*
740 * D_UTIL_interpol
741 *
742 * Parameters:
743 * x I: input vector
744 * fir I: filter coefficient
745 * frac I: fraction (0..resol)
746 * up_samp I: resolution
747 * nb_coef I: number of coefficients
748 *
749 * Function:
750 * Fractional interpolation of signal at position (frac/up_samp)
751 *
752 * Returns:
753 * result of interpolation
754 */
755Word16 D_UTIL_interpol(Word16 *x, Word16 const *fir, Word16 frac,
756 Word16 resol, Word16 nb_coef)
757{
758 Word32 i, k;
759 Word32 sum;
760
761 x = x - nb_coef + 1;
762 sum = 0L;
763
764 for (i = 0, k = ((resol - 1) - frac); i < 2 * nb_coef; i++,
765 k = (Word16)(k + resol)) {
766 sum = sum + (x[i] * fir[k]);
767 }
768
769 if ((sum < 536846336) & (sum > -536879104)) {
770 sum = (sum + 0x2000) >> 14;
771 } else if (sum > 536846336) {
772 sum = 32767;
773 } else {
774 sum = -32768;
775 }
776
777 return((Word16)sum); /* saturation can occur here */
778}
779
780
781/*
782 * D_UTIL_up_samp
783 *
784 * Parameters:
785 * res_d I: signal to upsampling
786 * res_u O: upsampled output
787 * L_frame I: length of output
788 *
789 * Function:
790 * Upsampling
791 *
792 * Returns:
793 * void
794 */
795static void D_UTIL_up_samp(Word16 *sig_d, Word16 *sig_u, Word16 L_frame)
796{
797 Word32 pos, i, j;
798 Word16 frac;
799
800 pos = 0; /* position with 1/5 resolution */
801
802 for (j = 0; j < L_frame; j++) {
803 i = (pos * INV_FAC5) >> 15; /* integer part = pos * 1/5 */
804 frac = (Word16)(pos - ((i << 2) + i)); /* frac = pos - (pos/5)*5 */
805 sig_u[j] = D_UTIL_interpol(&sig_d[i], D_ROM_fir_up, frac, FAC5, NB_COEF_UP);
806 pos = pos + FAC4; /* position + 4/5 */
807 }
808
809 return;
810}
811
812
813/*
814 * D_UTIL_oversamp_16k
815 *
816 * Parameters:
817 * sig12k8 I: signal to oversampling
818 * lg I: length of input
819 * sig16k O: oversampled signal
820 * mem I/O: memory (2*12)
821 *
822 * Function:
823 * Oversampling from 12.8kHz to 16kHz
824 *
825 * Returns:
826 * void
827 */
828static void D_UTIL_oversamp_16k(Word16 sig12k8[], Word16 lg, Word16 sig16k[],
829 Word16 mem[])
830{
831 Word16 lg_up;
832 Word16 signal[L_SUBFR + (2 * NB_COEF_UP)];
833
834 memcpy(signal, mem, (2 * NB_COEF_UP) * sizeof(Word16));
835 memcpy(signal + (2 * NB_COEF_UP), sig12k8, lg * sizeof(Word16));
836 lg_up = (Word16)(((lg * UP_FAC) >> 15) << 1);
837 D_UTIL_up_samp(signal + NB_COEF_UP, sig16k, lg_up);
838 memcpy(mem, signal + lg, (2 * NB_COEF_UP) * sizeof(Word16));
839
840 return;
841}
842
843
844/*
845 * D_UTIL_hp400_12k8
846 *
847 * Parameters:
848 * signal I/O: signal
849 * lg I: lenght of signal
850 * mem I/O: filter memory [6]
851 *
852 * Function:
853 * 2nd order high pass filter with cut off frequency at 400 Hz.
854 *
855 * Algorithm:
856 *
857 * y[i] = b[0]*x[i] + b[1]*x[i-1] + b[2]*x[i-2]
858 * + a[1]*y[i-1] + a[2]*y[i-2];
859 *
860 * b[3] = {0.893554687, -1.787109375, 0.893554687};
861 * a[3] = {1.000000000, 1.787109375, -0.864257812};
862 *
863 *
864 * Returns:
865 * void
866 */
867void D_UTIL_hp400_12k8(Word16 signal[], Word16 lg, Word16 mem[])
868{
869
870 Word32 i, L_tmp;
871 Word16 y2_hi, y2_lo, y1_hi, y1_lo, x0, x1, x2;
872
873 y2_hi = mem[0];
874 y2_lo = mem[1];
875 y1_hi = mem[2];
876 y1_lo = mem[3];
877 x0 = mem[4];
878 x1 = mem[5];
879
880 for (i = 0; i < lg; i++) {
881 x2 = x1;
882 x1 = x0;
883 x0 = signal[i];
884
885 /* y[i] = b[0]*x[i] + b[1]*x[i-1] + b140[2]*x[i-2] */
886 /* + a[1]*y[i-1] + a[2] * y[i-2]; */
887 L_tmp = 8192L + (y1_lo * 29280);
888 L_tmp = L_tmp + (y2_lo * (-14160));
889 L_tmp = (L_tmp >> 14);
890 L_tmp = L_tmp + (y1_hi * 58560);
891 L_tmp = L_tmp + (y2_hi * (-28320));
892 L_tmp = L_tmp + (x0 * 1830);
893 L_tmp = L_tmp + (x1 * (-3660));
894 L_tmp = L_tmp + (x2 * 1830);
895 L_tmp = (L_tmp << 1); /* coeff Q12 --> Q13 */
896 y2_hi = y1_hi;
897 y2_lo = y1_lo;
898 D_UTIL_l_extract(L_tmp, &y1_hi, &y1_lo);
899
900 /* signal is divided by 16 to avoid overflow in energy computation */
901 signal[i] = (Word16)((L_tmp + 0x8000) >> 16);
902 }
903 mem[0] = y2_hi;
904 mem[1] = y2_lo;
905 mem[2] = y1_hi;
906 mem[3] = y1_lo;
907 mem[4] = x0;
908 mem[5] = x1;
909
910 return;
911}
912
913
914/*
915 * D_UTIL_synthesis
916 *
917 * Parameters:
918 * a I: LP filter coefficients
919 * m I: order of LP filter
920 * x I: input signal
921 * y O: output signal
922 * lg I: size of filtering
923 * mem I/O: initial filter states
924 * update_m I: update memory flag
925 *
926 * Function:
927 * Perform the synthesis filtering 1/A(z).
928 *
929 * Returns:
930 * void
931 */
932static void D_UTIL_synthesis(Word16 a[], Word16 m, Word16 x[], Word16 y[],
933 Word16 lg, Word16 mem[], Word16 update)
934{
935 Word32 i, j, tmp, s;
936 Word16 y_buf[L_SUBFR16k + M16k], a0;
937 Word16 *yy;
938
939 yy = &y_buf[m];
940
941 /* See if a[0] is scaled */
942 s = D_UTIL_norm_s(a[0]) - 2;
943 /* copy initial filter states into synthesis buffer */
944 memcpy(y_buf, mem, m * sizeof(Word16));
945
946 a0 = (Word16)(a[0] >> 1); /* input / 2 */
947
948 /* Do the filtering. */
949 for (i = 0; i < lg; i++) {
950 tmp = x[i] * a0;
951
952 for (j = 1; j <= m; j++) {
953 tmp -= a[j] * yy[i - j];
954 }
955 tmp <<= s;
956
957 y[i] = yy[i] = (Word16)((tmp + 0x800) >> 12);
958 }
959
960 /* Update memory if required */
961 if (update) {
962 memcpy(mem, &yy[lg - m], m * sizeof(Word16));
963 }
964
965 return;
966}
967
968
969/*
970 * D_UTIL_bp_6k_7k
971 *
972 * Parameters:
973 * signal I/O: signal
974 * lg I: lenght of signal
975 * mem I/O: filter memory [4]
976 *
977 * Function:
978 * 15th order band pass 6kHz to 7kHz FIR filter.
979 *
980 * Returns:
981 * void
982 */
983void D_UTIL_bp_6k_7k(Word16 signal[], Word16 lg, Word16 mem[])
984{
985 Word32 x[L_SUBFR16k + (L_FIR - 1)];
986 Word32 i, j, tmp;
987
988 for (i = 0; i < (L_FIR - 1); i++) {
989 x[i] = (Word16)mem[i]; /* gain of filter = 4 */
990 }
991
992 for (i = 0; i < lg; i++) {
993 x[i + L_FIR - 1] = signal[i] >> 2; /* gain of filter = 4 */
994 }
995
996 for (i = 0; i < lg; i++) {
997 tmp = 0;
998
999 for (j = 0; j < L_FIR; j++) {
1000 tmp += x[i + j] * D_ROM_fir_6k_7k[j];
1001 }
1002
1003 signal[i] = (Word16)((tmp + 0x4000) >> 15);
1004 }
1005
1006 for (i = 0; i < (L_FIR - 1); i++) {
1007 mem[i] = (Word16)x[lg + i]; /* gain of filter = 4 */
1008 }
1009
1010 return;
1011}
1012
1013
1014/*
1015 * D_UTIL_hp_7k
1016 *
1017 * Parameters:
1018 * signal I/O: ISF vector
1019 * lg I: length of signal
1020 * mem I/O: memory (30)
1021 *
1022 * Function:
1023 * 15th order high pass 7kHz FIR filter
1024 *
1025 * Returns:
1026 * void
1027 */
1028static void D_UTIL_hp_7k(Word16 signal[], Word16 lg, Word16 mem[])
1029{
1030
1031 Word32 i, j, tmp;
1032 Word16 x[L_SUBFR16k + (L_FIR - 1)];
1033
1034 memcpy(x, mem, (L_FIR - 1) * sizeof(Word16));
1035 memcpy(&x[L_FIR - 1], signal, lg * sizeof(Word16));
1036
1037 for (i = 0; i < lg; i++) {
1038 tmp = 0;
1039
1040 for (j = 0; j < L_FIR; j++) {
1041 tmp += x[i + j] * D_ROM_fir_7k[j];
1042 }
1043
1044 signal[i] = (Word16)((tmp + 0x4000) >> 15);
1045 }
1046
1047 memcpy(mem, x + lg, (L_FIR - 1) * sizeof(Word16));
1048
1049 return;
1050}
1051
1052
1053/*
1054 * D_UTIL_Dec_synthesis
1055 *
1056 * Parameters:
1057 * Aq I: quantized Az
1058 * exc I: excitation at 12kHz
1059 * Q_new I: scaling performed on exc
1060 * synth16k O: 16kHz synthesis signal
1061 * prms I: parameters
1062 * HfIsf I/O: High frequency ISF:s
1063 * mode I: codec mode
1064 * newDTXState I: dtx state
1065 * bfi I: bad frame indicator
1066 * st I/O: State structure
1067 *
1068 * Function:
1069 * Synthesis of signal at 16kHz with HF extension.
1070 *
1071 * Returns:
1072 * void
1073 */
1074void D_UTIL_dec_synthesis(Word16 Aq[], Word16 exc[], Word16 Q_new,
1075 Word16 synth16k[], Word16 prms, Word16 HfIsf[],
1076 Word16 mode, Word16 newDTXState, Word16 bfi,
1077 Decoder_State *st)
1078{
1079 Word32 tmp, i;
1080 Word16 exp;
1081 Word16 ener, exp_ener;
1082 Word32 fac;
1083 Word16 synth_hi[M + L_SUBFR], synth_lo[M + L_SUBFR];
1084 Word16 synth[L_SUBFR];
1085 Word16 HF[L_SUBFR16k]; /* High Frequency vector */
1086 Word16 Ap[M16k + 1];
1087 Word16 HfA[M16k + 1];
1088 Word16 HF_corr_gain;
1089 Word16 HF_gain_ind;
1090 Word32 gain1, gain2;
1091 Word16 weight1, weight2;
1092
1093 /*
1094 * Speech synthesis
1095 *
1096 * - Find synthesis speech corresponding to exc2[].
1097 * - Perform fixed deemphasis and hp 50hz filtering.
1098 * - Oversampling from 12.8kHz to 16kHz.
1099 */
1100 memcpy(synth_hi, st->mem_syn_hi, M * sizeof(Word16));
1101 memcpy(synth_lo, st->mem_syn_lo, M * sizeof(Word16));
1102 D_UTIL_synthesis_32(Aq, M, exc, Q_new, synth_hi + M, synth_lo + M, L_SUBFR);
1103 memcpy(st->mem_syn_hi, synth_hi + L_SUBFR, M * sizeof(Word16));
1104 memcpy(st->mem_syn_lo, synth_lo + L_SUBFR, M * sizeof(Word16));
1105 D_UTIL_deemph_32(synth_hi + M, synth_lo + M, synth, PREEMPH_FAC, L_SUBFR,
1106 &(st->mem_deemph));
1107 D_UTIL_hp50_12k8(synth, L_SUBFR, st->mem_sig_out);
1108 D_UTIL_oversamp_16k(synth, L_SUBFR, synth16k, st->mem_oversamp);
1109
1110 /*
1111 * HF noise synthesis
1112 *
1113 * - Generate HF noise between 5.5 and 7.5 kHz.
1114 * - Set energy of noise according to synthesis tilt.
1115 * tilt > 0.8 ==> - 14 dB (voiced)
1116 * tilt 0.5 ==> - 6 dB (voiced or noise)
1117 * tilt < 0.0 ==> 0 dB (noise)
1118 */
1119
1120 /* generate white noise vector */
1121 for (i = 0; i < L_SUBFR16k; i++) {
1122 HF[i] = (Word16)(D_UTIL_random(&(st->mem_seed2)) >> 3);
1123 }
1124
1125 /* energy of excitation */
1126 D_UTIL_signal_down_scale(exc, L_SUBFR, 3);
1127 Q_new = (Word16)(Q_new - 3);
1128 ener = (Word16)(D_UTIL_dot_product12(exc, exc, L_SUBFR, &exp_ener) >> 16);
1129 exp_ener = (Word16)(exp_ener - (Q_new << 1));
1130
1131 /* set energy of white noise to energy of excitation */
1132 tmp = (Word16)(D_UTIL_dot_product12(HF, HF, L_SUBFR16k, &exp) >> 16);
1133
1134 if (tmp > ener) {
1135 tmp = tmp >> 1; /* Be sure tmp < ener */
1136 exp = (Word16)(exp + 1);
1137 }
1138
1139 tmp = (tmp << 15) / ener;
1140
1141 if (tmp > 32767) {
1142 tmp = 32767;
1143 }
1144
1145 tmp = tmp << 16; /* result is normalized */
1146 exp = (Word16)(exp - exp_ener);
1147 D_UTIL_normalised_inverse_sqrt(&tmp, &exp);
1148
1149 /* L_tmp x 2, L_tmp in Q31 */
1150 /* tmp = 2 x sqrt(ener_exc/ener_hf) */
1151 if (exp >= 0) {
1152 tmp = tmp >> (15 - exp);
1153 } else {
1154 tmp = tmp >> (-exp);
1155 tmp = tmp >> 15;
1156 }
1157
1158 /* saturation */
1159 if (tmp > 0x7FFF) {
1160 tmp = 0x7FFF;
1161 }
1162
1163 for (i = 0; i < L_SUBFR16k; i++) {
1164 HF[i] = (Word16)((HF[i] * tmp) >> 15);
1165 }
1166
1167 /* find tilt of synthesis speech (tilt: 1=voiced, -1=unvoiced) */
1168 D_UTIL_hp400_12k8(synth, L_SUBFR, st->mem_hp400);
1169 tmp = 0L;
1170
1171 for (i = 0; i < L_SUBFR; i++) {
1172 tmp = tmp + (synth[i] * synth[i]);
1173 }
1174
1175 tmp = (tmp << 1) + 1;
1176 exp = D_UTIL_norm_l(tmp);
1177 ener = (Word16)((tmp << exp) >> 16); /* ener = r[0] */
1178 tmp = 0L;
1179
1180 for (i = 1; i < L_SUBFR; i++) {
1181 tmp = tmp + (synth[i] * synth[i - 1]);
1182 }
1183
1184 tmp = (tmp << 1) + 1;
1185 tmp = (tmp << exp) >> 16; /* tmp = r[1] */
1186
1187 if (tmp > 0) {
1188 fac = ((tmp << 15) / ener);
1189
1190 if (fac > 32767) {
1191 fac = 32767;
1192 }
1193 } else {
1194 fac = 0;
1195 }
1196
1197 /* modify energy of white noise according to synthesis tilt */
1198 gain1 = (32767 - fac);
1199 gain2 = ((32767 - fac) * 20480) >> 15;
1200 gain2 = (gain2 << 1);
1201
1202 if (gain2 > 32767) {
1203 gain2 = 32767;
1204 }
1205
1206 if (st->mem_vad_hist > 0) {
1207 weight1 = 0;
1208 weight2 = 32767;
1209 } else {
1210 weight1 = 32767;
1211 weight2 = 0;
1212 }
1213
1214 tmp = (weight1 * gain1) >> 15;
1215 tmp = tmp + ((weight2 * gain2) >> 15);
1216
1217 if (tmp != 0) {
1218 tmp = tmp + 1;
1219 }
1220
1221 if (tmp < 3277) {
1222 tmp = 3277; /* 0.1 in Q15 */
1223 }
1224
1225 if ((mode == MODE_24k) & (bfi == 0)) {
1226 /* HF correction gain */
1227 HF_gain_ind = prms;
1228 HF_corr_gain = D_ROM_hp_gain[HF_gain_ind];
1229
1230 /* HF gain */
1231 for (i = 0; i < L_SUBFR16k; i++) {
1232 HF[i] = (Word16)(((HF[i] * HF_corr_gain) >> 15) << 1);
1233 }
1234 } else {
1235 for (i = 0; i < L_SUBFR16k; i++) {
1236 HF[i] = (Word16)((HF[i] * tmp) >> 15);
1237 }
1238 }
1239
1240 if ((mode <= MODE_7k) & (newDTXState == SPEECH)) {
1241 D_LPC_isf_extrapolation(HfIsf);
1242 D_LPC_isp_a_conversion(HfIsf, HfA, 0, M16k);
1243 D_LPC_a_weight(HfA, Ap, 29491, M16k); /* fac=0.9 */
1244 D_UTIL_synthesis(Ap, M16k, HF, HF, L_SUBFR16k, st->mem_syn_hf, 1);
1245 } else {
1246 /* synthesis of noise: 4.8kHz..5.6kHz --> 6kHz..7kHz */
1247 D_LPC_a_weight(Aq, Ap, 19661, M); /* fac=0.6 */
1248 D_UTIL_synthesis(Ap, M, HF, HF, L_SUBFR16k, st->mem_syn_hf + (M16k - M), 1);
1249 }
1250
1251 /* noise High Pass filtering (1ms of delay) */
1252 D_UTIL_bp_6k_7k(HF, L_SUBFR16k, st->mem_hf);
1253
1254 if (mode == MODE_24k) {
1255 /* Low Pass filtering (7 kHz) */
1256 D_UTIL_hp_7k(HF, L_SUBFR16k, st->mem_hf3);
1257 }
1258
1259 /* add filtered HF noise to speech synthesis */
1260 for (i = 0; i < L_SUBFR16k; i++) {
1261 tmp = (synth16k[i] + HF[i]);
1262 synth16k[i] = D_UTIL_saturate(tmp);
1263 }
1264
1265 return;
1266}
1267
1268
1269/*
1270 * D_UTIL_preemph
1271 *
1272 * Parameters:
1273 * x I/O: signal
1274 * mu I: preemphasis factor
1275 * lg I: vector size
1276 * mem I/O: memory (x[-1])
1277 *
1278 * Function:
1279 * Filtering through 1 - mu z^-1
1280 *
1281 *
1282 * Returns:
1283 * void
1284 */
1285void D_UTIL_preemph(Word16 x[], Word16 mu, Word16 lg, Word16 *mem)
1286{
1287 Word32 i, L_tmp;
1288 Word16 temp;
1289
1290 temp = x[lg - 1];
1291
1292 for (i = lg - 1; i > 0; i--) {
1293 L_tmp = x[i] << 15;
1294 L_tmp = L_tmp - (x[i - 1] * mu);
1295 x[i] = (Word16)((L_tmp + 0x4000) >> 15);
1296 }
1297
1298 L_tmp = x[0] << 15;
1299 L_tmp = L_tmp - (*mem * mu);
1300 x[0] = (Word16)((L_tmp + 0x4000) >> 15);
1301 *mem = temp;
1302
1303 return;
1304}
1305
generated by cgit at 2024-05-18 21:50:01 (GMT)