/****************************************************************************/
/*	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 as they are.
*/

#define	X		r0					// The argument
#define	E		r1					// The exponent of the argument
#define	Y		r2					// Current estimate
#define	T		r3					// Temporary
#define	W		r12					// Multiplier work register

/*
*	Thus macro multiplies T with src and applies a sft left
*	shift on the result. The result is stored in T.
*	If the shift expression is zero, then no rounding is done,
*	otherwise simple rounding to nearest in is used.
*/

	.macro	mult src,sft
	umull	W,T,\src,T				// T:W is the 64-bit result
	.ifne	\sft
	mov		T,T,lsl #\sft			// Shift it up by the required amount
	orrs	T,T,W,lsr #(32-\sft)	// and set the carry to the rounding bit
	adc		T,T,#0					// Round, it works because T >= 0
	.endif
	.endm

/*
*	Square root of a single precision IEEE-754 float point number
*	~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
*/

	afunc	sqrtf

	// First, handle the special cases.
	// If the argument is +0.0 or -0.0, we return +0.0.
	// Otherwise if the argument is < 0.0, we return NaN.
	// Otherwise, if the argument is NaN or INF, we return the argument itself.

	movs	E,X,lsl #1				// Check for zero argument
	moveq	X,E						// If + or - 0, then we return +0.
	bxeq	lr
	movcs	X,#0x7fffffff			// If x is negative, return a quiet NaN
	cmn		X,#(1<<23)				// Set the signum flag if exp == 255
	bxmi	lr						// in which case we just return

	// We can process this is a number.
	// Extract the exponent and the mantissa.

	movs	E,X,lsr #23				// Extract the exponent, set the Z bit
	eor		X,X,E,lsl #23			// and delete it from the mantissa
	orrne	X,X,#(1<<23)			// If it's normalised, set the implicit bit
	mov		X,X,lsl #8				// Shift the mantissa up by 8 bits
	bne		normalised				// Skip normalisation if it was normalised

	// Denormalised number, we have to normalise it. Note that
	// the mantissa at this point is at least 0x00000100.

	mov		X,X,lsr #1
	tst		X,#0xff000000
	moveq	X,X,lsl #8
	subeq	E,E,#8
	tst		X,#0xff000000
	moveq	X,X,lsl #8
	subeq	E,E,#8
	tst		X,#0xf0000000
	moveq	X,X,lsl #4
	subeq	E,E,#4
	tst		X,#0xc0000000
	moveq	X,X,lsl #2
	subeq	E,E,#2
	tst		X,#0x80000000
	moveq	X,X,lsl #1
	subeq	E,E,#1

	// At this point we have the mantissa shifted up so that the top
	// bit of X is set. The exponent is still biased.

normalised:
	sub		E,E,#127				// Unbias the exponent
	movs	E,E,asr #1				// Divide it by two, bottom bit goes to C
	movcc	X,X,lsr #1				// If it was even, shift the mantissa

	// Now we have the final exponent already worked out. The mantissa is
	// now in the form that the radix point is after the second topmost
	// bit of the upper word, i.e. it is in the form of XX.xxx...xx00...00,
	// with at least 7 trailing zeroes. Furthermore, we know that the two
	// integer bits are not both 0, therefore the number is greater than or
	// equal to 1.0 but less than 4.0. We start the actual work now. Instead of
	// calculating sqrt(x), we calculate 1/sqrt(x). The Newton-Raphson method
	// converges very fast on that function and it has the form of:
	//
	//	y' = 0.5 * y * ( 3.0 - x * y^2 )
	//
	// where y is the current estimate for 1/sqrt(x) and y' is a better
	// estimate. This is a very nice algorithm, but since it uses a cubic
	// function (with 3 distinct zeroes) it's not stable. If the initial
	// estimate of y is not close enough, the algorithm diverges. However,
	// in our case the range for x is within 1 and 4, so we can have a table
	// of initial estimates that will guarantee that we are close to the real
	// value. We cut the [1,4) range to 24 equal sized intervals and take the
	// actual value of 1/sqrt(x) at the middle of the interval as our estimate.

	mov		T,X,lsr #27				// Table index + 8
	adr		Y,(sqrtab-8*4)			// Table address - 8 word
	ldr		T,[Y,T,lsl #2]			// Load table content (initial estimate)

	// This is the approximation loop, calculating the next y.
	// We do 3 iterations and unroll the loop.

	.rept	3
	mov		Y,T						// y' = t		y
	mult	X,2						// t *= x		x * y
	mult	Y,2						// t *= y		x * y^2
	rsb		T,T,#0xc0000000			// t  = 3 - t	3.0 - x * y^2
	mult	Y,1						// t *= y/2		0.5 * y * ( 3.0 - x * y^2 )
	.endr

	// At this point we have the latest y' in T. Since y = 1 / sqrt( x ),
	// we utilise this identity: sqrt(x) = x / sqrt(x) = x * ( 1 / sqrt(x) ).
	// This is the final calculation and since we will have to shift the
	// result into its place and properly round it anyway, we do not do any
	// normalisation shifting or rounding in the multiplier. This makes the
	// result being shifted down by 2 bits already.

	mult	X,0						// Now T is the final result / 4
	movs	Y,T,lsl #27				// Set the signum bit if we have to round
	mov		X,T,lsr #5				// Shift the result to place
	addmi	X,X,#1					// Round X (there'll never be an overflow)

	// Everything is done, just put the mantissa and exponent together.
	// Since we halved the exponent, the result is always normalised.

	add		E,E,#127				// Bias the exponent
	bic		X,X,#(1<<23)			// Get rid of the implicit bit
	orr		X,X,E,lsl #23			// Put the exponent in its place
	bx		lr						// and return
	.endfunc

/*
*	Precomputed initial values for y:
*
*	table[ n ] = (int) round( 1.073741824e9 / sqrt( 1.015625 + n / 8.0 ) )
*
*	where the table index n is between 0 and 23, inclusive.
*/

sqrtab:
	.word	0x3e16d092, 0x3abafd52, 0x37dd20ae, 0x3561335d
	.word	0x33333333, 0x314468ba, 0x2f89bacc, 0x2dfa9cf2
	.word	0x2c905a6f, 0x2b459b19, 0x2a160d52, 0x28fe28a0
	.word	0x27fb00f0, 0x270a2574, 0x262987b2, 0x25576878
	.word	0x24924925, 0x23d8e025, 0x232a0fda, 0x2284df58
	.word	0x21e8748c, 0x21540f7b, 0x20c70664, 0x2040c289

