Mercurial > hg > audiostuff

comparison spandsp-0.0.6pre17/src/gsm0610_lpc.c @ 4:26cd8f1ef0b1

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
import spandsp-0.0.6pre17
author Peter Meerwald <pmeerw@cosy.sbg.ac.at>
date Fri, 25 Jun 2010 15:50:58 +0200
parents
children
comparison
equal deleted inserted replaced
3:c6c5a16ce2f2 4:26cd8f1ef0b1
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 Lesser General Public License version 2.1,
14 * as 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 Lesser General Public License for more details.
20 *
21 * You should have received a copy of the GNU Lesser General Public
22 * License 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.29 2009/02/05 15:57:27 steveu Exp $
29 */
30
31 /*! \file */
32
33 #if defined(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 "floating_fudge.h"
46 #include <stdlib.h>
47 #include <memory.h>
48
49 #include "spandsp/telephony.h"
50 #include "spandsp/fast_convert.h"
51 #include "spandsp/bitstream.h"
52 #include "spandsp/bit_operations.h"
53 #include "spandsp/saturated.h"
54 #include "spandsp/vector_int.h"
55 #include "spandsp/gsm0610.h"
56
57 #include "gsm0610_local.h"
58
59 /* 4.2.4 .. 4.2.7 LPC ANALYSIS SECTION */
60
61 /* The number of left shifts needed to normalize the 32 bit
62 variable x for positive values on the interval
63 with minimum of
64 minimum of 1073741824 (01000000000000000000000000000000) and
65 maximum of 2147483647 (01111111111111111111111111111111)
66 and for negative values on the interval with
67 minimum of -2147483648 (-10000000000000000000000000000000) and
68 maximum of -1073741824 ( -1000000000000000000000000000000).
69
70 In order to normalize the result, the following
71 operation must be done: norm_var1 = x << gsm0610_norm(x);
72
73 (That's 'ffs', only from the left, not the right..)
74 */
75
76 int16_t gsm0610_norm(int32_t x)
77 {
78 assert(x != 0);
79
80 if (x < 0)
81 {
82 if (x <= -1073741824)
83 return 0;
84 /*endif*/
85 x = ~x;
86 }
87 /*endif*/
88 return (int16_t) (30 - top_bit(x));
89 }
90 /*- End of function --------------------------------------------------------*/
91
92 /*
93 (From p. 46, end of section 4.2.5)
94
95 NOTE: The following lines gives [sic] one correct implementation
96 of the div(num, denum) arithmetic operation. Compute div
97 which is the integer division of num by denom: with
98 denom >= num > 0
99 */
100 static int16_t gsm_div(int16_t num, int16_t denom)
101 {
102 int32_t num32;
103 int32_t denom32;
104 int16_t div;
105 int k;
106
107 /* The parameter num sometimes becomes zero.
108 Although this is explicitly guarded against in 4.2.5,
109 we assume that the result should then be zero as well. */
110
111 assert(num >= 0 && denom >= num);
112 if (num == 0)
113 return 0;
114 /*endif*/
115 num32 = num;
116 denom32 = denom;
117 div = 0;
118 k = 15;
119 while (k--)
120 {
121 div <<= 1;
122 num32 <<= 1;
123
124 if (num32 >= denom32)
125 {
126 num32 -= denom32;
127 div++;
128 }
129 /*endif*/
130 }
131 /*endwhile*/
132
133 return div;
134 }
135 /*- End of function --------------------------------------------------------*/
136
137 #if defined(__GNUC__) && defined(SPANDSP_USE_MMX)
138 static void gsm0610_vec_vsraw(const int16_t *p, int n, int bits)
139 {
140 static const int64_t ones = 0x0001000100010001LL;
141
142 if (n == 0)
143 return;
144 /*endif*/
145 #if defined(__x86_64__)
146 __asm__ __volatile__(
147 " leaq -16(%%rsi,%%rax,2),%%rdx;\n" /* edx = top - 16 */
148 " emms;\n"
149 " movd %%ecx,%%mm3;\n"
150 " movq %[ones],%%mm2;\n"
151 " psllw %%mm3,%%mm2;\n"
152 " psrlw $1,%%mm2;\n"
153 " cmpq %%rdx,%%rsi;"
154 " ja 4f;\n"
155
156 " .p2align 2;\n"
157 /* 8 words per iteration */
158 "6:\n"
159 " movq (%%rsi),%%mm0;\n"
160 " movq 8(%%rsi),%%mm1;\n"
161 " paddsw %%mm2,%%mm0;\n"
162 " psraw %%mm3,%%mm0;\n"
163 " paddsw %%mm2,%%mm1;\n"
164 " psraw %%mm3,%%mm1;\n"
165 " movq %%mm0,(%%rsi);\n"
166 " movq %%mm1,8(%%rsi);\n"
167 " addq $16,%%rsi;\n"
168 " cmpq %%rdx,%%rsi;\n"
169 " jbe 6b;\n"
170
171 " .p2align 2;\n"
172 "4:\n"
173 " addq $12,%%rdx;\n" /* now edx = top-4 */
174 " cmpq %%rdx,%%rsi;\n"
175 " ja 3f;\n"
176
177 " .p2align 2;\n"
178 /* do up to 6 words, two per iteration */
179 "5:\n"
180 " movd (%%rsi),%%mm0;\n"
181 " paddsw %%mm2,%%mm0;\n"
182 " psraw %%mm3,%%mm0;\n"
183 " movd %%mm0,(%%rsi);\n"
184 " addq $4,%%rsi;\n"
185 " cmpq %%rdx,%%rsi;\n"
186 " jbe 5b;\n"
187
188 " .p2align 2;\n"
189 "3:\n"
190 " addq $2,%%rdx;\n" /* now edx = top-2 */
191 " cmpq %%rdx,%%rsi;\n"
192 " ja 2f;\n"
193
194 " movzwl (%%rsi),%%eax;\n"
195 " movd %%eax,%%mm0;\n"
196 " paddsw %%mm2,%%mm0;\n"
197 " psraw %%mm3,%%mm0;\n"
198 " movd %%mm0,%%eax;\n"
199 " movw %%ax,(%%rsi);\n"
200
201 " .p2align 2;\n"
202 "2:\n"
203 " emms;\n"
204 :
205 : "S" (p), "a" (n), "c" (bits), [ones] "m" (ones)
206 : "edx"
207 );
208 #else
209 __asm__ __volatile__(
210 " leal -16(%%esi,%%eax,2),%%edx;\n" /* edx = top - 16 */
211 " emms;\n"
212 " movd %%ecx,%%mm3;\n"
213 " movq %[ones],%%mm2;\n"
214 " psllw %%mm3,%%mm2;\n"
215 " psrlw $1,%%mm2;\n"
216 " cmpl %%edx,%%esi;"
217 " ja 4f;\n"
218
219 " .p2align 2;\n"
220 /* 8 words per iteration */
221 "6:\n"
222 " movq (%%esi),%%mm0;\n"
223 " movq 8(%%esi),%%mm1;\n"
224 " paddsw %%mm2,%%mm0;\n"
225 " psraw %%mm3,%%mm0;\n"
226 " paddsw %%mm2,%%mm1;\n"
227 " psraw %%mm3,%%mm1;\n"
228 " movq %%mm0,(%%esi);\n"
229 " movq %%mm1,8(%%esi);\n"
230 " addl $16,%%esi;\n"
231 " cmpl %%edx,%%esi;\n"
232 " jbe 6b;\n"
233
234 " .p2align 2;\n"
235 "4:\n"
236 " addl $12,%%edx;\n" /* now edx = top-4 */
237 " cmpl %%edx,%%esi;\n"
238 " ja 3f;\n"
239
240 " .p2align 2;\n"
241 /* do up to 6 words, two per iteration */
242 "5:\n"
243 " movd (%%esi),%%mm0;\n"
244 " paddsw %%mm2,%%mm0;\n"
245 " psraw %%mm3,%%mm0;\n"
246 " movd %%mm0,(%%esi);\n"
247 " addl $4,%%esi;\n"
248 " cmpl %%edx,%%esi;\n"
249 " jbe 5b;\n"
250
251 " .p2align 2;\n"
252 "3:\n"
253 " addl $2,%%edx;\n" /* now edx = top-2 */
254 " cmpl %%edx,%%esi;\n"
255 " ja 2f;\n"
256
257 " movzwl (%%esi),%%eax;\n"
258 " movd %%eax,%%mm0;\n"
259 " paddsw %%mm2,%%mm0;\n"
260 " psraw %%mm3,%%mm0;\n"
261 " movd %%mm0,%%eax;\n"
262 " movw %%ax,(%%esi);\n"
263
264 " .p2align 2;\n"
265 "2:\n"
266 " emms;\n"
267 :
268 : "S" (p), "a" (n), "c" (bits), [ones] "m" (ones)
269 : "edx"
270 );
271 #endif
272 }
273 /*- End of function --------------------------------------------------------*/
274 #endif
275
276 /* 4.2.4 */
277 static void autocorrelation(int16_t amp[GSM0610_FRAME_LEN], int32_t L_ACF[9])
278 {
279 int k;
280 int16_t smax;
281 int16_t scalauto;
282 #if !(defined(__GNUC__) && defined(SPANDSP_USE_MMX))
283 int i;
284 int temp;
285 int16_t *sp;
286 int16_t sl;
287 #endif
288
289 /* The goal is to compute the array L_ACF[k]. The signal s[i] must
290 be scaled in order to avoid an overflow situation. */
291
292 /* Dynamic scaling of the array s[0..159] */
293 /* Search for the maximum. */
294 #if defined(__GNUC__) && defined(SPANDSP_USE_MMX)
295 smax = saturate(vec_min_maxi16(amp, GSM0610_FRAME_LEN, NULL));
296 #else
297 for (smax = 0, k = 0; k < GSM0610_FRAME_LEN; k++)
298 {
299 temp = saturated_abs16(amp[k]);
300 if (temp > smax)
301 smax = (int16_t) temp;
302 /*endif*/
303 }
304 /*endfor*/
305 #endif
306
307 /* Computation of the scaling factor. */
308 if (smax == 0)
309 {
310 scalauto = 0;
311 }
312 else
313 {
314 assert(smax > 0);
315 scalauto = (int16_t) (4 - gsm0610_norm((int32_t) smax << 16));
316 }
317 /*endif*/
318
319 /* Scaling of the array s[0...159] */
320 #if defined(__GNUC__) && defined(SPANDSP_USE_MMX)
321 if (scalauto > 0)
322 gsm0610_vec_vsraw(amp, GSM0610_FRAME_LEN, scalauto);
323 /*endif*/
324 #else
325 if (scalauto > 0)
326 {
327 for (k = 0; k < GSM0610_FRAME_LEN; k++)
328 amp[k] = gsm_mult_r(amp[k], 16384 >> (scalauto - 1));
329 /*endfor*/
330 }
331 /*endif*/
332 #endif
333
334 /* Compute the L_ACF[..]. */
335 #if defined(__GNUC__) && defined(SPANDSP_USE_MMX)
336 for (k = 0; k < 9; k++)
337 L_ACF[k] = vec_dot_prodi16(amp, amp + k, GSM0610_FRAME_LEN - k) << 1;
338 /*endfor*/
339 #else
340 sp = amp;
341 sl = *sp;
342 L_ACF[0] = ((int32_t) sl*(int32_t) sp[0]);
343 sl = *++sp;
344 L_ACF[0] += ((int32_t) sl*(int32_t) sp[0]);
345 L_ACF[1] = ((int32_t) sl*(int32_t) sp[-1]);
346 sl = *++sp;
347 L_ACF[0] += ((int32_t) sl*(int32_t) sp[0]);
348 L_ACF[1] += ((int32_t) sl*(int32_t) sp[-1]);
349 L_ACF[2] = ((int32_t) sl*(int32_t) sp[-2]);
350 sl = *++sp;
351 L_ACF[0] += ((int32_t) sl*(int32_t) sp[0]);
352 L_ACF[1] += ((int32_t) sl*(int32_t) sp[-1]);
353 L_ACF[2] += ((int32_t) sl*(int32_t) sp[-2]);
354 L_ACF[3] = ((int32_t) sl*(int32_t) sp[-3]);
355 sl = *++sp;
356 L_ACF[0] += ((int32_t) sl*(int32_t) sp[0]);
357 L_ACF[1] += ((int32_t) sl*(int32_t) sp[-1]);
358 L_ACF[2] += ((int32_t) sl*(int32_t) sp[-2]);
359 L_ACF[3] += ((int32_t) sl*(int32_t) sp[-3]);
360 L_ACF[4] = ((int32_t) sl*(int32_t) sp[-4]);
361 sl = *++sp;
362 L_ACF[0] += ((int32_t) sl*(int32_t) sp[0]);
363 L_ACF[1] += ((int32_t) sl*(int32_t) sp[-1]);
364 L_ACF[2] += ((int32_t) sl*(int32_t) sp[-2]);
365 L_ACF[3] += ((int32_t) sl*(int32_t) sp[-3]);
366 L_ACF[4] += ((int32_t) sl*(int32_t) sp[-4]);
367 L_ACF[5] = ((int32_t) sl*(int32_t) sp[-5]);
368 sl = *++sp;
369 L_ACF[0] += ((int32_t) sl*(int32_t) sp[0]);
370 L_ACF[1] += ((int32_t) sl*(int32_t) sp[-1]);
371 L_ACF[2] += ((int32_t) sl*(int32_t) sp[-2]);
372 L_ACF[3] += ((int32_t) sl*(int32_t) sp[-3]);
373 L_ACF[4] += ((int32_t) sl*(int32_t) sp[-4]);
374 L_ACF[5] += ((int32_t) sl*(int32_t) sp[-5]);
375 L_ACF[6] = ((int32_t) sl*(int32_t) sp[-6]);
376 sl = *++sp;
377 L_ACF[0] += ((int32_t) sl*(int32_t) sp[0]);
378 L_ACF[1] += ((int32_t) sl*(int32_t) sp[-1]);
379 L_ACF[2] += ((int32_t) sl*(int32_t) sp[-2]);
380 L_ACF[3] += ((int32_t) sl*(int32_t) sp[-3]);
381 L_ACF[4] += ((int32_t) sl*(int32_t) sp[-4]);
382 L_ACF[5] += ((int32_t) sl*(int32_t) sp[-5]);
383 L_ACF[6] += ((int32_t) sl*(int32_t) sp[-6]);
384 L_ACF[7] = ((int32_t) sl*(int32_t) sp[-7]);
385 sl = *++sp;
386 L_ACF[0] += ((int32_t) sl*(int32_t) sp[0]);
387 L_ACF[1] += ((int32_t) sl*(int32_t) sp[-1]);
388 L_ACF[2] += ((int32_t) sl*(int32_t) sp[-2]);
389 L_ACF[3] += ((int32_t) sl*(int32_t) sp[-3]);
390 L_ACF[4] += ((int32_t) sl*(int32_t) sp[-4]);
391 L_ACF[5] += ((int32_t) sl*(int32_t) sp[-5]);
392 L_ACF[6] += ((int32_t) sl*(int32_t) sp[-6]);
393 L_ACF[7] += ((int32_t) sl*(int32_t) sp[-7]);
394 L_ACF[8] = ((int32_t) sl*(int32_t) sp[-8]);
395 for (i = 9; i < GSM0610_FRAME_LEN; i++)
396 {
397 sl = *++sp;
398 L_ACF[0] += ((int32_t) sl*(int32_t) sp[0]);
399 L_ACF[1] += ((int32_t) sl*(int32_t) sp[-1]);
400 L_ACF[2] += ((int32_t) sl*(int32_t) sp[-2]);
401 L_ACF[3] += ((int32_t) sl*(int32_t) sp[-3]);
402 L_ACF[4] += ((int32_t) sl*(int32_t) sp[-4]);
403 L_ACF[5] += ((int32_t) sl*(int32_t) sp[-5]);
404 L_ACF[6] += ((int32_t) sl*(int32_t) sp[-6]);
405 L_ACF[7] += ((int32_t) sl*(int32_t) sp[-7]);
406 L_ACF[8] += ((int32_t) sl*(int32_t) sp[-8]);
407 }
408 /*endfor*/
409 for (k = 0; k < 9; k++)
410 L_ACF[k] <<= 1;
411 /*endfor*/
412 #endif
413 /* Rescaling of the array s[0..159] */
414 if (scalauto > 0)
415 {
416 assert(scalauto <= 4);
417 for (k = 0; k < GSM0610_FRAME_LEN; k++)
418 amp[k] <<= scalauto;
419 /*endfor*/
420 }
421 /*endif*/
422 }
423 /*- End of function --------------------------------------------------------*/
424
425 /* 4.2.5 */
426 static void reflection_coefficients(int32_t L_ACF[9], int16_t r[8])
427 {
428 int i;
429 int m;
430 int n;
431 int16_t temp;
432 int16_t ACF[9];
433 int16_t P[9];
434 int16_t K[9];
435
436 /* Schur recursion with 16 bits arithmetic. */
437 if (L_ACF[0] == 0)
438 {
439 for (i = 8; i--; *r++ = 0)
440 ;
441 /*endfor*/
442 return;
443 }
444 /*endif*/
445
446 assert(L_ACF[0] != 0);
447 temp = gsm0610_norm(L_ACF[0]);
448
449 assert(temp >= 0 && temp < 32);
450
451 /* ? overflow ? */
452 for (i = 0; i <= 8; i++)
453 ACF[i] = (int16_t) ((L_ACF[i] << temp) >> 16);
454 /*endfor*/
455
456 /* Initialize array P[..] and K[..] for the recursion. */
457 for (i = 1; i <= 7; i++)
458 K[i] = ACF[i];
459 /*endfor*/
460 for (i = 0; i <= 8; i++)
461 P[i] = ACF[i];
462 /*endfor*/
463 /* Compute reflection coefficients */
464 for (n = 1; n <= 8; n++, r++)
465 {
466 temp = P[1];
467 temp = saturated_abs16(temp);
468 if (P[0] < temp)
469 {
470 for (i = n; i <= 8; i++)
471 *r++ = 0;
472 /*endfor*/
473 return;
474 }
475 /*endif*/
476
477 *r = gsm_div(temp, P[0]);
478
479 assert(*r >= 0);
480 if (P[1] > 0)
481 *r = -*r; /* r[n] = sub(0, r[n]) */
482 /*endif*/
483 assert(*r != INT16_MIN);
484 if (n == 8)
485 return;
486 /*endif*/
487
488 /* Schur recursion */
489 temp = gsm_mult_r(P[1], *r);
490 P[0] = saturated_add16(P[0], temp);
491
492 for (m = 1; m <= 8 - n; m++)
493 {
494 temp = gsm_mult_r(K[m], *r);
495 P[m] = saturated_add16(P[m + 1], temp);
496
497 temp = gsm_mult_r(P[m + 1], *r);
498 K[m] = saturated_add16(K[m], temp);
499 }
500 /*endfor*/
501 }
502 /*endfor*/
503 }
504 /*- End of function --------------------------------------------------------*/
505
506 /* 4.2.6 */
507 static void transform_to_log_area_ratios(int16_t r[8])
508 {
509 int16_t temp;
510 int i;
511
512 /* The following scaling for r[..] and LAR[..] has been used:
513
514 r[..] = integer (real_r[..]*32768.); -1 <= real_r < 1.
515 LAR[..] = integer (real_LAR[..] * 16384);
516 with -1.625 <= real_LAR <= 1.625
517 */
518
519 /* Computation of the LAR[0..7] from the r[0..7] */
520 for (i = 1; i <= 8; i++, r++)
521 {
522 temp = saturated_abs16(*r);
523 assert(temp >= 0);
524
525 if (temp < 22118)
526 {
527 temp >>= 1;
528 }
529 else if (temp < 31130)
530 {
531 assert(temp >= 11059);
532 temp -= 11059;
533 }
534 else
535 {
536 assert(temp >= 26112);
537 temp -= 26112;
538 temp <<= 2;
539 }
540 /*endif*/
541
542 *r = (*r < 0) ? -temp : temp;
543 assert(*r != INT16_MIN);
544 }
545 /*endfor*/
546 }
547 /*- End of function --------------------------------------------------------*/
548
549 /* 4.2.7 */
550 static void quantization_and_coding(int16_t LAR[8])
551 {
552 int16_t temp;
553
554 /* This procedure needs four tables; the following equations
555 give the optimum scaling for the constants:
556
557 A[0..7] = integer(real_A[0..7] * 1024)
558 B[0..7] = integer(real_B[0..7] * 512)
559 MAC[0..7] = maximum of the LARc[0..7]
560 MIC[0..7] = minimum of the LARc[0..7] */
561
562 #undef STEP
563 #define STEP(A,B,MAC,MIC) \
564 temp = saturated_mul16(A, *LAR); \
565 temp = saturated_add16(temp, B); \
566 temp = saturated_add16(temp, 256); \
567 temp >>= 9; \
568 *LAR = (int16_t) ((temp > MAC) \
569 ? \
570 MAC - MIC \
571 : \
572 ((temp < MIC) ? 0 : temp - MIC)); \
573 LAR++;
574
575 STEP(20480, 0, 31, -32);
576 STEP(20480, 0, 31, -32);
577 STEP(20480, 2048, 15, -16);
578 STEP(20480, -2560, 15, -16);
579
580 STEP(13964, 94, 7, -8);
581 STEP(15360, -1792, 7, -8);
582 STEP( 8534, -341, 3, -4);
583 STEP( 9036, -1144, 3, -4);
584 #undef STEP
585 }
586 /*- End of function --------------------------------------------------------*/
587
588 void gsm0610_lpc_analysis(gsm0610_state_t *s,
589 int16_t amp[GSM0610_FRAME_LEN],
590 int16_t LARc[8])
591 {
592 int32_t L_ACF[9];
593
594 autocorrelation(amp, L_ACF);
595 reflection_coefficients(L_ACF, LARc);
596 transform_to_log_area_ratios(LARc);
597 quantization_and_coding(LARc);
598 }
599 /*- End of function --------------------------------------------------------*/
600 /*- End of file ------------------------------------------------------------*/

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