g722_decode.c

Go to the documentation of this file.
00001 /*
00002  * SpanDSP - a series of DSP components for telephony
00003  *
00004  * g722_decode.c - The ITU G.722 codec, decode part.
00005  *
00006  * Written by Steve Underwood <steveu@coppice.org>
00007  *
00008  * Copyright (C) 2005 Steve Underwood
00009  *
00010  *  Despite my general liking of the GPL, I place my own contributions 
00011  *  to this code in the public domain for the benefit of all mankind -
00012  *  even the slimy ones who might try to proprietize my work and use it
00013  *  to my detriment.
00014  *
00015  * Based in part on a single channel G.722 codec which is:
00016  *
00017  * Copyright (c) CMU 1993
00018  * Computer Science, Speech Group
00019  * Chengxiang Lu and Alex Hauptmann
00020  *
00021  * $Id: g722_decode.c 194722 2009-05-15 17:59:08Z russell $
00022  */
00023 
00024 /*! \file */
00025 
00026 #ifdef HAVE_CONFIG_H
00027 #include <config.h>
00028 #endif
00029 
00030 #include <stdio.h>
00031 #include <inttypes.h>
00032 #include <memory.h>
00033 #include <stdlib.h>
00034 #if 0
00035 #include <tgmath.h>
00036 #endif
00037 
00038 #include "g722.h"
00039 
00040 #if !defined(FALSE)
00041 #define FALSE 0
00042 #endif
00043 #if !defined(TRUE)
00044 #define TRUE (!FALSE)
00045 #endif
00046 
00047 static __inline__ int16_t saturate(int32_t amp)
00048 {
00049     int16_t amp16;
00050 
00051     /* Hopefully this is optimised for the common case - not clipping */
00052     amp16 = (int16_t) amp;
00053     if (amp == amp16)
00054         return amp16;
00055     if (amp > INT16_MAX)
00056         return  INT16_MAX;
00057     return  INT16_MIN;
00058 }
00059 /*- End of function --------------------------------------------------------*/
00060 
00061 static void block4(g722_decode_state_t *s, int band, int d);
00062 
00063 static void block4(g722_decode_state_t *s, int band, int d)
00064 {
00065     int wd1;
00066     int wd2;
00067     int wd3;
00068     int i;
00069 
00070     /* Block 4, RECONS */
00071     s->band[band].d[0] = d;
00072     s->band[band].r[0] = saturate(s->band[band].s + d);
00073 
00074     /* Block 4, PARREC */
00075     s->band[band].p[0] = saturate(s->band[band].sz + d);
00076 
00077     /* Block 4, UPPOL2 */
00078     for (i = 0;  i < 3;  i++)
00079         s->band[band].sg[i] = s->band[band].p[i] >> 15;
00080     wd1 = saturate(s->band[band].a[1] << 2);
00081 
00082     wd2 = (s->band[band].sg[0] == s->band[band].sg[1])  ?  -wd1  :  wd1;
00083     if (wd2 > 32767)
00084         wd2 = 32767;
00085     wd3 = (s->band[band].sg[0] == s->band[band].sg[2])  ?  128  :  -128;
00086     wd3 += (wd2 >> 7);
00087     wd3 += (s->band[band].a[2]*32512) >> 15;
00088     if (wd3 > 12288)
00089         wd3 = 12288;
00090     else if (wd3 < -12288)
00091         wd3 = -12288;
00092     s->band[band].ap[2] = wd3;
00093 
00094     /* Block 4, UPPOL1 */
00095     s->band[band].sg[0] = s->band[band].p[0] >> 15;
00096     s->band[band].sg[1] = s->band[band].p[1] >> 15;
00097     wd1 = (s->band[band].sg[0] == s->band[band].sg[1])  ?  192  :  -192;
00098     wd2 = (s->band[band].a[1]*32640) >> 15;
00099 
00100     s->band[band].ap[1] = saturate(wd1 + wd2);
00101     wd3 = saturate(15360 - s->band[band].ap[2]);
00102     if (s->band[band].ap[1] > wd3)
00103         s->band[band].ap[1] = wd3;
00104     else if (s->band[band].ap[1] < -wd3)
00105         s->band[band].ap[1] = -wd3;
00106 
00107     /* Block 4, UPZERO */
00108     wd1 = (d == 0)  ?  0  :  128;
00109     s->band[band].sg[0] = d >> 15;
00110     for (i = 1;  i < 7;  i++)
00111     {
00112         s->band[band].sg[i] = s->band[band].d[i] >> 15;
00113         wd2 = (s->band[band].sg[i] == s->band[band].sg[0])  ?  wd1  :  -wd1;
00114         wd3 = (s->band[band].b[i]*32640) >> 15;
00115         s->band[band].bp[i] = saturate(wd2 + wd3);
00116     }
00117 
00118     /* Block 4, DELAYA */
00119     for (i = 6;  i > 0;  i--)
00120     {
00121         s->band[band].d[i] = s->band[band].d[i - 1];
00122         s->band[band].b[i] = s->band[band].bp[i];
00123     }
00124     
00125     for (i = 2;  i > 0;  i--)
00126     {
00127         s->band[band].r[i] = s->band[band].r[i - 1];
00128         s->band[band].p[i] = s->band[band].p[i - 1];
00129         s->band[band].a[i] = s->band[band].ap[i];
00130     }
00131 
00132     /* Block 4, FILTEP */
00133     wd1 = saturate(s->band[band].r[1] + s->band[band].r[1]);
00134     wd1 = (s->band[band].a[1]*wd1) >> 15;
00135     wd2 = saturate(s->band[band].r[2] + s->band[band].r[2]);
00136     wd2 = (s->band[band].a[2]*wd2) >> 15;
00137     s->band[band].sp = saturate(wd1 + wd2);
00138 
00139     /* Block 4, FILTEZ */
00140     s->band[band].sz = 0;
00141     for (i = 6;  i > 0;  i--)
00142     {
00143         wd1 = saturate(s->band[band].d[i] + s->band[band].d[i]);
00144         s->band[band].sz += (s->band[band].b[i]*wd1) >> 15;
00145     }
00146     s->band[band].sz = saturate(s->band[band].sz);
00147 
00148     /* Block 4, PREDIC */
00149     s->band[band].s = saturate(s->band[band].sp + s->band[band].sz);
00150 }
00151 /*- End of function --------------------------------------------------------*/
00152 
00153 g722_decode_state_t *g722_decode_init(g722_decode_state_t *s, int rate, int options)
00154 {
00155     if (s == NULL)
00156     {
00157         if ((s = (g722_decode_state_t *) malloc(sizeof(*s))) == NULL)
00158             return NULL;
00159     }
00160     memset(s, 0, sizeof(*s));
00161     if (rate == 48000)
00162         s->bits_per_sample = 6;
00163     else if (rate == 56000)
00164         s->bits_per_sample = 7;
00165     else
00166         s->bits_per_sample = 8;
00167     if ((options & G722_SAMPLE_RATE_8000))
00168         s->eight_k = TRUE;
00169     if ((options & G722_PACKED)  &&  s->bits_per_sample != 8)
00170         s->packed = TRUE;
00171     else
00172         s->packed = FALSE;
00173     s->band[0].det = 32;
00174     s->band[1].det = 8;
00175     return s;
00176 }
00177 /*- End of function --------------------------------------------------------*/
00178 
00179 int g722_decode_release(g722_decode_state_t *s)
00180 {
00181     free(s);
00182     return 0;
00183 }
00184 /*- End of function --------------------------------------------------------*/
00185 
00186 int g722_decode(g722_decode_state_t *s, int16_t amp[], const uint8_t g722_data[], int len)
00187 {
00188     static const int wl[8] = {-60, -30, 58, 172, 334, 538, 1198, 3042 };
00189     static const int rl42[16] = {0, 7, 6, 5, 4, 3, 2, 1, 7, 6, 5, 4, 3,  2, 1, 0 };
00190     static const int ilb[32] =
00191     {
00192         2048, 2093, 2139, 2186, 2233, 2282, 2332,
00193         2383, 2435, 2489, 2543, 2599, 2656, 2714,
00194         2774, 2834, 2896, 2960, 3025, 3091, 3158,
00195         3228, 3298, 3371, 3444, 3520, 3597, 3676,
00196         3756, 3838, 3922, 4008
00197     };
00198     static const int wh[3] = {0, -214, 798};
00199     static const int rh2[4] = {2, 1, 2, 1};
00200     static const int qm2[4] = {-7408, -1616,  7408,   1616};
00201     static const int qm4[16] = 
00202     {
00203               0, -20456, -12896,  -8968, 
00204           -6288,  -4240,  -2584,  -1200,
00205           20456,  12896,   8968,   6288,
00206            4240,   2584,   1200,      0
00207     };
00208     static const int qm5[32] =
00209     {
00210            -280,   -280, -23352, -17560,
00211          -14120, -11664,  -9752,  -8184,
00212           -6864,  -5712,  -4696,  -3784,
00213           -2960,  -2208,  -1520,   -880,
00214           23352,  17560,  14120,  11664,
00215            9752,   8184,   6864,   5712,
00216            4696,   3784,   2960,   2208,
00217            1520,    880,    280,   -280
00218     };
00219     static const int qm6[64] =
00220     {
00221            -136,   -136,   -136,   -136,
00222          -24808, -21904, -19008, -16704,
00223          -14984, -13512, -12280, -11192,
00224          -10232,  -9360,  -8576,  -7856,
00225           -7192,  -6576,  -6000,  -5456,
00226           -4944,  -4464,  -4008,  -3576,
00227           -3168,  -2776,  -2400,  -2032,
00228           -1688,  -1360,  -1040,   -728,
00229           24808,  21904,  19008,  16704,
00230           14984,  13512,  12280,  11192,
00231           10232,   9360,   8576,   7856,
00232            7192,   6576,   6000,   5456,
00233            4944,   4464,   4008,   3576,
00234            3168,   2776,   2400,   2032,
00235            1688,   1360,   1040,    728,
00236             432,    136,   -432,   -136
00237     };
00238     static const int qmf_coeffs[12] =
00239     {
00240            3,  -11,   12,   32, -210,  951, 3876, -805,  362, -156,   53,  -11,
00241     };
00242 
00243     int dlowt;
00244     int rlow;
00245     int ihigh;
00246     int dhigh;
00247     int rhigh;
00248     int xout1;
00249     int xout2;
00250     int wd1;
00251     int wd2;
00252     int wd3;
00253     int code;
00254     int outlen;
00255     int i;
00256     int j;
00257 
00258     outlen = 0;
00259     rhigh = 0;
00260     for (j = 0;  j < len;  )
00261     {
00262         if (s->packed)
00263         {
00264             /* Unpack the code bits */
00265             if (s->in_bits < s->bits_per_sample)
00266             {
00267                 s->in_buffer |= (g722_data[j++] << s->in_bits);
00268                 s->in_bits += 8;
00269             }
00270             code = s->in_buffer & ((1 << s->bits_per_sample) - 1);
00271             s->in_buffer >>= s->bits_per_sample;
00272             s->in_bits -= s->bits_per_sample;
00273         }
00274         else
00275         {
00276             code = g722_data[j++];
00277         }
00278 
00279         switch (s->bits_per_sample)
00280         {
00281         default:
00282         case 8:
00283             wd1 = code & 0x3F;
00284             ihigh = (code >> 6) & 0x03;
00285             wd2 = qm6[wd1];
00286             wd1 >>= 2;
00287             break;
00288         case 7:
00289             wd1 = code & 0x1F;
00290             ihigh = (code >> 5) & 0x03;
00291             wd2 = qm5[wd1];
00292             wd1 >>= 1;
00293             break;
00294         case 6:
00295             wd1 = code & 0x0F;
00296             ihigh = (code >> 4) & 0x03;
00297             wd2 = qm4[wd1];
00298             break;
00299         }
00300         /* Block 5L, LOW BAND INVQBL */
00301         wd2 = (s->band[0].det*wd2) >> 15;
00302         /* Block 5L, RECONS */
00303         rlow = s->band[0].s + wd2;
00304         /* Block 6L, LIMIT */
00305         if (rlow > 16383)
00306             rlow = 16383;
00307         else if (rlow < -16384)
00308             rlow = -16384;
00309 
00310         /* Block 2L, INVQAL */
00311         wd2 = qm4[wd1];
00312         dlowt = (s->band[0].det*wd2) >> 15;
00313 
00314         /* Block 3L, LOGSCL */
00315         wd2 = rl42[wd1];
00316         wd1 = (s->band[0].nb*127) >> 7;
00317         wd1 += wl[wd2];
00318         if (wd1 < 0)
00319             wd1 = 0;
00320         else if (wd1 > 18432)
00321             wd1 = 18432;
00322         s->band[0].nb = wd1;
00323             
00324         /* Block 3L, SCALEL */
00325         wd1 = (s->band[0].nb >> 6) & 31;
00326         wd2 = 8 - (s->band[0].nb >> 11);
00327         wd3 = (wd2 < 0)  ?  (ilb[wd1] << -wd2)  :  (ilb[wd1] >> wd2);
00328         s->band[0].det = wd3 << 2;
00329 
00330         block4(s, 0, dlowt);
00331         
00332         if (!s->eight_k)
00333         {
00334             /* Block 2H, INVQAH */
00335             wd2 = qm2[ihigh];
00336             dhigh = (s->band[1].det*wd2) >> 15;
00337             /* Block 5H, RECONS */
00338             rhigh = dhigh + s->band[1].s;
00339             /* Block 6H, LIMIT */
00340             if (rhigh > 16383)
00341                 rhigh = 16383;
00342             else if (rhigh < -16384)
00343                 rhigh = -16384;
00344 
00345             /* Block 2H, INVQAH */
00346             wd2 = rh2[ihigh];
00347             wd1 = (s->band[1].nb*127) >> 7;
00348             wd1 += wh[wd2];
00349             if (wd1 < 0)
00350                 wd1 = 0;
00351             else if (wd1 > 22528)
00352                 wd1 = 22528;
00353             s->band[1].nb = wd1;
00354             
00355             /* Block 3H, SCALEH */
00356             wd1 = (s->band[1].nb >> 6) & 31;
00357             wd2 = 10 - (s->band[1].nb >> 11);
00358             wd3 = (wd2 < 0)  ?  (ilb[wd1] << -wd2)  :  (ilb[wd1] >> wd2);
00359             s->band[1].det = wd3 << 2;
00360 
00361             block4(s, 1, dhigh);
00362         }
00363 
00364         if (s->itu_test_mode)
00365         {
00366             amp[outlen++] = (int16_t) (rlow << 1);
00367             amp[outlen++] = (int16_t) (rhigh << 1);
00368         }
00369         else
00370         {
00371             if (s->eight_k)
00372             {
00373                 amp[outlen++] = (int16_t) (rlow << 1);
00374             }
00375             else
00376             {
00377                 /* Apply the receive QMF */
00378                 for (i = 0;  i < 22;  i++)
00379                     s->x[i] = s->x[i + 2];
00380                 s->x[22] = rlow + rhigh;
00381                 s->x[23] = rlow - rhigh;
00382 
00383                 xout1 = 0;
00384                 xout2 = 0;
00385                 for (i = 0;  i < 12;  i++)
00386                 {
00387                     xout2 += s->x[2*i]*qmf_coeffs[i];
00388                     xout1 += s->x[2*i + 1]*qmf_coeffs[11 - i];
00389                 }
00390                 amp[outlen++] = (int16_t) (xout1 >> 11);
00391                 amp[outlen++] = (int16_t) (xout2 >> 11);
00392             }
00393         }
00394     }
00395     return outlen;
00396 }
00397 /*- End of function --------------------------------------------------------*/
00398 /*- End of file ------------------------------------------------------------*/

Generated on Thu Apr 16 06:27:36 2015 for Asterisk - The Open Source Telephony Project by  doxygen 1.5.6