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
gsm_add(int16_t a,int16_t b)19 int16_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
gsm_sub(int16_t a,int16_t b)25 int16_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
gsm_mult(int16_t a,int16_t b)31 int16_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
gsm_mult_r(int16_t a,int16_t b)39 int16_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
gsm_abs(int16_t a)50 int16_t gsm_abs (int16_t a)
51 {
52 return a < 0 ? (a == MIN_WORD ? MAX_WORD : -a) : a ;
53 }
54
gsm_L_mult(int16_t a,int16_t b)55 int32_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
gsm_L_add(int32_t a,int32_t b)61 int32_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
gsm_L_sub(int32_t a,int32_t b)79 int32_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
99 static 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
gsm_norm(int32_t a)118 int16_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
gsm_L_asl(int32_t a,int n)154 int32_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
gsm_asr(int16_t a,int n)162 int16_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
gsm_asl(int16_t a,int n)171 int16_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
gsm_L_asr(int32_t a,int n)179 int32_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
gsm_div(int16_t num,int16_t denum)213 int16_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