cp1emu.c

MIPS YAMON, a famous monitor inc. source, make file and PDF manuals.

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

		case ctc_op:
			/* copregister rd <- rt */
			{
				unsigned value;

				if (MIPSInst_RT(ir) == 0)
					value = 0;
				else
					value = regs->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(regs->cp0_epc),
					     MIPSInst_RT(ir), value);
#endif
					ctx->sr = value;
					/* copy new rounding mode and
					   flush bit to ieee library state! */
					ieee754_csr.nod =
					    (ctx->sr & 0x1000000) != 0;
					ieee754_csr.rm =
					    ieee_rm[value & 0x3];
				}
				if ((ctx->sr >> 5) & ctx->sr & FPU_CSR_ALL_E) {
					return SIGFPE;
				}
			}
			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 *) REG_TO_VA 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, VA_TO_REG 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;
}

/* 
 * Conversion table from MIPS compare ops 48-63
 * cond = ieee754dp_cmp(x,y,IEEE754_UN,sig);
 */
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 */
};


#ifdef CP0_STATUS_FR_SUPPORT
#define SIFROMREG(si,x)	((si) = \
			(xcp->cp0_status & ST0_FR) || !(x & 1) ? \
			ctx->regs[x] : \
			ctx->regs[x & ~1] >> 32 )
#define SITOREG(si,x)	(ctx->regs[x & ~((xcp->cp0_status & ST0_FR) == 0)] = \
			(xcp->cp0_status & ST0_FR) || !(x & 1) ? \
			ctx->regs[x & ~1] >> 32 << 32 | (u32)(si) : \
			ctx->regs[x & ~1] << 32 >> 32 | (u64)(si) << 32)

#define DIFROMREG(di,x)	((di) = \
			ctx->regs[x & ~((xcp->cp0_status & ST0_FR) == 0)])
#define DITOREG(di,x)	(ctx->regs[x & ~((xcp->cp0_status & ST0_FR) == 0)] \
			= (di))
#define DIFROMREG(di,x)	((di) = \
			ctx->regs[x & ~((xcp->cp0_status & ST0_FR) == 0)])
#define DITOREG(di,x)	(ctx->regs[x & ~((xcp->cp0_status & ST0_FR) == 0)] \
			= (di))

#else

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

/* Beware: MIPS COP1 doubles are always little_word endian in registers */
#define DIFROMREG(di,x)	\
  ((di).bits = ((u64)ctx->regs[(x)+1] << 32) | ctx->regs[x])
#define DITOREG(di,x) \
  (ctx->regs[x] = (di).bits, ctx->regs[(x)+1] = (di).bits >> 32)
#endif

#define SPFROMREG(sp,x)	SIFROMREG((sp).bits,x)
#define SPTOREG(sp,x)	SITOREG((sp).bits,x)
#define DPFROMREG(dp,x)	DIFROMREG((dp).bits,x)
#define DPTOREG(dp,x)	DITOREG((dp).bits,x)

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

	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
		s64 l;
#endif
	} rv;			/* resulting value */

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