contrib/src/mmedia/g72x.cpp

   1 /*
   2  * This source code is a product of Sun Microsystems, Inc. and is provided
   3  * for unrestricted use.  Users may copy or modify this source code without
   4  * charge.
   5  *
   6  * SUN SOURCE CODE IS PROVIDED AS IS WITH NO WARRANTIES OF ANY KIND INCLUDING
   7  * THE WARRANTIES OF DESIGN, MERCHANTIBILITY AND FITNESS FOR A PARTICULAR
   8  * PURPOSE, OR ARISING FROM A COURSE OF DEALING, USAGE OR TRADE PRACTICE.
   9  *
  10  * Sun source code is provided with no support and without any obligation on
  11  * the part of Sun Microsystems, Inc. to assist in its use, correction,
  12  * modification or enhancement.
  13  *
  14  * SUN MICROSYSTEMS, INC. SHALL HAVE NO LIABILITY WITH RESPECT TO THE
  15  * INFRINGEMENT OF COPYRIGHTS, TRADE SECRETS OR ANY PATENTS BY THIS SOFTWARE
  16  * OR ANY PART THEREOF.
  17  *
  18  * In no event will Sun Microsystems, Inc. be liable for any lost revenue
  19  * or profits or other special, indirect and consequential damages, even if
  20  * Sun has been advised of the possibility of such damages.
  21  *
  22  * Sun Microsystems, Inc.
  23  * 2550 Garcia Avenue
  24  * Mountain View, California  94043
  25  */
  26
  27 /*
  28  * g72x.c
  29  *
  30  * Common routines for G.721 and G.723 conversions.
  31  */
  32
  33 #include "wx/wxprec.h"
  34 #include <stdlib.h>
  35 #include "wx/mmedia/internal/g72x.h"
  36
  37 static short power2[15] = {1, 2, 4, 8, 0x10, 0x20, 0x40, 0x80,
  38             0x100, 0x200, 0x400, 0x800, 0x1000, 0x2000, 0x4000};
  39
  40 /*
  41  * quan()
  42  *
  43  * quantizes the input val against the table of size short integers.
  44  * It returns i if table[i - 1] <= val < table[i].
  45  *
  46  * Using linear search for simple coding.
  47  */
  48 static int
  49 quan(
  50     int           val,
  51     short        *table,
  52     int           size)
  53 {
  54     int i;
  55
  56     for (i = 0; i < size; i++)
  57         if (val < *table++)
  58             break;
  59     return (i);
  60 }
  61
  62 static char quan2_tab[65536];
  63 static short base2_tab[65536];
  64 static int init_tabs_done = 0;
  65
  66 inline char quan2 (unsigned short val)
  67 {
  68     return quan2_tab[val];
  69 }
  70
  71 inline short base2 (unsigned short val)
  72 {
  73     return base2_tab[val];
  74 }
  75
  76 static void init_quan2_tab (void)
  77 {
  78     long i;
  79
  80     for (i = 0; i < 65536; i++) {
  81         quan2_tab[i] = quan (i, power2, 15);
  82     };
  83 }
  84
  85 static void init_base2_tab (void)
  86 {
  87     long i;
  88     short exp;
  89
  90     for (i = 0; i < 65536; i++) {
  91         exp = quan2 (short (i));
  92         base2_tab[i] = short ((exp << 6) + ((i << 6) >> exp));
  93     };
  94 }
  95
  96 static void init_tabs (void)
  97 {
  98     if (init_tabs_done) return;
  99
 100     init_quan2_tab();
 101     init_base2_tab();
 102
 103     init_tabs_done = 1;
 104 }
 105
 106 /*
 107  * fmult()
 108  *
 109  * returns the integer product of the 14-bit integer "an" and
 110  * "floating point" representation (4-bit exponent, 6-bit mantessa) "srn".
 111  */
 112 static int
 113 fmult(
 114     int        an,
 115     int        srn)
 116 {
 117     short        anmag, anexp, anmant;
 118     short        wanexp, wanmant;
 119     short        retval;
 120
 121     anmag = (an > 0) ? an : ((-an) & 0x1FFF);
 122     anexp = quan2(anmag) - 6;
 123     anmant = (anmag == 0) ? 32 :
 124         (anexp >= 0) ? anmag >> anexp : anmag << -anexp;
 125     wanexp = anexp + ((srn >> 6) & 0xF) - 13;
 126
 127     wanmant = (anmant * (srn & 077) + 0x30) >> 4;
 128     retval = (wanexp >= 0) ? ((wanmant << wanexp) & 0x7FFF) :
 129         (wanmant >> -wanexp);
 130
 131     return (((an ^ srn) < 0) ? -retval : retval);
 132 }
 133
 134 /*
 135  * g72x_init_state()
 136  *
 137  * This routine initializes and/or resets the g72x_state structure
 138  * pointed to by 'state_ptr'.
 139  * All the initial state values are specified in the CCITT G.721 document.
 140  */
 141 void
 142 g72x_init_state(
 143     struct g72x_state *state_ptr)
 144 {
 145     int cnta;
 146
 147     init_tabs ();
 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         state_ptr->a[cnta] = 0;
 156         state_ptr->pk[cnta] = 0;
 157         state_ptr->sr[cnta] = 32;
 158     }
 159     for (cnta = 0; cnta < 6; cnta++) {
 160         state_ptr->b[cnta] = 0;
 161         state_ptr->dq[cnta] = 32;
 162     }
 163     state_ptr->td = 0;
 164 }
 165
 166 /*
 167  * predictor_zero()
 168  *
 169  * computes the estimated signal from 6-zero predictor.
 170  *
 171  */
 172 int
 173 predictor_zero(
 174     struct g72x_state *state_ptr)
 175 {
 176     int        i;
 177     int        sezi;
 178
 179     sezi = fmult(state_ptr->b[0] >> 2, state_ptr->dq[0]);
 180     for (i = 1; i < 6; i++)            /* ACCUM */
 181         sezi += fmult(state_ptr->b[i] >> 2, state_ptr->dq[i]);
 182     return (sezi);
 183 }
 184 /*
 185  * predictor_pole()
 186  *
 187  * computes the estimated signal from 2-pole predictor.
 188  *
 189  */
 190 int
 191 predictor_pole(
 192     struct g72x_state *state_ptr)
 193 {
 194     return (fmult(state_ptr->a[1] >> 2, state_ptr->sr[1]) +
 195         fmult(state_ptr->a[0] >> 2, state_ptr->sr[0]));
 196 }
 197 /*
 198  * step_size()
 199  *
 200  * computes the quantization step size of the adaptive quantizer.
 201  *
 202  */
 203 int
 204 step_size(
 205     struct g72x_state *state_ptr)
 206 {
 207     int        y;
 208     int        dif;
 209     int        al;
 210
 211     if (state_ptr->ap >= 256)
 212         return (state_ptr->yu);
 213     else {
 214         y = state_ptr->yl >> 6;
 215         dif = state_ptr->yu - y;
 216         al = state_ptr->ap >> 2;
 217         if (dif > 0)
 218             y += (dif * al) >> 6;
 219         else if (dif < 0)
 220             y += (dif * al + 0x3F) >> 6;
 221         return (y);
 222     }
 223 }
 224
 225 /*
 226  * quantize()
 227  *
 228  * Given a raw sample, 'd', of the difference signal and a
 229  * quantization step size scale factor, 'y', this routine returns the
 230  * ADPCM codeword to which that sample gets quantized.  The step
 231  * size scale factor division operation is done in the log base 2 domain
 232  * as a subtraction.
 233  */
 234 int
 235 quantize(
 236     int        d,        /* Raw difference signal sample */
 237     int        y,        /* Step size multiplier */
 238     short     *table,    /* quantization table */
 239     int        size)     /* table size of short integers */
 240 {
 241     short        dqm;    /* Magnitude of 'd' */
 242     short        exp;    /* Integer part of base 2 log of 'd' */
 243     short        mant;   /* Fractional part of base 2 log */
 244     short        dl;     /* Log of magnitude of 'd' */
 245     short        dln;    /* Step size scale factor normalized log */
 246     int          i;
 247
 248     /*
 249      * LOG
 250      *
 251      * Compute base 2 log of 'd', and store in 'dl'.
 252      */
 253     dqm = abs(d);
 254     exp = quan2(dqm >> 1);
 255     mant = ((dqm << 7) >> exp) & 0x7F;    /* Fractional portion. */
 256     dl = (exp << 7) + mant;
 257
 258     /*
 259      * SUBTB
 260      *
 261      * "Divide" by step size multiplier.
 262      */
 263     dln = dl - (y >> 2);
 264
 265     /*
 266      * QUAN
 267      *
 268      * Obtain codword i for 'd'.
 269      */
 270     i = quan(dln, table, size);
 271     if (d < 0)            /* take 1's complement of i */
 272         return ((size << 1) + 1 - i);
 273     else if (i == 0)        /* take 1's complement of 0 */
 274         return ((size << 1) + 1); /* new in 1988 */
 275     else
 276         return (i);
 277 }
 278 /*
 279  * reconstruct()
 280  *
 281  * Returns reconstructed difference signal 'dq' obtained from
 282  * codeword 'i' and quantization step size scale factor 'y'.
 283  * Multiplication is performed in log base 2 domain as addition.
 284  */
 285 int
 286 reconstruct(
 287     int        sign,    /* 0 for non-negative value */
 288     int        dqln,    /* G.72x codeword */
 289     int        y)       /* Step size multiplier */
 290 {
 291     short        dql;   /* Log of 'dq' magnitude */
 292     short        dex;   /* Integer part of log */
 293     short        dqt;
 294     short        dq;    /* Reconstructed difference signal sample */
 295
 296     dql = dqln + (y >> 2);    /* ADDA */
 297
 298     if (dql < 0) {
 299         return ((sign) ? -0x8000 : 0);
 300     } else {        /* ANTILOG */
 301         dex = (dql >> 7) & 15;
 302         dqt = 128 + (dql & 127);
 303         dq = (dqt << 7) >> (14 - dex);
 304         return ((sign) ? (dq - 0x8000) : dq);
 305     }
 306 }
 307
 308
 309 /*
 310  * update()
 311  *
 312  * updates the state variables for each output code
 313  */
 314 void
 315 update(
 316     int        code_size,    /* distinguish 723_40 with others */
 317     int        y,            /* quantizer step size */
 318     int        wi,           /* scale factor multiplier */
 319     int        fi,           /* for long/short term energies */
 320     int        dq,           /* quantized prediction difference */
 321     int        sr,           /* reconstructed signal */
 322     int        dqsez,        /* difference from 2-pole predictor */
 323     struct g72x_state *state_ptr)    /* coder state pointer */
 324 {
 325     int          cnt;
 326     short        mag;        /* Adaptive predictor, FLOAT A */
 327     short        a2p;        /* LIMC */
 328     short        a1ul;       /* UPA1 */
 329     short        pks1;       /* UPA2 */
 330     short        fa1;
 331     char         tr;         /* tone/transition detector */
 332     short        ylint, thr2, dqthr;
 333     short        ylfrac, thr1;
 334     short        pk0;
 335
 336     pk0 = (dqsez < 0) ? 1 : 0;    /* needed in updating predictor poles */
 337
 338     mag = dq & 0x7FFF;        /* prediction difference magnitude */
 339     /* TRANS */
 340     ylint = short (state_ptr->yl >> 15);    /* exponent part of yl */
 341     ylfrac = (state_ptr->yl >> 10) & 0x1F;    /* fractional part of yl */
 342     thr1 = (32 + ylfrac) << ylint;        /* threshold */
 343     thr2 = (ylint > 9) ? 31 << 10 : thr1;    /* limit thr2 to 31 << 10 */
 344     dqthr = (thr2 + (thr2 >> 1)) >> 1;    /* dqthr = 0.75 * thr2 */
 345     if (state_ptr->td == 0)        /* signal supposed voice */
 346         tr = 0;
 347     else if (mag <= dqthr)        /* supposed data, but small mag */
 348         tr = 0;            /* treated as voice */
 349     else                /* signal is data (modem) */
 350         tr = 1;
 351
 352     /*
 353      * Quantizer scale factor adaptation.
 354      */
 355
 356     /* FUNCTW & FILTD & DELAY */
 357     /* update non-steady state step size multiplier */
 358     state_ptr->yu = y + ((wi - y) >> 5);
 359
 360     /* LIMB */
 361     if (state_ptr->yu < 544)    /* 544 <= yu <= 5120 */
 362         state_ptr->yu = 544;
 363     else if (state_ptr->yu > 5120)
 364         state_ptr->yu = 5120;
 365
 366     /* FILTE & DELAY */
 367     /* update steady state step size multiplier */
 368     state_ptr->yl += state_ptr->yu + ((-state_ptr->yl) >> 6);
 369
 370     /*
 371      * Adaptive predictor coefficients.
 372      */
 373     if (tr == 1) {            /* reset a's and b's for modem signal */
 374         state_ptr->a[0] = 0;
 375         state_ptr->a[1] = 0;
 376         state_ptr->b[0] = 0;
 377         state_ptr->b[1] = 0;
 378         state_ptr->b[2] = 0;
 379         state_ptr->b[3] = 0;
 380         state_ptr->b[4] = 0;
 381         state_ptr->b[5] = 0;
 382
 383         a2p = 0;        /* eliminate Compiler Warnings */
 384     } else {            /* update a's and b's */
 385         pks1 = pk0 ^ state_ptr->pk[0];        /* UPA2 */
 386
 387         /* update predictor pole a[1] */
 388         a2p = state_ptr->a[1] - (state_ptr->a[1] >> 7);
 389         if (dqsez != 0) {
 390             fa1 = (pks1) ? state_ptr->a[0] : -state_ptr->a[0];
 391             if (fa1 < -8191)    /* a2p = function of fa1 */
 392                 a2p -= 0x100;
 393             else if (fa1 > 8191)
 394                 a2p += 0xFF;
 395             else
 396                 a2p += fa1 >> 5;
 397
 398             if (pk0 ^ state_ptr->pk[1])
 399                 /* LIMC */
 400                 if (a2p <= -12160)
 401                     a2p = -12288;
 402                 else if (a2p >= 12416)
 403                     a2p = 12288;
 404                 else
 405                     a2p -= 0x80;
 406             else if (a2p <= -12416)
 407                 a2p = -12288;
 408             else if (a2p >= 12160)
 409                 a2p = 12288;
 410             else
 411                 a2p += 0x80;
 412         }
 413
 414         /* TRIGB & DELAY */
 415         state_ptr->a[1] = a2p;
 416
 417         /* UPA1 */
 418         /* update predictor pole a[0] */
 419         state_ptr->a[0] -= state_ptr->a[0] >> 8;
 420         if (dqsez != 0)
 421             if (pks1 == 0)
 422                 state_ptr->a[0] += 192;
 423             else
 424                 state_ptr->a[0] -= 192;
 425
 426         /* LIMD */
 427         a1ul = 15360 - a2p;
 428         if (state_ptr->a[0] < -a1ul)
 429             state_ptr->a[0] = -a1ul;
 430         else if (state_ptr->a[0] > a1ul)
 431             state_ptr->a[0] = a1ul;
 432
 433         /* UPB : update predictor zeros b[6] */
 434         for (cnt = 0; cnt < 6; cnt++) {
 435             if (code_size == 5)        /* for 40Kbps G.723 */
 436                 state_ptr->b[cnt] -= state_ptr->b[cnt] >> 9;
 437             else            /* for G.721 and 24Kbps G.723 */
 438                 state_ptr->b[cnt] -= state_ptr->b[cnt] >> 8;
 439             if (dq & 0x7FFF) {            /* XOR */
 440                 if ((dq ^ state_ptr->dq[cnt]) >= 0)
 441                     state_ptr->b[cnt] += 128;
 442                 else
 443                     state_ptr->b[cnt] -= 128;
 444             }
 445         }
 446     }
 447
 448     for (cnt = 5; cnt > 0; cnt--)
 449         state_ptr->dq[cnt] = state_ptr->dq[cnt-1];
 450     /* FLOAT A : convert dq[0] to 4-bit exp, 6-bit mantissa f.p. */
 451     if (mag == 0) {
 452         state_ptr->dq[0] = (dq >= 0) ? 0x20 : 0xFC20;
 453     } else {
 454         state_ptr->dq[0] = (dq >= 0) ?
 455              base2 (mag) : base2 (mag) - 0x400;
 456     }
 457
 458     state_ptr->sr[1] = state_ptr->sr[0];
 459     /* FLOAT B : convert sr to 4-bit exp., 6-bit mantissa f.p. */
 460     if (sr == 0) {
 461         state_ptr->sr[0] = 0x20;
 462     } else if (sr > 0) {
 463         state_ptr->sr[0] = base2(sr);
 464     } else if (sr > -32768) {
 465         mag = -sr;
 466         state_ptr->sr[0] = base2(mag) - 0x400;
 467     } else
 468         state_ptr->sr[0] = short (0xFC20);
 469
 470     /* DELAY A */
 471     state_ptr->pk[1] = state_ptr->pk[0];
 472     state_ptr->pk[0] = pk0;
 473
 474     /* TONE */
 475     if (tr == 1)        /* this sample has been treated as data */
 476         state_ptr->td = 0;    /* next one will be treated as voice */
 477     else if (a2p < -11776)    /* small sample-to-sample correlation */
 478         state_ptr->td = 1;    /* signal may be data */
 479     else                      /* signal is voice */
 480         state_ptr->td = 0;
 481
 482     /*
 483      * Adaptation speed control.
 484      */
 485     state_ptr->dms += (fi - state_ptr->dms) >> 5;        /* FILTA */
 486     state_ptr->dml += (((fi << 2) - state_ptr->dml) >> 7);    /* FILTB */
 487
 488     if (tr == 1)
 489         state_ptr->ap = 256;
 490     else if (y < 1536)                    /* SUBTC */
 491         state_ptr->ap += (0x200 - state_ptr->ap) >> 4;
 492     else if (state_ptr->td == 1)
 493         state_ptr->ap += (0x200 - state_ptr->ap) >> 4;
 494     else if (abs((state_ptr->dms << 2) - state_ptr->dml) >=
 495         (state_ptr->dml >> 3))
 496         state_ptr->ap += (0x200 - state_ptr->ap) >> 4;
 497     else
 498         state_ptr->ap += (-state_ptr->ap) >> 4;
 499 }
 500
 501 /*
 502  * tandem_adjust(sr, se, y, i, sign)
 503  *
 504  * At the end of ADPCM decoding, it simulates an encoder which may be receiving
 505  * the output of this decoder as a tandem process. If the output of the
 506  * simulated encoder differs from the input to this decoder, the decoder output
 507  * is adjusted by one level of A-law or u-law codes.
 508  *
 509  * Input:
 510  *    sr    decoder output linear PCM sample,
 511  *    se    predictor estimate sample,
 512  *    y     quantizer step size,
 513  *    i     decoder input code,
 514  *    sign  sign bit of code i
 515  *
 516  * Return:
 517  *    adjusted A-law or u-law compressed sample.
 518  */
 519 int
 520 tandem_adjust_alaw(
 521     int        sr,    /* decoder output linear PCM sample */
 522     int        se,    /* predictor estimate sample */
 523     int        y,     /* quantizer step size */
 524     int        i,     /* decoder input code */
 525     int        sign,
 526     short     *qtab)
 527 {
 528     unsigned char    sp;    /* A-law compressed 8-bit code */
 529     short            dx;    /* prediction error */
 530     char             id;    /* quantized prediction error */
 531     int              sd;    /* adjusted A-law decoded sample value */
 532     int              im;    /* biased magnitude of i */
 533     int              imx;   /* biased magnitude of id */
 534
 535     if (sr <= -32768)
 536         sr = -1;
 537     sp = linear2alaw((sr >> 1) << 3);    /* short to A-law compression */
 538     dx = (alaw2linear(sp) >> 2) - se;    /* 16-bit prediction error */
 539     id = quantize(dx, y, qtab, sign - 1);
 540
 541     if (id == i) {            /* no adjustment on sp */
 542         return (sp);
 543     } else {            /* sp adjustment needed */
 544         /* ADPCM codes : 8, 9, ... F, 0, 1, ... , 6, 7 */
 545         im = i ^ sign;        /* 2's complement to biased unsigned */
 546         imx = id ^ sign;
 547
 548         if (imx > im) {        /* sp adjusted to next lower value */
 549             if (sp & 0x80) {
 550                 sd = (sp == 0xD5) ? 0x55 :
 551                     ((sp ^ 0x55) - 1) ^ 0x55;
 552             } else {
 553                 sd = (sp == 0x2A) ? 0x2A :
 554                     ((sp ^ 0x55) + 1) ^ 0x55;
 555             }
 556         } else {        /* sp adjusted to next higher value */
 557             if (sp & 0x80)
 558                 sd = (sp == 0xAA) ? 0xAA :
 559                     ((sp ^ 0x55) + 1) ^ 0x55;
 560             else
 561                 sd = (sp == 0x55) ? 0xD5 :
 562                     ((sp ^ 0x55) - 1) ^ 0x55;
 563         }
 564         return (sd);
 565     }
 566 }
 567
 568 int
 569 tandem_adjust_ulaw(
 570     int        sr,    /* decoder output linear PCM sample */
 571     int        se,    /* predictor estimate sample */
 572     int        y,    /* quantizer step size */
 573     int        i,    /* decoder input code */
 574     int        sign,
 575     short     *qtab)
 576 {
 577     unsigned char    sp;    /* u-law compressed 8-bit code */
 578     short            dx;    /* prediction error */
 579     char             id;    /* quantized prediction error */
 580     int              sd;    /* adjusted u-law decoded sample value */
 581     int              im;    /* biased magnitude of i */
 582     int              imx;   /* biased magnitude of id */
 583
 584     if (sr <= -32768)
 585         sr = 0;
 586     sp = linear2ulaw(sr << 2);    /* short to u-law compression */
 587     dx = (ulaw2linear(sp) >> 2) - se;    /* 16-bit prediction error */
 588     id = quantize(dx, y, qtab, sign - 1);
 589     if (id == i) {
 590         return (sp);
 591     } else {
 592         /* ADPCM codes : 8, 9, ... F, 0, 1, ... , 6, 7 */
 593         im = i ^ sign;        /* 2's complement to biased unsigned */
 594         imx = id ^ sign;
 595         if (imx > im) {        /* sp adjusted to next lower value */
 596             if (sp & 0x80)
 597                 sd = (sp == 0xFF) ? 0x7E : sp + 1;
 598             else
 599                 sd = (sp == 0) ? 0 : sp - 1;
 600
 601         } else {        /* sp adjusted to next higher value */
 602             if (sp & 0x80)
 603                 sd = (sp == 0x80) ? 0x80 : sp - 1;
 604             else
 605                 sd = (sp == 0x7F) ? 0xFE : sp + 1;
 606         }
 607         return (sd);
 608     }
 609 }