17c478bdstevel@tonic-gate/*
27c478bdstevel@tonic-gate * CDDL HEADER START
37c478bdstevel@tonic-gate *
47c478bdstevel@tonic-gate * The contents of this file are subject to the terms of the
57c478bdstevel@tonic-gate * Common Development and Distribution License, Version 1.0 only
67c478bdstevel@tonic-gate * (the "License").  You may not use this file except in compliance
77c478bdstevel@tonic-gate * with the License.
87c478bdstevel@tonic-gate *
97c478bdstevel@tonic-gate * You can obtain a copy of the license at usr/src/OPENSOLARIS.LICENSE
107c478bdstevel@tonic-gate * or http://www.opensolaris.org/os/licensing.
117c478bdstevel@tonic-gate * See the License for the specific language governing permissions
127c478bdstevel@tonic-gate * and limitations under the License.
137c478bdstevel@tonic-gate *
147c478bdstevel@tonic-gate * When distributing Covered Code, include this CDDL HEADER in each
157c478bdstevel@tonic-gate * file and include the License file at usr/src/OPENSOLARIS.LICENSE.
167c478bdstevel@tonic-gate * If applicable, add the following below this CDDL HEADER, with the
177c478bdstevel@tonic-gate * fields enclosed by brackets "[]" replaced with your own identifying
187c478bdstevel@tonic-gate * information: Portions Copyright [yyyy] [name of copyright owner]
197c478bdstevel@tonic-gate *
207c478bdstevel@tonic-gate * CDDL HEADER END
217c478bdstevel@tonic-gate */
227c478bdstevel@tonic-gate/*
237c478bdstevel@tonic-gate * Copyright (c) 1992-2001 by Sun Microsystems, Inc.
247c478bdstevel@tonic-gate * All rights reserved.
257c478bdstevel@tonic-gate */
267c478bdstevel@tonic-gate
277c478bdstevel@tonic-gate#pragma ident	"%Z%%M%	%I%	%E% SMI"
287c478bdstevel@tonic-gate
297c478bdstevel@tonic-gate/*
307c478bdstevel@tonic-gate *
317c478bdstevel@tonic-gate * Description:
327c478bdstevel@tonic-gate *
337c478bdstevel@tonic-gate * g721_encode(), g721_decode(), g721_set_law()
347c478bdstevel@tonic-gate *
357c478bdstevel@tonic-gate * These routines comprise an implementation of the CCITT G.721 ADPCM coding
367c478bdstevel@tonic-gate * algorithm.  Essentially, this implementation is identical to
377c478bdstevel@tonic-gate * the bit level description except for a few deviations which
387c478bdstevel@tonic-gate * take advantage of work station attributes, such as hardware 2's
397c478bdstevel@tonic-gate * complement arithmetic and large memory. Specifically, certain time
407c478bdstevel@tonic-gate * consuming operations such as multiplications are replaced
417c478bdstevel@tonic-gate * with look up tables and software 2's complement operations are
427c478bdstevel@tonic-gate * replaced with hardware 2's complement.
437c478bdstevel@tonic-gate *
447c478bdstevel@tonic-gate * The deviation (look up tables) from the bit level
457c478bdstevel@tonic-gate * specification, preserves the bit level performance specifications.
467c478bdstevel@tonic-gate *
477c478bdstevel@tonic-gate * As outlined in the G.721 Recommendation, the algorithm is broken
487c478bdstevel@tonic-gate * down into modules.  Each section of code below is preceded by
497c478bdstevel@tonic-gate * the name of the module which it is implementing.
507c478bdstevel@tonic-gate *
517c478bdstevel@tonic-gate */
527c478bdstevel@tonic-gate#include <stdlib.h>
537c478bdstevel@tonic-gate#include <libaudio.h>
547c478bdstevel@tonic-gate
557c478bdstevel@tonic-gate/*
567c478bdstevel@tonic-gate * Maps G.721 code word to reconstructed scale factor normalized log
577c478bdstevel@tonic-gate * magnitude values.
587c478bdstevel@tonic-gate */
597c478bdstevel@tonic-gatestatic short	_dqlntab[16] = {-2048, 4, 135, 213, 273, 323, 373, 425,
607c478bdstevel@tonic-gate		    425, 373, 323, 273, 213, 135, 4, -2048};
617c478bdstevel@tonic-gate
627c478bdstevel@tonic-gate/* Maps G.721 code word to log of scale factor multiplier. */
637c478bdstevel@tonic-gatestatic long	_witab[16] = {-384, 576, 1312, 2048, 3584, 6336, 11360, 35904,
647c478bdstevel@tonic-gate		    35904, 11360, 6336, 3584, 2048, 1312, 576, -384};
657c478bdstevel@tonic-gate
667c478bdstevel@tonic-gate/*
677c478bdstevel@tonic-gate * Maps G.721 code words to a set of values whose long and short
687c478bdstevel@tonic-gate * term averages are computed and then compared to give an indication
697c478bdstevel@tonic-gate * how stationary (steady state) the signal is.
707c478bdstevel@tonic-gate */
717c478bdstevel@tonic-gatestatic short	_fitab[16] = {0, 0, 0, 0x200, 0x200, 0x200, 0x600, 0xE00,
727c478bdstevel@tonic-gate		    0xE00, 0x600, 0x200, 0x200, 0x200, 0, 0, 0};
737c478bdstevel@tonic-gate
747c478bdstevel@tonic-gate/*
757c478bdstevel@tonic-gate * g721_init_state()
767c478bdstevel@tonic-gate *
777c478bdstevel@tonic-gate * Description:
787c478bdstevel@tonic-gate *
797c478bdstevel@tonic-gate * This routine initializes and/or resets the audio_g72x_state structure
807c478bdstevel@tonic-gate * pointed to by 'state_ptr'.
817c478bdstevel@tonic-gate * All the initial state values are specified in the G.721 standard specs.
827c478bdstevel@tonic-gate */
837c478bdstevel@tonic-gatevoid
847c478bdstevel@tonic-gateg721_init_state(
857c478bdstevel@tonic-gate	struct audio_g72x_state *state_ptr)
867c478bdstevel@tonic-gate{
877c478bdstevel@tonic-gate	int cnta;
887c478bdstevel@tonic-gate
897c478bdstevel@tonic-gate	state_ptr->yl = 34816;
907c478bdstevel@tonic-gate	state_ptr->yu = 544;
917c478bdstevel@tonic-gate	state_ptr->dms = 0;
927c478bdstevel@tonic-gate	state_ptr->dml = 0;
937c478bdstevel@tonic-gate	state_ptr->ap = 0;
947c478bdstevel@tonic-gate	for (cnta = 0; cnta < 2; cnta++) {
957c478bdstevel@tonic-gate		state_ptr->a[cnta] = 0;
967c478bdstevel@tonic-gate		state_ptr->pk[cnta] = 0;
977c478bdstevel@tonic-gate		state_ptr->sr[cnta] = 32;
987c478bdstevel@tonic-gate	}
997c478bdstevel@tonic-gate	for (cnta = 0; cnta < 6; cnta++) {
1007c478bdstevel@tonic-gate		state_ptr->b[cnta] = 0;
1017c478bdstevel@tonic-gate		state_ptr->dq[cnta] = 32;
1027c478bdstevel@tonic-gate	}
1037c478bdstevel@tonic-gate	state_ptr->td = 0;
1047c478bdstevel@tonic-gate	state_ptr->leftover_cnt = 0;		/* no left over codes */
1057c478bdstevel@tonic-gate}
1067c478bdstevel@tonic-gate
1077c478bdstevel@tonic-gate/*
1087c478bdstevel@tonic-gate * _g721_fmult()
1097c478bdstevel@tonic-gate *
1107c478bdstevel@tonic-gate * returns the integer product of the "floating point" an and srn
1117c478bdstevel@tonic-gate * by the lookup table _fmultwanmant[].
1127c478bdstevel@tonic-gate *
1137c478bdstevel@tonic-gate */
1147c478bdstevel@tonic-gatestatic int
1157c478bdstevel@tonic-gate_g721_fmult(
1167c478bdstevel@tonic-gate	int	an,
1177c478bdstevel@tonic-gate	int	srn)
1187c478bdstevel@tonic-gate{
1197c478bdstevel@tonic-gate	short	anmag, anexp, anmant;
1207c478bdstevel@tonic-gate	short	wanexp;
1217c478bdstevel@tonic-gate
1227c478bdstevel@tonic-gate	if (an == 0) {
1237c478bdstevel@tonic-gate		return ((srn >= 0) ?
1247c478bdstevel@tonic-gate		    ((srn & 077) + 1) >> (18 - (srn >> 6)) :
1257c478bdstevel@tonic-gate		    -(((srn & 077) + 1) >> (2 - (srn >> 6))));
1267c478bdstevel@tonic-gate	} else if (an > 0) {
1277c478bdstevel@tonic-gate		anexp = _fmultanexp[an] - 12;
1287c478bdstevel@tonic-gate		anmant = ((anexp >= 0) ? an >> anexp : an << -anexp) & 07700;
1297c478bdstevel@tonic-gate		if (srn >= 0) {
1307c478bdstevel@tonic-gate			wanexp = anexp + (srn >> 6) - 7;
1317c478bdstevel@tonic-gate			return ((wanexp >= 0) ?
1327c478bdstevel@tonic-gate			    (_fmultwanmant[(srn & 077) + anmant] << wanexp)
1337c478bdstevel@tonic-gate			    & 0x7FFF :
1347c478bdstevel@tonic-gate			    _fmultwanmant[(srn & 077) + anmant] >> -wanexp);
1357c478bdstevel@tonic-gate		} else {
1367c478bdstevel@tonic-gate			wanexp = anexp + (srn >> 6) - 0xFFF7;
1377c478bdstevel@tonic-gate			return ((wanexp >= 0) ?
1387c478bdstevel@tonic-gate			    -((_fmultwanmant[(srn & 077) + anmant] << wanexp)
1397c478bdstevel@tonic-gate			    & 0x7FFF) :
1407c478bdstevel@tonic-gate			    -(_fmultwanmant[(srn & 077) + anmant] >> -wanexp));
1417c478bdstevel@tonic-gate		}
1427c478bdstevel@tonic-gate	} else {
1437c478bdstevel@tonic-gate		anmag = (-an) & 0x1FFF;
1447c478bdstevel@tonic-gate		anexp = _fmultanexp[anmag] - 12;
1457c478bdstevel@tonic-gate		anmant = ((anexp >= 0) ? anmag >> anexp : anmag << -anexp)
1467c478bdstevel@tonic-gate		    & 07700;
1477c478bdstevel@tonic-gate		if (srn >= 0) {
1487c478bdstevel@tonic-gate			wanexp = anexp + (srn >> 6) - 7;
1497c478bdstevel@tonic-gate			return ((wanexp >= 0) ?
1507c478bdstevel@tonic-gate			    -((_fmultwanmant[(srn & 077) + anmant] << wanexp)
1517c478bdstevel@tonic-gate			    & 0x7FFF) :
1527c478bdstevel@tonic-gate			    -(_fmultwanmant[(srn & 077) + anmant] >> -wanexp));
1537c478bdstevel@tonic-gate		} else {
1547c478bdstevel@tonic-gate			wanexp = anexp + (srn >> 6) - 0xFFF7;
1557c478bdstevel@tonic-gate			return ((wanexp >= 0) ?
1567c478bdstevel@tonic-gate			    (_fmultwanmant[(srn & 077) + anmant] << wanexp)
1577c478bdstevel@tonic-gate			    & 0x7FFF :
1587c478bdstevel@tonic-gate			    _fmultwanmant[(srn & 077) + anmant] >> -wanexp);
1597c478bdstevel@tonic-gate		}
1607c478bdstevel@tonic-gate	}
1617c478bdstevel@tonic-gate}
1627c478bdstevel@tonic-gate
1637c478bdstevel@tonic-gate/*
1647c478bdstevel@tonic-gate * _g721_update()
1657c478bdstevel@tonic-gate *
1667c478bdstevel@tonic-gate * updates the state variables for each output code
1677c478bdstevel@tonic-gate *
1687c478bdstevel@tonic-gate */
1697c478bdstevel@tonic-gatestatic void
1707c478bdstevel@tonic-gate_g721_update(
1717c478bdstevel@tonic-gate	int	y,
1727c478bdstevel@tonic-gate	int	i,
1737c478bdstevel@tonic-gate	int	dq,
1747c478bdstevel@tonic-gate	int	sr,
1757c478bdstevel@tonic-gate	int	pk0,
1767c478bdstevel@tonic-gate	struct audio_g72x_state *state_ptr,
1777c478bdstevel@tonic-gate	int	sigpk)
1787c478bdstevel@tonic-gate{
1797c478bdstevel@tonic-gate	int	cnt;
1807c478bdstevel@tonic-gate	long	fi;				/* FUNCTF */
1817c478bdstevel@tonic-gate	short	mag, exp;			/* FLOAT A */
1827c478bdstevel@tonic-gate	short	a2p;				/* LIMC */
1837c478bdstevel@tonic-gate	short	a1ul;				/* UPA1 */
1847c478bdstevel@tonic-gate	short	pks1, fa1;			/* UPA2 */
1857c478bdstevel@tonic-gate	char	tr;				/* tone/transition detector */
1867c478bdstevel@tonic-gate	short	thr2;
1877c478bdstevel@tonic-gate
1887c478bdstevel@tonic-gate	mag = dq & 0x3FFF;
1897c478bdstevel@tonic-gate	/* TRANS */
1907c478bdstevel@tonic-gate	if (state_ptr->td == 0) {
1917c478bdstevel@tonic-gate		tr = 0;
1927c478bdstevel@tonic-gate	} else if (state_ptr->yl > 0x40000) {
1937c478bdstevel@tonic-gate		tr = (mag <= 0x2F80) ? 0 : 1;
1947c478bdstevel@tonic-gate	} else {
1957c478bdstevel@tonic-gate		thr2 = (0x20 + ((state_ptr->yl >> 10) & 0x1F)) <<
1967c478bdstevel@tonic-gate		    (state_ptr->yl >> 15);
1977c478bdstevel@tonic-gate		if (mag >= thr2) {
1987c478bdstevel@tonic-gate			tr = 1;
1997c478bdstevel@tonic-gate		} else {
2007c478bdstevel@tonic-gate			tr = (mag <= (thr2 - (thr2 >> 2))) ? 0 : 1;
2017c478bdstevel@tonic-gate		}
2027c478bdstevel@tonic-gate	}
2037c478bdstevel@tonic-gate
2047c478bdstevel@tonic-gate	/*
2057c478bdstevel@tonic-gate	 * Quantizer scale factor adaptation.
2067c478bdstevel@tonic-gate	 */
2077c478bdstevel@tonic-gate
2087c478bdstevel@tonic-gate	/* FUNCTW & FILTD & DELAY */
2097c478bdstevel@tonic-gate	state_ptr->yu = y + ((_witab[i] - y) >> 5);
2107c478bdstevel@tonic-gate
2117c478bdstevel@tonic-gate	/* LIMB */
2127c478bdstevel@tonic-gate	if (state_ptr->yu < 544) {
2137c478bdstevel@tonic-gate		state_ptr->yu = 544;
2147c478bdstevel@tonic-gate	} else if (state_ptr->yu > 5120) {
2157c478bdstevel@tonic-gate		state_ptr->yu = 5120;
2167c478bdstevel@tonic-gate	}
2177c478bdstevel@tonic-gate
2187c478bdstevel@tonic-gate	/* FILTE & DELAY */
2197c478bdstevel@tonic-gate	state_ptr->yl += state_ptr->yu + ((-state_ptr->yl) >> 6);
2207c478bdstevel@tonic-gate
2217c478bdstevel@tonic-gate	/*
2227c478bdstevel@tonic-gate	 * Adaptive predictor.
2237c478bdstevel@tonic-gate	 */
2247c478bdstevel@tonic-gate	if (tr == 1) {
2257c478bdstevel@tonic-gate		state_ptr->a[0] = 0;
2267c478bdstevel@tonic-gate		state_ptr->a[1] = 0;
2277c478bdstevel@tonic-gate		state_ptr->b[0] = 0;
2287c478bdstevel@tonic-gate		state_ptr->b[1] = 0;
2297c478bdstevel@tonic-gate		state_ptr->b[2] = 0;
2307c478bdstevel@tonic-gate		state_ptr->b[3] = 0;
2317c478bdstevel@tonic-gate		state_ptr->b[4] = 0;
2327c478bdstevel@tonic-gate		state_ptr->b[5] = 0;
2337c478bdstevel@tonic-gate	} else {
2347c478bdstevel@tonic-gate
2357c478bdstevel@tonic-gate		/* UPA2 */
2367c478bdstevel@tonic-gate		pks1 = pk0 ^ state_ptr->pk[0];
2377c478bdstevel@tonic-gate
2387c478bdstevel@tonic-gate		a2p = state_ptr->a[1] - (state_ptr->a[1] >> 7);
2397c478bdstevel@tonic-gate		if (sigpk == 0) {
2407c478bdstevel@tonic-gate			fa1 = (pks1) ? state_ptr->a[0] : -state_ptr->a[0];
2417c478bdstevel@tonic-gate			if (fa1 < -8191) {
2427c478bdstevel@tonic-gate				a2p -= 0x100;
2437c478bdstevel@tonic-gate			} else if (fa1 > 8191) {
2447c478bdstevel@tonic-gate				a2p += 0xFF;
2457c478bdstevel@tonic-gate			} else {
2467c478bdstevel@tonic-gate				a2p += fa1 >> 5;
2477c478bdstevel@tonic-gate			}
2487c478bdstevel@tonic-gate
2497c478bdstevel@tonic-gate			if (pk0 ^ state_ptr->pk[1]) {
2507c478bdstevel@tonic-gate				/* LIMC */
2517c478bdstevel@tonic-gate				if (a2p <= -12160) {
2527c478bdstevel@tonic-gate					a2p = -12288;
2537c478bdstevel@tonic-gate				} else if (a2p >= 12416) {
2547c478bdstevel@tonic-gate					a2p = 12288;
2557c478bdstevel@tonic-gate				} else {
2567c478bdstevel@tonic-gate					a2p -= 0x80;
2577c478bdstevel@tonic-gate				}
2587c478bdstevel@tonic-gate			} else if (a2p <= -12416) {
2597c478bdstevel@tonic-gate				a2p = -12288;
2607c478bdstevel@tonic-gate			} else if (a2p >= 12160) {
2617c478bdstevel@tonic-gate				a2p = 12288;
2627c478bdstevel@tonic-gate			} else {
2637c478bdstevel@tonic-gate				a2p += 0x80;
2647c478bdstevel@tonic-gate			}
2657c478bdstevel@tonic-gate		}
2667c478bdstevel@tonic-gate
2677c478bdstevel@tonic-gate		/* TRIGB & DELAY */
2687c478bdstevel@tonic-gate		state_ptr->a[1] = a2p;
2697c478bdstevel@tonic-gate
2707c478bdstevel@tonic-gate		/* UPA1 */
2717c478bdstevel@tonic-gate		state_ptr->a[0] -= state_ptr->a[0] >> 8;
2727c478bdstevel@tonic-gate		if (sigpk == 0) {
2737c478bdstevel@tonic-gate			if (pks1 == 0) {
2747c478bdstevel@tonic-gate				state_ptr->a[0] += 192;
2757c478bdstevel@tonic-gate			} else {
2767c478bdstevel@tonic-gate				state_ptr->a[0] -= 192;
2777c478bdstevel@tonic-gate			}
2787c478bdstevel@tonic-gate		}
2797c478bdstevel@tonic-gate
2807c478bdstevel@tonic-gate		/* LIMD */
2817c478bdstevel@tonic-gate		a1ul = 15360 - a2p;
2827c478bdstevel@tonic-gate		if (state_ptr->a[0] < -a1ul)
2837c478bdstevel@tonic-gate			state_ptr->a[0] = -a1ul;
2847c478bdstevel@tonic-gate		else if (state_ptr->a[0] > a1ul)
2857c478bdstevel@tonic-gate			state_ptr->a[0] = a1ul;
2867c478bdstevel@tonic-gate
2877c478bdstevel@tonic-gate		/* UPB : update of b's */
2887c478bdstevel@tonic-gate		for (cnt = 0; cnt < 6; cnt++) {
2897c478bdstevel@tonic-gate			state_ptr->b[cnt] -= state_ptr->b[cnt] >> 8;
2907c478bdstevel@tonic-gate			if (dq & 0x3FFF) {
2917c478bdstevel@tonic-gate				/* XOR */
2927c478bdstevel@tonic-gate				if ((dq ^ state_ptr->dq[cnt]) >= 0)
2937c478bdstevel@tonic-gate					state_ptr->b[cnt] += 128;
2947c478bdstevel@tonic-gate				else
2957c478bdstevel@tonic-gate					state_ptr->b[cnt] -= 128;
2967c478bdstevel@tonic-gate			}
2977c478bdstevel@tonic-gate		}
2987c478bdstevel@tonic-gate	}
2997c478bdstevel@tonic-gate
3007c478bdstevel@tonic-gate	for (cnt = 5; cnt > 0; cnt--)
3017c478bdstevel@tonic-gate		state_ptr->dq[cnt] = state_ptr->dq[cnt-1];
3027c478bdstevel@tonic-gate	/* FLOAT A */
3037c478bdstevel@tonic-gate	if (mag == 0) {
3047c478bdstevel@tonic-gate		state_ptr->dq[0] = (dq >= 0) ? 0x20 : 0xFC20;
3057c478bdstevel@tonic-gate	} else {
3067c478bdstevel@tonic-gate		exp = _fmultanexp[mag];
3077c478bdstevel@tonic-gate		state_ptr->dq[0] = (dq >= 0) ?
3087c478bdstevel@tonic-gate		    (exp << 6) + ((mag << 6) >> exp) :
3097c478bdstevel@tonic-gate		    (exp << 6) + ((mag << 6) >> exp) - 0x400;
3107c478bdstevel@tonic-gate	}
3117c478bdstevel@tonic-gate
3127c478bdstevel@tonic-gate	state_ptr->sr[1] = state_ptr->sr[0];
3137c478bdstevel@tonic-gate	/* FLOAT B */
3147c478bdstevel@tonic-gate	if (sr == 0) {
3157c478bdstevel@tonic-gate		state_ptr->sr[0] = 0x20;
3167c478bdstevel@tonic-gate	} else if (sr > 0) {
3177c478bdstevel@tonic-gate		exp = _fmultanexp[sr];
3187c478bdstevel@tonic-gate		state_ptr->sr[0] = (exp << 6) + ((sr << 6) >> exp);
3197c478bdstevel@tonic-gate	} else {
3207c478bdstevel@tonic-gate		mag = -sr;
3217c478bdstevel@tonic-gate		exp = _fmultanexp[mag];
3227c478bdstevel@tonic-gate		state_ptr->sr[0] =  (exp << 6) + ((mag << 6) >> exp) - 0x400;
3237c478bdstevel@tonic-gate	}
3247c478bdstevel@tonic-gate
3257c478bdstevel@tonic-gate	/* DELAY A */
3267c478bdstevel@tonic-gate	state_ptr->pk[1] = state_ptr->pk[0];
3277c478bdstevel@tonic-gate	state_ptr->pk[0] = pk0;
3287c478bdstevel@tonic-gate
3297c478bdstevel@tonic-gate	/* TONE */
3307c478bdstevel@tonic-gate	if (tr == 1)
3317c478bdstevel@tonic-gate		state_ptr->td = 0;
3327c478bdstevel@tonic-gate	else if (a2p < -11776)
3337c478bdstevel@tonic-gate		state_ptr->td = 1;
3347c478bdstevel@tonic-gate	else
3357c478bdstevel@tonic-gate		state_ptr->td = 0;
3367c478bdstevel@tonic-gate
3377c478bdstevel@tonic-gate	/*
3387c478bdstevel@tonic-gate	 * Adaptation speed control.
3397c478bdstevel@tonic-gate	 */
3407c478bdstevel@tonic-gate	fi = _fitab[i];						/* FUNCTF */
3417c478bdstevel@tonic-gate	state_ptr->dms += (fi - state_ptr->dms) >> 5;		/* FILTA */
3427c478bdstevel@tonic-gate	state_ptr->dml += (((fi << 2) - state_ptr->dml) >> 7);	/* FILTB */
3437c478bdstevel@tonic-gate
3447c478bdstevel@tonic-gate	if (tr == 1)
3457c478bdstevel@tonic-gate		state_ptr->ap = 256;
3467c478bdstevel@tonic-gate	else if (y < 1536)					/* SUBTC */
3477c478bdstevel@tonic-gate		state_ptr->ap += (0x200 - state_ptr->ap) >> 4;
3487c478bdstevel@tonic-gate	else if (state_ptr->td == 1)
3497c478bdstevel@tonic-gate		state_ptr->ap += (0x200 - state_ptr->ap) >> 4;
3507c478bdstevel@tonic-gate	else if (abs((state_ptr->dms << 2) - state_ptr->dml) >=
3517c478bdstevel@tonic-gate	    (state_ptr->dml >> 3))
3527c478bdstevel@tonic-gate		state_ptr->ap += (0x200 - state_ptr->ap) >> 4;
3537c478bdstevel@tonic-gate	else
3547c478bdstevel@tonic-gate		state_ptr->ap += (-state_ptr->ap) >> 4;
3557c478bdstevel@tonic-gate}
3567c478bdstevel@tonic-gate
3577c478bdstevel@tonic-gate/*
3587c478bdstevel@tonic-gate * _g721_quantize()
3597c478bdstevel@tonic-gate *
3607c478bdstevel@tonic-gate * Description:
3617c478bdstevel@tonic-gate *
3627c478bdstevel@tonic-gate * Given a raw sample, 'd', of the difference signal and a
3637c478bdstevel@tonic-gate * quantization step size scale factor, 'y', this routine returns the
3647c478bdstevel@tonic-gate * G.721 codeword to which that sample gets quantized.  The step
3657c478bdstevel@tonic-gate * size scale factor division operation is done in the log base 2 domain
3667c478bdstevel@tonic-gate * as a subtraction.
3677c478bdstevel@tonic-gate */
3687c478bdstevel@tonic-gatestatic unsigned int
3697c478bdstevel@tonic-gate_g721_quantize(
3707c478bdstevel@tonic-gate	int	d,	/* Raw difference signal sample. */
3717c478bdstevel@tonic-gate	int	y)	/* Step size multiplier. */
3727c478bdstevel@tonic-gate{
3737c478bdstevel@tonic-gate	/* LOG */
3747c478bdstevel@tonic-gate	short	dqm;	/* Magnitude of 'd'. */
3757c478bdstevel@tonic-gate	short	exp;	/* Integer part of base 2 log of magnitude of 'd'. */
3767c478bdstevel@tonic-gate	short	mant;	/* Fractional part of base 2 log. */
3777c478bdstevel@tonic-gate	short	dl;	/* Log of magnitude of 'd'. */
3787c478bdstevel@tonic-gate
3797c478bdstevel@tonic-gate	/* SUBTB */
3807c478bdstevel@tonic-gate	short	dln;	/* Step size scale factor normalized log. */
3817c478bdstevel@tonic-gate
3827c478bdstevel@tonic-gate	/* QUAN */
3837c478bdstevel@tonic-gate	char	i;	/* G.721 codeword. */
3847c478bdstevel@tonic-gate
3857c478bdstevel@tonic-gate	/*
3867c478bdstevel@tonic-gate	 * LOG
3877c478bdstevel@tonic-gate	 *
3887c478bdstevel@tonic-gate	 * Compute base 2 log of 'd', and store in 'dln'.
3897c478bdstevel@tonic-gate	 *
3907c478bdstevel@tonic-gate	 */
3917c478bdstevel@tonic-gate	dqm = abs(d);
3927c478bdstevel@tonic-gate	exp = _fmultanexp[dqm >> 1];
3937c478bdstevel@tonic-gate	mant = ((dqm << 7) >> exp) & 0x7F;	/* Fractional portion. */
3947c478bdstevel@tonic-gate	dl = (exp << 7) + mant;
3957c478bdstevel@tonic-gate
3967c478bdstevel@tonic-gate	/*
3977c478bdstevel@tonic-gate	 * SUBTB
3987c478bdstevel@tonic-gate	 *
3997c478bdstevel@tonic-gate	 * "Divide" by step size multiplier.
4007c478bdstevel@tonic-gate	 */
4017c478bdstevel@tonic-gate	dln = dl - (y >> 2);
4027c478bdstevel@tonic-gate
4037c478bdstevel@tonic-gate	/*
4047c478bdstevel@tonic-gate	 * QUAN
4057c478bdstevel@tonic-gate	 *
4067c478bdstevel@tonic-gate	 * Obtain codword for 'd'.
4077c478bdstevel@tonic-gate	 */
4087c478bdstevel@tonic-gate	i = _quani[dln & 0xFFF];
4097c478bdstevel@tonic-gate	if (d < 0)
4107c478bdstevel@tonic-gate		i ^= 0xF;	/* Stuff in sign of 'd'. */
4117c478bdstevel@tonic-gate	else if (i == 0)
4127c478bdstevel@tonic-gate		i = 0xF;	/* New in 1988 revision */
4137c478bdstevel@tonic-gate
4147c478bdstevel@tonic-gate	return (i);
4157c478bdstevel@tonic-gate}
4167c478bdstevel@tonic-gate
4177c478bdstevel@tonic-gate/*
4187c478bdstevel@tonic-gate * _g721_reconstr()
4197c478bdstevel@tonic-gate *
4207c478bdstevel@tonic-gate * Description:
4217c478bdstevel@tonic-gate *
4227c478bdstevel@tonic-gate * Returns reconstructed difference signal 'dq' obtained from
4237c478bdstevel@tonic-gate * G.721 codeword 'i' and quantization step size scale factor 'y'.
4247c478bdstevel@tonic-gate * Multiplication is performed in log base 2 domain as addition.
4257c478bdstevel@tonic-gate */
4267c478bdstevel@tonic-gatestatic unsigned long
4277c478bdstevel@tonic-gate_g721_reconstr(
4287c478bdstevel@tonic-gate	int		i,	/* G.721 codeword. */
4297c478bdstevel@tonic-gate	unsigned long	y)	/* Step size multiplier. */
4307c478bdstevel@tonic-gate{
4317c478bdstevel@tonic-gate	/* ADD A */
4327c478bdstevel@tonic-gate	short	dql;	/* Log of 'dq' magnitude. */
4337c478bdstevel@tonic-gate
4347c478bdstevel@tonic-gate	/* ANTILOG */
4357c478bdstevel@tonic-gate	short	dex;	/* Integer part of log. */
4367c478bdstevel@tonic-gate	short	dqt;
4377c478bdstevel@tonic-gate	short	dq;	/* Reconstructed difference signal sample. */
4387c478bdstevel@tonic-gate
4397c478bdstevel@tonic-gate	dql = _dqlntab[i] + (y >> 2);	/* ADDA */
4407c478bdstevel@tonic-gate
4417c478bdstevel@tonic-gate	if (dql < 0)
4427c478bdstevel@tonic-gate		dq = 0;
4437c478bdstevel@tonic-gate	else {				/* ANTILOG */
4447c478bdstevel@tonic-gate		dex = (dql >> 7) & 15;
4457c478bdstevel@tonic-gate		dqt = 128 + (dql & 127);
4467c478bdstevel@tonic-gate		dq = (dqt << 7) >> (14 - dex);
4477c478bdstevel@tonic-gate	}
4487c478bdstevel@tonic-gate	if (i & 8)
4497c478bdstevel@tonic-gate		dq -= 0x4000;
4507c478bdstevel@tonic-gate
4517c478bdstevel@tonic-gate	return (dq);
4527c478bdstevel@tonic-gate}
4537c478bdstevel@tonic-gate
4547c478bdstevel@tonic-gate/*
4557c478bdstevel@tonic-gate * _tandem_adjust(sr, se, y, i)
4567c478bdstevel@tonic-gate *
4577c478bdstevel@tonic-gate * Description:
4587c478bdstevel@tonic-gate *
4597c478bdstevel@tonic-gate * At the end of ADPCM decoding, it simulates an encoder which may be receiving
4607c478bdstevel@tonic-gate * the output of this decoder as a tandem process. If the output of the
4617c478bdstevel@tonic-gate * simulated encoder differs from the input to this decoder, the decoder output
4627c478bdstevel@tonic-gate * is adjusted by one level of A-law or u-law codes.
4637c478bdstevel@tonic-gate *
4647c478bdstevel@tonic-gate * Input:
4657c478bdstevel@tonic-gate *	sr	decoder output linear PCM sample,
4667c478bdstevel@tonic-gate *	se	predictor estimate sample,
4677c478bdstevel@tonic-gate *	y	quantizer step size,
4687c478bdstevel@tonic-gate *	i	decoder input code
4697c478bdstevel@tonic-gate *
4707c478bdstevel@tonic-gate * Return:
4717c478bdstevel@tonic-gate *	adjusted A-law or u-law compressed sample.
4727c478bdstevel@tonic-gate */
4737c478bdstevel@tonic-gatestatic int
4747c478bdstevel@tonic-gate_tandem_adjust_alaw(
4757c478bdstevel@tonic-gate	int	sr,	/* decoder output linear PCM sample */
4767c478bdstevel@tonic-gate	int	se,	/* predictor estimate sample */
4777c478bdstevel@tonic-gate	int	y,	/* quantizer step size */
4787c478bdstevel@tonic-gate	int	i)	/* decoder input code */
4797c478bdstevel@tonic-gate{
4807c478bdstevel@tonic-gate	unsigned char	sp;	/* A-law compressed 8-bit code */
4817c478bdstevel@tonic-gate	short	dx;		/* prediction error */
4827c478bdstevel@tonic-gate	char	id;		/* quantized prediction error */
4837c478bdstevel@tonic-gate	int	sd;		/* adjusted A-law decoded sample value */
4847c478bdstevel@tonic-gate	int	im;		/* biased magnitude of i */
4857c478bdstevel@tonic-gate	int	imx;		/* biased magnitude of id */
4867c478bdstevel@tonic-gate
4877c478bdstevel@tonic-gate	sp = audio_s2a((sr <= -0x2000)? -0x8000 :
4887c478bdstevel@tonic-gate	    (sr >= 0x1FFF)? 0x7FFF : sr << 2);	/* short to A-law compression */
4897c478bdstevel@tonic-gate	dx = (audio_a2s(sp) >> 2) - se; 	/* 16-bit prediction error */
4907c478bdstevel@tonic-gate	id = _g721_quantize(dx, y);
4917c478bdstevel@tonic-gate
4927c478bdstevel@tonic-gate	if (id == i)			/* no adjustment on sp */
4937c478bdstevel@tonic-gate		return (sp);
4947c478bdstevel@tonic-gate	else {				/* sp adjustment needed */
4957c478bdstevel@tonic-gate		/* ADPCM codes : 8, 9, ... F, 0, 1, ... , 6, 7 */
4967c478bdstevel@tonic-gate		im = i ^ 8;		/* 2's complement to biased unsigned */
4977c478bdstevel@tonic-gate		imx = id ^ 8;
4987c478bdstevel@tonic-gate
4997c478bdstevel@tonic-gate		if (imx > im) {		/* sp adjusted to next lower value */
5007c478bdstevel@tonic-gate			if (sp & 0x80)
5017c478bdstevel@tonic-gate				sd = (sp == 0xD5)? 0x55 :
5027c478bdstevel@tonic-gate				    ((sp ^ 0x55) - 1) ^ 0x55;
5037c478bdstevel@tonic-gate			else
5047c478bdstevel@tonic-gate				sd = (sp == 0x2A)? 0x2A :
5057c478bdstevel@tonic-gate				    ((sp ^ 0x55) + 1) ^ 0x55;
5067c478bdstevel@tonic-gate		} else {		/* sp adjusted to next higher value */
5077c478bdstevel@tonic-gate			if (sp & 0x80)
5087c478bdstevel@tonic-gate				sd = (sp == 0xAA)? 0xAA :
5097c478bdstevel@tonic-gate				    ((sp ^ 0x55) + 1) ^ 0x55;
5107c478bdstevel@tonic-gate			else
5117c478bdstevel@tonic-gate				sd = (sp == 0x55)? 0xD5 :
5127c478bdstevel@tonic-gate				    ((sp ^ 0x55) - 1) ^ 0x55;
5137c478bdstevel@tonic-gate		}
5147c478bdstevel@tonic-gate		return (sd);
5157c478bdstevel@tonic-gate	}
5167c478bdstevel@tonic-gate}
5177c478bdstevel@tonic-gate
5187c478bdstevel@tonic-gatestatic int
5197c478bdstevel@tonic-gate_tandem_adjust_ulaw(
5207c478bdstevel@tonic-gate	int	sr,	/* decoder output linear PCM sample */
5217c478bdstevel@tonic-gate	int	se,	/* predictor estimate sample */
5227c478bdstevel@tonic-gate	int	y,	/* quantizer step size */
5237c478bdstevel@tonic-gate	int	i)	/* decoder input code */
5247c478bdstevel@tonic-gate{
5257c478bdstevel@tonic-gate	unsigned char   sp;	/* A-law compressed 8-bit code */
5267c478bdstevel@tonic-gate	short	dx;		/* prediction error */
5277c478bdstevel@tonic-gate	char	id;		/* quantized prediction error */
5287c478bdstevel@tonic-gate	int	sd;		/* adjusted A-law decoded sample value */
5297c478bdstevel@tonic-gate	int	im;		/* biased magnitude of i */
5307c478bdstevel@tonic-gate	int	imx;		/* biased magnitude of id */
5317c478bdstevel@tonic-gate
5327c478bdstevel@tonic-gate	sp = audio_s2u((sr <= -0x2000)? -0x8000 :
5337c478bdstevel@tonic-gate	    (sr >= 0x1FFF)? 0x7FFF : sr << 2); /* short to u-law compression */
5347c478bdstevel@tonic-gate	dx = (audio_u2s(sp) >> 2) - se;  /* 16-bit prediction error */
5357c478bdstevel@tonic-gate	id = _g721_quantize(dx, y);
5367c478bdstevel@tonic-gate	if (id == i)
5377c478bdstevel@tonic-gate		return (sp);
5387c478bdstevel@tonic-gate	else {
5397c478bdstevel@tonic-gate		/* ADPCM codes : 8, 9, ... F, 0, 1, ... , 6, 7 */
5407c478bdstevel@tonic-gate		im = i ^ 8;		/* 2's complement to biased unsigned */
5417c478bdstevel@tonic-gate		imx = id ^ 8;
5427c478bdstevel@tonic-gate		if (imx > im) {		/* sp adjusted to next lower value */
5437c478bdstevel@tonic-gate			if (sp & 0x80)
5447c478bdstevel@tonic-gate				sd = (sp == 0xFF)? 0x7F : sp + 1;
5457c478bdstevel@tonic-gate			else
5467c478bdstevel@tonic-gate				sd = (sp == 0)? 0 : sp - 1;
5477c478bdstevel@tonic-gate
5487c478bdstevel@tonic-gate		} else {		/* sp adjusted to next higher value */
5497c478bdstevel@tonic-gate			if (sp & 0x80)
5507c478bdstevel@tonic-gate				sd = (sp == 0x80)? 0x80 : sp - 1;
5517c478bdstevel@tonic-gate			else
5527c478bdstevel@tonic-gate				sd = (sp == 0x7F)? 0xFF : sp + 1;
5537c478bdstevel@tonic-gate		}
5547c478bdstevel@tonic-gate		return (sd);
5557c478bdstevel@tonic-gate	}
5567c478bdstevel@tonic-gate}
5577c478bdstevel@tonic-gate
5587c478bdstevel@tonic-gate/*
5597c478bdstevel@tonic-gate * g721_encode()
5607c478bdstevel@tonic-gate *
5617c478bdstevel@tonic-gate * Description:
5627c478bdstevel@tonic-gate *
5637c478bdstevel@tonic-gate * Encodes a buffer of linear PCM, A-law or u-law data pointed to by
5647c478bdstevel@tonic-gate * 'in_buf' according * the G.721 encoding algorithm and packs the
5657c478bdstevel@tonic-gate * resulting code words into bytes. The bytes of codeword pairs are
5667c478bdstevel@tonic-gate * written to a buffer pointed to by 'out_buf'.
5677c478bdstevel@tonic-gate *
5687c478bdstevel@tonic-gate * Notes:
5697c478bdstevel@tonic-gate *
5707c478bdstevel@tonic-gate * In the event that the total number of codewords which have to be
5717c478bdstevel@tonic-gate * written is odd, the last unpairable codeword is saved in the
5727c478bdstevel@tonic-gate * state structure till the next call. It is then paired off and
5737c478bdstevel@tonic-gate * packed with the first codeword of the new buffer. The number of
5747c478bdstevel@tonic-gate * valid bytes in 'out_buf' is returned in *out_size. Note that
5757c478bdstevel@tonic-gate * *out_size will not always be equal to half * of 'data_size' on input.
5767c478bdstevel@tonic-gate * On the final call to 'g721_encode()' the calling program might want to
5777c478bdstevel@tonic-gate * check if a codeword was left over. This can be
5787c478bdstevel@tonic-gate * done by calling 'g721_encode()' with data_size = 0, which returns in
5797c478bdstevel@tonic-gate * *out_size a 0 if nothing was leftover and 1 if a codeword was leftover
5807c478bdstevel@tonic-gate * which now is in out_buf[0].
5817c478bdstevel@tonic-gate *
5827c478bdstevel@tonic-gate * The 4 lower significant bits of an individual byte in the output byte
5837c478bdstevel@tonic-gate * stream is packed with a G.721 codeword first.  Then the 4 higher order
5847c478bdstevel@tonic-gate * bits are packed with the next codeword.
5857c478bdstevel@tonic-gate */
5867c478bdstevel@tonic-gateint
5877c478bdstevel@tonic-gateg721_encode(
5887c478bdstevel@tonic-gate	void		*in_buf,
5897c478bdstevel@tonic-gate	int		data_size,
5907c478bdstevel@tonic-gate	Audio_hdr	*in_header,
5917c478bdstevel@tonic-gate	unsigned char	*out_buf,
5927c478bdstevel@tonic-gate	int		*out_size,
5937c478bdstevel@tonic-gate	struct audio_g72x_state *state_ptr)
5947c478bdstevel@tonic-gate{
5957c478bdstevel@tonic-gate	short	sl;				/* EXPAND */
5967c478bdstevel@tonic-gate	short	sei, sezi, se, sez;		/* ACCUM */
5977c478bdstevel@tonic-gate	short	d;				/* SUBTA */
5987c478bdstevel@tonic-gate	float	al;		/* use floating point for faster multiply */
5997c478bdstevel@tonic-gate	short	y, dif;				/* MIX */
6007c478bdstevel@tonic-gate	short	sr;				/* ADDB */
6017c478bdstevel@tonic-gate	short	pk0, sigpk, dqsez;		/* ADDC */
6027c478bdstevel@tonic-gate	short	dq, i;
6037c478bdstevel@tonic-gate	int	cnt, cnta;
6047c478bdstevel@tonic-gate	int	out_leng;
6057c478bdstevel@tonic-gate	unsigned char *char_in;
6067c478bdstevel@tonic-gate	unsigned char *char_out;
6077c478bdstevel@tonic-gate	short	*short_ptr;
6087c478bdstevel@tonic-gate
6097c478bdstevel@tonic-gate	if (data_size == 0) {
6107c478bdstevel@tonic-gate		/* Actually, the leftover count will never be more than 4 */
6117c478bdstevel@tonic-gate		for (i = 0; state_ptr->leftover_cnt > 0; i++) {
6127c478bdstevel@tonic-gate			*out_buf++ = state_ptr->leftover[i];
6137c478bdstevel@tonic-gate			state_ptr->leftover_cnt -= 8;
6147c478bdstevel@tonic-gate		}
6157c478bdstevel@tonic-gate		*out_size = i;
6167c478bdstevel@tonic-gate		state_ptr->leftover_cnt = 0;
6177c478bdstevel@tonic-gate		return (AUDIO_SUCCESS);
6187c478bdstevel@tonic-gate	}
6197c478bdstevel@tonic-gate
6207c478bdstevel@tonic-gate	/* XXX - if linear, it had better be 16-bit! */
6217c478bdstevel@tonic-gate	if (in_header->encoding == AUDIO_ENCODING_LINEAR) {
6227c478bdstevel@tonic-gate		if (data_size & 1) {
6237c478bdstevel@tonic-gate			return (AUDIO_ERR_BADFRAME);
6247c478bdstevel@tonic-gate		} else {
6257c478bdstevel@tonic-gate			data_size >>= 1;	/* divide to get sample cnt */
6267c478bdstevel@tonic-gate			short_ptr = (short *)in_buf;
6277c478bdstevel@tonic-gate		}
6287c478bdstevel@tonic-gate	} else {
6297c478bdstevel@tonic-gate		char_in = (unsigned char *)in_buf;
6307c478bdstevel@tonic-gate	}
6317c478bdstevel@tonic-gate	char_out = (unsigned char *)out_buf;
6327c478bdstevel@tonic-gate	if (state_ptr->leftover_cnt > 0) {
6337c478bdstevel@tonic-gate		*char_out = state_ptr->leftover[0];
6347c478bdstevel@tonic-gate		state_ptr->leftover_cnt = 0;
6357c478bdstevel@tonic-gate		data_size += 1;
6367c478bdstevel@tonic-gate		cnta = 1;
6377c478bdstevel@tonic-gate	} else {
6387c478bdstevel@tonic-gate		cnta = 0;
6397c478bdstevel@tonic-gate	}
6407c478bdstevel@tonic-gate	out_leng = (data_size & ~0x01);		/* clear low order bit */
6417c478bdstevel@tonic-gate	for (; cnta < data_size; cnta++) {
6427c478bdstevel@tonic-gate		/*  EXPAND  */
6437c478bdstevel@tonic-gate		switch (in_header->encoding) {
6447c478bdstevel@tonic-gate		case AUDIO_ENCODING_LINEAR:
6457c478bdstevel@tonic-gate			sl = *short_ptr++ >> 2;
6467c478bdstevel@tonic-gate			break;
6477c478bdstevel@tonic-gate		case AUDIO_ENCODING_ALAW:
6487c478bdstevel@tonic-gate			sl = audio_a2s(*char_in++) >> 2;
6497c478bdstevel@tonic-gate			break;
6507c478bdstevel@tonic-gate		case AUDIO_ENCODING_ULAW:
6517c478bdstevel@tonic-gate			sl = audio_u2s(*char_in++) >> 2; /* u-law to short */
6527c478bdstevel@tonic-gate			break;
6537c478bdstevel@tonic-gate		default:
6547c478bdstevel@tonic-gate			return (AUDIO_ERR_ENCODING);
6557c478bdstevel@tonic-gate		}
6567c478bdstevel@tonic-gate
6577c478bdstevel@tonic-gate		/* ACCUM */
6587c478bdstevel@tonic-gate		sezi = _g721_fmult(state_ptr->b[0] >> 2, state_ptr->dq[0]);
6597c478bdstevel@tonic-gate		for (cnt = 1; cnt < 6; cnt++)
6607c478bdstevel@tonic-gate			sezi = sezi + _g721_fmult(state_ptr->b[cnt] >> 2,
6617c478bdstevel@tonic-gate			    state_ptr->dq[cnt]);
6627c478bdstevel@tonic-gate		sei = sezi;
6637c478bdstevel@tonic-gate		for (cnt = 1; cnt > -1; cnt--)
6647c478bdstevel@tonic-gate			sei = sei + _g721_fmult(state_ptr->a[cnt] >> 2,
6657c478bdstevel@tonic-gate			    state_ptr->sr[cnt]);
6667c478bdstevel@tonic-gate		sez = sezi >> 1;
6677c478bdstevel@tonic-gate		se = sei >> 1;
6687c478bdstevel@tonic-gate		d = sl - se;				/* SUBTA */
6697c478bdstevel@tonic-gate
6707c478bdstevel@tonic-gate		if (state_ptr->ap >= 256)
6717c478bdstevel@tonic-gate			y = state_ptr->yu;
6727c478bdstevel@tonic-gate		else {
6737c478bdstevel@tonic-gate			y = state_ptr->yl >> 6;
6747c478bdstevel@tonic-gate			dif = state_ptr->yu - y;
6757c478bdstevel@tonic-gate			al = state_ptr->ap >> 2;
6767c478bdstevel@tonic-gate			if (dif > 0)
6777c478bdstevel@tonic-gate				y += ((int)(dif * al)) >> 6;
6787c478bdstevel@tonic-gate			else if (dif < 0)
6797c478bdstevel@tonic-gate				y += ((int)(dif * al) + 0x3F) >> 6;
6807c478bdstevel@tonic-gate		}
6817c478bdstevel@tonic-gate
6827c478bdstevel@tonic-gate		i = _g721_quantize(d, y);
6837c478bdstevel@tonic-gate		dq = _g721_reconstr(i, y);
6847c478bdstevel@tonic-gate		/* ADDB */
6857c478bdstevel@tonic-gate		sr = (dq < 0) ? se - (dq & 0x3FFF) : se + dq;
6867c478bdstevel@tonic-gate
6877c478bdstevel@tonic-gate		if (cnta & 1) {
6887c478bdstevel@tonic-gate			*char_out++ += i << 4;
6897c478bdstevel@tonic-gate		} else if (cnta < out_leng) {
6907c478bdstevel@tonic-gate			*char_out = i;
6917c478bdstevel@tonic-gate		} else {
6927c478bdstevel@tonic-gate			/*
6937c478bdstevel@tonic-gate			 * save the last codeword which can not be paired into
6947c478bdstevel@tonic-gate			 * a byte in the state stucture and set leftover_flag.
6957c478bdstevel@tonic-gate			 */
6967c478bdstevel@tonic-gate			state_ptr->leftover[0] = i;
6977c478bdstevel@tonic-gate			state_ptr->leftover_cnt = 4;
6987c478bdstevel@tonic-gate		}
6997c478bdstevel@tonic-gate
7007c478bdstevel@tonic-gate		dqsez = sr + sez - se;		/* ADDC */
7017c478bdstevel@tonic-gate		if (dqsez == 0) {
7027c478bdstevel@tonic-gate			pk0 = 0;
7037c478bdstevel@tonic-gate			sigpk = 1;
7047c478bdstevel@tonic-gate		} else {
7057c478bdstevel@tonic-gate			pk0 = (dqsez < 0) ? 1 : 0;
7067c478bdstevel@tonic-gate			sigpk = 0;
7077c478bdstevel@tonic-gate		}
7087c478bdstevel@tonic-gate
7097c478bdstevel@tonic-gate		_g721_update(y, i, dq, sr, pk0, state_ptr, sigpk);
7107c478bdstevel@tonic-gate	}
7117c478bdstevel@tonic-gate	*out_size = cnta >> 1;
7127c478bdstevel@tonic-gate
7137c478bdstevel@tonic-gate	return (AUDIO_SUCCESS);
7147c478bdstevel@tonic-gate}
7157c478bdstevel@tonic-gate
7167c478bdstevel@tonic-gate/*
7177c478bdstevel@tonic-gate * g721_decode()
7187c478bdstevel@tonic-gate *
7197c478bdstevel@tonic-gate * Description:
7207c478bdstevel@tonic-gate *
7217c478bdstevel@tonic-gate * Decodes a buffer of G.721 encoded data pointed to by 'in_buf' and
7227c478bdstevel@tonic-gate * writes the resulting linear PCM, A-law or Mu-law bytes into a buffer
7237c478bdstevel@tonic-gate * pointed to by 'out_buf'.
7247c478bdstevel@tonic-gate */
7257c478bdstevel@tonic-gateint
7267c478bdstevel@tonic-gateg721_decode(
7277c478bdstevel@tonic-gate	unsigned char	*in_buf,	/* Buffer of g721 encoded data. */
7287c478bdstevel@tonic-gate	int		data_size,	/* Size in bytes of in_buf. */
7297c478bdstevel@tonic-gate	Audio_hdr	*out_header,
7307c478bdstevel@tonic-gate	void		*out_buf,	/* Decoded data buffer. */
7317c478bdstevel@tonic-gate	int		*out_size,
7327c478bdstevel@tonic-gate	struct audio_g72x_state *state_ptr) /* the decoder's state structure. */
7337c478bdstevel@tonic-gate{
7347c478bdstevel@tonic-gate	short	sezi, sei, sez, se;		/* ACCUM */
7357c478bdstevel@tonic-gate	float	al;		/* use floating point for faster multiply */
7367c478bdstevel@tonic-gate	short	y, dif;				/* MIX */
7377c478bdstevel@tonic-gate	short sr;				/* ADDB */
7387c478bdstevel@tonic-gate	char	pk0, i;				/* ADDC */
7397c478bdstevel@tonic-gate	short	dq;
7407c478bdstevel@tonic-gate	char	sigpk;
7417c478bdstevel@tonic-gate	short	dqsez;
7427c478bdstevel@tonic-gate	unsigned char *char_in;
7437c478bdstevel@tonic-gate	unsigned char *char_out;
7447c478bdstevel@tonic-gate	int	cnt, cnta;
7457c478bdstevel@tonic-gate	short	*linear_out;
7467c478bdstevel@tonic-gate
7477c478bdstevel@tonic-gate	*out_size = data_size << 1;
7487c478bdstevel@tonic-gate	char_in = (unsigned char *)in_buf;
7497c478bdstevel@tonic-gate	char_out = (unsigned char *)out_buf;
7507c478bdstevel@tonic-gate	linear_out = (short *)out_buf;
7517c478bdstevel@tonic-gate	for (cnta = 0; cnta < *out_size; cnta++) {
7527c478bdstevel@tonic-gate		if (cnta & 1)
7537c478bdstevel@tonic-gate			i = *char_in++ >> 4;
7547c478bdstevel@tonic-gate		else
7557c478bdstevel@tonic-gate			i = *char_in & 0xF;
7567c478bdstevel@tonic-gate		/* ACCUM */
7577c478bdstevel@tonic-gate		sezi = _g721_fmult(state_ptr->b[0] >> 2, state_ptr->dq[0]);
7587c478bdstevel@tonic-gate		for (cnt = 1; cnt < 6; cnt++)
7597c478bdstevel@tonic-gate			sezi = sezi + _g721_fmult(state_ptr->b[cnt] >> 2,
7607c478bdstevel@tonic-gate			    state_ptr->dq[cnt]);
7617c478bdstevel@tonic-gate		sei = sezi;
7627c478bdstevel@tonic-gate		for (cnt = 1; cnt >= 0; cnt--)
7637c478bdstevel@tonic-gate			sei = sei + _g721_fmult(state_ptr->a[cnt] >> 2,
7647c478bdstevel@tonic-gate			    state_ptr->sr[cnt]);
7657c478bdstevel@tonic-gate
7667c478bdstevel@tonic-gate		sez = sezi >> 1;
7677c478bdstevel@tonic-gate		se = sei >> 1;
7687c478bdstevel@tonic-gate		if (state_ptr->ap >= 256)
7697c478bdstevel@tonic-gate			y = state_ptr->yu;
7707c478bdstevel@tonic-gate		else {
7717c478bdstevel@tonic-gate			y = state_ptr->yl >> 6;
7727c478bdstevel@tonic-gate			dif = state_ptr->yu - y;
7737c478bdstevel@tonic-gate			al = state_ptr->ap >> 2;
7747c478bdstevel@tonic-gate			if (dif > 0)
7757c478bdstevel@tonic-gate				y += ((int)(dif * al)) >> 6;
7767c478bdstevel@tonic-gate			else if (dif < 0)
7777c478bdstevel@tonic-gate				y += ((int)(dif * al) + 0x3F) >> 6;
7787c478bdstevel@tonic-gate		}
7797c478bdstevel@tonic-gate
7807c478bdstevel@tonic-gate		dq = _g721_reconstr(i, y);
7817c478bdstevel@tonic-gate		/* ADDB */
7827c478bdstevel@tonic-gate		if (dq < 0)
7837c478bdstevel@tonic-gate			sr = se - (dq & 0x3FFF);
7847c478bdstevel@tonic-gate		else
7857c478bdstevel@tonic-gate			sr = se + dq;
7867c478bdstevel@tonic-gate
7877c478bdstevel@tonic-gate		switch (out_header->encoding) {
7887c478bdstevel@tonic-gate		case AUDIO_ENCODING_LINEAR:
7897c478bdstevel@tonic-gate			*linear_out++ = ((sr <= -0x2000) ? -0x8000 :
7907c478bdstevel@tonic-gate			    (sr >= 0x1FFF) ? 0x7FFF : sr << 2);
7917c478bdstevel@tonic-gate			break;
7927c478bdstevel@tonic-gate		case AUDIO_ENCODING_ALAW:
7937c478bdstevel@tonic-gate			*char_out++ = _tandem_adjust_alaw(sr, se, y, i);
7947c478bdstevel@tonic-gate			break;
7957c478bdstevel@tonic-gate		case AUDIO_ENCODING_ULAW:
7967c478bdstevel@tonic-gate			*char_out++ = _tandem_adjust_ulaw(sr, se, y, i);
7977c478bdstevel@tonic-gate			break;
7987c478bdstevel@tonic-gate		default:
7997c478bdstevel@tonic-gate			return (AUDIO_ERR_ENCODING);
8007c478bdstevel@tonic-gate		}
8017c478bdstevel@tonic-gate		/* ADDC */
8027c478bdstevel@tonic-gate		dqsez = sr - se + sez;
8037c478bdstevel@tonic-gate		pk0 = (dqsez < 0) ? 1 : 0;
8047c478bdstevel@tonic-gate		sigpk = (dqsez) ? 0 : 1;
8057c478bdstevel@tonic-gate
8067c478bdstevel@tonic-gate		_g721_update(y, i, dq, sr, pk0, state_ptr, sigpk);
8077c478bdstevel@tonic-gate	}
8087c478bdstevel@tonic-gate	*out_size = cnta;
8097c478bdstevel@tonic-gate
8107c478bdstevel@tonic-gate	return (AUDIO_SUCCESS);
8117c478bdstevel@tonic-gate}
812