[MIPS] MT: Improved multithreading support.

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

1 files changed, 57 insertions, 0 deletions

diff --git a/arch/mips/kernel/head.S b/arch/mips/kernel/head.S
index 2e9122a4213a..bdf6f6eff721 100644
--- a/arch/mips/kernel/head.S
+++ b/arch/mips/kernel/head.S
@@ -18,6 +18,7 @@
 #include <linux/threads.h>
 
 #include <asm/asm.h>
+#include <asm/asmmacro.h>
 #include <asm/regdef.h>
 #include <asm/page.h>
 #include <asm/mipsregs.h>
@@ -82,12 +83,33 @@
 	 */
 	.macro	setup_c0_status set clr
 	.set	push
+#ifdef 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
+	or	t0, ST0_CU0 | ST0_EXL | ST0_ERL | \set | \clr
+	xor	t0, ST0_EXL | ST0_ERL | \clr
+	mtc0	t0, CP0_STATUS
+#else
 	mfc0	t0, CP0_STATUS
 	or	t0, ST0_CU0|\set|0x1f|\clr
 	xor	t0, 0x1f|\clr
 	mtc0	t0, CP0_STATUS
 	.set	noreorder
 	sll	zero,3				# ehb
+#endif
 	.set	pop
 	.endm
 
@@ -134,6 +156,24 @@ NESTED(kernel_entry, 16, sp)			# kernel entry point
 
 	ARC64_TWIDDLE_PC
 
+#ifdef CONFIG_MIPS_MT_SMTC
+	/*
+	 * In SMTC kernel, "CLI" is thread-specific, in TCStatus.
+	 * We still need to enable interrupts globally in Status,
+	 * and clear EXL/ERL.
+	 *
+	 * TCContext is used to track interrupt levels under
+	 * service in SMTC kernel. Clear for boot TC before
+	 * allowing any interrupts.
+	 */
+	mtc0	zero, CP0_TCCONTEXT
+
+	mfc0	t0, CP0_STATUS
+	ori	t0, t0, 0xff1f
+	xori	t0, t0, 0x001e
+	mtc0	t0, CP0_STATUS
+#endif /* CONFIG_MIPS_MT_SMTC */
+
 	PTR_LA		t0, __bss_start		# clear .bss
 	LONG_S		zero, (t0)
 	PTR_LA		t1, __bss_stop - LONGSIZE
@@ -166,8 +206,25 @@ NESTED(kernel_entry, 16, sp)			# kernel entry point
  * function after setting up the stack and gp registers.
  */
 NESTED(smp_bootstrap, 16, sp)
+#ifdef CONFIG_MIPS_MT_SMTC
+	/*
+	 * Read-modify-writes of Status must be atomic, and this
+	 * is one case where CLI is invoked without EXL being
+	 * necessarily set. The CLI and setup_c0_status will
+	 * in fact be redundant for all but the first TC of
+	 * each VPE being booted.
+	 */
+	DMT	10	# dmt t2 /* t0, t1 are used by CLI and setup_c0_status() */
+	jal	mips_ihb
+#endif /* CONFIG_MIPS_MT_SMTC */
 	setup_c0_status_sec
 	smp_slave_setup
+#ifdef CONFIG_MIPS_MT_SMTC
+	andi	t2, t2, VPECONTROL_TE
+	beqz	t2, 2f
+	EMT		# emt
+2:
+#endif /* CONFIG_MIPS_MT_SMTC */
 	j	start_secondary
 	END(smp_bootstrap)
 #endif /* CONFIG_SMP */