diff options
Diffstat (limited to 'arch/arm/kernel/fiq.c')
| -rw-r--r-- | arch/arm/kernel/fiq.c | 43 |
1 files changed, 30 insertions, 13 deletions
diff --git a/arch/arm/kernel/fiq.c b/arch/arm/kernel/fiq.c index 2adda11f712f..d2c8e5313539 100644 --- a/arch/arm/kernel/fiq.c +++ b/arch/arm/kernel/fiq.c @@ -1,3 +1,4 @@ +// SPDX-License-Identifier: GPL-2.0 /* * linux/arch/arm/kernel/fiq.c * @@ -44,10 +45,17 @@ #include <asm/cacheflush.h> #include <asm/cp15.h> #include <asm/fiq.h> +#include <asm/mach/irq.h> #include <asm/irq.h> #include <asm/traps.h> -static unsigned long no_fiq_insn; +#define FIQ_OFFSET ({ \ + extern void *vector_fiq_offset; \ + (unsigned)&vector_fiq_offset; \ + }) + +static unsigned long dfl_fiq_insn; +static struct pt_regs dfl_fiq_regs; /* Default reacquire function * - we always relinquish FIQ control @@ -55,8 +63,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; } @@ -79,14 +94,14 @@ int show_fiq_list(struct seq_file *p, int prec) void set_fiq_handler(void *start, unsigned int length) { -#if defined(CONFIG_CPU_USE_DOMAINS) - memcpy((void *)0xffff001c, start, length); -#else - memcpy(vectors_page + 0x1c, start, length); -#endif - flush_icache_range(0xffff001c, 0xffff001c + length); - if (!vectors_high()) - flush_icache_range(0x1c, 0x1c + length); + void *base = vectors_page; + unsigned offset = FIQ_OFFSET; + + memcpy(base + offset, start, length); + if (!cache_is_vipt_nonaliasing()) + flush_icache_range((unsigned long)base + offset, + (unsigned long)base + offset + length); + flush_icache_range(0xffff0000 + offset, 0xffff0000 + offset + length); } int claim_fiq(struct fiq_handler *f) @@ -111,7 +126,7 @@ int claim_fiq(struct fiq_handler *f) void release_fiq(struct fiq_handler *f) { if (current_fiq != f) { - printk(KERN_ERR "%s FIQ trying to release %s FIQ\n", + pr_err("%s FIQ trying to release %s FIQ\n", f->name, current_fiq->name); dump_stack(); return; @@ -144,6 +159,8 @@ EXPORT_SYMBOL(disable_fiq); void __init init_FIQ(int start) { - no_fiq_insn = *(unsigned long *)0xffff001c; + unsigned offset = FIQ_OFFSET; + dfl_fiq_insn = *(unsigned long *)(0xffff0000 + offset); + get_fiq_regs(&dfl_fiq_regs); fiq_start = start; } |