git.gag.com Git - fw/altos/blob - src/core/ao_int64.c

   1 /*
   2  * Copyright © 2013 Keith Packard <keithp@keithp.com>
   3  *
   4  * This program is free software; you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation; version 2 of the License.
   7  *
   8  * This program is distributed in the hope that it will be useful, but
   9  * WITHOUT ANY WARRANTY; without even the implied warranty of
  10  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  11  * General Public License for more details.
  12  *
  13  * You should have received a copy of the GNU General Public License along
  14  * with this program; if not, write to the Free Software Foundation, Inc.,
  15  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
  16  */
  17
  18 #include <ao_int64.h>
  19
  20 void ao_plus64(ao_int64_t *r, ao_int64_t *a, ao_int64_t *b) {
  21         uint32_t        t;
  22
  23         r->low = t = a->low + b->low;
  24         r->high = a->high + b->high;
  25         if (t < a->low)
  26                 r->high++;
  27 }
  28
  29 void ao_rshift64(ao_int64_t *r, ao_int64_t *a, uint8_t d) {
  30         if (d < 32) {
  31                 r->high = (int32_t) a->high >> d;
  32                 r->low = a->low >> d;
  33                 if (d)
  34                         r->low |= a->high << (32 - d);
  35         } else {
  36                 d &= 0x1f;
  37                 r->high = 0;
  38                 r->low = (int32_t) a->high >> d;
  39         }
  40 }
  41
  42 void ao_lshift64(ao_int64_t *r, ao_int64_t *a, uint8_t d) {
  43         if (d < 32) {
  44                 r->high = a->high << d;
  45                 r->low = a->low << d;
  46                 if (d)
  47                         r->high |= a->low >> (32 - d);
  48         } else {
  49                 d &= 0x1f;
  50                 r->low = 0;
  51                 r->high = a->low << d;
  52         }
  53 }
  54
  55 void ao_umul64(ao_int64_t *r, uint32_t a, uint32_t b)
  56 {
  57         uint32_t        r1;
  58         uint32_t        r2, r3, r4;
  59         ao_int64_t      s,t,u,v;
  60         r1 = (uint32_t) (uint16_t) a * (uint16_t) b;
  61         r2 = (uint32_t) (uint16_t) (a >> 16) * (uint16_t) b;
  62         r3 = (uint32_t) (uint16_t) a * (uint16_t) (b >> 16);
  63         r4 = (uint32_t) (uint16_t) (a >> 16) * (uint16_t) (b >> 16);
  64
  65         s.low = r1;
  66         s.high = r4;
  67
  68         t.high = (uint32_t) r2 >> 16;
  69         t.low = r2 << 16;
  70         ao_plus64(&u, &s, &t);
  71
  72         v.high = (int32_t) r3 >> 16;
  73         v.low = r3 << 16;
  74         ao_plus64(r, &u, &v);
  75 }
  76
  77 void ao_neg64(ao_int64_t *r, ao_int64_t *a) {
  78         r->high = ~a->high;
  79         r->low = ~a->low;
  80         if (!++r->low)
  81                 r->high++;
  82 }
  83
  84 void ao_mul64(ao_int64_t *r, int32_t a, int32_t b) {
  85         uint8_t         negative = 0;
  86
  87         if (a < 0) {
  88                 a = -a;
  89                 negative = 1;
  90         }
  91         if (b < 0) {
  92                 b = -b;
  93                 negative = !negative;
  94         }
  95         ao_umul64(r, a, b);
  96         if (negative)
  97                 ao_neg64(r, r);
  98 }
  99
 100 void ao_umul64_16(ao_int64_t *r, ao_int64_t *a, uint16_t b) {
 101         uint32_t        low = a->low;
 102         ao_umul64(r, (uint32_t) low >> 1, (uint32_t) b << 1);
 103         if (low & 1) {
 104                 if ((uint32_t) (r->low += b) < (uint32_t) b)
 105                         r->high++;
 106         }
 107         r->high += a->high * b;
 108 }
 109
 110 void ao_mul64_16(ao_int64_t *r, ao_int64_t *a, uint16_t b) {
 111         if ((int32_t) a->high < 0) {
 112                 ao_int64_t      t;
 113
 114                 ao_neg64(&t, a);
 115                 ao_umul64_16(r, &t, b);
 116                 ao_neg64(r, r);
 117         } else
 118                 ao_umul64_16(r, a, b);
 119 }