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