diff --git a/arch/arm/Kconfig b/arch/arm/Kconfig
index a0cc9ca66ae0..d9ba6961b295 100644
--- a/arch/arm/Kconfig
+++ b/arch/arm/Kconfig
@@ -227,9 +227,6 @@ config GENERIC_ISA_DMA
 config FIQ
 	bool
 
-config NEED_RET_TO_USER
-	bool
-
 config ARCH_MTD_XIP
 	bool
 
@@ -371,9 +368,9 @@ config ARCH_IOP32X
 	bool "IOP32x-based"
 	depends on MMU
 	select CPU_XSCALE
+	select GENERIC_IRQ_MULTI_HANDLER
 	select GPIO_IOP
 	select GPIOLIB
-	select NEED_RET_TO_USER
 	select FORCE_PCI
 	select PLAT_IOP
 	help
diff --git a/arch/arm/kernel/entry-common.S b/arch/arm/kernel/entry-common.S
index ac86c34682bb..c928d6b04cce 100644
--- a/arch/arm/kernel/entry-common.S
+++ b/arch/arm/kernel/entry-common.S
@@ -16,12 +16,14 @@
 
 	.equ	NR_syscalls, __NR_syscalls
 
-#ifdef CONFIG_NEED_RET_TO_USER
-#include <mach/entry-macro.S>
-#else
-	.macro  arch_ret_to_user, tmp1, tmp2
-	.endm
+	.macro  arch_ret_to_user, tmp
+#ifdef CONFIG_ARCH_IOP32X
+	mrc	p15, 0, \tmp, c15, c1, 0
+	tst	\tmp, #(1 << 6)
+	bicne	\tmp, \tmp, #(1 << 6)
+	mcrne	p15, 0, \tmp, c15, c1, 0	@ Disable cp6 access
 #endif
+	.endm
 
 #include "entry-header.S"
 
@@ -55,7 +57,7 @@ __ret_fast_syscall:
 
 
 	/* perform architecture specific actions before user return */
-	arch_ret_to_user r1, lr
+	arch_ret_to_user r1
 
 	restore_user_regs fast = 1, offset = S_OFF
  UNWIND(.fnend		)
@@ -128,7 +130,7 @@ no_work_pending:
 	asm_trace_hardirqs_on save = 0
 
 	/* perform architecture specific actions before user return */
-	arch_ret_to_user r1, lr
+	arch_ret_to_user r1
 	ct_user_enter save = 0
 
 	restore_user_regs fast = 0, offset = 0
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);