4 files changed, 33 insertions, 32 deletions
diff --git a/arch/arm/mach-iop32x/cp6.c b/arch/arm/mach-iop32x/cp6.c
index ec74b07fb7e3..2882674a1c39 100644
--- a/arch/arm/mach-iop32x/cp6.c
+++ b/arch/arm/mach-iop32x/cp6.c
@@ -7,7 +7,7 @@
 #include <asm/traps.h>
 #include <asm/ptrace.h>
 
-static int cp6_trap(struct pt_regs *regs, unsigned int instr)
+void iop_enable_cp6(void)
 {
 	u32 temp;
 
@@ -16,7 +16,15 @@ static int cp6_trap(struct pt_regs *regs, unsigned int instr)
 		"mrc	p15, 0, %0, c15, c1, 0\n\t"
 		"orr	%0, %0, #(1 << 6)\n\t"
 		"mcr	p15, 0, %0, c15, c1, 0\n\t"
+		"mrc	p15, 0, %0, c15, c1, 0\n\t"
+		"mov	%0, %0\n\t"
+		"sub	pc, pc, #4  @ cp_wait\n\t"
 		: "=r"(temp));
+}
+
+static int cp6_trap(struct pt_regs *regs, unsigned int instr)
+{
+	iop_enable_cp6();
 
 	return 0;
 }
diff --git a/arch/arm/mach-iop32x/include/mach/entry-macro.S b/arch/arm/mach-iop32x/include/mach/entry-macro.S
deleted file mode 100644
index 341e5d9a6616..000000000000
--- a/arch/arm/mach-iop32x/include/mach/entry-macro.S
+++ /dev/null
@@ -1,31 +0,0 @@
-/*
- * arch/arm/mach-iop32x/include/mach/entry-macro.S
- *
- * Low-level IRQ helper macros for IOP32x-based platforms
- *
- * This file is licensed under the terms of the GNU General Public
- * License version 2. This program is licensed "as is" without any
- * warranty of any kind, whether express or implied.
- */
-	.macro get_irqnr_preamble, base, tmp
-	mrc	p15, 0, \tmp, c15, c1, 0
-	orr	\tmp, \tmp, #(1 << 6)
-	mcr	p15, 0, \tmp, c15, c1, 0	@ Enable cp6 access
-	mrc	p15, 0, \tmp, c15, c1, 0
-	mov	\tmp, \tmp
-	sub	pc, pc, #4			@ cp_wait
-	.endm
-
-	.macro  get_irqnr_and_base, irqnr, irqstat, base, tmp
-	mrc     p6, 0, \irqstat, c8, c0, 0	@ Read IINTSRC
-	cmp     \irqstat, #0
-	clzne   \irqnr, \irqstat
-	rsbne   \irqnr, \irqnr, #32
-	.endm
-
-	.macro arch_ret_to_user, tmp1, tmp2
-	mrc	p15, 0, \tmp1, c15, c1, 0
-	ands	\tmp2, \tmp1, #(1 << 6)
-	bicne	\tmp1, \tmp1, #(1 << 6)
-	mcrne	p15, 0, \tmp1, c15, c1, 0	@ Disable cp6 access
-	.endm
diff --git a/arch/arm/mach-iop32x/iop3xx.h b/arch/arm/mach-iop32x/iop3xx.h
index 46b4b34a4ad2..a6ec7ebadb35 100644
--- a/arch/arm/mach-iop32x/iop3xx.h
+++ b/arch/arm/mach-iop32x/iop3xx.h
@@ -225,6 +225,7 @@ extern int iop3xx_get_init_atu(void);
 #include <linux/reboot.h>
 
 void iop3xx_map_io(void);
+void iop_enable_cp6(void);
 void iop_init_cp6_handler(void);
 void iop_init_time(unsigned long tickrate);
 void iop3xx_restart(enum reboot_mode, const char *);
diff --git a/arch/arm/mach-iop32x/irq.c b/arch/arm/mach-iop32x/irq.c
index d1e8824cbd82..b820839eaae8 100644
--- a/arch/arm/mach-iop32x/irq.c
+++ b/arch/arm/mach-iop32x/irq.c
@@ -29,6 +29,15 @@ static void intstr_write(u32 val)
 	asm volatile("mcr p6, 0, %0, c4, c0, 0" : : "r" (val));
 }
 
+static u32 iintsrc_read(void)
+{
+	int irq;
+
+	asm volatile("mrc p6, 0, %0, c8, c0, 0" : "=r" (irq));
+
+	return irq;
+}
+
 static void
 iop32x_irq_mask(struct irq_data *d)
 {
@@ -50,11 +59,25 @@ struct irq_chip ext_chip = {
 	.irq_unmask	= iop32x_irq_unmask,
 };
 
+void iop_handle_irq(struct pt_regs *regs)
+{
+	u32 mask;
+
+	iop_enable_cp6();
+
+	do {
+		mask = iintsrc_read();
+		if (mask)
+			generic_handle_irq(fls(mask));
+	} while (mask);
+}
+
 void __init iop32x_init_irq(void)
 {
 	int i;
 
 	iop_init_cp6_handler();
+	set_handle_irq(iop_handle_irq);
 
 	intctl_write(0);
 	intstr_write(0);