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