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