/****************************************************************************/
/*	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 <chips/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

/*
*	Rotates the (x,y) vector by z angle
*	~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
*	See the library docs of the function for what it does.
*	It uses a CORDIC core, circular type, rotational mode.
*	The 28 iterations are partially unrolled, 4 loops containing
*	7 unrolled steps each.
*
*	You can extract the equivalent C function by issuing the following
*	at the unix command line:
*
*	cat vrot32.S | grep "^//" | sed -e 's/^\/\/\t\?//' > vrot32_code.c
*/

//	extern int	_blm_cordic_atan[ 29 ];
//
//	long long	vrot32( int x, int y, int z )
//	{
//	int			c, t;
//	const int	*p;
//

	afunc	vrot32
	stmdb	sp!,{r4-r11}

//		if ( z < 0 ) {
//
//			x = -x;
//			y = -y;
//		}
//
//		z <<= 1;
//
//		if ( z < 0 ) {
//
//			c  = y;
//			y  = x;
//			x  = -c;
//			z -= 0x80000000;
//		}
//

	adds	Z,Z,Z
	rsbcs	X,X,#0
	rsbcs	Y,Y,#0
	tst		Z,#0x80000000
	movne	C,Y
	movne	Y,X
	rsbne	X,C,#0
	subne	Z,Z,#0x80000000

//		z = ( (long long) z * 0xe38e38e4 + 0x80000000u ) >> 32;
//		p = _blm_cordic_atan;
//

	mov		C,#0xe3000000
	orr		C,C,#0x008e0000
	orr		C,C,#0x00003800
	orr		C,C,#0x000000e4
	umull	P,Z,C,Z
	add		Z,Z,P,lsr #31
	ldr		P,1f

//		for ( c = 0 ; c < 28 ; c++ ) {
//

	mov		C,#0
0:
	ldmia	P!,{r5-r11}
	.set	N,0
	.rept	7

//			if ( z >= 0 ) {
//
//				t = x - ( y >> c );
//				y = y + ( x >> c );
//				z = z - *p++;
//			}
//			else {
//
//				t = x + ( y >> c );
//				y = y - ( x >> c );
//				z = z + *p++;
//			}
//
//			x = t;

	tst		Z,Z
	subpl	T,X,Y,asr C
	addpl	Y,Y,X,asr C
	MODZ	subpl,%(N+A)
	addmi	T,X,Y,asr C
	submi	Y,Y,X,asr C
	MODZ	addmi,%(N+A)
	mov		X,T

//		}
//

	add		C,C,#1
	.set	N,N+1
	.endr
	cmp		C,#28
	bne		0b

//		z = *p;
//		c = x < 0;
//		x = c ? -x : x;
//		x = ( (unsigned) z * (long long) x + ( 1 << 31 ) ) >> 32;
//		x = c ? -x : x;
//		c = y < 0;
//		y = c ? -y : y;
//		y = ( (unsigned) z * (long long) y + ( 1 << 31 ) ) >> 32;
//		y = c ? -y : y;
//

	ldr		Z,[P]
	tst		X,X
	rsbmi	X,X,#0
	umull	T,X,Z,X
	add		X,X,T,lsr #31
	rsbmi	X,X,#0
	tst		Y,Y
	rsbmi	Y,Y,#0
	umull	T,Y,Z,Y
	add		Y,Y,T,lsr #31
	rsbmi	Y,Y,#0

//		return( ( (long long) y << 32 ) | (unsigned) x );
//	}

	ldmia	sp!,{r4-r11}
	bx		lr
1:	.word	_blm_cordic_atan
	.endfunc

