res_func.s

RTEMS (Real-Time Executive for Multiprocessor Systems) is a free open source real-time operating sys

	andiw	#0x8000,%d0
	orw	#0x3fff,%d0	//force the exponent to +/- 1
	movew	%d0,FPTEMP_EX(%a6) //in the denorm
	movel	USER_FPCR(%a6),%d0
	andil	#0x30,%d0
	fmovel	%d0,%fpcr		//set up users rmode and X
	fmovex	FPTEMP(%a6),%fp0
	fsubx	ETEMP(%a6),%fp0
	fmovel	%fpsr,%d1
	orl	%d1,USER_FPSR(%a6) //capture cc's and inex from fadd
	leal	WBTEMP(%a6),%a0	//point a0 to wbtemp in frame
	fmovex	%fp0,WBTEMP(%a6)	//write result to memory
	lsrl	#4,%d0		//put rmode in lower 2 bits
	movel	USER_FPCR(%a6),%d1
	andil	#0xc0,%d1
	lsrl	#6,%d1		//put precision in upper word
	swap	%d1
	orl	%d0,%d1		//set up for round call
	clrl	%d0		//force sticky to zero
	bclrb	#sign_bit,WBTEMP_EX(%a6)
	sne	WBTEMP_SGN(%a6)
	bsrl	round		//round result to users rmode & prec
	bfclr	WBTEMP_SGN(%a6){#0:#8}	//convert back to IEEE ext format
	beq	frcfpnr
	bsetb	#sign_bit,WBTEMP_EX(%a6)
	bra	frcfpnr
sub_u_srcd:
	movew	ETEMP_EX(%a6),%d0
	andiw	#0x8000,%d0
	orw	#0x3fff,%d0	//force the exponent to +/- 1
	movew	%d0,ETEMP_EX(%a6) //in the denorm
	movel	USER_FPCR(%a6),%d0
	andil	#0x30,%d0
	fmovel	%d0,%fpcr		//set up users rmode and X
	fmovex	FPTEMP(%a6),%fp0
	fsubx	ETEMP(%a6),%fp0
	fmovel	%fpsr,%d1
	orl	%d1,USER_FPSR(%a6) //capture cc's and inex from fadd
	leal	WBTEMP(%a6),%a0	//point a0 to wbtemp in frame
	fmovex	%fp0,WBTEMP(%a6)	//write result to memory
	lsrl	#4,%d0		//put rmode in lower 2 bits
	movel	USER_FPCR(%a6),%d1
	andil	#0xc0,%d1
	lsrl	#6,%d1		//put precision in upper word
	swap	%d1
	orl	%d0,%d1		//set up for round call
	clrl	%d0		//force sticky to zero
	bclrb	#sign_bit,WBTEMP_EX(%a6)
	sne	WBTEMP_SGN(%a6)
	bsrl	round		//round result to users rmode & prec
	bfclr	WBTEMP_SGN(%a6){#0:#8}	//convert back to IEEE ext format
	beq	frcfpnr
	bsetb	#sign_bit,WBTEMP_EX(%a6)
	bra	frcfpnr
//
// Signs are unlike:
//
sub_diff:
	cmpb	#0x0f,DNRM_FLG(%a6) //is dest the denorm?
	bnes	sub_s_srcd
sub_s_destd:
	leal	ETEMP(%a6),%a0
	movel	USER_FPCR(%a6),%d0
	andil	#0x30,%d0
	lsrl	#4,%d0		//put rmode in lower 2 bits
	movel	USER_FPCR(%a6),%d1
	andil	#0xc0,%d1
	lsrl	#6,%d1		//put precision in upper word
	swap	%d1
	orl	%d0,%d1		//set up for round call
	movel	#0x20000000,%d0	//set sticky for round
//
// Since the dest is the denorm, the sign is the opposite of the
// norm sign.
//
	eoriw	#0x8000,ETEMP_EX(%a6)	//flip sign on result
	tstw	ETEMP_EX(%a6)
	bgts	sub_s_dwr
	orl	#neg_mask,USER_FPSR(%a6)
sub_s_dwr:
	bclrb	#sign_bit,ETEMP_EX(%a6)
	sne	ETEMP_SGN(%a6)
	bsrl	round		//round result to users rmode & prec
	bfclr	ETEMP_SGN(%a6){#0:#8}	//convert back to IEEE ext format
	beqs	sub_s_dclr
	bsetb	#sign_bit,ETEMP_EX(%a6)
sub_s_dclr:
	leal	WBTEMP(%a6),%a0
	movel	ETEMP(%a6),(%a0)	//write result to wbtemp
	movel	ETEMP_HI(%a6),4(%a0)
	movel	ETEMP_LO(%a6),8(%a0)
	bra	sub_ckovf
sub_s_srcd:
	leal	FPTEMP(%a6),%a0
	movel	USER_FPCR(%a6),%d0
	andil	#0x30,%d0
	lsrl	#4,%d0		//put rmode in lower 2 bits
	movel	USER_FPCR(%a6),%d1
	andil	#0xc0,%d1
	lsrl	#6,%d1		//put precision in upper word
	swap	%d1
	orl	%d0,%d1		//set up for round call
	movel	#0x20000000,%d0	//set sticky for round
	bclrb	#sign_bit,FPTEMP_EX(%a6)
	sne	FPTEMP_SGN(%a6)
	bsrl	round		//round result to users rmode & prec
	bfclr	FPTEMP_SGN(%a6){#0:#8}	//convert back to IEEE ext format
	beqs	sub_s_sclr
	bsetb	#sign_bit,FPTEMP_EX(%a6)
sub_s_sclr:
	leal	WBTEMP(%a6),%a0
	movel	FPTEMP(%a6),(%a0)	//write result to wbtemp
	movel	FPTEMP_HI(%a6),4(%a0)
	movel	FPTEMP_LO(%a6),8(%a0)
	tstw	FPTEMP_EX(%a6)
	bgt	sub_ckovf
	orl	#neg_mask,USER_FPSR(%a6)
sub_ckovf:
	movew	WBTEMP_EX(%a6),%d0
	andiw	#0x7fff,%d0
	cmpiw	#0x7fff,%d0
	bne	frcfpnr
//
// The result has overflowed to $7fff exponent.  Set I, ovfl,
// and aovfl, and clr the mantissa (incorrectly set by the
// round routine.)
//
	orl	#inf_mask+ovfl_inx_mask,USER_FPSR(%a6)	
	clrl	4(%a0)
	bra	frcfpnr
//
// Inst is fcmp.
//
wrap_cmp:
	cmpb	#0xff,DNRM_FLG(%a6) //if both ops denorm, 
	beq	fix_stk		 //restore to fpu
//
// One of the ops is denormalized.  Test for wrap condition
// and complete the instruction.
//
	cmpb	#0x0f,DNRM_FLG(%a6) //check for dest denorm
	bnes	cmp_srcd
cmp_destd:
	bsrl	ckinf_ns
	bne	fix_stk
	bfextu	ETEMP_EX(%a6){#1:#15},%d0	//get src exp (always pos)
	bfexts	FPTEMP_EX(%a6){#1:#15},%d1	//get dest exp (always neg)
	subl	%d1,%d0			//subtract dest from src
	cmpl	#0x8000,%d0
	blt	fix_stk			//if less, not wrap case
	tstw	ETEMP_EX(%a6)		//set N to ~sign_of(src)
	bge	cmp_setn
	rts
cmp_srcd:
	bsrl	ckinf_nd
	bne	fix_stk
	bfextu	FPTEMP_EX(%a6){#1:#15},%d0	//get dest exp (always pos)
	bfexts	ETEMP_EX(%a6){#1:#15},%d1	//get src exp (always neg)
	subl	%d1,%d0			//subtract src from dest
	cmpl	#0x8000,%d0
	blt	fix_stk			//if less, not wrap case
	tstw	FPTEMP_EX(%a6)		//set N to sign_of(dest)
	blt	cmp_setn
	rts
cmp_setn:
	orl	#neg_mask,USER_FPSR(%a6)
	rts

//
// Inst is fmul.
//
wrap_mul:
	cmpb	#0xff,DNRM_FLG(%a6) //if both ops denorm, 
	beq	force_unf	//force an underflow (really!)
//
// One of the ops is denormalized.  Test for wrap condition
// and complete the instruction.
//
	cmpb	#0x0f,DNRM_FLG(%a6) //check for dest denorm
	bnes	mul_srcd
mul_destd:
	bsrl	ckinf_ns
	bne	fix_stk
	bfextu	ETEMP_EX(%a6){#1:#15},%d0	//get src exp (always pos)
	bfexts	FPTEMP_EX(%a6){#1:#15},%d1	//get dest exp (always neg)
	addl	%d1,%d0			//subtract dest from src
	bgt	fix_stk
	bra	force_unf
mul_srcd:
	bsrl	ckinf_nd
	bne	fix_stk
	bfextu	FPTEMP_EX(%a6){#1:#15},%d0	//get dest exp (always pos)
	bfexts	ETEMP_EX(%a6){#1:#15},%d1	//get src exp (always neg)
	addl	%d1,%d0			//subtract src from dest
	bgt	fix_stk
	
//
// This code handles the case of the instruction resulting in 
// an underflow condition.
//
force_unf:
	bclrb	#E1,E_BYTE(%a6)
	orl	#unfinx_mask,USER_FPSR(%a6)
	clrw	NMNEXC(%a6)
	clrb	WBTEMP_SGN(%a6)
	movew	ETEMP_EX(%a6),%d0		//find the sign of the result
	movew	FPTEMP_EX(%a6),%d1
	eorw	%d1,%d0
	andiw	#0x8000,%d0
	beqs	frcunfcont
	st	WBTEMP_SGN(%a6)
frcunfcont:
	lea	WBTEMP(%a6),%a0		//point a0 to memory location
	movew	CMDREG1B(%a6),%d0
	btstl	#6,%d0			//test for forced precision
	beqs	frcunf_fpcr
	btstl	#2,%d0			//check for double
	bnes	frcunf_dbl
	movel	#0x1,%d0			//inst is forced single
	bras	frcunf_rnd
frcunf_dbl:
	movel	#0x2,%d0			//inst is forced double
	bras	frcunf_rnd
frcunf_fpcr:
	bfextu	FPCR_MODE(%a6){#0:#2},%d0	//inst not forced - use fpcr prec
frcunf_rnd:
	bsrl	unf_sub			//get correct result based on
//					;round precision/mode.  This 
//					;sets FPSR_CC correctly
	bfclr	WBTEMP_SGN(%a6){#0:#8}	//convert back to IEEE ext format
	beqs	frcfpn
	bsetb	#sign_bit,WBTEMP_EX(%a6)
	bra	frcfpn

//
// Write the result to the user's fpn.  All results must be HUGE to be
// written; otherwise the results would have overflowed or underflowed.
// If the rounding precision is single or double, the ovf_res routine
// is needed to correctly supply the max value.
//
frcfpnr:
	movew	CMDREG1B(%a6),%d0
	btstl	#6,%d0			//test for forced precision
	beqs	frcfpn_fpcr
	btstl	#2,%d0			//check for double
	bnes	frcfpn_dbl
	movel	#0x1,%d0			//inst is forced single
	bras	frcfpn_rnd
frcfpn_dbl:
	movel	#0x2,%d0			//inst is forced double
	bras	frcfpn_rnd
frcfpn_fpcr:
	bfextu	FPCR_MODE(%a6){#0:#2},%d0	//inst not forced - use fpcr prec
	tstb	%d0
	beqs	frcfpn			//if extended, write what you got
frcfpn_rnd:
	bclrb	#sign_bit,WBTEMP_EX(%a6)
	sne	WBTEMP_SGN(%a6)
	bsrl	ovf_res			//get correct result based on
//					;round precision/mode.  This 
//					;sets FPSR_CC correctly
	bfclr	WBTEMP_SGN(%a6){#0:#8}	//convert back to IEEE ext format
	beqs	frcfpn_clr
	bsetb	#sign_bit,WBTEMP_EX(%a6)
frcfpn_clr:
	orl	#ovfinx_mask,USER_FPSR(%a6)
// 
// Perform the write.
//
frcfpn:
	bfextu	CMDREG1B(%a6){#6:#3},%d0	//extract fp destination register
	cmpib	#3,%d0
	bles	frc0123			//check if dest is fp0-fp3
	movel	#7,%d1
	subl	%d0,%d1
	clrl	%d0
	bsetl	%d1,%d0
	fmovemx WBTEMP(%a6),%d0
	rts
frc0123:
	cmpib	#0,%d0
	beqs	frc0_dst
	cmpib	#1,%d0
	beqs	frc1_dst 
	cmpib	#2,%d0
	beqs	frc2_dst 
frc3_dst:
	movel	WBTEMP_EX(%a6),USER_FP3(%a6)
	movel	WBTEMP_HI(%a6),USER_FP3+4(%a6)
	movel	WBTEMP_LO(%a6),USER_FP3+8(%a6)
	rts
frc2_dst:
	movel	WBTEMP_EX(%a6),USER_FP2(%a6)
	movel	WBTEMP_HI(%a6),USER_FP2+4(%a6)
	movel	WBTEMP_LO(%a6),USER_FP2+8(%a6)
	rts
frc1_dst:
	movel	WBTEMP_EX(%a6),USER_FP1(%a6)
	movel	WBTEMP_HI(%a6),USER_FP1+4(%a6)
	movel	WBTEMP_LO(%a6),USER_FP1+8(%a6)
	rts
frc0_dst:
	movel	WBTEMP_EX(%a6),USER_FP0(%a6)
	movel	WBTEMP_HI(%a6),USER_FP0+4(%a6)
	movel	WBTEMP_LO(%a6),USER_FP0+8(%a6)
	rts

//
// Write etemp to fpn.
// A check is made on enabled and signalled snan exceptions,
// and the destination is not overwritten if this condition exists.
// This code is designed to make fmoveins of unsupported data types
// faster.
//
wr_etemp:
	btstb	#snan_bit,FPSR_EXCEPT(%a6)	//if snan is set, and
	beqs	fmoveinc		//enabled, force restore
	btstb	#snan_bit,FPCR_ENABLE(%a6) //and don't overwrite
	beqs	fmoveinc		//the dest
	movel	ETEMP_EX(%a6),FPTEMP_EX(%a6)	//set up fptemp sign for 
//						;snan handler
	tstb	ETEMP(%a6)		//check for negative
	blts	snan_neg
	rts
snan_neg:
	orl	#neg_bit,USER_FPSR(%a6)	//snan is negative; set N
	rts
fmoveinc:
	clrw	NMNEXC(%a6)
	bclrb	#E1,E_BYTE(%a6)
	moveb	STAG(%a6),%d0		//check if stag is inf
	andib	#0xe0,%d0
	cmpib	#0x40,%d0
	bnes	fminc_cnan
	orl	#inf_mask,USER_FPSR(%a6) //if inf, nothing yet has set I
	tstw	LOCAL_EX(%a0)		//check sign
	bges	fminc_con
	orl	#neg_mask,USER_FPSR(%a6)
	bra	fminc_con
fminc_cnan:
	cmpib	#0x60,%d0			//check if stag is NaN
	bnes	fminc_czero
	orl	#nan_mask,USER_FPSR(%a6) //if nan, nothing yet has set NaN
	movel	ETEMP_EX(%a6),FPTEMP_EX(%a6)	//set up fptemp sign for 
//						;snan handler
	tstw	LOCAL_EX(%a0)		//check sign
	bges	fminc_con
	orl	#neg_mask,USER_FPSR(%a6)
	bra	fminc_con
fminc_czero:
	cmpib	#0x20,%d0			//check if zero
	bnes	fminc_con
	orl	#z_mask,USER_FPSR(%a6)	//if zero, set Z
	tstw	LOCAL_EX(%a0)		//check sign
	bges	fminc_con
	orl	#neg_mask,USER_FPSR(%a6)
fminc_con:
	bfextu	CMDREG1B(%a6){#6:#3},%d0	//extract fp destination register
	cmpib	#3,%d0
	bles	fp0123			//check if dest is fp0-fp3
	movel	#7,%d1
	subl	%d0,%d1
	clrl	%d0
	bsetl	%d1,%d0
	fmovemx ETEMP(%a6),%d0
	rts

fp0123:
	cmpib	#0,%d0
	beqs	fp0_dst
	cmpib	#1,%d0
	beqs	fp1_dst 
	cmpib	#2,%d0
	beqs	fp2_dst 
fp3_dst:
	movel	ETEMP_EX(%a6),USER_FP3(%a6)
	movel	ETEMP_HI(%a6),USER_FP3+4(%a6)
	movel	ETEMP_LO(%a6),USER_FP3+8(%a6)
	rts
fp2_dst:
	movel	ETEMP_EX(%a6),USER_FP2(%a6)
	movel	ETEMP_HI(%a6),USER_FP2+4(%a6)
	movel	ETEMP_LO(%a6),USER_FP2+8(%a6)
	rts
fp1_dst:
	movel	ETEMP_EX(%a6),USER_FP1(%a6)
	movel	ETEMP_HI(%a6),USER_FP1+4(%a6)
	movel	ETEMP_LO(%a6),USER_FP1+8(%a6)
	rts
fp0_dst:
	movel	ETEMP_EX(%a6),USER_FP0(%a6)
	movel	ETEMP_HI(%a6),USER_FP0+4(%a6)
	movel	ETEMP_LO(%a6),USER_FP0+8(%a6)
	rts

opclass3:
	st	CU_ONLY(%a6)
	movew	CMDREG1B(%a6),%d0	//check if packed moveout
	andiw	#0x0c00,%d0	//isolate last 2 bits of size field
	cmpiw	#0x0c00,%d0	//if size is 011 or 111, it is packed
	beq	pack_out	//else it is norm or denorm
	bra	mv_out

	
//
//	MOVE OUT
//

mv_tbl:
	.long	li
	.long 	sgp
	.long 	xp
	.long 	mvout_end	//should never be taken
	.long 	wi
	.long 	dp
	.long 	bi
	.long 	mvout_end	//should never be taken
mv_out:
	bfextu	CMDREG1B(%a6){#3:#3},%d1	//put source specifier in d1
	leal	mv_tbl,%a0
	movel	%a0@(%d1:l:4),%a0
	jmp	(%a0)

//
// This exit is for move-out to memory.  The aunfl bit is 
// set if the result is inex and unfl is signalled.
//
mvout_end:
	btstb	#inex2_bit,FPSR_EXCEPT(%a6)
	beqs	no_aufl
	btstb	#unfl_bit,FPSR_EXCEPT(%a6)
	beqs	no_aufl
	bsetb	#aunfl_bit,FPSR_AEXCEPT(%a6)
no_aufl:
	clrw	NMNEXC(%a6)
	bclrb	#E1,E_BYTE(%a6)
	fmovel	#0,%FPSR			//clear any cc bits from res_func
//
// Return ETEMP to extended format from internal extended format so
// that gen_except will have a correctly signed value for ovfl/unfl
// handlers.
//
	bfclr	ETEMP_SGN(%a6){#0:#8}
	beqs	mvout_con
	bsetb	#sign_bit,ETEMP_EX(%a6)
mvout_con:
	rts
//
// This exit is for move-out to int register.  The aunfl bit is 
// not set in any case for this move.
//
mvouti_end:
	clrw	NMNEXC(%a6)
	bclrb	#E1,E_BYTE(%a6)
	fmovel	#0,%FPSR			//clear any cc bits from res_func
//
// Return ETEMP to extended format from internal extended format so
// that gen_except will have a correctly signed value for ovfl/unfl
// handlers.
//
	bfclr	ETEMP_SGN(%a6){#0:#8}
	beqs	mvouti_con
	bsetb	#sign_bit,ETEMP_EX(%a6)
mvouti_con:
	rts
//
// li is used to handle a long integer source specifier
//

li:
	moveql	#4,%d0		//set byte count

	btstb	#7,STAG(%a6)	//check for extended denorm
	bne	int_dnrm	//if so, branch

	fmovemx ETEMP(%a6),%fp0-%fp0
	fcmpd	#0x41dfffffffc00000,%fp0
// 41dfffffffc00000 in dbl prec = 401d0000fffffffe00000000 in ext prec
	fbge	lo_plrg	
	fcmpd	#0xc1e0000000000000,%fp0
// c1e0000000000000 in dbl prec = c01e00008000000000000000 in ext prec
	fble	lo_nlrg
//
// at this point, the answer is between the largest pos and neg values
//
	movel	USER_FPCR(%a6),%d1	//use user's rounding mode
	andil	#0x30,%d1
	fmovel	%d1,%fpcr
	fmovel	%fp0,L_SCR1(%a6)	//let the 040 perform conversion
	fmovel %fpsr,%d1
	orl	%d1,USER_FPSR(%a6)	//capture inex2/ainex if set
	bra	int_wrt


lo_plrg:
	movel	#0x7fffffff,L_SCR1(%a6)	//answer is largest positive int
	fbeq	int_wrt			//exact answer
	fcmpd	#0x41dfffffffe00000,%fp0
// 41dfffffffe00000 in dbl prec = 401d0000ffffffff00000000 in ext prec
	fbge	int_operr		//set operr
	bra	int_inx			//set inexact

lo_nlrg:
	movel	#0x80000000,L_SCR1(%a6)
	fbeq	int_wrt			//exact answer
	fcmpd	#0xc1e0000000100000,%fp0
// c1e0000000100000 in dbl prec = c01e00008000000080000000 in ext prec
	fblt	int_operr		//set operr
	bra	int_inx			//set inexact

//