libgsm/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 /* $Header: /cvsroot/sox/sox/libgsm/add.c,v 1.1 2007/09/06 16:50:55 cbagwell Exp $ */
   8
   9 /*
  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)
  29 {
  30         longword diff = (longword)a - (longword)b;
  31         return saturate(diff);
  32 }
  33
  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;
  43         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 {
  63         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
 175                 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 {
 184         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
 196 /*
 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 }