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