res_func.s

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

	bra	cu_dndr			//load double neg zero w/lsb
//
// The move is fsmove or round precision is single.  Result is zero.
// Check for rp or rm and set lsb accordingly.
//
cu_dmrs:
	bfextu	FPCR_MODE(%a6){#2:#2},%d1	//get rmode
	tstw	LOCAL_EX(%a0)		//check sign
	blts	cu_dmsn
	cmpib	#3,%d1			//check for rp
	bne	cu_spd			//load single pos zero
	bra	cu_spdr			//load single pos zero w/lsb
cu_dmsn:
	cmpib	#2,%d1			//check for rm
	bne	cu_snd			//load single neg zero
	bra	cu_sndr			//load single neg zero w/lsb
//
// The precision is extended, so the result in etemp is correct.
// Simply set unfl (not inex2 or aunfl) and write the result to 
// the correct fp register.
cu_wrexd:
	orl	#unfl_mask,USER_FPSR(%a6)
	tstw	LOCAL_EX(%a0)
	beq	wr_etemp
	orl	#neg_mask,USER_FPSR(%a6)
	bra	wr_etemp
//
// These routines write +/- zero in double format.  The routines
// cu_dpdr and cu_dndr set the double lsb.
//
cu_dpd:
	movel	#0x3c010000,LOCAL_EX(%a0)	//force pos double zero
	clrl	LOCAL_HI(%a0)
	clrl	LOCAL_LO(%a0)
	orl	#z_mask,USER_FPSR(%a6)
	orl	#unfinx_mask,USER_FPSR(%a6)
	bra	wr_etemp
cu_dpdr:
	movel	#0x3c010000,LOCAL_EX(%a0)	//force pos double zero
	clrl	LOCAL_HI(%a0)
	movel	#0x800,LOCAL_LO(%a0)	//with lsb set
	orl	#unfinx_mask,USER_FPSR(%a6)
	bra	wr_etemp
cu_dnd:
	movel	#0xbc010000,LOCAL_EX(%a0)	//force pos double zero
	clrl	LOCAL_HI(%a0)
	clrl	LOCAL_LO(%a0)
	orl	#z_mask,USER_FPSR(%a6)
	orl	#neg_mask,USER_FPSR(%a6)
	orl	#unfinx_mask,USER_FPSR(%a6)
	bra	wr_etemp
cu_dndr:
	movel	#0xbc010000,LOCAL_EX(%a0)	//force pos double zero
	clrl	LOCAL_HI(%a0)
	movel	#0x800,LOCAL_LO(%a0)	//with lsb set
	orl	#neg_mask,USER_FPSR(%a6)
	orl	#unfinx_mask,USER_FPSR(%a6)
	bra	wr_etemp
//
// These routines write +/- zero in single format.  The routines
// cu_dpdr and cu_dndr set the single lsb.
//
cu_spd:
	movel	#0x3f810000,LOCAL_EX(%a0)	//force pos single zero
	clrl	LOCAL_HI(%a0)
	clrl	LOCAL_LO(%a0)
	orl	#z_mask,USER_FPSR(%a6)
	orl	#unfinx_mask,USER_FPSR(%a6)
	bra	wr_etemp
cu_spdr:
	movel	#0x3f810000,LOCAL_EX(%a0)	//force pos single zero
	movel	#0x100,LOCAL_HI(%a0)	//with lsb set
	clrl	LOCAL_LO(%a0)
	orl	#unfinx_mask,USER_FPSR(%a6)
	bra	wr_etemp
cu_snd:
	movel	#0xbf810000,LOCAL_EX(%a0)	//force pos single zero
	clrl	LOCAL_HI(%a0)
	clrl	LOCAL_LO(%a0)
	orl	#z_mask,USER_FPSR(%a6)
	orl	#neg_mask,USER_FPSR(%a6)
	orl	#unfinx_mask,USER_FPSR(%a6)
	bra	wr_etemp
cu_sndr:
	movel	#0xbf810000,LOCAL_EX(%a0)	//force pos single zero
	movel	#0x100,LOCAL_HI(%a0)	//with lsb set
	clrl	LOCAL_LO(%a0)
	orl	#neg_mask,USER_FPSR(%a6)
	orl	#unfinx_mask,USER_FPSR(%a6)
	bra	wr_etemp
	
//
// This code checks for 16-bit overflow conditions on dyadic
// operations which are not restorable into the floating-point
// unit and must be completed in software.  Basically, this
// condition exists with a very large norm and a denorm.  One
// of the operands must be denormalized to enter this code.
//
// Flags used:
//	DY_MO_FLG contains 0 for monadic op, $ff for dyadic
//	DNRM_FLG contains $00 for neither op denormalized
//	                  $0f for the destination op denormalized
//	                  $f0 for the source op denormalized
//	                  $ff for both ops denormalized
//
// The wrap-around condition occurs for add, sub, div, and cmp
// when 
//
//	abs(dest_exp - src_exp) >= $8000
//
// and for mul when
//
//	(dest_exp + src_exp) < $0
//
// we must process the operation here if this case is true.
//
// The rts following the frcfpn routine is the exit from res_func
// for this condition.  The restore flag (RES_FLG) is left clear.
// No frestore is done unless an exception is to be reported.
//
// For fadd: 
//	if(sign_of(dest) != sign_of(src))
//		replace exponent of src with $3fff (keep sign)
//		use fpu to perform dest+new_src (user's rmode and X)
//		clr sticky
//	else
//		set sticky
//	call round with user's precision and mode
//	move result to fpn and wbtemp
//
// For fsub:
//	if(sign_of(dest) == sign_of(src))
//		replace exponent of src with $3fff (keep sign)
//		use fpu to perform dest+new_src (user's rmode and X)
//		clr sticky
//	else
//		set sticky
//	call round with user's precision and mode
//	move result to fpn and wbtemp
//
// For fdiv/fsgldiv:
//	if(both operands are denorm)
//		restore_to_fpu;
//	if(dest is norm)
//		force_ovf;
//	else(dest is denorm)
//		force_unf:
//
// For fcmp:
//	if(dest is norm)
//		N = sign_of(dest);
//	else(dest is denorm)
//		N = sign_of(src);
//
// For fmul:
//	if(both operands are denorm)
//		force_unf;
//	if((dest_exp + src_exp) < 0)
//		force_unf:
//	else
//		restore_to_fpu;
//
// local equates:
	.set	addcode,0x22
	.set	subcode,0x28
	.set	mulcode,0x23
	.set	divcode,0x20
	.set	cmpcode,0x38
ck_wrap:
	| tstb	DY_MO_FLG(%a6)	;check for fsqrt
	beq	fix_stk		//if zero, it is fsqrt
	movew	CMDREG1B(%a6),%d0
	andiw	#0x3b,%d0		//strip to command bits
	cmpiw	#addcode,%d0
	beq	wrap_add
	cmpiw	#subcode,%d0
	beq	wrap_sub
	cmpiw	#mulcode,%d0
	beq	wrap_mul
	cmpiw	#cmpcode,%d0
	beq	wrap_cmp
//
// Inst is fdiv.  
//
wrap_div:
	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 force the result.
//
	cmpb	#0x0f,DNRM_FLG(%a6) //check for dest denorm
	bnes	div_srcd
div_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	#0x7fff,%d0
	blt	fix_stk			//if less, not wrap case
	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
	beq	force_unf
	st	WBTEMP_SGN(%a6)
	bra	force_unf

ckinf_ns:
	moveb	STAG(%a6),%d0		//check source tag for inf or nan
	bra	ck_in_com
ckinf_nd:
	moveb	DTAG(%a6),%d0		//check destination tag for inf or nan
ck_in_com:	
	andib	#0x60,%d0			//isolate tag bits
	cmpb	#0x40,%d0			//is it inf?
	beq	nan_or_inf		//not wrap case
	cmpb	#0x60,%d0			//is it nan?
	beq	nan_or_inf		//yes, not wrap case?
	cmpb	#0x20,%d0			//is it a zero?
	beq	nan_or_inf		//yes
	clrl	%d0
	rts				//then ; it is either a zero of norm,
//					;check wrap case
nan_or_inf:
	moveql	#-1,%d0
	rts



div_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
	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	force_ovf
	st	WBTEMP_SGN(%a6)
//
// This code handles the case of the instruction resulting in 
// an overflow condition.
//
force_ovf:
	bclrb	#E1,E_BYTE(%a6)
	orl	#ovfl_inx_mask,USER_FPSR(%a6)
	clrw	NMNEXC(%a6)
	leal	WBTEMP(%a6),%a0		//point a0 to memory location
	movew	CMDREG1B(%a6),%d0
	btstl	#6,%d0			//test for forced precision
	beqs	frcovf_fpcr
	btstl	#2,%d0			//check for double
	bnes	frcovf_dbl
	movel	#0x1,%d0			//inst is forced single
	bras	frcovf_rnd
frcovf_dbl:
	movel	#0x2,%d0			//inst is forced double
	bras	frcovf_rnd
frcovf_fpcr:
	bfextu	FPCR_MODE(%a6){#0:#2},%d0	//inst not forced - use fpcr prec
frcovf_rnd:

// The 881/882 does not set inex2 for the following case, so the 
// line is commented out to be compatible with 881/882
//	tst.b	%d0
//	beq.b	frcovf_x
//	or.l	#inex2_mask,USER_FPSR(%a6) ;if prec is s or d, set inex2

//frcovf_x:
	bsrl	ovf_res			//get correct result based on
//					;round precision/mode.  This 
//					;sets FPSR_CC correctly
//					;returns in external format
	bfclr	WBTEMP_SGN(%a6){#0:#8}
	beq	frcfpn
	bsetb	#sign_bit,WBTEMP_EX(%a6)
	bra	frcfpn
//
// Inst is fadd.
//
wrap_add:
	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	add_srcd
add_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
	bra	add_wrap
add_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
//
// Check the signs of the operands.  If they are unlike, the fpu
// can be used to add the norm and 1.0 with the sign of the
// denorm and it will correctly generate the result in extended
// precision.  We can then call round with no sticky and the result
// will be correct for the user's rounding mode and precision.  If
// the signs are the same, we call round with the sticky bit set
// and the result will be correct for the user's rounding mode and
// precision.
//
add_wrap:
	movew	ETEMP_EX(%a6),%d0
	movew	FPTEMP_EX(%a6),%d1
	eorw	%d1,%d0
	andiw	#0x8000,%d0
	beq	add_same
//
// The signs are unlike.
//
	cmpb	#0x0f,DNRM_FLG(%a6) //is dest the denorm?
	bnes	add_u_srcd
	movew	FPTEMP_EX(%a6),%d0
	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	ETEMP(%a6),%fp0
	faddx	FPTEMP(%a6),%fp0
	leal	WBTEMP(%a6),%a0	//point a0 to wbtemp in frame
	fmovel	%fpsr,%d1
	orl	%d1,USER_FPSR(%a6) //capture cc's and inex from fadd
	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
add_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	ETEMP(%a6),%fp0
	faddx	FPTEMP(%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)	//use internal format for round
	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 alike:
//
add_same:
	cmpb	#0x0f,DNRM_FLG(%a6) //is dest the denorm?
	bnes	add_s_srcd
add_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
	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	add_s_dclr
	bsetb	#sign_bit,ETEMP_EX(%a6)
add_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)
	tstw	ETEMP_EX(%a6)
	bgt	add_ckovf
	orl	#neg_mask,USER_FPSR(%a6)
	bra	add_ckovf
add_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	add_s_sclr
	bsetb	#sign_bit,FPTEMP_EX(%a6)
add_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	add_ckovf
	orl	#neg_mask,USER_FPSR(%a6)
add_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 fsub.
//
wrap_sub:
	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	sub_srcd
sub_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 src from dest
	cmpl	#0x8000,%d0
	blt	fix_stk			//if less, not wrap case
	bra	sub_wrap
sub_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 dest from src
	cmpl	#0x8000,%d0
	blt	fix_stk			//if less, not wrap case
//
// Check the signs of the operands.  If they are alike, the fpu
// can be used to subtract from the norm 1.0 with the sign of the
// denorm and it will correctly generate the result in extended
// precision.  We can then call round with no sticky and the result
// will be correct for the user's rounding mode and precision.  If
// the signs are unlike, we call round with the sticky bit set
// and the result will be correct for the user's rounding mode and
// precision.
//
sub_wrap:
	movew	ETEMP_EX(%a6),%d0
	movew	FPTEMP_EX(%a6),%d1
	eorw	%d1,%d0
	andiw	#0x8000,%d0
	bne	sub_diff
//
// The signs are alike.
//
	cmpb	#0x0f,DNRM_FLG(%a6) //is dest the denorm?
	bnes	sub_u_srcd
	movew	FPTEMP_EX(%a6),%d0