/* --- Copyright University of Sussex 1998. All rights reserved. ----------
 * File:            C.power/src/afloat.s
 * Purpose:
 * Author:          John Gibson, Mar  6 1998
 */

;;; ----------------- FLOATING POINT ROUTINES -----------------------------

;;;	***********************************************
;;;	****         NOTE ASSEMBLER BUG:          *****
;;;	****  (___reg) DOES NOT ASSEMBLE AS 0(___reg)   *****
;;;	***********************************************

#_<

#_INCLUDE 'asm.ph'

constant
	procedure Sys$-Float_qrem
	;


lconstant macro (
	_DD_1		= @@DD_1,

	_eEXCESS	= _:1022, ;;; excess value on exponents
	_eLSHIFT	= _:1, 	  ;;; left shift to align 11-bit expo at top
	);

#_IF WORD_BITS/==DOUBLE_BITS
lconstant macro _DD_2 = @@DD_2;
#_ENDIF

>_#


ASM_START_FILE


ASM_CODE_PSECT


;;; ------------------------------------------------------------------------

	;;; _pfcopy(__________dst_addr, __________src_addr)
	;;; copy a double float from one address to another

ASM_ALIGN_QUAD
DEF_C_LAB (_pfcopy)
	ldW	rt0, 0(rusp)		;;; __________src_addr
	ldW	rt1, _WOFFS(rusp)	;;; __________dst_addr
	lfd	ft0, 0(rt0)
	la	rusp, _WOFFS*2(rusp)
	stfd	ft0, 0(rt1)
	blr


;;; --- BASIC ARITHMETIC -------------------------------------------------

ASM_ALIGN_QUAD
fpe_error:
	stW	rfalse, 0(rusp)
	blr

ASM_ALIGN_QUAD
fpe_test_error:
	mcrfs	CR2, 1			;;; get UX, ZX, XX, VXSNAN in CR2
	cror	CR0*4+0, CR1*4+2, CR1*4+3	;;; CR0-0 if VX or OX ...
	cror	CR0*4+0, CR0*4+0, CR2*4+1	;;; ... or ZX
	bflr	CR0*4+0				;;; return if none set
	stW	rfalse, 0(rusp)			;;; else return false
	blr


/* 	These routines return their 1st arg (_________dfaddr1) rather than true
	(just quicker, and nothing relies on them returning true).
	false is returned for overflow, etc.
*/

#_<
define lconstant macro ARITH_ROUTINE _ op;
[
	ldW   \t rt1, _0(rusp)		\n	;;; _________dfaddr2
\t	ldWu  \t rt0, _WOFFS(rusp)	\n	;;; _________dfaddr1
\t	lfd   \t ft1, _0(rt1)		\n
\t	lfd   \t ft0, _0(rt0)		\n
\t	mtfsfi \t _0, _0		\n	;;; clear FX in FPSCR
\t	^op   \t ft0, ft0, ft1		\n
\t	stfd  \t ft0, _0(rt0)		\n	;;; return result in _________dfaddr1
\t	bflr  \t '1*4+0'		\n	;;; return unless FX set in CR1
\t	b     \t fpe_test_error
].dl
enddefine;
>_#


	;;; _pfadd(_________dfaddr1, _________dfaddr2) -> _________dfaddr1 or false
	;;; add _________dfaddr2 destructively into _________dfaddr1

ASM_ALIGN_QUAD
DEF_C_LAB (_pfadd)
	ARITH_ROUTINE fadd.


	;;; _pfsub(_________dfaddr1, _________dfaddr2) -> _________dfaddr1 or false
	;;; subtract _________dfaddr2 destructively from _________dfaddr1

ASM_ALIGN_QUAD
DEF_C_LAB (_pfsub)
	ARITH_ROUTINE fsub.


	;;; _pfmult(_________dfaddr1, _________dfaddr2) -> _________dfaddr1 or false
	;;; multiply _________dfaddr1 destructively by _________dfaddr2

ASM_ALIGN_QUAD
DEF_C_LAB (_pfmult)
	ARITH_ROUTINE fmul.


	;;; _pfdiv(_________dfaddr1, _________dfaddr2) -> _________dfaddr1 or false
	;;; divide _________dfaddr1 destructively by _________dfaddr2

ASM_ALIGN_QUAD
DEF_C_LAB (_pfdiv)
	ARITH_ROUTINE fdiv.



;;; --- OTHER ARITHMETIC OPERATIONS ---------------------------------------

	;;; _pfabs(________dfaddr)  (absolute value)

ASM_ALIGN_QUAD
DEF_C_LAB (_pfabs)
	ldW	rt0, 0(rusp)		;;; ________dfaddr
	lfd	ft0, 0(rt0)
	la	rusp, _WOFFS(rusp)
	fabs	ft0, ft0
	stfd	ft0, 0(rt0)
	blr


	;;; _pfnegate(________dfaddr)  (negate)

ASM_ALIGN_QUAD
DEF_C_LAB (_pfnegate)
	ldW	rt0, 0(rusp)		;;; ________dfaddr
	lfd	ft0, 0(rt0)
	la	rusp, _WOFFS(rusp)
	fneg	ft0, ft0
	stfd	ft0, 0(rt0)
	blr


	;;; Dummy that just chains to Float_qrem

ASM_ALIGN_QUAD
DEF_C_LAB (_pfqrem)
	ldW	rpb, _SVB_OFFS(Sys$-Float_qrem)(rsvb)
	b	XC_LAB(Sys$-Float_qrem)


;;; --- PREDICATES --------------------------------------------------------

#_<
define lconstant macro CMP_ROUTINE _ br_op;
[
	ldW  \t	 rt1, _0(rusp)	    \n		;;; _________dfaddr2
\t	ldWu \t	 rt0, _WOFFS(rusp)  \n		;;; _________dfaddr1
\t	lfd \t	 ft1, _0(rt1)	    \n
\t	lfd \t	 ft0, _0(rt0)	    \n
\t	la  \t	 rt0, _TRUEOFFS(rfalse) \n
\t	fcmpu \t CR0, ft0, ft1      \n
\t	^br_op  \t '$+12'	    \n
\t	stW  \t	 rt0, _0(rusp)	    \n
\t	blr			    \n
\t	stW \t rfalse, _0(rusp)	    \n
\t	blr			    \n
].dl
enddefine;
>_#

	;;; _pfeq(_________dfaddr1, _________dfaddr2) -> ____bool
ASM_ALIGN_QUAD
DEF_C_LAB (_pfeq)
	CMP_ROUTINE bne

	;;; _pfsgr(_________dfaddr1, _________dfaddr2) -> ____bool
ASM_ALIGN_QUAD
DEF_C_LAB (_pfsgr)
	CMP_ROUTINE ble

	;;; _pfsgreq(_________dfaddr1, _________dfaddr2) -> ____bool
ASM_ALIGN_QUAD
DEF_C_LAB (_pfsgreq)
	CMP_ROUTINE blt


#_<
define lconstant macro TST_ROUTINE _ br_op;
[
	ldW \t	 rt0, _0(rusp)	\n		;;; ________dfaddr
\t	la  \t	 rt1, _TRUEOFFS(rfalse) \n
\t	lfd \t	 ft0, _0(rt0)	\n
\t	li  \t   rt2, _0	\n
\t	stw \t   rt2, _0(rusp)	\n
\t	lfs \t   ft1, _0(rusp)	\n
\t	fcmpu \t CR0, ft0, ft1	\n
\t	^br_op \t '$+12'	\n
\t	stW  \t	 rt1, _0(rusp)	\n
\t	blr			\n
\t	stW \t   rfalse, _0(rusp) \n
\t	blr			\n
].dl
enddefine;
>_#

	;;; _pfneg(________dfaddr) -> ____bool
ASM_ALIGN_QUAD
DEF_C_LAB (_pfneg)
	TST_ROUTINE bge

	;;; _pfzero(________dfaddr) -> ____bool
ASM_ALIGN_QUAD
DEF_C_LAB (_pfzero)
	TST_ROUTINE bne



;;; --- CONVERSION -------------------------------------------------------

;;; ---- Routines to float sysints, decimals and ddecimals

	;;; _pf_sfloat_dec(_______decimal) -> ________sfloat

ASM_ALIGN_QUAD
DEF_C_LAB (_pf_sfloat_dec)
	ldW	rt0, 0(rusp)		;;; _______decimal
	rlwinm	rt0, rt0, 0, ~1		;;; clear tag bit
#_IF WORD_BITS==DOUBLE_BITS
#_ELSE
	stW	rt0, 0(rusp)		;;; return ________sfloat
#_ENDIF
	blr


	;;; _pf_dfloat_int(_____int, ________dfaddr)

ASM_ALIGN_QUAD
DEF_C_LAB (_pf_dfloat_int)

#_IF WORD_BITS==DOUBLE_BITS
#_ELSE
	;;; this machine has no int -> float instruction -- wierd!
	;;; (algorithm copied from C compiler output)
	ldW	rt0, _WOFFS(rusp)	;;; _____int
	ldW	rt2, 0(rusp)		;;; ________dfaddr
	xoris	rt0, rt0, 0x8000	;;; change to unsigned range (+2**31)
	lis	rt1, 0x4330		;;; load 0x43300000 for expo
	stw	rt0, 4(rt2)
	stw	rt1, 0(rt2)
	lis	rt0, 0x5980
	addi	rt0, rt0, 0x0004	;;; load 0x59800004
	stw	rt0, 0(rusp)		;;; store in mem word
	lfd	ft0, 0(rt2)
	lfs	ft1, 0(rusp)
	la	rusp, _WOFFS*2(rusp)
	fsub	ft0, ft0, ft1
	stfd	ft0, 0(rt2)		;;; store at ________dfaddr
	blr
#_ENDIF


	;;; _pf_dfloat_dec(_______decimal, ________dfaddr)

ASM_ALIGN_QUAD
DEF_C_LAB (_pf_dfloat_dec)
	ldW	rt0, _WOFFS(rusp)	;;; _______decimal
	ldW	rt2, 0(rusp)		;;; ________dfaddr
	rlwinm	rt0, rt0, 0, ~1		;;; clear tag bit on _______decimal
#_IF WORD_BITS==DOUBLE_BITS
#_ELSE
	stw	rt0, 0(rt2)		;;; store in mem
	lfs	ft0, 0(rt2)		;;; reload as sfloat
	la	rusp, _WOFFS*2(rusp)
	stfd	ft0, 0(rt2)		;;; store dfloat at ________dfaddr
#_ENDIF
	blr


	;;; _pf_dfloat_ddec(________ddecimal, ________dfaddr)

ASM_ALIGN_QUAD
DEF_C_LAB (_pf_dfloat_ddec)
	ldW	rt0, _WOFFS(rusp)	;;; ________ddecimal
	ldW	rt3, 0(rusp)		;;; ________dfaddr
	ldW	rt1, _DD_1(rt0)
#_IF WORD_BITS/==DOUBLE_BITS
	lwz	rt2, _DD_2(rt0)
#_ENDIF
	la	rusp, _WOFFS*2(rusp)
	stW	rt1, 0(rt3)
#_IF WORD_BITS/==DOUBLE_BITS
	stw	rt2, 4(rt3)
#_ENDIF
	blr



;;; ---- Routines to convert back from a double float

	;;; _pf_intof(________dfaddr) -> (_____int, ________dfaddr)  or -> false
	;;; get integer part of double float as an integer

ASM_ALIGN_QUAD
DEF_C_LAB (_pf_intof)
	ldW	rt0, 0(rusp)		;;; ________dfaddr
	lis	rt2, 0x5981		;;; load 0x59810000
	lfd	ft0, 0(rt0)		;;; load the dfloat from ________dfaddr
	stw	rt2, 0(rusp)		;;; store 0x59810000 in mem word
	lwz	rt1, 0(rt0)		;;; get sign/expo part for overflow check

	;;; convert dfloat -> int
	lfs	ft1, 0(rusp)		;;; reload 0x59810000 as single float
	mffs	ft2			;;; save FPSCR bits
	mtfsb1	30			;;; set rounding mode (FPSCR 30-31)
	mtfsb1	31
	cmpwi	CR1, rt1, 0		;;; test sign of float into CR1
	bge	CR1, Lb0		;;; branch if +ve
	mtfsb0	31			;;; rounding mode for negative
Lb0:	fadd	ft1, ft0, ft1		;;; add the value
	mtfsf	1, ft2			;;; restore FPSCR bits 28-31
	stfd	ft1, -8(rsp)		;;; put float in mem ...
	lwz	rt2, -4(rsp)		;;; ... then lo 32 bits is _____int

	rlwinm	rt1, rt1, 12, 0x7ff	;;; get exponent for check
	cmplwi	CR0, rt1, _eEXCESS+_:WORD_BITS
	bge	Lb2		;;; overflow poss if expo >= (_eEXCESS+WORD_BITS)

	;;; else OK
Lb1:	la	rusp, -_WOFFS(rusp)
#_IF WORD_BITS==DOUBLE_BITS
#_ELSE
	stw	rt2, _WOFFS(rusp)	;;; store _____int result
#_ENDIF
	stW	rt0, 0(rusp)		;;; return ________dfaddr also
	blr

	;;; expo >= (_eEXCESS+WORD_BITS), overflow possible
Lb2:	bgt	Lb3		;;; overflow if expo > (_eEXCESS+WORD_BITS)
	;;; expo = (_eEXCESS+WORD_BITS) -- overflows if +ve, or -ve and
	;;; result not -ve
	bge	CR1, Lb3		;;; overflows if float positive
	mr.	R0, rt2			;;; test _____int
	blt	Lb1			;;; OK if result negative (-2**31)

	;;; overflow
Lb3:	stW	rfalse, 0(rusp)
	blr


	;;; _pf_cvt_to_dec(________dfaddr) -> _______decimal or false
	;;; convert double to decimal (rounded appropriately),
	;;; or return false if too large

ASM_ALIGN_QUAD
DEF_C_LAB (_pf_cvt_to_dec)
	ldW	rt0, 0(rusp)		;;; ________dfaddr

#_IF WORD_BITS==DOUBLE_BITS
#_ELSE
	;;; set bits to force rounding of 22nd bit of mantissa in all
	;;; cases except where tied and 21st bit is 0 (even)
	lis	rt3, 0x3800		;;; = 23rd, 24th, 25th bits
	lis	rt2, 0x4000		;;; = 22nd bit
	lwz	rt4, 0(rt0)		;;; part with exponent
	lfd	ft0, 0(rt0)		;;; load the float
	lwz	rt1, 4(rt0)		;;; part with ls 32 bits
	rlwinm.	R0, rt4, 12, 0x7ff	;;; exponent zero?
	bz-	Lc2			;;; branch if so
	or	rt3, rt1, rt3		;;; set 23rd, 24th, 25th bits
	cmplw	CR0, rt1, rt2		;;; test tied and even
	bne+	Lc1			;;; branch if not
	mr	rt3, rt1		;;; use original if so
Lc1:	stw	rt3, 4(rt0)		;;; replace in mem
	lfd	ft0, 0(rt0)		;;; then after reloading float ...
	stw	rt1, 4(rt0)		;;; ... restore part we altered

Lc2:	mtfsfi	0, 0			;;; clear FX in FPSCR
	frsp.	ft0, ft0		;;; round to single float
	stfs	ft0, 0(rusp)		;;; store single in mem
	lwz	rt0, 0(rusp)		;;; reload in int reg
	ori	rt0, rt0, 1		;;; set simple tag bit
	rlwinm	rt0, rt0, 0, ~2		;;; clear integer tag bit
	stW	rt0, 0(rusp)		;;; return _______decimal
	bflr+	CR1*4+0			;;; return unless FX set in CR1
	b	fpe_test_error
#_ENDIF


	;;; _pf_cvt_to_ddec(________dfaddr, ________ddecimal)
	;;; fill in given ddecimal

ASM_ALIGN_QUAD
DEF_C_LAB (_pf_cvt_to_ddec)
	ldW	rt0, _WOFFS(rusp)	;;; ________dfaddr
	ldW	rt1, 0(rusp)		;;; ________ddecimal
	lwz	rt2, 0(rt0)		;;; get both parts of double float
	lwz	rt3, 4(rt0)
	la	rusp, _WOFFS*2(rusp)
	;;; replace denormal values or -0.0 with 0
	rlwinm.	R0, rt2, 12, 0x7ff	;;; exponent nonzero?
	bnz+	Ld1			;;; leave alone if so
	li	rt2, 0			;;; else replace both parts with 0
	li	rt3, 0
Ld1:	stw	rt2, _DD_1(rt1)		;;; store in ________ddecimal
	stw	rt3, _DD_2(rt1)
	blr


	;;; _pfmodf(__________fracaddr, ________dfaddr)
	;;; frac part of ________dfaddr into __________fracaddr, int part back into ________dfaddr

ASM_ALIGN_QUAD
DEF_C_LAB (_pfmodf)
	ldW	rt0, 0(rusp)		;;; ________dfaddr
	ldW	rt1, _WOFFS(rusp)	;;; __________fracaddr
	lwz	rt2, 0(rt0)		;;; get parts of float
	lwz	rt3, 4(rt0)
	lfd	ft0, 0(rt0)		;;; load the dfloat

	la	rusp, _WOFFS*2(rusp)

	rlwinm	rt5, rt2, 12, 0x7ff	;;; get exponent
	subfic	rt5, rt5, _eEXCESS+1	;;; (_eEXCESS+1) - exponent
	mr.	R0, rt5			;;; test it
	bgt	Le3			;;; br if expo <= _eEXCESS, no int part
	;;; has nonzero int part
	addic.	rt5, rt5, 52		;;; number of fractional ls bits
	ble	Le4			;;; if <= 0, no fractional bits

	;;; has both parts
	cmplwi	CR0, rt5, 32		;;; >= 32 frac bits?
	bge	Le1			;;; branch if so
	srw	rt3, rt3, rt5		;;; clear frac bits in lo 32 bits
	slw	rt3, rt3, rt5
	b	Le2
Le1:	li	rt3, 0			;;; zero lo part
	addi	rt5, rt5, -32		;;; shift - 32
	srw	rt2, rt2, rt5		;;; clear frac bits in hi 32 bits
	slw	rt2, rt2, rt5
Le2:	stw	rt2, 0(rt0)		;;; store revised image ...
	stw	rt3, 4(rt0)
	lfd	ft1, 0(rt0)		;;; ... and back into float reg
	fsub	ft0, ft0, ft1		;;; subtract int part from original
	stfd	ft1, 0(rt0)		;;; store int part in ________dfaddr
	stfd	ft0, 0(rt1)		;;; and frac remainder in __________fracaddr
	blr

	;;; entirely fractional
Le3:	stfd	ft0, 0(rt1)		;;; store num in __________fracaddr
	li	rt2, 0
	stw	rt2, 0(rt0)		;;; and zero in ________dfaddr
	stw	rt2, 4(rt0)
	blr

	;;; entirely integral
Le4:	stfd	ft0, 0(rt0)		;;; store num in ________dfaddr
	li	rt2, 0
	stw	rt2, 0(rt1)		;;; and zero in __________fracaddr
	stw	rt2, 4(rt1)
	blr


;;; --- Routines to extract/update sfloat and dfloat field values ----

	;;; _pf_extend_s_to_d(________dfaddr) -> ________dfaddr or false
	;;; extend single at ________dfaddr into double

ASM_ALIGN_QUAD
DEF_C_LAB (_pf_extend_s_to_d)
	ldW	rt0, 0(rusp)		;;; ________dfaddr
	lfs	ft0, 0(rt0)		;;; load the single float
	;;; single could be Inf or NaN -- return false if so
	lwz	rt1, 0(rt0)		;;; single float in int reg
	rlwinm	rt1, rt1, 9, 0xff	;;; get exponent
	cmplwi	CR0, rt1, 0xff		;;; = 255?
	beq-	fpe_error		;;; return false if so
	stfd	ft0, 0(rt0)		;;; re-store as double
	blr


	;;; _pf_check_d(________dfaddr) -> ________dfaddr or false
	;;; check normal double at ________dfaddr

ASM_ALIGN_QUAD
DEF_C_LAB (_pf_check_d)
	;;; double could be Inf or NaN -- return false if so
	ldW	rt0, 0(rusp)		;;; ________dfaddr
	lwz	rt1, 0(rt0)		;;; part with exponent
	rlwinm	rt1, rt1, 12, 0x7ff	;;; get exponent
	cmplwi	CR0, rt1, 0x7ff		;;; = 2047?
	bnelr+				;;; return if not
	b	fpe_error		;;; else return false


	;;; _pf_round_d_to_s(________dfaddr) -> ________dfaddr or false
	;;; round double to single back into ________dfaddr

ASM_ALIGN_QUAD
DEF_C_LAB (_pf_round_d_to_s)
	ldW	rt0, 0(rusp)		;;; ________dfaddr
	lfd	ft0, 0(rt0)		;;; the double
	mtfsfi	0, 0			;;; clear FX in FPSCR
	frsp.	ft0, ft0		;;; round to single float
	stfs	ft0, 0(rt0)		;;; store single back in first word
	bflr+	CR1*4+0			;;; return unless FX set in CR1
	b	fpe_test_error



;;; --- GET/SET EXPONENT --------------------------------------------------

	;;; get and set exponent ___E of a double float, where ___E is
	;;; the number of bits needed for the integer part, e.g. 1 for 1.0,
	;;; 0 for 0.5, -1 for 0.05, etc.

	;;; _pf_expof(________dfaddr) -> ___E

ASM_ALIGN_QUAD
DEF_C_LAB (_pf_expof)
	ldW	rt0, 0(rusp)		;;; ________dfaddr
	lwz	rt1, 0(rt0)		;;; part with exponent
	rlwinm	rt1, rt1, 12, 0x7ff	;;; extract exponent
	addi	rt1, rt1, -_eEXCESS	;;; ___E = expo - _eEXCESS
	stW	rt1, 0(rusp)		;;; return it
	blr


	;;; ___E -> _pf_expof(________dfaddr) -> ________dfaddr or false
	;;; (false if ___E too big/small )

ASM_ALIGN_QUAD
DEF_C_LAB (-> _pf_expof)
	ldW	rt1, 0(rusp)		;;; ________dfaddr
	ldWu	rt0, _WOFFS(rusp)	;;; ___E
	lwz	rt2, 0(rt1)		;;; get first word of float
	addic.	rt0, rt0, _eEXCESS	;;; new exponent = ___E + _eEXCESS
	ble-	fpe_error		;;; underflow if 0 or neg
	cmplwi	CR0, rt0, 0x7ff		;;; too large?
	bgt-	fpe_error		;;; branch if so
	rlwimi	rt2, rt0, 32-12, 1, 11	;;; insert new exponent
	stW	rt1, 0(rusp)		;;; return ________dfaddr
	stw	rt2, 0(rt1)		;;; update float in mem
	blr



ASM_END_FILE
