Merge branches 'for-4.11/upstream-fixes', 'for-4.12/accutouch', 'for-4.12/cp2112...

[mirror_ubuntu-artful-kernel.git] / arch / mips / kernel / genex.S

diff --git a/arch/mips/kernel/genex.S b/arch/mips/kernel/genex.S
index dc0b2961289136f6a17755106bb93a9c99919f98..7ec9612cb0078a4c6ca9825f2e5f2a80e05d3508 100644 (file)
--- a/arch/mips/kernel/genex.S
+++ b/arch/mips/kernel/genex.S
@@ -187,9 +187,44 @@ NESTED(handle_int, PT_SIZE, sp)
 
        LONG_L  s0, TI_REGS($28)
        LONG_S  sp, TI_REGS($28)
-       PTR_LA  ra, ret_from_irq
-       PTR_LA  v0, plat_irq_dispatch
-       jr      v0
+
+       /*
+        * SAVE_ALL ensures we are using a valid kernel stack for the thread.
+        * Check if we are already using the IRQ stack.
+        */
+       move    s1, sp # Preserve the sp
+
+       /* Get IRQ stack for this CPU */
+       ASM_CPUID_MFC0  k0, ASM_SMP_CPUID_REG
+#if defined(CONFIG_32BIT) || defined(KBUILD_64BIT_SYM32)
+       lui     k1, %hi(irq_stack)
+#else
+       lui     k1, %highest(irq_stack)
+       daddiu  k1, %higher(irq_stack)
+       dsll    k1, 16
+       daddiu  k1, %hi(irq_stack)
+       dsll    k1, 16
+#endif
+       LONG_SRL        k0, SMP_CPUID_PTRSHIFT
+       LONG_ADDU       k1, k0
+       LONG_L  t0, %lo(irq_stack)(k1)
+
+       # Check if already on IRQ stack
+       PTR_LI  t1, ~(_THREAD_SIZE-1)
+       and     t1, t1, sp
+       beq     t0, t1, 2f
+
+       /* Switch to IRQ stack */
+       li      t1, _IRQ_STACK_SIZE
+       PTR_ADD sp, t0, t1
+
+2:
+       jal     plat_irq_dispatch
+
+       /* Restore sp */
+       move    sp, s1
+
+       j       ret_from_irq
 #ifdef CONFIG_CPU_MICROMIPS
        nop
 #endif
@@ -262,8 +297,44 @@ NESTED(except_vec_vi_handler, 0, sp)
 
        LONG_L  s0, TI_REGS($28)
        LONG_S  sp, TI_REGS($28)
-       PTR_LA  ra, ret_from_irq
-       jr      v0
+
+       /*
+        * SAVE_ALL ensures we are using a valid kernel stack for the thread.
+        * Check if we are already using the IRQ stack.
+        */
+       move    s1, sp # Preserve the sp
+
+       /* Get IRQ stack for this CPU */
+       ASM_CPUID_MFC0  k0, ASM_SMP_CPUID_REG
+#if defined(CONFIG_32BIT) || defined(KBUILD_64BIT_SYM32)
+       lui     k1, %hi(irq_stack)
+#else
+       lui     k1, %highest(irq_stack)
+       daddiu  k1, %higher(irq_stack)
+       dsll    k1, 16
+       daddiu  k1, %hi(irq_stack)
+       dsll    k1, 16
+#endif
+       LONG_SRL        k0, SMP_CPUID_PTRSHIFT
+       LONG_ADDU       k1, k0
+       LONG_L  t0, %lo(irq_stack)(k1)
+
+       # Check if already on IRQ stack
+       PTR_LI  t1, ~(_THREAD_SIZE-1)
+       and     t1, t1, sp
+       beq     t0, t1, 2f
+
+       /* Switch to IRQ stack */
+       li      t1, _IRQ_STACK_SIZE
+       PTR_ADD sp, t0, t1
+
+2:
+       jalr    v0
+
+       /* Restore sp */
+       move    sp, s1
+
+       j       ret_from_irq
        END(except_vec_vi_handler)
 
 /*