git.gag.com Git - debian/gnuradio/blob - gnuradio-core/src/lib/g72x/g72x.c

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