res_func.s

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

sub_wrap:
	movew	a6@(ETEMP_EX),d0
	movew	a6@(FPTEMP_EX),d1
	eorw	d1,d0
	andiw	#0x8000,d0
	jne 	sub_diff
|
| The signs are alike.
|
	cmpb	#0x0f,a6@(DNRM_FLG) | is dest the denorm?
	jne 	sub_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@(FPTEMP),fp0
	fsubx	a6@(ETEMP),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)
	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
sub_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@(FPTEMP),fp0
	fsubx	a6@(ETEMP),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)
	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 unlike:
|
sub_diff:
	cmpb	#0x0f,a6@(DNRM_FLG) | is dest the denorm?
	jne 	sub_s_srcd
sub_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
|
| Since the dest is the denorm, the sign is the opposite of the
| norm sign.
|
	eoriw	#0x8000,a6@(ETEMP_EX)	| flip sign on result
	tstw	a6@(ETEMP_EX)
	jgt 	sub_s_dwr
	orl	#neg_mask,a6@(USER_FPSR)
sub_s_dwr:
	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 	sub_s_dclr
	bset	#sign_bit,a6@(ETEMP_EX)
sub_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)
	jra 	sub_ckovf
sub_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 	sub_s_sclr
	bset	#sign_bit,a6@(FPTEMP_EX)
sub_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 	sub_ckovf
	orl	#neg_mask,a6@(USER_FPSR)
sub_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 fcmp.
|
wrap_cmp:
	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 	cmp_srcd
cmp_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
	tstw	a6@(ETEMP_EX)		| set N to ~sign_of(src)
	jge 	cmp_setn
	rts
cmp_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
	tstw	a6@(FPTEMP_EX)		| set N to sign_of(dest)
	jlt 	cmp_setn
	rts
cmp_setn:
	orl	#neg_mask,a6@(USER_FPSR)
	rts

|
| Inst is fmul.
|
wrap_mul:
	cmpb	#0xff,a6@(DNRM_FLG) | if both ops denorm,
	jeq 	force_unf	| force an underflow (really!)
|
| One of the ops is denormalized.  Test for wrap condition
| and complete the instruction.
|
	cmpb	#0x0f,a6@(DNRM_FLG) | check for dest denorm
	jne 	mul_srcd
mul_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)
	addl	d1,d0			| subtract dest from src
	jgt 	fix_stk
	jra 	force_unf
mul_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)
	addl	d1,d0			| subtract src from dest
	jgt 	fix_stk

|
| This code handles the case of the instruction resulting in
| an underflow condition.
|
force_unf:
	bclr	#E1,a6@(E_BYTE)
	orl	#unfinx_mask,a6@(USER_FPSR)
	clrw	a6@(NMNEXC)
	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 	frcunfcont
	st	a6@(WBTEMP_SGN)
frcunfcont:
	lea	a6@(WBTEMP),a0		| point a0 to memory location
	movew	a6@(CMDREG1B),d0
	btst	#6,d0			| test for forced precision
	jeq 	frcunf_fpcr
	btst	#2,d0			| check for double
	jne 	frcunf_dbl
	movel	#0x1,d0			| inst is forced single
	jra 	frcunf_rnd
frcunf_dbl:
	movel	#0x2,d0			| inst is forced double
	jra 	frcunf_rnd
frcunf_fpcr:
	bfextu	a6@(fpcr_MODE){#0:#2},d0	| inst not forced - use fpcr prec
frcunf_rnd:
	bsrl	__x_unf_sub			| get correct result based on
|					| round precision/mode.  This
|					| sets FPSR_CC correctly
	bfclr	a6@(WBTEMP_SGN){#0:#8}	| convert back to IEEE ext format
	jeq 	frcfpn
	bset	#sign_bit,a6@(WBTEMP_EX)
	jra 	frcfpn

|
/* | Write the result to the user's fpn.  All results must be HUGE to be */
| written|  otherwise the results would have overflowed or underflowed.
| If the rounding precision is single or double, the __x_ovf_res routine
| is needed to correctly supply the max value.
|
frcfpnr:
	movew	a6@(CMDREG1B),d0
	btst	#6,d0			| test for forced precision
	jeq 	frcfpn_fpcr
	btst	#2,d0			| check for double
	jne 	frcfpn_dbl
	movel	#0x1,d0			| inst is forced single
	jra 	frcfpn_rnd
frcfpn_dbl:
	movel	#0x2,d0			| inst is forced double
	jra 	frcfpn_rnd
frcfpn_fpcr:
	bfextu	a6@(fpcr_MODE){#0:#2},d0	| inst not forced - use fpcr prec
	tstb	d0
	jeq 	frcfpn			| if extended, write what you got
frcfpn_rnd:
	bclr	#sign_bit,a6@(WBTEMP_EX)
	sne	a6@(WBTEMP_SGN)
	bsrl	__x_ovf_res			| get correct result based on
|					| round precision/mode.  This
|					| sets FPSR_CC correctly
	bfclr	a6@(WBTEMP_SGN){#0:#8}	| convert back to IEEE ext format
	jeq 	frcfpn_clr
	bset	#sign_bit,a6@(WBTEMP_EX)
frcfpn_clr:
	orl	#ovfinx_mask,a6@(USER_FPSR)
|
| Perform the write.
|
frcfpn:
	bfextu	a6@(CMDREG1B){#6:#3},d0	| extract fp destination register
	cmpib	#3,d0
	jle 	frc0123			| check if dest is fp0-fp3
	movel	#7,d1
	subl	d0,d1
	clrl	d0
	bset	d1,d0
	fmovemx	a6@(WBTEMP),d0
	rts
frc0123:
	cmpib	#0,d0
	jeq 	frc0_dst
	cmpib	#1,d0
	jeq 	frc1_dst
	cmpib	#2,d0
	jeq 	frc2_dst
frc3_dst:
	movel	a6@(WBTEMP_EX),a6@(USER_FP3)
	movel	a6@(WBTEMP_HI),a6@(USER_FP3+4)
	movel	a6@(WBTEMP_LO),a6@(USER_FP3+8)
	rts
frc2_dst:
	movel	a6@(WBTEMP_EX),a6@(USER_FP2)
	movel	a6@(WBTEMP_HI),a6@(USER_FP2+4)
	movel	a6@(WBTEMP_LO),a6@(USER_FP2+8)
	rts
frc1_dst:
	movel	a6@(WBTEMP_EX),a6@(USER_FP1)
	movel	a6@(WBTEMP_HI),a6@(USER_FP1+4)
	movel	a6@(WBTEMP_LO),a6@(USER_FP1+8)
	rts
frc0_dst:
	movel	a6@(WBTEMP_EX),a6@(USER_FP0)
	movel	a6@(WBTEMP_HI),a6@(USER_FP0+4)
	movel	a6@(WBTEMP_LO),a6@(USER_FP0+8)
	rts

|
| Write etemp to fpn.
| A check is made on enabled and signalled snan exceptions,
| and the destination is not overwritten if this condition exists.
| This code is designed to make fmoveins of unsupported data types
| faster.
|
wr_etemp:
	btst	#__x_snan_bit,a6@(FPSR_EXCEPT)	| if snan is set, and
	jeq 	fmoveinc			| enabled, force restore
	btst	#__x_snan_bit,a6@(fpcr_ENABLE) /* | and don't overwrite */
	jeq 	fmoveinc			| the dest
	movel	a6@(ETEMP_EX),a6@(FPTEMP_EX)	| set up fptemp sign for
|						| snan handler
	tstb	a6@(ETEMP)			| check for negative
	jlt 	__x_snan_neg
	rts
__x_snan_neg:
	orl	#neg_bit,a6@(USER_FPSR)		| snan is negative;  set N
	rts
fmoveinc:
	clrw	a6@(NMNEXC)
	bclr	#E1,a6@(E_BYTE)
	moveb	a6@(STAG),d0			| check if stag is inf
	andib	#0xe0,d0
	cmpib	#0x40,d0
	jne 	fminc_cnan
	orl	#inf_mask,a6@(USER_FPSR) 	| if inf, nothing yet has set I
	tstw	a0@(LOCAL_EX)			| check sign
	jge 	fminc_con
	orl	#neg_mask,a6@(USER_FPSR)
	jra 	fminc_con
fminc_cnan:
	cmpib	#0x60,d0			| check if stag is NaN
	jne 	fminc_czero
	orl	#nan_mask,a6@(USER_FPSR) 	| if nan nothing yet has set NaN
	movel	a6@(ETEMP_EX),a6@(FPTEMP_EX)	| set up fptemp sign for
|						| snan handler
	tstw	a0@(LOCAL_EX)			| check sign
	jge 	fminc_con
	orl	#neg_mask,a6@(USER_FPSR)
	jra 	fminc_con
fminc_czero:
	cmpib	#0x20,d0			| check if zero
	jne 	fminc_con
	orl	#z_mask,a6@(USER_FPSR)		| if zero, set Z
	tstw	a0@(LOCAL_EX)			| check sign
	jge 	fminc_con
	orl	#neg_mask,a6@(USER_FPSR)
fminc_con:
	bfextu	a6@(CMDREG1B){#6:#3},d0		| extract fp dest register
	cmpib	#3,d0
	jle 	fp0123				| check if dest is fp0-fp3
	movel	#7,d1
	subl	d0,d1
	clrl	d0
	bset	d1,d0
	fmovemx	a6@(ETEMP),d0
	rts

fp0123:
	cmpib	#0,d0
	jeq 	fp0_dst
	cmpib	#1,d0
	jeq 	fp1_dst
	cmpib	#2,d0
	jeq 	fp2_dst
fp3_dst:
	movel	a6@(ETEMP_EX),a6@(USER_FP3)
	movel	a6@(ETEMP_HI),a6@(USER_FP3+4)
	movel	a6@(ETEMP_LO),a6@(USER_FP3+8)
	rts
fp2_dst:
	movel	a6@(ETEMP_EX),a6@(USER_FP2)
	movel	a6@(ETEMP_HI),a6@(USER_FP2+4)
	movel	a6@(ETEMP_LO),a6@(USER_FP2+8)
	rts
fp1_dst:
	movel	a6@(ETEMP_EX),a6@(USER_FP1)
	movel	a6@(ETEMP_HI),a6@(USER_FP1+4)
	movel	a6@(ETEMP_LO),a6@(USER_FP1+8)
	rts
fp0_dst:
	movel	a6@(ETEMP_EX),a6@(USER_FP0)
	movel	a6@(ETEMP_HI),a6@(USER_FP0+4)
	movel	a6@(ETEMP_LO),a6@(USER_FP0+8)
	rts

opclass3:
	st	a6@(CU_ONLY)
	movew	a6@(CMDREG1B),d0	| check if packed moveout
	andiw	#0x0c00,d0		| isolate last 2 bits of size field
	cmpiw	#0x0c00,d0		| if size is 011 or 111, it is packed
	jeq 	pack_out		| else it is norm or denorm
	jra 	mv_out


|
|	MOVE OUT
|

mv_tbl:
	.long	li
	.long 	sgp
	.long 	xp
	.long 	mvout_end		| should never be taken
	.long 	wi
	.long 	dp
	.long 	bi
	.long 	mvout_end		| should never be taken
mv_out:
	bfextu	a6@(CMDREG1B){#3:#3},d1	| put source specifier in d1
	lea	mv_tbl,a0
	movel	a0@(d1:w:4),a0
	jmp	a0@

|
| This exit is for move-out to memory.  The aunfl bit is
| set if the result is inex and unfl is signalled.
|
mvout_end:
	btst	#__x_inex2_bit,a6@(FPSR_EXCEPT)
	jeq 	no_aufl
	btst	#__x_unfl_bit,a6@(FPSR_EXCEPT)
	jeq 	no_aufl
	bset	#aunfl_bit,a6@(FPSR_AEXCEPT)
no_aufl:
	clrw	a6@(NMNEXC)
	bclr	#E1,a6@(E_BYTE)
	fmovel	#0,FPSR			| clear any cc bits from __x_res_func
|
| Return ETEMP to extended format from internal extended format so
| that __x_gen_except will have a correctly signed value for ovfl/unfl
| handlers.
|
	bfclr	a6@(ETEMP_SGN){#0:#8}
	jeq 	mvout_con
	bset	#sign_bit,a6@(ETEMP_EX)
mvout_con:
	rts
|
| This exit is for move-out to int register.  The aunfl bit is
| not set in any case for this move.
|
mvouti_end:
	clrw	a6@(NMNEXC)
	bclr	#E1,a6@(E_BYTE)
	fmovel	#0,FPSR			| clear any cc bits from __x_res_func
|
| Return ETEMP to extended format from internal extended format so
| that __x_gen_except will have a correctly signed value for ovfl/unfl
| handlers.
|
	bfclr	a6@(ETEMP_SGN){#0:#8}
	jeq 	mvouti_con
	bset	#sign_bit,a6@(ETEMP_EX)
mvouti_con:
	rts
|
| li is used to handle a long integer source specifier
|

li:
	moveql	#4,d0			| set byte count

	btst	#7,a6@(STAG)		| check for extended denorm
	jne 	int_dnrm		| if so, branch

	fmovemx	a6@(ETEMP),fp0-fp0
	.long 0xf23c5438,0x41dfffff,0xffc00000
					/* fcmpd  &0x41dfffffffc00000,fp0 */
| 41dfffffffc00000 in dbl prec = 401d0000fffffffe00000000 in ext prec
	fbge	lo_plrg
	.long 0xf23c5438,0xc1e00000,0x00000000
					/* fcmpd  &0xc1e0000000000000,fp0 */
| c1e0000000000000 in dbl prec = c01e00008000000000000000 in ext prec
	fble	lo_nlrg
|
| at this point, the answer is between the largest pos and neg values
|
	movel	a6@(USER_FPCR),d1	/* | use user's rounding mode */
	andil	#0x30,d1
	fmovel	d1,fpcr
	fmovel	fp0,a6@(L_SCR1)	| let the 040 perform conversion
	fmovel fpsr,d1
	orl	d1,a6@(USER_FPSR)	| capture inex2/ainex if set
	jra 	int_wrt


lo_plrg:
	movel	#0x7fffffff,a6@(L_SCR1)	| answer is largest positive int
	fbeq	int_wrt			| exact answer
	.long 0xf23c5438,0x41dfffff,0xffe00000
					/* fcmpd  &0x41dfffffffe00000,fp0 */
| 41dfffffffe00000 in dbl prec = 401d0000ffffffff00000000 in ext prec
	fbge	int_operr		| set operr
	jra 	int_inx			| set inexact

lo_nlrg:
	movel	#0x80000000,a6@(L_SCR1)
	fbeq	int_wrt			| exact answer