5
|
1 /*
|
|
2 * SpanDSP - a series of DSP components for telephony
|
|
3 *
|
|
4 * gsm0610_lpc.c - GSM 06.10 full rate speech codec.
|
|
5 *
|
|
6 * Written by Steve Underwood <steveu@coppice.org>
|
|
7 *
|
|
8 * Copyright (C) 2006 Steve Underwood
|
|
9 *
|
|
10 * All rights reserved.
|
|
11 *
|
|
12 * This program is free software; you can redistribute it and/or modify
|
|
13 * it under the terms of the GNU General Public License version 2, as
|
|
14 * published by the Free Software Foundation.
|
|
15 *
|
|
16 * This program is distributed in the hope that it will be useful,
|
|
17 * but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
18 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
19 * GNU General Public License for more details.
|
|
20 *
|
|
21 * You should have received a copy of the GNU General Public License
|
|
22 * along with this program; if not, write to the Free Software
|
|
23 * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
|
|
24 *
|
|
25 * This code is based on the widely used GSM 06.10 code available from
|
|
26 * http://kbs.cs.tu-berlin.de/~jutta/toast.html
|
|
27 *
|
|
28 * $Id: gsm0610_lpc.c,v 1.13 2006/11/19 14:07:24 steveu Exp $
|
|
29 */
|
|
30
|
|
31 /*! \file */
|
|
32
|
|
33 #ifdef HAVE_CONFIG_H
|
|
34 #include <config.h>
|
|
35 #endif
|
|
36
|
|
37 #include <assert.h>
|
|
38 #include <inttypes.h>
|
|
39 #if defined(HAVE_TGMATH_H)
|
|
40 #include <tgmath.h>
|
|
41 #endif
|
|
42 #if defined(HAVE_MATH_H)
|
|
43 #include <math.h>
|
|
44 #endif
|
|
45 #include <stdlib.h>
|
|
46
|
|
47 #include "spandsp/telephony.h"
|
|
48 #include "spandsp/dc_restore.h"
|
|
49 #include "spandsp/bit_operations.h"
|
|
50 #include "spandsp/vector_int.h"
|
|
51 #include "spandsp/gsm0610.h"
|
|
52
|
|
53 #include "gsm0610_local.h"
|
|
54
|
|
55 /* 4.2.4 .. 4.2.7 LPC ANALYSIS SECTION */
|
|
56
|
|
57 /* The number of left shifts needed to normalize the 32 bit
|
|
58 variable x for positive values on the interval
|
|
59 with minimum of
|
|
60 minimum of 1073741824 (01000000000000000000000000000000) and
|
|
61 maximum of 2147483647 (01111111111111111111111111111111)
|
|
62 and for negative values on the interval with
|
|
63 minimum of -2147483648 (-10000000000000000000000000000000) and
|
|
64 maximum of -1073741824 ( -1000000000000000000000000000000).
|
|
65
|
|
66 In order to normalize the result, the following
|
|
67 operation must be done: norm_var1 = x << gsm0610_norm(x);
|
|
68
|
|
69 (That's 'ffs', only from the left, not the right..)
|
|
70 */
|
|
71
|
|
72 int16_t gsm0610_norm(int32_t x)
|
|
73 {
|
|
74 assert(x != 0);
|
|
75
|
|
76 if (x < 0)
|
|
77 {
|
|
78 if (x <= -1073741824)
|
|
79 return 0;
|
|
80 /*endif*/
|
|
81 x = ~x;
|
|
82 }
|
|
83 /*endif*/
|
|
84 return 30 - top_bit(x);
|
|
85 }
|
|
86 /*- End of function --------------------------------------------------------*/
|
|
87
|
|
88 /*
|
|
89 (From p. 46, end of section 4.2.5)
|
|
90
|
|
91 NOTE: The following lines gives [sic] one correct implementation
|
|
92 of the div(num, denum) arithmetic operation. Compute div
|
|
93 which is the integer division of num by denom: with
|
|
94 denom >= num > 0
|
|
95 */
|
|
96 static int16_t gsm_div(int16_t num, int16_t denom)
|
|
97 {
|
|
98 int32_t num32;
|
|
99 int32_t denom32;
|
|
100 int16_t div;
|
|
101 int k;
|
|
102
|
|
103 /* The parameter num sometimes becomes zero.
|
|
104 Although this is explicitly guarded against in 4.2.5,
|
|
105 we assume that the result should then be zero as well. */
|
|
106
|
|
107 assert(num >= 0 && denom >= num);
|
|
108 if (num == 0)
|
|
109 return 0;
|
|
110 /*endif*/
|
|
111 num32 = num;
|
|
112 denom32 = denom;
|
|
113 div = 0;
|
|
114 k = 15;
|
|
115 while (k--)
|
|
116 {
|
|
117 div <<= 1;
|
|
118 num32 <<= 1;
|
|
119
|
|
120 if (num32 >= denom32)
|
|
121 {
|
|
122 num32 -= denom32;
|
|
123 div++;
|
|
124 }
|
|
125 /*endif*/
|
|
126 }
|
|
127 /*endwhile*/
|
|
128
|
|
129 return div;
|
|
130 }
|
|
131 /*- End of function --------------------------------------------------------*/
|
|
132
|
|
133 #if defined(__GNUC__) && defined(__i386__)
|
|
134 void gsm0610_vec_vsraw(const int16_t *p, int n, int bits)
|
|
135 {
|
|
136 static const int64_t ones = 0x0001000100010001LL;
|
|
137
|
|
138 if (n == 0)
|
|
139 return;
|
|
140 /*endif*/
|
|
141 __asm__ __volatile__(
|
|
142 " leal -16(%%esi,%%eax,2),%%edx;\n" /* edx = top - 16 */
|
|
143 " emms;\n"
|
|
144 " movd %%ecx,%%mm3;\n"
|
|
145 " movq %[ones],%%mm2;\n"
|
|
146 " psllw %%mm3,%%mm2;\n"
|
|
147 " psrlw $1,%%mm2;\n"
|
|
148 " cmpl %%edx,%%esi;"
|
|
149 " ja 4f;\n"
|
|
150
|
|
151 " .p2align 2;\n"
|
|
152 /* 8 words per iteration */
|
|
153 "6:\n"
|
|
154 " movq (%%esi),%%mm0;\n"
|
|
155 " movq 8(%%esi),%%mm1;\n"
|
|
156 " paddsw %%mm2,%%mm0;\n"
|
|
157 " psraw %%mm3,%%mm0;\n"
|
|
158 " paddsw %%mm2,%%mm1;\n"
|
|
159 " psraw %%mm3,%%mm1;\n"
|
|
160 " movq %%mm0,(%%esi);\n"
|
|
161 " movq %%mm1,8(%%esi);\n"
|
|
162 " addl $16,%%esi;\n"
|
|
163 " cmpl %%edx,%%esi;\n"
|
|
164 " jbe 6b;\n"
|
|
165
|
|
166 " .p2align 2;\n"
|
|
167 "4:\n"
|
|
168 " addl $12,%%edx;\n" /* now edx = top-4 */
|
|
169 " cmpl %%edx,%%esi;\n"
|
|
170 " ja 3f;\n"
|
|
171
|
|
172 " .p2align 2;\n"
|
|
173 /* do up to 6 words, two per iteration */
|
|
174 "5:\n"
|
|
175 " movd (%%esi),%%mm0;\n"
|
|
176 " paddsw %%mm2,%%mm0;\n"
|
|
177 " psraw %%mm3,%%mm0;\n"
|
|
178 " movd %%mm0,(%%esi);\n"
|
|
179 " addl $4,%%esi;\n"
|
|
180 " cmpl %%edx,%%esi;\n"
|
|
181 " jbe 5b;\n"
|
|
182
|
|
183 " .p2align 2;\n"
|
|
184 "3:\n"
|
|
185 " addl $2,%%edx;\n" /* now edx = top-2 */
|
|
186 " cmpl %%edx,%%esi;\n"
|
|
187 " ja 2f;\n"
|
|
188
|
|
189 " movzwl (%%esi),%%eax;\n"
|
|
190 " movd %%eax,%%mm0;\n"
|
|
191 " paddsw %%mm2,%%mm0;\n"
|
|
192 " psraw %%mm3,%%mm0;\n"
|
|
193 " movd %%mm0,%%eax;\n"
|
|
194 " movw %%ax,(%%esi);\n"
|
|
195
|
|
196 " .p2align 2;\n"
|
|
197 "2:\n"
|
|
198 " emms;\n"
|
|
199 :
|
|
200 : "S" (p), "a" (n), "c" (bits), [ones] "m" (ones)
|
|
201 : "edx"
|
|
202 );
|
|
203 }
|
|
204 /*- End of function --------------------------------------------------------*/
|
|
205 #endif
|
|
206
|
|
207 /* 4.2.4 */
|
|
208 static void autocorrelation(int16_t amp[GSM0610_FRAME_LEN], int32_t L_ACF[9])
|
|
209 {
|
|
210 int k;
|
|
211 int16_t smax;
|
|
212 int16_t scalauto;
|
|
213 #if !(defined(__GNUC__) && defined(__i386__))
|
|
214 int i;
|
|
215 int temp;
|
|
216 int16_t *sp;
|
|
217 int16_t sl;
|
|
218 #endif
|
|
219
|
|
220 /* The goal is to compute the array L_ACF[k]. The signal s[i] must
|
|
221 be scaled in order to avoid an overflow situation. */
|
|
222
|
|
223 /* Dynamic scaling of the array s[0..159] */
|
|
224 /* Search for the maximum. */
|
|
225 #if defined(__GNUC__) && defined(__i386__)
|
|
226 smax = saturate(vec_min_maxi16(amp, GSM0610_FRAME_LEN, NULL));
|
|
227 #else
|
|
228 for (smax = 0, k = 0; k < GSM0610_FRAME_LEN; k++)
|
|
229 {
|
|
230 temp = gsm_abs(amp[k]);
|
|
231 if (temp > smax)
|
|
232 smax = temp;
|
|
233 /*endif*/
|
|
234 }
|
|
235 /*endfor*/
|
|
236 #endif
|
|
237
|
|
238 /* Computation of the scaling factor. */
|
|
239 if (smax == 0)
|
|
240 {
|
|
241 scalauto = 0;
|
|
242 }
|
|
243 else
|
|
244 {
|
|
245 assert(smax > 0);
|
|
246 scalauto = (int16_t) (4 - gsm0610_norm((int32_t) smax << 16));
|
|
247 }
|
|
248 /*endif*/
|
|
249
|
|
250 /* Scaling of the array s[0...159] */
|
|
251 #if defined(__GNUC__) && defined(__i386__)
|
|
252 if (scalauto > 0)
|
|
253 gsm0610_vec_vsraw(amp, GSM0610_FRAME_LEN, scalauto);
|
|
254 /*endif*/
|
|
255 #else
|
|
256 if (scalauto > 0)
|
|
257 {
|
|
258 for (k = 0; k < GSM0610_FRAME_LEN; k++)
|
|
259 amp[k] = gsm_mult_r(amp[k], 16384 >> (scalauto - 1));
|
|
260 /*endfor*/
|
|
261 }
|
|
262 /*endif*/
|
|
263 #endif
|
|
264
|
|
265 /* Compute the L_ACF[..]. */
|
|
266 #if defined(__GNUC__) && defined(__i386__)
|
|
267 for (k = 0; k < 9; k++)
|
|
268 L_ACF[k] = vec_dot_prodi16(amp, amp + k, GSM0610_FRAME_LEN - k) << 1;
|
|
269 /*endfor*/
|
|
270 #else
|
|
271 sp = amp;
|
|
272 sl = *sp;
|
|
273 L_ACF[0] = ((int32_t) sl*sp[0]);
|
|
274 sl = *++sp;
|
|
275 L_ACF[0] += ((int32_t) sl*sp[0]);
|
|
276 L_ACF[1] = ((int32_t) sl*sp[-1]);
|
|
277 sl = *++sp;
|
|
278 L_ACF[0] += ((int32_t) sl*sp[0]);
|
|
279 L_ACF[1] += ((int32_t) sl*sp[-1]);
|
|
280 L_ACF[2] = ((int32_t) sl*sp[-2]);
|
|
281 sl = *++sp;
|
|
282 L_ACF[0] += ((int32_t) sl*sp[0]);
|
|
283 L_ACF[1] += ((int32_t) sl*sp[-1]);
|
|
284 L_ACF[2] += ((int32_t) sl*sp[-2]);
|
|
285 L_ACF[3] = ((int32_t) sl*sp[-3]);
|
|
286 sl = *++sp;
|
|
287 L_ACF[0] += ((int32_t) sl*sp[0]);
|
|
288 L_ACF[1] += ((int32_t) sl*sp[-1]);
|
|
289 L_ACF[2] += ((int32_t) sl*sp[-2]);
|
|
290 L_ACF[3] += ((int32_t) sl*sp[-3]);
|
|
291 L_ACF[4] = ((int32_t) sl*sp[-4]);
|
|
292 sl = *++sp;
|
|
293 L_ACF[0] += ((int32_t) sl*sp[0]);
|
|
294 L_ACF[1] += ((int32_t) sl*sp[-1]);
|
|
295 L_ACF[2] += ((int32_t) sl*sp[-2]);
|
|
296 L_ACF[3] += ((int32_t) sl*sp[-3]);
|
|
297 L_ACF[4] += ((int32_t) sl*sp[-4]);
|
|
298 L_ACF[5] = ((int32_t) sl*sp[-5]);
|
|
299 sl = *++sp;
|
|
300 L_ACF[0] += ((int32_t) sl*sp[0]);
|
|
301 L_ACF[1] += ((int32_t) sl*sp[-1]);
|
|
302 L_ACF[2] += ((int32_t) sl*sp[-2]);
|
|
303 L_ACF[3] += ((int32_t) sl*sp[-3]);
|
|
304 L_ACF[4] += ((int32_t) sl*sp[-4]);
|
|
305 L_ACF[5] += ((int32_t) sl*sp[-5]);
|
|
306 L_ACF[6] = ((int32_t) sl*sp[-6]);
|
|
307 sl = *++sp;
|
|
308 L_ACF[0] += ((int32_t) sl*sp[0]);
|
|
309 L_ACF[1] += ((int32_t) sl*sp[-1]);
|
|
310 L_ACF[2] += ((int32_t) sl*sp[-2]);
|
|
311 L_ACF[3] += ((int32_t) sl*sp[-3]);
|
|
312 L_ACF[4] += ((int32_t) sl*sp[-4]);
|
|
313 L_ACF[5] += ((int32_t) sl*sp[-5]);
|
|
314 L_ACF[6] += ((int32_t) sl*sp[-6]);
|
|
315 L_ACF[7] = ((int32_t) sl*sp[-7]);
|
|
316 sl = *++sp;
|
|
317 L_ACF[0] += ((int32_t) sl*sp[0]);
|
|
318 L_ACF[1] += ((int32_t) sl*sp[-1]);
|
|
319 L_ACF[2] += ((int32_t) sl*sp[-2]);
|
|
320 L_ACF[3] += ((int32_t) sl*sp[-3]);
|
|
321 L_ACF[4] += ((int32_t) sl*sp[-4]);
|
|
322 L_ACF[5] += ((int32_t) sl*sp[-5]);
|
|
323 L_ACF[6] += ((int32_t) sl*sp[-6]);
|
|
324 L_ACF[7] += ((int32_t) sl*sp[-7]);
|
|
325 L_ACF[8] = ((int32_t) sl*sp[-8]);
|
|
326 for (i = 9; i < GSM0610_FRAME_LEN; i++)
|
|
327 {
|
|
328 sl = *++sp;
|
|
329 L_ACF[0] += ((int32_t) sl*sp[0]);
|
|
330 L_ACF[1] += ((int32_t) sl*sp[-1]);
|
|
331 L_ACF[2] += ((int32_t) sl*sp[-2]);
|
|
332 L_ACF[3] += ((int32_t) sl*sp[-3]);
|
|
333 L_ACF[4] += ((int32_t) sl*sp[-4]);
|
|
334 L_ACF[5] += ((int32_t) sl*sp[-5]);
|
|
335 L_ACF[6] += ((int32_t) sl*sp[-6]);
|
|
336 L_ACF[7] += ((int32_t) sl*sp[-7]);
|
|
337 L_ACF[8] += ((int32_t) sl*sp[-8]);
|
|
338 }
|
|
339 /*endfor*/
|
|
340 for (k = 0; k < 9; k++)
|
|
341 L_ACF[k] <<= 1;
|
|
342 /*endfor*/
|
|
343 #endif
|
|
344 /* Rescaling of the array s[0..159] */
|
|
345 if (scalauto > 0)
|
|
346 {
|
|
347 assert(scalauto <= 4);
|
|
348 for (k = 0; k < GSM0610_FRAME_LEN; k++)
|
|
349 amp[k] <<= scalauto;
|
|
350 /*endfor*/
|
|
351 }
|
|
352 /*endif*/
|
|
353 }
|
|
354 /*- End of function --------------------------------------------------------*/
|
|
355
|
|
356 /* 4.2.5 */
|
|
357 static void reflection_coefficients(int32_t L_ACF[9], int16_t r[8])
|
|
358 {
|
|
359 int i;
|
|
360 int m;
|
|
361 int n;
|
|
362 int16_t temp;
|
|
363 int16_t ACF[9];
|
|
364 int16_t P[9];
|
|
365 int16_t K[9];
|
|
366
|
|
367 /* Schur recursion with 16 bits arithmetic. */
|
|
368 if (L_ACF[0] == 0)
|
|
369 {
|
|
370 for (i = 8; i--; *r++ = 0)
|
|
371 ;
|
|
372 /*endfor*/
|
|
373 return;
|
|
374 }
|
|
375 /*endif*/
|
|
376
|
|
377 assert(L_ACF[0] != 0);
|
|
378 temp = gsm0610_norm(L_ACF[0]);
|
|
379
|
|
380 assert(temp >= 0 && temp < 32);
|
|
381
|
|
382 /* ? overflow ? */
|
|
383 for (i = 0; i <= 8; i++)
|
|
384 ACF[i] = (int16_t) ((L_ACF[i] << temp) >> 16);
|
|
385 /*endfor*/
|
|
386
|
|
387 /* Initialize array P[..] and K[..] for the recursion. */
|
|
388 for (i = 1; i <= 7; i++)
|
|
389 K[i] = ACF[i];
|
|
390 /*endfor*/
|
|
391 for (i = 0; i <= 8; i++)
|
|
392 P[i] = ACF[i];
|
|
393 /*endfor*/
|
|
394 /* Compute reflection coefficients */
|
|
395 for (n = 1; n <= 8; n++, r++)
|
|
396 {
|
|
397 temp = P[1];
|
|
398 temp = gsm_abs (temp);
|
|
399 if (P[0] < temp)
|
|
400 {
|
|
401 for (i = n; i <= 8; i++)
|
|
402 *r++ = 0;
|
|
403 /*endfor*/
|
|
404 return;
|
|
405 }
|
|
406 /*endif*/
|
|
407
|
|
408 *r = gsm_div(temp, P[0]);
|
|
409
|
|
410 assert(*r >= 0);
|
|
411 if (P[1] > 0)
|
|
412 *r = -*r; /* r[n] = sub(0, r[n]) */
|
|
413 /*endif*/
|
|
414 assert(*r != INT16_MIN);
|
|
415 if (n == 8)
|
|
416 return;
|
|
417 /*endif*/
|
|
418
|
|
419 /* Schur recursion */
|
|
420 temp = gsm_mult_r(P[1], *r);
|
|
421 P[0] = gsm_add(P[0], temp);
|
|
422
|
|
423 for (m = 1; m <= 8 - n; m++)
|
|
424 {
|
|
425 temp = gsm_mult_r(K[m], *r);
|
|
426 P[m] = gsm_add(P[m + 1], temp);
|
|
427
|
|
428 temp = gsm_mult_r(P[m + 1], *r);
|
|
429 K[m] = gsm_add(K[m], temp);
|
|
430 }
|
|
431 /*endfor*/
|
|
432 }
|
|
433 /*endfor*/
|
|
434 }
|
|
435 /*- End of function --------------------------------------------------------*/
|
|
436
|
|
437 /* 4.2.6 */
|
|
438 static void transform_to_log_area_ratios(int16_t r[8])
|
|
439 {
|
|
440 int16_t temp;
|
|
441 int i;
|
|
442
|
|
443 /* The following scaling for r[..] and LAR[..] has been used:
|
|
444
|
|
445 r[..] = integer (real_r[..]*32768.); -1 <= real_r < 1.
|
|
446 LAR[..] = integer (real_LAR[..] * 16384);
|
|
447 with -1.625 <= real_LAR <= 1.625
|
|
448 */
|
|
449
|
|
450 /* Computation of the LAR[0..7] from the r[0..7] */
|
|
451 for (i = 1; i <= 8; i++, r++)
|
|
452 {
|
|
453 temp = *r;
|
|
454 temp = gsm_abs(temp);
|
|
455 assert(temp >= 0);
|
|
456
|
|
457 if (temp < 22118)
|
|
458 {
|
|
459 temp >>= 1;
|
|
460 }
|
|
461 else if (temp < 31130)
|
|
462 {
|
|
463 assert(temp >= 11059);
|
|
464 temp -= 11059;
|
|
465 }
|
|
466 else
|
|
467 {
|
|
468 assert(temp >= 26112);
|
|
469 temp -= 26112;
|
|
470 temp <<= 2;
|
|
471 }
|
|
472 /*endif*/
|
|
473
|
|
474 *r = (*r < 0) ? -temp : temp;
|
|
475 assert(*r != INT16_MIN);
|
|
476 }
|
|
477 /*endfor*/
|
|
478 }
|
|
479 /*- End of function --------------------------------------------------------*/
|
|
480
|
|
481 /* 4.2.7 */
|
|
482 static void quantization_and_coding(int16_t LAR[8])
|
|
483 {
|
|
484 int16_t temp;
|
|
485
|
|
486 /* This procedure needs four tables; the following equations
|
|
487 give the optimum scaling for the constants:
|
|
488
|
|
489 A[0..7] = integer(real_A[0..7] * 1024)
|
|
490 B[0..7] = integer(real_B[0..7] * 512)
|
|
491 MAC[0..7] = maximum of the LARc[0..7]
|
|
492 MIC[0..7] = minimum of the LARc[0..7] */
|
|
493
|
|
494 #undef STEP
|
|
495 #define STEP(A,B,MAC,MIC) \
|
|
496 temp = gsm_mult(A, *LAR); \
|
|
497 temp = gsm_add(temp, B); \
|
|
498 temp = gsm_add(temp, 256); \
|
|
499 temp >>= 9; \
|
|
500 *LAR = (int16_t) ((temp > MAC) \
|
|
501 ? \
|
|
502 MAC - MIC \
|
|
503 : \
|
|
504 ((temp < MIC) ? 0 : temp - MIC)); \
|
|
505 LAR++;
|
|
506
|
|
507 STEP(20480, 0, 31, -32);
|
|
508 STEP(20480, 0, 31, -32);
|
|
509 STEP(20480, 2048, 15, -16);
|
|
510 STEP(20480, -2560, 15, -16);
|
|
511
|
|
512 STEP(13964, 94, 7, -8);
|
|
513 STEP(15360, -1792, 7, -8);
|
|
514 STEP( 8534, -341, 3, -4);
|
|
515 STEP( 9036, -1144, 3, -4);
|
|
516 #undef STEP
|
|
517 }
|
|
518 /*- End of function --------------------------------------------------------*/
|
|
519
|
|
520 void gsm0610_lpc_analysis(gsm0610_state_t *s,
|
|
521 int16_t amp[GSM0610_FRAME_LEN],
|
|
522 int16_t LARc[8])
|
|
523 {
|
|
524 int32_t L_ACF[9];
|
|
525
|
|
526 autocorrelation(amp, L_ACF);
|
|
527 reflection_coefficients(L_ACF, LARc);
|
|
528 transform_to_log_area_ratios(LARc);
|
|
529 quantization_and_coding(LARc);
|
|
530 }
|
|
531 /*- End of function --------------------------------------------------------*/
|
|
532 /*- End of file ------------------------------------------------------------*/
|