issue #5605
[asterisk/asterisk.git] / codecs / codec_g726.c
1 /*
2  * Asterisk -- An open source telephony toolkit.
3  *
4  * Copyright (C) 1999 - 2005, Digium, Inc.
5  *
6  * Mark Spencer <markster@digium.com>
7  *
8  * Based on frompcm.c and topcm.c from the Emiliano MIPL browser/
9  * interpreter.  See http://www.bsdtelephony.com.mx
10  *
11  * See http://www.asterisk.org for more information about
12  * the Asterisk project. Please do not directly contact
13  * any of the maintainers of this project for assistance;
14  * the project provides a web site, mailing lists and IRC
15  * channels for your use.
16  *
17  * This program is free software, distributed under the terms of
18  * the GNU General Public License Version 2. See the LICENSE file
19  * at the top of the source tree.
20  */
21
22
23 /*! \file
24  *
25  * \brief codec_g726.c - translate between signed linear and ITU G.726-32kbps
26  *
27  * \ingroup codecs
28  */
29
30 #include <fcntl.h>
31 #include <netinet/in.h>
32 #include <stdio.h>
33 #include <stdlib.h>
34 #include <string.h>
35 #include <unistd.h>
36
37 #include "asterisk.h"
38
39 ASTERISK_FILE_VERSION(__FILE__, "$Revision$")
40
41 #include "asterisk/lock.h"
42 #include "asterisk/logger.h"
43 #include "asterisk/module.h"
44 #include "asterisk/config.h"
45 #include "asterisk/options.h"
46 #include "asterisk/translate.h"
47 #include "asterisk/channel.h"
48
49 #define WANT_ASM
50 #include "log2comp.h"
51
52 /* define NOT_BLI to use a faster but not bit-level identical version */
53 /* #define NOT_BLI */
54
55 #if defined(NOT_BLI)
56 #       if defined(_MSC_VER)
57 typedef __int64 sint64;
58 #       elif defined(__GNUC__)
59 typedef long long sint64;
60 #       else
61 #               error 64-bit integer type is not defined for your compiler/platform
62 #       endif
63 #endif
64
65 #define BUFFER_SIZE   8096      /* size for the translation buffers */
66 #define BUF_SHIFT       5
67
68 AST_MUTEX_DEFINE_STATIC(localuser_lock);
69 static int localusecnt = 0;
70
71 static char *tdesc = "ITU G.726-32kbps G726 Transcoder";
72
73 static int useplc = 0;
74
75 /* Sample frame data */
76
77 #include "slin_g726_ex.h"
78 #include "g726_slin_ex.h"
79
80 /*
81  * The following is the definition of the state structure
82  * used by the G.721/G.723 encoder and decoder to preserve their internal
83  * state between successive calls.  The meanings of the majority
84  * of the state structure fields are explained in detail in the
85  * CCITT Recommendation G.721.  The field names are essentially indentical
86  * to variable names in the bit level description of the coding algorithm
87  * included in this Recommendation.
88  */
89 struct g726_state {
90         long yl;        /* Locked or steady state step size multiplier. */
91         int yu;         /* Unlocked or non-steady state step size multiplier. */
92         int dms;        /* Short term energy estimate. */
93         int dml;        /* Long term energy estimate. */
94         int ap;         /* Linear weighting coefficient of 'yl' and 'yu'. */
95
96         int a[2];       /* Coefficients of pole portion of prediction filter.
97                                  * stored as fixed-point 1==2^14 */
98         int b[6];       /* Coefficients of zero portion of prediction filter.
99                                  * stored as fixed-point 1==2^14 */
100         int pk[2];      /* Signs of previous two samples of a partially
101                          * reconstructed signal.
102                          */
103         int dq[6];  /* Previous 6 samples of the quantized difference signal
104                                  * stored as fixed point 1==2^12,
105                                  * or in internal floating point format */
106         int sr[2];      /* Previous 2 samples of the quantized difference signal
107                                  * stored as fixed point 1==2^12,
108                                  * or in internal floating point format */
109         int td; /* delayed tone detect, new in 1988 version */
110 };
111
112
113
114 static int qtab_721[7] = {-124, 80, 178, 246, 300, 349, 400};
115 /*
116  * Maps G.721 code word to reconstructed scale factor normalized log
117  * magnitude values.
118  */
119 static int _dqlntab[16] = {-2048, 4, 135, 213, 273, 323, 373, 425,
120                                 425, 373, 323, 273, 213, 135, 4, -2048};
121
122 /* Maps G.721 code word to log of scale factor multiplier. */
123 static int _witab[16] = {-12, 18, 41, 64, 112, 198, 355, 1122,
124                                 1122, 355, 198, 112, 64, 41, 18, -12};
125 /*
126  * Maps G.721 code words to a set of values whose long and short
127  * term averages are computed and then compared to give an indication
128  * how stationary (steady state) the signal is.
129  */
130 static int _fitab[16] = {0, 0, 0, 0x200, 0x200, 0x200, 0x600, 0xE00,
131                                 0xE00, 0x600, 0x200, 0x200, 0x200, 0, 0, 0};
132
133 /* Deprecated
134 static int power2[15] = {1, 2, 4, 8, 0x10, 0x20, 0x40, 0x80,
135                         0x100, 0x200, 0x400, 0x800, 0x1000, 0x2000, 0x4000};
136 */
137
138 /*
139  * g72x_init_state()
140  *
141  * This routine initializes and/or resets the g726_state structure
142  * pointed to by 'state_ptr'.
143  * All the initial state values are specified in the CCITT G.721 document.
144  */
145 static void g726_init_state(struct g726_state *state_ptr)
146 {
147         int             cnta;
148
149         state_ptr->yl = 34816;
150         state_ptr->yu = 544;
151         state_ptr->dms = 0;
152         state_ptr->dml = 0;
153         state_ptr->ap = 0;
154         for (cnta = 0; cnta < 2; cnta++)
155         {
156                 state_ptr->a[cnta] = 0;
157                 state_ptr->pk[cnta] = 0;
158 #ifdef NOT_BLI
159                 state_ptr->sr[cnta] = 1;
160 #else
161                 state_ptr->sr[cnta] = 32;
162 #endif
163         }
164         for (cnta = 0; cnta < 6; cnta++)
165         {
166                 state_ptr->b[cnta] = 0;
167 #ifdef NOT_BLI
168                 state_ptr->dq[cnta] = 1;
169 #else
170                 state_ptr->dq[cnta] = 32;
171 #endif
172         }
173         state_ptr->td = 0;
174 }
175
176 /*
177  * quan()
178  *
179  * quantizes the input val against the table of integers.
180  * It returns i if table[i - 1] <= val < table[i].
181  *
182  * Using linear search for simple coding.
183  */
184 static int quan(int val, int *table, int size)
185 {
186         int             i;
187
188         for (i = 0; i < size && val >= *table; ++i, ++table)
189                 ;
190         return (i);
191 }
192
193 #ifdef NOT_BLI /* faster non-identical version */
194
195 /*
196  * predictor_zero()
197  *
198  * computes the estimated signal from 6-zero predictor.
199  *
200  */
201 static int predictor_zero(struct g726_state *state_ptr)
202 {       /* divide by 2 is necessary here to handle negative numbers correctly */
203         int i;
204         sint64 sezi;
205         for (sezi = 0, i = 0; i < 6; i++)                       /* ACCUM */
206                 sezi += (sint64)state_ptr->b[i] * state_ptr->dq[i];
207         return (int)(sezi >> 13) / 2 /* 2^14 */;
208 }
209
210 /*
211  * predictor_pole()
212  *
213  * computes the estimated signal from 2-pole predictor.
214  *
215  */
216 static int predictor_pole(struct g726_state *state_ptr)
217 {       /* divide by 2 is necessary here to handle negative numbers correctly */
218         return (int)(((sint64)state_ptr->a[1] * state_ptr->sr[1] +
219                       (sint64)state_ptr->a[0] * state_ptr->sr[0]) >> 13) / 2 /* 2^14 */;
220 }
221
222 #else /* NOT_BLI - identical version */
223 /*
224  * fmult()
225  *
226  * returns the integer product of the fixed-point number "an" (1==2^12) and
227  * "floating point" representation (4-bit exponent, 6-bit mantessa) "srn".
228  */
229 static int fmult(int an, int srn)
230 {
231         int             anmag, anexp, anmant;
232         int             wanexp, wanmant;
233         int             retval;
234
235         anmag = (an > 0) ? an : ((-an) & 0x1FFF);
236         anexp = ilog2(anmag) - 5;
237         anmant = (anmag == 0) ? 32 :
238             (anexp >= 0) ? anmag >> anexp : anmag << -anexp;
239         wanexp = anexp + ((srn >> 6) & 0xF) - 13;
240
241         wanmant = (anmant * (srn & 077) + 0x30) >> 4;
242         retval = (wanexp >= 0) ? ((wanmant << wanexp) & 0x7FFF) :
243             (wanmant >> -wanexp);
244
245         return (((an ^ srn) < 0) ? -retval : retval);
246 }
247
248 static int predictor_zero(struct g726_state *state_ptr)
249 {
250         int             i;
251         int             sezi;
252         for (sezi = 0, i = 0; i < 6; i++)                       /* ACCUM */
253                 sezi += fmult(state_ptr->b[i] >> 2, state_ptr->dq[i]);
254         return sezi;
255 }
256
257 static int predictor_pole(struct g726_state *state_ptr)
258 {
259         return (fmult(state_ptr->a[1] >> 2, state_ptr->sr[1]) +
260                         fmult(state_ptr->a[0] >> 2, state_ptr->sr[0]));
261 }
262
263 #endif /* NOT_BLI */
264
265 /*
266  * step_size()
267  *
268  * computes the quantization step size of the adaptive quantizer.
269  *
270  */
271 static int step_size(struct g726_state *state_ptr)
272 {
273         int             y;
274         int             dif;
275         int             al;
276
277         if (state_ptr->ap >= 256)
278                 return (state_ptr->yu);
279         else {
280                 y = state_ptr->yl >> 6;
281                 dif = state_ptr->yu - y;
282                 al = state_ptr->ap >> 2;
283                 if (dif > 0)
284                         y += (dif * al) >> 6;
285                 else if (dif < 0)
286                         y += (dif * al + 0x3F) >> 6;
287                 return (y);
288         }
289 }
290
291 /*
292  * quantize()
293  *
294  * Given a raw sample, 'd', of the difference signal and a
295  * quantization step size scale factor, 'y', this routine returns the
296  * ADPCM codeword to which that sample gets quantized.  The step
297  * size scale factor division operation is done in the log base 2 domain
298  * as a subtraction.
299  */
300 static int quantize(
301         int             d,      /* Raw difference signal sample */
302         int             y,      /* Step size multiplier */
303         int             *table, /* quantization table */
304         int             size)   /* table size of integers */
305 {
306         int             dqm;    /* Magnitude of 'd' */
307         int             exp;    /* Integer part of base 2 log of 'd' */
308         int             mant;   /* Fractional part of base 2 log */
309         int             dl;             /* Log of magnitude of 'd' */
310         int             dln;    /* Step size scale factor normalized log */
311         int             i;
312
313         /*
314          * LOG
315          *
316          * Compute base 2 log of 'd', and store in 'dl'.
317          */
318         dqm = abs(d);
319         exp = ilog2(dqm);
320         if (exp < 0)
321                 exp = 0;
322         mant = ((dqm << 7) >> exp) & 0x7F;      /* Fractional portion. */
323         dl = (exp << 7) | mant;
324
325         /*
326          * SUBTB
327          *
328          * "Divide" by step size multiplier.
329          */
330         dln = dl - (y >> 2);
331
332         /*
333          * QUAN
334          *
335          * Obtain codword i for 'd'.
336          */
337         i = quan(dln, table, size);
338         if (d < 0)                      /* take 1's complement of i */
339                 return ((size << 1) + 1 - i);
340         else if (i == 0)                /* take 1's complement of 0 */
341                 return ((size << 1) + 1); /* new in 1988 */
342         else
343                 return (i);
344 }
345
346 /*
347  * reconstruct()
348  *
349  * Returns reconstructed difference signal 'dq' obtained from
350  * codeword 'i' and quantization step size scale factor 'y'.
351  * Multiplication is performed in log base 2 domain as addition.
352  */
353 static int reconstruct(
354         int             sign,   /* 0 for non-negative value */
355         int             dqln,   /* G.72x codeword */
356         int             y)      /* Step size multiplier */
357 {
358         int             dql;    /* Log of 'dq' magnitude */
359         int             dex;    /* Integer part of log */
360         int             dqt;
361         int             dq;     /* Reconstructed difference signal sample */
362
363         dql = dqln + (y >> 2);  /* ADDA */
364
365         if (dql < 0) {
366 #ifdef NOT_BLI
367                 return (sign) ? -1 : 1;
368 #else
369                 return (sign) ? -0x8000 : 0;
370 #endif
371         } else {                /* ANTILOG */
372                 dex = (dql >> 7) & 15;
373                 dqt = 128 + (dql & 127);
374 #ifdef NOT_BLI
375                 dq = ((dqt << 19) >> (14 - dex));
376                 return (sign) ? -dq : dq;
377 #else
378                 dq = (dqt << 7) >> (14 - dex);
379                 return (sign) ? (dq - 0x8000) : dq;
380 #endif
381         }
382 }
383
384 /*
385  * update()
386  *
387  * updates the state variables for each output code
388  */
389 static void update(
390         int             code_size,      /* distinguish 723_40 with others */
391         int             y,              /* quantizer step size */
392         int             wi,             /* scale factor multiplier */
393         int             fi,             /* for long/short term energies */
394         int             dq,             /* quantized prediction difference */
395         int             sr,             /* reconstructed signal */
396         int             dqsez,          /* difference from 2-pole predictor */
397         struct g726_state *state_ptr)   /* coder state pointer */
398 {
399         int             cnt;
400         int             mag;            /* Adaptive predictor, FLOAT A */
401 #ifndef NOT_BLI
402         int             exp;
403 #endif
404         int             a2p=0;          /* LIMC */
405         int             a1ul;           /* UPA1 */
406         int             pks1;           /* UPA2 */
407         int             fa1;
408         int             tr;                     /* tone/transition detector */
409         int             ylint, thr2, dqthr;
410         int             ylfrac, thr1;
411         int             pk0;
412
413         pk0 = (dqsez < 0) ? 1 : 0;      /* needed in updating predictor poles */
414
415 #ifdef NOT_BLI
416         mag = abs(dq / 0x1000); /* prediction difference magnitude */
417 #else
418         mag = dq & 0x7FFF;              /* prediction difference magnitude */
419 #endif
420         /* TRANS */
421         ylint = state_ptr->yl >> 15;    /* exponent part of yl */
422         ylfrac = (state_ptr->yl >> 10) & 0x1F;  /* fractional part of yl */
423         thr1 = (32 + ylfrac) << ylint;          /* threshold */
424         thr2 = (ylint > 9) ? 31 << 10 : thr1;   /* limit thr2 to 31 << 10 */
425         dqthr = (thr2 + (thr2 >> 1)) >> 1;      /* dqthr = 0.75 * thr2 */
426         if (state_ptr->td == 0)         /* signal supposed voice */
427                 tr = 0;
428         else if (mag <= dqthr)          /* supposed data, but small mag */
429                 tr = 0;                 /* treated as voice */
430         else                            /* signal is data (modem) */
431                 tr = 1;
432
433         /*
434          * Quantizer scale factor adaptation.
435          */
436
437         /* FUNCTW & FILTD & DELAY */
438         /* update non-steady state step size multiplier */
439         state_ptr->yu = y + ((wi - y) >> 5);
440
441         /* LIMB */
442         if (state_ptr->yu < 544)        /* 544 <= yu <= 5120 */
443                 state_ptr->yu = 544;
444         else if (state_ptr->yu > 5120)
445                 state_ptr->yu = 5120;
446
447         /* FILTE & DELAY */
448         /* update steady state step size multiplier */
449         state_ptr->yl += state_ptr->yu + ((-state_ptr->yl) >> 6);
450
451         /*
452          * Adaptive predictor coefficients.
453          */
454         if (tr == 1) {                  /* reset a's and b's for modem signal */
455                 state_ptr->a[0] = 0;
456                 state_ptr->a[1] = 0;
457                 state_ptr->b[0] = 0;
458                 state_ptr->b[1] = 0;
459                 state_ptr->b[2] = 0;
460                 state_ptr->b[3] = 0;
461                 state_ptr->b[4] = 0;
462                 state_ptr->b[5] = 0;
463         } else {                        /* update a's and b's */
464                 pks1 = pk0 ^ state_ptr->pk[0];          /* UPA2 */
465
466                 /* update predictor pole a[1] */
467                 a2p = state_ptr->a[1] - (state_ptr->a[1] >> 7);
468                 if (dqsez != 0) {
469                         fa1 = (pks1) ? state_ptr->a[0] : -state_ptr->a[0];
470                         if (fa1 < -8191)        /* a2p = function of fa1 */
471                                 a2p -= 0x100;
472                         else if (fa1 > 8191)
473                                 a2p += 0xFF;
474                         else
475                                 a2p += fa1 >> 5;
476
477                         if (pk0 ^ state_ptr->pk[1])
478                                 /* LIMC */
479                                 if (a2p <= -12160)
480                                         a2p = -12288;
481                                 else if (a2p >= 12416)
482                                         a2p = 12288;
483                                 else
484                                         a2p -= 0x80;
485                         else if (a2p <= -12416)
486                                 a2p = -12288;
487                         else if (a2p >= 12160)
488                                 a2p = 12288;
489                         else
490                                 a2p += 0x80;
491                 }
492
493                 /* TRIGB & DELAY */
494                 state_ptr->a[1] = a2p;
495
496                 /* UPA1 */
497                 /* update predictor pole a[0] */
498                 state_ptr->a[0] -= state_ptr->a[0] >> 8;
499                 if (dqsez != 0) {
500                         if (pks1 == 0)
501                                 state_ptr->a[0] += 192;
502                         else
503                                 state_ptr->a[0] -= 192;
504                 }
505                 /* LIMD */
506                 a1ul = 15360 - a2p;
507                 if (state_ptr->a[0] < -a1ul)
508                         state_ptr->a[0] = -a1ul;
509                 else if (state_ptr->a[0] > a1ul)
510                         state_ptr->a[0] = a1ul;
511
512                 /* UPB : update predictor zeros b[6] */
513                 for (cnt = 0; cnt < 6; cnt++) {
514                         if (code_size == 5)             /* for 40Kbps G.723 */
515                                 state_ptr->b[cnt] -= state_ptr->b[cnt] >> 9;
516                         else                    /* for G.721 and 24Kbps G.723 */
517                                 state_ptr->b[cnt] -= state_ptr->b[cnt] >> 8;
518                         if (mag)
519                         {       /* XOR */
520                                 if ((dq ^ state_ptr->dq[cnt]) >= 0)
521                                         state_ptr->b[cnt] += 128;
522                                 else
523                                         state_ptr->b[cnt] -= 128;
524                         }
525                 }
526         }
527
528         for (cnt = 5; cnt > 0; cnt--)
529                 state_ptr->dq[cnt] = state_ptr->dq[cnt-1];
530 #ifdef NOT_BLI
531         state_ptr->dq[0] = dq;
532 #else
533         /* FLOAT A : convert dq[0] to 4-bit exp, 6-bit mantissa f.p. */
534         if (mag == 0) {
535                 state_ptr->dq[0] = (dq >= 0) ? 0x20 : 0x20 - 0x400;
536         } else {
537                 exp = ilog2(mag) + 1;
538                 state_ptr->dq[0] = (dq >= 0) ?
539                     (exp << 6) + ((mag << 6) >> exp) :
540                     (exp << 6) + ((mag << 6) >> exp) - 0x400;
541         }
542 #endif
543
544         state_ptr->sr[1] = state_ptr->sr[0];
545 #ifdef NOT_BLI
546         state_ptr->sr[0] = sr;
547 #else
548         /* FLOAT B : convert sr to 4-bit exp., 6-bit mantissa f.p. */
549         if (sr == 0) {
550                 state_ptr->sr[0] = 0x20;
551         } else if (sr > 0) {
552                 exp = ilog2(sr) + 1;
553                 state_ptr->sr[0] = (exp << 6) + ((sr << 6) >> exp);
554         } else if (sr > -0x8000) {
555                 mag = -sr;
556                 exp = ilog2(mag) + 1;
557                 state_ptr->sr[0] =  (exp << 6) + ((mag << 6) >> exp) - 0x400;
558         } else
559                 state_ptr->sr[0] = 0x20 - 0x400;
560 #endif
561
562         /* DELAY A */
563         state_ptr->pk[1] = state_ptr->pk[0];
564         state_ptr->pk[0] = pk0;
565
566         /* TONE */
567         if (tr == 1)            /* this sample has been treated as data */
568                 state_ptr->td = 0;      /* next one will be treated as voice */
569         else if (a2p < -11776)  /* small sample-to-sample correlation */
570                 state_ptr->td = 1;      /* signal may be data */
571         else                            /* signal is voice */
572                 state_ptr->td = 0;
573
574         /*
575          * Adaptation speed control.
576          */
577         state_ptr->dms += (fi - state_ptr->dms) >> 5;           /* FILTA */
578         state_ptr->dml += (((fi << 2) - state_ptr->dml) >> 7);  /* FILTB */
579
580         if (tr == 1)
581                 state_ptr->ap = 256;
582         else if (y < 1536)                                      /* SUBTC */
583                 state_ptr->ap += (0x200 - state_ptr->ap) >> 4;
584         else if (state_ptr->td == 1)
585                 state_ptr->ap += (0x200 - state_ptr->ap) >> 4;
586         else if (abs((state_ptr->dms << 2) - state_ptr->dml) >=
587             (state_ptr->dml >> 3))
588                 state_ptr->ap += (0x200 - state_ptr->ap) >> 4;
589         else
590                 state_ptr->ap += (-state_ptr->ap) >> 4;
591 }
592
593 /*
594  * g726_decode()
595  *
596  * Description:
597  *
598  * Decodes a 4-bit code of G.726-32 encoded data of i and
599  * returns the resulting linear PCM, A-law or u-law value.
600  * return -1 for unknown out_coding value.
601  */
602 static int g726_decode(int      i, struct g726_state *state_ptr)
603 {
604         int             sezi, sez, se;  /* ACCUM */
605         int             y;                      /* MIX */
606         int             sr;                     /* ADDB */
607         int             dq;
608         int             dqsez;
609
610         i &= 0x0f;                      /* mask to get proper bits */
611 #ifdef NOT_BLI
612         sezi = predictor_zero(state_ptr);
613         sez = sezi;
614         se = sezi + predictor_pole(state_ptr);  /* estimated signal */
615 #else
616         sezi = predictor_zero(state_ptr);
617         sez = sezi >> 1;
618         se = (sezi + predictor_pole(state_ptr)) >> 1;   /* estimated signal */
619 #endif
620
621         y = step_size(state_ptr);       /* dynamic quantizer step size */
622
623         dq = reconstruct(i & 8, _dqlntab[i], y); /* quantized diff. */
624
625 #ifdef NOT_BLI
626         sr = se + dq;                           /* reconst. signal */
627         dqsez = dq + sez;                       /* pole prediction diff. */
628 #else
629         sr = (dq < 0) ? se - (dq & 0x3FFF) : se + dq;   /* reconst. signal */
630         dqsez = sr - se + sez;          /* pole prediction diff. */
631 #endif
632
633         update(4, y, _witab[i] << 5, _fitab[i], dq, sr, dqsez, state_ptr);
634
635 #ifdef NOT_BLI
636         return (sr >> 10);      /* sr was 26-bit dynamic range */
637 #else
638         return (sr << 2);       /* sr was 14-bit dynamic range */
639 #endif
640 }
641
642 /*
643  * g726_encode()
644  *
645  * Encodes the input vale of linear PCM, A-law or u-law data sl and returns
646  * the resulting code. -1 is returned for unknown input coding value.
647  */
648 static int g726_encode(int sl, struct g726_state *state_ptr)
649 {
650         int             sezi, se, sez;          /* ACCUM */
651         int             d;                      /* SUBTA */
652         int             sr;                     /* ADDB */
653         int             y;                      /* MIX */
654         int             dqsez;                  /* ADDC */
655         int             dq, i;
656
657 #ifdef NOT_BLI
658         sl <<= 10;                      /* 26-bit dynamic range */
659
660         sezi = predictor_zero(state_ptr);
661         sez = sezi;
662         se = sezi + predictor_pole(state_ptr);  /* estimated signal */
663 #else
664         sl >>= 2;                       /* 14-bit dynamic range */
665
666         sezi = predictor_zero(state_ptr);
667         sez = sezi >> 1;
668         se = (sezi + predictor_pole(state_ptr)) >> 1;   /* estimated signal */
669 #endif
670
671         d = sl - se;                            /* estimation difference */
672
673         /* quantize the prediction difference */
674         y = step_size(state_ptr);               /* quantizer step size */
675 #ifdef NOT_BLI
676         d /= 0x1000;
677 #endif
678         i = quantize(d, y, qtab_721, 7);        /* i = G726 code */
679
680         dq = reconstruct(i & 8, _dqlntab[i], y);        /* quantized est diff */
681
682 #ifdef NOT_BLI
683         sr = se + dq;                           /* reconst. signal */
684         dqsez = dq + sez;                       /* pole prediction diff. */
685 #else
686         sr = (dq < 0) ? se - (dq & 0x3FFF) : se + dq;   /* reconst. signal */
687         dqsez = sr - se + sez;                  /* pole prediction diff. */
688 #endif
689
690         update(4, y, _witab[i] << 5, _fitab[i], dq, sr, dqsez, state_ptr);
691
692         return (i);
693 }
694
695 /*
696  * Private workspace for translating signed linear signals to G726.
697  */
698
699 struct g726_encoder_pvt
700 {
701   struct ast_frame f;
702   char offset[AST_FRIENDLY_OFFSET];   /* Space to build offset */
703   unsigned char outbuf[BUFFER_SIZE];  /* Encoded G726, two nibbles to a word */
704   unsigned char next_flag;
705   struct g726_state g726;
706   int tail;
707 };
708
709 /*
710  * Private workspace for translating G726 signals to signed linear.
711  */
712
713 struct g726_decoder_pvt
714 {
715   struct ast_frame f;
716   char offset[AST_FRIENDLY_OFFSET];     /* Space to build offset */
717   short outbuf[BUFFER_SIZE];    /* Decoded signed linear values */
718   struct g726_state g726;
719   int tail;
720   plc_state_t plc;
721 };
722
723 /*
724  * G726ToLin_New
725  *  Create a new instance of g726_decoder_pvt.
726  *
727  * Results:
728  *  Returns a pointer to the new instance.
729  *
730  * Side effects:
731  *  None.
732  */
733
734 static struct ast_translator_pvt *
735 g726tolin_new (void)
736 {
737   struct g726_decoder_pvt *tmp;
738   tmp = malloc (sizeof (struct g726_decoder_pvt));
739   if (tmp)
740     {
741           memset(tmp, 0, sizeof(*tmp));
742       tmp->tail = 0;
743       plc_init(&tmp->plc);
744       localusecnt++;
745           g726_init_state(&tmp->g726);
746       ast_update_use_count ();
747     }
748   return (struct ast_translator_pvt *) tmp;
749 }
750
751 /*
752  * LinToG726_New
753  *  Create a new instance of g726_encoder_pvt.
754  *
755  * Results:
756  *  Returns a pointer to the new instance.
757  *
758  * Side effects:
759  *  None.
760  */
761
762 static struct ast_translator_pvt *
763 lintog726_new (void)
764 {
765   struct g726_encoder_pvt *tmp;
766   tmp = malloc (sizeof (struct g726_encoder_pvt));
767   if (tmp)
768     {
769           memset(tmp, 0, sizeof(*tmp));
770       localusecnt++;
771       tmp->tail = 0;
772           g726_init_state(&tmp->g726);
773       ast_update_use_count ();
774     }
775   return (struct ast_translator_pvt *) tmp;
776 }
777
778 /*
779  * G726ToLin_FrameIn
780  *  Fill an input buffer with packed 4-bit G726 values if there is room
781  *  left.
782  *
783  * Results:
784  *  Foo
785  *
786  * Side effects:
787  *  tmp->tail is the number of packed values in the buffer.
788  */
789
790 static int
791 g726tolin_framein (struct ast_translator_pvt *pvt, struct ast_frame *f)
792 {
793   struct g726_decoder_pvt *tmp = (struct g726_decoder_pvt *) pvt;
794   unsigned char *b;
795   int x;
796
797   if(f->datalen == 0) { /* perform PLC with nominal framesize of 20ms/160 samples */
798         if((tmp->tail + 160) > BUFFER_SIZE) {
799             ast_log(LOG_WARNING, "Out of buffer space\n");
800             return -1;
801         }
802         if(useplc) {
803             plc_fillin(&tmp->plc, tmp->outbuf+tmp->tail, 160);
804             tmp->tail += 160;
805         }
806         return 0;
807   }
808
809   b = f->data;
810   for (x=0;x<f->datalen;x++) {
811         if (tmp->tail >= BUFFER_SIZE) {
812                 ast_log(LOG_WARNING, "Out of buffer space!\n");
813                 return -1;
814         }
815         tmp->outbuf[tmp->tail++] = g726_decode((b[x] >> 4) & 0xf, &tmp->g726);
816         if (tmp->tail >= BUFFER_SIZE) {
817                 ast_log(LOG_WARNING, "Out of buffer space!\n");
818                 return -1;
819         }
820         tmp->outbuf[tmp->tail++] = g726_decode(b[x] & 0x0f, &tmp->g726);
821   }
822
823   if(useplc) plc_rx(&tmp->plc, tmp->outbuf+tmp->tail-f->datalen*2, f->datalen*2);
824
825   return 0;
826 }
827
828 /*
829  * G726ToLin_FrameOut
830  *  Convert 4-bit G726 encoded signals to 16-bit signed linear.
831  *
832  * Results:
833  *  Converted signals are placed in tmp->f.data, tmp->f.datalen
834  *  and tmp->f.samples are calculated.
835  *
836  * Side effects:
837  *  None.
838  */
839
840 static struct ast_frame *
841 g726tolin_frameout (struct ast_translator_pvt *pvt)
842 {
843   struct g726_decoder_pvt *tmp = (struct g726_decoder_pvt *) pvt;
844
845   if (!tmp->tail)
846     return NULL;
847
848   tmp->f.frametype = AST_FRAME_VOICE;
849   tmp->f.subclass = AST_FORMAT_SLINEAR;
850   tmp->f.datalen = tmp->tail * 2;
851   tmp->f.samples = tmp->tail;
852   tmp->f.mallocd = 0;
853   tmp->f.offset = AST_FRIENDLY_OFFSET;
854   tmp->f.src = __PRETTY_FUNCTION__;
855   tmp->f.data = tmp->outbuf;
856   tmp->tail = 0;
857   return &tmp->f;
858 }
859
860 /*
861  * LinToG726_FrameIn
862  *  Fill an input buffer with 16-bit signed linear PCM values.
863  *
864  * Results:
865  *  None.
866  *
867  * Side effects:
868  *  tmp->tail is number of signal values in the input buffer.
869  */
870
871 static int
872 lintog726_framein (struct ast_translator_pvt *pvt, struct ast_frame *f)
873 {
874   struct g726_encoder_pvt *tmp = (struct g726_encoder_pvt *) pvt;
875   short *s = f->data;
876   int samples = f->datalen / 2;
877   int x;
878   for (x=0;x<samples;x++) {
879         if (tmp->next_flag & 0x80) {
880                 if (tmp->tail >= BUFFER_SIZE) {
881                         ast_log(LOG_WARNING, "Out of buffer space\n");
882                         return -1;
883                 }
884                 tmp->outbuf[tmp->tail++] = ((tmp->next_flag & 0xf)<< 4) | g726_encode(s[x], &tmp->g726);
885                 tmp->next_flag = 0;
886         } else {
887                 tmp->next_flag = 0x80 | g726_encode(s[x], &tmp->g726);
888         }
889   }
890   return 0;
891 }
892
893 /*
894  * LinToG726_FrameOut
895  *  Convert a buffer of raw 16-bit signed linear PCM to a buffer
896  *  of 4-bit G726 packed two to a byte (Big Endian).
897  *
898  * Results:
899  *  Foo
900  *
901  * Side effects:
902  *  Leftover inbuf data gets packed, tail gets updated.
903  */
904
905 static struct ast_frame *
906 lintog726_frameout (struct ast_translator_pvt *pvt)
907 {
908   struct g726_encoder_pvt *tmp = (struct g726_encoder_pvt *) pvt;
909   
910   if (!tmp->tail)
911         return NULL;
912   tmp->f.frametype = AST_FRAME_VOICE;
913   tmp->f.subclass = AST_FORMAT_G726;
914   tmp->f.samples = tmp->tail * 2;
915   tmp->f.mallocd = 0;
916   tmp->f.offset = AST_FRIENDLY_OFFSET;
917   tmp->f.src = __PRETTY_FUNCTION__;
918   tmp->f.data = tmp->outbuf;
919   tmp->f.datalen = tmp->tail;
920
921   tmp->tail = 0;
922   return &tmp->f;
923 }
924
925
926 /*
927  * G726ToLin_Sample
928  */
929
930 static struct ast_frame *
931 g726tolin_sample (void)
932 {
933   static struct ast_frame f;
934   f.frametype = AST_FRAME_VOICE;
935   f.subclass = AST_FORMAT_G726;
936   f.datalen = sizeof (g726_slin_ex);
937   f.samples = sizeof(g726_slin_ex) * 2;
938   f.mallocd = 0;
939   f.offset = 0;
940   f.src = __PRETTY_FUNCTION__;
941   f.data = g726_slin_ex;
942   return &f;
943 }
944
945 /*
946  * LinToG726_Sample
947  */
948
949 static struct ast_frame *
950 lintog726_sample (void)
951 {
952   static struct ast_frame f;
953   f.frametype = AST_FRAME_VOICE;
954   f.subclass = AST_FORMAT_SLINEAR;
955   f.datalen = sizeof (slin_g726_ex);
956   /* Assume 8000 Hz */
957   f.samples = sizeof (slin_g726_ex) / 2;
958   f.mallocd = 0;
959   f.offset = 0;
960   f.src = __PRETTY_FUNCTION__;
961   f.data = slin_g726_ex;
962   return &f;
963 }
964
965 /*
966  * G726_Destroy
967  *  Destroys a private workspace.
968  *
969  * Results:
970  *  It's gone!
971  *
972  * Side effects:
973  *  None.
974  */
975
976 static void
977 g726_destroy (struct ast_translator_pvt *pvt)
978 {
979   free (pvt);
980   localusecnt--;
981   ast_update_use_count ();
982 }
983
984 /*
985  * The complete translator for G726ToLin.
986  */
987
988 static struct ast_translator g726tolin = {
989   "g726tolin",
990   AST_FORMAT_G726,
991   AST_FORMAT_SLINEAR,
992   g726tolin_new,
993   g726tolin_framein,
994   g726tolin_frameout,
995   g726_destroy,
996   /* NULL */
997   g726tolin_sample
998 };
999
1000 /*
1001  * The complete translator for LinToG726.
1002  */
1003
1004 static struct ast_translator lintog726 = {
1005   "lintog726",
1006   AST_FORMAT_SLINEAR,
1007   AST_FORMAT_G726,
1008   lintog726_new,
1009   lintog726_framein,
1010   lintog726_frameout,
1011   g726_destroy,
1012   /* NULL */
1013   lintog726_sample
1014 };
1015
1016 static void 
1017 parse_config(void)
1018 {
1019   struct ast_config *cfg;
1020   struct ast_variable *var;
1021   if ((cfg = ast_config_load("codecs.conf"))) {
1022     if ((var = ast_variable_browse(cfg, "plc"))) {
1023       while (var) {
1024        if (!strcasecmp(var->name, "genericplc")) {
1025          useplc = ast_true(var->value) ? 1 : 0;
1026          if (option_verbose > 2)
1027            ast_verbose(VERBOSE_PREFIX_3 "codec_g726: %susing generic PLC\n", useplc ? "" : "not ");
1028        }
1029        var = var->next;
1030       }
1031     }
1032     ast_config_destroy(cfg);
1033   }
1034 }
1035
1036 int
1037 reload(void)
1038 {
1039   parse_config();
1040   return 0;
1041 }
1042
1043 int
1044 unload_module (void)
1045 {
1046   int res;
1047   ast_mutex_lock (&localuser_lock);
1048   res = ast_unregister_translator (&lintog726);
1049   if (!res)
1050     res = ast_unregister_translator (&g726tolin);
1051   if (localusecnt)
1052     res = -1;
1053   ast_mutex_unlock (&localuser_lock);
1054   return res;
1055 }
1056
1057 int
1058 load_module (void)
1059 {
1060   int res;
1061   parse_config();
1062   res = ast_register_translator (&g726tolin);
1063   if (!res)
1064     res = ast_register_translator (&lintog726);
1065   else
1066     ast_unregister_translator (&g726tolin);
1067   return res;
1068 }
1069
1070 /*
1071  * Return a description of this module.
1072  */
1073
1074 char *
1075 description (void)
1076 {
1077   return tdesc;
1078 }
1079
1080 int
1081 usecount (void)
1082 {
1083   int res;
1084   STANDARD_USECOUNT (res);
1085   return res;
1086 }
1087
1088 char *
1089 key ()
1090 {
1091   return ASTERISK_GPL_KEY;
1092 }