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