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 */
67static short	_dqlntab[8] = {-2048, 135, 273, 373, 373, 273, 135, -2048};
68
69/* Maps G.723 code word to log of scale factor multiplier. */
70static 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 */
77static 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 */
88void
89g723_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 */
119static 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 */
175static 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 */
367static 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 */
425static 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 */
473static 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
517static 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
559static 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 */
645int
646g723_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 */
753int
754g723_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}
862