/****************************************************************************/
/*	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	F		r14					// Flag register
#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

/*
*	Calculates the absolute value and angle of a 2D vector
*	~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
*	See the library docs of the function for what it does.
*	It uses a CORDIC core, circular type, vectoring 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 vabs32.S | grep "^//" | sed -e 's/^\/\/\t\?//' > vabs32_code.c
*/

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

	afunc	vabs32
	stmdb	sp!,{r4-r11,r14}

//		p = _blm_cordic_atan;
//		z = 0;
//		f = 0;
//
//		if ( y < 0 ) {
//
//			y = -y;
//			f = 1;
//		}
//
//		if ( x < 0 ) {
//
//			x  = -x;
//			f ^= 3;
//		}
//

	ldr		P,1f
	mov		Z,#0
	movs	F,Y,lsr #31
	rsbne	Y,Y,#0
	tst		X,X
	rsbmi	X,X,#0
	eormi	F,F,#3

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

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

//			if ( y < 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		Y,Y
	submi	T,X,Y,asr C
	addmi	Y,Y,X,asr C
	MODZ	submi,%(N+A)
	addpl	T,X,Y,asr C
	subpl	Y,Y,X,asr C
	MODZ	addpl,%(N+A)
	mov		X,T

//		}
//

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

//		y = ( z >> 1 ) + ( z >> 4 );
//		if ( f & 1 ) y = -y;
//		if ( f & 2 ) y += ( 1 << 31 );

	mov		Y,Z,asr #1
	add		Y,Z,asr #4
	tst		F,#1
	rsbne	Y,Y,#0
	tst		F,#2
	addne	Y,Y,#0x80000000

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

	ldr		Z,[P]
	tst		X,X
	movmi	X,#0
	umullpl	T,X,Z,X
	addpl	X,X,T,lsr #31

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

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

