Mercurial > hg > audiostuff

comparison spandsp-0.0.3/spandsp-0.0.3/src/g726.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 * g726.c - The ITU G.726 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 * Based on G.721/G.723 code which is:
26 *
27 * This source code is a product of Sun Microsystems, Inc. and is provided
28 * for unrestricted use. Users may copy or modify this source code without
29 * charge.
30 *
31 * SUN SOURCE CODE IS PROVIDED AS IS WITH NO WARRANTIES OF ANY KIND INCLUDING
32 * THE WARRANTIES OF DESIGN, MERCHANTIBILITY AND FITNESS FOR A PARTICULAR
33 * PURPOSE, OR ARISING FROM A COURSE OF DEALING, USAGE OR TRADE PRACTICE.
34 *
35 * Sun source code is provided with no support and without any obligation on
36 * the part of Sun Microsystems, Inc. to assist in its use, correction,
37 * modification or enhancement.
38 *
39 * SUN MICROSYSTEMS, INC. SHALL HAVE NO LIABILITY WITH RESPECT TO THE
40 * INFRINGEMENT OF COPYRIGHTS, TRADE SECRETS OR ANY PATENTS BY THIS SOFTWARE
41 * OR ANY PART THEREOF.
42 *
43 * In no event will Sun Microsystems, Inc. be liable for any lost revenue
44 * or profits or other special, indirect and consequential damages, even if
45 * Sun has been advised of the possibility of such damages.
46 *
47 * Sun Microsystems, Inc.
48 * 2550 Garcia Avenue
49 * Mountain View, California 94043
50 *
51 * $Id: g726.c,v 1.17 2006/11/19 14:07:24 steveu Exp $
52 */
53
54 /*! \file */
55
56 #ifdef HAVE_CONFIG_H
57 #include <config.h>
58 #endif
59
60 #include <inttypes.h>
61 #include <memory.h>
62 #include <stdlib.h>
63 #if defined(HAVE_TGMATH_H)
64 #include <tgmath.h>
65 #endif
66 #if defined(HAVE_MATH_H)
67 #include <math.h>
68 #endif
69
70 #include "spandsp/telephony.h"
71 #include "spandsp/dc_restore.h"
72 #include "spandsp/bitstream.h"
73 #include "spandsp/bit_operations.h"
74 #include "spandsp/g711.h"
75 #include "spandsp/g726.h"
76
77 /*
78 * Maps G.726_16 code word to reconstructed scale factor normalized log
79 * magnitude values.
80 */
81 static const int g726_16_dqlntab[4] =
82 {
83 116, 365, 365, 116
84 };
85
86 /* Maps G.726_16 code word to log of scale factor multiplier. */
87 static const int g726_16_witab[4] =
88 {
89 -704, 14048, 14048, -704
90 };
91
92 /*
93 * Maps G.726_16 code words to a set of values whose long and short
94 * term averages are computed and then compared to give an indication
95 * how stationary (steady state) the signal is.
96 */
97 static const int g726_16_fitab[4] =
98 {
99 0x000, 0xE00, 0xE00, 0x000
100 };
101
102 static const int qtab_726_16[1] =
103 {
104 261
105 };
106
107 /*
108 * Maps G.726_24 code word to reconstructed scale factor normalized log
109 * magnitude values.
110 */
111 static const int g726_24_dqlntab[8] =
112 {
113 -2048, 135, 273, 373, 373, 273, 135, -2048
114 };
115
116 /* Maps G.726_24 code word to log of scale factor multiplier. */
117 static const int g726_24_witab[8] =
118 {
119 -128, 960, 4384, 18624, 18624, 4384, 960, -128
120 };
121
122 /*
123 * Maps G.726_24 code words to a set of values whose long and short
124 * term averages are computed and then compared to give an indication
125 * how stationary (steady state) the signal is.
126 */
127 static const int g726_24_fitab[8] =
128 {
129 0x000, 0x200, 0x400, 0xE00, 0xE00, 0x400, 0x200, 0x000
130 };
131
132 static const int qtab_726_24[3] =
133 {
134 8, 218, 331
135 };
136
137 /*
138 * Maps G.726_32 code word to reconstructed scale factor normalized log
139 * magnitude values.
140 */
141 static const int g726_32_dqlntab[16] =
142 {
143 -2048, 4, 135, 213, 273, 323, 373, 425,
144 425, 373, 323, 273, 213, 135, 4, -2048
145 };
146
147 /* Maps G.726_32 code word to log of scale factor multiplier. */
148 static const int g726_32_witab[16] =
149 {
150 -384, 576, 1312, 2048, 3584, 6336, 11360, 35904,
151 35904, 11360, 6336, 3584, 2048, 1312, 576, -384
152 };
153
154 /*
155 * Maps G.726_32 code words to a set of values whose long and short
156 * term averages are computed and then compared to give an indication
157 * how stationary (steady state) the signal is.
158 */
159 static const int g726_32_fitab[16] =
160 {
161 0x000, 0x000, 0x000, 0x200, 0x200, 0x200, 0x600, 0xE00,
162 0xE00, 0x600, 0x200, 0x200, 0x200, 0x000, 0x000, 0x000
163 };
164
165 static const int qtab_726_32[7] =
166 {
167 -124, 80, 178, 246, 300, 349, 400
168 };
169
170 /*
171 * Maps G.726_40 code word to ructeconstructed scale factor normalized log
172 * magnitude values.
173 */
174 static const int g726_40_dqlntab[32] =
175 {
176 -2048, -66, 28, 104, 169, 224, 274, 318,
177 358, 395, 429, 459, 488, 514, 539, 566,
178 566, 539, 514, 488, 459, 429, 395, 358,
179 318, 274, 224, 169, 104, 28, -66, -2048
180 };
181
182 /* Maps G.726_40 code word to log of scale factor multiplier. */
183 static const int g726_40_witab[32] =
184 {
185 448, 448, 768, 1248, 1280, 1312, 1856, 3200,
186 4512, 5728, 7008, 8960, 11456, 14080, 16928, 22272,
187 22272, 16928, 14080, 11456, 8960, 7008, 5728, 4512,
188 3200, 1856, 1312, 1280, 1248, 768, 448, 448
189 };
190
191 /*
192 * Maps G.726_40 code words to a set of values whose long and short
193 * term averages are computed and then compared to give an indication
194 * how stationary (steady state) the signal is.
195 */
196 static const int g726_40_fitab[32] =
197 {
198 0x000, 0x000, 0x000, 0x000, 0x000, 0x200, 0x200, 0x200,
199 0x200, 0x200, 0x400, 0x600, 0x800, 0xA00, 0xC00, 0xC00,
200 0xC00, 0xC00, 0xA00, 0x800, 0x600, 0x400, 0x200, 0x200,
201 0x200, 0x200, 0x200, 0x000, 0x000, 0x000, 0x000, 0x000
202 };
203
204 static const int qtab_726_40[15] =
205 {
206 -122, -16, 68, 139, 198, 250, 298, 339,
207 378, 413, 445, 475, 502, 528, 553
208 };
209
210 /*
211 * returns the integer product of the 14-bit integer "an" and
212 * "floating point" representation (4-bit exponent, 6-bit mantessa) "srn".
213 */
214 static int16_t fmult(int16_t an, int16_t srn)
215 {
216 int16_t anmag;
217 int16_t anexp;
218 int16_t anmant;
219 int16_t wanexp;
220 int16_t wanmant;
221 int16_t retval;
222
223 anmag = (an > 0) ? an : ((-an) & 0x1FFF);
224 anexp = (int16_t) (top_bit(anmag) - 5);
225 anmant = (anmag == 0) ? 32 : (anexp >= 0) ? (anmag >> anexp) : (anmag << -anexp);
226 wanexp = anexp + ((srn >> 6) & 0xF) - 13;
227
228 wanmant = (anmant*(srn & 0x3F) + 0x30) >> 4;
229 retval = (wanexp >= 0) ? ((wanmant << wanexp) & 0x7FFF) : (wanmant >> -wanexp);
230
231 return (((an ^ srn) < 0) ? -retval : retval);
232 }
233 /*- End of function --------------------------------------------------------*/
234
235 /*
236 * Compute the estimated signal from the 6-zero predictor.
237 */
238 static __inline__ int16_t predictor_zero(g726_state_t *s)
239 {
240 int i;
241 int sezi;
242
243 sezi = fmult(s->b[0] >> 2, s->dq[0]);
244 /* ACCUM */
245 for (i = 1; i < 6; i++)
246 sezi += fmult(s->b[i] >> 2, s->dq[i]);
247 return (int16_t) sezi;
248 }
249 /*- End of function --------------------------------------------------------*/
250
251 /*
252 * Computes the estimated signal from the 2-pole predictor.
253 */
254 static __inline__ int16_t predictor_pole(g726_state_t *s)
255 {
256 return (fmult(s->a[1] >> 2, s->sr[1]) + fmult(s->a[0] >> 2, s->sr[0]));
257 }
258 /*- End of function --------------------------------------------------------*/
259
260 /*
261 * Computes the quantization step size of the adaptive quantizer.
262 */
263 static int step_size(g726_state_t *s)
264 {
265 int y;
266 int dif;
267 int al;
268
269 if (s->ap >= 256)
270 return s->yu;
271 y = s->yl >> 6;
272 dif = s->yu - y;
273 al = s->ap >> 2;
274 if (dif > 0)
275 y += (dif*al) >> 6;
276 else if (dif < 0)
277 y += (dif*al + 0x3F) >> 6;
278 return y;
279 }
280 /*- End of function --------------------------------------------------------*/
281
282 /*
283 * Given a raw sample, 'd', of the difference signal and a
284 * quantization step size scale factor, 'y', this routine returns the
285 * ADPCM codeword to which that sample gets quantized. The step
286 * size scale factor division operation is done in the log base 2 domain
287 * as a subtraction.
288 */
289 static int16_t quantize(int d, /* Raw difference signal sample */
290 int y, /* Step size multiplier */
291 const int table[], /* quantization table */
292 int quantizer_states) /* table size of int16_t integers */
293 {
294 int16_t dqm; /* Magnitude of 'd' */
295 int16_t exp; /* Integer part of base 2 log of 'd' */
296 int16_t mant; /* Fractional part of base 2 log */
297 int16_t dl; /* Log of magnitude of 'd' */
298 int16_t dln; /* Step size scale factor normalized log */
299 int i;
300 int size;
301
302 /*
303 * LOG
304 *
305 * Compute base 2 log of 'd', and store in 'dl'.
306 */
307 dqm = (int16_t) abs(d);
308 exp = (int16_t) (top_bit(dqm >> 1) + 1);
309 /* Fractional portion. */
310 mant = ((dqm << 7) >> exp) & 0x7F;
311 dl = (exp << 7) + mant;
312
313 /*
314 * SUBTB
315 *
316 * "Divide" by step size multiplier.
317 */
318 dln = dl - (int16_t) (y >> 2);
319
320 /*
321 * QUAN
322 *
323 * Search for codword i for 'dln'.
324 */
325 size = (quantizer_states - 1) >> 1;
326 for (i = 0; i < size; i++)
327 {
328 if (dln < table[i])
329 break;
330 }
331 if (d < 0)
332 {
333 /* Take 1's complement of i */
334 return (int16_t) ((size << 1) + 1 - i);
335 }
336 if (i == 0 && (quantizer_states & 1))
337 {
338 /* Zero is only valid if there are an even number of states, so
339 take the 1's complement if the code is zero. */
340 return (int16_t) quantizer_states;
341 }
342 return (int16_t) i;
343 }
344 /*- End of function --------------------------------------------------------*/
345
346 /*
347 * Returns reconstructed difference signal 'dq' obtained from
348 * codeword 'i' and quantization step size scale factor 'y'.
349 * Multiplication is performed in log base 2 domain as addition.
350 */
351 static int16_t reconstruct(int sign, /* 0 for non-negative value */
352 int dqln, /* G.72x codeword */
353 int y) /* Step size multiplier */
354 {
355 int16_t dql; /* Log of 'dq' magnitude */
356 int16_t dex; /* Integer part of log */
357 int16_t dqt;
358 int16_t dq; /* Reconstructed difference signal sample */
359
360 dql = (int16_t) (dqln + (y >> 2)); /* ADDA */
361
362 if (dql < 0)
363 return ((sign) ? -0x8000 : 0);
364 /* ANTILOG */
365 dex = (dql >> 7) & 15;
366 dqt = 128 + (dql & 127);
367 dq = (dqt << 7) >> (14 - dex);
368 return ((sign) ? (dq - 0x8000) : dq);
369 }
370 /*- End of function --------------------------------------------------------*/
371
372 /*
373 * updates the state variables for each output code
374 */
375 static void update(g726_state_t *s,
376 int y, /* quantizer step size */
377 int wi, /* scale factor multiplier */
378 int fi, /* for long/short term energies */
379 int dq, /* quantized prediction difference */
380 int sr, /* reconstructed signal */
381 int dqsez) /* difference from 2-pole predictor */
382 {
383 int16_t mag;
384 int16_t exp;
385 int16_t a2p; /* LIMC */
386 int16_t a1ul; /* UPA1 */
387 int16_t pks1; /* UPA2 */
388 int16_t fa1;
389 int16_t ylint;
390 int16_t dqthr;
391 int16_t ylfrac;
392 int16_t thr;
393 int16_t pk0;
394 int i;
395 int tr;
396
397 a2p = 0;
398 /* Needed in updating predictor poles */
399 pk0 = (dqsez < 0) ? 1 : 0;
400
401 /* prediction difference magnitude */
402 mag = (int16_t) (dq & 0x7FFF);
403 /* TRANS */
404 ylint = (int16_t) (s->yl >> 15); /* exponent part of yl */
405 ylfrac = (int16_t) ((s->yl >> 10) & 0x1F); /* fractional part of yl */
406 /* Limit threshold to 31 << 10 */
407 thr = (ylint > 9) ? (31 << 10) : ((32 + ylfrac) << ylint);
408 dqthr = (thr + (thr >> 1)) >> 1; /* dqthr = 0.75 * thr */
409 if (!s->td) /* signal supposed voice */
410 tr = FALSE;
411 else if (mag <= dqthr) /* supposed data, but small mag */
412 tr = FALSE; /* treated as voice */
413 else /* signal is data (modem) */
414 tr = TRUE;
415
416 /*
417 * Quantizer scale factor adaptation.
418 */
419
420 /* FUNCTW & FILTD & DELAY */
421 /* update non-steady state step size multiplier */
422 s->yu = (int16_t) (y + ((wi - y) >> 5));
423
424 /* LIMB */
425 if (s->yu < 544)
426 s->yu = 544;
427 else if (s->yu > 5120)
428 s->yu = 5120;
429
430 /* FILTE & DELAY */
431 /* update steady state step size multiplier */
432 s->yl += s->yu + ((-s->yl) >> 6);
433
434 /*
435 * Adaptive predictor coefficients.
436 */
437 if (tr)
438 {
439 /* Reset the a's and b's for a modem signal */
440 s->a[0] = 0;
441 s->a[1] = 0;
442 s->b[0] = 0;
443 s->b[1] = 0;
444 s->b[2] = 0;
445 s->b[3] = 0;
446 s->b[4] = 0;
447 s->b[5] = 0;
448 }
449 else
450 {
451 /* Update the a's and b's */
452 /* UPA2 */
453 pks1 = pk0 ^ s->pk[0];
454
455 /* Update predictor pole a[1] */
456 a2p = s->a[1] - (s->a[1] >> 7);
457 if (dqsez != 0)
458 {
459 fa1 = (pks1) ? s->a[0] : -s->a[0];
460 /* a2p = function of fa1 */
461 if (fa1 < -8191)
462 a2p -= 0x100;
463 else if (fa1 > 8191)
464 a2p += 0xFF;
465 else
466 a2p += fa1 >> 5;
467
468 if (pk0 ^ s->pk[1])
469 {
470 /* LIMC */
471 if (a2p <= -12160)
472 a2p = -12288;
473 else if (a2p >= 12416)
474 a2p = 12288;
475 else
476 a2p -= 0x80;
477 }
478 else if (a2p <= -12416)
479 a2p = -12288;
480 else if (a2p >= 12160)
481 a2p = 12288;
482 else
483 a2p += 0x80;
484 }
485
486 /* TRIGB & DELAY */
487 s->a[1] = a2p;
488
489 /* UPA1 */
490 /* Update predictor pole a[0] */
491 s->a[0] -= s->a[0] >> 8;
492 if (dqsez != 0)
493 {
494 if (pks1 == 0)
495 s->a[0] += 192;
496 else
497 s->a[0] -= 192;
498 }
499 /* LIMD */
500 a1ul = 15360 - a2p;
501 if (s->a[0] < -a1ul)
502 s->a[0] = -a1ul;
503 else if (s->a[0] > a1ul)
504 s->a[0] = a1ul;
505
506 /* UPB : update predictor zeros b[6] */
507 for (i = 0; i < 6; i++)
508 {
509 /* Distinguish 40Kbps mode from the others */
510 s->b[i] -= s->b[i] >> ((s->bits_per_sample == 5) ? 9 : 8);
511 if (dq & 0x7FFF)
512 {
513 /* XOR */
514 if ((dq ^ s->dq[i]) >= 0)
515 s->b[i] += 128;
516 else
517 s->b[i] -= 128;
518 }
519 }
520 }
521
522 for (i = 5; i > 0; i--)
523 s->dq[i] = s->dq[i - 1];
524 /* FLOAT A : convert dq[0] to 4-bit exp, 6-bit mantissa f.p. */
525 if (mag == 0)
526 {
527 s->dq[0] = (dq >= 0) ? 0x20 : 0xFC20;
528 }
529 else
530 {
531 exp = (int16_t) (top_bit(mag) + 1);
532 s->dq[0] = (dq >= 0)
533 ? ((exp << 6) + ((mag << 6) >> exp))
534 : ((exp << 6) + ((mag << 6) >> exp) - 0x400);
535 }
536
537 s->sr[1] = s->sr[0];
538 /* FLOAT B : convert sr to 4-bit exp., 6-bit mantissa f.p. */
539 if (sr == 0)
540 {
541 s->sr[0] = 0x20;
542 }
543 else if (sr > 0)
544 {
545 exp = (int16_t) (top_bit(sr) + 1);
546 s->sr[0] = (int16_t) ((exp << 6) + ((sr << 6) >> exp));
547 }
548 else if (sr > -32768)
549 {
550 mag = (int16_t) -sr;
551 exp = (int16_t) (top_bit(mag) + 1);
552 s->sr[0] = (exp << 6) + ((mag << 6) >> exp) - 0x400;
553 }
554 else
555 {
556 s->sr[0] = (uint16_t) 0xFC20;
557 }
558
559 /* DELAY A */
560 s->pk[1] = s->pk[0];
561 s->pk[0] = pk0;
562
563 /* TONE */
564 if (tr) /* this sample has been treated as data */
565 s->td = FALSE; /* next one will be treated as voice */
566 else if (a2p < -11776) /* small sample-to-sample correlation */
567 s->td = TRUE; /* signal may be data */
568 else /* signal is voice */
569 s->td = FALSE;
570
571 /* Adaptation speed control. */
572 /* FILTA */
573 s->dms += ((int16_t) fi - s->dms) >> 5;
574 /* FILTB */
575 s->dml += (((int16_t) (fi << 2) - s->dml) >> 7);
576
577 if (tr)
578 s->ap = 256;
579 else if (y < 1536) /* SUBTC */
580 s->ap += (0x200 - s->ap) >> 4;
581 else if (s->td)
582 s->ap += (0x200 - s->ap) >> 4;
583 else if (abs((s->dms << 2) - s->dml) >= (s->dml >> 3))
584 s->ap += (0x200 - s->ap) >> 4;
585 else
586 s->ap += (-s->ap) >> 4;
587 }
588 /*- End of function --------------------------------------------------------*/
589
590 static int16_t tandem_adjust_alaw(int16_t sr, /* decoder output linear PCM sample */
591 int se, /* predictor estimate sample */
592 int y, /* quantizer step size */
593 int i, /* decoder input code */
594 int sign,
595 const int qtab[],
596 int quantizer_states)
597 {
598 uint8_t sp; /* A-law compressed 8-bit code */
599 int16_t dx; /* prediction error */
600 int id; /* quantized prediction error */
601 int sd; /* adjusted A-law decoded sample value */
602
603 if (sr <= -32768)
604 sr = -1;
605 sp = linear_to_alaw((sr >> 1) << 3);
606 /* 16-bit prediction error */
607 dx = (int16_t) ((alaw_to_linear(sp) >> 2) - se);
608 id = quantize(dx, y, qtab, quantizer_states);
609 if (id == i)
610 {
611 /* No adjustment of sp required */
612 return (int16_t) sp;
613 }
614 /* sp adjustment needed */
615 /* ADPCM codes : 8, 9, ... F, 0, 1, ... , 6, 7 */
616 /* 2's complement to biased unsigned */
617 if ((id ^ sign) > (i ^ sign))
618 {
619 /* sp adjusted to next lower value */
620 if (sp & 0x80)
621 sd = (sp == 0xD5) ? 0x55 : (((sp ^ 0x55) - 1) ^ 0x55);
622 else
623 sd = (sp == 0x2A) ? 0x2A : (((sp ^ 0x55) + 1) ^ 0x55);
624 }
625 else
626 {
627 /* sp adjusted to next higher value */
628 if (sp & 0x80)
629 sd = (sp == 0xAA) ? 0xAA : (((sp ^ 0x55) + 1) ^ 0x55);
630 else
631 sd = (sp == 0x55) ? 0xD5 : (((sp ^ 0x55) - 1) ^ 0x55);
632 }
633 return (int16_t) sd;
634 }
635 /*- End of function --------------------------------------------------------*/
636
637 static int16_t tandem_adjust_ulaw(int16_t sr, /* decoder output linear PCM sample */
638 int se, /* predictor estimate sample */
639 int y, /* quantizer step size */
640 int i, /* decoder input code */
641 int sign,
642 const int qtab[],
643 int quantizer_states)
644 {
645 uint8_t sp; /* u-law compressed 8-bit code */
646 int16_t dx; /* prediction error */
647 int id; /* quantized prediction error */
648 int sd; /* adjusted u-law decoded sample value */
649
650 if (sr <= -32768)
651 sr = 0;
652 sp = linear_to_ulaw(sr << 2);
653 /* 16-bit prediction error */
654 dx = (int16_t) ((ulaw_to_linear(sp) >> 2) - se);
655 id = quantize(dx, y, qtab, quantizer_states);
656 if (id == i)
657 {
658 /* No adjustment of sp required. */
659 return (int16_t) sp;
660 }
661 /* ADPCM codes : 8, 9, ... F, 0, 1, ... , 6, 7 */
662 /* 2's complement to biased unsigned */
663 if ((id ^ sign) > (i ^ sign))
664 {
665 /* sp adjusted to next lower value */
666 if (sp & 0x80)
667 sd = (sp == 0xFF) ? 0x7E : (sp + 1);
668 else
669 sd = (sp == 0x00) ? 0x00 : (sp - 1);
670 }
671 else
672 {
673 /* sp adjusted to next higher value */
674 if (sp & 0x80)
675 sd = (sp == 0x80) ? 0x80 : (sp - 1);
676 else
677 sd = (sp == 0x7F) ? 0xFE : (sp + 1);
678 }
679 return (int16_t) sd;
680 }
681 /*- End of function --------------------------------------------------------*/
682
683 /*
684 * Encodes a linear PCM, A-law or u-law input sample and returns its 3-bit code.
685 */
686 static uint8_t g726_16_encoder(g726_state_t *s, int16_t amp)
687 {
688 int y;
689 int16_t sei;
690 int16_t sezi;
691 int16_t se;
692 int16_t d;
693 int16_t sr;
694 int16_t dqsez;
695 int16_t dq;
696 int16_t i;
697
698 sezi = predictor_zero(s);
699 sei = sezi + predictor_pole(s);
700 se = sei >> 1;
701 d = amp - se;
702
703 /* Quantize prediction difference */
704 y = step_size(s);
705 i = quantize(d, y, qtab_726_16, 4);
706 dq = reconstruct(i & 2, g726_16_dqlntab[i], y);
707
708 /* Reconstruct the signal */
709 sr = (dq < 0) ? (se - (dq & 0x3FFF)) : (se + dq);
710
711 /* Pole prediction difference */
712 dqsez = sr + (sezi >> 1) - se;
713
714 update(s, y, g726_16_witab[i], g726_16_fitab[i], dq, sr, dqsez);
715 return (uint8_t) i;
716 }
717 /*- End of function --------------------------------------------------------*/
718
719 /*
720 * Decodes a 2-bit CCITT G.726_16 ADPCM code and returns
721 * the resulting 16-bit linear PCM, A-law or u-law sample value.
722 */
723 static int16_t g726_16_decoder(g726_state_t *s, uint8_t code)
724 {
725 int16_t sezi;
726 int16_t sei;
727 int16_t se;
728 int16_t sr;
729 int16_t dq;
730 int16_t dqsez;
731 int y;
732
733 /* Mask to get proper bits */
734 code &= 0x03;
735 sezi = predictor_zero(s);
736 sei = sezi + predictor_pole(s);
737
738 y = step_size(s);
739 dq = reconstruct(code & 2, g726_16_dqlntab[code], y);
740
741 /* Reconstruct the signal */
742 se = sei >> 1;
743 sr = (dq < 0) ? (se - (dq & 0x3FFF)) : (se + dq);
744
745 /* Pole prediction difference */
746 dqsez = sr + (sezi >> 1) - se;
747
748 update(s, y, g726_16_witab[code], g726_16_fitab[code], dq, sr, dqsez);
749
750 switch (s->ext_coding)
751 {
752 case G726_ENCODING_ALAW:
753 return tandem_adjust_alaw(sr, se, y, code, 2, qtab_726_16, 4);
754 case G726_ENCODING_ULAW:
755 return tandem_adjust_ulaw(sr, se, y, code, 2, qtab_726_16, 4);
756 }
757 return (sr << 2);
758 }
759 /*- End of function --------------------------------------------------------*/
760
761 /*
762 * Encodes a linear PCM, A-law or u-law input sample and returns its 3-bit code.
763 */
764 static uint8_t g726_24_encoder(g726_state_t *s, int16_t amp)
765 {
766 int16_t sei;
767 int16_t sezi;
768 int16_t se;
769 int16_t d;
770 int16_t sr;
771 int16_t dqsez;
772 int16_t dq;
773 int16_t i;
774 int y;
775
776 sezi = predictor_zero(s);
777 sei = sezi + predictor_pole(s);
778 se = sei >> 1;
779 d = amp - se;
780
781 /* Quantize prediction difference */
782 y = step_size(s);
783 i = quantize(d, y, qtab_726_24, 7);
784 dq = reconstruct(i & 4, g726_24_dqlntab[i], y);
785
786 /* Reconstruct the signal */
787 sr = (dq < 0) ? (se - (dq & 0x3FFF)) : (se + dq);
788
789 /* Pole prediction difference */
790 dqsez = sr + (sezi >> 1) - se;
791
792 update(s, y, g726_24_witab[i], g726_24_fitab[i], dq, sr, dqsez);
793 return (uint8_t) i;
794 }
795 /*- End of function --------------------------------------------------------*/
796
797 /*
798 * Decodes a 3-bit CCITT G.726_24 ADPCM code and returns
799 * the resulting 16-bit linear PCM, A-law or u-law sample value.
800 */
801 static int16_t g726_24_decoder(g726_state_t *s, uint8_t code)
802 {
803 int16_t sezi;
804 int16_t sei;
805 int16_t se;
806 int16_t sr;
807 int16_t dq;
808 int16_t dqsez;
809 int y;
810
811 /* Mask to get proper bits */
812 code &= 0x07;
813 sezi = predictor_zero(s);
814 sei = sezi + predictor_pole(s);
815
816 y = step_size(s);
817 dq = reconstruct(code & 4, g726_24_dqlntab[code], y);
818
819 /* Reconstruct the signal */
820 se = sei >> 1;
821 sr = (dq < 0) ? (se - (dq & 0x3FFF)) : (se + dq);
822
823 /* Pole prediction difference */
824 dqsez = sr + (sezi >> 1) - se;
825
826 update(s, y, g726_24_witab[code], g726_24_fitab[code], dq, sr, dqsez);
827
828 switch (s->ext_coding)
829 {
830 case G726_ENCODING_ALAW:
831 return tandem_adjust_alaw(sr, se, y, code, 4, qtab_726_24, 7);
832 case G726_ENCODING_ULAW:
833 return tandem_adjust_ulaw(sr, se, y, code, 4, qtab_726_24, 7);
834 }
835 return (sr << 2);
836 }
837 /*- End of function --------------------------------------------------------*/
838
839 /*
840 * Encodes a linear input sample and returns its 4-bit code.
841 */
842 static uint8_t g726_32_encoder(g726_state_t *s, int16_t amp)
843 {
844 int16_t sei;
845 int16_t sezi;
846 int16_t se;
847 int16_t d;
848 int16_t sr;
849 int16_t dqsez;
850 int16_t dq;
851 int16_t i;
852 int y;
853
854 sezi = predictor_zero(s);
855 sei = sezi + predictor_pole(s);
856 se = sei >> 1;
857 d = amp - se;
858
859 /* Quantize the prediction difference */
860 y = step_size(s);
861 i = quantize(d, y, qtab_726_32, 15);
862 dq = reconstruct(i & 8, g726_32_dqlntab[i], y);
863
864 /* Reconstruct the signal */
865 sr = (dq < 0) ? (se - (dq & 0x3FFF)) : (se + dq);
866
867 /* Pole prediction difference */
868 dqsez = sr + (sezi >> 1) - se;
869
870 update(s, y, g726_32_witab[i], g726_32_fitab[i], dq, sr, dqsez);
871 return (uint8_t) i;
872 }
873 /*- End of function --------------------------------------------------------*/
874
875 /*
876 * Decodes a 4-bit CCITT G.726_32 ADPCM code and returns
877 * the resulting 16-bit linear PCM, A-law or u-law sample value.
878 */
879 static int16_t g726_32_decoder(g726_state_t *s, uint8_t code)
880 {
881 int16_t sezi;
882 int16_t sei;
883 int16_t se;
884 int16_t sr;
885 int16_t dq;
886 int16_t dqsez;
887 int y;
888
889 /* Mask to get proper bits */
890 code &= 0x0F;
891 sezi = predictor_zero(s);
892 sei = sezi + predictor_pole(s);
893
894 y = step_size(s);
895 dq = reconstruct(code & 8, g726_32_dqlntab[code], y);
896
897 /* Reconstruct the signal */
898 se = sei >> 1;
899 sr = (dq < 0) ? (se - (dq & 0x3FFF)) : (se + dq);
900
901 /* Pole prediction difference */
902 dqsez = sr + (sezi >> 1) - se;
903
904 update(s, y, g726_32_witab[code], g726_32_fitab[code], dq, sr, dqsez);
905
906 switch (s->ext_coding)
907 {
908 case G726_ENCODING_ALAW:
909 return tandem_adjust_alaw(sr, se, y, code, 8, qtab_726_32, 15);
910 case G726_ENCODING_ULAW:
911 return tandem_adjust_ulaw(sr, se, y, code, 8, qtab_726_32, 15);
912 }
913 return (sr << 2);
914 }
915 /*- End of function --------------------------------------------------------*/
916
917 /*
918 * Encodes a 16-bit linear PCM, A-law or u-law input sample and retuens
919 * the resulting 5-bit CCITT G.726 40Kbps code.
920 */
921 static uint8_t g726_40_encoder(g726_state_t *s, int16_t amp)
922 {
923 int16_t sei;
924 int16_t sezi;
925 int16_t se;
926 int16_t d;
927 int16_t sr;
928 int16_t dqsez;
929 int16_t dq;
930 int16_t i;
931 int y;
932
933 sezi = predictor_zero(s);
934 sei = sezi + predictor_pole(s);
935 se = sei >> 1;
936 d = amp - se;
937
938 /* Quantize prediction difference */
939 y = step_size(s);
940 i = quantize(d, y, qtab_726_40, 31);
941 dq = reconstruct(i & 0x10, g726_40_dqlntab[i], y);
942
943 /* Reconstruct the signal */
944 sr = (dq < 0) ? (se - (dq & 0x7FFF)) : (se + dq);
945
946 /* Pole prediction difference */
947 dqsez = sr + (sezi >> 1) - se;
948
949 update(s, y, g726_40_witab[i], g726_40_fitab[i], dq, sr, dqsez);
950 return (uint8_t) i;
951 }
952 /*- End of function --------------------------------------------------------*/
953
954 /*
955 * Decodes a 5-bit CCITT G.726 40Kbps code and returns
956 * the resulting 16-bit linear PCM, A-law or u-law sample value.
957 */
958 static int16_t g726_40_decoder(g726_state_t *s, uint8_t code)
959 {
960 int16_t sezi;
961 int16_t sei;
962 int16_t se;
963 int16_t sr;
964 int16_t dq;
965 int16_t dqsez;
966 int y;
967
968 /* Mask to get proper bits */
969 code &= 0x1F;
970 sezi = predictor_zero(s);
971 sei = sezi + predictor_pole(s);
972
973 y = step_size(s);
974 dq = reconstruct(code & 0x10, g726_40_dqlntab[code], y);
975
976 /* Reconstruct the signal */
977 se = sei >> 1;
978 sr = (dq < 0) ? (se - (dq & 0x7FFF)) : (se + dq);
979
980 /* Pole prediction difference */
981 dqsez = sr + (sezi >> 1) - se;
982
983 update(s, y, g726_40_witab[code], g726_40_fitab[code], dq, sr, dqsez);
984
985 switch (s->ext_coding)
986 {
987 case G726_ENCODING_ALAW:
988 return tandem_adjust_alaw(sr, se, y, code, 0x10, qtab_726_40, 31);
989 case G726_ENCODING_ULAW:
990 return tandem_adjust_ulaw(sr, se, y, code, 0x10, qtab_726_40, 31);
991 }
992 return (sr << 2);
993 }
994 /*- End of function --------------------------------------------------------*/
995
996 g726_state_t *g726_init(g726_state_t *s, int bit_rate, int ext_coding, int packing)
997 {
998 int i;
999
1000 if (bit_rate != 16000 && bit_rate != 24000 && bit_rate != 32000 && bit_rate != 40000)
1001 return NULL;
1002 if (s == NULL)
1003 {
1004 if ((s = (g726_state_t *) malloc(sizeof(*s))) == NULL)
1005 return NULL;
1006 }
1007 s->yl = 34816;
1008 s->yu = 544;
1009 s->dms = 0;
1010 s->dml = 0;
1011 s->ap = 0;
1012 s->rate = bit_rate;
1013 s->ext_coding = ext_coding;
1014 s->packing = packing;
1015 for (i = 0; i < 2; i++)
1016 {
1017 s->a[i] = 0;
1018 s->pk[i] = 0;
1019 s->sr[i] = 32;
1020 }
1021 for (i = 0; i < 6; i++)
1022 {
1023 s->b[i] = 0;
1024 s->dq[i] = 32;
1025 }
1026 s->td = FALSE;
1027 switch (bit_rate)
1028 {
1029 case 16000:
1030 s->enc_func = g726_16_encoder;
1031 s->dec_func = g726_16_decoder;
1032 s->bits_per_sample = 2;
1033 break;
1034 case 24000:
1035 s->enc_func = g726_24_encoder;
1036 s->dec_func = g726_24_decoder;
1037 s->bits_per_sample = 3;
1038 break;
1039 case 32000:
1040 default:
1041 s->enc_func = g726_32_encoder;
1042 s->dec_func = g726_32_decoder;
1043 s->bits_per_sample = 4;
1044 break;
1045 case 40000:
1046 s->enc_func = g726_40_encoder;
1047 s->dec_func = g726_40_decoder;
1048 s->bits_per_sample = 5;
1049 break;
1050 }
1051 bitstream_init(&s->bs);
1052 return s;
1053 }
1054 /*- End of function --------------------------------------------------------*/
1055
1056 int g726_release(g726_state_t *s)
1057 {
1058 free(s);
1059 return 0;
1060 }
1061 /*- End of function --------------------------------------------------------*/
1062
1063 int g726_decode(g726_state_t *s,
1064 int16_t amp[],
1065 const uint8_t g726_data[],
1066 int g726_bytes)
1067 {
1068 int i;
1069 int samples;
1070 uint8_t code;
1071 int sl;
1072
1073 for (samples = i = 0; ; )
1074 {
1075 if (s->packing != G726_PACKING_NONE)
1076 {
1077 /* Unpack the code bits */
1078 if (s->packing != G726_PACKING_LEFT)
1079 {
1080 if (s->bs.residue < s->bits_per_sample)
1081 {
1082 if (i >= g726_bytes)
1083 break;
1084 s->bs.bitstream |= (g726_data[i++] << s->bs.residue);
1085 s->bs.residue += 8;
1086 }
1087 code = (uint8_t) (s->bs.bitstream & ((1 << s->bits_per_sample) - 1));
1088 s->bs.bitstream >>= s->bits_per_sample;
1089 }
1090 else
1091 {
1092 if (s->bs.residue < s->bits_per_sample)
1093 {
1094 if (i >= g726_bytes)
1095 break;
1096 s->bs.bitstream = (s->bs.bitstream << 8) | g726_data[i++];
1097 s->bs.residue += 8;
1098 }
1099 code = (uint8_t) ((s->bs.bitstream >> (s->bs.residue - s->bits_per_sample)) & ((1 << s->bits_per_sample) - 1));
1100 }
1101 s->bs.residue -= s->bits_per_sample;
1102 }
1103 else
1104 {
1105 if (i >= g726_bytes)
1106 break;
1107 code = g726_data[i++];
1108 }
1109 sl = s->dec_func(s, code);
1110 if (s->ext_coding != G726_ENCODING_LINEAR)
1111 ((uint8_t *) amp)[samples++] = (uint8_t) sl;
1112 else
1113 amp[samples++] = (int16_t) sl;
1114 }
1115 return samples;
1116 }
1117 /*- End of function --------------------------------------------------------*/
1118
1119 int g726_encode(g726_state_t *s,
1120 uint8_t g726_data[],
1121 const int16_t amp[],
1122 int len)
1123 {
1124 int i;
1125 int g726_bytes;
1126 int16_t sl;
1127 uint8_t code;
1128
1129 for (g726_bytes = i = 0; i < len; i++)
1130 {
1131 /* Linearize the input sample to 14-bit PCM */
1132 switch (s->ext_coding)
1133 {
1134 case G726_ENCODING_ALAW:
1135 sl = alaw_to_linear(((const uint8_t *) amp)[i]) >> 2;
1136 break;
1137 case G726_ENCODING_ULAW:
1138 sl = ulaw_to_linear(((const uint8_t *) amp)[i]) >> 2;
1139 break;
1140 default:
1141 sl = amp[i] >> 2;
1142 break;
1143 }
1144 code = s->enc_func(s, sl);
1145 if (s->packing != G726_PACKING_NONE)
1146 {
1147 /* Pack the code bits */
1148 if (s->packing != G726_PACKING_LEFT)
1149 {
1150 s->bs.bitstream |= (code << s->bs.residue);
1151 s->bs.residue += s->bits_per_sample;
1152 if (s->bs.residue >= 8)
1153 {
1154 g726_data[g726_bytes++] = (uint8_t) (s->bs.bitstream & 0xFF);
1155 s->bs.bitstream >>= 8;
1156 s->bs.residue -= 8;
1157 }
1158 }
1159 else
1160 {
1161 s->bs.bitstream = (s->bs.bitstream << s->bits_per_sample) | code;
1162 s->bs.residue += s->bits_per_sample;
1163 if (s->bs.residue >= 8)
1164 {
1165 g726_data[g726_bytes++] = (uint8_t) ((s->bs.bitstream >> (s->bs.residue - 8)) & 0xFF);
1166 s->bs.residue -= 8;
1167 }
1168 }
1169 }
1170 else
1171 {
1172 g726_data[g726_bytes++] = (uint8_t) code;
1173 }
1174 }
1175 return g726_bytes;
1176 }
1177 /*- End of function --------------------------------------------------------*/
1178 /*- End of file ------------------------------------------------------------*/

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