cp1emu.c

嵌入式系统设计与实验教材二源码linux内核移植与编译

	default:
		return SIGILL;
	}

	return 0;
}
#endif



/*
 * Emulate a single COP1 arithmetic instruction.
 */
static int fpu_emu(struct pt_regs *xcp, struct mips_fpu_soft_struct *ctx,
                   mips_instruction ir)
{
	int rfmt;		/* resulting format */
	unsigned rcsr = 0;	/* resulting csr */
	unsigned cond;
	union {
		ieee754dp d;
		ieee754sp s;
		int w;
#if __mips64
		long long l;
#endif
	} rv;			/* resulting value */

	fpuemuprivate.stats.cp1ops++;
	switch (rfmt = (MIPSInst_FFMT(ir) & 0xf)) {
	case s_fmt: {		/* 0 */
		ieee754sp(*handler) ();

		switch (MIPSInst_FUNC(ir)) {
			/* binary ops */
		case fadd_op:
			handler = ieee754sp_add;
			goto scopbop;
		case fsub_op:
			handler = ieee754sp_sub;
			goto scopbop;
		case fmul_op:
			handler = ieee754sp_mul;
			goto scopbop;
		case fdiv_op:
			handler = ieee754sp_div;
			goto scopbop;

			/* unary  ops */
#if __mips >= 2 || __mips64
		case fsqrt_op:
			handler = ieee754sp_sqrt;
			goto scopuop;
#endif
#if __mips >= 4 && __mips != 32
		case frsqrt_op:
			handler = fpemu_sp_rsqrt;
			goto scopuop;
		case frecip_op:
			handler = fpemu_sp_recip;
			goto scopuop;
#endif
#if __mips >= 4
		case fmovc_op:
			cond = fpucondbit[MIPSInst_FT(ir) >> 2];
			if (((ctx->sr & cond) != 0) !=
			    ((MIPSInst_FT(ir) & 1) != 0))
				return 0;
			SPFROMREG(rv.s, MIPSInst_FS(ir));
			break;
		case fmovz_op:
			if (xcp->regs[MIPSInst_FT(ir)] != 0)
				return 0;
			SPFROMREG(rv.s, MIPSInst_FS(ir));
			break;
		case fmovn_op:
			if (xcp->regs[MIPSInst_FT(ir)] == 0)
				return 0;
			SPFROMREG(rv.s, MIPSInst_FS(ir));
			break;
#endif
		case fabs_op:
			handler = ieee754sp_abs;
			goto scopuop;
		case fneg_op:
			handler = ieee754sp_neg;
			goto scopuop;
		case fmov_op:
			/* an easy one */
			SPFROMREG(rv.s, MIPSInst_FS(ir));
			break;
			/* binary op on handler */
scopbop:
			{
				ieee754sp fs, ft;

				SPFROMREG(fs, MIPSInst_FS(ir));
				SPFROMREG(ft, MIPSInst_FT(ir));

				rv.s = (*handler) (fs, ft);
				goto copcsr;
			}
scopuop:
			{
				ieee754sp fs;

				SPFROMREG(fs, MIPSInst_FS(ir));
				rv.s = (*handler) (fs);
				goto copcsr;
			}
copcsr:
			if (ieee754_cxtest(IEEE754_INEXACT))
				rcsr |= FPU_CSR_INE_X | FPU_CSR_INE_S;
			if (ieee754_cxtest(IEEE754_UNDERFLOW))
				rcsr |= FPU_CSR_UDF_X | FPU_CSR_UDF_S;
			if (ieee754_cxtest(IEEE754_OVERFLOW))
				rcsr |= FPU_CSR_OVF_X | FPU_CSR_OVF_S;
			if (ieee754_cxtest(IEEE754_ZERO_DIVIDE))
				rcsr |= FPU_CSR_DIV_X | FPU_CSR_DIV_S;
			if (ieee754_cxtest
				(IEEE754_INVALID_OPERATION)) rcsr |=
					    FPU_CSR_INV_X | FPU_CSR_INV_S;
				break;

				/* unary conv ops */
		case fcvts_op:
			return SIGILL;	/* not defined */
		case fcvtd_op: {
#ifdef SINGLE_ONLY_FPU
			return SIGILL;	/* not defined */
#else
			ieee754sp fs;

			SPFROMREG(fs, MIPSInst_FS(ir));
			rv.d = ieee754dp_fsp(fs);
			rfmt = d_fmt;
			goto copcsr;
		}
#endif
		case fcvtw_op: {
			ieee754sp fs;

			SPFROMREG(fs, MIPSInst_FS(ir));
			rv.w = ieee754sp_tint(fs);
			rfmt = w_fmt;
			goto copcsr;
		}

#if __mips >= 2 || __mips64
		case fround_op:
		case ftrunc_op:
		case fceil_op:
		case ffloor_op: {
			unsigned int oldrm = ieee754_csr.rm;
			ieee754sp fs;

			SPFROMREG(fs, MIPSInst_FS(ir));
			ieee754_csr.rm = ieee_rm[MIPSInst_FUNC(ir) & 0x3];
			rv.w = ieee754sp_tint(fs);
			ieee754_csr.rm = oldrm;
			rfmt = w_fmt;
			goto copcsr;
		}
#endif /* __mips >= 2 */

#if __mips64 && !defined(SINGLE_ONLY_FPU)
		case fcvtl_op: {
			ieee754sp fs;

			SPFROMREG(fs, MIPSInst_FS(ir));
			rv.l = ieee754sp_tlong(fs);
			rfmt = l_fmt;
			goto copcsr;
		}

		case froundl_op:
		case ftruncl_op:
		case fceill_op:
		case ffloorl_op: {
			unsigned int oldrm = ieee754_csr.rm;
			ieee754sp fs;

			SPFROMREG(fs, MIPSInst_FS(ir));
			ieee754_csr.rm = ieee_rm[MIPSInst_FUNC(ir) & 0x3];
			rv.l = ieee754sp_tlong(fs);
			ieee754_csr.rm = oldrm;
			rfmt = l_fmt;
			goto copcsr;
		}
#endif /* __mips64 && !fpu(single) */

		default:
			if (MIPSInst_FUNC(ir) >= fcmp_op) {
				unsigned cmpop = MIPSInst_FUNC(ir) - fcmp_op;
				ieee754sp fs, ft;

				SPFROMREG(fs, MIPSInst_FS(ir));
				SPFROMREG(ft, MIPSInst_FT(ir));
				rv.w = ieee754sp_cmp(fs, ft, cmptab[cmpop & 0x7]);
				rfmt = -1;
				if ((cmpop & 0x8) && ieee754_cxtest(IEEE754_INVALID_OPERATION))
					rcsr = FPU_CSR_INV_X | FPU_CSR_INV_S;
				} else {
					return SIGILL;
				}
				break;
			}
			break;
		}

#if !defined(SINGLE_ONLY_FPU)
	case d_fmt: {
		ieee754dp(*handler) ();

		switch (MIPSInst_FUNC(ir)) {
			/* binary ops */
		case fadd_op:
			handler = ieee754dp_add;
			goto dcopbop;
		case fsub_op:
			handler = ieee754dp_sub;
			goto dcopbop;
		case fmul_op:
			handler = ieee754dp_mul;
			goto dcopbop;
		case fdiv_op:
			handler = ieee754dp_div;
			goto dcopbop;

			/* unary  ops */
#if __mips >= 2 || __mips64
		case fsqrt_op:
			handler = ieee754dp_sqrt;
			goto dcopuop;
#endif
#if __mips >= 4 && __mips != 32
		case frsqrt_op:
			handler = fpemu_dp_rsqrt;
			goto dcopuop;
		case frecip_op:
			handler = fpemu_dp_recip;
			goto dcopuop;
#endif
#if __mips >= 4
		case fmovc_op:
			cond = fpucondbit[MIPSInst_FT(ir) >> 2];
			if (((ctx->sr & cond) != 0) != ((MIPSInst_FT(ir) & 1) != 0))
				return 0;
			DPFROMREG(rv.d, MIPSInst_FS(ir));
			break;
		case fmovz_op:
			if (xcp->regs[MIPSInst_FT(ir)] != 0)
				return 0;
			DPFROMREG(rv.d, MIPSInst_FS(ir));
			break;
		case fmovn_op:
			if (xcp->regs[MIPSInst_FT(ir)] == 0)
				return 0;
			DPFROMREG(rv.d, MIPSInst_FS(ir));
			break;
#endif
		case fabs_op:
			handler = ieee754dp_abs;
			goto dcopuop;

		case fneg_op:
			handler = ieee754dp_neg;
			goto dcopuop;

		case fmov_op:
			/* an easy one */
			DPFROMREG(rv.d, MIPSInst_FS(ir));
			break;

			/* binary op on handler */
dcopbop:
			{
				ieee754dp fs, ft;

				DPFROMREG(fs, MIPSInst_FS(ir));
				DPFROMREG(ft, MIPSInst_FT(ir));

				rv.d = (*handler) (fs, ft);
				goto copcsr;
			}
dcopuop:
			{
				ieee754dp fs;

				DPFROMREG(fs, MIPSInst_FS(ir));
				rv.d = (*handler) (fs);
				goto copcsr;
			}

		/* unary conv ops */
		case fcvts_op: {
			ieee754dp fs;

			DPFROMREG(fs, MIPSInst_FS(ir));
			rv.s = ieee754sp_fdp(fs);
			rfmt = s_fmt;
			goto copcsr;
		}
		case fcvtd_op:
			return SIGILL;	/* not defined */

		case fcvtw_op: {
			ieee754dp fs;

			DPFROMREG(fs, MIPSInst_FS(ir));
			rv.w = ieee754dp_tint(fs);	/* wrong */
			rfmt = w_fmt;
			goto copcsr;
		}

#if __mips >= 2 || __mips64
		case fround_op:
		case ftrunc_op:
		case fceil_op:
		case ffloor_op: {
			unsigned int oldrm = ieee754_csr.rm;
			ieee754dp fs;

			DPFROMREG(fs, MIPSInst_FS(ir));
			ieee754_csr.rm = ieee_rm[MIPSInst_FUNC(ir) & 0x3];
			rv.w = ieee754dp_tint(fs);
			ieee754_csr.rm = oldrm;
			rfmt = w_fmt;
			goto copcsr;
		}
#endif

#if __mips64 && !defined(SINGLE_ONLY_FPU)
		case fcvtl_op: {
			ieee754dp fs;

			DPFROMREG(fs, MIPSInst_FS(ir));
			rv.l = ieee754dp_tlong(fs);
			rfmt = l_fmt;
			goto copcsr;
		}

		case froundl_op:
		case ftruncl_op:
		case fceill_op:
		case ffloorl_op: {
			unsigned int oldrm = ieee754_csr.rm;
			ieee754dp fs;

			DPFROMREG(fs, MIPSInst_FS(ir));
			ieee754_csr.rm = ieee_rm[MIPSInst_FUNC(ir) & 0x3];
			rv.l = ieee754dp_tlong(fs);
			ieee754_csr.rm = oldrm;
			rfmt = l_fmt;
			goto copcsr;
		}
#endif /* __mips >= 3 && !fpu(single) */

		default:
			if (MIPSInst_FUNC(ir) >= fcmp_op) {
				unsigned cmpop = MIPSInst_FUNC(ir) - fcmp_op;
				ieee754dp fs, ft;

				DPFROMREG(fs, MIPSInst_FS(ir));
				DPFROMREG(ft, MIPSInst_FT(ir));
				rv.w = ieee754dp_cmp(fs, ft, cmptab[cmpop & 0x7]);
				rfmt = -1;
				if ((cmpop & 0x8) && ieee754_cxtest (IEEE754_INVALID_OPERATION))
					rcsr = FPU_CSR_INV_X | FPU_CSR_INV_S;
			} else {
				return SIGILL;
			}
			break;
		}
		break;
	}
#endif /* !defined(SINGLE_ONLY_FPU) */

	case w_fmt: {
		switch (MIPSInst_FUNC(ir)) {
		case fcvts_op:
			/* convert word to single precision real */
			rv.s = ieee754sp_fint(ctx-> regs[MIPSInst_FS(ir)]);
			rfmt = s_fmt;
			goto copcsr;
#if !defined(SINGLE_ONLY_FPU)
		case fcvtd_op:
			/* convert word to double precision real */
			rv.d = ieee754dp_fint(ctx-> regs[MIPSInst_FS(ir)]);
			rfmt = d_fmt;
			goto copcsr;
#endif
		default:
			return SIGILL;
		}
		break;
	}

#if __mips64 && !defined(SINGLE_ONLY_FPU)
	case l_fmt: {
		switch (MIPSInst_FUNC(ir)) {
		case fcvts_op:
			/* convert long to single precision real */
			rv.s = ieee754sp_flong(ctx-> regs[MIPSInst_FS(ir)]);
			rfmt = s_fmt;
			goto copcsr;
		case fcvtd_op:
			/* convert long to double precision real */
			rv.d = ieee754dp_flong(ctx-> regs[MIPSInst_FS(ir)]);
			rfmt = d_fmt;
			goto copcsr;
		default:
			return SIGILL;
		}
		break;
	}
#endif

	default:
		return SIGILL;
	}

	/*
	 * Update the fpu CSR register for this operation.
	 * If an exception is required, generate a tidy SIGFPE exception,
	 * without updating the result register.
	 * Note: cause exception bits do not accumulate, they are rewritten
	 * for each op; only the flag/sticky bits accumulate.
	 */
	ctx->sr = (ctx->sr & ~FPU_CSR_ALL_X) | rcsr;
	if ((ctx->sr >> 5) & ctx->sr & FPU_CSR_ALL_E) {
		/*printk ("SIGFPE: fpu csr = %08x\n",ctx->sr); */
		return SIGFPE;
	}

	/* 
	 * Now we can safely write the result back to the register file.
	 */
	switch (rfmt) {
	case -1: {
#if __mips >= 4
		cond = fpucondbit[MIPSInst_FD(ir) >> 2];
#else
		cond = FPU_CSR_COND;
#endif
		if (rv.w)
			ctx->sr |= cond;
		else
			ctx->sr &= ~cond;
		break;
	}
#if !defined(SINGLE_ONLY_FPU)
	case d_fmt:
		DPTOREG(rv.d, MIPSInst_FD(ir));
		break;
#endif
	case s_fmt:
		SPTOREG(rv.s, MIPSInst_FD(ir));
		break;
	case w_fmt:
		SITOREG(rv.w, MIPSInst_FD(ir));
		break;
#if __mips64 && !defined(SINGLE_ONLY_FPU)
	case l_fmt:
		DITOREG(rv.l, MIPSInst_FD(ir));
		break;
#endif
	default:
		return SIGILL;
	}

	return 0;
}


/*
 * Emulate the floating point instruction at EPC, and continue to run until we
 * hit a non-fp instruction, or a backward branch.  This cuts down dramatically
 * on the per instruction exception overhead.
 */
int fpu_emulator_cop1Handler(struct pt_regs *xcp)
{
	struct mips_fpu_soft_struct *ctx = &current->thread.fpu.soft;
	unsigned long oldepc, prevepc;
	unsigned int insn;
	int sig = 0;
	int err = 0;

	oldepc = xcp->cp0_epc;
	do {
		if (current->need_resched)
			schedule();

		prevepc = xcp->cp0_epc;
		insn = mips_get_word(xcp, REG_TO_VA(xcp->cp0_epc), &err);
		if (err) {
			fpuemuprivate.stats.errors++;
			return SIGBUS;
		}
		if (insn != 0)
			sig = cop1Emulate(xcp, ctx);
		else
			xcp->cp0_epc += 4;	/* skip nops */

		if (mips_cpu.options & MIPS_CPU_FPU)
			break;
	} while (xcp->cp0_epc > prevepc && sig == 0);

	/* SIGILL indicates a non-fpu instruction */
	if (sig == SIGILL && xcp->cp0_epc != oldepc)
		/* but if epc has advanced, then ignore it */
		sig = 0;

	return sig;
}


#ifdef NOTDEF
/*
 * Patch up the hardware fpu state when an f.p. exception occurs.  
 */
static int cop1Patcher(int xcptno, struct pt_regs *xcp)
{
	struct mips_fpu_soft_struct *ctx = &current->thread.fpu.soft;
	unsigned sr;
	int sig;

	/* reenable Cp1, else fpe_save() will get nested exception */
	sr = mips_bissr(ST0_CU1);

	/* get fpu registers and status, then clear pending exceptions */
	fpe_save(ctx);
	fpe_setsr(ctx->sr &= ~FPU_CSR_ALL_X);

	/* get current rounding mode for IEEE library, and emulate insn */
	ieee754_csr.rm = ieee_rm[ctx->sr & 0x3];
	sig = cop1Emulate(xcp, ctx);

	/* don't return with f.p. exceptions pending */
	ctx->sr &= ~FPU_CSR_ALL_X;
	fpe_restore(ctx);

	mips_setsr(sr);
	return sig;
}

void _cop1_init(int emulate)
{
	extern int _nofpu;

	if (emulate) {
		/* 
		 * Install cop1 emulator to handle "coprocessor unusable" exception
		 */
		xcption(XCPTCPU, cop1Handler);
		fpuemuactive = 1;	/* tell dbg.c that we are in charge */
		_nofpu = 0;	/* tell setjmp() it "has" an fpu */
	} else {
		/* 
		 * Install cop1 emulator for floating point exceptions only,
		 * i.e. denormalised results, underflow, overflow etc, which
		 * must be emulated in s/w.
		 */
#ifdef 1
		/* r4000 or above use dedicate exception */
		xcption(XCPTFPE, cop1Patcher);
#else
		/* r3000 et al use interrupt */
		extern int _sbd_getfpuintr(void);
		int intno = _sbd_getfpuintr();
		intrupt(intno, cop1Patcher, 0);
		mips_bissr(SR_IM0 << intno);
#endif

#if (#cpu(r4640) || #cpu(r4650)) && !defined(SINGLE_ONLY_FPU)
		/* For R4640/R4650 compiled *without* the -msingle-float flag,
		   then we share responsibility: the h/w handles the single
		   precision operations, and the trap emulator handles the
		   double precision. We set fpuemuactive so that dbg.c first
		   fetches the s/w state before saving the h/w state. */
		fpuemuactive = 1;
		{
			int i;
			/* initialise the unused d.p high order words to be NaN */
			for (i = 0; i < 32; i++)
				current->thread.fpu.soft.regs[i] =
				    0x7ff80bad00000000LL;
		}
#endif				/* (r4640 || r4650) && !fpu(single) */
	}
}
#endif