libs/libsndfile/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 #include        "gsm.h"
  16
  17 #define saturate(x)     \
  18         ((x) < MIN_WORD ? MIN_WORD : (x) > MAX_WORD ? MAX_WORD: (x))
  19
  20 word gsm_add ( word a, word b)
  21 {
  22         longword sum = (longword)a + (longword)b;
  23         return saturate(sum);
  24 }
  25
  26 word gsm_sub ( word a, word b)
  27 {
  28         longword diff = (longword)a - (longword)b;
  29         return saturate(diff);
  30 }
  31
  32 word gsm_mult ( word a, word b)
  33 {
  34         if (a == MIN_WORD && b == MIN_WORD)
  35                 return MAX_WORD;
  36
  37         return SASR_L( (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_asr (word a, int n)
 161 {
 162         if (n >= 16) return -(a < 0);
 163         if (n <= -16) return 0;
 164         if (n < 0) return a << -n;
 165
 166         return SASR_W (a, (word) n);
 167 }
 168
 169 word gsm_asl (word a, int n)
 170 {
 171         if (n >= 16) return 0;
 172         if (n <= -16) return -(a < 0);
 173         if (n < 0) return gsm_asr(a, -n);
 174         return a << n;
 175 }
 176
 177 longword gsm_L_asr (longword a, int n)
 178 {
 179         if (n >= 32) return -(a < 0);
 180         if (n <= -32) return 0;
 181         if (n < 0) return a << -n;
 182
 183         return SASR_L (a, (word) n);
 184 }
 185
 186 /*
 187 **      word gsm_asr (word a, int n)
 188 **      {
 189 **              if (n >= 16) return -(a < 0);
 190 **              if (n <= -16) return 0;
 191 **              if (n < 0) return a << -n;
 192 **
 193 **      #       ifdef   SASR_W
 194 **                      return a >> n;
 195 **      #       else
 196 **                      if (a >= 0) return a >> n;
 197 **                      else return -(word)( -(uword)a >> n );
 198 **      #       endif
 199 **      }
 200 **
 201 */
 202 /*
 203  *  (From p. 46, end of section 4.2.5)
 204  *
 205  *  NOTE: The following lines gives [sic] one correct implementation
 206  *        of the div(num, denum) arithmetic operation.  Compute div
 207  *        which is the integer division of num by denum: with denum
 208  *        >= num > 0
 209  */
 210
 211 word gsm_div (word num, word denum)
 212 {
 213         longword        L_num   = num;
 214         longword        L_denum = denum;
 215         word            div     = 0;
 216         int             k       = 15;
 217
 218         /* The parameter num sometimes becomes zero.
 219          * Although this is explicitly guarded against in 4.2.5,
 220          * we assume that the result should then be zero as well.
 221          */
 222
 223         /* assert(num != 0); */
 224
 225         assert(num >= 0 && denum >= num);
 226         if (num == 0)
 227             return 0;
 228
 229         while (k--) {
 230                 div   <<= 1;
 231                 L_num <<= 1;
 232
 233                 if (L_num >= L_denum) {
 234                         L_num -= L_denum;
 235                         div++;
 236                 }
 237         }
 238
 239         return div;
 240 }
 241 /*
 242 ** Do not edit or modify anything in this comment block.
 243 ** The arch-tag line is a file identity tag for the GNU Arch
 244 ** revision control system.
 245 **
 246 ** arch-tag: a7398579-e2e1-4733-aa2d-4c6efc0c58ff
 247 */
 248