[MIPS] MT: Improved multithreading support.

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

1 files changed, 184 insertions, 3 deletions

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