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