codecs/g722/g722_decode.c

   1 /*
   2  * SpanDSP - a series of DSP components for telephony
   3  *
   4  * g722_decode.c - The ITU G.722 codec, decode part.
   5  *
   6  * Written by Steve Underwood <steveu@coppice.org>
   7  *
   8  * Copyright (C) 2005 Steve Underwood
   9  *
  10  *  Despite my general liking of the GPL, I place my own contributions
  11  *  to this code in the public domain for the benefit of all mankind -
  12  *  even the slimy ones who might try to proprietize my work and use it
  13  *  to my detriment.
  14  *
  15  * Based in part on a single channel G.722 codec which is:
  16  *
  17  * Copyright (c) CMU 1993
  18  * Computer Science, Speech Group
  19  * Chengxiang Lu and Alex Hauptmann
  20  *
  21  * $Id$
  22  */
  23
  24 /*! \file */
  25
  26 #ifdef HAVE_CONFIG_H
  27 #include <config.h>
  28 #endif
  29
  30 #include <stdio.h>
  31 #include <inttypes.h>
  32 #include <memory.h>
  33 #include <stdlib.h>
  34 #if 0
  35 #include <tgmath.h>
  36 #endif
  37
  38 #include "g722.h"
  39
  40 #if !defined(FALSE)
  41 #define FALSE 0
  42 #endif
  43 #if !defined(TRUE)
  44 #define TRUE (!FALSE)
  45 #endif
  46
  47 static __inline__ int16_t saturate(int32_t amp)
  48 {
  49     int16_t amp16;
  50
  51     /* Hopefully this is optimised for the common case - not clipping */
  52     amp16 = (int16_t) amp;
  53     if (amp == amp16)
  54         return amp16;
  55     if (amp > INT16_MAX)
  56         return  INT16_MAX;
  57     return  INT16_MIN;
  58 }
  59 /*- End of function --------------------------------------------------------*/
  60
  61 static void block4(g722_decode_state_t *s, int band, int d);
  62
  63 static void block4(g722_decode_state_t *s, int band, int d)
  64 {
  65     int wd1;
  66     int wd2;
  67     int wd3;
  68     int i;
  69
  70     /* Block 4, RECONS */
  71     s->band[band].d[0] = d;
  72     s->band[band].r[0] = saturate(s->band[band].s + d);
  73
  74     /* Block 4, PARREC */
  75     s->band[band].p[0] = saturate(s->band[band].sz + d);
  76
  77     /* Block 4, UPPOL2 */
  78     for (i = 0;  i < 3;  i++)
  79         s->band[band].sg[i] = s->band[band].p[i] >> 15;
  80     wd1 = saturate(s->band[band].a[1] << 2);
  81
  82     wd2 = (s->band[band].sg[0] == s->band[band].sg[1])  ?  -wd1  :  wd1;
  83     if (wd2 > 32767)
  84         wd2 = 32767;
  85     wd3 = (s->band[band].sg[0] == s->band[band].sg[2])  ?  128  :  -128;
  86     wd3 += (wd2 >> 7);
  87     wd3 += (s->band[band].a[2]*32512) >> 15;
  88     if (wd3 > 12288)
  89         wd3 = 12288;
  90     else if (wd3 < -12288)
  91         wd3 = -12288;
  92     s->band[band].ap[2] = wd3;
  93
  94     /* Block 4, UPPOL1 */
  95     s->band[band].sg[0] = s->band[band].p[0] >> 15;
  96     s->band[band].sg[1] = s->band[band].p[1] >> 15;
  97     wd1 = (s->band[band].sg[0] == s->band[band].sg[1])  ?  192  :  -192;
  98     wd2 = (s->band[band].a[1]*32640) >> 15;
  99
 100     s->band[band].ap[1] = saturate(wd1 + wd2);
 101     wd3 = saturate(15360 - s->band[band].ap[2]);
 102     if (s->band[band].ap[1] > wd3)
 103         s->band[band].ap[1] = wd3;
 104     else if (s->band[band].ap[1] < -wd3)
 105         s->band[band].ap[1] = -wd3;
 106
 107     /* Block 4, UPZERO */
 108     wd1 = (d == 0)  ?  0  :  128;
 109     s->band[band].sg[0] = d >> 15;
 110     for (i = 1;  i < 7;  i++)
 111     {
 112         s->band[band].sg[i] = s->band[band].d[i] >> 15;
 113         wd2 = (s->band[band].sg[i] == s->band[band].sg[0])  ?  wd1  :  -wd1;
 114         wd3 = (s->band[band].b[i]*32640) >> 15;
 115         s->band[band].bp[i] = saturate(wd2 + wd3);
 116     }
 117
 118     /* Block 4, DELAYA */
 119     for (i = 6;  i > 0;  i--)
 120     {
 121         s->band[band].d[i] = s->band[band].d[i - 1];
 122         s->band[band].b[i] = s->band[band].bp[i];
 123     }
 124
 125     for (i = 2;  i > 0;  i--)
 126     {
 127         s->band[band].r[i] = s->band[band].r[i - 1];
 128         s->band[band].p[i] = s->band[band].p[i - 1];
 129         s->band[band].a[i] = s->band[band].ap[i];
 130     }
 131
 132     /* Block 4, FILTEP */
 133     wd1 = saturate(s->band[band].r[1] + s->band[band].r[1]);
 134     wd1 = (s->band[band].a[1]*wd1) >> 15;
 135     wd2 = saturate(s->band[band].r[2] + s->band[band].r[2]);
 136     wd2 = (s->band[band].a[2]*wd2) >> 15;
 137     s->band[band].sp = saturate(wd1 + wd2);
 138
 139     /* Block 4, FILTEZ */
 140     s->band[band].sz = 0;
 141     for (i = 6;  i > 0;  i--)
 142     {
 143         wd1 = saturate(s->band[band].d[i] + s->band[band].d[i]);
 144         s->band[band].sz += (s->band[band].b[i]*wd1) >> 15;
 145     }
 146     s->band[band].sz = saturate(s->band[band].sz);
 147
 148     /* Block 4, PREDIC */
 149     s->band[band].s = saturate(s->band[band].sp + s->band[band].sz);
 150 }
 151 /*- End of function --------------------------------------------------------*/
 152
 153 g722_decode_state_t *g722_decode_init(g722_decode_state_t *s, int rate, int options)
 154 {
 155     if (s == NULL)
 156     {
 157         if ((s = (g722_decode_state_t *) malloc(sizeof(*s))) == NULL)
 158             return NULL;
 159     }
 160     memset(s, 0, sizeof(*s));
 161     if (rate == 48000)
 162         s->bits_per_sample = 6;
 163     else if (rate == 56000)
 164         s->bits_per_sample = 7;
 165     else
 166         s->bits_per_sample = 8;
 167     if ((options & G722_SAMPLE_RATE_8000))
 168         s->eight_k = TRUE;
 169     if ((options & G722_PACKED)  &&  s->bits_per_sample != 8)
 170         s->packed = TRUE;
 171     else
 172         s->packed = FALSE;
 173     s->band[0].det = 32;
 174     s->band[1].det = 8;
 175     return s;
 176 }
 177 /*- End of function --------------------------------------------------------*/
 178
 179 int g722_decode_release(g722_decode_state_t *s)
 180 {
 181     free(s);
 182     return 0;
 183 }
 184 /*- End of function --------------------------------------------------------*/
 185
 186 int g722_decode(g722_decode_state_t *s, int16_t amp[], const uint8_t g722_data[], int len)
 187 {
 188     static const int wl[8] = {-60, -30, 58, 172, 334, 538, 1198, 3042 };
 189     static const int rl42[16] = {0, 7, 6, 5, 4, 3, 2, 1, 7, 6, 5, 4, 3,  2, 1, 0 };
 190     static const int ilb[32] =
 191     {
 192         2048, 2093, 2139, 2186, 2233, 2282, 2332,
 193         2383, 2435, 2489, 2543, 2599, 2656, 2714,
 194         2774, 2834, 2896, 2960, 3025, 3091, 3158,
 195         3228, 3298, 3371, 3444, 3520, 3597, 3676,
 196         3756, 3838, 3922, 4008
 197     };
 198     static const int wh[3] = {0, -214, 798};
 199     static const int rh2[4] = {2, 1, 2, 1};
 200     static const int qm2[4] = {-7408, -1616,  7408,   1616};
 201     static const int qm4[16] =
 202     {
 203               0, -20456, -12896,  -8968,
 204           -6288,  -4240,  -2584,  -1200,
 205           20456,  12896,   8968,   6288,
 206            4240,   2584,   1200,      0
 207     };
 208     static const int qm5[32] =
 209     {
 210            -280,   -280, -23352, -17560,
 211          -14120, -11664,  -9752,  -8184,
 212           -6864,  -5712,  -4696,  -3784,
 213           -2960,  -2208,  -1520,   -880,
 214           23352,  17560,  14120,  11664,
 215            9752,   8184,   6864,   5712,
 216            4696,   3784,   2960,   2208,
 217            1520,    880,    280,   -280
 218     };
 219     static const int qm6[64] =
 220     {
 221            -136,   -136,   -136,   -136,
 222          -24808, -21904, -19008, -16704,
 223          -14984, -13512, -12280, -11192,
 224          -10232,  -9360,  -8576,  -7856,
 225           -7192,  -6576,  -6000,  -5456,
 226           -4944,  -4464,  -4008,  -3576,
 227           -3168,  -2776,  -2400,  -2032,
 228           -1688,  -1360,  -1040,   -728,
 229           24808,  21904,  19008,  16704,
 230           14984,  13512,  12280,  11192,
 231           10232,   9360,   8576,   7856,
 232            7192,   6576,   6000,   5456,
 233            4944,   4464,   4008,   3576,
 234            3168,   2776,   2400,   2032,
 235            1688,   1360,   1040,    728,
 236             432,    136,   -432,   -136
 237     };
 238     static const int qmf_coeffs[12] =
 239     {
 240            3,  -11,   12,   32, -210,  951, 3876, -805,  362, -156,   53,  -11,
 241     };
 242
 243     int dlowt;
 244     int rlow;
 245     int ihigh;
 246     int dhigh;
 247     int rhigh;
 248     int xout1;
 249     int xout2;
 250     int wd1;
 251     int wd2;
 252     int wd3;
 253     int code;
 254     int outlen;
 255     int i;
 256     int j;
 257
 258     outlen = 0;
 259     rhigh = 0;
 260     for (j = 0;  j < len;  )
 261     {
 262         if (s->packed)
 263         {
 264             /* Unpack the code bits */
 265             if (s->in_bits < s->bits_per_sample)
 266             {
 267                 s->in_buffer |= (g722_data[j++] << s->in_bits);
 268                 s->in_bits += 8;
 269             }
 270             code = s->in_buffer & ((1 << s->bits_per_sample) - 1);
 271             s->in_buffer >>= s->bits_per_sample;
 272             s->in_bits -= s->bits_per_sample;
 273         }
 274         else
 275         {
 276             code = g722_data[j++];
 277         }
 278
 279         switch (s->bits_per_sample)
 280         {
 281         default:
 282         case 8:
 283             wd1 = code & 0x3F;
 284             ihigh = (code >> 6) & 0x03;
 285             wd2 = qm6[wd1];
 286             wd1 >>= 2;
 287             break;
 288         case 7:
 289             wd1 = code & 0x1F;
 290             ihigh = (code >> 5) & 0x03;
 291             wd2 = qm5[wd1];
 292             wd1 >>= 1;
 293             break;
 294         case 6:
 295             wd1 = code & 0x0F;
 296             ihigh = (code >> 4) & 0x03;
 297             wd2 = qm4[wd1];
 298             break;
 299         }
 300         /* Block 5L, LOW BAND INVQBL */
 301         wd2 = (s->band[0].det*wd2) >> 15;
 302         /* Block 5L, RECONS */
 303         rlow = s->band[0].s + wd2;
 304         /* Block 6L, LIMIT */
 305         if (rlow > 16383)
 306             rlow = 16383;
 307         else if (rlow < -16384)
 308             rlow = -16384;
 309
 310         /* Block 2L, INVQAL */
 311         wd2 = qm4[wd1];
 312         dlowt = (s->band[0].det*wd2) >> 15;
 313
 314         /* Block 3L, LOGSCL */
 315         wd2 = rl42[wd1];
 316         wd1 = (s->band[0].nb*127) >> 7;
 317         wd1 += wl[wd2];
 318         if (wd1 < 0)
 319             wd1 = 0;
 320         else if (wd1 > 18432)
 321             wd1 = 18432;
 322         s->band[0].nb = wd1;
 323
 324         /* Block 3L, SCALEL */
 325         wd1 = (s->band[0].nb >> 6) & 31;
 326         wd2 = 8 - (s->band[0].nb >> 11);
 327         wd3 = (wd2 < 0)  ?  (ilb[wd1] << -wd2)  :  (ilb[wd1] >> wd2);
 328         s->band[0].det = wd3 << 2;
 329
 330         block4(s, 0, dlowt);
 331
 332         if (!s->eight_k)
 333         {
 334             /* Block 2H, INVQAH */
 335             wd2 = qm2[ihigh];
 336             dhigh = (s->band[1].det*wd2) >> 15;
 337             /* Block 5H, RECONS */
 338             rhigh = dhigh + s->band[1].s;
 339             /* Block 6H, LIMIT */
 340             if (rhigh > 16383)
 341                 rhigh = 16383;
 342             else if (rhigh < -16384)
 343                 rhigh = -16384;
 344
 345             /* Block 2H, INVQAH */
 346             wd2 = rh2[ihigh];
 347             wd1 = (s->band[1].nb*127) >> 7;
 348             wd1 += wh[wd2];
 349             if (wd1 < 0)
 350                 wd1 = 0;
 351             else if (wd1 > 22528)
 352                 wd1 = 22528;
 353             s->band[1].nb = wd1;
 354
 355             /* Block 3H, SCALEH */
 356             wd1 = (s->band[1].nb >> 6) & 31;
 357             wd2 = 10 - (s->band[1].nb >> 11);
 358             wd3 = (wd2 < 0)  ?  (ilb[wd1] << -wd2)  :  (ilb[wd1] >> wd2);
 359             s->band[1].det = wd3 << 2;
 360
 361             block4(s, 1, dhigh);
 362         }
 363
 364         if (s->itu_test_mode)
 365         {
 366             amp[outlen++] = (int16_t) (rlow << 1);
 367             amp[outlen++] = (int16_t) (rhigh << 1);
 368         }
 369         else
 370         {
 371             if (s->eight_k)
 372             {
 373                 amp[outlen++] = (int16_t) rlow;
 374             }
 375             else
 376             {
 377                 /* Apply the receive QMF */
 378                 for (i = 0;  i < 22;  i++)
 379                     s->x[i] = s->x[i + 2];
 380                 s->x[22] = rlow + rhigh;
 381                 s->x[23] = rlow - rhigh;
 382
 383                 xout1 = 0;
 384                 xout2 = 0;
 385                 for (i = 0;  i < 12;  i++)
 386                 {
 387                     xout2 += s->x[2*i]*qmf_coeffs[i];
 388                     xout1 += s->x[2*i + 1]*qmf_coeffs[11 - i];
 389                 }
 390                 amp[outlen++] = (int16_t) (xout1 >> 12);
 391                 amp[outlen++] = (int16_t) (xout2 >> 12);
 392             }
 393         }
 394     }
 395     return outlen;
 396 }
 397 /*- End of function --------------------------------------------------------*/
 398 /*- End of file ------------------------------------------------------------*/