res_func.s

VxWorks BSP框架源代码包含头文件和驱动

/* res_func.s - Motorola 68040 FP restore function routine (EXC) */

/* Copyright 1991-1993 Wind River Systems, Inc. */
	.data
	.globl	_copyright_wind_river
	.long	_copyright_wind_river

/*
modification history
--------------------
01f,21jul93,kdl  added .text (SPR #2372).
01e,23aug92,jcf  changed bxxx to jxx.
01d,26may92,rrr  the tree shuffle
01c,10jan92,kdl  added modification history; general cleanup.
01b,16dec91,kdl	 put in changes from Motorola v3.9 (from FPSP 2.1):
		 change underflow handling for fmove, fneg, & fabs;
		 handle move to int format for all rounding modes;
		 correct catastrophic underflow check.
01a,15aug91,kdl  original version, from Motorola FPSP v2.0.
*/

/*
DESCRIPTION


	res_funcsa 3.7 4/26/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 (fmovep 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

NOMANUAL
*/

#include "fpsp040E.h"

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

|	xref	__x_mem_write
|	xref	__x_bindec
|	xref	__x_get_fline
|	xref	__x_round
|	xref	__x_denorm
|	xref	__x_dest_ext
|	xref	__x_dest_dbl
|	xref	__x_dest_sgl
|	xref	__x_unf_sub
|	xref	__x_nrm_set
|	xref	__x_dnrm_lp
|	xref	__x_ovf_res
|	xref	__x_reg_dest
|	xref	__x_t_ovfl
|	xref	__x_t_unfl

	.globl	__x_res_func
	.globl 	__x_p_move

	.text

__x_res_func:
	clrb	a6@(DNRM_FLG)
	clrb	a6@(RES_FLG)
	clrb	a6@(CU_ONLY)
	tstb	a6@(DY_MO_FLG)
	jeq 	monadic
dyadic:
	btst	#7,a6@(DTAG)	| if dop = norm=000, zero=001,
|				| inf=010 or nan=011
	jeq 	monadic		| then branch
|				| else denorm
| HANDLE DESTINATION DENORM HERE
|				| set dtag to norm
|				| write the tag # fpte15 to the fstack
	lea	a6@(FPTEMP),a0

	bclr	#sign_bit,a0@(LOCAL_EX)
	sne	a0@(LOCAL_SGN)

	bsrl	__x_nrm_set		| normalize number (exp will go negative)
	bclr	#sign_bit,a0@(LOCAL_EX) | get rid of false sign
	bfclr	a0@(LOCAL_SGN){#0:#8}	| change back to IEEE ext format
	jeq 	dpos
	bset	#sign_bit,a0@(LOCAL_EX)
dpos:
	bfclr	a6@(DTAG){#0:#4}	| set tag to normalized, FPTE15 = 0
	bset	#4,a6@(DTAG)	| set FPTE15
	orb	#0x0f,a6@(DNRM_FLG)
monadic:
	lea	a6@(ETEMP),a0
	btst	#direction_bit,a6@(CMDREG1B)	| check direction
	jne 	opclass3			| it is a mv out
|
| At this point, only oplcass 0 and 2 possible
|
	btst	#7,a6@(STAG)	| if sop = norm=000, zero=001,
|				| inf=010 or nan=011
	jne 	mon_dnrm	| else denorm
	tstb	a6@(DY_MO_FLG)	| all cases of dyadic instructions would
	jne 	__x_normal		| require normalization of denorm

| At this point:
|	monadic instructions:	fabsx  = 0x18  fnegx   = 0x1a  ftst   = 0x3a
|				fmovel = 0x00  fsmove = 0x40  fdmove = 0x44
|				fsqrtx = 0x05* fssqrt = 0x41  fdsqrt = 0x45
|				(*fsqrtx reencoded to 0x05)
|
	movew	a6@(CMDREG1B),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)
|
	btst	#0,d0
	jne 	__x_normal			| weed out fsqrtx instructions
|
| cu_norm handles fmovel in instructions with normalized inputs.
| The routine round is used to correctly round the input for the
| destination precision and mode.
|
cu_norm:
	st	a6@(CU_ONLY)		| set cu-only inst flag
	movew	a6@(CMDREG1B),d0
	andib	#0x3b,d0		| isolate bits to select inst
	tstb	d0
	jeq 	cu_nmove	| if zero, it is an fmove
	cmpib	#0x18,d0
	jeq 	cu_nabs		| if 0x18, it is fabs
	cmpib	#0x1a,d0
	jeq 	cu_nneg		| if 0x1a, 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	a0@(LOCAL_EX),d0
	bclr	#15,d0
	sne	a0@(LOCAL_SGN)
	jeq 	cu_ntpo
	orl	#neg_mask,a6@(USER_FPSR) | set N
cu_ntpo:
	cmpiw	#0x7fff,d0	| test for inf/nan
	jne 	cu_ntcz
	tstl	a0@(LOCAL_HI)
	jne 	cu_ntn
	tstl	a0@(LOCAL_LO)
	jne 	cu_ntn
	orl	#inf_mask,a6@(USER_FPSR)
	rts
cu_ntn:
	orl	#nan_mask,a6@(USER_FPSR)
	movel	a6@(ETEMP_EX),a6@(FPTEMP_EX)	| set up fptemp sign for
|						| snan handler

	rts
cu_ntcz:
	tstl	a0@(LOCAL_HI)
	jne 	cu_ntsx
	tstl	a0@(LOCAL_LO)
	jne 	cu_ntsx
	orl	#z_mask,a6@(USER_FPSR)
cu_ntsx:
	rts
|
| Inst is fabs.  Execute the absolute value function on the input.
| Branch to the fmovel code.  If the operand is NaN, do nothing.
|
cu_nabs:
	moveb	a6@(STAG),d0
	btst	#5,d0			| test for NaN or zero
	jne 	wr_etemp		| if either, simply write it
	bclr	#7,a0@(LOCAL_EX)		| do abs
	jra 	cu_nmove		| fmovel code will finish
|
| Inst is fneg.  Execute the negate value function on the input.
| Fall though to the fmovel code.  If the operand is NaN, do nothing.
|
cu_nneg:
	moveb	a6@(STAG),d0
	btst	#5,d0			| test for NaN or zero
	jne 	wr_etemp		| if either, simply write it
	bchg	#7,a0@(LOCAL_EX)		| 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	a6@(STAG),d0
	andib	#0xe0,d0			| isolate stag bits
	jne 	wr_etemp		| if not norm, simply write it
	btst	#2,a6@(CMDREG1B+1)	| check for rd
	jne 	cu_nmrd
	btst	#6,a6@(CMDREG1B+1)	| check for rs
	jne 	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	a6@(fpcr_MODE){#0:#2},d0
	tstb	d0			| check for extended
	jeq 	cu_wrexn		| if so, just write result
	cmpib	#1,d0			| check for single
	jeq 	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	a0@(LOCAL_EX),d1	| compare exponent to double threshold
	andw	#0x7fff,d1
	cmpw	#0x3c01,d1
	jls 	cu_nunfl
	bfextu	a6@(fpcr_MODE){#2:#2},d1	| get rmode
	orl	#0x00020000,d1		| or in rprec (double)
	clrl	d0			| clr g,r,s for round
	bclr	#sign_bit,a0@(LOCAL_EX)	| convert to internal format
	sne	a0@(LOCAL_SGN)
	bsrl	__x_round			| perform the round
	bfclr	a0@(LOCAL_SGN){#0:#8}
	jeq 	cu_nmrdc
	bset	#sign_bit,a0@(LOCAL_EX)
cu_nmrdc:
	movew	a0@(LOCAL_EX),d1
	andw	#0x7fff,d1
	cmpw	#0x43ff,d1
	jge 	cu_novfl		| take care of overflow
	jra 	cu_wrexn
|
| The move is fsmove or round precision is single.
|
cu_nmrs:
	movel	#1,d0
	movew	a0@(LOCAL_EX),d1
	andw	#0x7fff,d1
	cmpw	#0x3f81,d1
	jls 	cu_nunfl
	bfextu	a6@(fpcr_MODE){#2:#2},d1	| get rmode
	orl	#0x00010000,d1		| force single
	clrl	d0			| clr grs for round
	bclr	#sign_bit,a0@(LOCAL_EX)
	sne	a0@(LOCAL_SGN)
	bsrl	__x_round			| perform the round
	bfclr	a0@(LOCAL_SGN){#0:#8}
	jeq 	cu_nmrsc
	bset	#sign_bit,a0@(LOCAL_EX)
cu_nmrsc:
	movew	a0@(LOCAL_EX),d1
	andw	#0x7FFF,d1
	cmpw	#0x407f,d1
	jlt 	cu_wrexn
|
| The operand is above precision boundaries.  Use __x_t_ovfl to
| generate the correct value.
|
cu_novfl:
	bsrl	__x_t_ovfl
	jra 	cu_wrexn
|
| The operand is below precision boundaries.  Use __x_denorm to
| generate the correct value.
|
cu_nunfl:
	bclr	#sign_bit,a0@(LOCAL_EX)
	sne	a0@(LOCAL_SGN)
	bsrl	__x_denorm
	bfclr	a0@(LOCAL_SGN){#0:#8}	| change back to IEEE ext format
	jeq 	cu_nucont
	bset	#sign_bit,a0@(LOCAL_EX)
cu_nucont:
	bfextu	a6@(fpcr_MODE){#2:#2},d1
	btst	#2,a6@(CMDREG1B+1)	| check for rd
	jne 	inst_d
	btst	#6,a6@(CMDREG1B+1)	| check for rs
	jne 	inst_s
	swap	d1
	moveb	a6@(fpcr_MODE),d1
	lsrb	#6,d1
	swap	d1
	jra 	inst_sd
inst_d:
	orl	#0x00020000,d1
	jra 	inst_sd
inst_s:
	orl	#0x00010000,d1
inst_sd:
	bclr	#sign_bit,a0@(LOCAL_EX)
	sne	a0@(LOCAL_SGN)
	bsrl	__x_round
	bfclr	a0@(LOCAL_SGN){#0:#8}
	jeq 	cu_nuflp
	bset	#sign_bit,a0@(LOCAL_EX)
cu_nuflp:
	btst	#__x_inex2_bit,a6@(FPSR_EXCEPT)
	jeq 	cu_nuninx
	orl	#aunfl_mask,a6@(USER_FPSR) | if the round was inex, set AUNFL
cu_nuninx:
	tstl	a0@(LOCAL_HI)		| test for zero
	jne 	cu_nunzro
	tstl	a0@(LOCAL_LO)
	jne 	cu_nunzro
|
| The mantissa is zero from the denorm loop.  Check sign and rmode
| to see if rounding should have occured which would leave the lsb.
|
	movel	a6@(USER_FPCR),d0
	andil	#0x30,d0		| isolate rmode
	cmpil	#0x20,d0
	jlt 	cu_nzro
	jne 	cu_nrp
cu_nrm:
	tstw	a0@(LOCAL_EX)	| if positive, set lsb
	jge 	cu_nzro
	btst	#7,a6@(fpcr_MODE) | check for double
	jeq 	cu_nincs
	jra 	cu_nincd
cu_nrp:
	tstw	a0@(LOCAL_EX)	| if positive, set lsb
	jlt 	cu_nzro
	btst	#7,a6@(fpcr_MODE) | check for double
	jeq 	cu_nincs
cu_nincd:
	orl	#0x800,a0@(LOCAL_LO) | inc for double
	jra 	cu_nunzro
cu_nincs:
	orl	#0x100,a0@(LOCAL_HI) | inc for single
	jra 	cu_nunzro
cu_nzro:
	orl	#z_mask,a6@(USER_FPSR)
	moveb	a6@(STAG),d0
	andib	#0xe0,d0
	cmpib	#0x40,d0		| check if input was tagged zero
	jeq 	cu_numv
cu_nunzro:
	orl	#__x_unfl_mask,a6@(USER_FPSR) | set unfl
cu_numv:
	movel	a0@,a6@(ETEMP)
	movel	a0@(4),a6@(ETEMP_HI)
	movel	a0@(8),a6@(ETEMP_LO)
|
| Write the result to memory, setting the fpsr cc bits.  NaN and Inf
| bypass cu_wrexn.
|
cu_wrexn:
	tstw	a0@(LOCAL_EX)		| test for zero
	jeq 	cu_wrzero
	cmpw	#0x8000,a0@(LOCAL_EX)	| test for zero
	jne 	cu_wreon
cu_wrzero:
	orl	#z_mask,a6@(USER_FPSR)	| set Z bit
cu_wreon:
	tstw	a0@(LOCAL_EX)
	jpl 	wr_etemp
	orl	#neg_mask,a6@(USER_FPSR)
	jra 	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	a6@(DY_MO_FLG)	| all cases of dyadic instructions would
	jne 	nrm_src		| require normalization of denorm

| At this point:
|	monadic instructions:	fabsx  = 0x18  fnegx   = 0x1a  ftst   = 0x3a
|				fmovel = 0x00  fsmove = 0x40  fdmove = 0x44
|				fsqrtx = 0x05* fssqrt = 0x41  fdsqrt = 0x45
|				(*fsqrtx reencoded to 0x05)
|
	movew	a6@(CMDREG1B),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)
|
	btst	#0,d0
	jne 	nrm_src		| weed out fsqrtx instructions
	st	a6@(CU_ONLY)	| set cu-only inst flag
	jra 	cu_dnrm		| fmove, fabs, fneg, ftst
|				| cases go to cu_dnrm
nrm_src:
	bclr	#sign_bit,a0@(LOCAL_EX)
	sne	a0@(LOCAL_SGN)
	bsrl	__x_nrm_set		| normalize number (exponent will go
|				|  negative)
	bclr	#sign_bit,a0@(LOCAL_EX) | get rid of false sign

	bfclr	a0@(LOCAL_SGN){#0:#8}	| change back to IEEE ext format
	jeq 	spos
	bset	#sign_bit,a0@(LOCAL_EX)
spos:
	bfclr	a6@(STAG){#0:#4}	| set tag to normalized, FPTE15 = 0
	bset	#4,a6@(STAG)	| set ETE15
	orb	#0xf0,a6@(DNRM_FLG)
__x_normal:
	tstb	a6@(DNRM_FLG)	| check if any of the ops were denorms
	jne 	ck_wrap		| if so, check if it is a potential
|				| wrap-around case
fix_stk:
	moveb	#0xfe,a6@(CU_SAVEPC)
	bclr	#E1,a6@(E_BYTE)

	clrw	a6@(NMNEXC)

	st	a6@(RES_FLG)	| indicate that a restore is needed
	rts

|
| cu_dnrm handles all cu-only instructions (fmove, fabs, fneg, and
| ftst) completly in software without an frestore to the 040.
|
cu_dnrm:
	st	a6@(CU_ONLY)
	movew	a6@(CMDREG1B),d0
	andib	#0x3b,d0		| isolate bits to select inst
	tstb	d0
	jeq 	cu_dmove	| if zero, it is an fmove
	cmpib	#0x18,d0
	jeq 	cu_dabs		| if 0x18, it is fabs
	cmpib	#0x1a,d0
	jeq 	cu_dneg		| if 0x1a, 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	a0@(LOCAL_EX),d0
	bclr	#15,d0
	sne	a0@(LOCAL_SGN)
	jeq 	cu_dtpo
	orl	#neg_mask,a6@(USER_FPSR) | set N
cu_dtpo:
	cmpiw	#0x7fff,d0	| test for inf/nan
	jne 	cu_dtcz
	tstl	a0@(LOCAL_HI)
	jne 	cu_dtn
	tstl	a0@(LOCAL_LO)
	jne 	cu_dtn
	orl	#inf_mask,a6@(USER_FPSR)
	rts
cu_dtn:
	orl	#nan_mask,a6@(USER_FPSR)
	movel	a6@(ETEMP_EX),a6@(FPTEMP_EX)	| set up fptemp sign for
|						| snan handler
	rts
cu_dtcz:
	tstl	a0@(LOCAL_HI)
	jne 	cu_dtsx
	tstl	a0@(LOCAL_LO)
	jne 	cu_dtsx
	orl	#z_mask,a6@(USER_FPSR)
cu_dtsx:
	rts
|
| Inst is fabs.  Execute the absolute value function on the input.
| Branch to the fmovel code.
|
cu_dabs:
	bclr	#7,a0@(LOCAL_EX)		| do abs
	jra 	cu_dmove		| fmovel code will finish
|
| Inst is fneg.  Execute the negate value function on the input.
| Fall though to the fmovel code.
|
cu_dneg:
	bchg	#7,a0@(LOCAL_EX)		| 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:
	btst	#2,a6@(CMDREG1B+1)	| check for rd
	jne 	cu_dmrd
	btst	#6,a6@(CMDREG1B+1)	| check for rs
	jne 	cu_dmrs
|
| The move or operation is not with forced precision.  Use the
| fpcr_MODE byte to get rounding.
|
cu_dmnr:
	bfextu	a6@(fpcr_MODE){#0:#2},d0
	tstb	d0			| check for extended