[deliverable/linux.git] / lib / cordic.c

/*
 * Copyright (c) 2011 Broadcom Corporation
 *
 * Permission to use, copy, modify, and/or distribute this software for any
 * purpose with or without fee is hereby granted, provided that the above
 * copyright notice and this permission notice appear in all copies.
 *
 * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
 * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
 * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY
 * SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
 * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION
 * OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN
 * CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
 */
#include <linux/module.h>
#include <linux/cordic.h>

#define CORDIC_ANGLE_GEN	39797
#define CORDIC_PRECISION_SHIFT	16
#define	CORDIC_NUM_ITER		(CORDIC_PRECISION_SHIFT + 2)

#define	FIXED(X)	((s32)((X) << CORDIC_PRECISION_SHIFT))
#define	FLOAT(X)	(((X) >= 0) \
		? ((((X) >> (CORDIC_PRECISION_SHIFT - 1)) + 1) >> 1) \
		: -((((-(X)) >> (CORDIC_PRECISION_SHIFT - 1)) + 1) >> 1))

static const s32 arctan_table[] = {
	2949120,
	1740967,
	919879,
	466945,
	234379,
	117304,
	58666,
	29335,
	14668,
	7334,
	3667,
	1833,
	917,
	458,
	229,
	115,
	57,
	29
};

/*
 * cordic_calc_iq() - calculates the i/q coordinate for given angle
 *
 * theta: angle in degrees for which i/q coordinate is to be calculated
 * coord: function output parameter holding the i/q coordinate
 */
struct cordic_iq cordic_calc_iq(s32 theta)
{
	struct cordic_iq coord;
	s32 angle, valtmp;
	unsigned iter;
	int signx = 1;
	int signtheta;

	coord.i = CORDIC_ANGLE_GEN;
	coord.q = 0;
	angle = 0;

	theta = FIXED(theta);
	signtheta = (theta < 0) ? -1 : 1;
	theta = ((theta + FIXED(180) * signtheta) % FIXED(360)) -
		FIXED(180) * signtheta;

	if (FLOAT(theta) > 90) {
		theta -= FIXED(180);
		signx = -1;
	} else if (FLOAT(theta) < -90) {
		theta += FIXED(180);
		signx = -1;
	}

	for (iter = 0; iter < CORDIC_NUM_ITER; iter++) {
		if (theta > angle) {
			valtmp = coord.i - (coord.q >> iter);
			coord.q += (coord.i >> iter);
			angle += arctan_table[iter];
		} else {
			valtmp = coord.i + (coord.q >> iter);
			coord.q -= (coord.i >> iter);
			angle -= arctan_table[iter];
		}
		coord.i = valtmp;
	}

	coord.i *= signx;
	coord.q *= signx;
	return coord;
}
EXPORT_SYMBOL(cordic_calc_iq);

MODULE_DESCRIPTION("CORDIC algorithm");
MODULE_AUTHOR("Broadcom Corporation");
MODULE_LICENSE("Dual BSD/GPL");
Commit	Line	Data
10f8113e AS	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
d89ce936	99	MODULE_DESCRIPTION("CORDIC algorithm");
10f8113e AS	100	MODULE_AUTHOR("Broadcom Corporation");
10f8113e AS	101	MODULE_LICENSE("Dual BSD/GPL");