fpsoft.s

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

	mfc1	t6,$f21;	b	load_rt_done; 	nop
rt_cp_1w_fpr22:
	mfc1	t6,$f22;	b	load_rt_done; 	nop
rt_cp_1w_fpr23:
	mfc1	t6,$f23;	b	load_rt_done; 	nop
rt_cp_1w_fpr24:
	mfc1	t6,$f24;	b	load_rt_done; 	nop
rt_cp_1w_fpr25:
	mfc1	t6,$f25;	b	load_rt_done; 	nop
rt_cp_1w_fpr26:
	mfc1	t6,$f26;	b	load_rt_done; 	nop
rt_cp_1w_fpr27:
	mfc1	t6,$f27;	b	load_rt_done; 	nop
rt_cp_1w_fpr28:
	mfc1	t6,$f28;	b	load_rt_done; 	nop
rt_cp_1w_fpr29:
	mfc1	t6,$f29;	b	load_rt_done; 	nop
rt_cp_1w_fpr30:
	mfc1	t6,$f30;	b	load_rt_done; 	nop
rt_cp_1w_fpr31:
	mfc1	t6,$f31;	b	load_rt_done; 	nop
	.set	reorder

/*
 * Load the two words from the coprocessor for the FPR register specified by
 * the RT (v1) field into GPR registers t6,t7.
 */
rt_cp_2w:
#if	  (_WRS_FP_REGISTER_SIZE == 4)
	srl	v1, v1, 1		/* only allow even numbered registers */
#endif	  /* _WRS_FP_REGISTER_SIZE */
	sll	v1, v1, 3		/* 8 bytes per entry */
	la	t9,rt_cp_2w_tab	
	addu	v1, t9, v1
	j	v1

	.set	noreorder
rt_cp_2w_tab:
#if	  (_WRS_FP_REGISTER_SIZE == 4)
	b	rt_cp_2w_fpr0;	nop
	b	rt_cp_2w_fpr2;	nop
	b	rt_cp_2w_fpr4;	nop
	b	rt_cp_2w_fpr6;	nop
	b	rt_cp_2w_fpr8;	nop
	b	rt_cp_2w_fpr10;	nop
	b	rt_cp_2w_fpr12;	nop
	b	rt_cp_2w_fpr14;	nop
	b	rt_cp_2w_fpr16;	nop
	b	rt_cp_2w_fpr18;	nop
	b	rt_cp_2w_fpr20;	nop
	b	rt_cp_2w_fpr22;	nop
	b	rt_cp_2w_fpr24;	nop
	b	rt_cp_2w_fpr26;	nop
	b	rt_cp_2w_fpr28;	nop
	b	rt_cp_2w_fpr30;	nop
#elif	  (_WRS_FP_REGISTER_SIZE == 8)
	b	rt_cp_2w_fpr0; nop
	b	rt_cp_2w_fpr1; nop
	b	rt_cp_2w_fpr2; nop
	b	rt_cp_2w_fpr3; nop
	b	rt_cp_2w_fpr4; nop
	b	rt_cp_2w_fpr5; nop
	b	rt_cp_2w_fpr6; nop
	b	rt_cp_2w_fpr7; nop
	b	rt_cp_2w_fpr8; nop
	b	rt_cp_2w_fpr9; nop
	b	rt_cp_2w_fpr10; nop
	b	rt_cp_2w_fpr11; nop
	b	rt_cp_2w_fpr12; nop
	b	rt_cp_2w_fpr13; nop
	b	rt_cp_2w_fpr14; nop
	b	rt_cp_2w_fpr15; nop
	b	rt_cp_2w_fpr16; nop
	b	rt_cp_2w_fpr17; nop
	b	rt_cp_2w_fpr18; nop
	b	rt_cp_2w_fpr19; nop
	b	rt_cp_2w_fpr20; nop
	b	rt_cp_2w_fpr21; nop
	b	rt_cp_2w_fpr22; nop
	b	rt_cp_2w_fpr23; nop
	b	rt_cp_2w_fpr24; nop
	b	rt_cp_2w_fpr25; nop
	b	rt_cp_2w_fpr26; nop
	b	rt_cp_2w_fpr27; nop
	b	rt_cp_2w_fpr28; nop
	b	rt_cp_2w_fpr29; nop
	b	rt_cp_2w_fpr30; nop
	b	rt_cp_2w_fpr31; nop
#else	  /* _WRS_FP_REGISTER_SIZE */
#error "invalid _WRS_FP_REGISTER_SIZE value"
#endif	  /* _WRS_FP_REGISTER_SIZE */
	
#if	  (_WRS_FP_REGISTER_SIZE == 4)
rt_cp_2w_fpr0:
	mfc1	t7,$f0;		mfc1	t6,$f1;		b	load_rt_done
	nop
rt_cp_2w_fpr2:
	mfc1	t7,$f2;		mfc1	t6,$f3;		b	load_rt_done
	nop
rt_cp_2w_fpr4:
	mfc1	t7,$f4;		mfc1	t6,$f5;		b	load_rt_done
	nop
rt_cp_2w_fpr6:
	mfc1	t7,$f6;		mfc1	t6,$f7;		b	load_rt_done
	nop
rt_cp_2w_fpr8:
	mfc1	t7,$f8;		mfc1	t6,$f9;		b	load_rt_done
	nop
rt_cp_2w_fpr10:
	mfc1	t7,$f10;	mfc1	t6,$f11;	b	load_rt_done
	nop
rt_cp_2w_fpr12:
	mfc1	t7,$f12;	mfc1	t6,$f13;	b	load_rt_done
	nop
rt_cp_2w_fpr14:
	mfc1	t7,$f14;	mfc1	t6,$f15;	b	load_rt_done
	nop
rt_cp_2w_fpr16:
	mfc1	t7,$f16;	mfc1	t6,$f17;	b	load_rt_done
	nop
rt_cp_2w_fpr18:
	mfc1	t7,$f18;	mfc1	t6,$f19;	b	load_rt_done
	nop
rt_cp_2w_fpr20:
	mfc1	t7,$f20;	mfc1	t6,$f21;	b	load_rt_done
	nop
rt_cp_2w_fpr22:
	mfc1	t7,$f22;	mfc1	t6,$f23;	b	load_rt_done
	nop
rt_cp_2w_fpr24:
	mfc1	t7,$f24;	mfc1	t6,$f25;	b	load_rt_done
	nop
rt_cp_2w_fpr26:
	mfc1	t7,$f26;	mfc1	t6,$f27;	b	load_rt_done
	nop
rt_cp_2w_fpr28:
	mfc1	t7,$f28;	mfc1	t6,$f29;	b	load_rt_done
	nop
rt_cp_2w_fpr30:
	mfc1	t7,$f30;	mfc1	t6,$f31;	b	load_rt_done
	nop
#elif	  (_WRS_FP_REGISTER_SIZE == 8)
rt_cp_2w_fpr0:
	b	0f; dmfc1	t7,$f0
rt_cp_2w_fpr1:
	b	0f; dmfc1	t7,$f1
rt_cp_2w_fpr2:
	b	0f; dmfc1	t7,$f2
rt_cp_2w_fpr3:
	b	0f; dmfc1	t7,$f3
rt_cp_2w_fpr4:
	b	0f; dmfc1	t7,$f4
rt_cp_2w_fpr5:
	b	0f; dmfc1	t7,$f5
rt_cp_2w_fpr6:
	b	0f; dmfc1	t7,$f6
rt_cp_2w_fpr7:
	b	0f; dmfc1	t7,$f7
rt_cp_2w_fpr8:
	b	0f; dmfc1	t7,$f8
rt_cp_2w_fpr9:
	b	0f; dmfc1	t7,$f9
rt_cp_2w_fpr10:
	b	0f; dmfc1	t7,$f10
rt_cp_2w_fpr11:
	b	0f; dmfc1	t7,$f11
rt_cp_2w_fpr12:
	b	0f; dmfc1	t7,$f12
rt_cp_2w_fpr13:
	b	0f; dmfc1	t7,$f13
rt_cp_2w_fpr14:
	b	0f; dmfc1	t7,$f14
rt_cp_2w_fpr15:
	b	0f; dmfc1	t7,$f15
rt_cp_2w_fpr16:
	b	0f; dmfc1	t7,$f16
rt_cp_2w_fpr17:
	b	0f; dmfc1	t7,$f17
rt_cp_2w_fpr18:
	b	0f; dmfc1	t7,$f18
rt_cp_2w_fpr19:
	b	0f; dmfc1	t7,$f19
rt_cp_2w_fpr20:
	b	0f; dmfc1	t7,$f20
rt_cp_2w_fpr21:
	b	0f; dmfc1	t7,$f21
rt_cp_2w_fpr22:
	b	0f; dmfc1	t7,$f22
rt_cp_2w_fpr23:
	b	0f; dmfc1	t7,$f23
rt_cp_2w_fpr24:
	b	0f; dmfc1	t7,$f24
rt_cp_2w_fpr25:
	b	0f; dmfc1	t7,$f25
rt_cp_2w_fpr26:
	b	0f; dmfc1	t7,$f26
rt_cp_2w_fpr27:
	b	0f; dmfc1	t7,$f27
rt_cp_2w_fpr28:
	b	0f; dmfc1	t7,$f28
rt_cp_2w_fpr29:
	b	0f; dmfc1	t7,$f29
rt_cp_2w_fpr30:
	b	0f; dmfc1	t7,$f30
rt_cp_2w_fpr31:
	b	0f; dmfc1	t7,$f31

	.set	reorder
0:
	dsrl32	t6,t7,0
	srlv	t7,t7,zero
	b	load_rt_done	
	.set	noreorder
#else	  /* _WRS_FP_REGISTER_SIZE */
#error "invalid _WRS_FP_REGISTER_SIZE value"
#endif	  /* _WRS_FP_REGISTER_SIZE */
	.set	reorder

/*
 * At this point the floating-point value for the specified FPR register
 * in the RT field (v1) will loaded from the task control block (tcb)
 * of the current process for FMT specified (v0).
 */
rt_tcb:
	la	t9,rt_tcb_fmt_tab
	addu	t9, v0, t9
	j	t9

	.set	noreorder
rt_tcb_fmt_tab:
	b	rt_tcb_s;	nop
	b	rt_tcb_d;	nop
	b	illfpinst;	nop
	b	illfpinst;	nop
	b	rt_tcb_w;	nop
	b	rt_tcb_l;	nop
	.set	reorder
	
rt_tcb_s:
rt_tcb_w:
#if	  (_WRS_FP_REGISTER_SIZE == 4)
	sll	v1, v1, 2			/* 4 bytes per register */
#elif	  (_WRS_FP_REGISTER_SIZE == 8)
	sll	v1, v1, 3			/* 8 bytes per register */
#else	  /* _WRS_FP_REGISTER_SIZE */
#error "invalid _WRS_FP_REGISTER_SIZE value"
#endif	  /* _WRS_FP_REGISTER_SIZE */
	lw	t2, FRAMEA3(softFp)(sp)		/* restore pFpContext */
	addu	v1, t2				/* create register address */
	lw	t6, (v1)			/* read correct register */
	b	load_rt_done
rt_tcb_d:
rt_tcb_l:
#if	  (_WRS_FP_REGISTER_SIZE == 4)
	sll	v1, v1, 2			/* 4 bytes per register */
#elif	  (_WRS_FP_REGISTER_SIZE == 8)
	sll	v1, v1, 3			/* 8 bytes per register */
#else	  /* _WRS_FP_REGISTER_SIZE */
#error "invalid _WRS_FP_REGISTER_SIZE value"
#endif	  /* _WRS_FP_REGISTER_SIZE */
	lw	t2, FRAMEA3(softFp)(sp)		/* restore pFpContext */
	addu	v1, t2				/* create register address */
	lw	t6, (v1)			/* read correct register */
	lw	t7, 4(v1)			/* read next register */

/*
 * At this point the both the floating-point value for the specified FPR
 * registers of the RS and RT fields have been loaded into GPR registers.
 * What is done next is to decode the FUNC field (t8) for the dyadic operations.
 */
load_rt_done:
	HAZARD_CP_READ  /* many branches to this point have preceeding mfc1 */
#ifdef DEBUG
	sw	t8, _fp_val
	sw	t6,_fp_rt
	sw	t7,_fp_rt+4
#endif	
	bge	t8,C1_FUNC_1stCMP,comp

	sll	t8,t8,3
	la	t9,dy_func_tab
	addu	t9, t8, t9
	j	t9

	.set	noreorder
dy_func_tab:
	b	func_add;	nop
	b	func_add;	nop
	b	func_mul;	nop
	b	func_div;	nop

	.set	reorder
/*
 * Both add and subtract functions come here.  The difference is that
 * the FUNC field (t8) is zero for adds.
 */
func_add:
	la	v1,add_fmt_tab
	addu	v1, v0, v1
	j	v1

	.set	noreorder
add_fmt_tab:
	b	add_s;	nop
	b	add_d;	nop
	b	add_e;	nop
	b	add_q;	nop
	b	illfpinst; nop
	b	illfpinst; nop
	.set	reorder

/*******************************************************************************
*
* add_s -
*
* Add (and subtract) single RD = RS + RT (or RD = RS - RT).  Again the FUNC
* field (t8) is zero for adds.
*/
	.globl	GTEXT(add_s)
FUNC_LABEL(add_s)
	/*
	 * Break out the operands into their fields (sign,exp,fraction) and
	 * handle NaN operands by calling {rs,rt}breakout_s() .
	 */
	li	t9,C1_FMT_SINGLE*4
	li	v1,1
	jal	rs_breakout_s
	jal	rt_breakout_s

	beq	t8,zero,1f	/* - if doing a subtract then negate RT */
	lui	v0,(SIGNBIT>>16)&0xffff
	xor	t4,v0
1:
	/* Check for addition of infinities, and produce the correct action if so */
	bne	t1,SEXP_INF,5f	/* is RS an infinity? */
				/* RS is an infinity */
	bne	t5,SEXP_INF,4f	/* is RT also an infinity? */
				/* RT is an infinity */
	beq	t0,t4,3f	/* do the infinities have the same sign? */

	/*
	 * The infinities do NOT have the same sign thus this is an invalid
	 * operation for addition so set the invalid exception in the C1_SR
	 * (a3) and setup the result depending if the enable for the invalid
	 * exception is set.
	 */
	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,SQUIETNAN_LEAST
	move	v0,zero
	b	rd_1w

	/*
	 * This is just a normal infinity + infinity so the result is just
	 * an infinity with the sign of the operands.
	 */
3:	move	t2,t0
	sll	t1,t1,SEXP_SHIFT
	or	t2,t1
	move	v0,zero
	b	rd_1w

	/*
	 * This is infinity + x , where RS is the infinity so the result is
	 * just RS (the infinity).
	 */
4:	move	t2,t0
	sll	t1,t1,SEXP_SHIFT
	or	t2,t1
	move	v0,zero
	b	rd_1w

	/*
	 * Check RT for an infinity value.  At this point it is know that RS
	 * is not an infinity.  If RT is an infinity it will be the result.
	 */
5:	bne	t5,SEXP_INF,6f
	move	t2,t4
	sll	t5,t5,SEXP_SHIFT
	or	t2,t5
	move	v0,zero
	b	rd_1w
6:
	/* Check for the addition of zeros and produce the correct action if so */
	bne	t1,zero,3f	/* check RS for a zero value (first the exp) */
	bne	t2,zero,3f	/* then the fraction */
				/* Now RS is known to be zero */

	bne	t5,zero,2f	/* check RT for a zero value (first the exp) */
	bne	t6,zero,2f	/* then the fraction */
	/*
	 * Now RS and RT are known to be zeroes so set the correct result
	 * according to the rounding mode (in the C1_SR) and exit.
	 */
	and	v0,a3,CSR_RM_MASK	/* get the rounding mode */
	bne	v0,CSR_RM_RMI,1f	/* check for round to - infinity */
	or	t2,t0,t4		/* set the result and exit, for round to */
	move	v0,zero			/*  - infinity the zero result is the */
	b	rd_1w			/*  and of the operands signs */

1:	and	t2,t0,t4		/* set the result and exit, for other */
	move	v0,zero			/*  rounding modes the zero result is the */
	b	rd_1w			/*  or of the operands signs */

	/* RS is a zero and RT is non-zero so the result is RT */
2:	move	t2,t4
	sll	t5,t5,SEXP_SHIFT
	or	t2,t5
	or	t2,t6
	move	v0,zero
	b	rd_1w

	/* RS is now known not to be zero so check RT for a zero value. */
3:	bne	t5,zero,4f	/* check RT for a zero value (first the exp) */
	bne	t6,zero,4f	/* then the fraction */
	or	t2,t0		/* RT is a zero so the result is RS */
	sll	t1,t1,SEXP_SHIFT
	or	t2,t1
	move	v0,zero
	b	rd_1w
4:
	/*
	 * Now that all the NaN, infinity and zero cases have been taken care
	 * of what is left are values that can be added.  So get all values
	 * into a format that can be added.  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,-SEXP_BIAS+1	/* set denorms exponent */
	jal	rs_renorm_s	/* normalize it */
	b	2f
1:	subu	t1,SEXP_BIAS	/* - if RS is not denormalized then remove the */
	or	t2,SIMP_1BIT	/*  exponent bias, and set the implied 1 bit */
2:	bne	t5,zero,3f	/* check for RT being denormalized */
	li	t5,-SEXP_BIAS+1	/* set denorms exponent */
	jal	rt_renorm_s	/* normalize it */
	b	4f
3:	subu	t5,SEXP_BIAS	/* - if RT is not denormalized then remove the */
	or	t6,SIMP_1BIT	/*  exponent bias, and set the implied 1 bit */
4:
	/*
	 * If the two values are the same except the sign return the correct
	 * zero according to the rounding mode.
	 */
	beq	t0,t4,2f		/* - if the signs are the same continue */
	bne	t1,t5,2f		/* - if the exps are not the same continue */
	bne	t2,t6,2f		/* - if the fractions are not the */
					/*  same continue */

	and	v0,a3,CSR_RM_MASK	/* get the rounding mode */
	bne	v0,CSR_RM_RMI,1f	/* check for round to - infinity */
	or	t2,t0,t4		/* set the result and exit, for round to */
					/*  - infinity the zero result is the */
	move	v0,zero			/*  and of the operands signs */
	b	rd_1w

1:	and	t2,t0,t4		/* set the result and exit, for other */
					/*  rounding modes the zero result is the */
	move	v0,zero			/*  or of the operands signs */