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

/*
*	Definitions of the registers we use. Do not change it.
*/

#ifndef __ARM_EABI__
#define H		r0					// Mantissa high word
#define L		r1					// Mantissa low word
#else
#define H		r1					// Mantissa high word
#define L		r0					// Mantissa low word
#endif
#define P		r2					// Pointer to the octant
#define E		r3					// Exponent
#define A		r4					// Temporary for multiplication
#define B		r5					// Temporary for multiplication
#define Q		r6					// General temporary
#define R		r7					// Extended precision reg. bits 159 - 128
#define S		r8					// Extended precision reg. bits 127 -  96
#define T		r9					// Extended precision reg. bits  95 -  64
#define U		r10					// Extended precision reg. bits  63 -  32
#define V		r11					// Extended precision reg. bits  31 -   0
#define W		r12					// Temporary and octant (needs no saving)

/*
*	Payne-Hanek modular range reduction for trigonometric functions
*	~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
*
*	It has a prototype of double _blm_PayneHanek( double x, int *o );
*
*	If the argument is NaN or infinite, the function returns NaN and sets
*	the integer pointed by 'o' to -1. Otherwise it takes the absolute value
*	of the argument, then reduces it to [0,+PI/4), scales that range into
*	[0,+1) and returns that value.
*	In addition, the integer pointed by 'o' will be set to a value in [0,7]
*	indicating which octant of the 0-2*PI range the result is in; the octant
*	calculation is symmetrical to 0:
*
*					^
*	   ...    1   0	| 0	  1	  2	  3	  4	  5	  6	  7	  0	  1
*	+1 ---			+
*		   |\   |\  |  /|  /|  /|  /|  /|  /|  /|  /|  /|
*	   ... | \  | \ | / | / | / | / | / | / | / | / | / |  ...
*		   |  \ |  \|/	|/	|/	|/	|/	|/	|/	|/	|/  |/
*	------------------------------------------------------->
*  					|	PI	PI		PI			   2PI
* 						--	--
*					     4	 2
*
*	The function works on any input and the reduction is pretty precise.
*	Large numbers are treated as if they were an integer with at most 53
*	non-zero bits on their left and then a proper modulus is calculated
*	against that integer.
*
*	The function uses an algorithm by Payne and Hanek. Here is how it works.
*	To avoid writing PI / 4 I will use the shorthand P4 and 4 / PI will be
*	marked with 4P. In addition, the explanation omits the final scaling
*	to the [0,+1) interval but it actually comes for free, as you will see.
*
*	You have a number x and you want to calculate y such that:
*
*	0 <= y < P4 and |x| = y + k * P4, where k is a non-negative integer.
*
*	In addition, you need the value of o = k % 8, to know which octant of the
*	0 - 2*PI range are you in.

*	In the rest of text x is assumed to be positive, any further references
*	to the absolute value will be omitted.
*
*	If you had infinite precision the reduction would be simple:
*
*	k = floor( x / P4 )		= floor( x * 4P )
*	y = x - k * P4			= fract( x * 4P ) * P4
*	o = k % 8
*
*	Of course you don't have infinite precision. The value of x is presented
*	as an IEEE-754 format number, which is in the form of 1.mmmmm * 2^e where
*	mmmmm represents 52 binary fractional digits of the mantissa and e is an
*	integer in the range of -1074 to +1022. Now if e is less than -1 then you
*	do not have to do anything, because that means that the number is less
*	than 0.5 , and 0.5 * ( 4 / PI ) is less than 1, thus the floor operation
*	will always yield 0. That is, in such cases the range reduction
*	degenerates to the simple:
*
*	y = x
*	o = 0
*
*	So, consider the case when e is larger than -2. For a moment let's assume
*	that we have a fixpoint arithmetic subsystem that can properly represent
*	any number, thus it has infinite number of integer and fractional bits.
*	In that system x, if its exponent is 0, would look like this:
*
*								 52 fr. bits
*	... 00000000 ... 0000000001.mmmm ... mmmmm000000000000 ... 000000 ...
*
*	and if the exponent is say 4, it would look like this:
*
*							     48 fr. bits
*	... 00000000 ... 000001mmmm.mmmm ... m0000000000000000 ... 000000 ...
*
*	Let us also assume that we have 4 / PI (a.k.a. 4P) in full precision:
*
*	... 00000000 ... 0000000001.pppp ... ppppppppppppppppp ... pppppp ...
*
*	Let us now assume that e is a large number, larger than 52. Now consider
*	the following identity:
*
*	( m * 2^e ) * 4P = ( m ^ 2^52 ) * ( 4P * 2^(e-52) )
*
*	which basically means that instead of multiplying x (which is m * 2^e)
*	with 4P, we first shift m to the left by 52 bits, so that it now represents
*	an integer:
*
*	... 0000001m  ... mmmmmmmmmm.0000 ... 00000000000000000 ... 000000 ...
*
*	and then we shift 4P by e-52 bits to the left (would e-52 be negative, then
*	we would shift it by 52-e bits to the right). Let us now consider the case
*	when e is greater than 52, say it is 56. In that case we have to shift 4P
*	to the left by 4 bits:
*
*	... 00000000 ... 000001pppp.pppp ... ppppppppppppppppp ... pppppp ...
*
*	If we now do the multiply, we get:
*
*	... 00nnnnnn ... nnnnnnnNNN.FFFF ... FFfffffffffffffff ... ffffff ...
*
*	Since the shifted mantissa had 53 integer bits and the shifted 4P had
*	5 integer bits, the result has 58 integer bits.
*
*	The thing is that we only need NNN from the 58 bit integer part, therefore
*	there's no point in calculating the upper 55 bits at all. That means that
*	the bits of the shifted 4P that would influence only the part of the result
*	denoted with 'n' can be discarded *before* the multiplication. Since we
*	multiply the shifted version of 4P with an integer (i.e. >= 1), a bit can
*	affect the bits in the result at or left of its own position, but never
*	right to it. Therefore,
*
*	... 00000000 ... 000001pppp.pppp ... ppppppppppppppppp ... pppppp ...
*						  ^^
*	the bits marked with the carets will have absolutely no effect on the
*	outcome of the calculation and they can be substituted by 0 before we start
*	the multiply. In general, we can discard all but the last 3 bits of the
*	integer part of the shifted version of 4P. Since before the shifting 4P had
*	1 integer bit, after the shifting it will have e-52+1 bits and we need to
*	keep 3, so we can discard e-52+1-3 = e-50 bits (if e is less than 51, we
*	can't discard any leading bits).
*
*	Now focus on the other side of the radix point. The floor() operation
*	should yield an IEEE-754 double, which has a mantissa of 53 bits. So, we
*	would assume that we are not interested in the bits of the result right of
*	the 53rd fractional bit. Let us assume for a moment that that is true and
*	see what are its consequences. Since we multiplied the shifted version of
*	4P by a 53-bit integer, the k-th fraction bit of the result will not be
*	affected by any bit left of the (k+53)-rd bit of the fractional part of
*	the shifted version of 4P. Thus, the 53-rd bit of the result will not be
*	affected by bits right of the 106-th bit of the shifted 4P. Therefore we
*	can discard further bits before the multiplication:
*
*										  53					106
*										  |						|
*	... 00000000 ... 000001pppp.pppp ... ppppppppppppppppp ... pppppp ...
*						  ^^									 ^^^^ ^^^^
*	Alas, our assumption was not exactly true. If x happens to be very close to
*	an integer multiple of PI/4, then the fractional part is small, with lots
*	of 0-s after the radix point before the first valuable bit occurs. Note
*	that it is still true that left of the 53-rd fractional bit of the result
*	will not be affected by any bit right of the 106-th fractional bit of the
*	shifted 4P. You can do with those bits whatever you want, the first 53
*	fractional bits of the result remain the same. However, bits after the
*	53rd fractional bit will be affected. Our problem here is that if the
*	first 53 fractional bits of the result are all 0-s, then if we return 0,
*	we commit an error. We're working in an IEEE-754 system, where leading
*	zeroes simply mean that we decrement the exponent, they are not part of
*	the actual mantissa. So, if we want high precision, then we can not
*	just discard all those bits right of the 106-th.
*	The question is, how much precision do we need? Well, it's up to us.
*	Note that this is an issue only if the angle we received was larger
*	than PI/4 (otherwise we don't do range reduction at all). Thus the
*	relative error we commit by throwing away those bits is quite small.
*	I decided to have the extra precision of one more IEEE-754 mantissa,
*	i.e. 53 bits. It's a reasonable compromise between precision and time.
*	Since we know the result down to 106 bits, our absolute error is not
*	more than 2.46E-32, far beyond the actual resolution of our argument.
*	So, we need 106 fractional bits of the result, for which we need 159 bits
*	of the fraction of the shifted version of 4P. Thus, with the 3 integer
*	bits, after the shifting we only need 163 bits of 4P and we multiply that
*	with the 53 bits of x, getting a result that has 56 integer bits and 159
*	fractional bits. Of that, we do not have to calculate the upper 53 bits
*	of the integer part and we have to calculate but do not need to store the
*	bottom 53 bits of the fractional part.
*
*	Now let us consider how much of 4P do we need to actually store?
*	The largest exponent that an IEEE-754 double can have is 1022. That means
*	that we shift our number to the left by at most 970 bits. We need no more
*	than 159 bits of the fractional part of the shifted version, thus if we
*	store the first 1129 bits of 4P, we can handle any x representable in a
*	double.
*
*	Therefore, the overall cost of the modular range reduction for the entire
*	range, with an error less than the IEEE-754 limit:
*
*	- A constant table of 1129 bits (i.e. 36 words of code memory)
*	- A temporary storage for the multiplication (6 words of data memory)
*	- The processing time roughly equivalent to 3 IEEE double multiplications
*	- Everything else :-) (i.e. the code itself, administrative variables, etc)
*
*	Now, some practical considerations:
*
*	We have 32x32->64 bit multiply, so we want everything to be a multiple of
*	32 bits. We calculated that we needed 162 bits of 4P. We cheat a bit and
*	settle for only 51 bits of precision for the degenerate case where the
*	leading fractional bits of the result are 0 and load only 5*32 = 160 bits.
*	That increases our absolute error to about 1E-31, still quite acceptable.
*	Furthermore, when we have to shift 4P to the left (-2 < e < 52) then we
*	will loose at most 31 more bits, resulting in the shamefully large
*	2.1E-22 maximum absolute error. However, consider the fact that since the
*	argument is > 0.5 and it has an 53 bit mantissa, it is determined to
*	only 5E-17, so we're not faring that bad.
*
*	Fetching 160 bits means that that number has 3 integer and 157 fractional
*	bits and we multiply it with a 64-bit integer. Now consider the following.
*	Let ABCDE be the 5 words of 4P and XY be the two words storing the integer
*	form of m. In ABCDE the radix point is after the third topmost bit of A.
*	When we multiply, we calculate:
*
*				ABCDE * XY
*				-----
*				   YE
*				  XE.
*				  YD.
*				 XD..
*				 YC..
*				XC...
*				YB...
*			   XB....
*			   YA....
*			  XA.....
*			----------
*			  PQRSTUV
*
*	In the result the radix point is now after the third topmost bit of R.
*	Since we do not need P and Q, we can omit the calculation of XA and we can
*	reduce the XB and YA multiplications from 32x32->64 to 32x32->32, which is
*	somewhat faster. Thus, we have to do 7 32x32->64 and 2 32x32->32 multiplies
*	which is remarkable, considering that a double multiply uses 4 32x32->64s.
*	In the degenerate case, where e is less than 52, we unfortunately need
*	Q as well, in that case we do 9 32x32->64 and 1 32x32->32 multiplies. It
*	is still just a bit more than 2 double multiplications!
*
*	Now, back to the transforming of the [0,P4) reduced range to [0,1).
*	Well, it's free, if we have to do range reduction. That's actually what
*	we have in the fractional digits. The only time when we have to work for
*	it is when e is less than -1. In that case we just do a multiply with 4P,
*	as if we were doing a double multiplication. Considering that the whole
*	reduction is actually omitted in such cases, it is still practically
*	free, at least faster than doing reduction and not doing the scaling.
*/

	afunc	_blm_PayneHanek
	mov		H,H,lsl #1				// Delete the signum bit
	movs	E,H,lsr #21				// Extract the biased exponent
	mov		H,H,lsl #11				// Delete the exponent from the mantissa
	mov		H,H,lsr #12
	mov		W,#1024					// Load 1023 into W
	sub		W,W,#1
	sub		E,E,W					// Unbias the exponent
	orrne	H,H,#0x00100000			// Set the implicit bit for normalised
	cmp		E,W						// Check for NaN or infinite
	movgt	H,#0x7fffffff			// If it was either, then
	movgt	L,#0xffffffff			// the return value will be NaN. We also
	strgt	L,[P]					// set the octant to a negative value
	bxgt	lr						// and return immediately.
	stmdb	sp!,{r4-r11}			// Otherwise, save work registers
	cmp		E,#-1					// Check if the exponent is less than -1
	bge		medium					// and if not, go to the next test

	// The exponent is less than -1.
	// We do not have to go through the whole long multiplication business
	// as we know up front that the result will be less than 1.0. Thus,
	// all we have to do is a simple multiply with 4/PI to do the range
	// transformation.

	mov		H,H,lsl #8				// Scale the mantissa so that
	orr		H,H,L,lsr #24			// the result of the mul will
	mov		L,L,lsl #8				// be at the right place
	adr		R,fop					// Load the rounded value of the
	ldmia	R,{R,S}					// first 64 bits of 4/PI into R:S
	add		S,S,#1					// Round it up (we know that we should)
	mov		A,#0					// Prepare for the multiply
	mov		B,#0					// We do the multiply like this:
	umull	U,T,L,S					// T,U now set
	umlal	T,A,H,S					// Low word of res added to T, high is in A
	mov		S,#0					// We don't need S any more
	umlal	T,S,L,R					// Low word of res added to T, high is in S
	umlal	S,B,H,R					// Low word added to S, high is in B
	adds	S,S,A					// Add A to S, keep the carry
	adc		R,B,#0					// Add the carry to B and store it in R
	mov		V,#0					// V is by nature should be 0
	add		E,E,#2					// Adjust the exponent
	b		extract					// Go to extracting the result

	// We know that the exponent is at least -1. We have to check
	// if it is large enough so that we will have to shift 4*PI.

medium:
	subs	Q,E,#54					// Bit offset
	mov		E,#0					// Exponent will be 0
	bge		large					// If the bit offset is >= 0, go there

	// If we are here it means that the argument was larger than 0.5 but it
	// is less than 2^54. In that case we do not yet throw away bits from
	// the front of 4/P. Rather, we have to shift it to the right to get the
	// result in the place we want it. This unfortunately decreases our
	// precision somewhat, as we may use as few as 96 bits of 4/PI. In order
	// to keep the precision, if we should shift 4/PI to the right by more
	// than 32 bits, we shift 32 bits less and throw away the upper 32 bits
	// of the result (which is in the integer part) instead. That way we drop
	// from 160 not to 96 but to 128 bits, which is not great, but acceptable.

	rsb		Q,Q,#0					// Number of shifts to the right
	cmp		Q,#32					// Is it at least 32 ?
	subge	Q,Q,#32					// If yes, decrement it by 32 and
	movge	E,#32					// mark the exponent for the multiply
	adr		V,fop					// Get the address of 4/PI
	ldmia	V,{R,S,T,U,V}			// Load 160 bits from it
	tst		Q,Q						// Do we have to shift ?
	beq		multiply				// If not, go to the multiplication
	rsb		W,Q,#32					// This will be the left shift
	mov		V,V,lsr Q				// Shift R,S,T,U,V to the right
	orr		V,V,U,lsl W				// by Q bits
	mov		U,U,lsr Q
	orr		U,U,T,lsl W
	mov		T,T,lsr Q
	orr		T,T,S,lsl W
	mov		S,S,lsr Q
	orr		S,S,R,lsl W
	mov		R,R,lsr Q
	b		multiply				// then go to multiply

	// We know that the exponent is large enough so that we have to shift
	// 4/PI to the left by 0 or more bits before the multiply. When we get
	// here Q contains the number of bits we have to shift before the mul.

large:
	mov		A,Q,lsr #5				// Calculate the word index of the table
	adr		B,fop					// Get the address of 4/PI and calculate
	add		B,B,A,lsl #2			// the address of the first word to fetch
	ands	Q,Q,#31					// Z will be set if we do not have to shift
	ldmia	B,{R,S,T,U,V,W}			// Load a 192 bit wide slice of 4/PI
	beq		multiply				// If we don't shift, we go to multiply
	rsb		B,Q,#32					// Set B to the complement shift count
	mov		R,R,lsl Q				// Shift R:S:T:U:V to the left by Q bits,
	orr		R,R,S,lsr B				// shifting bits from W to V as we go
	mov		S,S,lsl Q
	orr		S,S,T,lsr B
	mov		T,T,lsl Q
	orr		T,T,U,lsr B
	mov		U,U,lsl Q
	orr		U,U,V,lsr B
	mov		V,V,lsl Q
	orr		V,V,W,lsr B

	// This part calculates the R:S:T:U:V * H:L value.
	// If E is 0, then the most significant 64 bits of the result will be
	// thrown away and the remaining 160 bits are stored in R:S:T:U:V.
	// If E is non-zero, then the most significant 32 and the least
	// significant 32 bits are discarded and the in-between 160 bits
	// are retained in R:S:T:U:V.

multiply:
	mov		Q,V						// Save V, we will need it later
	umull	A,B,Q,L					// Get L*V into B:A
	mov		V,A						// Store the lower word in V
	mov		A,#0					// A is 0, B contains the upper word

	// At this point V is set to the low word of V*L. The upper word is
	// in B, Q contains V and A is set to 0. From now on we repeat the
	// same regular structure, so only the first one is commented. At the
	// end of each block, we have the same setup as we have now.

	umlal	B,A,Q,H					// Do V*H multiply, adding B
	mov		Q,U						// Save U in Q
	mov		U,B						// Set U to the low word of the prev mult
	mov		B,#0					// and make B to be 0
	umlal	U,B,Q,L					// Calculate the U*L, add low word to U
	adds	B,B,A					// Add VH's high word to UL's high word
	sub		A,A,A					// Clear A and if the previous add
	adc		A,A,A					// had a carry, then put that into A

	// So, we're back to where we were: A:B is the carry (max 33 bits)
	// from the previous multiply and Q contains the previous multiplier.
	// We repeat the same thing for T and S.

	umlal	B,A,Q,H
	mov		Q,T
	mov		T,B
	mov		B,#0
	umlal	T,B,Q,L
	adds	B,B,A
	sub		A,A,A
	adc		A,A,A
	umlal	B,A,Q,H
	mov		Q,S
	mov		S,B
	mov		B,#0
	umlal	S,B,Q,L
	adds	B,B,A
	sub		A,A,A
	adc		A,A,A

	// Now we have to decide if we discard 64 bits on the left or 32 bits
	// on each end. If the exponent is not 0, then we should do both ends.

	tst		E,E

	// The Z bit is set if we discard 64 bits on the left. In that case we
	// have to do 2 more single precision multiplies, namely the R*L and
	// S*H ones, which should then be added to the lower word of the carry
	// forward from the last step (i.e. B). Note that A can be discarded,
	// because it is already part of the 64 bits that we throw away.

	mlaeq	B,R,L,B
	mlaeq	R,Q,H,B
	beq		extract

	// If Z was not set, then we have to discard 32 bits on the left
	// and 32 on the right. First, throw away the right bits by moving
	// B:S:T:U to S:T:U:V.

	mov		V,U
	mov		U,T
	mov		T,S
	mov		S,B

	// We should now mov A to B and clear A. INstead, we rename B to A.
	// Since we need to finish the last multiply (completing the new S)
	// and must generate R, we need to do 2 long multiplies and a short
	// one. We also need to clear the exponent as it was used as a flag.

	umlal	S,A,R,L
	umlal	S,A,Q,H
	mla		R,H,R,A
	mov		E,#0

	// The multiplication is done. Regardless of whether we had a small,
	// medium or large argument, the result is always scaled so that the
	// topmost 3 bits of R are the integer part and otherwise R:S:T:U:V
	// are the 157 fractional bits. The exponent is 0 or negative.

extract:
	mov		W,R,lsr #29				// Extract the integer part into W
	eor		R,R,W,lsl #29			// Remove it from the result
	sub		E,E,#9					// Adjust the exponent
	mov		Q,#6					// Loop count
wordup:
	tst		R,R						// If R is not 0
	bne		wordok					// then we're done with this bit.
	subs	Q,Q,#1					// Otherwise decrement the loop count
	moveq	E,#0					// If it's 0, then force the exponent
	beq		normdone				// to be 0 (already biased!) and finish
	mov		R,S						// Otherwise shift up the result by 32 bits
	mov		S,T
	mov		T,U
	mov		U,V
	mov		V,#0					// shifting in 0s
	sub		E,E,#32					// Adjust the exponent accordingly
	b		wordup					// and try again

	// We know that R is non-zero. Now we have to make R into
	// a format suitable for a mantissa.

wordok:
	mov		H,#0					// We can use H and L as temporaries now,
	mov		L,#0					// as we don't need the original mantissa.
	sub		L,L,#0x00200000			// L is now 0xffe00000
	tst		R,L						// Check if we have to shift left or right
	beq		lshift

	// The result has bits in its upper 11 bits. We have to work out how much
	// we have to shift. We do not want to loop because it's slow, so we do
	// some trickey here. We are free to use H, L, A and B.

	mov		A,R,lsr #21				// This will be the test material
	mov		B,#0					// This will be the shift amount
	tst		A,#0xfc0				// Any of the upper 6 bits set?
	addne	B,B,#6					// We will shift at least 6 bits
	movne	A,A,lsr #6				// and the bottom 6 bits are irrelevant
	tst		A,#0x038				// Any of the upper 3 bits are set?
	addne	B,B,#3					// Yes, so we shift 3 more bits
	movne	A,A,lsr #3				// and thow away the bits below
	tst		A,#0x006				// Any of the top 2 bits set?
	addne	B,B,#2					// Same deal, if yes, we shift 2 more
	movne	A,A,lsr #2				// and discard 2 bits
	add		B,B,A					// Only one bit is left, just add it

	// Now B contains the number or right shifts, which is guaranteed to be
	// non-zero. Do the shift. Note that we don't need U and V any more, so
	// we don't shift them.

	rsb		A,B,#32					// Complementary shift count
	mov		T,T,lsr B				// Shift R:S:T
	orr		T,T,S,lsl A
	mov		S,S,lsr B
	orr		S,S,R,lsl A
	mov		R,R,lsr B
	add		E,E,B					// Adjust the exponent
	b		shiftdone				// and we're done here

	// We know that we have to shift to the left, until the 0x00100000 bit
	// becomes set in the word. We have to find out the shift amount. We
	// use the same trickery as we used for the right shift, only we do
	// it backwards. We have one issue, though. To test for 11 shifts we
	// needed a mask with at most 6 bits in it, which neatly fits in an
	// immediate. To test for up to 20 bits of shift, the initial mask
	// has 10 bits in it, which doesn't fit in an immediate. So for that
	// single test we generate the constant and use that, for all subsequent
	// tests we can use an immediate.

lshift:
	mov		L,#0x00200000			// L = 0x00200000
	tst		R,L,lsr #1				// Is the 0x00100000 bit already set?
	bne		shiftdone				// If yes, we're done
	sub		L,L,L,lsr #10			// L = 0x001ff800
	mov		A,R						// Test material
	mov		B,#0					// Shift count
	tst		A,L						// Any bits in the upper 10 bits set?
	addeq	B,B,#10					// If not, we shift at least 10 bits
	moveq	A,A,lsl #10				// and adjust the test data
	tst		A,#0x001f0000			// The same deal for 5, 3 and 2 bits.
	addeq	B,B,#5
	moveq	A,A,lsl #5
	tst		A,#0x001c0000
	addeq	B,B,#3
	moveq	A,A,lsl #3
	tst		A,#0x00180000
	addeq	B,B,#2
	moveq	A,A,lsl #2
	tst		A,#0x00100000			// Last test, for 1 bit
	addeq	B,B,#1					// B is shift count. We know that it's !0
	rsb		A,B,#32					// Complementary shift amount
	mov		R,R,lsl B				// Shift left, we don't need U and V
	orr		R,R,S,lsr A
	mov		S,S,lsl B
	orr		S,S,T,lsr A
	mov		T,T,lsl B
	sub		E,E,B					// Adjust the exponent

	// We now have our number in R:S:T in a way that the 0x00100000 bit
	// is set (that's the implicit bit in the float point format). Now
	// we have to round the number. Since the sticky bit is always set,
	// due to 4/PI being an infinite sequence, all we have to do is to
	// test the guard bit and if it set, add 1 to the mantissa.

shiftdone:
//swi
	adds	S,S,T,lsr #31			// This adds the guard to the mantissa
	adc		R,R,#0					// and propagates the carry up
	tst		R,#0x00200000			// If the rounded result overflows,
	movne	R,#0x00100000			// then adjust it and also
	addne	E,E,#1					// adjust the exponent

	// Rounding is done, we have the final mantissa in R:S and the
	// exponent in E. W contains the current octant. We can now build
	// the float that we send back.
	// However, we have to cater for special cases. First, the exponent
	// should be negative at this point. If it's not, then after the final
	// rounding the result became +1.0. We should return 0 of the next octant.

rounddone:
	cmp		E,#0					// Test E. It should be negative.
	addge	W,W,#1					// If it is >= 0, we increment the octant
	movge	E,#0					// Force the exponent to *biased* 0
	bge		normdone				// and finish

	// Next step, we bias the exponent

	mov		A,#1024					// We need 1023 but we have to build it
	sub		A,A,#1
	adds	E,E,A					// The exponent is now biased
	bhi		normdone				// If the result is normalised, we're done

	// If the biased exponent is 0 or negative, we have to shift the
	// result to the right by 1 - e bits to create a denormalised result

	sbc		A,E,#1					// Shift count
	mov		E,#0					// Final biased exponent will be 0
	cmp		A,#53					// Denormalisation will result in 0?
	movge	R,#0					// If yes, force a 0 result
	movge	S,#0
	bge		normdone
	cmp		A,#32					// At least 32 bits ?
	movge	S,R						// If yes, move a whole
	movge	R,#0					// word and then
	subges	A,A,#32					// adjust the shift count
	beq		normdone				// If that was it, we're done
	rsb		B,A,#32					// Opposit shift number
	mov		S,S,lsr A				// Shift the mantissa
	orr		S,S,R,lsl B
	mov		R,R,lsr A

	// Everything is done. Just build the return value, restore
	// all the registers and return.

normdone:
	bic		H,R,#0x00100000			// Delete the implicit bit and move
	mov		L,S						// the mantissa into the result
	orr		H,H,E,lsl #20			// Put the exponent into place
	and		W,W,#7					// Make sure the octant is < 8
	str		W,[P]					// Store the octant
	ldmia	sp!,{r4-r11}			// Restore all registers
	bx		lr						// Return the result

/*
*	This contains the first 1280 bits of 4/PI. The radix point is
*	after the most significant bit of the first word. The word order
*	is big-endian.
*/

fop:
	.word	0xA2F9836E, 0x4E441529, 0xFC2757D1, 0xF534DDC0, 0xDB629599
	.word	0x3C439041, 0xFE5163AB, 0xDEBBC561, 0xB7246E3A, 0x424DD2E0
	.word	0x06492EEA, 0x09D1921C, 0xFE1DEB1C, 0xB129A73E, 0xE88235F5
	.word	0x2EBB4484, 0xE99C7026, 0xB45F7E41, 0x3991D639, 0x835339F4
	.word	0x9C845F8B, 0xBDF9283B, 0x1FF897FF, 0xDE05980F, 0xEF2F118B
	.word	0x5A0A6D1F, 0x6D367ECF, 0x27CB09B7, 0x4F463F66, 0x9E5FEA2D
	.word	0x7527BAC7, 0xEBE5F17B, 0x3D0739F7, 0x8A5292EA, 0x6BFB5FB1
	.word	0x1F8D5D08, 0x56033046, 0xFC7B6BAB, 0xF0CFBC20, 0x9AF4361D

