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