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 */