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 }