ARM: 8150/3: fiq: Replace default FIQ handler

This patch introduces a new default FIQ handler that is structured in a
similar way to the existing ARM exception handler and result in the FIQ
being handled by C code running on the SVC stack (despite this code run
in the FIQ handler is subject to severe limitations with respect to
locking making normal interaction with the kernel impossible).

This default handler allows concepts that on x86 would be handled using
NMIs to be realized on ARM.

Credit:

    This patch is a near complete re-write of a patch originally
    provided by Anton Vorontsov. Today only a couple of small fragments
    survive, however without Anton's work to build from this patch would
    not exist. Thanks also to Russell King for spoonfeeding me a variety
    of fixes during the review cycle.

Signed-off-by: Daniel Thompson <daniel.thompson@linaro.org>
Cc: Catalin Marinas <catalin.marinas@arm.com>
Acked-by: Nicolas Pitre <nico@linaro.org>
Signed-off-by: Russell King <rmk+kernel@arm.linux.org.uk>

5 files changed, 180 insertions, 16 deletions

diff --git a/arch/arm/kernel/entry-armv.S b/arch/arm/kernel/entry-armv.S
index 36276cdccfbc..859f56cb122b 100644
--- a/arch/arm/kernel/entry-armv.S
+++ b/arch/arm/kernel/entry-armv.S
@@ -146,7 +146,7 @@ ENDPROC(__und_invalid)
 #define SPFIX(code...)
 #endif
 
-        .macro  svc_entry, stack_hole=0
+        .macro  svc_entry, stack_hole=0, trace=1
  UNWIND(.fnstart                )
  UNWIND(.save {r0 - pc}         )
         sub     sp, sp, #(S_FRAME_SIZE + \stack_hole - 4)
@@ -182,9 +182,11 @@ ENDPROC(__und_invalid)
         @
         stmia   r7, {r2 - r6}
 
+        .if \trace
 #ifdef CONFIG_TRACE_IRQFLAGS
         bl      trace_hardirqs_off
 #endif
+        .endif
         .endm
 
         .align  5
@@ -295,6 +297,15 @@ __pabt_svc:
 ENDPROC(__pabt_svc)
 
         .align  5
+__fiq_svc:
+        svc_entry trace=0
+        mov     r0, sp                          @ struct pt_regs *regs
+        bl      handle_fiq_as_nmi
+        svc_exit_via_fiq
+ UNWIND(.fnend          )
+ENDPROC(__fiq_svc)
+
+        .align  5
 .LCcralign:
         .word   cr_alignment
 #ifdef MULTI_DABORT
@@ -305,6 +316,46 @@ ENDPROC(__pabt_svc)
         .word   fp_enter
 
 /*
+ * Abort mode handlers
+ */
+
+@
+@ Taking a FIQ in abort mode is similar to taking a FIQ in SVC mode
+@ and reuses the same macros. However in abort mode we must also
+@ save/restore lr_abt and spsr_abt to make nested aborts safe.
+@
+        .align 5
+__fiq_abt:
+        svc_entry trace=0
+
+ ARM(   msr     cpsr_c, #ABT_MODE | PSR_I_BIT | PSR_F_BIT )
+ THUMB( mov     r0, #ABT_MODE | PSR_I_BIT | PSR_F_BIT )
+ THUMB( msr     cpsr_c, r0 )
+        mov     r1, lr          @ Save lr_abt
+        mrs     r2, spsr        @ Save spsr_abt, abort is now safe
+ ARM(   msr     cpsr_c, #SVC_MODE | PSR_I_BIT | PSR_F_BIT )
+ THUMB( mov     r0, #SVC_MODE | PSR_I_BIT | PSR_F_BIT )
+ THUMB( msr     cpsr_c, r0 )
+        stmfd   sp!, {r1 - r2}
+
+        add     r0, sp, #8                      @ struct pt_regs *regs
+        bl      handle_fiq_as_nmi
+
+        ldmfd   sp!, {r1 - r2}
+ ARM(   msr     cpsr_c, #ABT_MODE | PSR_I_BIT | PSR_F_BIT )
+ THUMB( mov     r0, #ABT_MODE | PSR_I_BIT | PSR_F_BIT )
+ THUMB( msr     cpsr_c, r0 )
+        mov     lr, r1          @ Restore lr_abt, abort is unsafe
+        msr     spsr_cxsf, r2   @ Restore spsr_abt
+ ARM(   msr     cpsr_c, #SVC_MODE | PSR_I_BIT | PSR_F_BIT )
+ THUMB( mov     r0, #SVC_MODE | PSR_I_BIT | PSR_F_BIT )
+ THUMB( msr     cpsr_c, r0 )
+
+        svc_exit_via_fiq
+ UNWIND(.fnend          )
+ENDPROC(__fiq_abt)
+
+/*
  * User mode handlers
  *
  * EABI note: sp_svc is always 64-bit aligned here, so should S_FRAME_SIZE
@@ -314,7 +365,7 @@ ENDPROC(__pabt_svc)
 #error "sizeof(struct pt_regs) must be a multiple of 8"
 #endif
 
-        .macro  usr_entry
+        .macro  usr_entry, trace=1
  UNWIND(.fnstart        )
  UNWIND(.cantunwind     )       @ don't unwind the user space
         sub     sp, sp, #S_FRAME_SIZE
@@ -351,10 +402,12 @@ ENDPROC(__pabt_svc)
         @
         zero_fp
 
+        .if     \trace
 #ifdef CONFIG_IRQSOFF_TRACER
         bl      trace_hardirqs_off
 #endif
         ct_user_exit save = 0
+        .endif
         .endm
 
         .macro  kuser_cmpxchg_check
@@ -683,6 +736,17 @@ ENTRY(ret_from_exception)
 ENDPROC(__pabt_usr)
 ENDPROC(ret_from_exception)
 
+        .align  5
+__fiq_usr:
+        usr_entry trace=0
+        kuser_cmpxchg_check
+        mov     r0, sp                          @ struct pt_regs *regs
+        bl      handle_fiq_as_nmi
+        get_thread_info tsk
+        restore_user_regs fast = 0, offset = 0
+ UNWIND(.fnend          )
+ENDPROC(__fiq_usr)
+
 /*
  * Register switch for ARMv3 and ARMv4 processors
  * r0 = previous task_struct, r1 = previous thread_info, r2 = next thread_info
@@ -1118,17 +1182,29 @@ vector_addrexcptn:
         b       vector_addrexcptn
 
 /*=============================================================================
- * Undefined FIQs
+ * FIQ "NMI" handler
  *-----------------------------------------------------------------------------
- * Enter in FIQ mode, spsr = ANY CPSR, lr = ANY PC
- * MUST PRESERVE SVC SPSR, but need to switch to SVC mode to show our msg.
- * Basically to switch modes, we *HAVE* to clobber one register...  brain
- * damage alert!  I don't think that we can execute any code in here in any
- * other mode than FIQ...  Ok you can switch to another mode, but you can't
- * get out of that mode without clobbering one register.
+ * Handle a FIQ using the SVC stack allowing FIQ act like NMI on x86
+ * systems.
  */
-vector_fiq:
-        subs    pc, lr, #4
+        vector_stub     fiq, FIQ_MODE, 4
+
+        .long   __fiq_usr                       @  0  (USR_26 / USR_32)
+        .long   __fiq_svc                       @  1  (FIQ_26 / FIQ_32)
+        .long   __fiq_svc                       @  2  (IRQ_26 / IRQ_32)
+        .long   __fiq_svc                       @  3  (SVC_26 / SVC_32)
+        .long   __fiq_svc                       @  4
+        .long   __fiq_svc                       @  5
+        .long   __fiq_svc                       @  6
+        .long   __fiq_abt                       @  7
+        .long   __fiq_svc                       @  8
+        .long   __fiq_svc                       @  9
+        .long   __fiq_svc                       @  a
+        .long   __fiq_svc                       @  b
+        .long   __fiq_svc                       @  c
+        .long   __fiq_svc                       @  d
+        .long   __fiq_svc                       @  e
+        .long   __fiq_svc                       @  f
 
         .globl  vector_fiq_offset
         .equ    vector_fiq_offset, vector_fiq
diff --git a/arch/arm/kernel/entry-header.S b/arch/arm/kernel/entry-header.S
index 2fdf8679b46e..0d91ca05d55b 100644
--- a/arch/arm/kernel/entry-header.S
+++ b/arch/arm/kernel/entry-header.S
@@ -216,6 +216,34 @@
         ldmia   sp, {r0 - pc}^                  @ load r0 - pc, cpsr
         .endm
 
+        @
+        @ svc_exit_via_fiq - like svc_exit but switches to FIQ mode before exit
+        @
+        @ This macro acts in a similar manner to svc_exit but switches to FIQ
+        @ mode to restore the final part of the register state.
+        @
+        @ We cannot use the normal svc_exit procedure because that would
+        @ clobber spsr_svc (FIQ could be delivered during the first few
+        @ instructions of vector_swi meaning its contents have not been
+        @ saved anywhere).
+        @
+        @ Note that, unlike svc_exit, this macro also does not allow a caller
+        @ supplied rpsr. This is because the FIQ exceptions are not re-entrant
+        @ and the handlers cannot call into the scheduler (meaning the value
+        @ on the stack remains correct).
+        @
+        .macro  svc_exit_via_fiq
+        mov     r0, sp
+        ldmib   r0, {r1 - r14}  @ abort is deadly from here onward (it will
+                                @ clobber state restored below)
+        msr     cpsr_c, #FIQ_MODE | PSR_I_BIT | PSR_F_BIT
+        add     r8, r0, #S_PC
+        ldr     r9, [r0, #S_PSR]
+        msr     spsr_cxsf, r9
+        ldr     r0, [r0, #S_R0]
+        ldmia   r8, {pc}^
+        .endm
+
         .macro  restore_user_regs, fast = 0, offset = 0
         ldr     r1, [sp, #\offset + S_PSR]      @ get calling cpsr
         ldr     lr, [sp, #\offset + S_PC]!      @ get pc
@@ -267,6 +295,25 @@
         rfeia   sp!
         .endm
 
+        @
+        @ svc_exit_via_fiq - like svc_exit but switches to FIQ mode before exit
+        @
+        @ For full details see non-Thumb implementation above.
+        @
+        .macro  svc_exit_via_fiq
+        add     r0, sp, #S_R2
+        ldr     lr, [sp, #S_LR]
+        ldr     sp, [sp, #S_SP] @ abort is deadly from here onward (it will
+                                @ clobber state restored below)
+        ldmia   r0, {r2 - r12}
+        mov     r1, #FIQ_MODE | PSR_I_BIT | PSR_F_BIT
+        msr     cpsr_c, r1
+        sub     r0, #S_R2
+        add     r8, r0, #S_PC
+        ldmia   r0, {r0 - r1}
+        rfeia   r8
+        .endm
+
 #ifdef CONFIG_CPU_V7M
         /*
          * Note we don't need to do clrex here as clearing the local monitor is
diff --git a/arch/arm/kernel/fiq.c b/arch/arm/kernel/fiq.c
index 918875d96d5d..b37752a96652 100644
--- a/arch/arm/kernel/fiq.c
+++ b/arch/arm/kernel/fiq.c
@@ -52,7 +52,8 @@
                 (unsigned)&vector_fiq_offset;           \
         })
 
-static unsigned long no_fiq_insn;
+static unsigned long dfl_fiq_insn;
+static struct pt_regs dfl_fiq_regs;
 
 /* Default reacquire function
  * - we always relinquish FIQ control
@@ -60,8 +61,15 @@ static unsigned long no_fiq_insn;
  */
 static int fiq_def_op(void *ref, int relinquish)
 {
-        if (!relinquish)
-                set_fiq_handler(&no_fiq_insn, sizeof(no_fiq_insn));
+        if (!relinquish) {
+                /* Restore default handler and registers */
+                local_fiq_disable();
+                set_fiq_regs(&dfl_fiq_regs);
+                set_fiq_handler(&dfl_fiq_insn, sizeof(dfl_fiq_insn));
+                local_fiq_enable();
+
+                /* FIXME: notify irq controller to standard enable FIQs */
+        }
 
         return 0;
 }
@@ -150,6 +158,7 @@ EXPORT_SYMBOL(disable_fiq);
 void __init init_FIQ(int start)
 {
         unsigned offset = FIQ_OFFSET;
-        no_fiq_insn = *(unsigned long *)(0xffff0000 + offset);
+        dfl_fiq_insn = *(unsigned long *)(0xffff0000 + offset);
+        get_fiq_regs(&dfl_fiq_regs);
         fiq_start = start;
 }
diff --git a/arch/arm/kernel/setup.c b/arch/arm/kernel/setup.c
index 84db893dedc2..c03106378b49 100644
--- a/arch/arm/kernel/setup.c
+++ b/arch/arm/kernel/setup.c
@@ -133,6 +133,7 @@ struct stack {
         u32 irq[3];
         u32 abt[3];
         u32 und[3];
+        u32 fiq[3];
 } ____cacheline_aligned;
 
 #ifndef CONFIG_CPU_V7M
@@ -470,7 +471,10 @@ void notrace cpu_init(void)
         "msr    cpsr_c, %5\n\t"
         "add    r14, %0, %6\n\t"
         "mov    sp, r14\n\t"
-        "msr    cpsr_c, %7"
+        "msr    cpsr_c, %7\n\t"
+        "add    r14, %0, %8\n\t"
+        "mov    sp, r14\n\t"
+        "msr    cpsr_c, %9"
             :
             : "r" (stk),
               PLC (PSR_F_BIT | PSR_I_BIT | IRQ_MODE),
@@ -479,6 +483,8 @@ void notrace cpu_init(void)
               "I" (offsetof(struct stack, abt[0])),
               PLC (PSR_F_BIT | PSR_I_BIT | UND_MODE),
               "I" (offsetof(struct stack, und[0])),
+              PLC (PSR_F_BIT | PSR_I_BIT | FIQ_MODE),
+              "I" (offsetof(struct stack, fiq[0])),
               PLC (PSR_F_BIT | PSR_I_BIT | SVC_MODE)
             : "r14");
 #endif
diff --git a/arch/arm/kernel/traps.c b/arch/arm/kernel/traps.c
index c8e4bb714944..58348631671d 100644
--- a/arch/arm/kernel/traps.c
+++ b/arch/arm/kernel/traps.c
@@ -25,6 +25,7 @@
 #include <linux/delay.h>
 #include <linux/init.h>
 #include <linux/sched.h>
+#include <linux/irq.h>
 
 #include <linux/atomic.h>
 #include <asm/cacheflush.h>
@@ -467,6 +468,31 @@ asmlinkage void do_unexp_fiq (struct pt_regs *regs)
 }
 
 /*
+ * Handle FIQ similarly to NMI on x86 systems.
+ *
+ * The runtime environment for NMIs is extremely restrictive
+ * (NMIs can pre-empt critical sections meaning almost all locking is
+ * forbidden) meaning this default FIQ handling must only be used in
+ * circumstances where non-maskability improves robustness, such as
+ * watchdog or debug logic.
+ *
+ * This handler is not appropriate for general purpose use in drivers
+ * platform code and can be overrideen using set_fiq_handler.
+ */
+asmlinkage void __exception_irq_entry handle_fiq_as_nmi(struct pt_regs *regs)
+{
+        struct pt_regs *old_regs = set_irq_regs(regs);
+
+        nmi_enter();
+
+        /* nop. FIQ handlers for special arch/arm features can be added here. */
+
+        nmi_exit();
+
+        set_irq_regs(old_regs);
+}
+
+/*
  * bad_mode handles the impossible case in the vectors.  If you see one of
  * these, then it's extremely serious, and could mean you have buggy hardware.
  * It never returns, and never tries to sync.  We hope that we can at least