usr/src/cmd/audio/utilities/g723.c

   1 /*
   2  * CDDL HEADER START
   3  *
   4  * The contents of this file are subject to the terms of the
   5  * Common Development and Distribution License, Version 1.0 only
   6  * (the "License").  You may not use this file except in compliance
   7  * with the License.
   8  *
   9  * You can obtain a copy of the license at usr/src/OPENSOLARIS.LICENSE
  10  * or http://www.opensolaris.org/os/licensing.
  11  * See the License for the specific language governing permissions
  12  * and limitations under the License.
  13  *
  14  * When distributing Covered Code, include this CDDL HEADER in each
  15  * file and include the License file at usr/src/OPENSOLARIS.LICENSE.
  16  * If applicable, add the following below this CDDL HEADER, with the
  17  * fields enclosed by brackets "[]" replaced with your own identifying
  18  * information: Portions Copyright [yyyy] [name of copyright owner]
  19  *
  20  * CDDL HEADER END
  21  */
  22 /*
  23  * Copyright (c) 1992-2001 by Sun Microsystems, Inc.
  24  * All rights reserved.
  25  */
  26
  27 #pragma ident   "%Z%%M% %I%     %E% SMI"
  28
  29 /*
  30  * Description:
  31  *
  32  * g723_init_state(), g723_encode(), g723_decode()
  33  *
  34  * These routines comprise an implementation of the CCITT G.723 ADPCM coding
  35  * algorithm.  Essentially, this implementation is identical to
  36  * the bit level description except for a few deviations which
  37  * take advantage of work station attributes, such as hardware 2's
  38  * complement arithmetic and large memory. Specifically, certain time
  39  * consuming operations such as multiplications are replaced
  40  * with look up tables and software 2's complement operations are
  41  * replaced with hardware 2's complement.
  42  *
  43  * The deviation (look up tables) from the bit level
  44  * specification, preserves the bit level performance specifications.
  45  *
  46  * As outlined in the G.723 Recommendation, the algorithm is broken
  47  * down into modules.  Each section of code below is preceded by
  48  * the name of the module which it is implementing.
  49  *
  50  */
  51 #include <stdlib.h>
  52 #include <libaudio.h>
  53
  54 /*
  55  * g723_tables.c
  56  *
  57  * Description:
  58  *
  59  * This file contains statically defined lookup tables for
  60  * use with the G.723 coding routines.
  61  */
  62
  63 /*
  64  * Maps G.723 code word to reconstructed scale factor normalized log
  65  * magnitude values.
  66  */
  67 static short    _dqlntab[8] = {-2048, 135, 273, 373, 373, 273, 135, -2048};
  68
  69 /* Maps G.723 code word to log of scale factor multiplier. */
  70 static short    _witab[8] = {-128, 960, 4384, 18624, 18624, 4384, 960, -128};
  71
  72 /*
  73  * Maps G.723 code words to a set of values whose long and short
  74  * term averages are computed and then compared to give an indication
  75  * how stationary (steady state) the signal is.
  76  */
  77 static short    _fitab[8] = {0, 0x200, 0x400, 0xE00, 0xE00, 0x400, 0x200, 0};
  78
  79 /*
  80  * g723_init_state()
  81  *
  82  * Description:
  83  *
  84  * This routine initializes and/or resets the audio_encode_state structure
  85  * pointed to by 'state_ptr'.
  86  * All the state initial values are specified in the G.723 standard specs.
  87  */
  88 void
  89 g723_init_state(
  90         struct audio_g72x_state *state_ptr)
  91 {
  92         int cnta;
  93
  94         state_ptr->yl = 34816;
  95         state_ptr->yu = 544;
  96         state_ptr->dms = 0;
  97         state_ptr->dml = 0;
  98         state_ptr->ap = 0;
  99         for (cnta = 0; cnta < 2; cnta++) {
 100                 state_ptr->a[cnta] = 0;
 101                 state_ptr->pk[cnta] = 0;
 102                 state_ptr->sr[cnta] = 32;
 103         }
 104         for (cnta = 0; cnta < 6; cnta++) {
 105                 state_ptr->b[cnta] = 0;
 106                 state_ptr->dq[cnta] = 32;
 107         }
 108         state_ptr->td = 0;
 109         state_ptr->leftover_cnt = 0;            /* no left over codes */
 110 }
 111
 112 /*
 113  * _g723_fmult()
 114  *
 115  * returns the integer product of the "floating point" an and srn
 116  * by the lookup table _fmultwanmant[].
 117  *
 118  */
 119 static int
 120 _g723_fmult(
 121                 int an,
 122                 int srn)
 123 {
 124         short   anmag, anexp, anmant;
 125         short   wanexp;
 126
 127         if (an == 0) {
 128                 return ((srn >= 0) ?
 129                     ((srn & 077) + 1) >> (18 - (srn >> 6)) :
 130                     -(((srn & 077) + 1) >> (2 - (srn >> 6))));
 131         } else if (an > 0) {
 132                 anexp = _fmultanexp[an] - 12;
 133                 anmant = ((anexp >= 0) ? an >> anexp : an << -anexp) & 07700;
 134                 if (srn >= 0) {
 135                         wanexp = anexp + (srn >> 6) - 7;
 136                         return ((wanexp >= 0) ?
 137                             (_fmultwanmant[(srn & 077) + anmant] << wanexp)
 138                             & 0x7FFF :
 139                             _fmultwanmant[(srn & 077) + anmant] >> -wanexp);
 140                 } else {
 141                         wanexp = anexp + (srn >> 6) - 0xFFF7;
 142                         return ((wanexp >= 0) ?
 143                             -((_fmultwanmant[(srn & 077) + anmant] << wanexp)
 144                             & 0x7FFF) :
 145                             -(_fmultwanmant[(srn & 077) + anmant] >> -wanexp));
 146                 }
 147         } else {
 148                 anmag = (-an) & 0x1FFF;
 149                 anexp = _fmultanexp[anmag] - 12;
 150                 anmant = ((anexp >= 0) ? anmag >> anexp : anmag << -anexp)
 151                     & 07700;
 152                 if (srn >= 0) {
 153                         wanexp = anexp + (srn >> 6) - 7;
 154                         return ((wanexp >= 0) ?
 155                             -((_fmultwanmant[(srn & 077) + anmant] << wanexp)
 156                             & 0x7FFF) :
 157                             -(_fmultwanmant[(srn & 077) + anmant] >> -wanexp));
 158                 } else {
 159                         wanexp = anexp + (srn >> 6) - 0xFFF7;
 160                         return ((wanexp >= 0) ?
 161                             (_fmultwanmant[(srn & 077) + anmant] << wanexp)
 162                             & 0x7FFF :
 163                             _fmultwanmant[(srn & 077) + anmant] >> -wanexp);
 164                 }
 165         }
 166
 167 }
 168
 169 /*
 170  * _g723_update()
 171  *
 172  * updates the state variables for each output code
 173  *
 174  */
 175 static void
 176 _g723_update(
 177         int     y,
 178         int     i,
 179         int     dq,
 180         int     sr,
 181         int     pk0,
 182         struct audio_g72x_state *state_ptr,
 183         int     sigpk)
 184 {
 185         int     cnt;
 186         long    fi;                     /* Adaptation speed control, FUNCTF */
 187         short   mag, exp;               /* Adaptive predictor, FLOAT A */
 188         short   a2p;                    /* LIMC */
 189         short   a1ul;                   /* UPA1 */
 190         short   pks1, fa1;              /* UPA2 */
 191         char    tr;                     /* tone/transition detector */
 192         short   thr2;
 193
 194         mag = dq & 0x3FFF;
 195         /* TRANS */
 196         if (state_ptr->td == 0)
 197                 tr = 0;
 198         else if (state_ptr->yl > 0x40000)
 199                 tr = (mag <= 0x2F80) ? 0 : 1;
 200         else {
 201                 thr2 = (0x20 + ((state_ptr->yl >> 10) & 0x1F)) <<
 202                     (state_ptr->yl >> 15);
 203                 if (mag >= thr2)
 204                         tr = 1;
 205                 else
 206                         tr = (mag <= (thr2 - (thr2 >> 2))) ? 0 : 1;
 207         }
 208
 209         /*
 210          * Quantizer scale factor adaptation.
 211          */
 212
 213         /* FUNCTW & FILTD & DELAY */
 214         state_ptr->yu = y + ((_witab[i] - y) >> 5);
 215
 216         /* LIMB */
 217         if (state_ptr->yu < 544)
 218                 state_ptr->yu = 544;
 219         else if (state_ptr->yu > 5120)
 220                 state_ptr->yu = 5120;
 221
 222         /* FILTE & DELAY */
 223         state_ptr->yl += state_ptr->yu + ((-state_ptr->yl) >> 6);
 224
 225         /*
 226          * Adaptive predictor coefficients.
 227          */
 228         if (tr == 1) {
 229                 state_ptr->a[0] = 0;
 230                 state_ptr->a[1] = 0;
 231                 state_ptr->b[0] = 0;
 232                 state_ptr->b[1] = 0;
 233                 state_ptr->b[2] = 0;
 234                 state_ptr->b[3] = 0;
 235                 state_ptr->b[4] = 0;
 236                 state_ptr->b[5] = 0;
 237         } else {
 238
 239                 /* UPA2 */
 240                 pks1 = pk0 ^ state_ptr->pk[0];
 241
 242                 a2p = state_ptr->a[1] - (state_ptr->a[1] >> 7);
 243                 if (sigpk == 0) {
 244                         fa1 = (pks1) ? state_ptr->a[0] : -state_ptr->a[0];
 245                         if (fa1 < -8191)
 246                                 a2p -= 0x100;
 247                         else if (fa1 > 8191)
 248                                 a2p += 0xFF;
 249                         else
 250                                 a2p += fa1 >> 5;
 251
 252                         if (pk0 ^ state_ptr->pk[1])
 253                                 /* LIMC */
 254                                 if (a2p <= -12160)
 255                                         a2p = -12288;
 256                                 else if (a2p >= 12416)
 257                                         a2p = 12288;
 258                                 else
 259                                         a2p -= 0x80;
 260                         else if (a2p <= -12416)
 261                                 a2p = -12288;
 262                         else if (a2p >= 12160)
 263                                 a2p = 12288;
 264                         else
 265                                 a2p += 0x80;
 266                 }
 267
 268                 /* TRIGB & DELAY */
 269                 state_ptr->a[1] = a2p;
 270
 271                 /* UPA1 */
 272                 state_ptr->a[0] -= state_ptr->a[0] >> 8;
 273                 if (sigpk == 0)
 274                         if (pks1 == 0)
 275                                 state_ptr->a[0] += 192;
 276                         else
 277                                 state_ptr->a[0] -= 192;
 278
 279                 /* LIMD */
 280                 a1ul = 15360 - a2p;
 281                 if (state_ptr->a[0] < -a1ul)
 282                         state_ptr->a[0] = -a1ul;
 283                 else if (state_ptr->a[0] > a1ul)
 284                         state_ptr->a[0] = a1ul;
 285
 286                 /* UPB : update of b's */
 287                 for (cnt = 0; cnt < 6; cnt++) {
 288                         state_ptr->b[cnt] -= state_ptr->b[cnt] >> 8;
 289                         if (dq & 0x3FFF) {
 290                                 /* XOR */
 291                                 if ((dq ^ state_ptr->dq[cnt]) >= 0)
 292                                         state_ptr->b[cnt] += 128;
 293                                 else
 294                                         state_ptr->b[cnt] -= 128;
 295                         }
 296                 }
 297         }
 298
 299         for (cnt = 5; cnt > 0; cnt--)
 300                 state_ptr->dq[cnt] = state_ptr->dq[cnt-1];
 301         /* FLOAT A */
 302         if (mag == 0) {
 303                 state_ptr->dq[0] = (dq >= 0) ? 0x20 : 0xFC20;
 304         } else {
 305                 exp = _fmultanexp[mag];
 306                 state_ptr->dq[0] = (dq >= 0) ?
 307                     (exp << 6) + ((mag << 6) >> exp) :
 308                     (exp << 6) + ((mag << 6) >> exp) - 0x400;
 309         }
 310
 311         state_ptr->sr[1] = state_ptr->sr[0];
 312         /* FLOAT B */
 313         if (sr == 0) {
 314                 state_ptr->sr[0] = 0x20;
 315         } else if (sr > 0) {
 316                 exp = _fmultanexp[sr];
 317                 state_ptr->sr[0] = (exp << 6) + ((sr << 6) >> exp);
 318         } else {
 319                 mag = -sr;
 320                 exp = _fmultanexp[mag];
 321                 state_ptr->sr[0] =  (exp << 6) + ((mag << 6) >> exp) - 0x400;
 322         }
 323
 324         /* DELAY A */
 325         state_ptr->pk[1] = state_ptr->pk[0];
 326         state_ptr->pk[0] = pk0;
 327
 328         /* TONE */
 329         if (tr == 1)
 330                 state_ptr->td = 0;
 331         else if (a2p < -11776)
 332                 state_ptr->td = 1;
 333         else
 334                 state_ptr->td = 0;
 335
 336         /*
 337          * Adaptation speed control.
 338          */
 339         fi = _fitab[i];                                         /* FUNCTF */
 340         state_ptr->dms += (fi - state_ptr->dms) >> 5;           /* FILTA */
 341         state_ptr->dml += (((fi << 2) - state_ptr->dml) >> 7);  /* FILTB */
 342
 343         if (tr == 1)
 344                 state_ptr->ap = 256;
 345         else if (y < 1536)                                      /* SUBTC */
 346                 state_ptr->ap += (0x200 - state_ptr->ap) >> 4;
 347         else if (state_ptr->td == 1)
 348                 state_ptr->ap += (0x200 - state_ptr->ap) >> 4;
 349         else if (abs((state_ptr->dms << 2) - state_ptr->dml) >=
 350             (state_ptr->dml >> 3))
 351                 state_ptr->ap += (0x200 - state_ptr->ap) >> 4;
 352         else
 353                 state_ptr->ap += (-state_ptr->ap) >> 4;
 354 }
 355
 356 /*
 357  * _g723_quantize()
 358  *
 359  * Description:
 360  *
 361  * Given a raw sample, 'd', of the difference signal and a
 362  * quantization step size scale factor, 'y', this routine returns the
 363  * G.723 codeword to which that sample gets quantized.  The step
 364  * size scale factor division operation is done in the log base 2 domain
 365  * as a subtraction.
 366  */
 367 static unsigned int
 368 _g723_quantize(
 369         int     d,      /* Raw difference signal sample. */
 370         int     y)      /* Step size multiplier. */
 371 {
 372         /* LOG */
 373         short   dqm;    /* Magnitude of 'd'. */
 374         short   exp;    /* Integer part of base 2 log of magnitude of 'd'. */
 375         short   mant;   /* Fractional part of base 2 log. */
 376         short   dl;     /* Log of magnitude of 'd'. */
 377
 378         /* SUBTB */
 379         short   dln;    /* Step size scale factor normalized log. */
 380
 381         /* QUAN */
 382         unsigned char   i;      /* G.723 codeword. */
 383
 384         /*
 385          * LOG
 386          *
 387          * Compute base 2 log of 'd', and store in 'dln'.
 388          *
 389          */
 390         dqm = abs(d);
 391         exp = _fmultanexp[dqm >> 1];
 392         mant = ((dqm << 7) >> exp) & 0x7F;      /* Fractional portion. */
 393         dl = (exp << 7) + mant;
 394
 395         /*
 396          * SUBTB
 397          *
 398          * "Divide" by step size multiplier.
 399          */
 400         dln = dl - (y >> 2);
 401
 402         /*
 403          * QUAN
 404          *
 405          * Obtain codword for 'd'.
 406          */
 407         i = _g723quani[dln & 0xFFF];
 408         if (d < 0)
 409                 i ^= 7;         /* Stuff in sign of 'd'. */
 410         else if (i == 0)
 411                 i = 7;          /* New in 1988 revision */
 412
 413         return (i);
 414 }
 415
 416 /*
 417  * _g723_reconstr()
 418  *
 419  * Description:
 420  *
 421  * Returns reconstructed difference signal 'dq' obtained from
 422  * G.723 codeword 'i' and quantization step size scale factor 'y'.
 423  * Multiplication is performed in log base 2 domain as addition.
 424  */
 425 static int
 426 _g723_reconstr(
 427         int             i,      /* G.723 codeword. */
 428         unsigned long   y)      /* Step size multiplier. */
 429 {
 430         /* ADD A */
 431         short   dql;    /* Log of 'dq' magnitude. */
 432
 433         /* ANTILOG */
 434         short   dex;    /* Integer part of log. */
 435         short   dqt;
 436         short   dq;     /* Reconstructed difference signal sample. */
 437
 438
 439         dql = _dqlntab[i] + (y >> 2);   /* ADDA */
 440
 441         if (dql < 0)
 442                 dq = 0;
 443         else {                          /* ANTILOG */
 444                 dex = (dql >> 7) & 15;
 445                 dqt = 128 + (dql & 127);
 446                 dq = (dqt << 7) >> (14 - dex);
 447         }
 448         if (i & 4)
 449                 dq -= 0x8000;
 450
 451         return (dq);
 452 }
 453
 454 /*
 455  * _tandem_adjust(sr, se, y, i)
 456  *
 457  * Description:
 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 Mu-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  *
 470  * Return:
 471  *      adjusted A-law or Mu-law compressed sample.
 472  */
 473 static int
 474 _tandem_adjust_alaw(
 475         int     sr,     /* decoder output linear PCM sample */
 476         int     se,     /* predictor estimate sample */
 477         int     y,      /* quantizer step size */
 478         int     i)      /* decoder input code */
 479 {
 480         unsigned char   sp;     /* A-law compressed 8-bit code */
 481         short   dx;             /* prediction error */
 482         char    id;             /* quantized prediction error */
 483         int     sd;             /* adjusted A-law decoded sample value */
 484         int     im;             /* biased magnitude of i */
 485         int     imx;            /* biased magnitude of id */
 486
 487         sp = audio_s2a((sr <= -0x2000)? -0x8000 :
 488             (sr < 0x1FFF)? sr << 2 : 0x7FFF); /* short to A-law compression */
 489         dx = (audio_a2s(sp) >> 2) - se;  /* 16-bit prediction error */
 490         id = _g723_quantize(dx, y);
 491
 492         if (id == i)                    /* no adjustment on sp */
 493                 return (sp);
 494         else {                          /* sp adjustment needed */
 495                 im = i ^ 4;             /* 2's complement to biased unsigned */
 496                 imx = id ^ 4;
 497
 498                 if (imx > im) {         /* sp adjusted to next lower value */
 499                         if (sp & 0x80)
 500                                 sd = (sp == 0xD5)? 0x55 :
 501                                     ((sp ^ 0x55) - 1) ^ 0x55;
 502                         else
 503                                 sd = (sp == 0x2A)? 0x2A :
 504                                     ((sp ^ 0x55) + 1) ^ 0x55;
 505                 } else {        /* sp adjusted to next higher value */
 506                         if (sp & 0x80)
 507                                 sd = (sp == 0xAA)? 0xAA :
 508                                     ((sp ^ 0x55) + 1) ^ 0x55;
 509                         else
 510                                 sd = (sp == 0x55)? 0xD5 :
 511                                     ((sp ^ 0x55) - 1) ^ 0x55;
 512                 }
 513                 return (sd);
 514         }
 515 }
 516
 517 static int
 518 _tandem_adjust_ulaw(
 519         int     sr,             /* decoder output linear PCM sample */
 520         int     se,             /* predictor estimate sample */
 521         int     y,              /* quantizer step size */
 522         int     i)              /* decoder input code */
 523 {
 524         unsigned char   sp;     /* A-law compressed 8-bit code */
 525         short   dx;             /* prediction error */
 526         char    id;             /* quantized prediction error */
 527         int     sd;             /* adjusted A-law decoded sample value */
 528         int     im;             /* biased magnitude of i */
 529         int     imx;            /* biased magnitude of id */
 530
 531         sp = audio_s2u((sr <= -0x2000)? -0x8000 :
 532             (sr >= 0x1FFF)? 0x7FFF : sr << 2); /* short to u-law compression */
 533         dx = (audio_u2s(sp) >> 2) - se;  /* 16-bit prediction error */
 534         id = _g723_quantize(dx, y);
 535         if (id == i)
 536                 return (sp);
 537         else {
 538                 /* ADPCM codes : 8, 9, ... F, 0, 1, ... , 6, 7 */
 539                 im = i ^ 4;             /* 2's complement to biased unsigned */
 540                 imx = id ^ 4;
 541
 542                 /* u-law codes : 0, 1, ... 7E, 7F, FF, FE, ... 81, 80 */
 543                 if (imx > im) {         /* sp adjusted to next lower value */
 544                         if (sp & 0x80)
 545                                 sd = (sp == 0xFF)? 0x7E : sp + 1;
 546                         else
 547                                 sd = (sp == 0)? 0 : sp - 1;
 548
 549                 } else {                /* sp adjusted to next higher value */
 550                         if (sp & 0x80)
 551                                 sd = (sp == 0x80)? 0x80 : sp - 1;
 552                         else
 553                                 sd = (sp == 0x7F)? 0xFE : sp + 1;
 554                 }
 555                 return (sd);
 556         }
 557 }
 558
 559 static unsigned char
 560 _encoder(
 561         int             sl,
 562         struct audio_g72x_state *state_ptr)
 563 {
 564         short   sei, sezi, se, sez;     /* ACCUM */
 565         short   d;                      /* SUBTA */
 566         float   al;             /* use floating point for faster multiply */
 567         short   y, dif;                 /* MIX */
 568         short   sr;                     /* ADDB */
 569         short   pk0, sigpk, dqsez;      /* ADDC */
 570         short   dq, i;
 571         int     cnt;
 572
 573         /* ACCUM */
 574         sezi = _g723_fmult(state_ptr->b[0] >> 2, state_ptr->dq[0]);
 575         for (cnt = 1; cnt < 6; cnt++)
 576                 sezi = sezi + _g723_fmult(state_ptr->b[cnt] >> 2,
 577                     state_ptr->dq[cnt]);
 578         sei = sezi;
 579         for (cnt = 1; cnt > -1; cnt--)
 580                 sei = sei + _g723_fmult(state_ptr->a[cnt] >> 2,
 581                     state_ptr->sr[cnt]);
 582         sez = sezi >> 1;
 583         se = sei >> 1;
 584
 585         d = sl - se;                                    /* SUBTA */
 586
 587         if (state_ptr->ap >= 256)
 588                 y = state_ptr->yu;
 589         else {
 590                 y = state_ptr->yl >> 6;
 591                 dif = state_ptr->yu - y;
 592                 al = state_ptr->ap >> 2;
 593                 if (dif > 0)
 594                         y += ((int)(dif * al)) >> 6;
 595                 else if (dif < 0)
 596                         y += ((int)(dif * al) + 0x3F) >> 6;
 597         }
 598
 599         i = _g723_quantize(d, y);
 600         dq = _g723_reconstr(i, y);
 601
 602         sr = (dq < 0) ? se - (dq & 0x3FFF) : se + dq;   /* ADDB */
 603
 604         dqsez = sr + sez - se;                          /* ADDC */
 605         if (dqsez == 0) {
 606                 pk0 = 0;
 607                 sigpk = 1;
 608         } else {
 609                 pk0 = (dqsez < 0) ? 1 : 0;
 610                 sigpk = 0;
 611         }
 612
 613         _g723_update(y, i, dq, sr, pk0, state_ptr, sigpk);
 614
 615         return (i);
 616 }
 617
 618 /*
 619  * g723_encode()
 620  *
 621  * Description:
 622  *
 623  * Encodes a buffer of linear PCM, A-law or Mu-law data pointed to by 'in_buf'
 624  * according the G.723 encoding algorithm and packs the resulting code words
 625  * into bytes. The bytes of codewords are written to a buffer
 626  * pointed to by 'out_buf'.
 627  *
 628  * Notes:
 629  *
 630  * In the event that the number packed codes is shorter than a sample unit,
 631  * the remainder is saved in the state stucture till next call.  It is then
 632  * packed into the new buffer on the next call.
 633  * The number of valid bytes in 'out_buf' is returned in *out_size.  Note that
 634  * this will not always be equal to 3/8 of 'data_size' on input. On the
 635  * final call to 'g723_encode()' the calling program might want to
 636  * check if any code bits was left over.  This can be
 637  * done by calling 'g723_encode()' with data_size = 0, which returns in
 638  * *out_size a* 0 if nothing was leftover and the number of bits left over in
 639  * the state structure which now is in out_buf[0].
 640  *
 641  * The 3 lower significant bits of an individual byte in the output byte
 642  * stream is packed with a G.723 code first.  Then the 3 higher order
 643  * bits are packed with the next code.
 644  */
 645 int
 646 g723_encode(
 647         void            *in_buf,
 648         int             data_size,
 649         Audio_hdr       *in_header,
 650         unsigned char   *out_buf,
 651         int             *out_size,
 652         struct audio_g72x_state *state_ptr)
 653 {
 654         int             i;
 655         unsigned char   *out_ptr;
 656         unsigned char   *leftover;
 657         unsigned int    bits;
 658         unsigned int    codes;
 659         int             offset;
 660         short           *short_ptr;
 661         unsigned char   *char_ptr;
 662
 663         /* Dereference the array pointer for faster access */
 664         leftover = &state_ptr->leftover[0];
 665
 666         /* Return all cached leftovers */
 667         if (data_size == 0) {
 668                 for (i = 0; state_ptr->leftover_cnt > 0; i++) {
 669                         *out_buf++ = leftover[i];
 670                         state_ptr->leftover_cnt -= 8;
 671                 }
 672                 if (i > 0) {
 673                         /* Round up to a complete sample unit */
 674                         for (; i < 3; i++)
 675                                 *out_buf++ = 0;
 676                 }
 677                 *out_size = i;
 678                 state_ptr->leftover_cnt = 0;
 679                 return (AUDIO_SUCCESS);
 680         }
 681
 682         /* XXX - if linear, it had better be 16-bit! */
 683         if (in_header->encoding == AUDIO_ENCODING_LINEAR) {
 684                 if (data_size & 1) {
 685                         return (AUDIO_ERR_BADFRAME);
 686                 } else {
 687                         data_size >>= 1;
 688                         short_ptr = (short *)in_buf;
 689                 }
 690         } else {
 691                 char_ptr = (unsigned char *)in_buf;
 692         }
 693         out_ptr = (unsigned char *)out_buf;
 694
 695         offset = state_ptr->leftover_cnt / 8;
 696         bits = state_ptr->leftover_cnt % 8;
 697         codes = (bits > 0) ? leftover[offset] : 0;
 698
 699         while (data_size--) {
 700                 switch (in_header->encoding) {
 701                 case AUDIO_ENCODING_LINEAR:
 702                         i = _encoder(*short_ptr++ >> 2, state_ptr);
 703                         break;
 704                 case AUDIO_ENCODING_ALAW:
 705                         i = _encoder(audio_a2s(*char_ptr++) >> 2, state_ptr);
 706                         break;
 707                 case AUDIO_ENCODING_ULAW:
 708                         i = _encoder(audio_u2s(*char_ptr++) >> 2, state_ptr);
 709                         break;
 710                 default:
 711                         return (AUDIO_ERR_ENCODING);
 712                 }
 713                 /* pack the resulting code into leftover buffer */
 714                 codes += i << bits;
 715                 bits += 3;
 716                 if (bits >= 8) {
 717                         leftover[offset] = codes & 0xff;
 718                         bits -= 8;
 719                         codes >>= 8;
 720                         offset++;
 721                 }
 722                 state_ptr->leftover_cnt += 3;
 723
 724                 /* got a whole sample unit so copy it out and reset */
 725                 if (bits == 0) {
 726                         *out_ptr++ = leftover[0];
 727                         *out_ptr++ = leftover[1];
 728                         *out_ptr++ = leftover[2];
 729                         codes = 0;
 730                         state_ptr->leftover_cnt = 0;
 731                         offset = 0;
 732                 }
 733         }
 734         /* If any residual bits, save them for the next call */
 735         if (bits > 0) {
 736                 leftover[offset] = codes & 0xff;
 737                 state_ptr->leftover_cnt += bits;
 738         }
 739         *out_size = (out_ptr - (unsigned char *)out_buf);
 740         return (AUDIO_SUCCESS);
 741 }
 742
 743 /*
 744  * g723_decode()
 745  *
 746  * Description:
 747  *
 748  * Decodes a buffer of G.723 encoded data pointed to by 'in_buf' and
 749  * writes the resulting linear PCM, A-law or Mu-law words into a buffer
 750  * pointed to by 'out_buf'.
 751  *
 752  */
 753 int
 754 g723_decode(
 755         unsigned char   *in_buf,        /* Buffer of g723 encoded data. */
 756         int             data_size,      /* Size in bytes of in_buf. */
 757         Audio_hdr       *out_header,
 758         void            *out_buf,       /* Decoded data buffer. */
 759         int             *out_size,
 760         struct audio_g72x_state *state_ptr) /* the decoder's state structure. */
 761 {
 762         unsigned char   *inbuf_end;
 763         unsigned char   *in_ptr, *out_ptr;
 764         short           *linear_ptr;
 765         unsigned int    codes;
 766         unsigned int    bits;
 767         int             cnt;
 768
 769         short   sezi, sei, sez, se;             /* ACCUM */
 770         float   al;             /* use floating point for faster multiply */
 771         short   y, dif;                         /* MIX */
 772         short   sr;                             /* ADDB */
 773         char    pk0;                            /* ADDC */
 774         short   dq;
 775         char    sigpk;
 776         short   dqsez;
 777         unsigned char i;
 778
 779         in_ptr = in_buf;
 780         inbuf_end = in_buf + data_size;
 781         out_ptr = (unsigned char *)out_buf;
 782         linear_ptr = (short *)out_buf;
 783
 784         /* Leftovers in decoding are only up to 8 bits */
 785         bits = state_ptr->leftover_cnt;
 786         codes = (bits > 0) ? state_ptr->leftover[0] : 0;
 787
 788         while ((bits >= 3) || (in_ptr < (unsigned char *)inbuf_end)) {
 789                 if (bits < 3) {
 790                         codes += *in_ptr++ << bits;
 791                         bits += 8;
 792                 }
 793
 794                 /* ACCUM */
 795                 sezi = _g723_fmult(state_ptr->b[0] >> 2, state_ptr->dq[0]);
 796                 for (cnt = 1; cnt < 6; cnt++)
 797                         sezi = sezi + _g723_fmult(state_ptr->b[cnt] >> 2,
 798                             state_ptr->dq[cnt]);
 799                 sei = sezi;
 800                 for (cnt = 1; cnt >= 0; cnt--)
 801                         sei = sei + _g723_fmult(state_ptr->a[cnt] >> 2,
 802                             state_ptr->sr[cnt]);
 803
 804                 sez = sezi >> 1;
 805                 se = sei >> 1;
 806                 if (state_ptr->ap >= 256)
 807                         y = state_ptr->yu;
 808                 else {
 809                         y = state_ptr->yl >> 6;
 810                         dif = state_ptr->yu - y;
 811                         al = state_ptr->ap >> 2;
 812                         if (dif > 0)
 813                                 y += ((int)(dif * al)) >> 6;
 814                         else if (dif < 0)
 815                                 y += ((int)(dif * al) + 0x3F) >> 6;
 816                 }
 817
 818                 i = codes & 7;
 819                 dq = _g723_reconstr(i, y);
 820                 /* ADDB */
 821                 if (dq < 0)
 822                         sr = se - (dq & 0x3FFF);
 823                 else
 824                         sr = se + dq;
 825
 826
 827                 dqsez = sr - se + sez;                  /* ADDC */
 828                 pk0 = (dqsez < 0) ? 1 : 0;
 829                 sigpk = (dqsez) ? 0 : 1;
 830
 831                 _g723_update(y, i, dq, sr, pk0, state_ptr, sigpk);
 832
 833                 switch (out_header->encoding) {
 834                 case AUDIO_ENCODING_LINEAR:
 835                         *linear_ptr++ = ((sr <= -0x2000) ? -0x8000 :
 836                             (sr >= 0x1FFF) ? 0x7FFF : sr << 2);
 837                         break;
 838                 case AUDIO_ENCODING_ALAW:
 839                         *out_ptr++ = _tandem_adjust_alaw(sr, se, y, i);
 840                         break;
 841                 case AUDIO_ENCODING_ULAW:
 842                         *out_ptr++ = _tandem_adjust_ulaw(sr, se, y, i);
 843                         break;
 844                 default:
 845                         return (AUDIO_ERR_ENCODING);
 846                 }
 847                 codes >>= 3;
 848                 bits -= 3;
 849         }
 850         state_ptr->leftover_cnt = bits;
 851         if (bits > 0)
 852                 state_ptr->leftover[0] = codes;
 853
 854         /* Calculate number of samples returned */
 855         if (out_header->encoding == AUDIO_ENCODING_LINEAR)
 856                 *out_size = linear_ptr - (short *)out_buf;
 857         else
 858                 *out_size = out_ptr - (unsigned char *)out_buf;
 859
 860         return (AUDIO_SUCCESS);
 861 }