fpsoft.s

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

	addi	t8, -1
	bne	t8, zero, 2b

	srl	t6, t6, 2		/*  shift answer right by 2 */
					/*    to eliminate extra bits */

	move	t8, t4			/*  form sticky bit */
	move	t2, t6

	b	norm_s

/*******************************************************************************
*
* sqrt_d - Square root double
*
*/
FUNC_LABEL(sqrt_d)
	/*
	 * Break out the operand into its fields (sign,exp,fraction) and
	 * handle a NaN operand by calling rs_breakout_d() .
	 */
	li	t9,C1_FMT_DOUBLE*4
	move	v1,zero
	jal	rs_breakout_d

	/* Check for sqrt of infinity, and produce the correct action if so */
	bne	t1,DEXP_INF,4f	/* is RS an infinity? */
				/* RS is an infinity */
	beq	t0,zero,3f	/* check for -infinity */
	/*
	 * This is -infinity so this is an invalid operation for sqrt so set
	 * the invalid exception in the C1_SR (a3) and setup the result
	 * depending if the enable for the invalid exception is set.
	 */
1:	or	a3,INVALID_EXC
	and	v0,a3,INVALID_ENABLE
	beq	v0,zero,2f
	/*
	 * The invalid trap was enabled so signal a SIGFPE and leave the
	 * result register unmodified.
	 */
	li	v0, IV_FPA_INV_VEC
	jal	post_signal
	li	v0,1
	b	store_C1_SR
	/*
	 * The invalid trap was NOT enabled so the result is a quiet NaN.
	 * So use the default quiet NaN and exit softFp().
	 */
2:	li	t2,DQUIETNAN_LESS
	li	t3,DQUIETNAN_LEAST
	move	v0,zero
	b	rd_2w
	/*
	 * This is +infinity so the result is just +infinity.
	 */
3:	sll	t2,t1,DEXP_SHIFT
	move	v0,zero
	b	rd_2w
4:	/* Check for the sqrt of zero and produce the correct action if so */
	bne	t1,zero,5f	/* check RS for a zero value (first the exp) */
	bne	t2,zero,5f	/* then the high part of the fraction */
	bne	t3,zero,5f	/* then the low part of the fraction */
	/* Now RS is known to be zero so just return it */
	move	t2,t0		/* get the sign of the zero */
	move	v0,zero
	b	rd_2w
5:	/* Check for sqrt of a negitive number if so it is an invalid */
	bne	t0,zero,1b

	/*
	 * Now that all the NaN, infinity and zero and negitive cases have
	 * been taken care of what is left is a value that the sqrt can be
	 * taken.  So get the value into a format that can be used.  For
	 * normalized numbers set the implied one and remove the exponent
	 * bias.  For denormalized numbers convert to normalized numbers
	 * with the correct exponent.
	 */
	bne	t1,zero,1f	/* check for RS being denormalized */
	li	t1,-DEXP_BIAS+1	/* set denorms exponent */
	jal	rs_renorm_d	/* normalize it */
	b	2f
1:	subu	t1,DEXP_BIAS	/* - if RS is not denormalized then remove the */
	or	t2,DIMP_1BIT	/*  exponent bias, and set the implied 1 bit */
2:
	/*
	 * Now take the sqrt of the value.  Written by George Tayor.
	 *  t1		-- twos comp exponent
	 *  t2, t3	-- 53-bit fraction
	 *  t8, t9	-- temps
	 *  v0, v1	-- trial subtraction
	 *  t4, t5	-- remainder
	 *  t6, t7	-- 54-bit result
	 *  t8		-- sticky
	 */

	andi	t9, t1, 1		/*  last bit of unbiased exponent */
	sra	t1, 1			/*  divide exponent by 2 */
	addi	t1, -1			/*  subtract 1, deliver 54-bit result */
	beq	t9, zero, 1f

	sll	t2, t2, 1		/*  shift operand left by 1 */
	srl	t9, t3, 31 		/*    if exponent was odd */
	or	t2, t9
	sll	t3, t3, 1

1:	move	t6, zero		/*  initialize answer msw */
	li	t7, 1			/*  initialize answer lsw */
	move	t4, zero		/*  initialize remainder msw */
	move	t5, zero		/*  initialize remainder lsw */

	srl	t5, t2, 20		/*  shift operand left by 12 so that */
	sll	t2, t2, 12		/*    2 bits go into remainder */
	srl	t9, t3, 20
	or	t2, t9
	sll	t3, t3, 12

	li	t8, 54			/*  set cycle counter */

2:	sltu	t9, t5, t7		/*  trial subtraction */
	subu	v1, t5, t7
	subu	v0, t4, t6
	subu	v0, t9

	sll	t6, t6, 1		/*  shift answer left by 1 */
	srl	t9, t7, 31
	or	t6, t9
	sll	t7, t7, 1
	li	t9, -4			/*  put 01 back in low order bits */
	and	t7, t9			/*    using 0xfffffffc mask */
	or	t7, 1

	bltz	v0, 3f			/*  branch on sign of trial subtract */
	ori	t7, 4			/*  set new bit of answer */
	sll	t4, v0, 2		/*  shift trial result left by 2 */
	srl	t9, v1, 30		/*    and put in remainder */
	or	t4, t9
	sll	t5, v1, 2
	b	4f
3:
	sll	t4, t4, 2		/*  shift remainder left by 2 */
	srl	t9, t5, 30
	or	t4, t9
	sll	t5, t5, 2

4:	srl	t9, t2, 30		/*  shift operand left by 2 */
	or	t5, t9
	sll	t2, t2, 2
	srl	t9, t3, 30
	or	t2, t9
	sll	t3, t3, 2

	addi	t8, -1
	bne	t8, zero, 2b

	srl	t7, t7, 2		/*  shift answer right by 2 */
	sll	t9, t6, 30		/*    to eliminate extra bits */
	or	t7, t9
	srl	t6, t6, 2

	or	t8, t4, t5		/*  form sticky bit */
 	move	t2, t6
 	move	t3, t7

	b	norm_d

sqrt_e:
sqrt_q:
	b	illfpinst

func_abs:
	la	v1,abs_fmt_tab
	addu	v1, v0, v1
	j	v1

	.set 	noreorder
abs_fmt_tab:
	b	abs_s;	nop
	b	abs_d;	nop
	b	abs_e;	nop
	b	abs_q;	nop
	b	illfpinst;	nop
	b	illfpinst;	nop
	.set	reorder
/*******************************************************************************
*
* abs_s - Absolute value single
*
*/
FUNC_LABEL(abs_s)
	move	t6,t2		/* save the unmodified word */
	/*
	 * Handle a NaN operand by calling rs_breakout_s().
	 * The broken out results are discarded.
	 */
	li	t9,C1_FMT_SINGLE*4
	jal	rs_breakout_s
	/*
	 * Now just clear the signbit after restoring the unmodified word.
	 */
	move	t2,t6
	and	t2,~SIGNBIT
	move	v0,zero
	b	rd_1w

/*******************************************************************************
*
* abs_d - Absolute value double
*
*/
FUNC_LABEL(abs_d)
	move	t6,t2		/* save the unmodified word */
	/*
	 * Handle a NaN operand by calling rs_breakout_d().
	 * The broken out results are discarded.
	 */
	li	t9,C1_FMT_DOUBLE*4
	jal	rs_breakout_d
	/*
	 * Now just clear the signbit after restoring the unmodified word.
	 */
	move	t2,t6
	and	t2,~SIGNBIT
	move	v0,zero
	b	rd_2w

abs_e:
abs_q:
	b	illfpinst

func_mov:
	la	v1,mov_fmt_tab
	addu	v1, v0, v1
	j	v1
	
	.set	noreorder
mov_fmt_tab:
	b	mov_s;	nop
	b	mov_d;	nop
	b	mov_q;	nop
	b	illfpinst; nop
	b	illfpinst; nop
	b	illfpinst; nop
	.set reorder

/*******************************************************************************
*
* mov_s - Move single
*
*/
FUNC_LABEL(mov_s)
	move	v0,zero
	b	rd_1w

/*******************************************************************************
*
* mov_d - Move double
*
*/
FUNC_LABEL(mov_d)
	move	v0,zero
	b	rd_2w

mov_e:
mov_q:
	b	illfpinst

func_neg:
	la	v1,neg_fmt_tab
	addu	v1, v0, v1
	j	v1

	.set	noreorder
neg_fmt_tab:
	b	neg_s;	nop
	b	neg_d;	nop
	b	neg_e;	nop
	b	neg_q;	nop
	b	illfpinst; nop
	b	illfpinst; nop
	.set	reorder

/*******************************************************************************
*
* neg_s - Negation single
*
*/
FUNC_LABEL(neg_s)
	move	t6,t2		/* save the unmodified word */
	/*
	 * Handle a NaN operand by calling rs_breakout_s().
	 * The broken out results are discarded.
	 */
	li	t9,C1_FMT_SINGLE*4
	jal	rs_breakout_s
	/*
	 * Now just negate the operand after restoring the unmodified word.
	 */
	move	t2,t6
	.set	noat
	li	AT,SIGNBIT
	xor	t2,AT
	.set	at
	move	v0,zero
	b	rd_1w

/*******************************************************************************
*
* neg_d - Negation double
*
*/
FUNC_LABEL(neg_d)
	move	t6,t2		/* save the unmodified word */
	/*
	 * Handle a NaN operand by calling rs_breakout_d().
	 * The broken out results are discarded.
	 */
	li	t9,C1_FMT_DOUBLE*4
	jal	rs_breakout_d
	/*
	 * Now just negate the operand after restoring the unmodified word.
	 */
	move	t2,t6
	.set	noat
	li	AT,SIGNBIT
	xor	t2,AT
	.set	at
	move	v0,zero
	b	rd_2w

neg_e:
neg_q:
	b	illfpinst

/*******************************************************************************
*
* load_rt -
*
* Load the floating point value from the register specified by the
* RT field into gp registers.  The gp registers which are used for
* the value specified by the RT feild is dependent on its format
* as follows:
* 	single		t6
*	double		t6,t7
*	extended	t6,t7,s6,s7	(where t7 is really zero)
*	quad		t6,t7,s6,s7
*/
load_rt:
	srl	v1,a1,RT_SHIFT		/* get the RT field */
	and	v1,RT_MASK
#ifdef DEBUG
	sw	v1,_fp_rt_reg
#endif

	/*
	 * If a2 (int or exception) is non-zero then the floating-point values
	 * are loaded from the coprocessor else they are loaded from the tcb.
	 */
	beq	a2,zero,rt_tcb

	/*
	 * At this point the floating-point value for the specified FPR register
	 * in the RT field will loaded from the coprocessor registers for the
	 * FMT specified (v0).
	 */
				/* setup to branch to the code to load */
	la	t9, cp_rt_fmt_tab
	addu	t9, v0, t9
	j	t9			/*  cp for the specified format. */

	.set	noreorder
cp_rt_fmt_tab:
	b	rt_cp_1w;	nop
	b	rt_cp_2w;	nop
	b	illfpinst;	nop
	b	illfpinst;	nop
	b	rt_cp_1w;	nop
	b	rt_cp_2w;	nop
	.set	reorder
/*******************************************************************************
*
* rt_cp_1w -
*
* Load the one word from the coprocessor for the FPR register specified by
* the RT (v1) field into GPR register t6.
*/
rt_cp_1w:
	sll	v1, v1, 3		/* 8 bytes per entry */
	la	t9,rt_cp_1w_tab
	addu	v1, t9, v1
	j	v1

	.set	noreorder
rt_cp_1w_tab:
	b	rt_cp_1w_fpr0; nop
	b	rt_cp_1w_fpr1; nop
	b	rt_cp_1w_fpr2; nop
	b	rt_cp_1w_fpr3; nop
	b	rt_cp_1w_fpr4; nop
	b	rt_cp_1w_fpr5; nop
	b	rt_cp_1w_fpr6; nop
	b	rt_cp_1w_fpr7; nop
	b	rt_cp_1w_fpr8; nop
	b	rt_cp_1w_fpr9; nop
	b	rt_cp_1w_fpr10; nop
	b	rt_cp_1w_fpr11; nop
	b	rt_cp_1w_fpr12; nop
	b	rt_cp_1w_fpr13; nop
	b	rt_cp_1w_fpr14; nop
	b	rt_cp_1w_fpr15; nop
	b	rt_cp_1w_fpr16; nop
	b	rt_cp_1w_fpr17; nop
	b	rt_cp_1w_fpr18; nop
	b	rt_cp_1w_fpr19; nop
	b	rt_cp_1w_fpr20; nop
	b	rt_cp_1w_fpr21; nop
	b	rt_cp_1w_fpr22; nop
	b	rt_cp_1w_fpr23; nop
	b	rt_cp_1w_fpr24; nop
	b	rt_cp_1w_fpr25; nop
	b	rt_cp_1w_fpr26; nop
	b	rt_cp_1w_fpr27; nop
	b	rt_cp_1w_fpr28; nop
	b	rt_cp_1w_fpr29; nop
	b	rt_cp_1w_fpr30; nop
	b	rt_cp_1w_fpr31; nop
	.set	reorder

	.set 	noreorder
rt_cp_1w_fpr0:
	mfc1	t6,$f0;		b	load_rt_done; 	nop
rt_cp_1w_fpr1:
	mfc1	t6,$f1;		b	load_rt_done; 	nop
rt_cp_1w_fpr2:
	mfc1	t6,$f2;		b	load_rt_done; 	nop
rt_cp_1w_fpr3:
	mfc1	t6,$f3;		b	load_rt_done; 	nop
rt_cp_1w_fpr4:
	mfc1	t6,$f4;		b	load_rt_done; 	nop
rt_cp_1w_fpr5:
	mfc1	t6,$f5;		b	load_rt_done; 	nop
rt_cp_1w_fpr6:
	mfc1	t6,$f6;		b	load_rt_done; 	nop
rt_cp_1w_fpr7:
	mfc1	t6,$f7;		b	load_rt_done; 	nop
rt_cp_1w_fpr8:
	mfc1	t6,$f8;		b	load_rt_done; 	nop
rt_cp_1w_fpr9:
	mfc1	t6,$f9;		b	load_rt_done; 	nop
rt_cp_1w_fpr10:
	mfc1	t6,$f10;	b	load_rt_done; 	nop
rt_cp_1w_fpr11:
	mfc1	t6,$f11;	b	load_rt_done; 	nop
rt_cp_1w_fpr12:
	mfc1	t6,$f12;	b	load_rt_done; 	nop
rt_cp_1w_fpr13:
	mfc1	t6,$f13;	b	load_rt_done; 	nop
rt_cp_1w_fpr14:
	mfc1	t6,$f14;	b	load_rt_done; 	nop
rt_cp_1w_fpr15:
	mfc1	t6,$f15;	b	load_rt_done; 	nop
rt_cp_1w_fpr16:
	mfc1	t6,$f16;	b	load_rt_done; 	nop
rt_cp_1w_fpr17:
	mfc1	t6,$f17;	b	load_rt_done; 	nop
rt_cp_1w_fpr18:
	mfc1	t6,$f18;	b	load_rt_done; 	nop
rt_cp_1w_fpr19:
	mfc1	t6,$f19;	b	load_rt_done; 	nop
rt_cp_1w_fpr20:
	mfc1	t6,$f20;	b	load_rt_done; 	nop
rt_cp_1w_fpr21: