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

[firefly-linux-kernel-4.4.55.git] / arch / arm / kernel / fiq.c

diff --git a/arch/arm/kernel/fiq.c b/arch/arm/kernel/fiq.c
index 918875d96d5dc598985c7dce050e0a1785637b49..b37752a966521b64faa3dd9fb612d5e7b3e1a5b1 100644 (file)
--- 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;
 }