src/GSM610/add.c

   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