Mercurial > hg > audiostuff

comparison spandsp-0.0.6pre17/src/gsm0610_rpe.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_rpe.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_rpe.c,v 1.25.4.2 2009/12/28 11:54:58 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
48 #include "mmx_sse_decs.h"
49
50 #include "spandsp/telephony.h"
51 #include "spandsp/fast_convert.h"
52 #include "spandsp/bitstream.h"
53 #include "spandsp/saturated.h"
54 #include "spandsp/gsm0610.h"
55
56 #include "gsm0610_local.h"
57
58 /* 4.2.13 .. 4.2.17 RPE ENCODING SECTION */
59
60 /* 4.2.13 */
61 static void weighting_filter(int16_t x[40],
62 const int16_t *e) // signal [-5..0.39.44] IN)
63 {
64 #if defined(__GNUC__) && defined(SPANDSP_USE_MMX) && defined(__x86_64__)
65 /* Table 4.4 Coefficients of the weighting filter */
66 /* This must be padded to a multiple of 4 for MMX to work */
67 static const union
68 {
69 int16_t gsm_H[12];
70 __m64 x[3];
71 } gsm_H =
72 {
73 {
74 -134, -374, 0, 2054, 5741, 8192, 5741, 2054, 0, -374, -134, 0
75 }
76
77 };
78
79 __asm__ __volatile__(
80 " emms;\n"
81 " addq $-10,%%rcx;\n"
82 " leaq %[gsm_H],%%rax;\n"
83 " movq (%%rax),%%mm1;\n"
84 " movq 8(%%rax),%%mm2;\n"
85 " movq 16(%%rax),%%mm3;\n"
86 " movq $0x1000,%%rax;\n"
87 " movq %%rax,%%mm5;\n" /* For rounding */
88 " xorq %%rsi,%%rsi;\n"
89 " .p2align 2;\n"
90 "1:\n"
91 " movq (%%rcx,%%rsi,2),%%mm0;\n"
92 " pmaddwd %%mm1,%%mm0;\n"
93
94 " movq 8(%%rcx,%%rsi,2),%%mm4;\n"
95 " pmaddwd %%mm2,%%mm4;\n"
96 " paddd %%mm4,%%mm0;\n"
97
98 " movq 16(%%rcx,%%rsi,2),%%mm4;\n"
99 " pmaddwd %%mm3,%%mm4;\n"
100 " paddd %%mm4,%%mm0;\n"
101
102 " movq %%mm0,%%mm4;\n"
103 " punpckhdq %%mm0,%%mm4;\n" /* mm4 has high int32 of mm0 dup'd */
104 " paddd %%mm4,%%mm0;\n"
105
106 " paddd %%mm5,%%mm0;\n" /* Add for roundoff */
107 " psrad $13,%%mm0;\n"
108 " packssdw %%mm0,%%mm0;\n"
109 " movd %%mm0,%%eax;\n" /* eax has result */
110 " movw %%ax,(%%rdi,%%rsi,2);\n"
111 " incq %%rsi;\n"
112 " cmpq $39,%%rsi;\n"
113 " jle 1b;\n"
114 " emms;\n"
115 :
116 : "c" (e), "D" (x), [gsm_H] "X" (gsm_H)
117 : "rax", "rdx", "rsi", "memory"
118 );
119 #elif defined(__GNUC__) && defined(SPANDSP_USE_MMX) && defined(__i386__)
120 /* Table 4.4 Coefficients of the weighting filter */
121 /* This must be padded to a multiple of 4 for MMX to work */
122 static const union
123 {
124 int16_t gsm_H[12];
125 __m64 x[3];
126 } gsm_H =
127 {
128 {
129 -134, -374, 0, 2054, 5741, 8192, 5741, 2054, 0, -374, -134, 0
130 }
131
132 };
133
134 __asm__ __volatile__(
135 " emms;\n"
136 " addl $-10,%%ecx;\n"
137 " leal %[gsm_H],%%eax;\n"
138 " movq (%%eax),%%mm1;\n"
139 " movq 8(%%eax),%%mm2;\n"
140 " movq 16(%%eax),%%mm3;\n"
141 " movl $0x1000,%%eax;\n"
142 " movd %%eax,%%mm5;\n" /* For rounding */
143 " xorl %%esi,%%esi;\n"
144 " .p2align 2;\n"
145 "1:\n"
146 " movq (%%ecx,%%esi,2),%%mm0;\n"
147 " pmaddwd %%mm1,%%mm0;\n"
148
149 " movq 8(%%ecx,%%esi,2),%%mm4;\n"
150 " pmaddwd %%mm2,%%mm4;\n"
151 " paddd %%mm4,%%mm0;\n"
152
153 " movq 16(%%ecx,%%esi,2),%%mm4;\n"
154 " pmaddwd %%mm3,%%mm4;\n"
155 " paddd %%mm4,%%mm0;\n"
156
157 " movq %%mm0,%%mm4;\n"
158 " punpckhdq %%mm0,%%mm4;\n" /* mm4 has high int32 of mm0 dup'd */
159 " paddd %%mm4,%%mm0;\n"
160
161 " paddd %%mm5,%%mm0;\n" /* Add for roundoff */
162 " psrad $13,%%mm0;\n"
163 " packssdw %%mm0,%%mm0;\n"
164 " movd %%mm0,%%eax;\n" /* eax has result */
165 " movw %%ax,(%%edi,%%esi,2);\n"
166 " incl %%esi;\n"
167 " cmpl $39,%%esi;\n"
168 " jle 1b;\n"
169 " emms;\n"
170 :
171 : "c" (e), "D" (x), [gsm_H] "X" (gsm_H)
172 : "eax", "edx", "esi", "memory"
173 );
174 #else
175 int32_t L_result;
176 int k;
177
178 /* The coefficients of the weighting filter are stored in a table
179 (see table 4.4). The following scaling is used:
180
181 H[0..10] = integer(real_H[0..10] * 8192);
182 */
183 /* Initialization of a temporary working array wt[0...49] */
184
185 /* for (k = 0; k <= 4; k++) wt[k] = 0;
186 * for (k = 5; k <= 44; k++) wt[k] = *e++;
187 * for (k = 45; k <= 49; k++) wt[k] = 0;
188 *
189 * (e[-5..-1] and e[40..44] are allocated by the caller,
190 * are initially zero and are not written anywhere.)
191 */
192 e -= 5;
193
194 /* Compute the signal x[0..39] */
195 for (k = 0; k < 40; k++)
196 {
197 L_result = 8192 >> 1;
198
199 /* for (i = 0; i <= 10; i++)
200 * {
201 * L_temp = saturated_mul_16_32(wt[k + i], gsm_H[i]);
202 * L_result = saturated_add32(L_result, L_temp);
203 * }
204 */
205
206 #undef STEP
207 #define STEP(i,H) (e[k + i] * (int32_t) H)
208
209 /* Every one of these multiplications is done twice,
210 but I don't see an elegant way to optimize this.
211 Do you?
212 */
213 L_result += STEP( 0, -134);
214 L_result += STEP( 1, -374);
215 /* += STEP( 2, 0 ); */
216 L_result += STEP( 3, 2054);
217 L_result += STEP( 4, 5741);
218 L_result += STEP( 5, 8192);
219 L_result += STEP( 6, 5741);
220 L_result += STEP( 7, 2054);
221 /* += STEP( 8, 0 ); */
222 L_result += STEP( 9, -374);
223 L_result += STEP(10, -134);
224
225 /* 2 adds vs. >> 16 => 14, minus one shift to compensate for
226 those we lost when replacing L_MULT by '*'. */
227 L_result >>= 13;
228 x[k] = saturate(L_result);
229 }
230 /*endfor*/
231 #endif
232 }
233 /*- End of function --------------------------------------------------------*/
234
235 /* 4.2.14 */
236 static void rpe_grid_selection(int16_t x[40], int16_t xM[13], int16_t *Mc_out)
237 {
238 int i;
239 int32_t L_result;
240 int32_t L_temp;
241 int32_t EM; /* xxx should be L_EM? */
242 int16_t Mc;
243 int32_t L_common_0_3;
244
245 /* The signal x[0..39] is used to select the RPE grid which is
246 represented by Mc. */
247
248 EM = 0;
249 Mc = 0;
250
251 #undef STEP
252 #define STEP(m,i) \
253 L_temp = x[m + 3*i] >> 2; \
254 L_result += L_temp*L_temp;
255
256 /* Common part of 0 and 3 */
257 L_result = 0;
258 STEP(0, 1);
259 STEP(0, 2);
260 STEP(0, 3);
261 STEP(0, 4);
262 STEP(0, 5);
263 STEP(0, 6);
264 STEP(0, 7);
265 STEP(0, 8);
266 STEP(0, 9);
267 STEP(0, 10);
268 STEP(0, 11);
269 STEP(0, 12);
270 L_common_0_3 = L_result;
271
272 /* i = 0 */
273
274 STEP(0, 0);
275 L_result <<= 1; /* implicit in L_MULT */
276 EM = L_result;
277
278 /* i = 1 */
279
280 L_result = 0;
281 STEP(1, 0);
282 STEP(1, 1);
283 STEP(1, 2);
284 STEP(1, 3);
285 STEP(1, 4);
286 STEP(1, 5);
287 STEP(1, 6);
288 STEP(1, 7);
289 STEP(1, 8);
290 STEP(1, 9);
291 STEP(1, 10);
292 STEP(1, 11);
293 STEP(1, 12);
294 L_result <<= 1;
295 if (L_result > EM)
296 {
297 Mc = 1;
298 EM = L_result;
299 }
300 /*endif*/
301
302 /* i = 2 */
303
304 L_result = 0;
305 STEP(2, 0);
306 STEP(2, 1);
307 STEP(2, 2);
308 STEP(2, 3);
309 STEP(2, 4);
310 STEP(2, 5);
311 STEP(2, 6);
312 STEP(2, 7);
313 STEP(2, 8);
314 STEP(2, 9);
315 STEP(2, 10);
316 STEP(2, 11);
317 STEP(2, 12);
318 L_result <<= 1;
319 if (L_result > EM)
320 {
321 Mc = 2;
322 EM = L_result;
323 }
324 /*endif*/
325
326 /* i = 3 */
327
328 L_result = L_common_0_3;
329 STEP(3, 12);
330 L_result <<= 1;
331 if (L_result > EM)
332 {
333 Mc = 3;
334 EM = L_result;
335 }
336 /*endif*/
337
338 /* Down-sampling by a factor 3 to get the selected xM[0..12]
339 RPE sequence. */
340 for (i = 0; i < 13; i++)
341 xM[i] = x[Mc + 3*i];
342 /*endfor*/
343 *Mc_out = Mc;
344 }
345 /*- End of function --------------------------------------------------------*/
346
347 /* 4.12.15 */
348 static void apcm_quantization_xmaxc_to_exp_mant(int16_t xmaxc,
349 int16_t *exp_out,
350 int16_t *mant_out)
351 {
352 int16_t exp;
353 int16_t mant;
354
355 /* Compute exponent and mantissa of the decoded version of xmaxc */
356 exp = 0;
357 if (xmaxc > 15)
358 exp = (int16_t) ((xmaxc >> 3) - 1);
359 /*endif*/
360 mant = xmaxc - (exp << 3);
361
362 if (mant == 0)
363 {
364 exp = -4;
365 mant = 7;
366 }
367 else
368 {
369 while (mant <= 7)
370 {
371 mant = (int16_t) (mant << 1 | 1);
372 exp--;
373 }
374 /*endwhile*/
375 mant -= 8;
376 }
377 /*endif*/
378
379 assert(exp >= -4 && exp <= 6);
380 assert(mant >= 0 && mant <= 7);
381
382 *exp_out = exp;
383 *mant_out = mant;
384 }
385 /*- End of function --------------------------------------------------------*/
386
387 static void apcm_quantization(int16_t xM[13],
388 int16_t xMc[13],
389 int16_t *mant_out,
390 int16_t *exp_out,
391 int16_t *xmaxc_out)
392 {
393 /* Table 4.5 Normalized inverse mantissa used to compute xM/xmax */
394 static const int16_t gsm_NRFAC[8] =
395 {
396 29128, 26215, 23832, 21846, 20165, 18725, 17476, 16384
397 };
398 int i;
399 int itest;
400 int16_t xmax;
401 int16_t xmaxc;
402 int16_t temp;
403 int16_t temp1;
404 int16_t temp2;
405 int16_t exp;
406 int16_t mant;
407
408 /* Find the maximum absolute value xmax of xM[0..12]. */
409 xmax = 0;
410 for (i = 0; i < 13; i++)
411 {
412 temp = xM[i];
413 temp = saturated_abs16(temp);
414 if (temp > xmax)
415 xmax = temp;
416 /*endif*/
417 }
418 /*endfor*/
419
420 /* Quantizing and coding of xmax to get xmaxc. */
421 exp = 0;
422 temp = xmax >> 9;
423 itest = 0;
424
425 for (i = 0; i <= 5; i++)
426 {
427 itest |= (temp <= 0);
428 temp >>= 1;
429
430 assert(exp <= 5);
431 if (itest == 0)
432 exp++;
433 /*endif*/
434 }
435 /*endfor*/
436
437 assert(exp <= 6 && exp >= 0);
438 temp = (int16_t) (exp + 5);
439
440 assert(temp <= 11 && temp >= 0);
441 xmaxc = saturated_add16((xmax >> temp), exp << 3);
442
443 /* Quantizing and coding of the xM[0..12] RPE sequence
444 to get the xMc[0..12] */
445 apcm_quantization_xmaxc_to_exp_mant(xmaxc, &exp, &mant);
446
447 /* This computation uses the fact that the decoded version of xmaxc
448 can be calculated by using the exponent and the mantissa part of
449 xmaxc (logarithmic table).
450 So, this method avoids any division and uses only a scaling
451 of the RPE samples by a function of the exponent. A direct
452 multiplication by the inverse of the mantissa (NRFAC[0..7]
453 found in table 4.5) gives the 3 bit coded version xMc[0..12]
454 of the RPE samples.
455 */
456 /* Direct computation of xMc[0..12] using table 4.5 */
457 assert(exp <= 4096 && exp >= -4096);
458 assert(mant >= 0 && mant <= 7);
459
460 temp1 = (int16_t) (6 - exp); /* Normalization by the exponent */
461 temp2 = gsm_NRFAC[mant]; /* Inverse mantissa */
462
463 for (i = 0; i < 13; i++)
464 {
465 assert(temp1 >= 0 && temp1 < 16);
466
467 temp = xM[i] << temp1;
468 temp = saturated_mul16(temp, temp2);
469 temp >>= 12;
470 xMc[i] = (int16_t) (temp + 4); /* See note below */
471 }
472 /*endfor*/
473
474 /* NOTE: This equation is used to make all the xMc[i] positive. */
475 *mant_out = mant;
476 *exp_out = exp;
477 *xmaxc_out = xmaxc;
478 }
479 /*- End of function --------------------------------------------------------*/
480
481 /* 4.2.16 */
482 static void apcm_inverse_quantization(int16_t xMc[13],
483 int16_t mant,
484 int16_t exp,
485 int16_t xMp[13])
486 {
487 /* Table 4.6 Normalized direct mantissa used to compute xM/xmax */
488 static const int16_t gsm_FAC[8] =
489 {
490 18431, 20479, 22527, 24575, 26623, 28671, 30719, 32767
491 };
492 int i;
493 int16_t temp;
494 int16_t temp1;
495 int16_t temp2;
496 int16_t temp3;
497
498 /* This part is for decoding the RPE sequence of coded xMc[0..12]
499 samples to obtain the xMp[0..12] array. Table 4.6 is used to get
500 the mantissa of xmaxc (FAC[0..7]).
501 */
502 #if 0
503 assert(mant >= 0 && mant <= 7);
504 #endif
505
506 temp1 = gsm_FAC[mant]; /* See 4.2-15 for mant */
507 temp2 = saturated_sub16(6, exp); /* See 4.2-15 for exp */
508 temp3 = gsm_asl(1, saturated_sub16(temp2, 1));
509
510 for (i = 0; i < 13; i++)
511 {
512 assert(xMc[i] >= 0 && xMc[i] <= 7); /* 3 bit unsigned */
513
514 temp = (int16_t) ((xMc[i] << 1) - 7); /* Restore sign */
515 assert(temp <= 7 && temp >= -7); /* 4 bit signed */
516
517 temp <<= 12; /* 16 bit signed */
518 temp = gsm_mult_r(temp1, temp);
519 temp = saturated_add16(temp, temp3);
520 xMp[i] = gsm_asr(temp, temp2);
521 }
522 /*endfor*/
523 }
524 /*- End of function --------------------------------------------------------*/
525
526 /* 4.2.17 */
527 static void rpe_grid_positioning(int16_t Mc,
528 int16_t xMp[13],
529 int16_t ep[40])
530 {
531 int i = 13;
532
533 /* This procedure computes the reconstructed long term residual signal
534 ep[0..39] for the LTP analysis filter. The inputs are the Mc
535 which is the grid position selection and the xMp[0..12] decoded
536 RPE samples which are upsampled by a factor of 3 by inserting zero
537 values.
538 */
539 assert(0 <= Mc && Mc <= 3);
540
541 switch (Mc)
542 {
543 case 3:
544 *ep++ = 0;
545 case 2:
546 do
547 {
548 *ep++ = 0;
549 case 1:
550 *ep++ = 0;
551 case 0:
552 *ep++ = *xMp++;
553 }
554 while (--i);
555 }
556 /*endswitch*/
557 while (++Mc < 4)
558 *ep++ = 0;
559 /*endwhile*/
560 }
561 /*- End of function --------------------------------------------------------*/
562
563 void gsm0610_rpe_encoding(gsm0610_state_t *s,
564 int16_t *e, // [-5..-1][0..39][40..44]
565 int16_t *xmaxc,
566 int16_t *Mc,
567 int16_t xMc[13])
568 {
569 int16_t x[40];
570 int16_t xM[13];
571 int16_t xMp[13];
572 int16_t mant;
573 int16_t exp;
574
575 weighting_filter(x, e);
576 rpe_grid_selection(x, xM, Mc);
577
578 apcm_quantization(xM, xMc, &mant, &exp, xmaxc);
579 apcm_inverse_quantization(xMc, mant, exp, xMp);
580
581 rpe_grid_positioning(*Mc, xMp, e);
582 }
583 /*- End of function --------------------------------------------------------*/
584
585 void gsm0610_rpe_decoding(gsm0610_state_t *s,
586 int16_t xmaxc,
587 int16_t Mcr,
588 int16_t xMcr[13],
589 int16_t erp[40])
590 {
591 int16_t exp;
592 int16_t mant;
593 int16_t xMp[13];
594
595 apcm_quantization_xmaxc_to_exp_mant(xmaxc, &exp, &mant);
596 apcm_inverse_quantization(xMcr, mant, exp, xMp);
597 rpe_grid_positioning(Mcr, xMp, erp);
598 }
599 /*- End of function --------------------------------------------------------*/
600 /*- End of file ------------------------------------------------------------*/

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