Mercurial > hg > audiostuff

comparison spandsp-0.0.6pre17/src/g722.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 * g722.c - The ITU G.722 codec.
5 *
6 * Written by Steve Underwood <steveu@coppice.org>
7 *
8 * Copyright (C) 2005 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 * Based in part on a single channel G.722 codec which is:
26 *
27 * Copyright (c) CMU 1993
28 * Computer Science, Speech Group
29 * Chengxiang Lu and Alex Hauptmann
30 *
31 * $Id: g722.c,v 1.10 2009/04/22 12:57:40 steveu Exp $
32 */
33
34 /*! \file */
35
36 #if defined(HAVE_CONFIG_H)
37 #include "config.h"
38 #endif
39
40 #include <inttypes.h>
41 #include <memory.h>
42 #include <stdlib.h>
43 #if defined(HAVE_TGMATH_H)
44 #include <tgmath.h>
45 #endif
46 #if defined(HAVE_MATH_H)
47 #include <math.h>
48 #endif
49 #include "floating_fudge.h"
50
51 #include "spandsp/telephony.h"
52 #include "spandsp/fast_convert.h"
53 #include "spandsp/saturated.h"
54 #include "spandsp/vector_int.h"
55 #include "spandsp/g722.h"
56
57 #include "spandsp/private/g722.h"
58
59 static const int16_t qmf_coeffs_fwd[12] =
60 {
61 3, -11, 12, 32, -210, 951, 3876, -805, 362, -156, 53, -11,
62 };
63
64 static const int16_t qmf_coeffs_rev[12] =
65 {
66 -11, 53, -156, 362, -805, 3876, 951, -210, 32, 12, -11, 3
67 };
68
69 static const int16_t qm2[4] =
70 {
71 -7408, -1616, 7408, 1616
72 };
73
74 static const int16_t qm4[16] =
75 {
76 0, -20456, -12896, -8968,
77 -6288, -4240, -2584, -1200,
78 20456, 12896, 8968, 6288,
79 4240, 2584, 1200, 0
80 };
81
82 static const int16_t qm5[32] =
83 {
84 -280, -280, -23352, -17560,
85 -14120, -11664, -9752, -8184,
86 -6864, -5712, -4696, -3784,
87 -2960, -2208, -1520, -880,
88 23352, 17560, 14120, 11664,
89 9752, 8184, 6864, 5712,
90 4696, 3784, 2960, 2208,
91 1520, 880, 280, -280
92 };
93
94 static const int16_t qm6[64] =
95 {
96 -136, -136, -136, -136,
97 -24808, -21904, -19008, -16704,
98 -14984, -13512, -12280, -11192,
99 -10232, -9360, -8576, -7856,
100 -7192, -6576, -6000, -5456,
101 -4944, -4464, -4008, -3576,
102 -3168, -2776, -2400, -2032,
103 -1688, -1360, -1040, -728,
104 24808, 21904, 19008, 16704,
105 14984, 13512, 12280, 11192,
106 10232, 9360, 8576, 7856,
107 7192, 6576, 6000, 5456,
108 4944, 4464, 4008, 3576,
109 3168, 2776, 2400, 2032,
110 1688, 1360, 1040, 728,
111 432, 136, -432, -136
112 };
113
114 static const int16_t q6[32] =
115 {
116 0, 35, 72, 110,
117 150, 190, 233, 276,
118 323, 370, 422, 473,
119 530, 587, 650, 714,
120 786, 858, 940, 1023,
121 1121, 1219, 1339, 1458,
122 1612, 1765, 1980, 2195,
123 2557, 2919, 0, 0
124 };
125
126 static const int16_t ilb[32] =
127 {
128 2048, 2093, 2139, 2186,
129 2233, 2282, 2332, 2383,
130 2435, 2489, 2543, 2599,
131 2656, 2714, 2774, 2834,
132 2896, 2960, 3025, 3091,
133 3158, 3228, 3298, 3371,
134 3444, 3520, 3597, 3676,
135 3756, 3838, 3922, 4008
136 };
137
138 static const int16_t iln[32] =
139 {
140 0, 63, 62, 31, 30, 29, 28, 27,
141 26, 25, 24, 23, 22, 21, 20, 19,
142 18, 17, 16, 15, 14, 13, 12, 11,
143 10, 9, 8, 7, 6, 5, 4, 0
144 };
145
146 static const int16_t ilp[32] =
147 {
148 0, 61, 60, 59, 58, 57, 56, 55,
149 54, 53, 52, 51, 50, 49, 48, 47,
150 46, 45, 44, 43, 42, 41, 40, 39,
151 38, 37, 36, 35, 34, 33, 32, 0
152 };
153
154 static const int16_t ihn[3] =
155 {
156 0, 1, 0
157 };
158
159 static const int16_t ihp[3] =
160 {
161 0, 3, 2
162 };
163
164 static const int16_t wl[8] =
165 {
166 -60, -30, 58, 172, 334, 538, 1198, 3042
167 };
168
169 static const int16_t rl42[16] =
170 {
171 0, 7, 6, 5, 4, 3, 2, 1,
172 7, 6, 5, 4, 3, 2, 1, 0
173 };
174
175 static const int16_t wh[3] =
176 {
177 0, -214, 798
178 };
179
180 static const int16_t rh2[4] =
181 {
182 2, 1, 2, 1
183 };
184
185 static void block4(g722_band_t *s, int16_t dx)
186 {
187 int16_t wd1;
188 int16_t wd2;
189 int16_t wd3;
190 int16_t sp;
191 int16_t r;
192 int16_t p;
193 int16_t ap[2];
194 int32_t wd32;
195 int32_t sz;
196 int i;
197
198 /* RECONS */
199 r = saturated_add16(s->s, dx);
200 /* PARREC */
201 p = saturated_add16(s->sz, dx);
202
203 /* UPPOL2 */
204 wd1 = saturate((int32_t) s->a[0] << 2);
205 wd32 = ((p ^ s->p[0]) & 0x8000) ? wd1 : -wd1;
206 if (wd32 > 32767)
207 wd32 = 32767;
208 wd3 = (int16_t) ((((p ^ s->p[1]) & 0x8000) ? -128 : 128)
209 + (wd32 >> 7)
210 + (((int32_t) s->a[1]*(int32_t) 32512) >> 15));
211 if (abs(wd3) > 12288)
212 wd3 = (wd3 < 0) ? -12288 : 12288;
213 ap[1] = wd3;
214
215 /* UPPOL1 */
216 wd1 = ((p ^ s->p[0]) & 0x8000) ? -192 : 192;
217 wd2 = (int16_t) (((int32_t) s->a[0]*(int32_t) 32640) >> 15);
218 ap[0] = saturated_add16(wd1, wd2);
219
220 wd3 = saturated_sub16(15360, ap[1]);
221 if (abs(ap[0]) > wd3)
222 ap[0] = (ap[0] < 0) ? -wd3 : wd3;
223
224 /* FILTEP */
225 wd1 = saturated_add16(r, r);
226 wd1 = (int16_t) (((int32_t) ap[0]*(int32_t) wd1) >> 15);
227 wd2 = saturated_add16(s->r, s->r);
228 wd2 = (int16_t) (((int32_t) ap[1]*(int32_t) wd2) >> 15);
229 sp = saturated_add16(wd1, wd2);
230 s->r = r;
231 s->a[1] = ap[1];
232 s->a[0] = ap[0];
233 s->p[1] = s->p[0];
234 s->p[0] = p;
235
236 /* UPZERO */
237 /* DELAYA */
238 /* FILTEZ */
239 wd1 = (dx == 0) ? 0 : 128;
240 s->d[0] = dx;
241 sz = 0;
242 for (i = 5; i >= 0; i--)
243 {
244 wd2 = ((s->d[i + 1] ^ dx) & 0x8000) ? -wd1 : wd1;
245 wd3 = (int16_t) (((int32_t) s->b[i]*(int32_t) 32640) >> 15);
246 s->b[i] = saturated_add16(wd2, wd3);
247 wd3 = saturated_add16(s->d[i], s->d[i]);
248 sz += ((int32_t) s->b[i]*(int32_t) wd3) >> 15;
249 s->d[i + 1] = s->d[i];
250 }
251 s->sz = saturate(sz);
252
253 /* PREDIC */
254 s->s = saturated_add16(sp, s->sz);
255 }
256 /*- End of function --------------------------------------------------------*/
257
258 SPAN_DECLARE(g722_decode_state_t *) g722_decode_init(g722_decode_state_t *s, int rate, int options)
259 {
260 if (s == NULL)
261 {
262 if ((s = (g722_decode_state_t *) malloc(sizeof(*s))) == NULL)
263 return NULL;
264 }
265 memset(s, 0, sizeof(*s));
266 if (rate == 48000)
267 s->bits_per_sample = 6;
268 else if (rate == 56000)
269 s->bits_per_sample = 7;
270 else
271 s->bits_per_sample = 8;
272 if ((options & G722_SAMPLE_RATE_8000))
273 s->eight_k = TRUE;
274 if ((options & G722_PACKED) && s->bits_per_sample != 8)
275 s->packed = TRUE;
276 else
277 s->packed = FALSE;
278 s->band[0].det = 32;
279 s->band[1].det = 8;
280 return s;
281 }
282 /*- End of function --------------------------------------------------------*/
283
284 SPAN_DECLARE(int) g722_decode_release(g722_decode_state_t *s)
285 {
286 return 0;
287 }
288 /*- End of function --------------------------------------------------------*/
289
290 SPAN_DECLARE(int) g722_decode_free(g722_decode_state_t *s)
291 {
292 free(s);
293 return 0;
294 }
295 /*- End of function --------------------------------------------------------*/
296
297 SPAN_DECLARE(int) g722_decode(g722_decode_state_t *s, int16_t amp[], const uint8_t g722_data[], int len)
298 {
299 int rlow;
300 int ihigh;
301 int16_t dlow;
302 int16_t dhigh;
303 int rhigh;
304 int wd1;
305 int wd2;
306 int wd3;
307 int code;
308 int outlen;
309 int j;
310
311 outlen = 0;
312 rhigh = 0;
313 for (j = 0; j < len; )
314 {
315 if (s->packed)
316 {
317 /* Unpack the code bits */
318 if (s->in_bits < s->bits_per_sample)
319 {
320 s->in_buffer |= (g722_data[j++] << s->in_bits);
321 s->in_bits += 8;
322 }
323 code = s->in_buffer & ((1 << s->bits_per_sample) - 1);
324 s->in_buffer >>= s->bits_per_sample;
325 s->in_bits -= s->bits_per_sample;
326 }
327 else
328 {
329 code = g722_data[j++];
330 }
331
332 switch (s->bits_per_sample)
333 {
334 default:
335 case 8:
336 wd1 = code & 0x3F;
337 ihigh = (code >> 6) & 0x03;
338 wd2 = qm6[wd1];
339 wd1 >>= 2;
340 break;
341 case 7:
342 wd1 = code & 0x1F;
343 ihigh = (code >> 5) & 0x03;
344 wd2 = qm5[wd1];
345 wd1 >>= 1;
346 break;
347 case 6:
348 wd1 = code & 0x0F;
349 ihigh = (code >> 4) & 0x03;
350 wd2 = qm4[wd1];
351 break;
352 }
353 /* Block 5L, LOW BAND INVQBL */
354 wd2 = ((int32_t) s->band[0].det*(int32_t) wd2) >> 15;
355 /* Block 5L, RECONS */
356 /* Block 6L, LIMIT */
357 rlow = saturate15(s->band[0].s + wd2);
358
359 /* Block 2L, INVQAL */
360 wd2 = qm4[wd1];
361 dlow = (int16_t) (((int32_t) s->band[0].det*(int32_t) wd2) >> 15);
362
363 /* Block 3L, LOGSCL */
364 wd2 = rl42[wd1];
365 wd1 = ((int32_t) s->band[0].nb*(int32_t) 127) >> 7;
366 wd1 += wl[wd2];
367 if (wd1 < 0)
368 wd1 = 0;
369 else if (wd1 > 18432)
370 wd1 = 18432;
371 s->band[0].nb = (int16_t) wd1;
372
373 /* Block 3L, SCALEL */
374 wd1 = (s->band[0].nb >> 6) & 31;
375 wd2 = 8 - (s->band[0].nb >> 11);
376 wd3 = (wd2 < 0) ? (ilb[wd1] << -wd2) : (ilb[wd1] >> wd2);
377 s->band[0].det = (int16_t) (wd3 << 2);
378
379 block4(&s->band[0], dlow);
380
381 if (!s->eight_k)
382 {
383 /* Block 2H, INVQAH */
384 wd2 = qm2[ihigh];
385 dhigh = (int16_t) (((int32_t) s->band[1].det*(int32_t) wd2) >> 15);
386 /* Block 5H, RECONS */
387 /* Block 6H, LIMIT */
388 rhigh = saturate15(dhigh + s->band[1].s);
389
390 /* Block 2H, INVQAH */
391 wd2 = rh2[ihigh];
392 wd1 = ((int32_t) s->band[1].nb*(int32_t) 127) >> 7;
393 wd1 += wh[wd2];
394 if (wd1 < 0)
395 wd1 = 0;
396 else if (wd1 > 22528)
397 wd1 = 22528;
398 s->band[1].nb = (int16_t) wd1;
399
400 /* Block 3H, SCALEH */
401 wd1 = (s->band[1].nb >> 6) & 31;
402 wd2 = 10 - (s->band[1].nb >> 11);
403 wd3 = (wd2 < 0) ? (ilb[wd1] << -wd2) : (ilb[wd1] >> wd2);
404 s->band[1].det = (int16_t) (wd3 << 2);
405
406 block4(&s->band[1], dhigh);
407 }
408
409 if (s->itu_test_mode)
410 {
411 amp[outlen++] = (int16_t) (rlow << 1);
412 amp[outlen++] = (int16_t) (rhigh << 1);
413 }
414 else
415 {
416 if (s->eight_k)
417 {
418 /* We shift by 1 to allow for the 15 bit input to the G.722 algorithm. */
419 amp[outlen++] = (int16_t) (rlow << 1);
420 }
421 else
422 {
423 /* Apply the QMF to build the final signal */
424 s->x[s->ptr] = (int16_t) (rlow + rhigh);
425 s->y[s->ptr] = (int16_t) (rlow - rhigh);
426 if (++s->ptr >= 12)
427 s->ptr = 0;
428 /* We shift by 12 to allow for the QMF filters (DC gain = 4096), less 1
429 to allow for the 15 bit input to the G.722 algorithm. */
430 amp[outlen++] = (int16_t) (vec_circular_dot_prodi16(s->y, qmf_coeffs_rev, 12, s->ptr) >> 11);
431 amp[outlen++] = (int16_t) (vec_circular_dot_prodi16(s->x, qmf_coeffs_fwd, 12, s->ptr) >> 11);
432 }
433 }
434 }
435 return outlen;
436 }
437 /*- End of function --------------------------------------------------------*/
438
439 SPAN_DECLARE(g722_encode_state_t *) g722_encode_init(g722_encode_state_t *s, int rate, int options)
440 {
441 if (s == NULL)
442 {
443 if ((s = (g722_encode_state_t *) malloc(sizeof(*s))) == NULL)
444 return NULL;
445 }
446 memset(s, 0, sizeof(*s));
447 if (rate == 48000)
448 s->bits_per_sample = 6;
449 else if (rate == 56000)
450 s->bits_per_sample = 7;
451 else
452 s->bits_per_sample = 8;
453 if ((options & G722_SAMPLE_RATE_8000))
454 s->eight_k = TRUE;
455 if ((options & G722_PACKED) && s->bits_per_sample != 8)
456 s->packed = TRUE;
457 else
458 s->packed = FALSE;
459 s->band[0].det = 32;
460 s->band[1].det = 8;
461 return s;
462 }
463 /*- End of function --------------------------------------------------------*/
464
465 SPAN_DECLARE(int) g722_encode_release(g722_encode_state_t *s)
466 {
467 return 0;
468 }
469 /*- End of function --------------------------------------------------------*/
470
471 SPAN_DECLARE(int) g722_encode_free(g722_encode_state_t *s)
472 {
473 free(s);
474 return 0;
475 }
476 /*- End of function --------------------------------------------------------*/
477
478 SPAN_DECLARE(int) g722_encode(g722_encode_state_t *s, uint8_t g722_data[], const int16_t amp[], int len)
479 {
480 int16_t dlow;
481 int16_t dhigh;
482 int el;
483 int wd;
484 int wd1;
485 int ril;
486 int wd2;
487 int il4;
488 int ih2;
489 int wd3;
490 int eh;
491 int g722_bytes;
492 int ihigh;
493 int ilow;
494 int code;
495 /* Low and high band PCM from the QMF */
496 int16_t xlow;
497 int16_t xhigh;
498 int32_t sumeven;
499 int32_t sumodd;
500 int mih;
501 int i;
502 int j;
503
504 g722_bytes = 0;
505 xhigh = 0;
506 for (j = 0; j < len; )
507 {
508 if (s->itu_test_mode)
509 {
510 xlow =
511 xhigh = amp[j++] >> 1;
512 }
513 else
514 {
515 if (s->eight_k)
516 {
517 /* We shift by 1 to allow for the 15 bit input to the G.722 algorithm. */
518 xlow = amp[j++] >> 1;
519 }
520 else
521 {
522 /* Apply the transmit QMF */
523 s->x[s->ptr] = amp[j++];
524 s->y[s->ptr] = amp[j++];
525 if (++s->ptr >= 12)
526 s->ptr = 0;
527 sumodd = vec_circular_dot_prodi16(s->x, qmf_coeffs_fwd, 12, s->ptr);
528 sumeven = vec_circular_dot_prodi16(s->y, qmf_coeffs_rev, 12, s->ptr);
529 /* We shift by 12 to allow for the QMF filters (DC gain = 4096), plus 1
530 to allow for us summing two filters, plus 1 to allow for the 15 bit
531 input to the G.722 algorithm. */
532 xlow = (int16_t) ((sumeven + sumodd) >> 14);
533 xhigh = (int16_t) ((sumeven - sumodd) >> 14);
534 }
535 }
536 /* Block 1L, SUBTRA */
537 el = saturated_sub16(xlow, s->band[0].s);
538
539 /* Block 1L, QUANTL */
540 wd = (el >= 0) ? el : ~el;
541
542 for (i = 1; i < 30; i++)
543 {
544 wd1 = ((int32_t) q6[i]*(int32_t) s->band[0].det) >> 12;
545 if (wd < wd1)
546 break;
547 }
548 ilow = (el < 0) ? iln[i] : ilp[i];
549
550 /* Block 2L, INVQAL */
551 ril = ilow >> 2;
552 wd2 = qm4[ril];
553 dlow = (int16_t) (((int32_t) s->band[0].det*(int32_t) wd2) >> 15);
554
555 /* Block 3L, LOGSCL */
556 il4 = rl42[ril];
557 wd = ((int32_t) s->band[0].nb*(int32_t) 127) >> 7;
558 s->band[0].nb = (int16_t) (wd + wl[il4]);
559 if (s->band[0].nb < 0)
560 s->band[0].nb = 0;
561 else if (s->band[0].nb > 18432)
562 s->band[0].nb = 18432;
563
564 /* Block 3L, SCALEL */
565 wd1 = (s->band[0].nb >> 6) & 31;
566 wd2 = 8 - (s->band[0].nb >> 11);
567 wd3 = (wd2 < 0) ? (ilb[wd1] << -wd2) : (ilb[wd1] >> wd2);
568 s->band[0].det = (int16_t) (wd3 << 2);
569
570 block4(&s->band[0], dlow);
571
572 if (s->eight_k)
573 {
574 /* Just leave the high bits as zero */
575 code = (0xC0 | ilow) >> (8 - s->bits_per_sample);
576 }
577 else
578 {
579 /* Block 1H, SUBTRA */
580 eh = saturated_sub16(xhigh, s->band[1].s);
581
582 /* Block 1H, QUANTH */
583 wd = (eh >= 0) ? eh : ~eh;
584 wd1 = (564*s->band[1].det) >> 12;
585 mih = (wd >= wd1) ? 2 : 1;
586 ihigh = (eh < 0) ? ihn[mih] : ihp[mih];
587
588 /* Block 2H, INVQAH */
589 wd2 = qm2[ihigh];
590 dhigh = (int16_t) (((int32_t) s->band[1].det*(int32_t) wd2) >> 15);
591
592 /* Block 3H, LOGSCH */
593 ih2 = rh2[ihigh];
594 wd = ((int32_t) s->band[1].nb*(int32_t) 127) >> 7;
595 s->band[1].nb = (int16_t) (wd + wh[ih2]);
596 if (s->band[1].nb < 0)
597 s->band[1].nb = 0;
598 else if (s->band[1].nb > 22528)
599 s->band[1].nb = 22528;
600
601 /* Block 3H, SCALEH */
602 wd1 = (s->band[1].nb >> 6) & 31;
603 wd2 = 10 - (s->band[1].nb >> 11);
604 wd3 = (wd2 < 0) ? (ilb[wd1] << -wd2) : (ilb[wd1] >> wd2);
605 s->band[1].det = (int16_t) (wd3 << 2);
606
607 block4(&s->band[1], dhigh);
608 code = ((ihigh << 6) | ilow) >> (8 - s->bits_per_sample);
609 }
610
611 if (s->packed)
612 {
613 /* Pack the code bits */
614 s->out_buffer |= (code << s->out_bits);
615 s->out_bits += s->bits_per_sample;
616 if (s->out_bits >= 8)
617 {
618 g722_data[g722_bytes++] = (uint8_t) (s->out_buffer & 0xFF);
619 s->out_bits -= 8;
620 s->out_buffer >>= 8;
621 }
622 }
623 else
624 {
625 g722_data[g722_bytes++] = (uint8_t) code;
626 }
627 }
628 return g722_bytes;
629 }
630 /*- End of function --------------------------------------------------------*/
631 /*- End of file ------------------------------------------------------------*/

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