/****************************************************************************/
/*	Copyright (c) 2009, Bendor Research Pty. Ltd. All rights reserved.		*/
/*	This software is released under the GNU General Public License, version	*/
/*	3, with the exemptions that 1) the header files installed by this 	 	*/
/*	package can be included in closed-source code and 2) the libraries		*/
/*	built by this package can be statically linked into a closed-source 	*/
/*	binary. See the file LICENSE for the details.							*/
/*	This software is provided "AS IS", without any warranty or guarantee.	*/
/*	By using this software you acknowledge that neither Bendor Research nor	*/
/*	any of its personnel or affiliates can be held liable for any damages,	*/
/*	including, but not limited to, loss of business, loss of money, loss of */
/*	life, loss of reputation, arising out of this software being faulty or	*/
/*	not performing as advertised, even if Bendor Research was aware of such	*/
/*	faults in the software. You use this software at your own risk, any and */
/*	all liabilities that arise out of your using this software are strictly	*/
/*	yours.																	*/
/****************************************************************************/

#include <arm_func.h>

/*
*	Register definitions. Leave them alone!
*/

#define	X		r0					// Current X
#define	Y		r1					// Current Y
#define	Z		r2					// Current Z
#define	P		r3					// Pointer to the atan() table
#define	T		r4					// Temporary
#define	C		r12					// Loop counter
#define	A		5					// Base register for table prefetch

/*
*	This macro is used for Z compensation
*/

	.altmacro
	.macro	MODZ insn,cnt
	insn	Z,Z,r\cnt
	.endm

/*
*	CORDIC core, circular type, vectoring mode
*	~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
*	At entry r0 is X, r1 is Y and r2 is an initial angle.
*	The angle must be between -PI/2 and +PI/2, the radix point
*	is after the 4th bit of the word (i.e. the most significant
*	hex digit represents the integer part). The X and Y values
*	can be of any pursuation, as long as they are signed and have
*	no more than 28 significant bits.
*
*	At exit r0 contains the length of the vector denoted by X and Y
*	and r1 contains the initial angle - atan( Y / X ).
*	The compensation for the CORDIC scaling factor is already done.
*/

	afunc	_blm_cordicv
	stmdb	sp!,{r4-r11}			// Save work registers
	ldr		P,1f					// Load atan() table address
	mov		C,#0					// Initialise the loop count, i.e. i = 0;
0:
	ldmia	P!,{r5-r11}				// Prefetch 7 tags from the table
	.set	N,0						// Repeat counter
	.rept	7						//	for ( ... ) {
	tst		Y,Y						//		if ( y < 0 ) {
	submi	T,X,Y,asr C				//			t = x - ( y >> i );
	addmi	Y,Y,X,asr C				//			y = y + ( x >> i );
	MODZ	submi,%(N+A)			//			z = z - table[ i ];
									//		} else {
	addpl	T,X,Y,asr C				//			t = x + ( y >> i );
	subpl	Y,Y,X,asr C				//			y = y - ( x >> i );
	MODZ	addpl,%(N+A)			//			z = z + table[ i ];
	mov		X,T						//		} x = t;
	add		C,C,#1					//	i++;
	.set	N,N+1					// Adjust the repeat counter
	.endr							// End of the unrolled loop
	cmp		C,#28					// Have we done 28 iterations ?
	bne		0b						// If not, keep going
	mov		Y,Z						// Z is unscaled, just move to its place
	ldr		Z,[P]					// Fetch the inverse scale factor - 1 / 2
	smull	T,C,Z,X					// Un-scale X
	adds	T,T,T
	adc		C,C,C
	adds	T,T,X
	adc		X,C,#0
	ldmia	sp!,{r4-r11}			// Restore all registers
	bx		lr						// and return
1:	.word	_blm_cordict			// Pointer to the atan() table
	.endfunc

