1/*
2 * Copyright 1992 by Jutta Degener and Carsten Bormann, Technische
3 * Universitaet Berlin.  See the accompanying file "COPYRIGHT" for
4 * details.  THERE IS ABSOLUTELY NO WARRANTY FOR THIS SOFTWARE.
5 */
6
7/* $Header: /tmp_amd/presto/export/kbs/jutta/src/gsm/RCS/add.c,v 1.6 1996/07/02 09:57:33 jutta Exp $ */
8
9/*
10 *  See private.h for the more commonly used macro versions.
11 */
12
13#include	<stdio.h>
14#include	<assert.h>
15
16#include	"private.h"
17#include	"gsm.h"
18#include	"proto.h"
19
20#define	saturate(x) 	\
21	((x) < MIN_WORD ? MIN_WORD : (x) > MAX_WORD ? MAX_WORD: (x))
22
23word gsm_add P2((a,b), word a, word b)
24{
25	longword sum = (longword)a + (longword)b;
26	return saturate(sum);
27}
28
29word gsm_sub P2((a,b), word a, word b)
30{
31	longword diff = (longword)a - (longword)b;
32	return saturate(diff);
33}
34
35word gsm_mult P2((a,b), word a, word b)
36{
37	if (a == MIN_WORD && b == MIN_WORD) return MAX_WORD;
38	else return SASR( (longword)a * (longword)b, 15 );
39}
40
41word gsm_mult_r P2((a,b), word a, word b)
42{
43	if (b == MIN_WORD && a == MIN_WORD) return MAX_WORD;
44	else {
45		longword prod = (longword)a * (longword)b + 16384;
46		prod >>= 15;
47		return prod & 0xFFFF;
48	}
49}
50
51word gsm_abs P1((a), word a)
52{
53	return a < 0 ? (a == MIN_WORD ? MAX_WORD : -a) : a;
54}
55
56longword gsm_L_mult P2((a,b),word a, word b)
57{
58	assert( a != MIN_WORD || b != MIN_WORD );
59	return ((longword)a * (longword)b) << 1;
60}
61
62longword gsm_L_add P2((a,b), longword a, longword b)
63{
64	if (a < 0) {
65		if (b >= 0) return a + b;
66		else {
67			ulongword A = (ulongword)-(a + 1) + (ulongword)-(b + 1);
68			return A >= MAX_LONGWORD ? MIN_LONGWORD :-(longword)A-2;
69		}
70	}
71	else if (b <= 0) return a + b;
72	else {
73		ulongword A = (ulongword)a + (ulongword)b;
74		return A > MAX_LONGWORD ? MAX_LONGWORD : A;
75	}
76}
77
78longword gsm_L_sub P2((a,b), longword a, longword b)
79{
80	if (a >= 0) {
81		if (b >= 0) return a - b;
82		else {
83			/* a>=0, b<0 */
84
85			ulongword A = (ulongword)a + -(b + 1);
86			return A >= MAX_LONGWORD ? MAX_LONGWORD : (A + 1);
87		}
88	}
89	else if (b <= 0) return a - b;
90	else {
91		/* a<0, b>0 */
92
93		ulongword A = (ulongword)-(a + 1) + b;
94		return A >= MAX_LONGWORD ? MIN_LONGWORD : -(longword)A - 1;
95	}
96}
97
98static unsigned char const bitoff[ 256 ] = {
99	 8, 7, 6, 6, 5, 5, 5, 5, 4, 4, 4, 4, 4, 4, 4, 4,
100	 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3,
101	 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2,
102	 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2,
103	 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
104	 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
105	 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
106	 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
107	 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
108	 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
109	 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
110	 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
111	 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
112	 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
113	 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
114	 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0
115};
116
117word gsm_norm P1((a), longword a )
118/*
119 * the number of left shifts needed to normalize the 32 bit
120 * variable L_var1 for positive values on the interval
121 *
122 * with minimum of
123 * minimum of 1073741824  (01000000000000000000000000000000) and
124 * maximum of 2147483647  (01111111111111111111111111111111)
125 *
126 *
127 * and for negative values on the interval with
128 * minimum of -2147483648 (-10000000000000000000000000000000) and
129 * maximum of -1073741824 ( -1000000000000000000000000000000).
130 *
131 * in order to normalize the result, the following
132 * operation must be done: L_norm_var1 = L_var1 << norm( L_var1 );
133 *
134 * (That's 'ffs', only from the left, not the right..)
135 */
136{
137	assert(a != 0);
138
139	if (a < 0) {
140		if (a <= -1073741824) return 0;
141		a = ~a;
142	}
143
144	return    a & 0xffff0000
145		? ( a & 0xff000000
146		  ?  -1 + bitoff[ 0xFF & (a >> 24) ]
147		  :   7 + bitoff[ 0xFF & (a >> 16) ] )
148		: ( a & 0xff00
149		  ?  15 + bitoff[ 0xFF & (a >> 8) ]
150		  :  23 + bitoff[ 0xFF & a ] );
151}
152
153longword gsm_L_asl P2((a,n), longword a, int n)
154{
155	if (n >= 32) return 0;
156	if (n <= -32) return -(a < 0);
157	if (n < 0) return gsm_L_asr(a, -n);
158	return a << n;
159}
160
161word gsm_asl P2((a,n), word a, int n)
162{
163	if (n >= 16) return 0;
164	if (n <= -16) return -(a < 0);
165	if (n < 0) return gsm_asr(a, -n);
166	return a << n;
167}
168
169longword gsm_L_asr P2((a,n), longword a, int n)
170{
171	if (n >= 32) return -(a < 0);
172	if (n <= -32) return 0;
173	if (n < 0) return a << -n;
174
175#	ifdef	SASR
176		return a >> n;
177#	else
178		if (a >= 0) return a >> n;
179		else return -(longword)( -(ulongword)a >> n );
180#	endif
181}
182
183word gsm_asr P2((a,n), word a, int n)
184{
185	if (n >= 16) return -(a < 0);
186	if (n <= -16) return 0;
187	if (n < 0) return a << -n;
188
189#	ifdef	SASR
190		return a >> n;
191#	else
192		if (a >= 0) return a >> n;
193		else return -(word)( -(uword)a >> n );
194#	endif
195}
196
197/*
198 *  (From p. 46, end of section 4.2.5)
199 *
200 *  NOTE: The following lines gives [sic] one correct implementation
201 *  	  of the div(num, denum) arithmetic operation.  Compute div
202 *        which is the integer division of num by denum: with denum
203 *	  >= num > 0
204 */
205
206word gsm_div P2((num,denum), word num, word denum)
207{
208	longword	L_num   = num;
209	longword	L_denum = denum;
210	word		div 	= 0;
211	int		k 	= 15;
212
213	/* The parameter num sometimes becomes zero.
214	 * Although this is explicitly guarded against in 4.2.5,
215	 * we assume that the result should then be zero as well.
216	 */
217
218	/* assert(num != 0); */
219
220	assert(num >= 0 && denum >= num);
221	if (num == 0)
222	    return 0;
223
224	while (k--) {
225		div   <<= 1;
226		L_num <<= 1;
227
228		if (L_num >= L_denum) {
229			L_num -= L_denum;
230			div++;
231		}
232	}
233
234	return div;
235}
236