kernel/lib/cordic.c

   1 /*
   2  * Copyright (c) 2011 Broadcom Corporation
   3  *
   4  * Permission to use, copy, modify, and/or distribute this software for any
   5  * purpose with or without fee is hereby granted, provided that the above
   6  * copyright notice and this permission notice appear in all copies.
   7  *
   8  * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
   9  * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
  10  * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY
  11  * SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
  12  * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION
  13  * OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN
  14  * CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
  15  */
  16 #include <linux/module.h>
  17 #include <linux/cordic.h>
  18
  19 #define CORDIC_ANGLE_GEN        39797
  20 #define CORDIC_PRECISION_SHIFT  16
  21 #define CORDIC_NUM_ITER         (CORDIC_PRECISION_SHIFT + 2)
  22
  23 #define FIXED(X)        ((s32)((X) << CORDIC_PRECISION_SHIFT))
  24 #define FLOAT(X)        (((X) >= 0) \
  25                 ? ((((X) >> (CORDIC_PRECISION_SHIFT - 1)) + 1) >> 1) \
  26                 : -((((-(X)) >> (CORDIC_PRECISION_SHIFT - 1)) + 1) >> 1))
  27
  28 static const s32 arctan_table[] = {
  29         2949120,
  30         1740967,
  31         919879,
  32         466945,
  33         234379,
  34         117304,
  35         58666,
  36         29335,
  37         14668,
  38         7334,
  39         3667,
  40         1833,
  41         917,
  42         458,
  43         229,
  44         115,
  45         57,
  46         29
  47 };
  48
  49 /*
  50  * cordic_calc_iq() - calculates the i/q coordinate for given angle
  51  *
  52  * theta: angle in degrees for which i/q coordinate is to be calculated
  53  * coord: function output parameter holding the i/q coordinate
  54  */
  55 struct cordic_iq cordic_calc_iq(s32 theta)
  56 {
  57         struct cordic_iq coord;
  58         s32 angle, valtmp;
  59         unsigned iter;
  60         int signx = 1;
  61         int signtheta;
  62
  63         coord.i = CORDIC_ANGLE_GEN;
  64         coord.q = 0;
  65         angle = 0;
  66
  67         theta = FIXED(theta);
  68         signtheta = (theta < 0) ? -1 : 1;
  69         theta = ((theta + FIXED(180) * signtheta) % FIXED(360)) -
  70                 FIXED(180) * signtheta;
  71
  72         if (FLOAT(theta) > 90) {
  73                 theta -= FIXED(180);
  74                 signx = -1;
  75         } else if (FLOAT(theta) < -90) {
  76                 theta += FIXED(180);
  77                 signx = -1;
  78         }
  79
  80         for (iter = 0; iter < CORDIC_NUM_ITER; iter++) {
  81                 if (theta > angle) {
  82                         valtmp = coord.i - (coord.q >> iter);
  83                         coord.q += (coord.i >> iter);
  84                         angle += arctan_table[iter];
  85                 } else {
  86                         valtmp = coord.i + (coord.q >> iter);
  87                         coord.q -= (coord.i >> iter);
  88                         angle -= arctan_table[iter];
  89                 }
  90                 coord.i = valtmp;
  91         }
  92
  93         coord.i *= signx;
  94         coord.q *= signx;
  95         return coord;
  96 }
  97 EXPORT_SYMBOL(cordic_calc_iq);
  98
  99 MODULE_DESCRIPTION("CORDIC algorithm");
 100 MODULE_AUTHOR("Broadcom Corporation");
 101 MODULE_LICENSE("Dual BSD/GPL");