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 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
  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
  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
  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
  50 int16_t gsm_abs (int16_t a)
  51 {
  52         return a < 0 ? (a == MIN_WORD ? MAX_WORD : -a) : a ;
  53 }
  54
  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
  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
  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
 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
 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
 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
 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
 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
 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