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