xref: /titanic_44/usr/src/cmd/audio/utilities/g721.c (revision 7c478bd95313f5f23a4c958a745db2134aa03244)
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  */
59 static 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. */
63 static 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  */
71 static 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  */
83 void
g721_init_state(struct audio_g72x_state * state_ptr)84 g721_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  */
114 static int
_g721_fmult(int an,int srn)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  */
169 static void
_g721_update(int y,int i,int dq,int sr,int pk0,struct audio_g72x_state * state_ptr,int sigpk)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  */
368 static unsigned int
_g721_quantize(int d,int y)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  */
426 static unsigned long
_g721_reconstr(int i,unsigned long y)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  */
473 static int
_tandem_adjust_alaw(int sr,int se,int y,int i)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 
518 static int
_tandem_adjust_ulaw(int sr,int se,int y,int i)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  */
586 int
g721_encode(void * in_buf,int data_size,Audio_hdr * in_header,unsigned char * out_buf,int * out_size,struct audio_g72x_state * state_ptr)587 g721_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  */
725 int
g721_decode(unsigned char * in_buf,int data_size,Audio_hdr * out_header,void * out_buf,int * out_size,struct audio_g72x_state * state_ptr)726 g721_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