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