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

#ifndef __ARM_EABI__
#define	XH	r0						// Mantissa high
#define	XL	r1						// Mantissa low
#else
#define	XH	r1						// Mantissa high
#define	XL	r0						// Mantissa low
#endif
#define	RH	r2						// Result high
#define	RL	r3						// Result low
#define	BM	r4						// Bit mask
#define	CT	r5						// Loop count
#define	TH	r6						// Temporary high
#define	TL	r7						// Temporary low
#define	TM	r8						// Temporary mask (low)
#define	SH	r9						// Subtract result high
#define	SL	r10						// Subtract result low
#define	XE	r12						// Exponent

/*
*	Square root core while we process the upper word.
*	It realises the following part of the algorithm:
*
*	res += bit;
*	tmp -= res;
*	if ( carry )
*		res += bit;
*	else
*		tmp += res, res -= bit;
*	tmp <<= 1;
*
*	It has a parameter with which it shifts 'bit' to the right by the
*	given number of times before applying it. This saves an instruction.
*	Also, since 'bit' is in the upper word, it can not have any effect
*	on the lower word of 'res', which is at the moment 0, therefore
*	can't affect the lower word of 'tmp'. Thus, only the upper words
*	have to be processed.
*/

	.macro	upper,n
	add		RH,RH,BM,lsr #\n
	subs	XH,XH,RH
	addcs	RH,RH,BM,lsr #\n
	addcc	XH,XH,RH
	subcc	RH,RH,BM,lsr #\n
	adds	XL,XL,XL
	adc		XH,XH,XH
	.endm

/*
*	This macro is used for the lower word.
*	The algorithm is the same as for the previous macro, bit, alas, we
*	have to re-organise it. The problem is that we now have to process
*	both the upper and the lower words and thus we need to propagate the
*	carry from the lower word to the upper one. This means that we can't
*	rely on the condition code remaining constant after the subtraction.
*	So, we re-organise things a bit:
*
*	msk = res + bit
*	nrs = msk + bit;
*	sub = tmp - msk
*	if (carry) {
*		res = nrs;
*		tmp = sub;
*	}
*	tmp <<= 1;
*	bit >>= 1;
*/

	.macro	lower
	add		TM,RL,BM
	adds	TL,TM,BM
	adc		TH,RH,#0
	subs	SL,XL,TM
	sbcs	SH,XH,RH
	movcs	RH,TH
	movcs	RL,TL
	movcs	XH,SH
	movcs	XL,SL
	mov		BM,BM,lsr #1
	adds	XL,XL,XL
	adc		XH,XH,XH
	.endm

/*
*	IEEE-754 double precision square root
*	~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
*	Returns 0 if the argument is 0 (even if it is -0), NaN if the
*	argument is NaN or negative, +INF if the argument is +INF and
*	the square root of the argument otherwise.
*
*	The algorithm below is based on an integer square root algorithm,
*	which does a successive approximation starting at the highest bit.
*	For simplicity's sake, the integer algorithm is shown in 32 bit.
*	If the largest unsigend integer is 0xffffffff, then the largest
*	possible result is 0xffff (due to rounding down in integer domain):
*
*	That is, to get the square root of x you can:
*
*	APR = 0x00000000;
*	BIT = 0x00008000;
*
*	while ( BIT ) {
*
*		APR |= BIT;
*		if ( APR * APR > x ) APR &= ~ BIT;
*		BIT >>= 1;
*	}
*
*	Of course we want to get rid of the multiplication, and we can.
*	If at a certain point you have the approximation of APRn, then
*	the next value for that will be APRn+1 = APRn + 2^k, where k is
*	counting down as n increases (BIT). Now we can realise that:
*
*	(APRn+1)^2 = ( APRn + 2^k )^2 =
*
*	APRn^2 + 2 * APRn * 2^k + 2^2k =
*
*	APRn^2 + APRn * 2^(k+1) + 2^2k
*
*	Now since 'k' is a quantity that counts down from 15 to 0, the 2^2k
*	is just a word starting at 1 << 2*15 and shifted to the right twice
*	every round. APRn * 2^x is just a shifted version of APRn and since
*	the shift amount just counts down, it is in reality turns into a
*	left shift. Since you always derive APRn+1 from APRn by or-ing a
*	single bit to it, it really does not mean any extra calculations.
*	As was shown above, (APRn+1)^2 is very easily derived from (APRn)^2.
*	Instead of doing anything multiplicative, you can just keep a running
*	version of the square of the best approximation and compare that to
*	the number of which you want the square root.
*
*	The float version actually does not do much more, it does the integer
*	square root on the mantissa and halves the exponent (if the original
*	exponent is odd, then the mantissa is shifted by 1 position, thus
*	making the exponent even). The only thing is that while an integer
*	square root of a 32 bit number is at most 16 bits, a 53-bit double's
*	square root should be 53 bits, therefore the integer algorithm is let
*	to run longer. In addition, with a bit of trickery you can reorder
*	things so that most of the shifting and some of the temporaries can
*	be avoided.
*
*	The actual algorithm is presented below. The processing of the exponent,
*	the rounding and the dealing with denormalised numbers is omitted for
*	the sake of brevity. The input, x, is the mantissa i.e. a number
*	in [1,2). The value of 0x20000000 represents 1. The returned result
*	is the square root of that value, in the same 1.25 fixpoint format.
*	All variables are treated as unsigned 64 bit integers.
*
*	sqrt( x )
*	{
*		tmp = x;
*		res = 0;
*		bit = 0x2000000000000000;
*
*		while ( bit ) {
*
*			res |= bit;
*
*			if ( tmp >= res ) {
*
*				tmp -= res;
*				res += bit;
*			}
*			else {
*
*				res ^= bit;
*			}
*
*			bit >>= 1;
*			tmp <<= 1;
*	    }
*
*		return( res >> 1 );
*	}
*
*	The reason for 0x200..000 is that you need 2 more bits to avoid
*	unsigned arithmetic overflow (yes, two, one is not enough). Since
*	the mantissa is actually only 53 bits long, it causes no loss of
*	precision. In the assembly code below the compare business is
*	done slightly differently:
*
*			res += bit;
*			tmp -= res;
*
*			if ( carry ) { 	// The carry indicates that tmp >= res was true
*
*				res += bit;
*			}
*			else {
*
*				tmp += res;
*				res -= bit;
*			}
*
*	simply because it is more convenient that way. In addition, since a
*	64-bit int is actually 2 32-bit ones, while 'bit' is in the upper word,
*	we don't bother with the lower word of the result, as it always contains
*	0. Furthermore, we terminate after only 56 rounds instead of waiting for
*	bit to become 0 (which would be 62 rounds). The last 6 rounds generate
*	bits that we throw away anyway. 56 rounds generates all the bits needed
*	for the actual mantissa plus enough for proper rounding.
*
*	Using the above algorithm you can calculate a square root in rougly
*	as much time as a (software) division takes.
*/

	afunc	sqrt

	// 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.

	orrs	XE,XL,XH,lsl #1			// Check for zero argument, signum ignored
	biceq	XH,XH,#0x80000000		// If 0, make sure that it is positive
	bxeq	lr						// and return with the result
	tst		XH,XH					// Otherwise, if the number is negative
	movmi	XH,#0x7fffffff			// we have to return a quiet NaN. We set
	movmi	XL,#0xffffffff			// all flag bits on it, nobody knows
	bxmi	lr						// what they mean anyway...
	cmn		XH,#(1<<20)				// Set the signum flag if exp == 2047
	bxmi	lr						// in which case we just return

	// This is a number that we can handle. Extract the exponent
	// and the mantissa.

	stmdb	sp!,{r4-r10}			// Save registers
	movs	XE,XH,lsr #20			// Extract the exponent, set the Z bit
	eor		XH,XH,XE,lsl #20		// and delete it from the mantissa
	orrne	XH,XH,#(1<<20)			// If it's normalised, set the implicit bit
	mov		XH,XH,lsl #11			// Shift the mantissa up by 11 bits
	orr		XH,XH,XL,lsr #21		// regardless of whether it was
	mov		XL,XL,lsl #11			// normalised or not
	bne		normalised				// Skip normalisation if it was normalised

	// Denormalised number, normalise it

	tst		XH,XH					// Can we shift in 32 bits ?
	moveq	XH,XL					// If yes, do it
	moveq	XL,#0					// and modify the lower bit
	subeq	XE,XE,#32				// then adjust the exponent
	tst		XH,XH					// Test XH again
	bmi		normalised				// If negative, we're done

	// Now we have to shift the number up between 1 and 31 times. To work
	// out the shift count we can do looping, which is small but very slow
	// or we could do a successive approximation thing, which is much
	// faster but needs some code. Well, if you deal with doubles then
	// FLASH should not be your concern, so here it goes...

	mov		CT,#-1					// 0xffffffff
	mov		CT,CT,lsl #16			// 0xffff0000
	tst		XH,CT					// 0x00000001
	movne	CT,#1
	moveq	CT,#17
	moveq	XH,XH,lsl #16			// 0x0001000
	tst		XH,#0xff000000
	addeq	CT,CT,#8
	moveq	XH,XH,lsl #8			// 0x0100000
	tst		XH,#0xf0000000
	addeq	CT,CT,#4
	moveq	XH,XH,lsl #4			// 0x1000000
	tst		XH,#0xc0000000
	addeq	CT,CT,#2
	moveq	XH,XH,lsl #2			// 0x400000
	mov		XH,XH,lsl #1			// We know we have to do this
	rsb		BM,CT,#32
	orr		XH,XH,XL,lsr BM
	mov		XL,XL,lsl CT
	sub		XE,XE,CT
normalised:
	mov		XL,XL,lsr #2			// Make 2 spare bits in the mantissa
	orr		XL,XL,XH,lsl #30
	mov		XH,XH,lsr #2

	// Now the mantissa is shifted up so that it has the form of
	// 001.x xxxx ... and the exponent is still biased.
	// Unbias then halve the exponent and if it was odd, then
	// add 1 to it and shift the mantissa down by 1 bit

	sub		XE,XE,#1024
	add		XE,XE,#1
	movs	XE,XE,asr #1
	movcs	XL,XL,lsr #1
	orrcs	XL,XL,XH,lsl #31
	movcs	XH,XH,lsr #1
	adc		XE,XE,#0

	// We are ready for the loop. We have to execute the upper loop 30 times
	// (we have 2 bits on the left that we don't process) and the lower loop
	// 26 times (that gives us 56 bits, enough for proper rounding).
	// We unroll the loops, the upper loop is done in 5 rounds of 6 steps
	// each, the lower loop is done 2 steps + 4 rounds of 6 steps each.

	mov		RH,#0					// Result upper word
	mov		BM,#0x20000000			// Bit mask
upperloop:
	upper	0						// Do 6 unrolled loops
	upper	1
	upper	2
	upper	3
	upper	4
	upper	5
	movs	BM,BM,lsr #6			// CY is set if the loop is finished
	bcc		upperloop				// Keep working if not done yet
	mov		RL,#0					// Result lower word
	mov		BM,#0x80000000			// Bit mask
	mov		CT,#5					// Loop counter. We want 26 steps in all
	b		lowerstart				// so in the first loop we only do 2.
lowerloop:
	lower							// Do 6 unrolled loops
	lower
	lower
	lower
lowerstart:
	lower
	lower
	subs	CT,CT,#1				// Decrement the loop counter
	bne		lowerloop				// and keep working if not done yet

	// Now RH:RL contains the result, 0x.xx xxxx ... format
	// Alas, the integer bit might be 0, so we might have to shift
	// it up and adjust the exponent

	tst		RH,#0x40000000
	subeq	XE,XE,#1
	moveq	RH,RH,lsl #1
	orreq	RH,RH,RL,lsr #31
	moveq	RL,RL,lsl #1

	// Now we have to round. If there's anything left in X, then we would
	// generate more bits if we continued, i.e. we have to guarantee that
	// the sticky bit is set.

	orrs	XH,XH,XL
	orrne	RL,RL,#1

	// Now, the result is in RH:RL in this format:
	// 0nnn ... nnn : nnn ... nnnrrr ... rrr
	// where n-s are the 53 bits of the final mantissa. Since we have a
	// leading 0 (1 bit) and 53 bits of the mantissa, we have thus 10
	// remaining (extra) bits to use for rounding. As per IEEE terminology,
	// the masks for the various bits used for the rounding are the following:
	// 0x400 for L, 0x200 for G, 0x100 for R and 0xff for S.

	mov		BM,#0x600
	sub		BM,BM,#1
	tst		RL,#0x200
	tstne	RL,BM
	moveq	BM,#0
	movne	BM,#0x400
	adds	RL,RL,BM
	adcs	RH,RH,#0

	// If the rounding created an extra bit, we need to shift
	// an extra bit and adjust the exponent accordingly. If that
	// was the case the result became negative, so we don't have
	// to test again. In any case, we need to shift the number to
	// the right by, so depending on the signum we shift by 10 or 11
	// bits.

	movpl	RL,RL,lsr #10
	orrpl	XL,RL,RH,lsl #22
	movpl	XH,RH,lsr #10
	movmi	RL,RL,lsr #11
	orrmi	XL,RL,RH,lsl #21
	movmi	XH,RH,lsr #11
	addmi	XE,XE,#1

	// Delete the implicit bit and bias the exponent, put them together
	// and return. Note that we don't have to check for denormalised numbers,
	// because the square root of any denormalised number is normalised.

	bic		XH,XH,#0x00100000
	add		XE,XE,#1024
	sub		XE,XE,#1
	orr		XH,XH,XE,lsl #20
	ldmia	sp!,{r4-r10}
	bx		lr

