diff options
Diffstat (limited to 'arch/arm/kernel/fiq.c')
-rw-r--r-- | arch/arm/kernel/fiq.c | 17 |
1 files changed, 13 insertions, 4 deletions
diff --git a/arch/arm/kernel/fiq.c b/arch/arm/kernel/fiq.c index 918875d..b37752a 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; } |