Mercurial > hg > audiostuff

comparison spandsp-0.0.3/spandsp-0.0.3/src/gsm0610_lpc.c @ 5:f762bf195c4b

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
import spandsp-0.0.3
author Peter Meerwald <pmeerw@cosy.sbg.ac.at>
date Fri, 25 Jun 2010 16:00:21 +0200
parents
children
comparison
equal deleted inserted replaced
4:26cd8f1ef0b1 5:f762bf195c4b
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 ------------------------------------------------------------*/

Repositories maintained by Peter Meerwald, pmeerw@pmeerw.net.