fp_util.s

MIZI Research, Inc.发布的嵌入式Linux内核源码

	jra	9b			| positive, round to zero
1:	tst.b	(1,%a0)			| to +inf
	jeq	fp_ne_doroundup		| positive, round to infinity
	jra	9b			| negative, round to zero
#endif
	| Zeros and subnormal numbers
	| These are probably merely subnormal, rather than "denormalized"
	|  numbers, so we will try to make them normal again.
fp_ne_small:
	jne	fp_ne_small1		| high lword zero?
	move.l	(4,%a0),%d0
	jne	fp_ne_small2
#ifdef CONFIG_M68KFPU_EMU_EXTRAPREC
	clr.l	%d0
	move.b	(-4,%a0),%d0
	jne	fp_ne_small3
#endif
	| Genuine zero.
	clr.w	-(%a0)
	subq.l	#2,%a0
	printf	PNORM,"%p(",1,%a0
	printx	PNORM,%a0@
	printf	PNORM,")\n"
	rts
	| Subnormal.
fp_ne_small1:
	bfffo	%d0{#0,#32},%d1
	move.w	-(%a0),%d2
	sub.w	%d1,%d2
	jcc	1f
	| Pathologically small, denormalize.
	add.w	%d2,%d1
	clr.w	%d2
	fp_set_sr FPSR_EXC_UNFL
1:	move.w	%d2,(%a0)+
	move.w	%d1,%d2
	jeq	fp_ne_checkround
	| This is exactly the same 64-bit double shift as seen above.
	lsl.l	%d2,%d0
	move.l	%d0,(%a0)+
	move.l	(%a0),%d0
	move.l	%d0,%d1
	lsl.l	%d2,%d0
	move.l	%d0,(%a0)
	neg.w	%d2
	and.w	#0x1f,%d2
	lsr.l	%d2,%d1
	or.l	%d1,-(%a0)
#ifdef CONFIG_M68KFPU_EMU_EXTRAPREC
fp_ne_extra1:
	clr.l	%d0
	move.b	(-4,%a0),%d0
	neg.w	%d2
	add.w	#24,%d2
	jcc	1f
	clr.b	(-4,%a0)
	lsl.l	%d2,%d0
	or.l	%d0,(4,%a0)
	jra	fp_ne_checkround
1:	addq.w	#8,%d2
	lsl.l	%d2,%d0
	move.b	%d0,(-4,%a0)
	lsr.l	#8,%d0
	or.l	%d0,(4,%a0)
#endif
	jra	fp_ne_checkround
	| May or may not be subnormal, if so, only 32 bits to shift.
fp_ne_small2:
	bfffo	%d0{#0,#32},%d1
	add.w	#32,%d1
	move.w	-(%a0),%d2
	sub.w	%d1,%d2
	jcc	1f
	| Beyond pathologically small, denormalize.
	add.w	%d2,%d1
	clr.w	%d2
	fp_set_sr FPSR_EXC_UNFL
1:	move.w	%d2,(%a0)+
	ext.l	%d1
	jeq	fp_ne_checkround
	clr.l	(4,%a0)
	sub.w	#32,%d1
	jcs	1f
	lsl.l	%d1,%d0			| lower lword needs only to be shifted
	move.l	%d0,(%a0)		| into the higher lword
#ifdef CONFIG_M68KFPU_EMU_EXTRAPREC
	clr.l	%d0
	move.b	(-4,%a0),%d0
	clr.b	(-4,%a0)
	neg.w	%d1
	add.w	#32,%d1
	bfins	%d0,(%a0){%d1,#8}
#endif
	jra	fp_ne_checkround
1:	neg.w	%d1			| lower lword is splitted between
	bfins	%d0,(%a0){%d1,#32}	| higher and lower lword
#ifndef CONFIG_M68KFPU_EMU_EXTRAPREC
	jra	fp_ne_checkround
#else
	move.w	%d1,%d2
	jra	fp_ne_extra1
	| These are extremely small numbers, that will mostly end up as zero
	| anyway, so this is only important for correct rounding.
fp_ne_small3:
	bfffo	%d0{#24,#8},%d1
	add.w	#40,%d1
	move.w	-(%a0),%d2
	sub.w	%d1,%d2
	jcc	1f
	| Pathologically small, denormalize.
	add.w	%d2,%d1
	clr.w	%d2
1:	move.w	%d2,(%a0)+
	ext.l	%d1
	jeq	fp_ne_checkround
	cmp.w	#8,%d1
	jcs	2f
1:	clr.b	(-4,%a0)
	sub.w	#64,%d1
	jcs	1f
	add.w	#24,%d1
	lsl.l	%d1,%d0
	move.l	%d0,(%a0)
	jra	fp_ne_checkround
1:	neg.w	%d1
	bfins	%d0,(%a0){%d1,#8}
	jra	fp_ne_checkround
2:	lsl.l	%d1,%d0
	move.b	%d0,(-4,%a0)
	lsr.l	#8,%d0
	move.b	%d0,(7,%a0)
	jra	fp_ne_checkround
#endif
	| Infinities and NaNs, again, same as above.
fp_ne_large:
	move.l	(%a0)+,%d0
	jne	3f
1:	tst.l	(%a0)
	jne	4f
2:	subq.l	#8,%a0
	printf	PNORM,"%p(",1,%a0
	printx	PNORM,%a0@
	printf	PNORM,")\n"
	rts
	| we have maybe a NaN, shift off the highest bit
3:	move.l	%d0,%d1
	lsl.l	#1,%d1
	jne	4f
	clr.l	(-4,%a0)
	jra	1b
	| we have a NaN, test if it is signaling
4:	bset	#30,%d0
	jne	2b
	fp_set_sr FPSR_EXC_SNAN
	move.l	%d0,(-4,%a0)
	jra	2b

	| these next two do rounding as per the IEEE standard.
	| values for the rounding modes appear to be:
	| 0:	Round to nearest
	| 1:	Round to zero
	| 2:	Round to -Infinity
	| 3:	Round to +Infinity
	| both functions expect that fp_normalize was already
	| called (and extended argument is already normalized
	| as far as possible), these are used if there is different
	| rounding precision is selected and before converting
	| into single/double

	| fp_normalize_double:
	| normalize an extended with double (52-bit) precision
	| args:	 %a0 (struct fp_ext *)

fp_normalize_double:
	printf	PNORM,"nd: %p(",1,%a0
	printx	PNORM,%a0@
	printf	PNORM,"), "
	move.l	(%a0)+,%d2
	tst.w	%d2
	jeq	fp_nd_zero		| zero / denormalized
	cmp.w	#0x7fff,%d2
	jeq	fp_nd_huge		| NaN / infinitive.
	sub.w	#0x4000-0x3ff,%d2	| will the exponent fit?
	jcs	fp_nd_small		| too small.
	cmp.w	#0x7fe,%d2
	jcc	fp_nd_large		| too big.
	addq.l	#4,%a0
	move.l	(%a0),%d0		| low lword of mantissa
	| now, round off the low 11 bits.
fp_nd_round:
	moveq	#21,%d1
	lsl.l	%d1,%d0			| keep 11 low bits.
	jne	fp_nd_checkround	| Are they non-zero?
	| nothing to do here
9:	subq.l	#8,%a0
	printf	PNORM,"%p(",1,%a0
	printx	PNORM,%a0@
	printf	PNORM,")\n"
	rts
	| Be careful with the X bit! It contains the lsb
	| from the shift above, it is needed for round to nearest.
fp_nd_checkround:
	fp_set_sr FPSR_EXC_INEX2	| INEX2 bit
	and.w	#0xf800,(2,%a0)		| clear bits 0-10
	move.w	(FPD_RND,FPDATA),%d2	| rounding mode
	jne	2f			| %d2 == 0, round to nearest
	tst.l	%d0			| test guard bit
	jpl	9b			| zero is closer
	| here we test the X bit by adding it to %d2
	clr.w	%d2			| first set z bit, addx only clears it
	addx.w	%d2,%d2			| test lsb bit
	| IEEE754-specified "round to even" behaviour.  If the guard
	| bit is set, then the number is odd, so rounding works like
	| in grade-school arithmetic (i.e. 1.5 rounds to 2.0)
	| Otherwise, an equal distance rounds towards zero, so as not
	| to produce an odd number.  This is strange, but it is what
	| the standard says.
	jne	fp_nd_doroundup		| round to infinity
	lsl.l	#1,%d0			| check low bits
	jeq	9b			| round to zero
fp_nd_doroundup:
	| round (the mantissa, that is) towards infinity
	add.l	#0x800,(%a0)
	jcc	9b			| no overflow, good.
	addq.l	#1,-(%a0)		| extend to high lword
	jcc	1f			| no overflow, good.
	| Yow! we have managed to overflow the mantissa.  Since this
	| only happens when %d1 was 0xfffff800, it is now zero, so
	| reset the high bit, and increment the exponent.
	move.w	#0x8000,(%a0)
	addq.w	#1,-(%a0)
	cmp.w	#0x43ff,(%a0)+		| exponent now overflown?
	jeq	fp_nd_large		| yes, so make it infinity.
1:	subq.l	#4,%a0
	printf	PNORM,"%p(",1,%a0
	printx	PNORM,%a0@
	printf	PNORM,")\n"
	rts
2:	subq.w	#2,%d2
	jcs	9b			| %d2 < 2, round to zero
	jhi	3f			| %d2 > 2, round to +infinity
	| Round to +Inf or -Inf.  High word of %d2 contains the
	| sign of the number, by the way.
	swap	%d2			| to -inf
	tst.b	%d2
	jne	fp_nd_doroundup		| negative, round to infinity
	jra	9b			| positive, round to zero
3:	swap	%d2			| to +inf
	tst.b	%d2
	jeq	fp_nd_doroundup		| positive, round to infinity
	jra	9b			| negative, round to zero
	| Exponent underflow.  Try to make a denormal, and set it to
	| the smallest possible fraction if this fails.
fp_nd_small:
	fp_set_sr FPSR_EXC_UNFL		| set UNFL bit
	move.w	#0x3c01,(-2,%a0)	| 2**-1022
	neg.w	%d2			| degree of underflow
	cmp.w	#32,%d2			| single or double shift?
	jcc	1f
	| Again, another 64-bit double shift.
	move.l	(%a0),%d0
	move.l	%d0,%d1
	lsr.l	%d2,%d0
	move.l	%d0,(%a0)+
	move.l	(%a0),%d0
	lsr.l	%d2,%d0
	neg.w	%d2
	add.w	#32,%d2
	lsl.l	%d2,%d1
	or.l	%d1,%d0
	move.l	(%a0),%d1
	move.l	%d0,(%a0)
	| Check to see if we shifted off any significant bits
	lsl.l	%d2,%d1
	jeq	fp_nd_round		| Nope, round.
	bset	#0,%d0			| Yes, so set the "sticky bit".
	jra	fp_nd_round		| Now, round.
	| Another 64-bit single shift and store
1:	sub.w	#32,%d2
	cmp.w	#32,%d2			| Do we really need to shift?
	jcc	2f			| No, the number is too small.
	move.l	(%a0),%d0
	clr.l	(%a0)+
	move.l	%d0,%d1
	lsr.l	%d2,%d0
	neg.w	%d2
	add.w	#32,%d2
	| Again, check to see if we shifted off any significant bits.
	tst.l	(%a0)
	jeq	1f
	bset	#0,%d0			| Sticky bit.
1:	move.l	%d0,(%a0)
	lsl.l	%d2,%d1
	jeq	fp_nd_round
	bset	#0,%d0
	jra	fp_nd_round
	| Sorry, the number is just too small.
2:	clr.l	(%a0)+
	clr.l	(%a0)
	moveq	#1,%d0			| Smallest possible fraction,
	jra	fp_nd_round		| round as desired.
	| zero and denormalized
fp_nd_zero:
	tst.l	(%a0)+
	jne	1f
	tst.l	(%a0)
	jne	1f
	subq.l	#8,%a0
	printf	PNORM,"%p(",1,%a0
	printx	PNORM,%a0@
	printf	PNORM,")\n"
	rts				| zero.  nothing to do.
	| These are not merely subnormal numbers, but true denormals,
	| i.e. pathologically small (exponent is 2**-16383) numbers.
	| It is clearly impossible for even a normal extended number
	| with that exponent to fit into double precision, so just
	| write these ones off as "too darn small".
1:	fp_set_sr FPSR_EXC_UNFL		| Set UNFL bit
	clr.l	(%a0)
	clr.l	-(%a0)
	move.w	#0x3c01,-(%a0)		| i.e. 2**-1022
	addq.l	#6,%a0
	moveq	#1,%d0
	jra	fp_nd_round		| round.
	| Exponent overflow.  Just call it infinity.
fp_nd_large:
	move.w	#0x7ff,%d0
	and.w	(6,%a0),%d0
	jeq	1f
	fp_set_sr FPSR_EXC_INEX2
1:	fp_set_sr FPSR_EXC_OVFL
	move.w	(FPD_RND,FPDATA),%d2
	jne	3f			| %d2 = 0 round to nearest
1:	move.w	#0x7fff,(-2,%a0)
	clr.l	(%a0)+
	clr.l	(%a0)
2:	subq.l	#8,%a0
	printf	PNORM,"%p(",1,%a0
	printx	PNORM,%a0@
	printf	PNORM,")\n"
	rts
3:	subq.w	#2,%d2
	jcs	5f			| %d2 < 2, round to zero
	jhi	4f			| %d2 > 2, round to +infinity
	tst.b	(-3,%a0)		| to -inf
	jne	1b
	jra	5f
4:	tst.b	(-3,%a0)		| to +inf
	jeq	1b
5:	move.w	#0x43fe,(-2,%a0)
	moveq	#-1,%d0
	move.l	%d0,(%a0)+
	move.w	#0xf800,%d0
	move.l	%d0,(%a0)
	jra	2b
	| Infinities or NaNs
fp_nd_huge:
	subq.l	#4,%a0
	printf	PNORM,"%p(",1,%a0
	printx	PNORM,%a0@
	printf	PNORM,")\n"
	rts

	| fp_normalize_single:
	| normalize an extended with single (23-bit) precision
	| args:	 %a0 (struct fp_ext *)

fp_normalize_single:
	printf	PNORM,"ns: %p(",1,%a0
	printx	PNORM,%a0@
	printf	PNORM,") "
	addq.l	#2,%a0
	move.w	(%a0)+,%d2
	jeq	fp_ns_zero		| zero / denormalized
	cmp.w	#0x7fff,%d2
	jeq	fp_ns_huge		| NaN / infinitive.
	sub.w	#0x4000-0x7f,%d2	| will the exponent fit?
	jcs	fp_ns_small		| too small.
	cmp.w	#0xfe,%d2
	jcc	fp_ns_large		| too big.
	move.l	(%a0)+,%d0		| get high lword of mantissa
fp_ns_round:
	tst.l	(%a0)			| check the low lword
	jeq	1f
	| Set a sticky bit if it is non-zero.  This should only
	| affect the rounding in what would otherwise be equal-
	| distance situations, which is what we want it to do.
	bset	#0,%d0
1:	clr.l	(%a0)			| zap it from memory.
	| now, round off the low 8 bits of the hi lword.
	tst.b	%d0			| 8 low bits.
	jne	fp_ns_checkround	| Are they non-zero?
	| nothing to do here
	subq.l	#8,%a0
	printf	PNORM,"%p(",1,%a0
	printx	PNORM,%a0@
	printf	PNORM,")\n"
	rts
fp_ns_checkround:
	fp_set_sr FPSR_EXC_INEX2	| INEX2 bit
	clr.b	-(%a0)			| clear low byte of high lword
	subq.l	#3,%a0
	move.w	(FPD_RND,FPDATA),%d2	| rounding mode
	jne	2f			| %d2 == 0, round to nearest
	tst.b	%d0			| test guard bit
	jpl	9f			| zero is closer
	btst	#8,%d0			| test lsb bit
	| round to even behaviour, see above.
	jne	fp_ns_doroundup		| round to infinity
	lsl.b	#1,%d0			| check low bits
	jeq	9f			| round to zero
fp_ns_doroundup:
	| round (the mantissa, that is) towards infinity
	add.l	#0x100,(%a0)
	jcc	9f			| no overflow, good.
	| Overflow.  This means that the %d1 was 0xffffff00, so it
	| is now zero.  We will set the mantissa to reflect this, and
	| increment the exponent (checking for overflow there too)
	move.w	#0x8000,(%a0)
	addq.w	#1,-(%a0)
	cmp.w	#0x407f,(%a0)+		| exponent now overflown?
	jeq	fp_ns_large		| yes, so make it infinity.
9:	subq.l	#4,%a0
	printf	PNORM,"%p(",1,%a0
	printx	PNORM,%a0@
	printf	PNORM,")\n"
	rts
	| check nondefault rounding modes
2:	subq.w	#2,%d2
	jcs	9b			| %d2 < 2, round to zero
	jhi	3f			| %d2 > 2, round to +infinity
	tst.b	(-3,%a0)		| to -inf
	jne	fp_ns_doroundup		| negative, round to infinity
	jra	9b			| positive, round to zero
3:	tst.b	(-3,%a0)		| to +inf
	jeq	fp_ns_doroundup		| positive, round to infinity
	jra	9b			| negative, round to zero
	| Exponent underflow.  Try to make a denormal, and set it to
	| the smallest possible fraction if this fails.
fp_ns_small:
	fp_set_sr FPSR_EXC_UNFL		| set UNFL bit
	move.w	#0x3f81,(-2,%a0)	| 2**-126
	neg.w	%d2			| degree of underflow
	cmp.w	#32,%d2			| single or double shift?
	jcc	2f
	| a 32-bit shift.
	move.l	(%a0),%d0
	move.l	%d0,%d1
	lsr.l	%d2,%d0
	move.l	%d0,(%a0)+
	| Check to see if we shifted off any significant bits.
	neg.w	%d2
	add.w	#32,%d2
	lsl.l	%d2,%d1
	jeq	1f
	bset	#0,%d0			| Sticky bit.
	| Check the lower lword
1:	tst.l	(%a0)
	jeq	fp_ns_round
	clr	(%a0)
	bset	#0,%d0			| Sticky bit.
	jra	fp_ns_round
	| Sorry, the number is just too small.
2:	clr.l	(%a0)+
	clr.l	(%a0)
	moveq	#1,%d0			| Smallest possible fraction,
	jra	fp_ns_round		| round as desired.
	| Exponent overflow.  Just call it infinity.
fp_ns_large:
	tst.b	(3,%a0)
	jeq	1f
	fp_set_sr FPSR_EXC_INEX2
1:	fp_set_sr FPSR_EXC_OVFL
	move.w	(FPD_RND,FPDATA),%d2
	jne	3f			| %d2 = 0 round to nearest
1:	move.w	#0x7fff,(-2,%a0)
	clr.l	(%a0)+
	clr.l	(%a0)
2:	subq.l	#8,%a0
	printf	PNORM,"%p(",1,%a0
	printx	PNORM,%a0@
	printf	PNORM,")\n"
	rts
3:	subq.w	#2,%d2
	jcs	5f			| %d2 < 2, round to zero
	jhi	4f			| %d2 > 2, round to +infinity