res_func.s

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

	.long 0xf23c5438,0xc1e00000,0x00100000
					/* fcmpd  &0xc1e0000000100000,fp0 */
| c1e0000000100000 in dbl prec = c01e00008000000080000000 in ext prec
	fblt	int_operr		| set operr
	jra 	int_inx			| set inexact

|
| wi is used to handle a word integer source specifier
|

wi:
	moveql	#2,d0		| set byte count

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

	fmovemx	a6@(ETEMP),fp0-fp0
	.long 0xf23c4438,0x46fffe00	/* fcmps  &0x46fffe00,fp0 */

| 46fffe00 in sgl prec = 400d0000fffe000000000000 in ext prec
	fbge	wo_plrg
	.long 0xf23c4438,0xc7000000	/* fcmps  &0xc7000000,fp0 */

| c7000000 in sgl prec = c00e00008000000000000000 in ext prec
	fble	wo_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
	fmovew	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

wo_plrg:
	movew	#0x7fff,a6@(L_SCR1)	| answer is largest positive int
	fbeq	int_wrt			| exact answer
	.long 0xf23c4438,0x46ffff00	/* fcmps  &0x46ffff00,fp0 */
| 46ffff00 in sgl prec = 400d0000ffff000000000000 in ext prec
	fbge	int_operr		| set operr
	jra 	int_inx			| set inexact

wo_nlrg:
	movew	#0x8000,a6@(L_SCR1)
	fbeq	int_wrt			| exact answer
	.long 0xf23c4438,0xc7000080	/* fcmps  &0xc7000080,fp0 */
| c7000080 in sgl prec = c00e00008000800000000000 in ext prec
	fblt	int_operr		| set operr
	jra 	int_inx			| set inexact

|
| bi is used to handle a byte integer source specifier
|

bi:
	moveql	#1,d0		| set byte count

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

	fmovemx	a6@(ETEMP),fp0-fp0
	.long 0xf23c4438,0x42fe0000	/* fcmps &0x42fe0000,fp0 */
| 42fe0000 in sgl prec = 40050000fe00000000000000 in ext prec
	fbge	by_plrg
	.long 0xf23c4438,0xc3000000	/* fcmps  &0xc3000000,fp0 */
| c3000000 in sgl prec = c00600008000000000000000 in ext prec
	fble	by_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
	fmoveb	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

by_plrg:
	moveb	#0x7f,a6@(L_SCR1)		| answer is largest positive int
	fbeq	int_wrt			| exact answer
	.long 0xf23c4438,0x42ff0000	/*  fcmps  &0x42ff0000,fp0 */
| 42ff0000 in sgl prec = 40050000ff00000000000000 in ext prec
	fbge	int_operr		| set operr
	jra 	int_inx			| set inexact

by_nlrg:
	moveb	#0x80,a6@(L_SCR1)
	fbeq	int_wrt			| exact answer
	.long 0xf23c4438,0xc3008000	/*  fcmps&0xc3008000,fp0 */
| c3008000 in sgl prec = c00600008080000000000000 in ext prec
	fblt	int_operr		| set operr
	jra 	int_inx			| set inexact

|
| Common integer routines
|
| int_drnrm---account for possible nonzero result for round up with positive
| operand and round down for negative answer.  In the first case (result = 1)
| byte-width (store in d0) of result must be honored.  In the second case,
| -1 in	a6@(L_SCR1) will cover all contingencies (FMOVE.B/W/L out).

int_dnrm:
	movel	#0,a6@(L_SCR1)	| initialize result to 0
	bfextu	a6@(fpcr_MODE){#2:#2},d1	|  d1 is the rounding mode
	cmpb	#2,d1
	jmi 	int_inx		|  if RN or RZ, done
	jne 	int_rp		|  if RP, continue below
	tstw	a6@(ETEMP)	|  RM: store -1 in L_SCR1 if src is negative
	jpl 	int_inx		|  otherwise result is 0
	movel	#-1,a6@(L_SCR1)
	jra 	int_inx
int_rp:
	tstw	a6@(ETEMP)	|  RP: store +1 of proper width in L_SCR1 if
|				|  source is greater than 0
	jmi 	int_inx		|  otherwise, result is 0
	lea	a6@(L_SCR1),a1	|  a1 is address of L_SCR1
	addal	d0,a1		|  offset by destination width -1
	subal	#1,a1
	bset	#0,a1@		|  set low bit at a1 address
int_inx:
	oril	#inx2a_mask,a6@(USER_FPSR)
	jra 	int_wrt
int_operr:
	fmovemx fp0-fp0,a6@(FPTEMP)	| FPTEMP must contain the extended
|				| precision source that needs to be
|				| converted to integer this is required
|				| if the operr exception is enabled.
|				| set operr/aiop (no inex2 on int ovfl)

	oril	#opaop_mask,a6@(USER_FPSR)
|				| fall through to perform int_wrt
int_wrt:
	movel	a6@(EXC_EA),a1	| load destination address
	tstl	a1		| check to see if it is a dest register
	jeq 	wrt_dn		| write data register
	lea	a6@(L_SCR1),a0	| point to supervisor source address
	bsrl	__x_mem_write
	jra 	mvouti_end

wrt_dn:
	movel	d0,a7@-		| d0 currently contains the size to write
	bsrl	__x_get_fline	| get_fline returns Dn in d0
	andiw	#0x7,d0		| isolate register
	movel	a7@+,d1		| get size
	cmpil	#4,d1		| most frequent case
	jeq 	sz_long
	cmpil	#2,d1
	jne 	sz_con
	orl	#8,d0		/* | add 'word' size to register# */
	jra 	sz_con
sz_long:
	orl	#0x10,d0		/* | add 'long' size to register# */
sz_con:
	movel	d0,d1		| reg_dest expects size:reg in d1
	bsrl	__x_reg_dest	| load proper data register
	jra 	mvouti_end
xp:
	lea	a6@(ETEMP),a0
	bclr	#sign_bit,a0@(LOCAL_EX)
	sne	a0@(LOCAL_SGN)
	btst	#7,a6@(STAG)	| check for extended denorm
	jne 	xdnrm
	clrl	d0
	jra 	do_fp		| do normal case
sgp:
	lea	a6@(ETEMP),a0
	bclr	#sign_bit,a0@(LOCAL_EX)
	sne	a0@(LOCAL_SGN)
	btst	#7,a6@(STAG)	| check for extended denorm
	jne 	sp_catas	| branch if so
	movew	a0@(LOCAL_EX),d0
	lea	sp_bnds,a1
	cmpw	a1@,d0
	jlt 	sp_under
	cmpw	a1@(2),d0
	jgt 	sp_over
	movel	#1,d0		| set destination format to single
	jra 	do_fp		| do normal case
dp:
	lea	a6@(ETEMP),a0
	bclr	#sign_bit,a0@(LOCAL_EX)
	sne	a0@(LOCAL_SGN)

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

	movew	a0@(LOCAL_EX),d0
	lea	dp_bnds,a1

	cmpw	a1@,d0
	jlt 	dp_under
	cmpw	a1@(2),d0
	jgt 	dp_over

	movel	#2,d0		| set destination format to double
|				| fall through to do_fp
|
do_fp:
	bfextu	a6@(fpcr_MODE){#2:#2},d1	| rnd mode in d1
	swap	d0			| rnd prec in upper word
	addl	d0,d1			| d1 has PREC/MODE info

	clrl	d0			| clear g,r,s

	bsrl	__x_round			| round

	movel	a0,a1
	movel	a6@(EXC_EA),a0

	bfextu	a6@(CMDREG1B){#3:#3},d1	| extract destination format
|					| at this point only the dest
|					| formats sgl, dbl, ext are
|					| possible
	cmpb	#2,d1
	jgt 	ddbl			| double=5, extended=2, single=1
	jne 	dsgl
|					| fall through to dext
dext:
	bsrl	__x_dest_ext
	jra 	mvout_end
dsgl:
	bsrl	__x_dest_sgl
	jra 	mvout_end
ddbl:
	bsrl	__x_dest_dbl
	jra 	mvout_end

|
| Handle possible denorm or catastrophic underflow cases here
|
xdnrm:
	bsrl	set_xop		| initialize WBTEMP
	bset	#wbtemp15_bit,a6@(WB_BYTE) | set wbtemp15

	movel	a0,a1
	movel	a6@(EXC_EA),a0	| a0 has the destination pointer
	bsrl	__x_dest_ext	| store to memory
	bset	#__x_unfl_bit,a6@(FPSR_EXCEPT)
	jra 	mvout_end

sp_under:
	bset	#etemp15_bit,a6@(STAG)

	cmpw	a1@(4),d0
	jle 	sp_catas	| catastrophic underflow case

	movel	#1,d0		| load in round precision
	movel	#sgl_thresh,d1	| load in single denorm threshold
	bsrl	dpspdnrm	| expects d1 to have the proper
|				| denorm threshold
	bsrl	__x_dest_sgl	| stores value to destination
	bset	#__x_unfl_bit,a6@(FPSR_EXCEPT)
	jra 	mvout_end	| exit

dp_under:
	bset	#etemp15_bit,a6@(STAG)

	cmpw	a1@(4),d0
	jle 	dp_catas	| catastrophic underflow case

	movel	#dbl_thresh,d1	| load in double precision threshold
	movel	#2,d0
	bsrl	dpspdnrm	| expects d1 to have proper
|				| denorm threshold
|				| expects d0 to have round precision
	bsrl	__x_dest_dbl	| store value to destination
	bset	#__x_unfl_bit,a6@(FPSR_EXCEPT)
	jra 	mvout_end	| exit

|
| Handle catastrophic underflow cases here
|
sp_catas:
| Temp fix for z bit set in __x_unf_sub
	movel	a6@(USER_FPSR),a7@-

	movel	#1,d0		| set round precision to sgl

	bsrl	__x_unf_sub		| a0 points to result

	movel	a7@+,a6@(USER_FPSR)

	movel	#1,d0
	subw	d0,a0@(LOCAL_EX) | account for difference between
|				| denorm/norm bias

	movel	a0,a1		| a1 has the operand input
	movel	a6@(EXC_EA),a0	| a0 has the destination pointer

	bsrl	__x_dest_sgl	| store the result
	oril	#unfinx_mask,a6@(USER_FPSR)
	jra 	mvout_end

dp_catas:
| Temp fix for z bit set in __x_unf_sub
	movel	a6@(USER_FPSR),a7@-

	movel	#2,d0		| set round precision to dbl
	bsrl	__x_unf_sub		| a0 points to result

	movel	a7@+,a6@(USER_FPSR)

	movel	#1,d0
	subw	d0,a0@(LOCAL_EX) | account for difference between
|				| denorm/norm bias

	movel	a0,a1		| a1 has the operand input
	movel	a6@(EXC_EA),a0	| a0 has the destination pointer

	bsrl	__x_dest_dbl	| store the result
	oril	#unfinx_mask,a6@(USER_FPSR)
	jra 	mvout_end

|
| Handle catastrophic overflow cases here
|
sp_over:
| Temp fix for z bit set in __x_unf_sub
	movel	a6@(USER_FPSR),a7@-

	movel	#1,d0
	lea	a6@(FP_SCR1),a0	| use FP_SCR1 for creating result
	movel	a6@(ETEMP_EX),a0@
	movel	a6@(ETEMP_HI),a0@(4)
	movel	a6@(ETEMP_LO),a0@(8)
	bsrl	__x_ovf_res

	movel	a7@+,a6@(USER_FPSR)

	movel	a0,a1
	movel	a6@(EXC_EA),a0
	bsrl	__x_dest_sgl
	orl	#ovfinx_mask,a6@(USER_FPSR)
	jra 	mvout_end

dp_over:
| Temp fix for z bit set in __x_ovf_res
	movel	a6@(USER_FPSR),a7@-

	movel	#2,d0
	lea	a6@(FP_SCR1),a0	| use FP_SCR1 for creating result
	movel	a6@(ETEMP_EX),a0@
	movel	a6@(ETEMP_HI),a0@(4)
	movel	a6@(ETEMP_LO),a0@(8)
	bsrl	__x_ovf_res

	movel	a7@+,a6@(USER_FPSR)

	movel	a0,a1
	movel	a6@(EXC_EA),a0
	bsrl	__x_dest_dbl
	orl	#ovfinx_mask,a6@(USER_FPSR)
	jra 	mvout_end

|
| 	DPSPDNRM
|
| This subroutine takes an extended normalized number and denormalizes
| it to the given round precision. This subroutine also decrements
/* | the input operand's exponent by 1 to account for the fact that */
/* | __x_dest_sgl or __x_dest_dbl expects a normalized number's bias. */
|
| Input: a0  points to a normalized number in internal extended format
|	 d0  is the round precision (=1 for sgl|  =2 for dbl)
|	 d1  is the the single precision or double precision
|	     denorm threshold
|
| Output: (In the format for __x_dest_sgl or __x_dest_dbl)
|	 a0   points to the destination
|   	 a1   points to the operand
|
| Exceptions: Reports inexact 2 exception by setting USER_FPSR bits
|
dpspdnrm:
	movel	d0,a7@-	| save round precision
	clrl	d0		| clear initial g,r,s
	bsrl	__x_dnrm_lp	/*  careful with d0, it's needed by __x_round */

	bfextu	a6@(fpcr_MODE){#2:#2},d1 | get rounding mode
	swap	d1
	movew	a7@(2),d1	| set rounding precision
	swap	d1		| at this point d1 has PREC/MODE info
	bsrl	__x_round		| round result, sets the inex bit in
|				| USER_FPSR if needed

	movew	#1,d0
	subw	d0,a0@(LOCAL_EX) | account for difference in denorm
|				| vs norm bias

	movel	a0,a1		| a1 has the operand input
	movel	a6@(EXC_EA),a0	| a0 has the destination pointer
	addw	#4,a7		| pop stack
	rts
|
| SET_XOP initialized WBTEMP with the value pointed to by a0
| input: a0 points to input operand in the internal extended format
|
set_xop:
	movel	a0@(LOCAL_EX),a6@(WBTEMP_EX)
	movel	a0@(LOCAL_HI),a6@(WBTEMP_HI)
	movel	a0@(LOCAL_LO),a6@(WBTEMP_LO)
	bfclr	a6@(WBTEMP_SGN){#0:#8}
	jeq 	sxop
	bset	#sign_bit,a6@(WBTEMP_EX)
sxop:
	bfclr	a6@(STAG){#5:#4}	| clear wbtm66,wbtm1,wbtm0,sbit
	rts
|
|	P_MOVE
|
__x_p_movet:
	.long	__x_p_move
	.long	__x_p_movez
	.long	__x_p_movei
	.long	__x_p_moven
	.long	__x_p_move
p_regd:
	.long	p_dyd0
	.long	p_dyd1
	.long	p_dyd2
	.long	p_dyd3
	.long	p_dyd4
	.long	p_dyd5
	.long	p_dyd6
	.long	p_dyd7

pack_out:
 	lea	__x_p_movet,a0	| load jmp table address
	movew	a6@(STAG),d0	| get source tag
	bfextu	d0{#16:#3},d0	| isolate source bits
	movel	a0@(d0:w:4),a0	| load a0 with routine label for tag
	jmp	a0@		| go to the routine

p_write:
	movel	#0x0c,d0 	| get byte count
	movel	a6@(EXC_EA),a1	| get the destination address
	bsrl 	__x_mem_write	/* | write the user's destination */
	moveb	#0,a6@(CU_SAVEPC) /* | set the cu save pc to all 0's */

|
| Also note that the dtag must be set to norm here - this is because
| the 040 uses the dtag to execute the correct microcode.
|
        bfclr   	a6@(DTAG){#0:#3}  | set dtag to norm

	rts

| Notes on handling of special case (zero, inf, and nan) inputs:
|	1. Operr is not signalled if the k-factor is greater than 18.
|	2. Per the manual, status bits are not set.
|

__x_p_move:
	movew	a6@(CMDREG1B),d0
	btst	#kfact_bit,d0	| test for dynamic k-factor
	jeq 	statick		| if clear, k-factor is static
dynamick:
	bfextu	d0{#25:#3},d0	| isolate register for dynamic k-factor
	lea	p_regd,a0
	movel	a0@(d0:w:4),a0
	jmp	a0@
statick:
	andiw	#0x007f,d0	| get k-factor
	bfexts	d0{#25:#7},d0	| sign extend d0 for __x_bindec
	lea	a6@(ETEMP),a0	| a0 will point to the packed decimal
	bsrl	__x_bindec		| perform the convert;  data at a6
	lea	a6@(FP_SCR1),a0	| load a0 with result address
	jra 	p_write
__x_p_movez:
	lea	a6@(ETEMP),a0	| a0 will point to the packed decimal
	clrw	a0@(2)		| clear lower word of exp
	clrl	a0@(4)		| load second lword of ZERO
	clrl	a0@(8)		| load third lword of ZERO
	jra 	p_write		| go write results
__x_p_movei:
	fmovel	#0,FPSR		| clear aiop
	lea	a6@(ETEMP),a0	| a0 will point to the packed decimal
	clrw	a0@(2)		| clear lower word of exp
	jra 	p_write		| go write the result
__x_p_moven:
	lea	a6@(ETEMP),a0	| a0 will point to the packed decimal
	clrw	a0@(2)		| clear lower word of exp
	jra 	p_write		| go write the result

|
| Routines to read the dynamic k-factor from Dn.
|
p_dyd0:
	movel	a6@(USER_D0),d0
	jra 	statick
p_dyd1:
	movel	a6@(USER_D1),d0
	jra 	statick
p_dyd2:
	movel	d2,d0
	jra 	statick
p_dyd3:
	movel	d3,d0
	jra 	statick
p_dyd4:
	movel	d4,d0
	jra 	statick
p_dyd5:
	movel	d5,d0
	jra 	statick
p_dyd6:
	movel	d6,d0
	jra 	statick
p_dyd7:
	movel	d7,d0
	jra 	statick

|	end