1 files changed, 24 insertions, 56 deletions

diff --git a/arch/arm/kernel/fiq.c b/arch/arm/kernel/fiq.c
index e72dc34eea1..918875d96d5 100644
--- a/arch/arm/kernel/fiq.c
+++ b/arch/arm/kernel/fiq.c
@@ -42,11 +42,16 @@
 #include <linux/seq_file.h>
 
 #include <asm/cacheflush.h>
+#include <asm/cp15.h>
 #include <asm/fiq.h>
 #include <asm/irq.h>
-#include <asm/system.h>
 #include <asm/traps.h>
 
+#define FIQ_OFFSET ({					\
+		extern void *vector_fiq_offset;		\
+		(unsigned)&vector_fiq_offset;		\
+	})
+
 static unsigned long no_fiq_insn;
 
 /* Default reacquire function
@@ -79,55 +84,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);
-}
-
-/*
- * Taking an interrupt in FIQ mode is death, so both these functions
- * disable irqs for the duration.  Note - these functions are almost
- * entirely coded in assembly.
- */
-void __naked set_fiq_regs(struct pt_regs *regs)
-{
-	register unsigned long tmp;
-	asm volatile (
-	"mov	ip, sp\n\
-	stmfd	sp!, {fp, ip, lr, pc}\n\
-	sub	fp, ip, #4\n\
-	mrs	%0, cpsr\n\
-	msr	cpsr_c, %2	@ select FIQ mode\n\
-	mov	r0, r0\n\
-	ldmia	%1, {r8 - r14}\n\
-	msr	cpsr_c, %0	@ return to SVC mode\n\
-	mov	r0, r0\n\
-	ldmfd	sp, {fp, sp, pc}"
-	: "=&r" (tmp)
-	: "r" (&regs->ARM_r8), "I" (PSR_I_BIT | PSR_F_BIT | FIQ_MODE));
-}
-
-void __naked get_fiq_regs(struct pt_regs *regs)
-{
-	register unsigned long tmp;
-	asm volatile (
-	"mov	ip, sp\n\
-	stmfd	sp!, {fp, ip, lr, pc}\n\
-	sub	fp, ip, #4\n\
-	mrs	%0, cpsr\n\
-	msr	cpsr_c, %2	@ select FIQ mode\n\
-	mov	r0, r0\n\
-	stmia	%1, {r8 - r14}\n\
-	msr	cpsr_c, %0	@ return to SVC mode\n\
-	mov	r0, r0\n\
-	ldmfd	sp, {fp, sp, pc}"
-	: "=&r" (tmp)
-	: "r" (&regs->ARM_r8), "I" (PSR_I_BIT | PSR_F_BIT | FIQ_MODE));
+	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, offset +
+				   length);
+	flush_icache_range(0xffff0000 + offset, 0xffff0000 + offset + length);
 }
 
 int claim_fiq(struct fiq_handler *f)
@@ -163,25 +127,29 @@ void release_fiq(struct fiq_handler *f)
 	while (current_fiq->fiq_op(current_fiq->dev_id, 0));
 }
 
+static int fiq_start;
+
 void enable_fiq(int fiq)
 {
-	enable_irq(fiq + FIQ_START);
+	enable_irq(fiq + fiq_start);
 }
 
 void disable_fiq(int fiq)
 {
-	disable_irq(fiq + FIQ_START);
+	disable_irq(fiq + fiq_start);
 }
 
 EXPORT_SYMBOL(set_fiq_handler);
-EXPORT_SYMBOL(set_fiq_regs);
-EXPORT_SYMBOL(get_fiq_regs);
+EXPORT_SYMBOL(__set_fiq_regs);	/* defined in fiqasm.S */
+EXPORT_SYMBOL(__get_fiq_regs);	/* defined in fiqasm.S */
 EXPORT_SYMBOL(claim_fiq);
 EXPORT_SYMBOL(release_fiq);
 EXPORT_SYMBOL(enable_fiq);
 EXPORT_SYMBOL(disable_fiq);
 
-void __init init_FIQ(void)
+void __init init_FIQ(int start)
 {
-	no_fiq_insn = *(unsigned long *)0xffff001c;
+	unsigned offset = FIQ_OFFSET;
+	no_fiq_insn = *(unsigned long *)(0xffff0000 + offset);
+	fiq_start = start;
 }