util.s

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

	andil	#0x00440000,%d0	//clear all bits except bits 6 and 2
	cmpil	#0x00400000,%d0
	beql	unff_sgl	//force single
	cmpil	#0x00440000,%d0	//force double
	beql	unff_dbl
	movel	CMDREG1B(%a6),%d0	//get the command word again
	andil	#0x007f0000,%d0	//clear all bits except the operation
	cmpil	#0x00270000,%d0
	beql	unf_fsgl	//fsglmul
	cmpil	#0x00240000,%d0
	beql	unf_fsgl	//fsgldiv
	bra	unf_fpcr

//
// Convert to return format.  The values from cmdreg3b and the return
// values are:
//	cmdreg3b	return	     precision
//	--------	------	     ---------
//	  00,01		  0		ext
//	   10		  1		sgl
//	   11		  2		dbl
// Force single
//
unff_sgl:
	movel	#1,%d0		//return 1
	rts
//
// Force double
//
unff_dbl:
	movel	#2,%d0		//return 2
	rts
//
// Force extended
//
unf_fsgl:
	movel	#0,%d0		
	rts
//
// Get rounding precision set in FPCR{7:6}.
//
unf_fpcr:
	movel	USER_FPCR(%a6),%d0 //rounding precision bits in d0{7:6}
	bfextu	%d0{#24:#2},%d0	//move the rounding prec bits to d0{1:0}
	rts
//
//	g_opcls --- put opclass in d0{2:0}
//
g_opcls:
	btstb	#E3,E_BYTE(%a6)
	beqs	opc_1b		//if set, go to cmdreg1b
opc_3b:
	clrl	%d0		//if E3, only opclass 0x0 is possible
	rts
opc_1b:
	movel	CMDREG1B(%a6),%d0
	bfextu	%d0{#0:#3},%d0	//shift opclass bits d0{31:29} to d0{2:0}
	rts
//
//	g_dfmtou --- put destination format in d0{1:0}
//
//	If E1, the format is from cmdreg1b{12:10}
//	If E3, the format is extended.
//
//	Dest. Fmt.	
//		extended  010 -> 00
//		single    001 -> 01
//		double    101 -> 10
//
g_dfmtou:
	btstb	#E3,E_BYTE(%a6)
	beqs	op011
	clrl	%d0		//if E1, size is always ext
	rts
op011:
	movel	CMDREG1B(%a6),%d0
	bfextu	%d0{#3:#3},%d0	//dest fmt from cmdreg1b{12:10}
	cmpb	#1,%d0		//check for single
	bnes	not_sgl
	movel	#1,%d0
	rts
not_sgl:
	cmpb	#5,%d0		//check for double
	bnes	not_dbl
	movel	#2,%d0
	rts
not_dbl:
	clrl	%d0		//must be extended
	rts

//
//
// Final result table for unf_sub. Note that the negative counterparts
// are unnecessary as unf_sub always returns the sign separately from
// the exponent.
//					;+zero
EXT_PZRO:	.long	0x00000000,0x00000000,0x00000000,0x00000000	
//					;+zero
SGL_PZRO:	.long	0x3f810000,0x00000000,0x00000000,0x00000000	
//					;+zero
DBL_PZRO:	.long	0x3c010000,0x00000000,0x00000000,0x00000000	
//					;smallest +ext denorm
EXT_PSML:	.long	0x00000000,0x00000000,0x00000001,0x00000000	
//					;smallest +sgl denorm
SGL_PSML:	.long	0x3f810000,0x00000100,0x00000000,0x00000000	
//					;smallest +dbl denorm
DBL_PSML:	.long	0x3c010000,0x00000000,0x00000800,0x00000000	
//
//	UNF_SUB --- underflow result calculation
//
// Input:
//	d0 	contains round precision
//	a0	points to input operand in the internal extended format
//
// Output:
//	a0 	points to correct internal extended precision result.
//

tblunf:
	.long	uEXT_RN
	.long	uEXT_RZ
	.long	uEXT_RM
	.long	uEXT_RP
	.long	uSGL_RN
	.long	uSGL_RZ
	.long	uSGL_RM
	.long	uSGL_RP
	.long	uDBL_RN
	.long	uDBL_RZ
	.long	uDBL_RM
	.long	uDBL_RP
	.long	uDBL_RN
	.long	uDBL_RZ
	.long	uDBL_RM
	.long	uDBL_RP

	.global	unf_sub
unf_sub:
	lsll	#2,%d0		//move round precision to d0{3:2}
	bfextu	FPCR_MODE(%a6){#2:#2},%d1 //set round mode
	orl	%d1,%d0		//index is fmt:mode in d0{3:0}
	leal	tblunf,%a1	//load a1 with table address
	movel	%a1@(%d0:l:4),%a1	//use d0 as index to the table
	jmp	(%a1)		//go to the correct routine
//
//case DEST_FMT = EXT
//
uEXT_RN:
	leal	EXT_PZRO,%a1	//answer is +/- zero
	bsetb	#z_bit,FPSR_CC(%a6)
	bra	uset_sign	//now go set the sign	
uEXT_RZ:
	leal	EXT_PZRO,%a1	//answer is +/- zero
	bsetb	#z_bit,FPSR_CC(%a6)
	bra	uset_sign	//now go set the sign
uEXT_RM:
	tstb	LOCAL_SGN(%a0)	//if negative underflow
	beqs	ue_rm_pos
ue_rm_neg:
	leal	EXT_PSML,%a1	//answer is negative smallest denorm
	bsetb	#neg_bit,FPSR_CC(%a6)
	bra	end_unfr
ue_rm_pos:
	leal	EXT_PZRO,%a1	//answer is positive zero
	bsetb	#z_bit,FPSR_CC(%a6)
	bra	end_unfr
uEXT_RP:
	tstb	LOCAL_SGN(%a0)	//if negative underflow
	beqs	ue_rp_pos
ue_rp_neg:
	leal	EXT_PZRO,%a1	//answer is negative zero
	oril	#negz_mask,USER_FPSR(%a6)
	bra	end_unfr
ue_rp_pos:
	leal	EXT_PSML,%a1	//answer is positive smallest denorm
	bra	end_unfr
//
//case DEST_FMT = DBL
//
uDBL_RN:
	leal	DBL_PZRO,%a1	//answer is +/- zero
	bsetb	#z_bit,FPSR_CC(%a6)
	bra	uset_sign
uDBL_RZ:
	leal	DBL_PZRO,%a1	//answer is +/- zero
	bsetb	#z_bit,FPSR_CC(%a6)
	bra	uset_sign	//now go set the sign
uDBL_RM:
	tstb	LOCAL_SGN(%a0)	//if negative overflow
	beqs	ud_rm_pos
ud_rm_neg:
	leal	DBL_PSML,%a1	//answer is smallest denormalized negative
	bsetb	#neg_bit,FPSR_CC(%a6)
	bra	end_unfr
ud_rm_pos:
	leal	DBL_PZRO,%a1	//answer is positive zero
	bsetb	#z_bit,FPSR_CC(%a6)
	bra	end_unfr
uDBL_RP:
	tstb	LOCAL_SGN(%a0)	//if negative overflow
	beqs	ud_rp_pos
ud_rp_neg:
	leal	DBL_PZRO,%a1	//answer is negative zero
	oril	#negz_mask,USER_FPSR(%a6)
	bra	end_unfr
ud_rp_pos:
	leal	DBL_PSML,%a1	//answer is smallest denormalized negative
	bra	end_unfr
//
//case DEST_FMT = SGL
//
uSGL_RN:
	leal	SGL_PZRO,%a1	//answer is +/- zero
	bsetb	#z_bit,FPSR_CC(%a6)
	bras	uset_sign
uSGL_RZ:
	leal	SGL_PZRO,%a1	//answer is +/- zero
	bsetb	#z_bit,FPSR_CC(%a6)
	bras	uset_sign
uSGL_RM:
	tstb	LOCAL_SGN(%a0)	//if negative overflow
	beqs	us_rm_pos
us_rm_neg:
	leal	SGL_PSML,%a1	//answer is smallest denormalized negative
	bsetb	#neg_bit,FPSR_CC(%a6)
	bras	end_unfr
us_rm_pos:
	leal	SGL_PZRO,%a1	//answer is positive zero
	bsetb	#z_bit,FPSR_CC(%a6)
	bras	end_unfr
uSGL_RP:
	tstb	LOCAL_SGN(%a0)	//if negative overflow
	beqs	us_rp_pos
us_rp_neg:
	leal	SGL_PZRO,%a1	//answer is negative zero
	oril	#negz_mask,USER_FPSR(%a6)
	bras	end_unfr
us_rp_pos:
	leal	SGL_PSML,%a1	//answer is smallest denormalized positive
	bras	end_unfr

uset_sign:
	tstb	LOCAL_SGN(%a0)	//if negative overflow
	beqs	end_unfr
uneg_sign:
	bsetb	#neg_bit,FPSR_CC(%a6)

end_unfr:
	movew	LOCAL_EX(%a1),LOCAL_EX(%a0) //be careful not to overwrite sign
	movel	LOCAL_HI(%a1),LOCAL_HI(%a0)
	movel	LOCAL_LO(%a1),LOCAL_LO(%a0)
	rts
//
//	reg_dest --- write byte, word, or long data to Dn
//
//
// Input:
//	L_SCR1: Data 
//	d1:     data size and dest register number formatted as:
//
//	32		5    4     3     2     1     0
//       -----------------------------------------------
//       |        0        |    Size   |  Dest Reg #   |
//       -----------------------------------------------
//
//	Size is:
//		0 - Byte
//		1 - Word
//		2 - Long/Single
//
pregdst:
	.long	byte_d0
	.long	byte_d1
	.long	byte_d2
	.long	byte_d3
	.long	byte_d4
	.long	byte_d5
	.long	byte_d6
	.long	byte_d7
	.long	word_d0
	.long	word_d1
	.long	word_d2
	.long	word_d3
	.long	word_d4
	.long	word_d5
	.long	word_d6
	.long	word_d7
	.long	long_d0
	.long	long_d1
	.long	long_d2
	.long	long_d3
	.long	long_d4
	.long	long_d5
	.long	long_d6
	.long	long_d7

reg_dest:
	leal	pregdst,%a0
	movel	%a0@(%d1:l:4),%a0
	jmp	(%a0)

byte_d0:
	moveb	L_SCR1(%a6),USER_D0+3(%a6)
	rts
byte_d1:
	moveb	L_SCR1(%a6),USER_D1+3(%a6)
	rts
byte_d2:
	moveb	L_SCR1(%a6),%d2
	rts
byte_d3:
	moveb	L_SCR1(%a6),%d3
	rts
byte_d4:
	moveb	L_SCR1(%a6),%d4
	rts
byte_d5:
	moveb	L_SCR1(%a6),%d5
	rts
byte_d6:
	moveb	L_SCR1(%a6),%d6
	rts
byte_d7:
	moveb	L_SCR1(%a6),%d7
	rts
word_d0:
	movew	L_SCR1(%a6),USER_D0+2(%a6)
	rts
word_d1:
	movew	L_SCR1(%a6),USER_D1+2(%a6)
	rts
word_d2:
	movew	L_SCR1(%a6),%d2
	rts
word_d3:
	movew	L_SCR1(%a6),%d3
	rts
word_d4:
	movew	L_SCR1(%a6),%d4
	rts
word_d5:
	movew	L_SCR1(%a6),%d5
	rts
word_d6:
	movew	L_SCR1(%a6),%d6
	rts
word_d7:
	movew	L_SCR1(%a6),%d7
	rts
long_d0:
	movel	L_SCR1(%a6),USER_D0(%a6)
	rts
long_d1:
	movel	L_SCR1(%a6),USER_D1(%a6)
	rts
long_d2:
	movel	L_SCR1(%a6),%d2
	rts
long_d3:
	movel	L_SCR1(%a6),%d3
	rts
long_d4:
	movel	L_SCR1(%a6),%d4
	rts
long_d5:
	movel	L_SCR1(%a6),%d5
	rts
long_d6:
	movel	L_SCR1(%a6),%d6
	rts
long_d7:
	movel	L_SCR1(%a6),%d7
	rts
	|end