g722_encode.c

Go to the documentation of this file.
00001 /*
00002  * SpanDSP - a series of DSP components for telephony
00003  *
00004  * g722_encode.c - The ITU G.722 codec, encode part.
00005  *
00006  * Written by Steve Underwood <steveu@coppice.org>
00007  *
00008  * Copyright (C) 2005 Steve Underwood
00009  *
00010  * All rights reserved.
00011  *
00012  *  Despite my general liking of the GPL, I place my own contributions 
00013  *  to this code in the public domain for the benefit of all mankind -
00014  *  even the slimy ones who might try to proprietize my work and use it
00015  *  to my detriment.
00016  *
00017  * Based on a single channel 64kbps only G.722 codec which is:
00018  *
00019  *****    Copyright (c) CMU    1993      *****
00020  * Computer Science, Speech Group
00021  * Chengxiang Lu and Alex Hauptmann
00022  *
00023  * $Id: g722_encode.c 194722 2009-05-15 17:59:08Z russell $
00024  */
00025 
00026 /*! \file */
00027 
00028 #ifdef HAVE_CONFIG_H
00029 #include <config.h>
00030 #endif
00031 
00032 #include <stdio.h>
00033 #include <inttypes.h>
00034 #include <memory.h>
00035 #include <stdlib.h>
00036 #if 0
00037 #include <tgmath.h>
00038 #endif
00039 
00040 #include "g722.h"
00041 
00042 #if !defined(FALSE)
00043 #define FALSE 0
00044 #endif
00045 #if !defined(TRUE)
00046 #define TRUE (!FALSE)
00047 #endif
00048 
00049 static __inline__ int16_t saturate(int32_t amp)
00050 {
00051     int16_t amp16;
00052 
00053     /* Hopefully this is optimised for the common case - not clipping */
00054     amp16 = (int16_t) amp;
00055     if (amp == amp16)
00056         return amp16;
00057     if (amp > INT16_MAX)
00058         return  INT16_MAX;
00059     return  INT16_MIN;
00060 }
00061 /*- End of function --------------------------------------------------------*/
00062 
00063 static void block4(g722_encode_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 = (wd2 >> 7) + ((s->band[band].sg[0] == s->band[band].sg[2])  ?  128  :  -128);
00086     wd3 += (s->band[band].a[2]*32512) >> 15;
00087     if (wd3 > 12288)
00088         wd3 = 12288;
00089     else if (wd3 < -12288)
00090         wd3 = -12288;
00091     s->band[band].ap[2] = wd3;
00092 
00093     /* Block 4, UPPOL1 */
00094     s->band[band].sg[0] = s->band[band].p[0] >> 15;
00095     s->band[band].sg[1] = s->band[band].p[1] >> 15;
00096     wd1 = (s->band[band].sg[0] == s->band[band].sg[1])  ?  192  :  -192;
00097     wd2 = (s->band[band].a[1]*32640) >> 15;
00098 
00099     s->band[band].ap[1] = saturate(wd1 + wd2);
00100     wd3 = saturate(15360 - s->band[band].ap[2]);
00101     if (s->band[band].ap[1] > wd3)
00102         s->band[band].ap[1] = wd3;
00103     else if (s->band[band].ap[1] < -wd3)
00104         s->band[band].ap[1] = -wd3;
00105 
00106     /* Block 4, UPZERO */
00107     wd1 = (d == 0)  ?  0  :  128;
00108     s->band[band].sg[0] = d >> 15;
00109     for (i = 1;  i < 7;  i++)
00110     {
00111         s->band[band].sg[i] = s->band[band].d[i] >> 15;
00112         wd2 = (s->band[band].sg[i] == s->band[band].sg[0])  ?  wd1  :  -wd1;
00113         wd3 = (s->band[band].b[i]*32640) >> 15;
00114         s->band[band].bp[i] = saturate(wd2 + wd3);
00115     }
00116 
00117     /* Block 4, DELAYA */
00118     for (i = 6;  i > 0;  i--)
00119     {
00120         s->band[band].d[i] = s->band[band].d[i - 1];
00121         s->band[band].b[i] = s->band[band].bp[i];
00122     }
00123     
00124     for (i = 2;  i > 0;  i--)
00125     {
00126         s->band[band].r[i] = s->band[band].r[i - 1];
00127         s->band[band].p[i] = s->band[band].p[i - 1];
00128         s->band[band].a[i] = s->band[band].ap[i];
00129     }
00130 
00131     /* Block 4, FILTEP */
00132     wd1 = saturate(s->band[band].r[1] + s->band[band].r[1]);
00133     wd1 = (s->band[band].a[1]*wd1) >> 15;
00134     wd2 = saturate(s->band[band].r[2] + s->band[band].r[2]);
00135     wd2 = (s->band[band].a[2]*wd2) >> 15;
00136     s->band[band].sp = saturate(wd1 + wd2);
00137 
00138     /* Block 4, FILTEZ */
00139     s->band[band].sz = 0;
00140     for (i = 6;  i > 0;  i--)
00141     {
00142         wd1 = saturate(s->band[band].d[i] + s->band[band].d[i]);
00143         s->band[band].sz += (s->band[band].b[i]*wd1) >> 15;
00144     }
00145     s->band[band].sz = saturate(s->band[band].sz);
00146 
00147     /* Block 4, PREDIC */
00148     s->band[band].s = saturate(s->band[band].sp + s->band[band].sz);
00149 }
00150 /*- End of function --------------------------------------------------------*/
00151 
00152 g722_encode_state_t *g722_encode_init(g722_encode_state_t *s, int rate, int options)
00153 {
00154     if (s == NULL)
00155     {
00156         if ((s = (g722_encode_state_t *) malloc(sizeof(*s))) == NULL)
00157             return NULL;
00158     }
00159     memset(s, 0, sizeof(*s));
00160     if (rate == 48000)
00161         s->bits_per_sample = 6;
00162     else if (rate == 56000)
00163         s->bits_per_sample = 7;
00164     else
00165         s->bits_per_sample = 8;
00166     if ((options & G722_SAMPLE_RATE_8000))
00167         s->eight_k = TRUE;
00168     if ((options & G722_PACKED)  &&  s->bits_per_sample != 8)
00169         s->packed = TRUE;
00170     else
00171         s->packed = FALSE;
00172     s->band[0].det = 32;
00173     s->band[1].det = 8;
00174     return s;
00175 }
00176 /*- End of function --------------------------------------------------------*/
00177 
00178 int g722_encode_release(g722_encode_state_t *s)
00179 {
00180     free(s);
00181     return 0;
00182 }
00183 /*- End of function --------------------------------------------------------*/
00184 
00185 int g722_encode(g722_encode_state_t *s, uint8_t g722_data[], const int16_t amp[], int len)
00186 {
00187     static const int q6[32] =
00188     {
00189            0,   35,   72,  110,  150,  190,  233,  276,
00190          323,  370,  422,  473,  530,  587,  650,  714,
00191          786,  858,  940, 1023, 1121, 1219, 1339, 1458,
00192         1612, 1765, 1980, 2195, 2557, 2919,    0,    0
00193     };
00194     static const int iln[32] =
00195     {
00196          0, 63, 62, 31, 30, 29, 28, 27,
00197         26, 25, 24, 23, 22, 21, 20, 19,
00198         18, 17, 16, 15, 14, 13, 12, 11,
00199         10,  9,  8,  7,  6,  5,  4,  0
00200     };
00201     static const int ilp[32] =
00202     {
00203          0, 61, 60, 59, 58, 57, 56, 55,
00204         54, 53, 52, 51, 50, 49, 48, 47,
00205         46, 45, 44, 43, 42, 41, 40, 39,
00206         38, 37, 36, 35, 34, 33, 32,  0
00207     };
00208     static const int wl[8] =
00209     {
00210         -60, -30, 58, 172, 334, 538, 1198, 3042
00211     };
00212     static const int rl42[16] =
00213     {
00214         0, 7, 6, 5, 4, 3, 2, 1, 7, 6, 5, 4, 3, 2, 1, 0
00215     };
00216     static const int ilb[32] =
00217     {
00218         2048, 2093, 2139, 2186, 2233, 2282, 2332,
00219         2383, 2435, 2489, 2543, 2599, 2656, 2714,
00220         2774, 2834, 2896, 2960, 3025, 3091, 3158,
00221         3228, 3298, 3371, 3444, 3520, 3597, 3676,
00222         3756, 3838, 3922, 4008
00223     };
00224     static const int qm4[16] =
00225     {
00226              0, -20456, -12896, -8968,
00227          -6288,  -4240,  -2584, -1200,
00228          20456,  12896,   8968,  6288,
00229           4240,   2584,   1200,     0
00230     };
00231     static const int qm2[4] =
00232     {
00233         -7408,  -1616,   7408,   1616
00234     };
00235     static const int qmf_coeffs[12] =
00236     {
00237            3,  -11,   12,   32, -210,  951, 3876, -805,  362, -156,   53,  -11,
00238     };
00239     static const int ihn[3] = {0, 1, 0};
00240     static const int ihp[3] = {0, 3, 2};
00241     static const int wh[3] = {0, -214, 798};
00242     static const int rh2[4] = {2, 1, 2, 1};
00243 
00244     int dlow;
00245     int dhigh;
00246     int el;
00247     int wd;
00248     int wd1;
00249     int ril;
00250     int wd2;
00251     int il4;
00252     int ih2;
00253     int wd3;
00254     int eh;
00255     int mih;
00256     int i;
00257     int j;
00258     /* Low and high band PCM from the QMF */
00259     int xlow;
00260     int xhigh;
00261     int g722_bytes;
00262     /* Even and odd tap accumulators */
00263     int sumeven;
00264     int sumodd;
00265     int ihigh;
00266     int ilow;
00267     int code;
00268 
00269     g722_bytes = 0;
00270     xhigh = 0;
00271     for (j = 0;  j < len;  )
00272     {
00273         if (s->itu_test_mode)
00274         {
00275             xlow =
00276             xhigh = amp[j++] >> 1;
00277         }
00278         else
00279         {
00280             if (s->eight_k)
00281             {
00282                 xlow = amp[j++] >> 1;
00283             }
00284             else
00285             {
00286                 /* Apply the transmit QMF */
00287                 /* Shuffle the buffer down */
00288                 for (i = 0;  i < 22;  i++)
00289                     s->x[i] = s->x[i + 2];
00290                 s->x[22] = amp[j++];
00291                 s->x[23] = amp[j++];
00292     
00293                 /* Discard every other QMF output */
00294                 sumeven = 0;
00295                 sumodd = 0;
00296                 for (i = 0;  i < 12;  i++)
00297                 {
00298                     sumodd += s->x[2*i]*qmf_coeffs[i];
00299                     sumeven += s->x[2*i + 1]*qmf_coeffs[11 - i];
00300                 }
00301                 xlow = (sumeven + sumodd) >> 14;
00302                 xhigh = (sumeven - sumodd) >> 14;
00303             }
00304         }
00305         /* Block 1L, SUBTRA */
00306         el = saturate(xlow - s->band[0].s);
00307 
00308         /* Block 1L, QUANTL */
00309         wd = (el >= 0)  ?  el  :  -(el + 1);
00310 
00311         for (i = 1;  i < 30;  i++)
00312         {
00313             wd1 = (q6[i]*s->band[0].det) >> 12;
00314             if (wd < wd1)
00315                 break;
00316         }
00317         ilow = (el < 0)  ?  iln[i]  :  ilp[i];
00318 
00319         /* Block 2L, INVQAL */
00320         ril = ilow >> 2;
00321         wd2 = qm4[ril];
00322         dlow = (s->band[0].det*wd2) >> 15;
00323 
00324         /* Block 3L, LOGSCL */
00325         il4 = rl42[ril];
00326         wd = (s->band[0].nb*127) >> 7;
00327         s->band[0].nb = wd + wl[il4];
00328         if (s->band[0].nb < 0)
00329             s->band[0].nb = 0;
00330         else if (s->band[0].nb > 18432)
00331             s->band[0].nb = 18432;
00332 
00333         /* Block 3L, SCALEL */
00334         wd1 = (s->band[0].nb >> 6) & 31;
00335         wd2 = 8 - (s->band[0].nb >> 11);
00336         wd3 = (wd2 < 0)  ?  (ilb[wd1] << -wd2)  :  (ilb[wd1] >> wd2);
00337         s->band[0].det = wd3 << 2;
00338 
00339         block4(s, 0, dlow);
00340         
00341         if (s->eight_k)
00342         {
00343             /* Just leave the high bits as zero */
00344             code = (0xC0 | ilow) >> (8 - s->bits_per_sample);
00345         }
00346         else
00347         {
00348             /* Block 1H, SUBTRA */
00349             eh = saturate(xhigh - s->band[1].s);
00350 
00351             /* Block 1H, QUANTH */
00352             wd = (eh >= 0)  ?  eh  :  -(eh + 1);
00353             wd1 = (564*s->band[1].det) >> 12;
00354             mih = (wd >= wd1)  ?  2  :  1;
00355             ihigh = (eh < 0)  ?  ihn[mih]  :  ihp[mih];
00356 
00357             /* Block 2H, INVQAH */
00358             wd2 = qm2[ihigh];
00359             dhigh = (s->band[1].det*wd2) >> 15;
00360 
00361             /* Block 3H, LOGSCH */
00362             ih2 = rh2[ihigh];
00363             wd = (s->band[1].nb*127) >> 7;
00364             s->band[1].nb = wd + wh[ih2];
00365             if (s->band[1].nb < 0)
00366                 s->band[1].nb = 0;
00367             else if (s->band[1].nb > 22528)
00368                 s->band[1].nb = 22528;
00369 
00370             /* Block 3H, SCALEH */
00371             wd1 = (s->band[1].nb >> 6) & 31;
00372             wd2 = 10 - (s->band[1].nb >> 11);
00373             wd3 = (wd2 < 0)  ?  (ilb[wd1] << -wd2)  :  (ilb[wd1] >> wd2);
00374             s->band[1].det = wd3 << 2;
00375 
00376             block4(s, 1, dhigh);
00377             code = ((ihigh << 6) | ilow) >> (8 - s->bits_per_sample);
00378         }
00379 
00380         if (s->packed)
00381         {
00382             /* Pack the code bits */
00383             s->out_buffer |= (code << s->out_bits);
00384             s->out_bits += s->bits_per_sample;
00385             if (s->out_bits >= 8)
00386             {
00387                 g722_data[g722_bytes++] = (uint8_t) (s->out_buffer & 0xFF);
00388                 s->out_bits -= 8;
00389                 s->out_buffer >>= 8;
00390             }
00391         }
00392         else
00393         {
00394             g722_data[g722_bytes++] = (uint8_t) code;
00395         }
00396     }
00397     return g722_bytes;
00398 }
00399 /*- End of function --------------------------------------------------------*/
00400 /*- End of file ------------------------------------------------------------*/

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