From fbf6ede2ce05592661cec04e04b88f6bab00eb09 Mon Sep 17 00:00:00 2001
From: "Maciej W. Rozycki" <macro@linux-mips.org>
Date: Mon, 21 May 2007 13:47:22 +0100
Subject: [PATCH] [MIPS] Fix KMODE for the R3000

 This must be the oldest bug that we have got.  Leaving interrupts "as
they are" for the R3000 obviously means copying IEp to IEc.  Since we have
got STATMASK now, I took this opportunity to mask the status register
"correctly" for the R3000 now too.  Oh, and the R3000 hardly ever is
64-bit.

Signed-off-by: Maciej W. Rozycki <macro@linux-mips.org>
Signed-off-by: Ralf Baechle <ralf@linux-mips.org>
---
 include/asm-mips/stackframe.h | 52 +++++++++++++++++++----------------
 1 file changed, 28 insertions(+), 24 deletions(-)

diff --git a/include/asm-mips/stackframe.h b/include/asm-mips/stackframe.h
index 7afa1fdf70..ed33366b85 100644
--- a/include/asm-mips/stackframe.h
+++ b/include/asm-mips/stackframe.h
@@ -17,6 +17,18 @@
 #include <asm/mipsregs.h>
 #include <asm/asm-offsets.h>
 
+/*
+ * For SMTC kernel, global IE should be left set, and interrupts
+ * controlled exclusively via IXMT.
+ */
+#ifdef CONFIG_MIPS_MT_SMTC
+#define STATMASK 0x1e
+#elif defined(CONFIG_CPU_R3000) || defined(CONFIG_CPU_TX39XX)
+#define STATMASK 0x3f
+#else
+#define STATMASK 0x1f
+#endif
+
 #ifdef CONFIG_MIPS_MT_SMTC
 #include <asm/mipsmtregs.h>
 #endif /* CONFIG_MIPS_MT_SMTC */
@@ -236,10 +248,10 @@
 		.set	reorder
 		.set	noat
 		mfc0	a0, CP0_STATUS
-		ori	a0, 0x1f
-		xori	a0, 0x1f
-		mtc0	a0, CP0_STATUS
 		li	v1, 0xff00
+		ori	a0, STATMASK
+		xori	a0, STATMASK
+		mtc0	a0, CP0_STATUS
 		and	a0, v1
 		LONG_L	v0, PT_STATUS(sp)
 		nor	v1, $0, v1
@@ -249,10 +261,6 @@
 		LONG_L	$31, PT_R31(sp)
 		LONG_L	$28, PT_R28(sp)
 		LONG_L	$25, PT_R25(sp)
-#ifdef CONFIG_64BIT
-		LONG_L	$8, PT_R8(sp)
-		LONG_L	$9, PT_R9(sp)
-#endif
 		LONG_L	$7,  PT_R7(sp)
 		LONG_L	$6,  PT_R6(sp)
 		LONG_L	$5,  PT_R5(sp)
@@ -273,16 +281,6 @@
 		.endm
 
 #else
-/*
- * For SMTC kernel, global IE should be left set, and interrupts
- * controlled exclusively via IXMT.
- */
-
-#ifdef CONFIG_MIPS_MT_SMTC
-#define STATMASK 0x1e
-#else
-#define STATMASK 0x1f
-#endif
 		.macro	RESTORE_SOME
 		.set	push
 		.set	reorder
@@ -385,9 +383,9 @@
 		.macro	CLI
 #if !defined(CONFIG_MIPS_MT_SMTC)
 		mfc0	t0, CP0_STATUS
-		li	t1, ST0_CU0 | 0x1f
+		li	t1, ST0_CU0 | STATMASK
 		or	t0, t1
-		xori	t0, 0x1f
+		xori	t0, STATMASK
 		mtc0	t0, CP0_STATUS
 #else /* CONFIG_MIPS_MT_SMTC */
 		/*
@@ -420,9 +418,9 @@
 		.macro	STI
 #if !defined(CONFIG_MIPS_MT_SMTC)
 		mfc0	t0, CP0_STATUS
-		li	t1, ST0_CU0 | 0x1f
+		li	t1, ST0_CU0 | STATMASK
 		or	t0, t1
-		xori	t0, 0x1e
+		xori	t0, STATMASK & ~1
 		mtc0	t0, CP0_STATUS
 #else /* CONFIG_MIPS_MT_SMTC */
 		/*
@@ -451,7 +449,8 @@
 		.endm
 
 /*
- * Just move to kernel mode and leave interrupts as they are.
+ * Just move to kernel mode and leave interrupts as they are.  Note
+ * for the R3000 this means copying the previous enable from IEp.
  * Set cp0 enable bit as sign that we're running on the kernel stack
  */
 		.macro	KMODE
@@ -482,9 +481,14 @@
 		move	ra, t0
 #endif /* CONFIG_MIPS_MT_SMTC */
 		mfc0	t0, CP0_STATUS
-		li	t1, ST0_CU0 | 0x1e
+		li	t1, ST0_CU0 | (STATMASK & ~1)
+#if defined(CONFIG_CPU_R3000) || defined(CONFIG_CPU_TX39XX)
+		andi	t2, t0, ST0_IEP
+		srl	t2, 2
+		or	t0, t2
+#endif
 		or	t0, t1
-		xori	t0, 0x1e
+		xori	t0, STATMASK & ~1
 		mtc0	t0, CP0_STATUS
 #ifdef CONFIG_MIPS_MT_SMTC
 		_ehb
-- 
2.39.5