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