cp1emu.c

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

				unsigned value;

				if (MIPSInst_RT(ir) == 0)
					value = 0;
				else
					value = xcp->regs[MIPSInst_RT(ir)];

				/* we only have one writable control reg
				 */
				if (MIPSInst_RD(ir) == FPCREG_CSR) {
#ifdef CSRTRACE
					printk
					    ("%p gpr[%d]->csr=%08x\n",
					     REG_TO_VA(xcp->cp0_epc),
					     MIPSInst_RT(ir), value);
#endif
					ctx->sr = value;
					/* copy new rounding mode to ieee library state! */
					ieee754_csr.rm =
					    ieee_rm[value & 0x3];
				}
			}
			break;

		case bc_op: {
			int likely = 0;

			if (xcp->cp0_cause & CAUSEF_BD)
				return SIGILL;

#if __mips >= 4
			cond = ctx-> sr & fpucondbit[MIPSInst_RT(ir) >> 2];
#else
			cond = ctx->sr & FPU_CSR_COND;
#endif
			switch (MIPSInst_RT(ir) & 3) {
			case bcfl_op:
				likely = 1;
			case bcf_op:
				cond = !cond;
				break;
			case bctl_op:
				likely = 1;
			case bct_op:
				break;
			default:
				/* thats an illegal instruction */
				return SIGILL;
			}

			xcp->cp0_cause |= CAUSEF_BD;
			if (cond) {
				/* branch taken: emulate dslot instruction */
				xcp->cp0_epc += 4;
				contpc = REG_TO_VA xcp->cp0_epc +
				         (MIPSInst_SIMM(ir) << 2);

				ir = mips_get_word(xcp, REG_TO_VA(xcp->cp0_epc),
				      &err);
				if (err) {
					fpuemuprivate.stats.errors++;
					return SIGBUS;
				}

				switch (MIPSInst_OPCODE(ir)) {
				case lwc1_op:
				case swc1_op:
#if (__mips >= 2 || __mips64) && !defined(SINGLE_ONLY_FPU)
				case ldc1_op:
				case sdc1_op:
#endif
				case cop1_op:
#if __mips >= 4 && __mips != 32
				case cop1x_op:
#endif
					/* its one of ours */
					goto emul;
#if __mips >= 4
				case spec_op:
					if (MIPSInst_FUNC(ir) == movc_op)
						goto emul;
					break;
#endif
				}

				/*
				 * Single step the non-cp1 instruction in the
				 * dslot
				 */
				return mips_dsemul(xcp, ir, contpc);
			} else {
				/* branch not taken */
				if (likely)
					/*
					 * branch likely nullifies dslot if not
					 * taken
					 */
					xcp->cp0_epc += 4;
					/* else continue & execute dslot as normal insn */
			}
			break;
		}

		default: {
			int sig;

			if (!(MIPSInst_RS(ir) & 0x10))
				return SIGILL;

			/* a real fpu computation instruction */
				if ((sig = fpu_emu(xcp, ctx, ir)))
					return sig;
			}
		}
		break;

#if __mips >= 4 && __mips != 32
	case cop1x_op:
		{
			int sig;
			if ((sig = fpux_emu(xcp, ctx, ir)))
				return sig;
		}
		break;
#endif

#if __mips >= 4
	case spec_op:
		if (MIPSInst_FUNC(ir) != movc_op)
			return SIGILL;
		cond = fpucondbit[MIPSInst_RT(ir) >> 2];
		if (((ctx->sr & cond) != 0) !=
		    ((MIPSInst_RT(ir) & 1) != 0)) return 0;
		xcp->regs[MIPSInst_RD(ir)] = xcp->regs[MIPSInst_RS(ir)];
		break;
#endif

	default:
		return SIGILL;
	}

	/* we did it !! */
	xcp->cp0_epc = VA_TO_REG(contpc);
	xcp->cp0_cause &= ~CAUSEF_BD;
	return 0;
}

/*
 * Emulate the arbritrary instruction ir at xcp->cp0_epc.  Required when
 * we have to emulate the instruction in a COP1 branch delay slot.  Do
 * not change cp0_epc due to the instruction
 *
 * According to the spec:
 * 1) it shouldnt be a branch :-)
 * 2) it can be a COP instruction :-(
 * 3) if we are tring to run a protected memory space we must take
 *    special care on memory access instructions :-(
 */

/*
 * "Trampoline" return routine to catch exception following
 *  execution of delay-slot instruction execution.
 */

int do_dsemulret(struct pt_regs *xcp)
{
#ifdef DSEMUL_TRACE
	printk("desemulret\n");
#endif
	/* Set EPC to return to post-branch instruction */
	xcp->cp0_epc = current->thread.dsemul_epc;
	/*
	 * Clear the state that got us here.
	 */
	current->thread.dsemul_aerpc = (unsigned long) 0;

	return 0;
}


#define AdELOAD 0x8c000001	/* lw $0,1($0) */

static int
mips_dsemul(struct pt_regs *xcp, mips_instruction ir, vaddr_t cpc)
{
	mips_instruction *dsemul_insns;
	mips_instruction forcetrap;
	extern asmlinkage void handle_dsemulret(void);

	if (ir == 0) {		/* a nop is easy */
		xcp->cp0_epc = VA_TO_REG(cpc);
		return 0;
	}
#ifdef DSEMUL_TRACE
	printk("desemul %p %p\n", REG_TO_VA(xcp->cp0_epc), cpc);
#endif

	/* 
	 * The strategy is to push the instruction onto the user stack 
	 * and put a trap after it which we can catch and jump to 
	 * the required address any alternative apart from full 
	 * instruction emulation!!.
	 */
	dsemul_insns = (mips_instruction *) (xcp->regs[29] & ~3);
	dsemul_insns -= 3;	/* Two instructions, plus one for luck ;-) */

	/* Verify that the stack pointer is not competely insane */
	if (verify_area(VERIFY_WRITE, dsemul_insns,
	                sizeof(mips_instruction) * 2))
		return SIGBUS;

	if (mips_put_word(xcp, &dsemul_insns[0], ir)) {
		fpuemuprivate.stats.errors++;
		return SIGBUS;
	}

	/* 
	 * Algorithmics used a system call instruction, and
	 * borrowed that vector.  MIPS/Linux version is a bit
	 * more heavyweight in the interests of portability and
	 * multiprocessor support.  We flag the thread for special
	 * handling in the unaligned access handler and force an
	 * address error excpetion.
	 */

	/* If one is *really* paranoid, one tests for a bad stack pointer */
	if ((xcp->regs[29] & 0x3) == 0x3)
		forcetrap = AdELOAD - 1;
	else
		forcetrap = AdELOAD;

	if (mips_put_word(xcp, &dsemul_insns[1], forcetrap)) {
		fpuemuprivate.stats.errors++;
		return (SIGBUS);
	}

	/* Set thread state to catch and handle the exception */
	current->thread.dsemul_epc = (unsigned long) cpc;
	current->thread.dsemul_aerpc = (unsigned long) &dsemul_insns[1];
	xcp->cp0_epc = VA_TO_REG & dsemul_insns[0];
	flush_cache_sigtramp((unsigned long) dsemul_insns);

	return SIGILL;		/* force out of emulation loop */
}

/* 
 * Conversion table from MIPS compare ops 48-63
 * cond = ieee754dp_cmp(x,y,IEEE754_UN);
 */
static const unsigned char cmptab[8] = {
	0,					/* cmp_0 (sig) cmp_sf */
	IEEE754_CUN,				/* cmp_un (sig) cmp_ngle */
	IEEE754_CEQ,				/* cmp_eq (sig) cmp_seq */
	IEEE754_CEQ | IEEE754_CUN,		/* cmp_ueq (sig) cmp_ngl  */
	IEEE754_CLT,				/* cmp_olt (sig) cmp_lt */
	IEEE754_CLT | IEEE754_CUN,		/* cmp_ult (sig) cmp_nge */
	IEEE754_CLT | IEEE754_CEQ,		/* cmp_ole (sig) cmp_le */
	IEEE754_CLT | IEEE754_CEQ | IEEE754_CUN, /* cmp_ule (sig) cmp_ngt */
};

#define SIFROMREG(si,x)	((si) = ctx->regs[x])
#define SITOREG(si,x)	(ctx->regs[x] = (int)(si))

#if __mips64 && !defined(SINGLE_ONLY_FPU)
#define DIFROMREG(di,x)	((di) = ctx->regs[x])
#define DITOREG(di,x)	(ctx->regs[x] = (di))
#endif

#define SPFROMREG(sp,x)	((sp).bits = ctx->regs[x])
#define SPTOREG(sp,x)	(ctx->regs[x] = (sp).bits)

#ifdef CP0_STATUS_FR_SUPPORT
#define DPFROMREG(dp,x)	((dp).bits = \
			ctx->regs[(xcp->cp0_status & ST0_FR) ? x : (x & ~1)])
#define DPTOREG(dp,x)	(ctx->regs[(xcp->cp0_status & ST0_FR) ? x : (x & ~1)]\
			= (dp).bits)
#else
/* Beware: MIPS COP1 doubles are always little_word endian in registers */
#define DPFROMREG(dp,x)	\
  ((dp).bits = ((unsigned long long)ctx->regs[(x)+1] << 32) | ctx->regs[x])
#define DPTOREG(dp,x) \
  (ctx->regs[x] = (dp).bits, ctx->regs[(x)+1] = (dp).bits >> 32)
#endif

#if __mips >= 4 && __mips != 32

/*
 * Additional MIPS4 instructions
 */

static ieee754dp fpemu_dp_recip(ieee754dp d)
{
	return ieee754dp_div(ieee754dp_one(0), d);
}

static ieee754dp fpemu_dp_rsqrt(ieee754dp d)
{
	return ieee754dp_div(ieee754dp_one(0), ieee754dp_sqrt(d));
}

static ieee754sp fpemu_sp_recip(ieee754sp s)
{
	return ieee754sp_div(ieee754sp_one(0), s);
}

static ieee754sp fpemu_sp_rsqrt(ieee754sp s)
{
	return ieee754sp_div(ieee754sp_one(0), ieee754sp_sqrt(s));
}


static ieee754dp fpemu_dp_madd(ieee754dp r, ieee754dp s, ieee754dp t)
{
	return ieee754dp_add(ieee754dp_mul(s, t), r);
}

static ieee754dp fpemu_dp_msub(ieee754dp r, ieee754dp s, ieee754dp t)
{
	return ieee754dp_sub(ieee754dp_mul(s, t), r);
}

static ieee754dp fpemu_dp_nmadd(ieee754dp r, ieee754dp s, ieee754dp t)
{
	return ieee754dp_neg(ieee754dp_add(ieee754dp_mul(s, t), r));
}

static ieee754dp fpemu_dp_nmsub(ieee754dp r, ieee754dp s, ieee754dp t)
{
	return ieee754dp_neg(ieee754dp_sub(ieee754dp_mul(s, t), r));
}


static ieee754sp fpemu_sp_madd(ieee754sp r, ieee754sp s, ieee754sp t)
{
	return ieee754sp_add(ieee754sp_mul(s, t), r);
}

static ieee754sp fpemu_sp_msub(ieee754sp r, ieee754sp s, ieee754sp t)
{
	return ieee754sp_sub(ieee754sp_mul(s, t), r);
}

static ieee754sp fpemu_sp_nmadd(ieee754sp r, ieee754sp s, ieee754sp t)
{
	return ieee754sp_neg(ieee754sp_add(ieee754sp_mul(s, t), r));
}

static ieee754sp fpemu_sp_nmsub(ieee754sp r, ieee754sp s, ieee754sp t)
{
	return ieee754sp_neg(ieee754sp_sub(ieee754sp_mul(s, t), r));
}

static int fpux_emu(struct pt_regs *xcp, struct mips_fpu_soft_struct *ctx,
                    mips_instruction ir)
{
	unsigned rcsr = 0;	/* resulting csr */

	fpuemuprivate.stats.cp1xops++;

	switch (MIPSInst_FMA_FFMT(ir)) {
	case s_fmt:		/* 0 */
		{
			ieee754sp(*handler) (ieee754sp, ieee754sp,
					     ieee754sp);
			ieee754sp fd, fr, fs, ft;

			switch (MIPSInst_FUNC(ir)) {
			case lwxc1_op:
				{
					void *va =
					    REG_TO_VA(xcp->
						      regs[MIPSInst_FR(ir)]
						      +
						      xcp->
						      regs[MIPSInst_FT
							   (ir)]);
					fpureg_t val;
					int err = 0;
					val = mips_get_word(xcp, va, &err);
					if (err) {
						fpuemuprivate.stats.
						    errors++;
						return SIGBUS;
					}
					if (xcp->cp0_status & ST0_FR) {
						/* load whole register */
						ctx->
						    regs[MIPSInst_FD(ir)] =
						    val;
					} else if (MIPSInst_FD(ir) & 1) {
						/* load to m.s. 32 bits */
#if defined(SINGLE_ONLY_FPU)
						/* illegal register in single-float mode */
						return SIGILL;
#else
						ctx->
						    regs[
							 (MIPSInst_FD(ir) &
							  ~1)] &=
						    0xffffffff;
						ctx->
						    regs[
							 (MIPSInst_FD(ir) &
							  ~1)] |=
						    val << 32;
#endif
					} else {
						/* load to l.s. 32 bits */
						ctx->
						    regs[MIPSInst_FD(ir)]
						    &= ~0xffffffffLL;
						ctx->
						    regs[MIPSInst_FD(ir)]
						    |= val;
					}
				}
				break;

			case swxc1_op:
				{
					void *va =
					    REG_TO_VA(xcp->
						      regs[MIPSInst_FR(ir)]
						      +
						      xcp->
						      regs[MIPSInst_FT
							   (ir)]);
					unsigned int val;
					if (xcp->cp0_status & ST0_FR) {
						/* store whole register */
						val =
						    ctx->
						    regs[MIPSInst_FS(ir)];
					} else if (MIPSInst_FS(ir) & 1) {
#if defined(SINGLE_ONLY_FPU)
						/* illegal register in single-float mode */
						return SIGILL;
#else
						/* store from m.s. 32 bits */
						val =
						    ctx->
						    regs[
							 (MIPSInst_FS(ir) &
							  ~1)] >> 32;
#endif
					} else {
						/* store from l.s. 32 bits */
						val =
						    ctx->
						    regs[MIPSInst_FS(ir)];
					}
					if (mips_put_word(xcp, va, val)) {
						fpuemuprivate.stats.
						    errors++;
						return SIGBUS;
					}
				}
				break;

			case madd_s_op:
				handler = fpemu_sp_madd;
				goto scoptop;
			case msub_s_op:
				handler = fpemu_sp_msub;
				goto scoptop;
			case nmadd_s_op:
				handler = fpemu_sp_nmadd;
				goto scoptop;
			case nmsub_s_op:
				handler = fpemu_sp_nmsub;
				goto scoptop;

			      scoptop:
				SPFROMREG(fr, MIPSInst_FR(ir));
				SPFROMREG(fs, MIPSInst_FS(ir));
				SPFROMREG(ft, MIPSInst_FT(ir));
				fd = (*handler) (fr, fs, ft);
				SPTOREG(fd, MIPSInst_FD(ir));

			      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_INVALID_OPERATION)) rcsr |=
					    FPU_CSR_INV_X | FPU_CSR_INV_S;

				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;
				}

				break;

			default:
				return SIGILL;
			}
		}
		break;

#if !defined(SINGLE_ONLY_FPU)
	case d_fmt:		/* 1 */
		{
			ieee754dp(*handler) (ieee754dp, ieee754dp,
					     ieee754dp);
			ieee754dp fd, fr, fs, ft;

			switch (MIPSInst_FUNC(ir)) {
			case ldxc1_op:
				{
					void *va =
					    REG_TO_VA(xcp->
						      regs[MIPSInst_FR(ir)]
						      +
						      xcp->
						      regs[MIPSInst_FT
							   (ir)]);
					int err = 0;
					ctx->regs[MIPSInst_FD(ir)] =
					    mips_get_dword(xcp, va, &err);
					if (err) {
						fpuemuprivate.stats.
						    errors++;
						return SIGBUS;
					}
				}
				break;

			case sdxc1_op:
				{
					void *va =
					    REG_TO_VA(xcp->
						      regs[MIPSInst_FR(ir)]
						      +
						      xcp->
						      regs[MIPSInst_FT
							   (ir)]);
					if (mips_put_dword
					    (xcp, va,
					     ctx->regs[MIPSInst_FS(ir)])) {
						fpuemuprivate.stats.
						    errors++;
						return SIGBUS;
					}
				}
				break;

			case madd_d_op:
				handler = fpemu_dp_madd;
				goto dcoptop;
			case msub_d_op:
				handler = fpemu_dp_msub;
				goto dcoptop;
			case nmadd_d_op:
				handler = fpemu_dp_nmadd;
				goto dcoptop;
			case nmsub_d_op:
				handler = fpemu_dp_nmsub;
				goto dcoptop;

			      dcoptop:
				DPFROMREG(fr, MIPSInst_FR(ir));
				DPFROMREG(fs, MIPSInst_FS(ir));
				DPFROMREG(ft, MIPSInst_FT(ir));
				fd = (*handler) (fr, fs, ft);
				DPTOREG(fd, MIPSInst_FD(ir));
				goto copcsr;

			default:
				return SIGILL;
			}
		}
		break;
#endif

	case 0x7:		/* 7 */
		{
			if (MIPSInst_FUNC(ir) != pfetch_op) {
				return SIGILL;
			}
			/* ignore prefx operation */
		}
		break;