--- zzzz-none-000/linux-2.4.17/arch/mips/math-emu/cp1emu.c	2001-09-09 17:43:01.000000000 +0000
+++ sangam-fb-322/linux-2.4.17/arch/mips/math-emu/cp1emu.c	2004-11-24 13:22:41.000000000 +0000
@@ -173,7 +173,7 @@
 
 	if (!user_mode(xcp)) {
 		*perr = 0;
-		return (*(unsigned long *) va);
+		return *(unsigned long *) va;
 	}
 
 	*perr = (int) get_user(temp, (unsigned long *) va);
@@ -187,7 +187,7 @@
 
 	if (!user_mode(xcp)) {
 		*perr = 0;
-		return (*(unsigned long long *) va);
+		return *(unsigned long long *) va;
 	}
 
 	*perr = (int) get_user(temp, (unsigned long long *) va);
@@ -230,7 +230,7 @@
  * Two instructions if the instruction is in a branch delay slot.
  */
 
-static int cop1Emulate(struct pt_regs *xcp, struct mips_fpu_soft_struct *ctx)
+static int cop1Emulate(struct pt_regs *regs, struct mips_fpu_soft_struct *ctx)
 {
 	mips_instruction ir;
 	vaddr_t emulpc;
@@ -238,17 +238,17 @@
 	unsigned int cond;
 	int err = 0;
 
-	ir = mips_get_word(xcp, REG_TO_VA xcp->cp0_epc, &err);
+	ir = mips_get_word(regs, REG_TO_VA regs->cp0_epc, &err);
 	if (err) {
 		fpuemuprivate.stats.errors++;
 		return SIGBUS;
 	}
 
 	/* XXX NEC Vr54xx bug workaround */
-	if ((xcp->cp0_cause & CAUSEF_BD) && !isBranchInstr(&ir))
-		xcp->cp0_cause &= ~CAUSEF_BD;
+	if ((regs->cp0_cause & CAUSEF_BD) && !isBranchInstr(&ir))
+		regs->cp0_cause &= ~CAUSEF_BD;
 
-	if (xcp->cp0_cause & CAUSEF_BD) {
+	if (regs->cp0_cause & CAUSEF_BD) {
 		/*
 		 * The instruction to be emulated is in a branch delay slot
 		 * which means that we have to  emulate the branch instruction
@@ -261,24 +261,27 @@
 		 * Linux MIPS branch emulator operates on context, updating the
 		 * cp0_epc.
 		 */
-		emulpc = REG_TO_VA(xcp->cp0_epc + 4);	/* Snapshot emulation target */
+		emulpc = REG_TO_VA(regs->cp0_epc + 4);	/* Snapshot emulation target */
 
-		if (__compute_return_epc(xcp)) {
+		if (__compute_return_epc(regs)) {
 #ifdef CP1DBG
 			printk("failed to emulate branch at %p\n",
-				    REG_TO_VA(xcp->cp0_epc));
+				    REG_TO_VA(regs->cp0_epc));
 #endif
-			return SIGILL;;
+			return SIGILL;
 		}
-		ir = mips_get_word(xcp, emulpc, &err);
+		ir = mips_get_word(regs, emulpc, &err);
 		if (err) {
 			fpuemuprivate.stats.errors++;
 			return SIGBUS;
 		}
-		contpc = REG_TO_VA xcp->cp0_epc;
+		/* __computer_return_epc() will have updated cp0_epc */
+		contpc = REG_TO_VA regs->cp0_epc;
+		/* In order not to confuse ptrace() et al, tweak context */
+		regs->cp0_epc = VA_TO_REG emulpc - 4;
 	} else {
-		emulpc = REG_TO_VA xcp->cp0_epc;
-		contpc = REG_TO_VA xcp->cp0_epc + 4;
+		emulpc = REG_TO_VA regs->cp0_epc;
+		contpc = REG_TO_VA regs->cp0_epc + 4;
 	}
 
 emul:
@@ -289,12 +292,12 @@
 #ifndef SINGLE_ONLY_FPU
 	case ldc1_op:
 		{
-			void *va = REG_TO_VA(xcp->regs[MIPSInst_RS(ir)])
+			void *va = REG_TO_VA(regs->regs[MIPSInst_RS(ir)])
 			    + MIPSInst_SIMM(ir);
 			int ft = MIPSInst_RT(ir);
-			if (!(xcp->cp0_status & ST0_FR))
+			if (!(regs->cp0_status & ST0_FR))
 				ft &= ~1;
-			ctx->regs[ft] = mips_get_dword(xcp, va, &err);
+			ctx->regs[ft] = mips_get_dword(regs, va, &err);
 			fpuemuprivate.stats.loads++;
 			if (err) {
 				fpuemuprivate.stats.errors++;
@@ -305,13 +308,13 @@
 
 	case sdc1_op:
 		{
-			void *va = REG_TO_VA(xcp->regs[MIPSInst_RS(ir)])
+			void *va = REG_TO_VA(regs->regs[MIPSInst_RS(ir)])
 			    + MIPSInst_SIMM(ir);
 			int ft = MIPSInst_RT(ir);
-			if (!(xcp->cp0_status & ST0_FR))
+			if (!(regs->cp0_status & ST0_FR))
 				ft &= ~1;
 			fpuemuprivate.stats.stores++;
-			if (mips_put_dword(xcp, va, ctx->regs[ft])) {
+			if (mips_put_dword(regs, va, ctx->regs[ft])) {
 				fpuemuprivate.stats.errors++;
 				return SIGBUS;
 			}
@@ -321,17 +324,17 @@
 
 	case lwc1_op:
 		{
-			void *va = REG_TO_VA(xcp->regs[MIPSInst_RS(ir)])
+			void *va = REG_TO_VA(regs->regs[MIPSInst_RS(ir)])
 			    + MIPSInst_SIMM(ir);
 			fpureg_t val;
 			int ft = MIPSInst_RT(ir);
 			fpuemuprivate.stats.loads++;
-			val = mips_get_word(xcp, va, &err);
+			val = mips_get_word(regs, va, &err);
 			if (err) {
 				fpuemuprivate.stats.errors++;
 				return SIGBUS;
 			}
-			if (xcp->cp0_status & ST0_FR) {
+			if (regs->cp0_status & ST0_FR) {
 				/* load whole register */
 				ctx->regs[ft] = val;
 			} else if (ft & 1) {
@@ -353,12 +356,12 @@
 
 	case swc1_op:
 		{
-			void *va = REG_TO_VA(xcp->regs[MIPSInst_RS(ir)])
+			void *va = REG_TO_VA(regs->regs[MIPSInst_RS(ir)])
 			    + MIPSInst_SIMM(ir);
 			unsigned int val;
 			int ft = MIPSInst_RT(ir);
 			fpuemuprivate.stats.stores++;
-			if (xcp->cp0_status & ST0_FR) {
+			if (regs->cp0_status & ST0_FR) {
 				/* store whole register */
 				val = ctx->regs[ft];
 			} else if (ft & 1) {
@@ -373,7 +376,7 @@
 				/* store from l.s. 32 bits */
 				val = ctx->regs[ft];
 			}
-			if (mips_put_word(xcp, va, val)) {
+			if (mips_put_word(regs, va, val)) {
 				fpuemuprivate.stats.errors++;
 				return SIGBUS;
 			}
@@ -382,10 +385,10 @@
 #else				/* old 32-bit fpu registers */
 	case lwc1_op:
 		{
-			void *va = REG_TO_VA(xcp->regs[MIPSInst_RS(ir)])
+			void *va = REG_TO_VA(regs->regs[MIPSInst_RS(ir)])
 			    + MIPSInst_SIMM(ir);
 			ctx->regs[MIPSInst_RT(ir)] =
-			    mips_get_word(xcp, va, &err);
+			    mips_get_word(regs, va, &err);
 			fpuemuprivate.stats.loads++;
 			if (err) {
 				fpuemuprivate.stats.errors++;
@@ -396,11 +399,11 @@
 
 	case swc1_op:
 		{
-			void *va = REG_TO_VA(xcp->regs[MIPSInst_RS(ir)])
+			void *va = REG_TO_VA(regs->regs[MIPSInst_RS(ir)])
 			    + MIPSInst_SIMM(ir);
 			fpuemuprivate.stats.stores++;
 			if (mips_put_word
-			    (xcp, va, ctx->regs[MIPSInst_RT(ir)])) {
+			    (regs, va, ctx->regs[MIPSInst_RT(ir)])) {
 				fpuemuprivate.stats.errors++;
 				return SIGBUS;
 			}
@@ -408,24 +411,24 @@
 		break;
 	case ldc1_op:
 		{
-			void *va = REG_TO_VA(xcp->regs[MIPSInst_RS(ir)])
+			void *va = REG_TO_VA(regs->regs[MIPSInst_RS(ir)])
 			    + MIPSInst_SIMM(ir);
 			unsigned int rt = MIPSInst_RT(ir) & ~1;
 			int errs = 0;
 			fpuemuprivate.stats.loads++;
 #if (defined(BYTE_ORDER) && BYTE_ORDER == BIG_ENDIAN) || defined(__MIPSEB__)
 			ctx->regs[rt + 1] =
-			    mips_get_word(xcp, va + 0, &err);
+			    mips_get_word(regs, va + 0, &err);
 			errs += err;
 			ctx->regs[rt + 0] =
-			    mips_get_word(xcp, va + 4, &err);
+			    mips_get_word(regs, va + 4, &err);
 			errs += err;
 #else
 			ctx->regs[rt + 0] =
-			    mips_get_word(xcp, va + 0, &err);
+			    mips_get_word(regs, va + 0, &err);
 			errs += err;
 			ctx->regs[rt + 1] =
-			    mips_get_word(xcp, va + 4, &err);
+			    mips_get_word(regs, va + 4, &err);
 			errs += err;
 #endif
 			if (err)
@@ -435,19 +438,19 @@
 
 	case sdc1_op:
 		{
-			void *va = REG_TO_VA(xcp->regs[MIPSInst_RS(ir)])
+			void *va = REG_TO_VA(regs->regs[MIPSInst_RS(ir)])
 			    + MIPSInst_SIMM(ir);
 			unsigned int rt = MIPSInst_RT(ir) & ~1;
 			fpuemuprivate.stats.stores++;
 #if (defined(BYTE_ORDER) && BYTE_ORDER == BIG_ENDIAN) || defined(__MIPSEB__)
-			if (mips_put_word(xcp, va + 0, ctx->regs[rt + 1]))
+			if (mips_put_word(regs, va + 0, ctx->regs[rt + 1]))
 				return SIGBUS;
-			if (mips_put_word(xcp, va + 4, ctx->regs[rt + 0]))
+			if (mips_put_word(regs, va + 4, ctx->regs[rt + 0]))
 				return SIGBUS;
 #else
-			if (mips_put_word(xcp, va + 0, ctx->regs[rt + 0]))
+			if (mips_put_word(regs, va + 0, ctx->regs[rt + 0]))
 				return SIGBUS;
-			if (mips_put_word(xcp, va + 4, ctx->regs[rt + 1]))
+			if (mips_put_word(regs, va + 4, ctx->regs[rt + 1]))
 				return SIGBUS;
 #endif
 		}
@@ -463,9 +466,9 @@
 			/* copregister fs -> gpr[rt] */
 			if (MIPSInst_RT(ir) != 0) {
 				int fs = MIPSInst_RD(ir);
-				if (!(xcp->cp0_status & ST0_FR))
+				if (!(regs->cp0_status & ST0_FR))
 					fs &= ~1;
-				xcp->regs[MIPSInst_RT(ir)] = ctx->regs[fs];
+				regs->regs[MIPSInst_RT(ir)] = ctx->regs[fs];
 			}
 			break;
 
@@ -473,11 +476,11 @@
 			/* copregister fs <- rt */
 				fpureg_t value;
 				int fs = MIPSInst_RD(ir);
-				if (!(xcp->cp0_status & ST0_FR))
+				if (!(regs->cp0_status & ST0_FR))
 					fs &= ~1;
 				value =
 				    (MIPSInst_RT(ir) ==
-				     0) ? 0 : xcp->regs[MIPSInst_RT(ir)];
+				     0) ? 0 : regs->regs[MIPSInst_RT(ir)];
 				ctx->regs[fs] = value;
 			break;
 		}
@@ -493,7 +496,7 @@
 					/* illegal register in single-float mode */
 					return SIGILL;
 #else
-					if (!(xcp->cp0_status & ST0_FR)) {
+					if (!(regs->cp0_status & ST0_FR)) {
 						/* move from m.s. 32 bits */
 						value =
 						    ctx->
@@ -502,7 +505,7 @@
 					}
 #endif
 				}
-				xcp->regs[MIPSInst_RT(ir)] = value;
+				regs->regs[MIPSInst_RT(ir)] = value;
 			}
 			break;
 
@@ -514,14 +517,14 @@
 					value = 0;
 				else
 					value =
-					    (unsigned int) xcp->
+					    (unsigned int) regs->
 					    regs[MIPSInst_RT(ir)];
 				if (MIPSInst_RD(ir) & 1) {
 #ifdef SINGLE_ONLY_FPU
 					/* illegal register in single-float mode */
 					return SIGILL;
 #else
-					if (!(xcp->cp0_status & ST0_FR)) {
+					if (!(regs->cp0_status & ST0_FR)) {
 						/* move to m.s. 32 bits */
 						ctx->
 						    regs[
@@ -550,7 +553,7 @@
 			if (MIPSInst_RT(ir) != 0) {
 				unsigned value =
 				    ctx->regs[MIPSInst_RD(ir)];
-				xcp->regs[MIPSInst_RT(ir)] = value;
+				regs->regs[MIPSInst_RT(ir)] = value;
 			}
 			break;
 
@@ -560,7 +563,7 @@
 				unsigned value;
 				value =
 				    (MIPSInst_RT(ir) ==
-				     0) ? 0 : xcp->regs[MIPSInst_RT(ir)];
+				     0) ? 0 : regs->regs[MIPSInst_RT(ir)];
 				ctx->regs[MIPSInst_RD(ir)] = value;
 			}
 			break;
@@ -576,7 +579,7 @@
 #ifdef CSRTRACE
 					printk
 					    ("%p gpr[%d]<-csr=%08x\n",
-					     REG_TO_VA(xcp->cp0_epc),
+					     REG_TO_VA(regs->cp0_epc),
 					     MIPSInst_RT(ir), value);
 #endif
 				} else if (MIPSInst_RD(ir) == FPCREG_RID)
@@ -584,7 +587,7 @@
 				else
 					value = 0;
 				if (MIPSInst_RT(ir))
-					xcp->regs[MIPSInst_RT(ir)] = value;
+					regs->regs[MIPSInst_RT(ir)] = value;
 			}
 			break;
 
@@ -596,7 +599,7 @@
 				if (MIPSInst_RT(ir) == 0)
 					value = 0;
 				else
-					value = xcp->regs[MIPSInst_RT(ir)];
+					value = regs->regs[MIPSInst_RT(ir)];
 
 				/* we only have one writable control reg
 				 */
@@ -604,11 +607,14 @@
 #ifdef CSRTRACE
 					printk
 					    ("%p gpr[%d]->csr=%08x\n",
-					     REG_TO_VA(xcp->cp0_epc),
+					     REG_TO_VA(regs->cp0_epc),
 					     MIPSInst_RT(ir), value);
 #endif
 					ctx->sr = value;
-					/* copy new rounding mode to ieee library state! */
+					/* 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];
 				}
@@ -618,7 +624,7 @@
 		case bc_op: {
 			int likely = 0;
 
-			if (xcp->cp0_cause & CAUSEF_BD)
+			if (regs->cp0_cause & CAUSEF_BD)
 				return SIGILL;
 
 #if __mips >= 4
@@ -641,14 +647,14 @@
 				return SIGILL;
 			}
 
-			xcp->cp0_cause |= CAUSEF_BD;
+			regs->cp0_cause |= CAUSEF_BD;
 			if (cond) {
 				/* branch taken: emulate dslot instruction */
-				xcp->cp0_epc += 4;
-				contpc = REG_TO_VA xcp->cp0_epc +
+				regs->cp0_epc += 4;
+				contpc = REG_TO_VA regs->cp0_epc +
 				         (MIPSInst_SIMM(ir) << 2);
 
-				ir = mips_get_word(xcp, REG_TO_VA(xcp->cp0_epc),
+				ir = mips_get_word(regs, REG_TO_VA(regs->cp0_epc),
 				      &err);
 				if (err) {
 					fpuemuprivate.stats.errors++;
@@ -680,16 +686,21 @@
 				 * Single step the non-cp1 instruction in the
 				 * dslot
 				 */
-				return mips_dsemul(xcp, ir, contpc);
+				return mips_dsemul(regs, ir, contpc);
 			} else {
 				/* branch not taken */
-				if (likely)
+				if (likely) {
 					/*
 					 * branch likely nullifies dslot if not
 					 * taken
 					 */
-					xcp->cp0_epc += 4;
-					/* else continue & execute dslot as normal insn */
+					regs->cp0_epc += 4;
+					contpc += 4;
+					/*
+					 * else continue & execute dslot as
+					 * normal insn
+					 */
+				}
 			}
 			break;
 		}
@@ -701,7 +712,7 @@
 				return SIGILL;
 
 			/* a real fpu computation instruction */
-				if ((sig = fpu_emu(xcp, ctx, ir)))
+				if ((sig = fpu_emu(regs, ctx, ir)))
 					return sig;
 			}
 		}
@@ -711,7 +722,7 @@
 	case cop1x_op:
 		{
 			int sig;
-			if ((sig = fpux_emu(xcp, ctx, ir)))
+			if ((sig = fpux_emu(regs, ctx, ir)))
 				return sig;
 		}
 		break;
@@ -722,9 +733,9 @@
 		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)];
+		if (((ctx->sr & cond) != 0) != ((MIPSInst_RT(ir) & 1) != 0))
+			return 0;
+		regs->regs[MIPSInst_RD(ir)] = regs->regs[MIPSInst_RS(ir)];
 		break;
 #endif
 
@@ -733,8 +744,8 @@
 	}
 
 	/* we did it !! */
-	xcp->cp0_epc = VA_TO_REG(contpc);
-	xcp->cp0_cause &= ~CAUSEF_BD;
+	regs->cp0_epc = VA_TO_REG(contpc);
+	regs->cp0_cause &= ~CAUSEF_BD;
 	return 0;
 }
 
@@ -755,37 +766,84 @@
  *  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 */
+
 int do_dsemulret(struct pt_regs *xcp)
 {
+	unsigned long *pinst;
+	unsigned long stackitem;
+	int err = 0;
+
+	/* See if this trap was deliberate. First check the instruction */
+
+	pinst = (unsigned long *) REG_TO_VA(xcp->cp0_epc);
+
+	/* 
+	 * 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, pinst, sizeof(unsigned long) * 3))
+		return 0;
+
+	/* Is the instruction pointed to by the EPC an AdELOAD? */
+	stackitem = mips_get_word(xcp, pinst, &err);
+	if (err || (stackitem != AdELOAD))
+		return 0;
+
+	/* Is the following memory word the BD_COOKIE? */
+
+	stackitem = mips_get_word(xcp, pinst+1, &err);
+	if (err || (stackitem != BD_COOKIE))
+		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
+	/* Fetch the Saved EPC to Resume */
+
+	stackitem = mips_get_word(xcp, pinst+2, &err);
+	if (err) {
+		/* This is not a good situation to be in */
+		fpuemuprivate.stats.errors++;
+		force_sig(SIGBUS, current);
+		return 1;
+	}
+
 	/* 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;
+	xcp->cp0_epc = stackitem;
 
-	return 0;
+	return 1;
 }
 
 
 #define AdELOAD 0x8c000001	/* lw $0,1($0) */
 
-static int
-mips_dsemul(struct pt_regs *xcp, mips_instruction ir, vaddr_t cpc)
+static int mips_dsemul(struct pt_regs *regs, 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);
+		regs->cp0_epc = VA_TO_REG(cpc);
+		regs->cp0_cause &= ~CAUSEF_BD;
 		return 0;
 	}
 #ifdef DSEMUL_TRACE
-	printk("desemul %p %p\n", REG_TO_VA(xcp->cp0_epc), cpc);
+	printk("desemul %p %p\n", REG_TO_VA(regs->cp0_epc), cpc);
 #endif
 
 	/* 
@@ -793,21 +851,7 @@
 	 * 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
@@ -816,23 +860,39 @@
 	 * 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;
+	/* 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 */
+
+	/* Verify that the stack pointer is not competely insane */
+	if (verify_area
+	    (VERIFY_WRITE, dsemul_insns, sizeof(mips_instruction) * 4))
+		return SIGBUS;
 
-	if (mips_put_word(xcp, &dsemul_insns[1], forcetrap)) {
+	if (mips_put_word(regs, &dsemul_insns[0], ir)) {
 		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);
+	if (mips_put_word(regs, &dsemul_insns[1], (mips_instruction)AdELOAD)) {
+		fpuemuprivate.stats.errors++;
+		return (SIGBUS);
+	}
+	
+	if (mips_put_word(regs, &dsemul_insns[2], 
+			  (mips_instruction)BD_COOKIE)) {
+		fpuemuprivate.stats.errors++;
+		return (SIGBUS);
+	}
 
+	if (mips_put_word(regs, &dsemul_insns[3], (mips_instruction)cpc)) {
+		fpuemuprivate.stats.errors++;
+		return (SIGBUS);
+	}
+
+	regs->cp0_epc = VA_TO_REG & dsemul_insns[0];
+
+	flush_cache_sigtramp((unsigned long)dsemul_insns);
 	return SIGILL;		/* force out of emulation loop */
 }
 
@@ -881,6 +941,22 @@
  * 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);
@@ -901,47 +977,14 @@
 	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));
-}
+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)
@@ -1086,6 +1129,8 @@
 
 				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); */
@@ -1274,7 +1319,8 @@
 		case fmov_op:
 			/* an easy one */
 			SPFROMREG(rv.s, MIPSInst_FS(ir));
-			break;
+			goto copcsr;
+
 			/* binary op on handler */
 scopbop:
 			{
@@ -1386,13 +1432,16 @@
 				rfmt = -1;
 				if ((cmpop & 0x8) && ieee754_cxtest(IEEE754_INVALID_OPERATION))
 					rcsr = FPU_CSR_INV_X | FPU_CSR_INV_S;
-				} else {
-					return SIGILL;
-				}
-				break;
+				else
+					goto copcsr;
+
+			} else {
+				return SIGILL;
 			}
 			break;
 		}
+		break;
+	}
 
 #if !defined(SINGLE_ONLY_FPU)
 	case d_fmt: {
@@ -1456,7 +1505,7 @@
 		case fmov_op:
 			/* an easy one */
 			DPFROMREG(rv.d, MIPSInst_FS(ir));
-			break;
+			goto copcsr;
 
 			/* binary op on handler */
 dcopbop:
@@ -1553,6 +1602,9 @@
 				rfmt = -1;
 				if ((cmpop & 0x8) && ieee754_cxtest (IEEE754_INVALID_OPERATION))
 					rcsr = FPU_CSR_INV_X | FPU_CSR_INV_S;
+				else
+					goto copcsr;
+
 			} else {
 				return SIGILL;
 			}
@@ -1683,8 +1735,14 @@
 			fpuemuprivate.stats.errors++;
 			return SIGBUS;
 		}
-		if (insn != 0)
+		if (insn != 0) {
+			/* Update ieee754_csr. Only relevant if we have a
+			   h/w FPU */
+			ieee754_csr.nod = (ctx->sr & 0x1000000) != 0;
+			ieee754_csr.rm = ieee_rm[ctx->sr & 0x3];
+			ieee754_csr.cx = (ctx->sr >> 12) & 0x1f;
 			sig = cop1Emulate(xcp, ctx);
+		}
 		else
 			xcp->cp0_epc += 4;	/* skip nops */
 
@@ -1747,7 +1805,7 @@
 		 * i.e. denormalised results, underflow, overflow etc, which
 		 * must be emulated in s/w.
 		 */
-#ifdef 1
+#if 1
 		/* r4000 or above use dedicate exception */
 		xcption(XCPTFPE, cop1Patcher);
 #else