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