[MIPS] MT: Improved multithreading support.

    
Signed-off-by: Ralf Baechle <ralf@linux-mips.org>

16 files changed, 924 insertions, 21 deletions

diff --git a/include/asm-mips/asmmacro.h b/include/asm-mips/asmmacro.h
index 30b18ea6cb11..f54aa147ec19 100644
--- a/include/asm-mips/asmmacro.h
+++ b/include/asm-mips/asmmacro.h
@@ -17,7 +17,26 @@
 #ifdef CONFIG_64BIT
 #include <asm/asmmacro-64.h>
 #endif
+#ifdef CONFIG_MIPS_MT_SMTC
+#include <asm/mipsmtregs.h>
+#endif
 
+#ifdef CONFIG_MIPS_MT_SMTC
+	.macro	local_irq_enable reg=t0
+	mfc0	\reg, CP0_TCSTATUS
+	ori	\reg, \reg, TCSTATUS_IXMT
+	xori	\reg, \reg, TCSTATUS_IXMT
+	mtc0	\reg, CP0_TCSTATUS
+	ehb
+	.endm
+
+	.macro	local_irq_disable reg=t0
+	mfc0	\reg, CP0_TCSTATUS
+	ori	\reg, \reg, TCSTATUS_IXMT
+	mtc0	\reg, CP0_TCSTATUS
+	ehb
+	.endm
+#else
 	.macro	local_irq_enable reg=t0
 	mfc0	\reg, CP0_STATUS
 	ori	\reg, \reg, 1
@@ -32,6 +51,7 @@
 	mtc0	\reg, CP0_STATUS
 	irq_disable_hazard
 	.endm
+#endif /* CONFIG_MIPS_MT_SMTC */
 
 #ifdef CONFIG_CPU_SB1
 	.macro	fpu_enable_hazard
@@ -48,4 +68,31 @@
 	.endm
 #endif
 
+/*
+ * Temporary until all gas have MT ASE support
+ */
+	.macro	DMT	reg=0
+	.word	(0x41600bc1 | (\reg << 16))
+	.endm
+
+	.macro	EMT	reg=0
+	.word	(0x41600be1 | (\reg << 16))
+	.endm
+
+	.macro	DVPE	reg=0
+	.word	(0x41600001 | (\reg << 16))
+	.endm
+
+	.macro	EVPE	reg=0
+	.word	(0x41600021 | (\reg << 16))
+	.endm
+
+	.macro	MFTR	rt=0, rd=0, u=0, sel=0
+	 .word	(0x41000000 | (\rt << 16) | (\rd << 11) | (\u << 5) | (\sel))
+	.endm
+
+	.macro	MTTR	rt=0, rd=0, u=0, sel=0
+	 .word	(0x41800000 | (\rt << 16) | (\rd << 11) | (\u << 5) | (\sel))
+	.endm
+
 #endif /* _ASM_ASMMACRO_H */
diff --git a/include/asm-mips/cpu-info.h b/include/asm-mips/cpu-info.h
index 140be1c67da7..6572ac703662 100644
--- a/include/asm-mips/cpu-info.h
+++ b/include/asm-mips/cpu-info.h
@@ -73,6 +73,16 @@ struct cpuinfo_mips {
 	struct cache_desc	dcache;	/* Primary D or combined I/D cache */
 	struct cache_desc	scache;	/* Secondary cache */
 	struct cache_desc	tcache;	/* Tertiary/split secondary cache */
+#if defined(CONFIG_MIPS_MT_SMTC)
+	/*
+	 * In the MIPS MT "SMTC" model, each TC is considered
+	 * to be a "CPU" for the purposes of scheduling, but
+	 * exception resources, ASID spaces, etc, are common
+	 * to all TCs within the same VPE.
+	 */
+	int			vpe_id;  /* Virtual Processor number */
+	int			tc_id;   /* Thread Context number */
+#endif /* CONFIG_MIPS_MT */
 	void 			*data;	/* Additional data */
 } __attribute__((aligned(SMP_CACHE_BYTES)));
 
diff --git a/include/asm-mips/hazards.h b/include/asm-mips/hazards.h
index feb29a793888..dadc05188db7 100644
--- a/include/asm-mips/hazards.h
+++ b/include/asm-mips/hazards.h
@@ -284,6 +284,8 @@ do {									\
 #define instruction_hazard() do { } while (0)
 #endif
 
+extern void mips_ihb(void);
+
 #endif /* __ASSEMBLY__ */
 
 #endif /* _ASM_HAZARDS_H */
diff --git a/include/asm-mips/interrupt.h b/include/asm-mips/interrupt.h
index 774348734fa0..4bb9c06f4410 100644
--- a/include/asm-mips/interrupt.h
+++ b/include/asm-mips/interrupt.h
@@ -19,7 +19,12 @@ __asm__ (
 	"	.set	push						\n"
 	"	.set	reorder						\n"
 	"	.set	noat						\n"
-#ifdef CONFIG_CPU_MIPSR2
+#ifdef CONFIG_MIPS_MT_SMTC
+	"	mfc0	$1, $2, 1	# SMTC - clear TCStatus.IXMT	\n"
+	"	ori	$1, 0x400					\n"
+	"	xori	$1, 0x400					\n"
+	"	mtc0	$1, $2, 1					\n"
+#elif defined(CONFIG_CPU_MIPSR2)
 	"	ei							\n"
 #else
 	"	mfc0	$1,$12						\n"
@@ -62,7 +67,12 @@ __asm__ (
 	"	.macro	local_irq_disable\n"
 	"	.set	push						\n"
 	"	.set	noat						\n"
-#ifdef CONFIG_CPU_MIPSR2
+#ifdef CONFIG_MIPS_MT_SMTC
+	"	mfc0	$1, $2, 1					\n"
+	"	ori	$1, 0x400					\n"
+	"	.set	noreorder					\n"
+	"	mtc0	$1, $2, 1					\n"
+#elif defined(CONFIG_CPU_MIPSR2)
 	"	di							\n"
 #else
 	"	mfc0	$1,$12						\n"
@@ -88,7 +98,11 @@ __asm__ (
 	"	.macro	local_save_flags flags				\n"
 	"	.set	push						\n"
 	"	.set	reorder						\n"
+#ifdef CONFIG_MIPS_MT_SMTC
+	"	mfc0	\\flags, $2, 1					\n"
+#else
 	"	mfc0	\\flags, $12					\n"
+#endif
 	"	.set	pop						\n"
 	"	.endm							\n");
 
@@ -102,7 +116,13 @@ __asm__ (
 	"	.set	push						\n"
 	"	.set	reorder						\n"
 	"	.set	noat						\n"
-#ifdef CONFIG_CPU_MIPSR2
+#ifdef CONFIG_MIPS_MT_SMTC
+	"	mfc0	\\result, $2, 1					\n"
+	"	ori	$1, \\result, 0x400				\n"
+	"	.set	noreorder					\n"
+	"	mtc0	$1, $2, 1					\n"
+	"	andi	\\result, \\result, 0x400			\n"
+#elif defined(CONFIG_CPU_MIPSR2)
 	"	di	\\result					\n"
 	"	andi	\\result, 1					\n"
 #else
@@ -128,7 +148,14 @@ __asm__ (
 	"	.set	push						\n"
 	"	.set	noreorder					\n"
 	"	.set	noat						\n"
-#if defined(CONFIG_CPU_MIPSR2) && defined(CONFIG_IRQ_CPU)
+#ifdef CONFIG_MIPS_MT_SMTC
+	"mfc0	$1, $2, 1						\n"
+	"andi	\\flags, 0x400						\n"
+	"ori	$1, 0x400						\n"
+	"xori	$1, 0x400						\n"
+	"or	\\flags, $1						\n"
+	"mtc0	\\flags, $2, 1						\n"
+#elif defined(CONFIG_CPU_MIPSR2) && defined(CONFIG_IRQ_CPU)
 	/*
 	 * Slow, but doesn't suffer from a relativly unlikely race
 	 * condition we're having since days 1.
@@ -167,11 +194,29 @@ do {									\
 		: "memory");						\
 } while(0)
 
-#define irqs_disabled()							\
-({									\
-	unsigned long flags;						\
-	local_save_flags(flags);					\
-	!(flags & 1);							\
-})
+static inline int irqs_disabled(void)
+{
+#ifdef CONFIG_MIPS_MT_SMTC
+	/*
+	 * SMTC model uses TCStatus.IXMT to disable interrupts for a thread/CPU
+	 */
+	unsigned long __result;
+
+	__asm__ __volatile__(
+	"	.set	noreorder					\n"
+	"	mfc0	%0, $2, 1					\n"
+	"	andi	%0, 0x400					\n"
+	"	slt	%0, $0, %0					\n"
+	"	.set	reorder						\n"
+	: "=r" (__result));
+
+	return __result;
+#else
+	unsigned long flags;
+	local_save_flags(flags);
+
+	return !(flags & 1);
+#endif
+}
 
 #endif /* _ASM_INTERRUPT_H */
diff --git a/include/asm-mips/irq.h b/include/asm-mips/irq.h
index d7aecca3b95f..dde677f02bc0 100644
--- a/include/asm-mips/irq.h
+++ b/include/asm-mips/irq.h
@@ -11,6 +11,9 @@
 
 #include <linux/config.h>
 #include <linux/linkage.h>
+
+#include <asm/mipsmtregs.h>
+
 #include <irq.h>
 
 #ifdef CONFIG_I8259
@@ -26,6 +29,23 @@ struct pt_regs;
 
 extern asmlinkage unsigned int do_IRQ(unsigned int irq, struct pt_regs *regs);
 
+#ifdef CONFIG_MIPS_MT_SMTC
+/*
+ * Clear interrupt mask handling "backstop" if irq_hwmask
+ * entry so indicates. This implies that the ack() or end()
+ * functions will take over re-enabling the low-level mask.
+ * Otherwise it will be done on return from exception.
+ */
+#define __DO_IRQ_SMTC_HOOK()						\
+do {									\
+	if (irq_hwmask[irq] & 0x0000ff00)				\
+		write_c0_tccontext(read_c0_tccontext() &		\
+		                   ~(irq_hwmask[irq] & 0x0000ff00));	\
+} while (0)
+#else
+#define __DO_IRQ_SMTC_HOOK() do { } while (0)
+#endif
+
 #ifdef CONFIG_PREEMPT
 
 /*
@@ -39,6 +59,7 @@ extern asmlinkage unsigned int do_IRQ(unsigned int irq, struct pt_regs *regs);
 #define do_IRQ(irq, regs)						\
 do {									\
 	irq_enter();							\
+	__DO_IRQ_SMTC_HOOK();						\
 	__do_IRQ((irq), (regs));					\
 	irq_exit();							\
 } while (0)
@@ -48,4 +69,12 @@ do {									\
 extern void arch_init_irq(void);
 extern void spurious_interrupt(struct pt_regs *regs);
 
+#ifdef CONFIG_MIPS_MT_SMTC
+struct irqaction;
+
+extern unsigned long irq_hwmask[];
+extern int setup_irq_smtc(unsigned int irq, struct irqaction * new,
+                          unsigned long hwmask);
+#endif /* CONFIG_MIPS_MT_SMTC */
+
 #endif /* _ASM_IRQ_H */
diff --git a/include/asm-mips/mips_mt.h b/include/asm-mips/mips_mt.h
new file mode 100644
index 000000000000..c31a312b9783
--- /dev/null
+++ b/include/asm-mips/mips_mt.h
@@ -0,0 +1,15 @@
+/*
+ * Definitions and decalrations for MIPS MT support
+ * that are common between SMTC, VSMP, and/or AP/SP
+ * kernel models.
+ */
+#ifndef __ASM_MIPS_MT_H
+#define __ASM_MIPS_MT_H
+
+extern cpumask_t mt_fpu_cpumask;
+extern unsigned long mt_fpemul_threshold;
+
+extern void mips_mt_regdump(unsigned long previous_mvpcontrol_value);
+extern void mips_mt_set_cpuoptions(void);
+
+#endif /* __ASM_MIPS_MT_H */
diff --git a/include/asm-mips/mipsmtregs.h b/include/asm-mips/mipsmtregs.h
index a5ac1a62f4f4..f637ce70758f 100644
--- a/include/asm-mips/mipsmtregs.h
+++ b/include/asm-mips/mipsmtregs.h
@@ -165,7 +165,7 @@
 
 #ifndef __ASSEMBLY__
 
-extern void mips_mt_regdump(void);
+extern void mips_mt_regdump(unsigned long previous_mvpcontrol_value);
 
 static inline unsigned int dvpe(void)
 {
@@ -282,8 +282,11 @@ static inline void ehb(void)
 									\
 	__asm__ __volatile__(						\
 	"	.set	push					\n"	\
+	"	.set	noat					\n"	\
 	"	.set	mips32r2				\n"	\
-	"	mftgpr	%0," #rt "				\n"	\
+	"	# mftgpr $1," #rt "				\n"	\
+	"	.word	0x41000820 | (" #rt " << 16)		\n"	\
+	"	move	%0, $1					\n"	\
 	"	.set	pop					\n"	\
 	: "=r" (__res));						\
 									\
@@ -295,9 +298,7 @@ static inline void ehb(void)
 	unsigned long __res;						\
 									\
 	__asm__ __volatile__(						\
-	".set noat\n\t"							\
-	"mftr\t%0, " #rt ", " #u ", " #sel "\n\t"			\
-	".set at\n\t"							\
+	"	mftr	%0, " #rt ", " #u ", " #sel "		\n"	\
 	: "=r" (__res));						\
 									\
 	__res;								\
diff --git a/include/asm-mips/mipsregs.h b/include/asm-mips/mipsregs.h
index e85a42e2ea0c..a2ef579f6b1a 100644
--- a/include/asm-mips/mipsregs.h
+++ b/include/asm-mips/mipsregs.h
@@ -861,7 +861,19 @@ do {									\
 #define write_c0_compare3(val)	__write_32bit_c0_register($11, 7, val)
 
 #define read_c0_status()	__read_32bit_c0_register($12, 0)
+#ifdef CONFIG_MIPS_MT_SMTC
+#define write_c0_status(val)						\
+do {									\
+	__write_32bit_c0_register($12, 0, val);				\
+	__ehb();							\
+} while (0)
+#else
+/*
+ * Legacy non-SMTC code, which may be hazardous
+ * but which might not support EHB
+ */
 #define write_c0_status(val)	__write_32bit_c0_register($12, 0, val)
+#endif /* CONFIG_MIPS_MT_SMTC */
 
 #define read_c0_cause()		__read_32bit_c0_register($13, 0)
 #define write_c0_cause(val)	__write_32bit_c0_register($13, 0, val)
@@ -1004,6 +1016,9 @@ do {									\
 #define read_c0_taglo()		__read_32bit_c0_register($28, 0)
 #define write_c0_taglo(val)	__write_32bit_c0_register($28, 0, val)
 
+#define read_c0_dtaglo()	__read_32bit_c0_register($28, 2)
+#define write_c0_dtaglo(val)	__write_32bit_c0_register($28, 2, val)
+
 #define read_c0_taghi()		__read_32bit_c0_register($29, 0)
 #define write_c0_taghi(val)	__write_32bit_c0_register($29, 0, val)
 
@@ -1357,6 +1372,11 @@ static inline void tlb_write_random(void)
 /*
  * Manipulate bits in a c0 register.
  */
+#ifndef CONFIG_MIPS_MT_SMTC
+/*
+ * SMTC Linux requires shutting-down microthread scheduling
+ * during CP0 register read-modify-write sequences.
+ */
 #define __BUILD_SET_C0(name)					\
 static inline unsigned int					\
 set_c0_##name(unsigned int set)					\
@@ -1395,6 +1415,119 @@ change_c0_##name(unsigned int change, unsigned int new)		\
 	return res;						\
 }
 
+#else /* SMTC versions that manage MT scheduling */
+
+#include <asm/interrupt.h>
+
+/*
+ * This is a duplicate of dmt() in mipsmtregs.h to avoid problems with
+ * header file recursion.
+ */
+static inline unsigned int __dmt(void)
+{
+	int res;
+
+	__asm__ __volatile__(
+	"	.set	push						\n"
+	"	.set	mips32r2					\n"
+	"	.set	noat						\n"
+	"	.word	0x41610BC1			# dmt $1	\n"
+	"	ehb							\n"
+	"	move	%0, $1						\n"
+	"	.set	pop						\n"
+	: "=r" (res));
+
+	instruction_hazard();
+
+	return res;
+}
+
+#define __VPECONTROL_TE_SHIFT	15
+#define __VPECONTROL_TE		(1UL << __VPECONTROL_TE_SHIFT)
+
+#define __EMT_ENABLE		__VPECONTROL_TE
+
+static inline void __emt(unsigned int previous)
+{
+	if ((previous & __EMT_ENABLE))
+		__asm__ __volatile__(
+		"	.set	noreorder				\n"
+		"	.set	mips32r2				\n"
+		"	.word	0x41600be1		# emt		\n"
+		"	ehb						\n"
+		"	.set	mips0					\n"
+		"	.set	reorder					\n");
+}
+
+static inline void __ehb(void)
+{
+	__asm__ __volatile__(
+	"	ehb							\n");
+}
+
+/*
+ * Note that local_irq_save/restore affect TC-specific IXMT state,
+ * not Status.IE as in non-SMTC kernel.
+ */
+
+#define __BUILD_SET_C0(name)					\
+static inline unsigned int					\
+set_c0_##name(unsigned int set)					\
+{								\
+	unsigned int res;					\
+	unsigned int omt;					\
+	unsigned int flags;					\
+								\
+	local_irq_save(flags);					\
+	omt = __dmt();						\
+	res = read_c0_##name();					\
+	res |= set;						\
+	write_c0_##name(res);					\
+	__emt(omt);						\
+	local_irq_restore(flags);				\
+								\
+	return res;						\
+}								\
+								\
+static inline unsigned int					\
+clear_c0_##name(unsigned int clear)				\
+{								\
+	unsigned int res;					\
+	unsigned int omt;					\
+	unsigned int flags;					\
+								\
+	local_irq_save(flags);					\
+	omt = __dmt();						\
+	res = read_c0_##name();					\
+	res &= ~clear;						\
+	write_c0_##name(res);					\
+	__emt(omt);						\
+	local_irq_restore(flags);				\
+								\
+	return res;						\
+}								\
+								\
+static inline unsigned int					\
+change_c0_##name(unsigned int change, unsigned int new)		\
+{								\
+	unsigned int res;					\
+	unsigned int omt;					\
+	unsigned int flags;					\
+								\
+	local_irq_save(flags);					\
+								\
+	omt = __dmt();						\
+	res = read_c0_##name();					\
+	res &= ~change;						\
+	res |= (new & change);					\
+	write_c0_##name(res);					\
+	__emt(omt);						\
+	local_irq_restore(flags);				\
+								\
+	return res;						\
+}
+#endif
+
 __BUILD_SET_C0(status)
 __BUILD_SET_C0(cause)
 __BUILD_SET_C0(config)
diff --git a/include/asm-mips/mmu_context.h b/include/asm-mips/mmu_context.h
index 61cf22588137..6e09f4c87211 100644
--- a/include/asm-mips/mmu_context.h
+++ b/include/asm-mips/mmu_context.h
@@ -17,6 +17,10 @@
 #include <linux/slab.h>
 #include <asm/cacheflush.h>
 #include <asm/tlbflush.h>
+#ifdef CONFIG_MIPS_MT_SMTC
+#include <asm/mipsmtregs.h>
+#include <asm/smtc.h>
+#endif /* SMTC */
 
 /*
  * For the fast tlb miss handlers, we keep a per cpu array of pointers
@@ -54,6 +58,14 @@ extern unsigned long pgd_current[];
 #define ASID_INC	0x1
 #define ASID_MASK	0xfff
 
+/* SMTC/34K debug hack - but maybe we'll keep it */
+#elif defined(CONFIG_MIPS_MT_SMTC)
+
+#define ASID_INC	0x1
+extern unsigned long smtc_asid_mask;
+#define ASID_MASK	(smtc_asid_mask)
+#define	HW_ASID_MASK	0xff
+/* End SMTC/34K debug hack */
 #else /* FIXME: not correct for R6000 */
 
 #define ASID_INC	0x1
@@ -76,6 +88,8 @@ static inline void enter_lazy_tlb(struct mm_struct *mm, struct task_struct *tsk)
 #define ASID_VERSION_MASK  ((unsigned long)~(ASID_MASK|(ASID_MASK-1)))
 #define ASID_FIRST_VERSION ((unsigned long)(~ASID_VERSION_MASK) + 1)
 
+#ifndef CONFIG_MIPS_MT_SMTC
+/* Normal, classic MIPS get_new_mmu_context */
 static inline void
 get_new_mmu_context(struct mm_struct *mm, unsigned long cpu)
 {
@@ -91,6 +105,12 @@ get_new_mmu_context(struct mm_struct *mm, unsigned long cpu)
 	cpu_context(cpu, mm) = asid_cache(cpu) = asid;
 }
 
+#else /* CONFIG_MIPS_MT_SMTC */
+
+#define get_new_mmu_context(mm,cpu) smtc_get_new_mmu_context((mm),(cpu))
+
+#endif /* CONFIG_MIPS_MT_SMTC */
+
 /*
  * Initialize the context related info for a new mm_struct
  * instance.
@@ -111,14 +131,46 @@ static inline void switch_mm(struct mm_struct *prev, struct mm_struct *next,
 {
 	unsigned int cpu = smp_processor_id();
 	unsigned long flags;
-
+#ifdef CONFIG_MIPS_MT_SMTC
+	unsigned long oldasid;
+	unsigned long mtflags;
+	int mytlb = (smtc_status & SMTC_TLB_SHARED) ? 0 : cpu_data[cpu].vpe_id;
 	local_irq_save(flags);
+	mtflags = dvpe();
+#else /* Not SMTC */
+	local_irq_save(flags);
+#endif /* CONFIG_MIPS_MT_SMTC */
 
 	/* Check if our ASID is of an older version and thus invalid */
 	if ((cpu_context(cpu, next) ^ asid_cache(cpu)) & ASID_VERSION_MASK)
 		get_new_mmu_context(next, cpu);
-
+#ifdef CONFIG_MIPS_MT_SMTC
+	/*
+	 * If the EntryHi ASID being replaced happens to be
+	 * the value flagged at ASID recycling time as having
+	 * an extended life, clear the bit showing it being
+	 * in use by this "CPU", and if that's the last bit,
+	 * free up the ASID value for use and flush any old
+	 * instances of it from the TLB.
+	 */
+	oldasid = (read_c0_entryhi() & ASID_MASK);
+	if(smtc_live_asid[mytlb][oldasid]) {
+		smtc_live_asid[mytlb][oldasid] &= ~(0x1 << cpu);
+		if(smtc_live_asid[mytlb][oldasid] == 0)
+			smtc_flush_tlb_asid(oldasid);
+	}
+	/*
+	 * Tread softly on EntryHi, and so long as we support
+	 * having ASID_MASK smaller than the hardware maximum,
+	 * make sure no "soft" bits become "hard"...
+	 */
+	write_c0_entryhi((read_c0_entryhi() & ~HW_ASID_MASK)
+			| (cpu_context(cpu, next) & ASID_MASK));
+	ehb(); /* Make sure it propagates to TCStatus */
+	evpe(mtflags);
+#else
 	write_c0_entryhi(cpu_context(cpu, next));
+#endif /* CONFIG_MIPS_MT_SMTC */
 	TLBMISS_HANDLER_SETUP_PGD(next->pgd);
 
 	/*
@@ -151,12 +203,34 @@ activate_mm(struct mm_struct *prev, struct mm_struct *next)
 	unsigned long flags;
 	unsigned int cpu = smp_processor_id();
 
+#ifdef CONFIG_MIPS_MT_SMTC
+	unsigned long oldasid;
+	unsigned long mtflags;
+	int mytlb = (smtc_status & SMTC_TLB_SHARED) ? 0 : cpu_data[cpu].vpe_id;
+#endif /* CONFIG_MIPS_MT_SMTC */
+
 	local_irq_save(flags);
 
 	/* Unconditionally get a new ASID.  */
 	get_new_mmu_context(next, cpu);
 
+#ifdef CONFIG_MIPS_MT_SMTC
+	/* See comments for similar code above */
+	mtflags = dvpe();
+	oldasid = read_c0_entryhi() & ASID_MASK;
+	if(smtc_live_asid[mytlb][oldasid]) {
+		smtc_live_asid[mytlb][oldasid] &= ~(0x1 << cpu);
+       		if(smtc_live_asid[mytlb][oldasid] == 0)
+               		 smtc_flush_tlb_asid(oldasid);
+	}
+	/* See comments for similar code above */
+	write_c0_entryhi((read_c0_entryhi() & ~HW_ASID_MASK) |
+	                 (cpu_context(cpu, next) & ASID_MASK));
+	ehb(); /* Make sure it propagates to TCStatus */
+	evpe(mtflags);
+#else
 	write_c0_entryhi(cpu_context(cpu, next));
+#endif /* CONFIG_MIPS_MT_SMTC */
 	TLBMISS_HANDLER_SETUP_PGD(next->pgd);
 
 	/* mark mmu ownership change */
@@ -174,17 +248,49 @@ static inline void
 drop_mmu_context(struct mm_struct *mm, unsigned cpu)
 {
 	unsigned long flags;
+#ifdef CONFIG_MIPS_MT_SMTC
+	unsigned long oldasid;
+	/* Can't use spinlock because called from TLB flush within DVPE */
+	unsigned int prevvpe;
+	int mytlb = (smtc_status & SMTC_TLB_SHARED) ? 0 : cpu_data[cpu].vpe_id;
+#endif /* CONFIG_MIPS_MT_SMTC */
 
 	local_irq_save(flags);
 
 	if (cpu_isset(cpu, mm->cpu_vm_mask))  {
 		get_new_mmu_context(mm, cpu);
+#ifdef CONFIG_MIPS_MT_SMTC
+		/* See comments for similar code above */
+		prevvpe = dvpe();
+		oldasid = (read_c0_entryhi() & ASID_MASK);
+		if(smtc_live_asid[mytlb][oldasid]) {
+		  smtc_live_asid[mytlb][oldasid] &= ~(0x1 << cpu);
+		  if(smtc_live_asid[mytlb][oldasid] == 0)
+			smtc_flush_tlb_asid(oldasid);
+		}
+		/* See comments for similar code above */
+		write_c0_entryhi((read_c0_entryhi() & ~HW_ASID_MASK)
+				| cpu_asid(cpu, mm));
+		ehb(); /* Make sure it propagates to TCStatus */
+		evpe(prevvpe);
+#else /* not CONFIG_MIPS_MT_SMTC */
 		write_c0_entryhi(cpu_asid(cpu, mm));
+#endif /* CONFIG_MIPS_MT_SMTC */
 	} else {
 		/* will get a new context next time */
+#ifndef CONFIG_MIPS_MT_SMTC
 		cpu_context(cpu, mm) = 0;
+#else /* SMTC */
+		int i;
+
+		/* SMTC shares the TLB (and ASIDs) across VPEs */
+		for (i = 0; i < num_online_cpus(); i++) {
+	    	    if((smtc_status & SMTC_TLB_SHARED)
+	    	    || (cpu_data[i].vpe_id == cpu_data[cpu].vpe_id))
+			cpu_context(i, mm) = 0;
+		}
+#endif /* CONFIG_MIPS_MT_SMTC */
 	}
-
 	local_irq_restore(flags);
 }
 
diff --git a/include/asm-mips/processor.h b/include/asm-mips/processor.h
index 39d2bd50fece..786651340de1 100644
--- a/include/asm-mips/processor.h
+++ b/include/asm-mips/processor.h
@@ -12,6 +12,7 @@
 #define _ASM_PROCESSOR_H
 
 #include <linux/config.h>
+#include <linux/cpumask.h>
 #include <linux/threads.h>
 
 #include <asm/cachectl.h>
@@ -107,6 +108,10 @@ struct mips_dsp_state {
 
 #define INIT_DSP {{0,},}
 
+#define INIT_CPUMASK { \
+	{0,} \
+}
+
 typedef struct {
 	unsigned long seg;
 } mm_segment_t;
@@ -142,6 +147,7 @@ struct thread_struct {
 #define MF_LOGADE	2		/* Log address errors to syslog */
 #define MF_32BIT_REGS	4		/* also implies 16/32 fprs */
 #define MF_32BIT_ADDR	8		/* 32-bit address space (o32/n32) */
+#define MF_FPUBOUND	0x10		/* thread bound to FPU-full CPU set */
 	unsigned long mflags;
 	unsigned long irix_trampoline;  /* Wheee... */
 	unsigned long irix_oldctx;
diff --git a/include/asm-mips/ptrace.h b/include/asm-mips/ptrace.h
index 95c5839ac465..fa9d8713c12a 100644
--- a/include/asm-mips/ptrace.h
+++ b/include/asm-mips/ptrace.h
@@ -45,6 +45,10 @@ struct pt_regs {
 	unsigned long cp0_badvaddr;
 	unsigned long cp0_cause;
 	unsigned long cp0_epc;
+#ifdef CONFIG_MIPS_MT_SMTC
+	unsigned long cp0_tcstatus;
+	unsigned long smtc_pad;
+#endif /* CONFIG_MIPS_MT_SMTC */
 };
 
 /* Arbitrarily choose the same ptrace numbers as used by the Sparc code. */
diff --git a/include/asm-mips/r4kcache.h b/include/asm-mips/r4kcache.h
index 2f2eb95387f6..3c8e3c8d1a9a 100644
--- a/include/asm-mips/r4kcache.h
+++ b/include/asm-mips/r4kcache.h
@@ -15,6 +15,7 @@
 #include <asm/asm.h>
 #include <asm/cacheops.h>
 #include <asm/cpu-features.h>
+#include <asm/mipsmtregs.h>
 
 /*
  * This macro return a properly sign-extended address suitable as base address
@@ -39,14 +40,118 @@
 	:								\
 	: "i" (op), "R" (*(unsigned char *)(addr)))
 
+#ifdef CONFIG_MIPS_MT
+/*
+ * Temporary hacks for SMTC debug. Optionally force single-threaded
+ * execution during I-cache flushes.
+ */
+
+#define PROTECT_CACHE_FLUSHES 1
+
+#ifdef PROTECT_CACHE_FLUSHES
+
+extern int mt_protiflush;
+extern int mt_protdflush;
+extern void mt_cflush_lockdown(void);
+extern void mt_cflush_release(void);
+
+#define BEGIN_MT_IPROT \
+	unsigned long flags = 0;			\
+	unsigned long mtflags = 0;			\
+	if(mt_protiflush) {				\
+		local_irq_save(flags);			\
+		ehb();					\
+		mtflags = dvpe();			\
+		mt_cflush_lockdown();			\
+	}
+
+#define END_MT_IPROT \
+	if(mt_protiflush) {				\
+		mt_cflush_release();			\
+		evpe(mtflags);				\
+		local_irq_restore(flags);		\
+	}
+
+#define BEGIN_MT_DPROT \
+	unsigned long flags = 0;			\
+	unsigned long mtflags = 0;			\
+	if(mt_protdflush) {				\
+		local_irq_save(flags);			\
+		ehb();					\
+		mtflags = dvpe();			\
+		mt_cflush_lockdown();			\
+	}
+
+#define END_MT_DPROT \
+	if(mt_protdflush) {				\
+		mt_cflush_release();			\
+		evpe(mtflags);				\
+		local_irq_restore(flags);		\
+	}
+
+#else
+
+#define BEGIN_MT_IPROT
+#define BEGIN_MT_DPROT
+#define END_MT_IPROT
+#define END_MT_DPROT
+
+#endif /* PROTECT_CACHE_FLUSHES */
+
+#define __iflush_prologue						\
+	unsigned long redundance;					\
+	extern int mt_n_iflushes;					\
+	BEGIN_MT_IPROT							\
+	for (redundance = 0; redundance < mt_n_iflushes; redundance++) {
+
+#define __iflush_epilogue						\
+	END_MT_IPROT							\
+	}
+
+#define __dflush_prologue						\
+	unsigned long redundance;					\
+	extern int mt_n_dflushes;					\
+	BEGIN_MT_DPROT							\
+	for (redundance = 0; redundance < mt_n_dflushes; redundance++) {
+
+#define __dflush_epilogue \
+	END_MT_DPROT	 \
+	}
+
+#define __inv_dflush_prologue __dflush_prologue
+#define __inv_dflush_epilogue __dflush_epilogue
+#define __sflush_prologue {
+#define __sflush_epilogue }
+#define __inv_sflush_prologue __sflush_prologue
+#define __inv_sflush_epilogue __sflush_epilogue
+
+#else /* CONFIG_MIPS_MT */
+
+#define __iflush_prologue {
+#define __iflush_epilogue }
+#define __dflush_prologue {
+#define __dflush_epilogue }
+#define __inv_dflush_prologue {
+#define __inv_dflush_epilogue }
+#define __sflush_prologue {
+#define __sflush_epilogue }
+#define __inv_sflush_prologue {
+#define __inv_sflush_epilogue }
+
+#endif /* CONFIG_MIPS_MT */
+
 static inline void flush_icache_line_indexed(unsigned long addr)
 {
+	__iflush_prologue
 	cache_op(Index_Invalidate_I, addr);
+	__iflush_epilogue
 }
 
 static inline void flush_dcache_line_indexed(unsigned long addr)
 {
+	__dflush_prologue
 	cache_op(Index_Writeback_Inv_D, addr);
+	__dflush_epilogue
 }
 
 static inline void flush_scache_line_indexed(unsigned long addr)
@@ -56,17 +161,23 @@ static inline void flush_scache_line_indexed(unsigned long addr)
 
 static inline void flush_icache_line(unsigned long addr)
 {
+	__iflush_prologue
 	cache_op(Hit_Invalidate_I, addr);
+	__iflush_epilogue
 }
 
 static inline void flush_dcache_line(unsigned long addr)
 {
+	__dflush_prologue
 	cache_op(Hit_Writeback_Inv_D, addr);
+	__dflush_epilogue
 }
 
 static inline void invalidate_dcache_line(unsigned long addr)
 {
+	__dflush_prologue
 	cache_op(Hit_Invalidate_D, addr);
+	__dflush_epilogue
 }
 
 static inline void invalidate_scache_line(unsigned long addr)
@@ -239,9 +350,13 @@ static inline void blast_##pfx##cache##lsize(void)			\
 	                       current_cpu_data.desc.waybit;		\
 	unsigned long ws, addr;						\
 									\
+	__##pfx##flush_prologue						\
+									\
 	for (ws = 0; ws < ws_end; ws += ws_inc)				\
 		for (addr = start; addr < end; addr += lsize * 32)	\
 			cache##lsize##_unroll32(addr|ws,indexop);	\
+									\
+	__##pfx##flush_epilogue						\
 }									\
 									\
 static inline void blast_##pfx##cache##lsize##_page(unsigned long page)	\
@@ -249,10 +364,14 @@ static inline void blast_##pfx##cache##lsize##_page(unsigned long page)	\
 	unsigned long start = page;					\
 	unsigned long end = page + PAGE_SIZE;				\
 									\
+	__##pfx##flush_prologue						\
+									\
 	do {								\
 		cache##lsize##_unroll32(start,hitop);			\
 		start += lsize * 32;					\
 	} while (start < end);						\
+									\
+	__##pfx##flush_epilogue						\
 }									\
 									\
 static inline void blast_##pfx##cache##lsize##_page_indexed(unsigned long page) \
@@ -265,9 +384,13 @@ static inline void blast_##pfx##cache##lsize##_page_indexed(unsigned long page)
 	                       current_cpu_data.desc.waybit;		\
 	unsigned long ws, addr;						\
 									\
+	__##pfx##flush_prologue						\
+									\
 	for (ws = 0; ws < ws_end; ws += ws_inc)				\
 		for (addr = start; addr < end; addr += lsize * 32)	\
 			cache##lsize##_unroll32(addr|ws,indexop);	\
+									\
+	__##pfx##flush_epilogue						\
 }
 
 __BUILD_BLAST_CACHE(d, dcache, Index_Writeback_Inv_D, Hit_Writeback_Inv_D, 16)
@@ -288,12 +411,17 @@ static inline void prot##blast_##pfx##cache##_range(unsigned long start, \
 	unsigned long lsize = cpu_##desc##_line_size();			\
 	unsigned long addr = start & ~(lsize - 1);			\
 	unsigned long aend = (end - 1) & ~(lsize - 1);			\
+									\
+	__##pfx##flush_prologue						\
+									\
 	while (1) {							\
 		prot##cache_op(hitop, addr);				\
 		if (addr == aend)					\
 			break;						\
 		addr += lsize;						\
 	}								\
+									\
+	__##pfx##flush_epilogue						\
 }
 
 __BUILD_BLAST_CACHE_RANGE(d, dcache, Hit_Writeback_Inv_D, protected_)
diff --git a/include/asm-mips/smtc.h b/include/asm-mips/smtc.h
new file mode 100644
index 000000000000..e1941d1b8726
--- /dev/null
+++ b/include/asm-mips/smtc.h
@@ -0,0 +1,55 @@
+#ifndef _ASM_SMTC_MT_H
+#define _ASM_SMTC_MT_H
+
+/*
+ * Definitions for SMTC multitasking on MIPS MT cores
+ */
+
+#include <asm/mips_mt.h>
+
+/*
+ * System-wide SMTC status information
+ */
+
+extern unsigned int smtc_status;
+
+#define SMTC_TLB_SHARED	0x00000001
+#define SMTC_MTC_ACTIVE	0x00000002
+
+/*
+ * TLB/ASID Management information
+ */
+
+#define MAX_SMTC_TLBS 2
+#define MAX_SMTC_ASIDS 256
+#if NR_CPUS <= 8
+typedef char asiduse;
+#else
+#if NR_CPUS <= 16
+typedef short asiduse;
+#else
+typedef long asiduse;
+#endif
+#endif
+
+extern asiduse smtc_live_asid[MAX_SMTC_TLBS][MAX_SMTC_ASIDS];
+
+void smtc_get_new_mmu_context(struct mm_struct *mm, unsigned long cpu);
+
+void smtc_flush_tlb_asid(unsigned long asid);
+extern int mipsmt_build_cpu_map(int startslot);
+extern void mipsmt_prepare_cpus(void);
+extern void smtc_smp_finish(void);
+extern void smtc_boot_secondary(int cpu, struct task_struct *t);
+
+/*
+ * Sharing the TLB between multiple VPEs means that the
+ * "random" index selection function is not allowed to
+ * select the current value of the Index register. To
+ * avoid additional TLB pressure, the Index registers
+ * are "parked" with an non-Valid value.
+ */
+
+#define PARKED_INDEX	((unsigned int)0x80000000)
+
+#endif /*  _ASM_SMTC_MT_H */
diff --git a/include/asm-mips/smtc_ipi.h b/include/asm-mips/smtc_ipi.h
new file mode 100644
index 000000000000..f22c3e2f993a
--- /dev/null
+++ b/include/asm-mips/smtc_ipi.h
@@ -0,0 +1,118 @@
+/*
+ * Definitions used in MIPS MT SMTC "Interprocessor Interrupt" code.
+ */
+#ifndef __ASM_SMTC_IPI_H
+#define __ASM_SMTC_IPI_H
+
+//#define SMTC_IPI_DEBUG
+
+#ifdef SMTC_IPI_DEBUG
+#include <asm/mipsregs.h>
+#include <asm/mipsmtregs.h>
+#endif /* SMTC_IPI_DEBUG */
+
+/*
+ * An IPI "message"
+ */
+
+struct smtc_ipi {
+	struct smtc_ipi *flink;
+	int type;
+	void *arg;
+	int dest;
+#ifdef	SMTC_IPI_DEBUG
+	int sender;
+	long stamp;
+#endif /* SMTC_IPI_DEBUG */
+};
+
+/*
+ * Defined IPI Types
+ */
+
+#define LINUX_SMP_IPI 1
+#define SMTC_CLOCK_TICK 2
+
+/*
+ * A queue of IPI messages
+ */
+
+struct smtc_ipi_q {
+	struct smtc_ipi *head;
+	spinlock_t lock;
+	struct smtc_ipi *tail;
+	int depth;
+};
+
+extern struct smtc_ipi_q IPIQ[NR_CPUS];
+extern struct smtc_ipi_q freeIPIq;
+
+static inline void smtc_ipi_nq(struct smtc_ipi_q *q, struct smtc_ipi *p)
+{
+	long flags;
+
+	spin_lock_irqsave(&q->lock, flags);
+	if (q->head == NULL)
+		q->head = q->tail = p;
+	else
+		q->tail->flink = p;
+	p->flink = NULL;
+	q->tail = p;
+	q->depth++;
+#ifdef	SMTC_IPI_DEBUG
+	p->sender = read_c0_tcbind();
+	p->stamp = read_c0_count();
+#endif /* SMTC_IPI_DEBUG */
+	spin_unlock_irqrestore(&q->lock, flags);
+}
+
+static inline struct smtc_ipi *smtc_ipi_dq(struct smtc_ipi_q *q)
+{
+	struct smtc_ipi *p;
+	long flags;
+
+	spin_lock_irqsave(&q->lock, flags);
+	if (q->head == NULL)
+		p = NULL;
+	else {
+		p = q->head;
+		q->head = q->head->flink;
+		q->depth--;
+		/* Arguably unnecessary, but leaves queue cleaner */
+		if (q->head == NULL)
+			q->tail = NULL;
+	}
+	spin_unlock_irqrestore(&q->lock, flags);
+	return p;
+}
+
+static inline void smtc_ipi_req(struct smtc_ipi_q *q, struct smtc_ipi *p)
+{
+	long flags;
+
+	spin_lock_irqsave(&q->lock, flags);
+	if (q->head == NULL) {
+		q->head = q->tail = p;
+		p->flink = NULL;
+	} else {
+		p->flink = q->head;
+		q->head = p;
+	}
+	q->depth++;
+	spin_unlock_irqrestore(&q->lock, flags);
+}
+
+static inline int smtc_ipi_qdepth(struct smtc_ipi_q *q)
+{
+	long flags;
+	int retval;
+
+	spin_lock_irqsave(&q->lock, flags);
+	retval = q->depth;
+	spin_unlock_irqrestore(&q->lock, flags);
+	return retval;
+}
+
+extern void smtc_send_ipi(int cpu, int type, unsigned int action);
+
+#endif /* __ASM_SMTC_IPI_H */
diff --git a/include/asm-mips/smtc_proc.h b/include/asm-mips/smtc_proc.h
new file mode 100644
index 000000000000..25da651f1f5f
--- /dev/null
+++ b/include/asm-mips/smtc_proc.h
@@ -0,0 +1,23 @@
+/*
+ * Definitions for SMTC /proc entries
+ * Copyright(C) 2005 MIPS Technologies Inc.
+ */
+#ifndef __ASM_SMTC_PROC_H
+#define __ASM_SMTC_PROC_H
+
+/*
+ * per-"CPU" statistics
+ */
+
+struct smtc_cpu_proc {
+	unsigned long timerints;
+	unsigned long selfipis;
+};
+
+extern struct smtc_cpu_proc smtc_cpu_stats[NR_CPUS];
+
+/* Count of number of recoveries of "stolen" FPU access rights on 34K */
+
+extern atomic_t smtc_fpu_recoveries;
+
+#endif /* __ASM_SMTC_PROC_H */
diff --git a/include/asm-mips/stackframe.h b/include/asm-mips/stackframe.h
index 2acf3e844f00..c4856a874965 100644
--- a/include/asm-mips/stackframe.h
+++ b/include/asm-mips/stackframe.h
@@ -14,9 +14,14 @@
 #include <linux/threads.h>
 
 #include <asm/asm.h>
+#include <asm/asmmacro.h>
 #include <asm/mipsregs.h>
 #include <asm/asm-offsets.h>
 
+#ifdef CONFIG_MIPS_MT_SMTC
+#include <asm/mipsmtregs.h>
+#endif /* CONFIG_MIPS_MT_SMTC */
+
 		.macro	SAVE_AT
 		.set	push
 		.set	noat
@@ -57,13 +62,30 @@
 #ifdef CONFIG_SMP
 		.macro	get_saved_sp	/* SMP variation */
 #ifdef CONFIG_32BIT
+#ifdef CONFIG_MIPS_MT_SMTC
+		.set	mips32
+		mfc0	k0, CP0_TCBIND;
+		.set	mips0
+		lui	k1, %hi(kernelsp)
+		srl	k0, k0, 19
+		/* No need to shift down and up to clear bits 0-1 */
+#else
 		mfc0	k0, CP0_CONTEXT
 		lui	k1, %hi(kernelsp)
 		srl	k0, k0, 23
+#endif
 		addu	k1, k0
 		LONG_L	k1, %lo(kernelsp)(k1)
 #endif
 #ifdef CONFIG_64BIT
+#ifdef CONFIG_MIPS_MT_SMTC
+		.set	mips64
+		mfc0	k0, CP0_TCBIND;
+		.set	mips0
+		lui	k0, %highest(kernelsp)
+		dsrl	k1, 19
+		/* No need to shift down and up to clear bits 0-2 */
+#else
 		MFC0	k1, CP0_CONTEXT
 		lui	k0, %highest(kernelsp)
 		dsrl	k1, 23
@@ -71,20 +93,31 @@
 		dsll	k0, k0, 16
 		daddiu	k0, %hi(kernelsp)
 		dsll	k0, k0, 16
+#endif /* CONFIG_MIPS_MT_SMTC */
 		daddu	k1, k1, k0
 		LONG_L	k1, %lo(kernelsp)(k1)
-#endif
+#endif /* CONFIG_64BIT */
 		.endm
 
 		.macro	set_saved_sp stackp temp temp2
 #ifdef CONFIG_32BIT
+#ifdef CONFIG_MIPS_MT_SMTC
+		mfc0	\temp, CP0_TCBIND
+		srl	\temp, 19
+#else
 		mfc0	\temp, CP0_CONTEXT
 		srl	\temp, 23
 #endif
+#endif
 #ifdef CONFIG_64BIT
+#ifdef CONFIG_MIPS_MT_SMTC
+		mfc0	\temp, CP0_TCBIND
+		dsrl	\temp, 19
+#else
 		MFC0	\temp, CP0_CONTEXT
 		dsrl	\temp, 23
 #endif
+#endif
 		LONG_S	\stackp, kernelsp(\temp)
 		.endm
 #else
@@ -122,10 +155,25 @@
 		PTR_SUBU sp, k1, PT_SIZE
 		LONG_S	k0, PT_R29(sp)
 		LONG_S	$3, PT_R3(sp)
+		/*
+		 * You might think that you don't need to save $0,
+		 * but the FPU emulator and gdb remote debug stub
+		 * need it to operate correctly
+		 */
 		LONG_S	$0, PT_R0(sp)
 		mfc0	v1, CP0_STATUS
 		LONG_S	$2, PT_R2(sp)
 		LONG_S	v1, PT_STATUS(sp)
+#ifdef CONFIG_MIPS_MT_SMTC
+		/*
+		 * Ideally, these instructions would be shuffled in
+		 * to cover the pipeline delay.
+		 */
+		.set	mips32
+		mfc0	v1, CP0_TCSTATUS
+		.set	mips0
+		LONG_S	v1, PT_TCSTATUS(sp)
+#endif /* CONFIG_MIPS_MT_SMTC */
 		LONG_S	$4, PT_R4(sp)
 		mfc0	v1, CP0_CAUSE
 		LONG_S	$5, PT_R5(sp)
@@ -234,14 +282,36 @@
 		.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
 		.set	noat
+#ifdef CONFIG_MIPS_MT_SMTC
+		.set	mips32r2
+		/*
+		 * This may not really be necessary if ints are already
+		 * inhibited here.
+		 */
+		mfc0	v0, CP0_TCSTATUS
+		ori	v0, TCSTATUS_IXMT
+		mtc0	v0, CP0_TCSTATUS
+		ehb
+		DMT	5				# dmt a1
+		jal	mips_ihb
+#endif /* CONFIG_MIPS_MT_SMTC */
 		mfc0	a0, CP0_STATUS
-		ori	a0, 0x1f
-		xori	a0, 0x1f
+		ori	a0, STATMASK
+		xori	a0, STATMASK
 		mtc0	a0, CP0_STATUS
 		li	v1, 0xff00
 		and	a0, v1
@@ -250,6 +320,26 @@
 		and	v0, v1
 		or	v0, a0
 		mtc0	v0, CP0_STATUS
+#ifdef CONFIG_MIPS_MT_SMTC
+/*
+ * Only after EXL/ERL have been restored to status can we
+ * restore TCStatus.IXMT.
+ */
+		LONG_L	v1, PT_TCSTATUS(sp)
+		ehb
+		mfc0	v0, CP0_TCSTATUS
+		andi	v1, TCSTATUS_IXMT
+		/* We know that TCStatua.IXMT should be set from above */
+		xori	v0, v0, TCSTATUS_IXMT
+		or	v0, v0, v1
+		mtc0	v0, CP0_TCSTATUS
+		ehb
+		andi	a1, a1, VPECONTROL_TE
+		beqz	a1, 1f
+		emt
+1:
+		.set	mips0
+#endif /* CONFIG_MIPS_MT_SMTC */
 		LONG_L	v1, PT_EPC(sp)
 		MTC0	v1, CP0_EPC
 		LONG_L	$31, PT_R31(sp)
@@ -302,11 +392,33 @@
  * Set cp0 enable bit as sign that we're running on the kernel stack
  */
 		.macro	CLI
+#if !defined(CONFIG_MIPS_MT_SMTC)
 		mfc0	t0, CP0_STATUS
 		li	t1, ST0_CU0 | 0x1f
 		or	t0, t1
 		xori	t0, 0x1f
 		mtc0	t0, CP0_STATUS
+#else /* CONFIG_MIPS_MT_SMTC */
+		/*
+		 * For SMTC, we need to set privilege
+		 * and disable interrupts only for the
+		 * current TC, using the TCStatus register.
+		 */
+		mfc0	t0,CP0_TCSTATUS
+		/* Fortunately CU 0 is in the same place in both registers */
+		/* Set TCU0, TMX, TKSU (for later inversion) and IXMT */
+		li	t1, ST0_CU0 | 0x08001c00
+		or	t0,t1
+		/* Clear TKSU, leave IXMT */
+		xori	t0, 0x00001800
+		mtc0	t0, CP0_TCSTATUS
+		ehb
+		/* We need to leave the global IE bit set, but clear EXL...*/
+		mfc0	t0, CP0_STATUS
+		ori	t0, ST0_EXL | ST0_ERL
+		xori	t0, ST0_EXL | ST0_ERL
+		mtc0	t0, CP0_STATUS
+#endif /* CONFIG_MIPS_MT_SMTC */
 		irq_disable_hazard
 		.endm
 
@@ -315,11 +427,35 @@
  * Set cp0 enable bit as sign that we're running on the kernel stack
  */
 		.macro	STI
+#if !defined(CONFIG_MIPS_MT_SMTC)
 		mfc0	t0, CP0_STATUS
 		li	t1, ST0_CU0 | 0x1f
 		or	t0, t1
 		xori	t0, 0x1e
 		mtc0	t0, CP0_STATUS
+#else /* CONFIG_MIPS_MT_SMTC */
+		/*
+		 * For SMTC, we need to set privilege
+		 * and enable interrupts only for the
+		 * current TC, using the TCStatus register.
+		 */
+		ehb
+		mfc0	t0,CP0_TCSTATUS
+		/* Fortunately CU 0 is in the same place in both registers */
+		/* Set TCU0, TKSU (for later inversion) and IXMT */
+		li	t1, ST0_CU0 | 0x08001c00
+		or	t0,t1
+		/* Clear TKSU *and* IXMT */
+		xori	t0, 0x00001c00
+		mtc0	t0, CP0_TCSTATUS
+		ehb
+		/* We need to leave the global IE bit set, but clear EXL...*/
+		mfc0	t0, CP0_STATUS
+		ori	t0, ST0_EXL
+		xori	t0, ST0_EXL
+		mtc0	t0, CP0_STATUS
+		/* irq_enable_hazard below should expand to EHB for 24K/34K cpus */
+#endif /* CONFIG_MIPS_MT_SMTC */
 		irq_enable_hazard
 		.endm
 
@@ -328,11 +464,56 @@
  * Set cp0 enable bit as sign that we're running on the kernel stack
  */
 		.macro	KMODE
+#ifdef CONFIG_MIPS_MT_SMTC
+		/*
+		 * This gets baroque in SMTC.  We want to
+		 * protect the non-atomic clearing of EXL
+		 * with DMT/EMT, but we don't want to take
+		 * an interrupt while DMT is still in effect.
+		 */
+
+		/* KMODE gets invoked from both reorder and noreorder code */
+		.set	push
+		.set	mips32r2
+		.set	noreorder
+		mfc0	v0, CP0_TCSTATUS
+		andi	v1, v0, TCSTATUS_IXMT
+		ori	v0, TCSTATUS_IXMT
+		mtc0	v0, CP0_TCSTATUS
+		ehb
+		DMT	2				# dmt	v0
+		/*
+		 * We don't know a priori if ra is "live"
+		 */
+		move	t0, ra
+		jal	mips_ihb
+		nop	/* delay slot */
+		move	ra, t0
+#endif /* CONFIG_MIPS_MT_SMTC */
 		mfc0	t0, CP0_STATUS
 		li	t1, ST0_CU0 | 0x1e
 		or	t0, t1
 		xori	t0, 0x1e
 		mtc0	t0, CP0_STATUS
+#ifdef CONFIG_MIPS_MT_SMTC
+		ehb
+		andi	v0, v0, VPECONTROL_TE
+		beqz	v0, 2f
+		nop	/* delay slot */
+		emt
+2:
+		mfc0	v0, CP0_TCSTATUS
+		/* Clear IXMT, then OR in previous value */
+		ori	v0, TCSTATUS_IXMT
+		xori	v0, TCSTATUS_IXMT
+		or	v0, v1, v0
+		mtc0	v0, CP0_TCSTATUS
+		/*
+		 * irq_disable_hazard below should expand to EHB
+		 * on 24K/34K CPUS
+		 */
+		.set pop
+#endif /* CONFIG_MIPS_MT_SMTC */
 		irq_disable_hazard
 		.endm