res_func.s

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

	jeq 	cu_wrexd		| if so, just write result
	cmpib	#1,d0			| check for single
	jeq 	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	a6@(fpcr_MODE){#2:#2},d1	| get rmode
	tstw	a0@(LOCAL_EX)		| check sign
	jlt 	cu_dmdn
	cmpib	#3,d1			| check for rp
	jne 	cu_dpd			| load double pos zero
	jra 	cu_dpdr			| load double pos zero w/lsb
cu_dmdn:
	cmpib	#2,d1			| check for rm
	jne 	cu_dnd			| load double neg zero
	jra 	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	a6@(fpcr_MODE){#2:#2},d1	| get rmode
	tstw	a0@(LOCAL_EX)		| check sign
	jlt 	cu_dmsn
	cmpib	#3,d1			| check for rp
	jne 	cu_spd			| load single pos zero
	jra 	cu_spdr			| load single pos zero w/lsb
cu_dmsn:
	cmpib	#2,d1			| check for rm
	jne 	cu_snd			| load single neg zero
	jra 	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	#__x_unfl_mask,a6@(USER_FPSR)
	tstw	a0@(LOCAL_EX)
	jeq 	wr_etemp
	orl	#neg_mask,a6@(USER_FPSR)
	jra 	wr_etemp
|
| These routines write +/- zero in double format.  The routines
| cu_dpdr and cu_dndr set the double lsb.
|
cu_dpd:
	movel	#0x3c010000,a0@(LOCAL_EX)	| force pos double zero
	clrl	a0@(LOCAL_HI)
	clrl	a0@(LOCAL_LO)
	orl	#z_mask,a6@(USER_FPSR)
	orl	#unfinx_mask,a6@(USER_FPSR)
	jra 	wr_etemp
cu_dpdr:
	movel	#0x3c010000,a0@(LOCAL_EX)	| force pos double zero
	clrl	a0@(LOCAL_HI)
	movel	#0x800,a0@(LOCAL_LO)		| with lsb set
	orl	#unfinx_mask,a6@(USER_FPSR)
	jra 	wr_etemp
cu_dnd:
	movel	#0xbc010000,a0@(LOCAL_EX)	| force pos double zero
	clrl	a0@(LOCAL_HI)
	clrl	a0@(LOCAL_LO)
	orl	#z_mask,a6@(USER_FPSR)
	orl	#neg_mask,a6@(USER_FPSR)
	orl	#unfinx_mask,a6@(USER_FPSR)
	jra 	wr_etemp
cu_dndr:
	movel	#0xbc010000,a0@(LOCAL_EX)	| force pos double zero
	clrl	a0@(LOCAL_HI)
	movel	#0x800,a0@(LOCAL_LO)		| with lsb set
	orl	#neg_mask,a6@(USER_FPSR)
	orl	#unfinx_mask,a6@(USER_FPSR)
	jra 	wr_etemp
|
| These routines write +/- zero in single format.  The routines
| cu_dpdr and cu_dndr set the single lsb.
|
cu_spd:
	movel	#0x3f810000,a0@(LOCAL_EX)	| force pos single zero
	clrl	a0@(LOCAL_HI)
	clrl	a0@(LOCAL_LO)
	orl	#z_mask,a6@(USER_FPSR)
	orl	#unfinx_mask,a6@(USER_FPSR)
	jra 	wr_etemp
cu_spdr:
	movel	#0x3f810000,a0@(LOCAL_EX)	| force pos single zero
	movel	#0x100,a0@(LOCAL_HI)		| with lsb set
	clrl	a0@(LOCAL_LO)
	orl	#unfinx_mask,a6@(USER_FPSR)
	jra 	wr_etemp
cu_snd:
	movel	#0xbf810000,a0@(LOCAL_EX)	| force pos single zero
	clrl	a0@(LOCAL_HI)
	clrl	a0@(LOCAL_LO)
	orl	#z_mask,a6@(USER_FPSR)
	orl	#neg_mask,a6@(USER_FPSR)
	orl	#unfinx_mask,a6@(USER_FPSR)
	jra 	wr_etemp
cu_sndr:
	movel	#0xbf810000,a0@(LOCAL_EX)	| force pos single zero
	movel	#0x100,a0@(LOCAL_HI)		| with lsb set
	clrl	a0@(LOCAL_LO)
	orl	#neg_mask,a6@(USER_FPSR)
	orl	#unfinx_mask,a6@(USER_FPSR)
	jra 	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, 0xff for dyadic
|	DNRM_FLG contains 0x00 for neither op denormalized
|	                  0x0f for the destination op denormalized
|	                  0xf0 for the source op denormalized
|	                  0xff for both ops denormalzed
|
| The wrap-around condition occurs for add, sub, div, and cmp
| when
|
|	abs(dest_exp - src_exp) >= 0x8000
|
| and for mul when
|
|	(dest_exp + src_exp) < 0x0
|
| we must process the operation here if this case is true.
|
| The rts following the frcfpn routine is the exit from __x_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 0x3fff (keep sign)
|		use fpu to perform dest+new_src (user's rmode and X)
|		clr sticky
|	else
|		set sticky
|	call __x_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 0x3fff (keep sign)
|		use fpu to perform dest+new_src (user's rmode and X)
|		clr sticky
|	else
|		set sticky
|	call __x_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:
#define	addcode		0x22
#define	subcode		0x28
#define	mulcode		0x23
#define	divcode		0x20
#define	cmpcode		0x38
ck_wrap:
	tstb	a6@(DY_MO_FLG)	| check for fsqrt
	jeq 	fix_stk		| if zero, it is fsqrt
	movew	a6@(CMDREG1B),d0
	andiw	#0x3b,d0		| strip to command bits
	cmpiw	#addcode,d0
	jeq 	wrap_add
	cmpiw	#subcode,d0
	jeq 	wrap_sub
	cmpiw	#mulcode,d0
	jeq 	wrap_mul
	cmpiw	#cmpcode,d0
	jeq 	wrap_cmp
|
| Inst is fdiv.
|
wrap_div:
	cmpb	#0xff,a6@(DNRM_FLG) | if both ops denorm,
	jeq 	fix_stk		 | restore to fpu
|
| One of the ops is denormalized.  Test for wrap condition
| and force the result.
|
	cmpb	#0x0f,a6@(DNRM_FLG) | check for dest denorm
	jne 	div_srcd
div_destd:
	bsrl	ckinf_ns
	jne 	fix_stk
	bfextu	a6@(ETEMP_EX){#1:#15},d0	| get src exp (always pos)
	bfexts	a6@(FPTEMP_EX){#1:#15},d1	| get dest exp (always neg)
	subl	d1,d0			| subtract dest from src
	cmpl	#0x7fff,d0
	jlt 	fix_stk			| if less, not wrap case
	clrb	a6@(WBTEMP_SGN)
	movew	a6@(ETEMP_EX),d0		| find the sign of the result
	movew	a6@(FPTEMP_EX),d1
	eorw	d1,d0
	andiw	#0x8000,d0
	jeq 	force_unf
	st	a6@(WBTEMP_SGN)
	jra 	force_unf

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



div_srcd:
	bsrl	ckinf_nd
	jne 	fix_stk
	bfextu	a6@(FPTEMP_EX){#1:#15},d0	| get dest exp (always pos)
	bfexts	a6@(ETEMP_EX){#1:#15},d1	| get src exp (always neg)
	subl	d1,d0			| subtract src from dest
	cmpl	#0x8000,d0
	jlt 	fix_stk			| if less, not wrap case
	clrb	a6@(WBTEMP_SGN)
	movew	a6@(ETEMP_EX),d0		| find the sign of the result
	movew	a6@(FPTEMP_EX),d1
	eorw	d1,d0
	andiw	#0x8000,d0
	jeq 	force_ovf
	st	a6@(WBTEMP_SGN)
|
| This code handles the case of the instruction resulting in
| an overflow condition.
|
force_ovf:
	bclr	#E1,a6@(E_BYTE)
	orl	#__x_ovfl_inx_mask,a6@(USER_FPSR)
	clrw	a6@(NMNEXC)
	lea	a6@(WBTEMP),a0		| point a0 to memory location
	movew	a6@(CMDREG1B),d0
	btst	#6,d0			| test for forced precision
	jeq 	frcovf_fpcr
	btst	#2,d0			| check for double
	jne 	frcovf_dbl
	movel	#0x1,d0			| inst is forced single
	jra 	frcovf_rnd
frcovf_dbl:
	movel	#0x2,d0			| inst is forced double
	jra 	frcovf_rnd
frcovf_fpcr:
	bfextu	a6@(fpcr_MODE){#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
|	tstb	d0
|	jeq 	frcovf_x
|	orl	#__x_inex2_mask,a6@(USER_FPSR) | if prec is s or d, set inex2

|frcovf_x:
	bsrl	__x_ovf_res			| get correct result based on
|					| round precision/mode.  This
|					| sets FPSR_CC correctly
|					| returns in external format
	bfclr	a6@(WBTEMP_SGN){#0:#8}
	jeq 	frcfpn
	bset	#sign_bit,a6@(WBTEMP_EX)
	jra 	frcfpn
|
| Inst is fadd.
|
wrap_add:
	cmpb	#0xff,a6@(DNRM_FLG) | if both ops denorm,
	jeq 	fix_stk		 | restore to fpu
|
| One of the ops is denormalized.  Test for wrap condition
| and complete the instruction.
|
	cmpb	#0x0f,a6@(DNRM_FLG) | check for dest denorm
	jne 	add_srcd
add_destd:
	bsrl	ckinf_ns
	jne 	fix_stk
	bfextu	a6@(ETEMP_EX){#1:#15},d0	| get src exp (always pos)
	bfexts	a6@(FPTEMP_EX){#1:#15},d1	| get dest exp (always neg)
	subl	d1,d0			| subtract dest from src
	cmpl	#0x8000,d0
	jlt 	fix_stk			| if less, not wrap case
	jra 	add_wrap
add_srcd:
	bsrl	ckinf_nd
	jne 	fix_stk
	bfextu	a6@(FPTEMP_EX){#1:#15},d0	| get dest exp (always pos)
	bfexts	a6@(ETEMP_EX){#1:#15},d1	| get src exp (always neg)
	subl	d1,d0			| subtract src from dest
	cmpl	#0x8000,d0
	jlt 	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 __x_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 __x_round with the sticky bit set
/* | and the result will be correctfor the user's rounding mode and */
| precision.
|
add_wrap:
	movew	a6@(ETEMP_EX),d0
	movew	a6@(FPTEMP_EX),d1
	eorw	d1,d0
	andiw	#0x8000,d0
	jeq 	add_same
|
| The signs are unlike.
|
	cmpb	#0x0f,a6@(DNRM_FLG) | is dest the denorm?
	jne 	add_u_srcd
	movew	a6@(FPTEMP_EX),d0
	andiw	#0x8000,d0
	orw	#0x3fff,d0	| force the exponent to +/- 1
	movew	d0,a6@(FPTEMP_EX) | in the denorm
	movel	a6@(USER_FPCR),d0
	andil	#0x30,d0
	fmovel	d0,fpcr		| set up users rmode and X
	fmovex	a6@(ETEMP),fp0
	faddx	a6@(FPTEMP),fp0
	lea	a6@(WBTEMP),a0	| point a0 to wbtemp in frame
	fmovel	fpsr,d1
	orl	d1,a6@(USER_FPSR) /* | capture cc's and inex from fadd */
	fmovex	fp0,a6@(WBTEMP)	| write result to memory
	lsrl	#4,d0		| put rmode in lower 2 bits
	movel	a6@(USER_FPCR),d1
	andil	#0xc0,d1
	lsrl	#6,d1		| put precision in upper word
	swap	d1
	orl	d0,d1		| set up for __x_round call
	clrl	d0		| force sticky to zero
	bclr	#sign_bit,a6@(WBTEMP_EX)
	sne	a6@(WBTEMP_SGN)
	bsrl	__x_round		| round result to users rmode # prec
	bfclr	a6@(WBTEMP_SGN){#0:#8}	| convert back to IEEE ext format
	jeq 	frcfpnr
	bset	#sign_bit,a6@(WBTEMP_EX)
	jra 	frcfpnr
add_u_srcd:
	movew	a6@(ETEMP_EX),d0
	andiw	#0x8000,d0
	orw	#0x3fff,d0	| force the exponent to +/- 1
	movew	d0,a6@(ETEMP_EX) | in the denorm
	movel	a6@(USER_FPCR),d0
	andil	#0x30,d0
	fmovel	d0,fpcr		| set up users rmode and X
	fmovex	a6@(ETEMP),fp0
	faddx	a6@(FPTEMP),fp0
	fmovel	fpsr,d1
	orl	d1,a6@(USER_FPSR) /* | capture cc's and inex from fadd */
	lea	a6@(WBTEMP),a0		| point a0 to wbtemp in frame
	fmovex	fp0,a6@(WBTEMP)		| write result to memory
	lsrl	#4,d0			| put rmode in lower 2 bits
	movel	a6@(USER_FPCR),d1
	andil	#0xc0,d1
	lsrl	#6,d1			| put precision in upper word
	swap	d1
	orl	d0,d1			| set up for __x_round call
	clrl	d0			| force sticky to zero
	bclr	#sign_bit,a6@(WBTEMP_EX)
	sne	a6@(WBTEMP_SGN)		| use internal format for __x_round
	bsrl	__x_round		| round result to users rmode # prec
	bfclr	a6@(WBTEMP_SGN){#0:#8}	| convert back to IEEE ext format
	jeq 	frcfpnr
	bset	#sign_bit,a6@(WBTEMP_EX)
	jra 	frcfpnr
|
| Signs are alike:
|
add_same:
	cmpb	#0x0f,a6@(DNRM_FLG) 	| is dest the denorm?
	jne 	add_s_srcd
add_s_destd:
	lea	a6@(ETEMP),a0
	movel	a6@(USER_FPCR),d0
	andil	#0x30,d0
	lsrl	#4,d0			| put rmode in lower 2 bits
	movel	a6@(USER_FPCR),d1
	andil	#0xc0,d1
	lsrl	#6,d1			| put precision in upper word
	swap	d1
	orl	d0,d1			| set up for __x_round call
	movel	#0x20000000,d0		| set sticky for __x_round
	bclr	#sign_bit,a6@(ETEMP_EX)
	sne	a6@(ETEMP_SGN)
	bsrl	__x_round		| round result to users rmode # prec
	bfclr	a6@(ETEMP_SGN){#0:#8}	| convert back to IEEE ext format
	jeq 	add_s_dclr
	bset	#sign_bit,a6@(ETEMP_EX)
add_s_dclr:
	lea	a6@(WBTEMP),a0
	movel	a6@(ETEMP),a0@		| write result to wbtemp
	movel	a6@(ETEMP_HI),a0@(4)
	movel	a6@(ETEMP_LO),a0@(8)
	tstw	a6@(ETEMP_EX)
	jgt 	add_ckovf
	orl	#neg_mask,a6@(USER_FPSR)
	jra 	add_ckovf
add_s_srcd:
	lea	a6@(FPTEMP),a0
	movel	a6@(USER_FPCR),d0
	andil	#0x30,d0
	lsrl	#4,d0			| put rmode in lower 2 bits
	movel	a6@(USER_FPCR),d1
	andil	#0xc0,d1
	lsrl	#6,d1			| put precision in upper word
	swap	d1
	orl	d0,d1			| set up for __x_round call
	movel	#0x20000000,d0		| set sticky for __x_round
	bclr	#sign_bit,a6@(FPTEMP_EX)
	sne	a6@(FPTEMP_SGN)
	bsrl	__x_round		| round result to users rmode # prec
	bfclr	a6@(FPTEMP_SGN){#0:#8}	| convert back to IEEE ext format
	jeq 	add_s_sclr
	bset	#sign_bit,a6@(FPTEMP_EX)
add_s_sclr:
	lea	a6@(WBTEMP),a0
	movel	a6@(FPTEMP),a0@		| write result to wbtemp
	movel	a6@(FPTEMP_HI),a0@(4)
	movel	a6@(FPTEMP_LO),a0@(8)
	tstw	a6@(FPTEMP_EX)
	jgt 	add_ckovf
	orl	#neg_mask,a6@(USER_FPSR)
add_ckovf:
	movew	a6@(WBTEMP_EX),d0
	andiw	#0x7fff,d0
	cmpiw	#0x7fff,d0
	jne 	frcfpnr
|
| The result has overflowed to 0x7fff exponent.  Set I, ovfl,
| and aovfl, and clr the mantissa (incorrectly set by the
| __x_round routine.)
|
	orl	#inf_mask+__x_ovfl_inx_mask,a6@(USER_FPSR)
	clrl	a0@(4)
	jra 	frcfpnr
|
| Inst is fsub.
|
wrap_sub:
	cmpb	#0xff,a6@(DNRM_FLG) 	| if both ops denorm,
	jeq 	fix_stk		 	| restore to fpu
|
| One of the ops is denormalized.  Test for wrap condition
| and complete the instruction.
|
	cmpb	#0x0f,a6@(DNRM_FLG) 	| check for dest denorm
	jne 	sub_srcd
sub_destd:
	bsrl	ckinf_ns
	jne 	fix_stk
	bfextu	a6@(ETEMP_EX){#1:#15},d0	| get src exp (always pos)
	bfexts	a6@(FPTEMP_EX){#1:#15},d1	| get dest exp (always neg)
	subl	d1,d0				| subtract src from dest
	cmpl	#0x8000,d0
	jlt 	fix_stk				| if less, not wrap case
	jra 	sub_wrap
sub_srcd:
	bsrl	ckinf_nd
	jne 	fix_stk
	bfextu	a6@(FPTEMP_EX){#1:#15},d0	| get dest exp (always pos)
	bfexts	a6@(ETEMP_EX){#1:#15},d1	| get src exp (always neg)
	subl	d1,d0				| subtract dest from src
	cmpl	#0x8000,d0
	jlt 	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 __x_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 __x_round with the sticky bit set
| and the result will be correctfor the user's rounding mode and
| precision.
|
*/