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