res_func.s

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

//
//      $Id: res_func.S,v 1.1 1998/12/14 23:15:22 joel Exp $
//
//	res_func.sa 3.9 7/29/91
//
// Normalizes denormalized numbers if necessary and updates the
// stack frame.  The function is then restored back into the
// machine and the 040 completes the operation.  This routine
// is only used by the unsupported data type/format handler.
// (Exception vector 55).
//
// For packed move out (fmove.p fpm,<ea>) the operation is
// completed here; data is packed and moved to user memory. 
// The stack is restored to the 040 only in the case of a
// reportable exception in the conversion.
//
//
//		Copyright (C) Motorola, Inc. 1990
//			All Rights Reserved
//
//	THIS IS UNPUBLISHED PROPRIETARY SOURCE CODE OF MOTOROLA 
//	The copyright notice above does not evidence any  
//	actual or intended publication of such source code.

RES_FUNC:    //idnt    2,1 | Motorola 040 Floating Point Software Package

	|section	8

#include "fpsp.defs"

sp_bnds:	.short	0x3f81,0x407e
		.short	0x3f6a,0x0000
dp_bnds:	.short	0x3c01,0x43fe
		.short	0x3bcd,0x0000

	|xref	mem_write
	|xref	bindec
	|xref	get_fline
	|xref	round
	|xref	denorm
	|xref	dest_ext
	|xref	dest_dbl
	|xref	dest_sgl
	|xref	unf_sub
	|xref	nrm_set
	|xref	dnrm_lp
	|xref	ovf_res
	|xref	reg_dest
	|xref	t_ovfl
	|xref	t_unfl

	.global	res_func
	.global 	p_move

res_func:
	clrb	DNRM_FLG(%a6)
	clrb	RES_FLG(%a6)
	clrb	CU_ONLY(%a6)
	tstb	DY_MO_FLG(%a6)
	beqs	monadic
dyadic:
	btstb	#7,DTAG(%a6)	//if dop = norm=000, zero=001,
//				;inf=010 or nan=011
	beqs	monadic		//then branch
//				;else denorm
// HANDLE DESTINATION DENORM HERE
//				;set dtag to norm
//				;write the tag & fpte15 to the fstack
	leal	FPTEMP(%a6),%a0

	bclrb	#sign_bit,LOCAL_EX(%a0)
	sne	LOCAL_SGN(%a0)

	bsr	nrm_set		//normalize number (exp will go negative)
	bclrb	#sign_bit,LOCAL_EX(%a0) //get rid of false sign
	bfclr	LOCAL_SGN(%a0){#0:#8}	//change back to IEEE ext format
	beqs	dpos
	bsetb	#sign_bit,LOCAL_EX(%a0)
dpos:
	bfclr	DTAG(%a6){#0:#4}	//set tag to normalized, FPTE15 = 0
	bsetb	#4,DTAG(%a6)	//set FPTE15
	orb	#0x0f,DNRM_FLG(%a6)
monadic:
	leal	ETEMP(%a6),%a0
	btstb	#direction_bit,CMDREG1B(%a6)	//check direction
	bne	opclass3			//it is a mv out
//
// At this point, only opclass 0 and 2 possible
//
	btstb	#7,STAG(%a6)	//if sop = norm=000, zero=001,
//				;inf=010 or nan=011
	bne	mon_dnrm	//else denorm
	tstb	DY_MO_FLG(%a6)	//all cases of dyadic instructions would
	bne	normal		//require normalization of denorm

// At this point:
//	monadic instructions:	fabs  = $18  fneg   = $1a  ftst   = $3a
//				fmove = $00  fsmove = $40  fdmove = $44
//				fsqrt = $05* fssqrt = $41  fdsqrt = $45
//				(*fsqrt reencoded to $05)
//
	movew	CMDREG1B(%a6),%d0	//get command register
	andil	#0x7f,%d0			//strip to only command word
//
// At this point, fabs, fneg, fsmove, fdmove, ftst, fsqrt, fssqrt, and 
// fdsqrt are possible.
// For cases fabs, fneg, fsmove, and fdmove goto spos (do not normalize)
// For cases fsqrt, fssqrt, and fdsqrt goto nrm_src (do normalize)
//
	btstl	#0,%d0
	bne	normal			//weed out fsqrt instructions
//
// cu_norm handles fmove in instructions with normalized inputs.
// The routine round is used to correctly round the input for the
// destination precision and mode.
//
cu_norm:
	st	CU_ONLY(%a6)		//set cu-only inst flag
	movew	CMDREG1B(%a6),%d0
	andib	#0x3b,%d0		//isolate bits to select inst
	tstb	%d0
	beql	cu_nmove	//if zero, it is an fmove
	cmpib	#0x18,%d0
	beql	cu_nabs		//if $18, it is fabs
	cmpib	#0x1a,%d0
	beql	cu_nneg		//if $1a, it is fneg
//
// Inst is ftst.  Check the source operand and set the cc's accordingly.
// No write is done, so simply rts.
//
cu_ntst:
	movew	LOCAL_EX(%a0),%d0
	bclrl	#15,%d0
	sne	LOCAL_SGN(%a0)
	beqs	cu_ntpo
	orl	#neg_mask,USER_FPSR(%a6) //set N
cu_ntpo:
	cmpiw	#0x7fff,%d0	//test for inf/nan
	bnes	cu_ntcz
	tstl	LOCAL_HI(%a0)
	bnes	cu_ntn
	tstl	LOCAL_LO(%a0)
	bnes	cu_ntn
	orl	#inf_mask,USER_FPSR(%a6)
	rts
cu_ntn:
	orl	#nan_mask,USER_FPSR(%a6)
	movel	ETEMP_EX(%a6),FPTEMP_EX(%a6)	//set up fptemp sign for 
//						;snan handler

	rts
cu_ntcz:
	tstl	LOCAL_HI(%a0)
	bnel	cu_ntsx
	tstl	LOCAL_LO(%a0)
	bnel	cu_ntsx
	orl	#z_mask,USER_FPSR(%a6)
cu_ntsx:
	rts
//
// Inst is fabs.  Execute the absolute value function on the input.
// Branch to the fmove code.  If the operand is NaN, do nothing.
//
cu_nabs:
	moveb	STAG(%a6),%d0
	btstl	#5,%d0			//test for NaN or zero
	bne	wr_etemp		//if either, simply write it
	bclrb	#7,LOCAL_EX(%a0)		//do abs
	bras	cu_nmove		//fmove code will finish
//
// Inst is fneg.  Execute the negate value function on the input.
// Fall though to the fmove code.  If the operand is NaN, do nothing.
//
cu_nneg:
	moveb	STAG(%a6),%d0
	btstl	#5,%d0			//test for NaN or zero
	bne	wr_etemp		//if either, simply write it
	bchgb	#7,LOCAL_EX(%a0)		//do neg
//
// Inst is fmove.  This code also handles all result writes.
// If bit 2 is set, round is forced to double.  If it is clear,
// and bit 6 is set, round is forced to single.  If both are clear,
// the round precision is found in the fpcr.  If the rounding precision
// is double or single, round the result before the write.
//
cu_nmove:
	moveb	STAG(%a6),%d0
	andib	#0xe0,%d0			//isolate stag bits
	bne	wr_etemp		//if not norm, simply write it
	btstb	#2,CMDREG1B+1(%a6)	//check for rd
	bne	cu_nmrd
	btstb	#6,CMDREG1B+1(%a6)	//check for rs
	bne	cu_nmrs
//
// The move or operation is not with forced precision.  Test for
// nan or inf as the input; if so, simply write it to FPn.  Use the
// FPCR_MODE byte to get rounding on norms and zeros.
//
cu_nmnr:
	bfextu	FPCR_MODE(%a6){#0:#2},%d0
	tstb	%d0			//check for extended
	beq	cu_wrexn		//if so, just write result
	cmpib	#1,%d0			//check for single
	beq	cu_nmrs			//fall through to double
//
// The move is fdmove or round precision is double.
//
cu_nmrd:
	movel	#2,%d0			//set up the size for denorm
	movew	LOCAL_EX(%a0),%d1		//compare exponent to double threshold
	andw	#0x7fff,%d1	
	cmpw	#0x3c01,%d1
	bls	cu_nunfl
	bfextu	FPCR_MODE(%a6){#2:#2},%d1	//get rmode
	orl	#0x00020000,%d1		//or in rprec (double)
	clrl	%d0			//clear g,r,s for round
	bclrb	#sign_bit,LOCAL_EX(%a0)	//convert to internal format
	sne	LOCAL_SGN(%a0)
	bsrl	round
	bfclr	LOCAL_SGN(%a0){#0:#8}
	beqs	cu_nmrdc
	bsetb	#sign_bit,LOCAL_EX(%a0)
cu_nmrdc:
	movew	LOCAL_EX(%a0),%d1		//check for overflow
	andw	#0x7fff,%d1
	cmpw	#0x43ff,%d1
	bge	cu_novfl		//take care of overflow case
	bra	cu_wrexn
//
// The move is fsmove or round precision is single.
//
cu_nmrs:
	movel	#1,%d0
	movew	LOCAL_EX(%a0),%d1
	andw	#0x7fff,%d1
	cmpw	#0x3f81,%d1
	bls	cu_nunfl
	bfextu	FPCR_MODE(%a6){#2:#2},%d1
	orl	#0x00010000,%d1
	clrl	%d0
	bclrb	#sign_bit,LOCAL_EX(%a0)
	sne	LOCAL_SGN(%a0)
	bsrl	round
	bfclr	LOCAL_SGN(%a0){#0:#8}
	beqs	cu_nmrsc
	bsetb	#sign_bit,LOCAL_EX(%a0)
cu_nmrsc:
	movew	LOCAL_EX(%a0),%d1
	andw	#0x7FFF,%d1
	cmpw	#0x407f,%d1
	blt	cu_wrexn
//
// The operand is above precision boundaries.  Use t_ovfl to
// generate the correct value.
//
cu_novfl:
	bsr	t_ovfl
	bra	cu_wrexn
//
// The operand is below precision boundaries.  Use denorm to
// generate the correct value.
//
cu_nunfl:
	bclrb	#sign_bit,LOCAL_EX(%a0)
	sne	LOCAL_SGN(%a0)
	bsr	denorm
	bfclr	LOCAL_SGN(%a0){#0:#8}	//change back to IEEE ext format
	beqs	cu_nucont
	bsetb	#sign_bit,LOCAL_EX(%a0)
cu_nucont:
	bfextu	FPCR_MODE(%a6){#2:#2},%d1
	btstb	#2,CMDREG1B+1(%a6)	//check for rd
	bne	inst_d
	btstb	#6,CMDREG1B+1(%a6)	//check for rs
	bne	inst_s
	swap	%d1
	moveb	FPCR_MODE(%a6),%d1
	lsrb	#6,%d1
	swap	%d1
	bra	inst_sd
inst_d:
	orl	#0x00020000,%d1
	bra	inst_sd
inst_s:
	orl	#0x00010000,%d1
inst_sd:
	bclrb	#sign_bit,LOCAL_EX(%a0)
	sne	LOCAL_SGN(%a0)
	bsrl	round
	bfclr	LOCAL_SGN(%a0){#0:#8}
	beqs	cu_nuflp
	bsetb	#sign_bit,LOCAL_EX(%a0)
cu_nuflp:
	btstb	#inex2_bit,FPSR_EXCEPT(%a6)
	beqs	cu_nuninx
	orl	#aunfl_mask,USER_FPSR(%a6) //if the round was inex, set AUNFL
cu_nuninx:
	tstl	LOCAL_HI(%a0)		//test for zero
	bnes	cu_nunzro
	tstl	LOCAL_LO(%a0)
	bnes	cu_nunzro
//
// The mantissa is zero from the denorm loop.  Check sign and rmode
// to see if rounding should have occurred which would leave the lsb.
//
	movel	USER_FPCR(%a6),%d0
	andil	#0x30,%d0		//isolate rmode
	cmpil	#0x20,%d0
	blts	cu_nzro
	bnes	cu_nrp
cu_nrm:
	tstw	LOCAL_EX(%a0)	//if positive, set lsb
	bges	cu_nzro
	btstb	#7,FPCR_MODE(%a6) //check for double
	beqs	cu_nincs
	bras	cu_nincd
cu_nrp:
	tstw	LOCAL_EX(%a0)	//if positive, set lsb
	blts	cu_nzro
	btstb	#7,FPCR_MODE(%a6) //check for double
	beqs	cu_nincs
cu_nincd:
	orl	#0x800,LOCAL_LO(%a0) //inc for double
	bra	cu_nunzro
cu_nincs:
	orl	#0x100,LOCAL_HI(%a0) //inc for single
	bra	cu_nunzro
cu_nzro:
	orl	#z_mask,USER_FPSR(%a6)
	moveb	STAG(%a6),%d0
	andib	#0xe0,%d0
	cmpib	#0x40,%d0		//check if input was tagged zero
	beqs	cu_numv
cu_nunzro:
	orl	#unfl_mask,USER_FPSR(%a6) //set unfl
cu_numv:
	movel	(%a0),ETEMP(%a6)
	movel	4(%a0),ETEMP_HI(%a6)
	movel	8(%a0),ETEMP_LO(%a6)
//
// Write the result to memory, setting the fpsr cc bits.  NaN and Inf
// bypass cu_wrexn.
//
cu_wrexn:
	tstw	LOCAL_EX(%a0)		//test for zero
	beqs	cu_wrzero
	cmpw	#0x8000,LOCAL_EX(%a0)	//test for zero
	bnes	cu_wreon
cu_wrzero:
	orl	#z_mask,USER_FPSR(%a6)	//set Z bit
cu_wreon:
	tstw	LOCAL_EX(%a0)
	bpl	wr_etemp
	orl	#neg_mask,USER_FPSR(%a6)
	bra	wr_etemp

//
// HANDLE SOURCE DENORM HERE
//
//				;clear denorm stag to norm
//				;write the new tag & ete15 to the fstack
mon_dnrm:
//
// At this point, check for the cases in which normalizing the 
// denorm produces incorrect results.
//
	tstb	DY_MO_FLG(%a6)	//all cases of dyadic instructions would
	bnes	nrm_src		//require normalization of denorm

// At this point:
//	monadic instructions:	fabs  = $18  fneg   = $1a  ftst   = $3a
//				fmove = $00  fsmove = $40  fdmove = $44
//				fsqrt = $05* fssqrt = $41  fdsqrt = $45
//				(*fsqrt reencoded to $05)
//
	movew	CMDREG1B(%a6),%d0	//get command register
	andil	#0x7f,%d0			//strip to only command word
//
// At this point, fabs, fneg, fsmove, fdmove, ftst, fsqrt, fssqrt, and 
// fdsqrt are possible.
// For cases fabs, fneg, fsmove, and fdmove goto spos (do not normalize)
// For cases fsqrt, fssqrt, and fdsqrt goto nrm_src (do normalize)
//
	btstl	#0,%d0
	bnes	nrm_src		//weed out fsqrt instructions
	st	CU_ONLY(%a6)	//set cu-only inst flag
	bra	cu_dnrm		//fmove, fabs, fneg, ftst 
//				;cases go to cu_dnrm
nrm_src:
	bclrb	#sign_bit,LOCAL_EX(%a0)
	sne	LOCAL_SGN(%a0)
	bsr	nrm_set		//normalize number (exponent will go 
//				; negative)
	bclrb	#sign_bit,LOCAL_EX(%a0) //get rid of false sign

	bfclr	LOCAL_SGN(%a0){#0:#8}	//change back to IEEE ext format
	beqs	spos
	bsetb	#sign_bit,LOCAL_EX(%a0)
spos:
	bfclr	STAG(%a6){#0:#4}	//set tag to normalized, FPTE15 = 0
	bsetb	#4,STAG(%a6)	//set ETE15
	orb	#0xf0,DNRM_FLG(%a6)
normal:
	tstb	DNRM_FLG(%a6)	//check if any of the ops were denorms
	bne	ck_wrap		//if so, check if it is a potential
//				;wrap-around case
fix_stk:
	moveb	#0xfe,CU_SAVEPC(%a6)
	bclrb	#E1,E_BYTE(%a6)

	clrw	NMNEXC(%a6)

	st	RES_FLG(%a6)	//indicate that a restore is needed
	rts

//
// cu_dnrm handles all cu-only instructions (fmove, fabs, fneg, and
// ftst) completely in software without an frestore to the 040. 
//
cu_dnrm:
	st	CU_ONLY(%a6)
	movew	CMDREG1B(%a6),%d0
	andib	#0x3b,%d0		//isolate bits to select inst
	tstb	%d0
	beql	cu_dmove	//if zero, it is an fmove
	cmpib	#0x18,%d0
	beql	cu_dabs		//if $18, it is fabs
	cmpib	#0x1a,%d0
	beql	cu_dneg		//if $1a, it is fneg
//
// Inst is ftst.  Check the source operand and set the cc's accordingly.
// No write is done, so simply rts.
//
cu_dtst:
	movew	LOCAL_EX(%a0),%d0
	bclrl	#15,%d0
	sne	LOCAL_SGN(%a0)
	beqs	cu_dtpo
	orl	#neg_mask,USER_FPSR(%a6) //set N
cu_dtpo:
	cmpiw	#0x7fff,%d0	//test for inf/nan
	bnes	cu_dtcz
	tstl	LOCAL_HI(%a0)
	bnes	cu_dtn
	tstl	LOCAL_LO(%a0)
	bnes	cu_dtn
	orl	#inf_mask,USER_FPSR(%a6)
	rts
cu_dtn:
	orl	#nan_mask,USER_FPSR(%a6)
	movel	ETEMP_EX(%a6),FPTEMP_EX(%a6)	//set up fptemp sign for 
//						;snan handler
	rts
cu_dtcz:
	tstl	LOCAL_HI(%a0)
	bnel	cu_dtsx
	tstl	LOCAL_LO(%a0)
	bnel	cu_dtsx
	orl	#z_mask,USER_FPSR(%a6)
cu_dtsx:
	rts
//
// Inst is fabs.  Execute the absolute value function on the input.
// Branch to the fmove code.
//
cu_dabs:
	bclrb	#7,LOCAL_EX(%a0)		//do abs
	bras	cu_dmove		//fmove code will finish
//
// Inst is fneg.  Execute the negate value function on the input.
// Fall though to the fmove code.
//
cu_dneg:
	bchgb	#7,LOCAL_EX(%a0)		//do neg
//
// Inst is fmove.  This code also handles all result writes.
// If bit 2 is set, round is forced to double.  If it is clear,
// and bit 6 is set, round is forced to single.  If both are clear,
// the round precision is found in the fpcr.  If the rounding precision
// is double or single, the result is zero, and the mode is checked
// to determine if the lsb of the result should be set.
//
cu_dmove:
	btstb	#2,CMDREG1B+1(%a6)	//check for rd
	bne	cu_dmrd
	btstb	#6,CMDREG1B+1(%a6)	//check for rs
	bne	cu_dmrs
//
// The move or operation is not with forced precision.  Use the
// FPCR_MODE byte to get rounding.
//
cu_dmnr:
	bfextu	FPCR_MODE(%a6){#0:#2},%d0
	tstb	%d0			//check for extended
	beq	cu_wrexd		//if so, just write result
	cmpib	#1,%d0			//check for single
	beq	cu_dmrs			//fall through to double
//
// The move is fdmove or round precision is double.  Result is zero.
// Check rmode for rp or rm and set lsb accordingly.
//
cu_dmrd:
	bfextu	FPCR_MODE(%a6){#2:#2},%d1	//get rmode
	tstw	LOCAL_EX(%a0)		//check sign
	blts	cu_dmdn
	cmpib	#3,%d1			//check for rp
	bne	cu_dpd			//load double pos zero
	bra	cu_dpdr			//load double pos zero w/lsb
cu_dmdn:
	cmpib	#2,%d1			//check for rm
	bne	cu_dnd			//load double neg zero