cp1emu.c

microwindows移植到S3C44B0的源码

					ieee754_csr.rm =
					    ieee_rm[value & 0x3];
				}
			}
			break;

		case bc_op: {
			int likely = 0;

			if (regs->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;
			}

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

				err = get_user(ir,
					(mips_instruction *)regs->cp0_epc);
				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(regs, ir, contpc);
			} else {
				/* branch not taken */
				if (likely) {
					/*
					 * branch likely nullifies dslot if not
					 * taken
					 */
					regs->cp0_epc += 4;
					contpc += 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(regs, ctx, ir)))
					return sig;
			}
		}
		break;

#if __mips >= 4 && __mips != 32
	case cop1x_op:
		{
			int sig;
			if ((sig = fpux_emu(regs, 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;
		regs->regs[MIPSInst_RD(ir)] = regs->regs[MIPSInst_RS(ir)];
		break;
#endif

	default:
		return SIGILL;
	}

	/* we did it !! */
	regs->cp0_epc = VA_TO_REG(contpc);
	regs->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.
 */

/* Instruction inserted following delay slot instruction to force trap */
#define AdELOAD 0x8c000001	/* lw $0,1($0) */

/* Instruction inserted following the AdELOAD to further tag the sequence */
#define BD_COOKIE 0x0000bd36	/* tne $0,$0 with baggage */

struct emuframe {
	mips_instruction	emul;
	mips_instruction	adel;
	mips_instruction	cookie;
	unsigned long	epc;
};

int do_dsemulret(struct pt_regs *xcp)
{
	struct emuframe *fr;
	unsigned long epc;
	u32 insn, cookie;
	int err = 0;

	fr = (struct emuframe *) (xcp->cp0_epc - sizeof(mips_instruction));

	/* 
	 * If we can't even access the area, something is very wrong, but we'll
	 * leave that to the default handling
	 */
	if (verify_area(VERIFY_READ, fr, sizeof(struct emuframe)))
		return 0;

	/*
	 * Do some sanity checking on the stackframe:
	 *
	 *  - Is the instruction pointed to by the EPC an AdELOAD?
	 *  - Is the following memory word the BD_COOKIE?
	 */
	err = __get_user(insn, &fr->adel);
	err |= __get_user(cookie, &fr->cookie);

	if (unlikely(err || (insn != AdELOAD) || (cookie != BD_COOKIE))) {
		fpuemuprivate.stats.errors++;

		return 0;
	}

	/* 
	 * At this point, we are satisfied that it's a BD emulation trap.  Yes,
	 * a user might have deliberately put two malformed and useless
	 * instructions in a row in his program, in which case he's in for a
	 * nasty surprise - the next instruction will be treated as a
	 * continuation address!  Alas, this seems to be the only way that we
	 * can handle signals, recursion, and longjmps() in the context of
	 * emulating the branch delay instruction.
	 */

#ifdef DSEMUL_TRACE
	printk("desemulret\n");
#endif
	if (__get_user(epc, &fr->epc)) {		/* Saved EPC */
		/* This is not a good situation to be in */
		force_sig(SIGBUS, current);

		return 1;
	}

	/* Set EPC to return to post-branch instruction */
	xcp->cp0_epc = epc;

	return 1;
}

static int mips_dsemul(struct pt_regs *regs, mips_instruction ir,
	unsigned long cpc)
{
	extern asmlinkage void handle_dsemulret(void);
	mips_instruction *dsemul_insns;
	struct emuframe *fr;
	int err;

	if (ir == 0) {		/* a nop is easy */
		regs->cp0_epc = cpc;
		regs->cp0_cause &= ~CAUSEF_BD;
		return 0;
	}
#ifdef DSEMUL_TRACE
	printk("desemul %lx %lx\n", regs->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!!.
	 *
	 * 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.
	 */

	/* Ensure that the two instructions are in the same cache line */
	dsemul_insns = (mips_instruction *) (regs->regs[29] & ~0xf);
	dsemul_insns -= 4;	/* Retain 16-byte alignment */
	fr = (struct emuframe *) dsemul_insns;

	/* Verify that the stack pointer is not competely insane */
	if (unlikely(verify_area(VERIFY_WRITE, fr, sizeof(struct emuframe))))
		return SIGBUS;

	err = __put_user(ir, &dsemul_insns[0]);
	err |= __put_user((mips_instruction)AdELOAD, &fr->adel);
	err |= __put_user((mips_instruction)BD_COOKIE, &fr->cookie);
	err |= __put_user(cpc, &fr->epc);

	if (unlikely(err)) {
		fpuemuprivate.stats.errors++;
		return SIGBUS;
	}

	regs->cp0_epc = VA_TO_REG & dsemul_insns[0];

	flush_cache_sigtramp((unsigned long)&fr->adel);

	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 = ((u64)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
 */

#define DEF3OP(name, p, f1, f2, f3) \
static ieee754##p fpemu_##p##_##name (ieee754##p r, ieee754##p s, \
    ieee754##p t) \
{ \
    struct ieee754_csr ieee754_csr_save; \
    s = f1 (s, t); \
    ieee754_csr_save = ieee754_csr; \
    s = f2 (s, r); \
    ieee754_csr_save.cx |= ieee754_csr.cx; \
    ieee754_csr_save.sx |= ieee754_csr.sx; \
    s = f3 (s); \
    ieee754_csr.cx |= ieee754_csr_save.cx; \
    ieee754_csr.sx |= ieee754_csr_save.sx; \
    return s; \
}    

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

DEF3OP(madd, sp, ieee754sp_mul, ieee754sp_add, );
DEF3OP(msub, sp, ieee754sp_mul, ieee754sp_sub, );
DEF3OP(nmadd, sp, ieee754sp_mul, ieee754sp_add, ieee754sp_neg);
DEF3OP(nmsub, sp, ieee754sp_mul, ieee754sp_sub, ieee754sp_neg);
DEF3OP(madd, dp, ieee754dp_mul, ieee754dp_add, );
DEF3OP(msub, dp, ieee754dp_mul, ieee754dp_sub, );
DEF3OP(nmadd, dp, ieee754dp_mul, ieee754dp_add, ieee754dp_neg);
DEF3OP(nmsub, dp, ieee754dp_mul, ieee754dp_sub, ieee754dp_neg);

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:
				{
					u32 *va = REG_TO_VA(
						xcp->regs[MIPSInst_FR(ir)] +
						xcp->regs[MIPSInst_FT(ir)]);
					fpureg_t val;

					if (get_user(val, va)) {
						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:
				{
					u32 *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 (put_user(val, va)) {
						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 (ieee754_csr.nod)
				    ctx->sr |= 0x1000000;
				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:
				{
					u64 *va = REG_TO_VA(
						xcp->regs[MIPSInst_FR(ir)] +
						xcp->regs[MIPSInst_FT(ir)]);
					u64 val;

					if (get_user(val, va)) {
						fpuemuprivate.stats.errors++;
						return SIGBUS;
					}
					ctx->regs[MIPSInst_FD(ir)] = val;
				}
				break;

			case sdxc1_op:
				{
					u64 *va = REG_TO_VA(
						xcp->regs[MIPSInst_FR(ir)] +
						xcp->regs[MIPSInst_FT(ir)]);
					u64 val;

					val = ctx->regs[MIPSInst_FS(ir)];
					if (put_user(val, va)) {
						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;