diff --git a/arch/arm/kernel/entry-armv.S b/arch/arm/kernel/entry-armv.S
index cc10a093a5455fa9f1b156b502c5346e64a985b6..d645897652c2d50a073bc529533127beff369c8d 100644
--- a/arch/arm/kernel/entry-armv.S
+++ b/arch/arm/kernel/entry-armv.S
@@ -27,6 +27,7 @@
  * Interrupt handling.  Preserves r7, r8, r9
  */
 	.macro	irq_handler
+	get_irqnr_preamble r5, lr
 1:	get_irqnr_and_base r0, r6, r5, lr
 	movne	r1, sp
 	@
diff --git a/arch/arm/kernel/entry-common.S b/arch/arm/kernel/entry-common.S
index 6f5e7c50d42ff2d78bd9b29d050f9902904f1af7..c589dc3ecd1ac0fb48cc619496fd3456649cb0a6 100644
--- a/arch/arm/kernel/entry-common.S
+++ b/arch/arm/kernel/entry-common.S
@@ -9,6 +9,7 @@
  */
 
 #include <asm/unistd.h>
+#include <asm/arch/entry-macro.S>
 
 #include "entry-header.S"
 
@@ -25,6 +26,9 @@ ret_fast_syscall:
 	tst	r1, #_TIF_WORK_MASK
 	bne	fast_work_pending
 
+	/* perform architecture specific actions before user return */
+	arch_ret_to_user r1, lr
+
 	@ fast_restore_user_regs
 	ldr	r1, [sp, #S_OFF + S_PSR]	@ get calling cpsr
 	ldr	lr, [sp, #S_OFF + S_PC]!	@ get pc
@@ -61,6 +65,9 @@ ret_slow_syscall:
 	tst	r1, #_TIF_WORK_MASK
 	bne	work_pending
 no_work_pending:
+	/* perform architecture specific actions before user return */
+	arch_ret_to_user r1, lr
+
 	@ slow_restore_user_regs
 	ldr	r1, [sp, #S_PSR]		@ get calling cpsr
 	ldr	lr, [sp, #S_PC]!		@ get pc
diff --git a/arch/arm/mach-iop13xx/Makefile b/arch/arm/mach-iop13xx/Makefile
index c3d6c08f2d4cfbc3cb0d1ee960b8c101617a8636..4185e0586c33f18e2a7b729336a6b79f4f9650de 100644
--- a/arch/arm/mach-iop13xx/Makefile
+++ b/arch/arm/mach-iop13xx/Makefile
@@ -5,7 +5,6 @@ obj-			:=
 
 obj-$(CONFIG_ARCH_IOP13XX) += setup.o
 obj-$(CONFIG_ARCH_IOP13XX) += irq.o
-obj-$(CONFIG_ARCH_IOP13XX) += time.o
 obj-$(CONFIG_ARCH_IOP13XX) += pci.o
 obj-$(CONFIG_ARCH_IOP13XX) += io.o
 obj-$(CONFIG_MACH_IQ81340SC) += iq81340sc.o
diff --git a/arch/arm/mach-iop13xx/iq81340mc.c b/arch/arm/mach-iop13xx/iq81340mc.c
index 2a1bbfe9896fcdf21c6183886176de0d7ea63cd9..a519d707571c81e678c81b0e2ed8d7a639943635 100644
--- a/arch/arm/mach-iop13xx/iq81340mc.c
+++ b/arch/arm/mach-iop13xx/iq81340mc.c
@@ -25,6 +25,7 @@
 #include <asm/mach/arch.h>
 #include <asm/arch/pci.h>
 #include <asm/mach/time.h>
+#include <asm/arch/time.h>
 
 extern int init_atu; /* Flag to select which ATU(s) to initialize / disable */
 
@@ -78,12 +79,12 @@ static void __init iq81340mc_init(void)
 
 static void __init iq81340mc_timer_init(void)
 {
-	iop13xx_init_time(400000000);
+	iop_init_time(400000000);
 }
 
 static struct sys_timer iq81340mc_timer = {
        .init       = iq81340mc_timer_init,
-       .offset     = iop13xx_gettimeoffset,
+       .offset     = iop_gettimeoffset,
 };
 
 MACHINE_START(IQ81340MC, "Intel IQ81340MC")
diff --git a/arch/arm/mach-iop13xx/iq81340sc.c b/arch/arm/mach-iop13xx/iq81340sc.c
index 5ad2b62c9bfde0582cc614f386f80591c7e82fe8..0e71fbcabe00a3dbafa3d85713a2182bf57fc10c 100644
--- a/arch/arm/mach-iop13xx/iq81340sc.c
+++ b/arch/arm/mach-iop13xx/iq81340sc.c
@@ -25,6 +25,7 @@
 #include <asm/mach/arch.h>
 #include <asm/arch/pci.h>
 #include <asm/mach/time.h>
+#include <asm/arch/time.h>
 
 extern int init_atu;
 
@@ -80,12 +81,12 @@ static void __init iq81340sc_init(void)
 
 static void __init iq81340sc_timer_init(void)
 {
-	iop13xx_init_time(400000000);
+	iop_init_time(400000000);
 }
 
 static struct sys_timer iq81340sc_timer = {
        .init       = iq81340sc_timer_init,
-       .offset     = iop13xx_gettimeoffset,
+       .offset     = iop_gettimeoffset,
 };
 
 MACHINE_START(IQ81340SC, "Intel IQ81340SC")
diff --git a/arch/arm/mach-iop13xx/irq.c b/arch/arm/mach-iop13xx/irq.c
index 162b93214965435d395a95b437162d49a49c935f..b2eb0b961031abcc64d4f73c68850c58eea129b7 100644
--- a/arch/arm/mach-iop13xx/irq.c
+++ b/arch/arm/mach-iop13xx/irq.c
@@ -161,65 +161,49 @@ static void write_intsize(u32 val)
 static void
 iop13xx_irq_mask0 (unsigned int irq)
 {
-	u32 cp_flags = iop13xx_cp6_save();
 	write_intctl_0(read_intctl_0() & ~(1 << (irq - 0)));
-	iop13xx_cp6_restore(cp_flags);
 }
 
 static void
 iop13xx_irq_mask1 (unsigned int irq)
 {
-	u32 cp_flags = iop13xx_cp6_save();
 	write_intctl_1(read_intctl_1() & ~(1 << (irq - 32)));
-	iop13xx_cp6_restore(cp_flags);
 }
 
 static void
 iop13xx_irq_mask2 (unsigned int irq)
 {
-	u32 cp_flags = iop13xx_cp6_save();
 	write_intctl_2(read_intctl_2() & ~(1 << (irq - 64)));
-	iop13xx_cp6_restore(cp_flags);
 }
 
 static void
 iop13xx_irq_mask3 (unsigned int irq)
 {
-	u32 cp_flags = iop13xx_cp6_save();
 	write_intctl_3(read_intctl_3() & ~(1 << (irq - 96)));
-	iop13xx_cp6_restore(cp_flags);
 }
 
 static void
 iop13xx_irq_unmask0(unsigned int irq)
 {
-	u32 cp_flags = iop13xx_cp6_save();
 	write_intctl_0(read_intctl_0() | (1 << (irq - 0)));
-	iop13xx_cp6_restore(cp_flags);
 }
 
 static void
 iop13xx_irq_unmask1(unsigned int irq)
 {
-	u32 cp_flags = iop13xx_cp6_save();
 	write_intctl_1(read_intctl_1() | (1 << (irq - 32)));
-	iop13xx_cp6_restore(cp_flags);
 }
 
 static void
 iop13xx_irq_unmask2(unsigned int irq)
 {
-	u32 cp_flags = iop13xx_cp6_save();
 	write_intctl_2(read_intctl_2() | (1 << (irq - 64)));
-	iop13xx_cp6_restore(cp_flags);
 }
 
 static void
 iop13xx_irq_unmask3(unsigned int irq)
 {
-	u32 cp_flags = iop13xx_cp6_save();
 	write_intctl_3(read_intctl_3() | (1 << (irq - 96)));
-	iop13xx_cp6_restore(cp_flags);
 }
 
 static struct irq_chip iop13xx_irqchip1 = {
@@ -256,7 +240,6 @@ void __init iop13xx_init_irq(void)
 {
 	unsigned int i;
 
-	u32 cp_flags = iop13xx_cp6_save();
 	iop_init_cp6_handler();
 
 	/* disable all interrupts */
@@ -288,6 +271,4 @@ void __init iop13xx_init_irq(void)
 		set_irq_handler(i, handle_level_irq);
 		set_irq_flags(i, IRQF_VALID | IRQF_PROBE);
 	}
-
-	iop13xx_cp6_restore(cp_flags);
 }
diff --git a/arch/arm/mach-iop13xx/time.c b/arch/arm/mach-iop13xx/time.c
deleted file mode 100644
index 8b21365f653f5ec60941004eeaeaa052e2e139c3..0000000000000000000000000000000000000000
--- a/arch/arm/mach-iop13xx/time.c
+++ /dev/null
@@ -1,102 +0,0 @@
-/*
- * arch/arm/mach-iop13xx/time.c
- *
- * Timer code for IOP13xx (copied from IOP32x/IOP33x implementation)
- *
- * Author: Deepak Saxena <dsaxena@mvista.com>
- *
- * Copyright 2002-2003 MontaVista Software Inc.
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms of the GNU General Public License as published by the
- * Free Software Foundation; either version 2 of the License, or (at your
- * option) any later version.
- */
-
-#include <linux/kernel.h>
-#include <linux/interrupt.h>
-#include <linux/time.h>
-#include <linux/init.h>
-#include <linux/timex.h>
-#include <asm/io.h>
-#include <asm/irq.h>
-#include <asm/uaccess.h>
-#include <asm/mach/irq.h>
-#include <asm/mach/time.h>
-
-static unsigned long ticks_per_jiffy;
-static unsigned long ticks_per_usec;
-static unsigned long next_jiffy_time;
-
-static inline u32 read_tcr1(void)
-{
-	u32 val;
-	asm volatile("mrc p6, 0, %0, c3, c9, 0" : "=r" (val));
-	return val;
-}
-
-unsigned long iop13xx_gettimeoffset(void)
-{
-	unsigned long offset;
-	u32 cp_flags;
-
-	cp_flags = iop13xx_cp6_save();
-	offset = next_jiffy_time - read_tcr1();
-	iop13xx_cp6_restore(cp_flags);
-
-	return offset / ticks_per_usec;
-}
-
-static irqreturn_t
-iop13xx_timer_interrupt(int irq, void *dev_id)
-{
-	u32 cp_flags = iop13xx_cp6_save();
-
-	write_seqlock(&xtime_lock);
-
-	asm volatile("mcr p6, 0, %0, c6, c9, 0" : : "r" (1));
-
-	while ((signed long)(next_jiffy_time - read_tcr1())
-							>= ticks_per_jiffy) {
-		timer_tick();
-		next_jiffy_time -= ticks_per_jiffy;
-	}
-
-	write_sequnlock(&xtime_lock);
-
-	iop13xx_cp6_restore(cp_flags);
-
-	return IRQ_HANDLED;
-}
-
-static struct irqaction iop13xx_timer_irq = {
-	.name		= "IOP13XX Timer Tick",
-	.handler	= iop13xx_timer_interrupt,
-	.flags		= IRQF_DISABLED | IRQF_TIMER,
-};
-
-void __init iop13xx_init_time(unsigned long tick_rate)
-{
-	u32 timer_ctl;
-	u32 cp_flags;
-
-	ticks_per_jiffy = (tick_rate + HZ/2) / HZ;
-	ticks_per_usec = tick_rate / 1000000;
-	next_jiffy_time = 0xffffffff;
-
-	timer_ctl = IOP13XX_TMR_EN | IOP13XX_TMR_PRIVILEGED |
-			IOP13XX_TMR_RELOAD | IOP13XX_TMR_RATIO_1_1;
-
-	/*
-	 * We use timer 0 for our timer interrupt, and timer 1 as
-	 * monotonic counter for tracking missed jiffies.
-	 */
-	cp_flags = iop13xx_cp6_save();
-	asm volatile("mcr p6, 0, %0, c4, c9, 0" : : "r" (ticks_per_jiffy - 1));
-	asm volatile("mcr p6, 0, %0, c0, c9, 0" : : "r" (timer_ctl));
-	asm volatile("mcr p6, 0, %0, c5, c9, 0" : : "r" (0xffffffff));
-	asm volatile("mcr p6, 0, %0, c1, c9, 0" : : "r" (timer_ctl));
-	iop13xx_cp6_restore(cp_flags);
-
-	setup_irq(IRQ_IOP13XX_TIMER0, &iop13xx_timer_irq);
-}
diff --git a/arch/arm/mach-iop32x/glantank.c b/arch/arm/mach-iop32x/glantank.c
index b9b765057dbecd8d2a74841bb5fd6ee2c28ce7a0..45f4f13ae11bc38d4634cd265e2d8a14435c1562 100644
--- a/arch/arm/mach-iop32x/glantank.c
+++ b/arch/arm/mach-iop32x/glantank.c
@@ -31,6 +31,7 @@
 #include <asm/mach/time.h>
 #include <asm/mach-types.h>
 #include <asm/page.h>
+#include <asm/arch/time.h>
 
 /*
  * GLAN Tank timer tick configuration.
@@ -38,12 +39,12 @@
 static void __init glantank_timer_init(void)
 {
 	/* 33.333 MHz crystal.  */
-	iop3xx_init_time(200000000);
+	iop_init_time(200000000);
 }
 
 static struct sys_timer glantank_timer = {
 	.init		= glantank_timer_init,
-	.offset		= iop3xx_gettimeoffset,
+	.offset		= iop_gettimeoffset,
 };
 
 
diff --git a/arch/arm/mach-iop32x/iq31244.c b/arch/arm/mach-iop32x/iq31244.c
index be4aedfa0de6abd0182202967a70f83867386cd0..571ac35bc2c75e482278c330a8f86fff754abbb2 100644
--- a/arch/arm/mach-iop32x/iq31244.c
+++ b/arch/arm/mach-iop32x/iq31244.c
@@ -36,7 +36,7 @@
 #include <asm/mach-types.h>
 #include <asm/page.h>
 #include <asm/pgtable.h>
-
+#include <asm/arch/time.h>
 
 /*
  * The EP80219 and IQ31244 use the same machine ID.  To find out
@@ -56,16 +56,16 @@ static void __init iq31244_timer_init(void)
 {
 	if (is_80219()) {
 		/* 33.333 MHz crystal.  */
-		iop3xx_init_time(200000000);
+		iop_init_time(200000000);
 	} else {
 		/* 33.000 MHz crystal.  */
-		iop3xx_init_time(198000000);
+		iop_init_time(198000000);
 	}
 }
 
 static struct sys_timer iq31244_timer = {
 	.init		= iq31244_timer_init,
-	.offset		= iop3xx_gettimeoffset,
+	.offset		= iop_gettimeoffset,
 };
 
 
diff --git a/arch/arm/mach-iop32x/iq80321.c b/arch/arm/mach-iop32x/iq80321.c
index 1f37b550188840d6263c5275c52fb50e4e64b371..361c70c0f64cfba07ee39d37f35bfdffeb052c9c 100644
--- a/arch/arm/mach-iop32x/iq80321.c
+++ b/arch/arm/mach-iop32x/iq80321.c
@@ -33,6 +33,7 @@
 #include <asm/mach-types.h>
 #include <asm/page.h>
 #include <asm/pgtable.h>
+#include <asm/arch/time.h>
 
 /*
  * IQ80321 timer tick configuration.
@@ -40,12 +41,12 @@
 static void __init iq80321_timer_init(void)
 {
 	/* 33.333 MHz crystal.  */
-	iop3xx_init_time(200000000);
+	iop_init_time(200000000);
 }
 
 static struct sys_timer iq80321_timer = {
 	.init		= iq80321_timer_init,
-	.offset		= iop3xx_gettimeoffset,
+	.offset		= iop_gettimeoffset,
 };
 
 
diff --git a/arch/arm/mach-iop32x/irq.c b/arch/arm/mach-iop32x/irq.c
index 8b0ac5590ae4611ad2ac519b220dd75bde0b33fd..82598dc18d802b511666d1030af00434701026c6 100644
--- a/arch/arm/mach-iop32x/irq.c
+++ b/arch/arm/mach-iop32x/irq.c
@@ -23,16 +23,12 @@ static u32 iop32x_mask;
 
 static inline void intctl_write(u32 val)
 {
-	iop3xx_cp6_enable();
 	asm volatile("mcr p6, 0, %0, c0, c0, 0" : : "r" (val));
-	iop3xx_cp6_disable();
 }
 
 static inline void intstr_write(u32 val)
 {
-	iop3xx_cp6_enable();
 	asm volatile("mcr p6, 0, %0, c4, c0, 0" : : "r" (val));
-	iop3xx_cp6_disable();
 }
 
 static void
diff --git a/arch/arm/mach-iop32x/n2100.c b/arch/arm/mach-iop32x/n2100.c
index 966aa51aee09f2a2ae2f430357234d7e178bce85..5f07344d96f3dfa65a6ab33d01d9d6cde606e9b0 100644
--- a/arch/arm/mach-iop32x/n2100.c
+++ b/arch/arm/mach-iop32x/n2100.c
@@ -37,6 +37,7 @@
 #include <asm/mach-types.h>
 #include <asm/page.h>
 #include <asm/pgtable.h>
+#include <asm/arch/time.h>
 
 /*
  * N2100 timer tick configuration.
@@ -44,12 +45,12 @@
 static void __init n2100_timer_init(void)
 {
 	/* 33.000 MHz crystal.  */
-	iop3xx_init_time(198000000);
+	iop_init_time(198000000);
 }
 
 static struct sys_timer n2100_timer = {
 	.init		= n2100_timer_init,
-	.offset		= iop3xx_gettimeoffset,
+	.offset		= iop_gettimeoffset,
 };
 
 
diff --git a/arch/arm/mach-iop33x/iq80331.c b/arch/arm/mach-iop33x/iq80331.c
index 97a7b7488264ea8609fcd99461a9568b3f631d3c..1a9e36138d8046ff9a7ccbc427852141100f794c 100644
--- a/arch/arm/mach-iop33x/iq80331.c
+++ b/arch/arm/mach-iop33x/iq80331.c
@@ -32,6 +32,7 @@
 #include <asm/mach-types.h>
 #include <asm/page.h>
 #include <asm/pgtable.h>
+#include <asm/arch/time.h>
 
 /*
  * IQ80331 timer tick configuration.
@@ -40,14 +41,14 @@ static void __init iq80331_timer_init(void)
 {
 	/* D-Step parts run at a higher internal bus frequency */
 	if (*IOP3XX_ATURID >= 0xa)
-		iop3xx_init_time(333000000);
+		iop_init_time(333000000);
 	else
-		iop3xx_init_time(266000000);
+		iop_init_time(266000000);
 }
 
 static struct sys_timer iq80331_timer = {
 	.init		= iq80331_timer_init,
-	.offset		= iop3xx_gettimeoffset,
+	.offset		= iop_gettimeoffset,
 };
 
 
diff --git a/arch/arm/mach-iop33x/iq80332.c b/arch/arm/mach-iop33x/iq80332.c
index 9887bfc1c07861269dc08fec92a4546f584b5cd0..96d6f0f3cd215d384348fb18b4add2a66cdc8658 100644
--- a/arch/arm/mach-iop33x/iq80332.c
+++ b/arch/arm/mach-iop33x/iq80332.c
@@ -32,6 +32,7 @@
 #include <asm/mach-types.h>
 #include <asm/page.h>
 #include <asm/pgtable.h>
+#include <asm/arch/time.h>
 
 /*
  * IQ80332 timer tick configuration.
@@ -40,14 +41,14 @@ static void __init iq80332_timer_init(void)
 {
 	/* D-Step parts and the iop333 run at a higher internal bus frequency */
 	if (*IOP3XX_ATURID >= 0xa || *IOP3XX_ATUDID == 0x374)
-		iop3xx_init_time(333000000);
+		iop_init_time(333000000);
 	else
-		iop3xx_init_time(266000000);
+		iop_init_time(266000000);
 }
 
 static struct sys_timer iq80332_timer = {
 	.init		= iq80332_timer_init,
-	.offset		= iop3xx_gettimeoffset,
+	.offset		= iop_gettimeoffset,
 };
 
 
diff --git a/arch/arm/mach-iop33x/irq.c b/arch/arm/mach-iop33x/irq.c
index effbe6b782d04b2dc3d7f6f0abbc5839013ba044..c65ea78a2427eb4073ab73e29321e56fa312e3d9 100644
--- a/arch/arm/mach-iop33x/irq.c
+++ b/arch/arm/mach-iop33x/irq.c
@@ -24,44 +24,32 @@ static u32 iop33x_mask1;
 
 static inline void intctl0_write(u32 val)
 {
-	iop3xx_cp6_enable();
 	asm volatile("mcr p6, 0, %0, c0, c0, 0" : : "r" (val));
-	iop3xx_cp6_disable();
 }
 
 static inline void intctl1_write(u32 val)
 {
-	iop3xx_cp6_enable();
 	asm volatile("mcr p6, 0, %0, c1, c0, 0" : : "r" (val));
-	iop3xx_cp6_disable();
 }
 
 static inline void intstr0_write(u32 val)
 {
-	iop3xx_cp6_enable();
 	asm volatile("mcr p6, 0, %0, c2, c0, 0" : : "r" (val));
-	iop3xx_cp6_disable();
 }
 
 static inline void intstr1_write(u32 val)
 {
-	iop3xx_cp6_enable();
 	asm volatile("mcr p6, 0, %0, c3, c0, 0" : : "r" (val));
-	iop3xx_cp6_disable();
 }
 
 static inline void intbase_write(u32 val)
 {
-	iop3xx_cp6_enable();
 	asm volatile("mcr p6, 0, %0, c12, c0, 0" : : "r" (val));
-	iop3xx_cp6_disable();
 }
 
 static inline void intsize_write(u32 val)
 {
-	iop3xx_cp6_enable();
 	asm volatile("mcr p6, 0, %0, c13, c0, 0" : : "r" (val));
-	iop3xx_cp6_disable();
 }
 
 static void
diff --git a/arch/arm/plat-iop/Makefile b/arch/arm/plat-iop/Makefile
index 3250d732a17142fec31636d18a551df97b57e1ba..4d2b1da3cd82e6105fcc1269d71b82cf7488d265 100644
--- a/arch/arm/plat-iop/Makefile
+++ b/arch/arm/plat-iop/Makefile
@@ -24,6 +24,7 @@ obj-$(CONFIG_ARCH_IOP33X) += cp6.o
 
 # IOP13XX
 obj-$(CONFIG_ARCH_IOP13XX) += cp6.o
+obj-$(CONFIG_ARCH_IOP13XX) += time.o
 
 obj-m                  :=
 obj-n                  :=
diff --git a/arch/arm/plat-iop/time.c b/arch/arm/plat-iop/time.c
index f530abdaa7a1e7fb5bd16ea5e5a49263d39f8cad..16300adfb4de2af53a30ccf0ecf79aa9cbde3260 100644
--- a/arch/arm/plat-iop/time.c
+++ b/arch/arm/plat-iop/time.c
@@ -24,39 +24,45 @@
 #include <asm/uaccess.h>
 #include <asm/mach/irq.h>
 #include <asm/mach/time.h>
-
-#ifdef CONFIG_ARCH_IOP32X
-#define IRQ_IOP3XX_TIMER0	IRQ_IOP32X_TIMER0
-#else
-#ifdef CONFIG_ARCH_IOP33X
-#define IRQ_IOP3XX_TIMER0	IRQ_IOP33X_TIMER0
-#endif
-#endif
+#include <asm/arch/time.h>
 
 static unsigned long ticks_per_jiffy;
 static unsigned long ticks_per_usec;
 static unsigned long next_jiffy_time;
 
-unsigned long iop3xx_gettimeoffset(void)
+unsigned long iop_gettimeoffset(void)
 {
-	unsigned long offset;
+	unsigned long offset, temp1, temp2;
 
-	offset = next_jiffy_time - *IOP3XX_TU_TCR1;
+	/* enable cp6, if necessary, to avoid taking the overhead of an
+	 * undefined instruction trap
+	 */
+	asm volatile (
+	"mrc	p15, 0, %0, c15, c1, 0\n\t"
+	"ands	%1, %0, #(1 << 6)\n\t"
+	"orreq	%0, %0, #(1 << 6)\n\t"
+	"mcreq	p15, 0, %0, c15, c1, 0\n\t"
+#ifdef CONFIG_XSCALE
+	"mrceq	p15, 0, %0, c15, c1, 0\n\t"
+	"moveq	%0, %0\n\t"
+	"subeq	pc, pc, #4\n\t"
+#endif
+	: "=r"(temp1), "=r"(temp2) : : "cc");
+
+	offset = next_jiffy_time - read_tcr1();
 
 	return offset / ticks_per_usec;
 }
 
 static irqreturn_t
-iop3xx_timer_interrupt(int irq, void *dev_id)
+iop_timer_interrupt(int irq, void *dev_id)
 {
 	write_seqlock(&xtime_lock);
 
-	iop3xx_cp6_enable();
-	asm volatile("mcr p6, 0, %0, c6, c1, 0" : : "r" (1));
-	iop3xx_cp6_disable();
+	write_tisr(1);
 
-	while ((signed long)(next_jiffy_time - *IOP3XX_TU_TCR1)
-							>= ticks_per_jiffy) {
+	while ((signed long)(next_jiffy_time - read_tcr1())
+		>= ticks_per_jiffy) {
 		timer_tick();
 		next_jiffy_time -= ticks_per_jiffy;
 	}
@@ -66,13 +72,13 @@ iop3xx_timer_interrupt(int irq, void *dev_id)
 	return IRQ_HANDLED;
 }
 
-static struct irqaction iop3xx_timer_irq = {
-	.name		= "IOP3XX Timer Tick",
-	.handler	= iop3xx_timer_interrupt,
+static struct irqaction iop_timer_irq = {
+	.name		= "IOP Timer Tick",
+	.handler	= iop_timer_interrupt,
 	.flags		= IRQF_DISABLED | IRQF_TIMER,
 };
 
-void __init iop3xx_init_time(unsigned long tick_rate)
+void __init iop_init_time(unsigned long tick_rate)
 {
 	u32 timer_ctl;
 
@@ -80,19 +86,17 @@ void __init iop3xx_init_time(unsigned long tick_rate)
 	ticks_per_usec = tick_rate / 1000000;
 	next_jiffy_time = 0xffffffff;
 
-	timer_ctl = IOP3XX_TMR_EN | IOP3XX_TMR_PRIVILEGED |
-			IOP3XX_TMR_RELOAD | IOP3XX_TMR_RATIO_1_1;
+	timer_ctl = IOP_TMR_EN | IOP_TMR_PRIVILEGED |
+			IOP_TMR_RELOAD | IOP_TMR_RATIO_1_1;
 
 	/*
 	 * We use timer 0 for our timer interrupt, and timer 1 as
 	 * monotonic counter for tracking missed jiffies.
 	 */
-	iop3xx_cp6_enable();
-	asm volatile("mcr p6, 0, %0, c4, c1, 0" : : "r" (ticks_per_jiffy - 1));
-	asm volatile("mcr p6, 0, %0, c0, c1, 0" : : "r" (timer_ctl));
-	asm volatile("mcr p6, 0, %0, c5, c1, 0" : : "r" (0xffffffff));
-	asm volatile("mcr p6, 0, %0, c1, c1, 0" : : "r" (timer_ctl));
-	iop3xx_cp6_disable();
-
-	setup_irq(IRQ_IOP3XX_TIMER0, &iop3xx_timer_irq);
+	write_trr0(ticks_per_jiffy - 1);
+	write_tmr0(timer_ctl);
+	write_trr1(0xffffffff);
+	write_tmr1(timer_ctl);
+
+	setup_irq(IRQ_IOP_TIMER0, &iop_timer_irq);
 }
diff --git a/include/asm-arm/arch-aaec2000/entry-macro.S b/include/asm-arm/arch-aaec2000/entry-macro.S
index 1eb3503bd16e1a78303a610eb4723422cde8988a..83fdf68f6b7c9c216a1d6bbfe6152089fa414785 100644
--- a/include/asm-arm/arch-aaec2000/entry-macro.S
+++ b/include/asm-arm/arch-aaec2000/entry-macro.S
@@ -15,6 +15,12 @@
 		.macro	disable_fiq
 		.endm
 
+		.macro  get_irqnr_preamble, base, tmp
+		.endm
+
+		.macro  arch_ret_to_user, tmp1, tmp2
+		.endm
+
 		.macro	get_irqnr_and_base, irqnr, irqstat, base, tmp
 		mov	r4, #0xf8000000
 		add	r4, r4, #0x00000500
diff --git a/include/asm-arm/arch-at91/entry-macro.S b/include/asm-arm/arch-at91/entry-macro.S
index 76c8cccf73aa18bc6c10dcf82507f5b45078c31c..cc1d850a0788012574c2c44310245789a8b1fbb5 100644
--- a/include/asm-arm/arch-at91/entry-macro.S
+++ b/include/asm-arm/arch-at91/entry-macro.S
@@ -16,6 +16,12 @@
 	.macro	disable_fiq
 	.endm
 
+	.macro  get_irqnr_preamble, base, tmp
+	.endm
+
+	.macro  arch_ret_to_user, tmp1, tmp2
+	.endm
+
 	.macro	get_irqnr_and_base, irqnr, irqstat, base, tmp
 	ldr	\base, =(AT91_VA_BASE_SYS + AT91_AIC)		@ base virtual address of AIC peripheral
 	ldr	\irqnr, [\base, #(AT91_AIC_IVR - AT91_AIC)]	@ read IRQ vector register: de-asserts nIRQ to processor (and clears interrupt)
diff --git a/include/asm-arm/arch-at91rm9200/entry-macro.S b/include/asm-arm/arch-at91rm9200/entry-macro.S
new file mode 100644
index 0000000000000000000000000000000000000000..0e0aadf2a175b28be92f8a789a9943985122f3e1
--- /dev/null
+++ b/include/asm-arm/arch-at91rm9200/entry-macro.S
@@ -0,0 +1,32 @@
+/*
+ * include/asm-arm/arch-at91rm9200/entry-macro.S
+ *
+ *  Copyright (C) 2003-2005 SAN People
+ *
+ * Low-level IRQ helper macros for AT91RM9200 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.
+ */
+
+#include <asm/hardware.h>
+#include <asm/arch/at91_aic.h>
+
+	.macro	disable_fiq
+	.endm
+
+	.macro  get_irqnr_preamble, base, tmp
+	.endm
+
+	.macro  arch_ret_to_user, tmp1, tmp2
+	.endm
+
+	.macro	get_irqnr_and_base, irqnr, irqstat, base, tmp
+	ldr	\base, =(AT91_VA_BASE_SYS)		@ base virtual address of SYS peripherals
+	ldr	\irqnr, [\base, #AT91_AIC_IVR]		@ read IRQ vector register: de-asserts nIRQ to processor (and clears interrupt)
+	ldr	\irqstat, [\base, #AT91_AIC_ISR]	@ read interrupt source number
+	teq	\irqstat, #0				@ ISR is 0 when no current interrupt, or spurious interrupt
+	streq	\tmp, [\base, #AT91_AIC_EOICR]		@ not going to be handled further, then ACK it now.
+	.endm
+
diff --git a/include/asm-arm/arch-cl7500/entry-macro.S b/include/asm-arm/arch-cl7500/entry-macro.S
index c9e5395e510614a50259c4cd5719d724f4320ad9..0cfb89b229d18da94b1595aa4d429b1d24a5f7ab 100644
--- a/include/asm-arm/arch-cl7500/entry-macro.S
+++ b/include/asm-arm/arch-cl7500/entry-macro.S
@@ -1,3 +1,8 @@
 #include <asm/hardware.h>
 #include <asm/hardware/entry-macro-iomd.S>
+	.macro  get_irqnr_preamble, base, tmp
+	.endm
+
+	.macro  arch_ret_to_user, tmp1, tmp2
+	.endm
 
diff --git a/include/asm-arm/arch-clps711x/entry-macro.S b/include/asm-arm/arch-clps711x/entry-macro.S
index de4481dd8ba0ebe6fbc21dc11f9c9bcf9e55ee52..cd8c5a0bc7bce6dfae9b8060b4347840a4b539d6 100644
--- a/include/asm-arm/arch-clps711x/entry-macro.S
+++ b/include/asm-arm/arch-clps711x/entry-macro.S
@@ -13,6 +13,12 @@
 		.macro	disable_fiq
 		.endm
 
+		.macro	get_irqnr_preamble, base, tmp
+		.endm
+
+		.macro	arch_ret_to_user, tmp1, tmp2
+		.endm
+
 #if (INTSR2 - INTSR1) != (INTMR2 - INTMR1)
 #error INTSR stride != INTMR stride
 #endif
diff --git a/include/asm-arm/arch-ebsa110/entry-macro.S b/include/asm-arm/arch-ebsa110/entry-macro.S
index b12ca04f998c657feaacb1cc5e0a680d16ed0e6f..aa23c5d6c69e4b4b31daf9ca1726f53daf10dcc6 100644
--- a/include/asm-arm/arch-ebsa110/entry-macro.S
+++ b/include/asm-arm/arch-ebsa110/entry-macro.S
@@ -15,6 +15,12 @@
 	.macro	disable_fiq
 	.endm
 
+	.macro  get_irqnr_preamble, base, tmp
+	.endm
+
+	.macro  arch_ret_to_user, tmp1, tmp2
+	.endm
+
 	.macro	get_irqnr_and_base, irqnr, stat, base, tmp
 	mov	\base, #IRQ_STAT
 	ldrb	\stat, [\base]			@ get interrupts
diff --git a/include/asm-arm/arch-ebsa285/entry-macro.S b/include/asm-arm/arch-ebsa285/entry-macro.S
index ce812d4f4a33687cdbd2deeadfec5508331619fc..4203dbf106624d95a8955f953e968822579af99b 100644
--- a/include/asm-arm/arch-ebsa285/entry-macro.S
+++ b/include/asm-arm/arch-ebsa285/entry-macro.S
@@ -14,6 +14,12 @@
 		.macro	disable_fiq
 		.endm
 
+		.macro  get_irqnr_preamble, base, tmp
+		.endm
+
+		.macro  arch_ret_to_user, tmp1, tmp2
+		.endm
+
 		.equ	dc21285_high, ARMCSR_BASE & 0xff000000
 		.equ	dc21285_low, ARMCSR_BASE & 0x00ffffff
 
diff --git a/include/asm-arm/arch-ep93xx/entry-macro.S b/include/asm-arm/arch-ep93xx/entry-macro.S
index 84140a28dfcf8bec343ef7a3341ac1671b9a8341..241ec221a0471e4bef8f71a253e5db061ce394d1 100644
--- a/include/asm-arm/arch-ep93xx/entry-macro.S
+++ b/include/asm-arm/arch-ep93xx/entry-macro.S
@@ -14,6 +14,12 @@
 		.macro	disable_fiq
 		.endm
 
+		.macro  get_irqnr_preamble, base, tmp
+		.endm
+
+		.macro  arch_ret_to_user, tmp1, tmp2
+		.endm
+
 		.macro	get_irqnr_and_base, irqnr, irqstat, base, tmp
 		ldr	\base, =(EP93XX_AHB_VIRT_BASE)
 		orr	\base, \base, #0x000b0000
diff --git a/include/asm-arm/arch-h720x/entry-macro.S b/include/asm-arm/arch-h720x/entry-macro.S
index 8f165648e2af0718a5d21e9fbc40ceea6987be00..38dd63ae104ea4c3c78f3f60fb645b477fb38cda 100644
--- a/include/asm-arm/arch-h720x/entry-macro.S
+++ b/include/asm-arm/arch-h720x/entry-macro.S
@@ -11,6 +11,12 @@
 		.macro  disable_fiq
 		.endm
 
+		.macro  get_irqnr_preamble, base, tmp
+		.endm
+
+		.macro  arch_ret_to_user, tmp1, tmp2
+		.endm
+
 		.macro  get_irqnr_and_base, irqnr, irqstat, base, tmp
 #if defined (CONFIG_CPU_H7201) || defined (CONFIG_CPU_H7202)
 		@ we could use the id register on H7202, but this is not
diff --git a/include/asm-arm/arch-imx/entry-macro.S b/include/asm-arm/arch-imx/entry-macro.S
index 61bb0bdc1b1651744c0b888a5ffd73a8461dd50c..0b84e81031c34f75a5d9223001e46626a3c20c31 100644
--- a/include/asm-arm/arch-imx/entry-macro.S
+++ b/include/asm-arm/arch-imx/entry-macro.S
@@ -11,6 +11,13 @@
 
 		.macro	disable_fiq
 		.endm
+
+		.macro	get_irqnr_preamble, base, tmp
+		.endm
+
+		.macro	arch_ret_to_user, tmp1, tmp2
+		.endm
+
 #define AITC_NIVECSR   0x40
 		.macro	get_irqnr_and_base, irqnr, irqstat, base, tmp
 		ldr	\base, =IO_ADDRESS(IMX_AITC_BASE)
diff --git a/include/asm-arm/arch-integrator/entry-macro.S b/include/asm-arm/arch-integrator/entry-macro.S
index 69838d04f90bc9882c36c06ffa65a3b513737021..491af1a23de55ca28cf35703009ce72e30142d68 100644
--- a/include/asm-arm/arch-integrator/entry-macro.S
+++ b/include/asm-arm/arch-integrator/entry-macro.S
@@ -13,6 +13,12 @@
  		.macro	disable_fiq
 		.endm
 
+		.macro  get_irqnr_preamble, base, tmp
+		.endm
+
+		.macro  arch_ret_to_user, tmp1, tmp2
+		.endm
+
 		.macro	get_irqnr_and_base, irqnr, irqstat, base, tmp
 /* FIXME: should not be using soo many LDRs here */
 		ldr	\base, =IO_ADDRESS(INTEGRATOR_IC_BASE)
diff --git a/include/asm-arm/arch-iop13xx/entry-macro.S b/include/asm-arm/arch-iop13xx/entry-macro.S
index 94c50283dc56388f5cce96ba78ea0536d71f03f3..a624a7870c64b36deb67a91598af6e765fbbeba6 100644
--- a/include/asm-arm/arch-iop13xx/entry-macro.S
+++ b/include/asm-arm/arch-iop13xx/entry-macro.S
@@ -19,21 +19,27 @@
 	.macro  disable_fiq
 	.endm
 
+	.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
+	.endm
+
 	/*
 	 * Note: a 1-cycle window exists where iintvec will return the value
 	 * of iintbase, so we explicitly check for "bad zeros"
 	 */
 	.macro  get_irqnr_and_base, irqnr, irqstat, 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	p6, 0, \irqnr, c3, c2, 0  	@ Read IINTVEC
 	cmp	\irqnr, #0
 	mrceq	p6, 0, \irqnr, c3, c2, 0  	@ Re-read on potentially bad zero
 	adds	\irqstat, \irqnr, #1	  	@ Check for 0xffffffff
 	movne	\irqnr, \irqnr, lsr #2	  	@ Convert to irqnr
+	.endm
 
-	biceq	\tmp, \tmp, #(1 << 6)
-	mcreq	p15, 0, \tmp, c15, c1, 0	@ Disable cp6 access if no more interrupts
+	.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/include/asm-arm/arch-iop13xx/iop13xx.h b/include/asm-arm/arch-iop13xx/iop13xx.h
index a88522a0ff8ea8480993f877a4d62cbad46bd72c..d26b755a98795f57dadfad23e54fc97faa4ebf76 100644
--- a/include/asm-arm/arch-iop13xx/iop13xx.h
+++ b/include/asm-arm/arch-iop13xx/iop13xx.h
@@ -9,34 +9,6 @@ void iop13xx_init_irq(void);
 void iop13xx_map_io(void);
 void iop13xx_platform_init(void);
 void iop13xx_init_irq(void);
-void iop13xx_init_time(unsigned long tickrate);
-unsigned long iop13xx_gettimeoffset(void);
-
-/* handle cp6 access
- * to do: handle access in entry-armv5.S and unify with
- * the iop3xx implementation
- * note: use iop13xx_cp6_enable_irq_save and iop13xx_cp6_irq_restore (irq.h)
- * when interrupts are enabled
- */
-static inline unsigned long iop13xx_cp6_save(void)
-{
-	u32 temp, cp_flags;
-
-	asm volatile (
-		"mrc	p15, 0, %1, c15, c1, 0\n\t"
-		"orr	%0, %1, #(1 << 6)\n\t"
-		"mcr	p15, 0, %0, c15, c1, 0\n\t"
-		: "=r" (temp), "=r"(cp_flags));
-
-	return cp_flags;
-}
-
-static inline void iop13xx_cp6_restore(unsigned long cp_flags)
-{
-	asm volatile (
-		"mcr	p15, 0, %0, c15, c1, 0\n\t"
-		: : "r" (cp_flags) );
-}
 
 /* CPUID CP6 R0 Page 0 */
 static inline int iop13xx_cpu_id(void)
@@ -479,14 +451,4 @@ static inline int iop13xx_cpu_id(void)
 #define IOP13XX_PBI_BAR1      		IOP13XX_PBI_OFFSET(0x10)
 #define IOP13XX_PBI_LR1       		IOP13XX_PBI_OFFSET(0x14)
 
-#define IOP13XX_TMR_TC			0x01
-#define IOP13XX_TMR_EN			0x02
-#define IOP13XX_TMR_RELOAD		0x04
-#define IOP13XX_TMR_PRIVILEGED		0x08
-
-#define IOP13XX_TMR_RATIO_1_1		0x00
-#define IOP13XX_TMR_RATIO_4_1		0x10
-#define IOP13XX_TMR_RATIO_8_1		0x20
-#define IOP13XX_TMR_RATIO_16_1		0x30
-
 #endif /* _IOP13XX_HW_H_ */
diff --git a/include/asm-arm/arch-iop13xx/irqs.h b/include/asm-arm/arch-iop13xx/irqs.h
index 442e35a403596afdf42bf0bc3c9331394081c853..5c6fac2a400497118bc2209effb0698a4b5cea27 100644
--- a/include/asm-arm/arch-iop13xx/irqs.h
+++ b/include/asm-arm/arch-iop13xx/irqs.h
@@ -3,8 +3,6 @@
 
 #ifndef __ASSEMBLER__
 #include <linux/types.h>
-#include <asm/system.h> /* local_irq_save */
-#include <asm/arch/iop13xx.h> /* iop13xx_cp6_* */
 
 /* INTPND0 CP6 R0 Page 3
  */
@@ -41,21 +39,6 @@ static inline u32 read_intpnd_3(void)
 	asm volatile("mrc p6, 0, %0, c3, c3, 0":"=r" (val));
 	return val;
 }
-
-static inline void
-iop13xx_cp6_enable_irq_save(unsigned long *cp_flags, unsigned long *irq_flags)
-{
-	local_irq_save(*irq_flags);
-	*cp_flags = iop13xx_cp6_save();
-}
-
-static inline void
-iop13xx_cp6_irq_restore(unsigned long *cp_flags,
-	unsigned long *irq_flags)
-{
-	iop13xx_cp6_restore(*cp_flags);
-	local_irq_restore(*irq_flags);
-}
 #endif
 
 #define INTBASE 0
diff --git a/include/asm-arm/arch-iop13xx/system.h b/include/asm-arm/arch-iop13xx/system.h
index ee3a62530af26525f5b164c754f6a20b82afb709..127827058e1f4841e1eb00f248b544b94db6d23a 100644
--- a/include/asm-arm/arch-iop13xx/system.h
+++ b/include/asm-arm/arch-iop13xx/system.h
@@ -48,12 +48,10 @@ static inline void arch_reset(char mode)
 	/*
 	 * Reset the internal bus (warning both cores are reset)
 	 */
-	u32 cp_flags = iop13xx_cp6_save();
 	write_wdtcr(IOP13XX_WDTCR_EN_ARM);
 	write_wdtcr(IOP13XX_WDTCR_EN);
 	write_wdtsr(IOP13XX_WDTSR_WRITE_EN | IOP13XX_WDTCR_IB_RESET);
 	write_wdtcr(0x1000);
-	iop13xx_cp6_restore(cp_flags);
 
 	for(;;);
 }
diff --git a/include/asm-arm/arch-iop13xx/time.h b/include/asm-arm/arch-iop13xx/time.h
new file mode 100644
index 0000000000000000000000000000000000000000..77a837a02dec00a255585d9baf488ccb1f91aea9
--- /dev/null
+++ b/include/asm-arm/arch-iop13xx/time.h
@@ -0,0 +1,51 @@
+#ifndef _IOP13XX_TIME_H_
+#define _IOP13XX_TIME_H_
+#define IRQ_IOP_TIMER0 IRQ_IOP13XX_TIMER0
+
+#define IOP_TMR_EN	    0x02
+#define IOP_TMR_RELOAD	    0x04
+#define IOP_TMR_PRIVILEGED 0x08
+#define IOP_TMR_RATIO_1_1  0x00
+
+void iop_init_time(unsigned long tickrate);
+unsigned long iop_gettimeoffset(void);
+
+static inline void write_tmr0(u32 val)
+{
+	asm volatile("mcr p6, 0, %0, c0, c9, 0" : : "r" (val));
+}
+
+static inline void write_tmr1(u32 val)
+{
+	asm volatile("mcr p6, 0, %0, c1, c9, 0" : : "r" (val));
+}
+
+static inline u32 read_tcr0(void)
+{
+	u32 val;
+	asm volatile("mrc p6, 0, %0, c2, c9, 0" : "=r" (val));
+	return val;
+}
+
+static inline u32 read_tcr1(void)
+{
+	u32 val;
+	asm volatile("mrc p6, 0, %0, c3, c9, 0" : "=r" (val));
+	return val;
+}
+
+static inline void write_trr0(u32 val)
+{
+	asm volatile("mcr p6, 0, %0, c4, c9, 0" : : "r" (val));
+}
+
+static inline void write_trr1(u32 val)
+{
+	asm volatile("mcr p6, 0, %0, c5, c9, 0" : : "r" (val));
+}
+
+static inline void write_tisr(u32 val)
+{
+	asm volatile("mcr p6, 0, %0, c6, c9, 0" : : "r" (val));
+}
+#endif
diff --git a/include/asm-arm/arch-iop32x/entry-macro.S b/include/asm-arm/arch-iop32x/entry-macro.S
index 1500cbbd2295802644dbea87c3575e8477f5e98d..207db99dfbd2130f655235cc86d447817dd2216a 100644
--- a/include/asm-arm/arch-iop32x/entry-macro.S
+++ b/include/asm-arm/arch-iop32x/entry-macro.S
@@ -9,13 +9,28 @@
  */
 #include <asm/arch/iop32x.h>
 
-		.macro	disable_fiq
-		.endm
+	.macro	disable_fiq
+	.endm
 
-		.macro	get_irqnr_and_base, irqnr, irqstat, base, tmp
-		ldr	\base, =IOP3XX_REG_ADDR(0x07D8)
-		ldr	\irqstat, [\base]		@ Read IINTSRC
-		cmp	\irqstat, #0
-		clzne	\irqnr, \irqstat
-		rsbne	\irqnr, \irqnr, #31
-		.endm
+	.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, #31
+	.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/include/asm-arm/arch-iop32x/time.h b/include/asm-arm/arch-iop32x/time.h
new file mode 100644
index 0000000000000000000000000000000000000000..0f28c99496233f6b067245bcc28d4aab4acfbaf3
--- /dev/null
+++ b/include/asm-arm/arch-iop32x/time.h
@@ -0,0 +1,4 @@
+#ifndef _IOP32X_TIME_H_
+#define _IOP32X_TIME_H_
+#define IRQ_IOP_TIMER0 IRQ_IOP32X_TIMER0
+#endif
diff --git a/include/asm-arm/arch-iop33x/entry-macro.S b/include/asm-arm/arch-iop33x/entry-macro.S
index 92b791702e3418fbf85fc80d52212f4d54b46457..b8e3d449e88276fb271430cd7315b8848417e2be 100644
--- a/include/asm-arm/arch-iop33x/entry-macro.S
+++ b/include/asm-arm/arch-iop33x/entry-macro.S
@@ -9,14 +9,29 @@
  */
 #include <asm/arch/iop33x.h>
 
-		.macro	disable_fiq
-		.endm
+	.macro	disable_fiq
+	.endm
 
-		.macro	get_irqnr_and_base, irqnr, irqstat, base, tmp
-		ldr	\base, =IOP3XX_REG_ADDR(0x07C8)
-		ldr	\irqstat, [\base]		@ Read IINTVEC
-		cmp	\irqstat, #0
-		ldreq	\irqstat, [\base]		@ erratum 63 workaround
-		adds	\irqnr, \irqstat, #1
-		movne	\irqnr, \irqstat, lsr #2
-		.endm
+	.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, c14, c0, 0     @ Read IINTVEC
+	cmp     \irqstat, #0
+	mrceq   p6, 0, \irqstat, c14, c0, 0     @ erratum 63 workaround
+	adds    \irqnr, \irqstat, #1
+	movne   \irqnr, \irqstat, lsr #2
+	.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/include/asm-arm/arch-iop33x/time.h b/include/asm-arm/arch-iop33x/time.h
new file mode 100644
index 0000000000000000000000000000000000000000..4ac4d7664f850453dd9fad32240873a89d5f4dfb
--- /dev/null
+++ b/include/asm-arm/arch-iop33x/time.h
@@ -0,0 +1,4 @@
+#ifndef _IOP33X_TIME_H_
+#define _IOP33X_TIME_H_
+#define IRQ_IOP_TIMER0 IRQ_IOP33X_TIMER0
+#endif
diff --git a/include/asm-arm/arch-ixp2000/entry-macro.S b/include/asm-arm/arch-ixp2000/entry-macro.S
index 16e1e6124b319bec4e7b043abb3e13c9d126229d..11d512ad594586274e147cf64d8402ed65336deb 100644
--- a/include/asm-arm/arch-ixp2000/entry-macro.S
+++ b/include/asm-arm/arch-ixp2000/entry-macro.S
@@ -12,6 +12,12 @@
 		.macro  disable_fiq
 		.endm
 
+		.macro  get_irqnr_preamble, base, tmp
+		.endm
+
+		.macro  arch_ret_to_user, tmp1, tmp2
+		.endm
+
 		.macro  get_irqnr_and_base, irqnr, irqstat, base, tmp
 
 		mov	\irqnr, #0x0              @clear out irqnr as default
diff --git a/include/asm-arm/arch-ixp23xx/entry-macro.S b/include/asm-arm/arch-ixp23xx/entry-macro.S
index 867761677b572d6c17816284a6d42bd88d2fcd14..ec9dd6fc2d0f6592d44dcd2f0d96023d2cb7ea2c 100644
--- a/include/asm-arm/arch-ixp23xx/entry-macro.S
+++ b/include/asm-arm/arch-ixp23xx/entry-macro.S
@@ -5,6 +5,12 @@
 		.macro	disable_fiq
 		.endm
 
+		.macro  get_irqnr_preamble, base, tmp
+		.endm
+
+		.macro  arch_ret_to_user, tmp1, tmp2
+		.endm
+
 		.macro	get_irqnr_and_base, irqnr, irqstat, base, tmp
 		ldr	\irqnr, =(IXP23XX_INTC_VIRT + IXP23XX_INTR_IRQ_ENC_ST_OFFSET)
 		ldr	\irqnr, [\irqnr]	@ get interrupt number
diff --git a/include/asm-arm/arch-ixp4xx/entry-macro.S b/include/asm-arm/arch-ixp4xx/entry-macro.S
index 27e124132e4cde02926eae5b52aeb26cdefbd896..dadb568b7ef0f07a00c747e61b3f57836bbe7ead 100644
--- a/include/asm-arm/arch-ixp4xx/entry-macro.S
+++ b/include/asm-arm/arch-ixp4xx/entry-macro.S
@@ -12,6 +12,12 @@
 		.macro	disable_fiq
 		.endm
 
+		.macro  get_irqnr_preamble, base, tmp
+		.endm
+
+		.macro  arch_ret_to_user, tmp1, tmp2
+		.endm
+
 		.macro	get_irqnr_and_base, irqnr, irqstat, base, tmp
 		ldr	\irqstat, =(IXP4XX_INTC_BASE_VIRT+IXP4XX_ICIP_OFFSET)
 		ldr	\irqstat, [\irqstat]		@ get interrupts
diff --git a/include/asm-arm/arch-l7200/entry-macro.S b/include/asm-arm/arch-l7200/entry-macro.S
index 8b6342dc4be2aaee619483e76e0bbcbfc62cd641..63411d3e9df49e87108215364d29a55bb8b4f53f 100644
--- a/include/asm-arm/arch-l7200/entry-macro.S
+++ b/include/asm-arm/arch-l7200/entry-macro.S
@@ -14,6 +14,12 @@
 		.macro  disable_fiq
 		.endm
 
+		.macro  get_irqnr_preamble, base, tmp
+		.endm
+
+		.macro  arch_ret_to_user, tmp1, tmp2
+		.endm
+
 		.macro  get_irqnr_and_base, irqnr, irqstat, base, tmp
 		mov     \irqstat, #irq_base_addr		@ Virt addr IRQ regs
 		add	\irqstat, \irqstat, #0x00001000		@ Status reg
diff --git a/include/asm-arm/arch-lh7a40x/entry-macro.S b/include/asm-arm/arch-lh7a40x/entry-macro.S
index 9fc7f4988124af1b81247778df5890095e6acd6f..502700604e0000752716988e64f1a13bde66e56a 100644
--- a/include/asm-arm/arch-lh7a40x/entry-macro.S
+++ b/include/asm-arm/arch-lh7a40x/entry-macro.S
@@ -26,6 +26,12 @@
 		.macro	disable_fiq
 		.endm
 
+		.macro  get_irqnr_preamble, base, tmp
+		.endm
+
+		.macro  arch_ret_to_user, tmp1, tmp2
+		.endm
+
 		.macro	get_irqnr_and_base, irqnr, irqstat, base, tmp
 
 branch_irq_lh7a400: b 1000f
diff --git a/include/asm-arm/arch-netx/entry-macro.S b/include/asm-arm/arch-netx/entry-macro.S
index 658df4d60ff36f13836a34b14fc0d4d39cbe5651..83ad188a08474c4239224526282ef6d6f22deb04 100644
--- a/include/asm-arm/arch-netx/entry-macro.S
+++ b/include/asm-arm/arch-netx/entry-macro.S
@@ -23,6 +23,12 @@
 		.macro  disable_fiq
 		.endm
 
+		.macro  get_irqnr_preamble, base, tmp
+		.endm
+
+		.macro  arch_ret_to_user, tmp1, tmp2
+		.endm
+
 		.macro  get_irqnr_and_base, irqnr, irqstat, base, tmp
 		mov	\base, #io_p2v(0x00100000)
 		add	\base, \base, #0x000ff000
diff --git a/include/asm-arm/arch-omap/entry-macro.S b/include/asm-arm/arch-omap/entry-macro.S
index 0ffb1185f1ac90b930fd486e6fe57c85bf0a0bcd..c90dff4828f730f9bc8d8b2a1760e716f46ea75c 100644
--- a/include/asm-arm/arch-omap/entry-macro.S
+++ b/include/asm-arm/arch-omap/entry-macro.S
@@ -29,6 +29,12 @@
  		.macro	disable_fiq
 		.endm
 
+		.macro  get_irqnr_preamble, base, tmp
+		.endm
+
+		.macro  arch_ret_to_user, tmp1, tmp2
+		.endm
+
 		.macro	get_irqnr_and_base, irqnr, irqstat, base, tmp
 		ldr	\base, =IO_ADDRESS(OMAP_IH1_BASE)
 		ldr	\irqnr, [\base, #IRQ_ITR_REG_OFFSET]
diff --git a/include/asm-arm/arch-pnx4008/entry-macro.S b/include/asm-arm/arch-pnx4008/entry-macro.S
index c1c198e3680bde3bf442be47b3ab469bc2c573dc..f11731974e5d5a2b2b3123783b16cb8b7bc19375 100644
--- a/include/asm-arm/arch-pnx4008/entry-macro.S
+++ b/include/asm-arm/arch-pnx4008/entry-macro.S
@@ -28,6 +28,12 @@
 		.macro	disable_fiq
 		.endm
 
+		.macro  get_irqnr_preamble, base, tmp
+		.endm
+
+		.macro  arch_ret_to_user, tmp1, tmp2
+		.endm
+
 		.macro	get_irqnr_and_base, irqnr, irqstat, base, tmp
 /* decode the MIC interrupt numbers */
 		ldr	\base, =IO_ADDRESS(PNX4008_INTCTRLMIC_BASE)
diff --git a/include/asm-arm/arch-pxa/entry-macro.S b/include/asm-arm/arch-pxa/entry-macro.S
index 4985e33afc12de2cc389dad3e76dc60182345d57..1d5fbb9b379a03652b3c8347ef1adaed15bce146 100644
--- a/include/asm-arm/arch-pxa/entry-macro.S
+++ b/include/asm-arm/arch-pxa/entry-macro.S
@@ -13,6 +13,12 @@
 		.macro	disable_fiq
 		.endm
 
+		.macro  get_irqnr_preamble, base, tmp
+		.endm
+
+		.macro  arch_ret_to_user, tmp1, tmp2
+		.endm
+
 		.macro	get_irqnr_and_base, irqnr, irqstat, base, tmp
 #ifdef CONFIG_PXA27x
 		mrc	p6, 0, \irqstat, c0, c0, 0		@ ICIP
diff --git a/include/asm-arm/arch-realview/entry-macro.S b/include/asm-arm/arch-realview/entry-macro.S
index 1a6eec86bd476581b0e29f5ae566d9a49c61d27b..138838d4ad7575460b0995cb2b9cf3d90c0aa39a 100644
--- a/include/asm-arm/arch-realview/entry-macro.S
+++ b/include/asm-arm/arch-realview/entry-macro.S
@@ -13,6 +13,12 @@
 		.macro	disable_fiq
 		.endm
 
+		.macro  get_irqnr_preamble, base, tmp
+		.endm
+
+		.macro  arch_ret_to_user, tmp1, tmp2
+		.endm
+
 		/*
 		 * The interrupt numbering scheme is defined in the
 		 * interrupt controller spec.  To wit:
diff --git a/include/asm-arm/arch-rpc/entry-macro.S b/include/asm-arm/arch-rpc/entry-macro.S
index c9e5395e510614a50259c4cd5719d724f4320ad9..0cfb89b229d18da94b1595aa4d429b1d24a5f7ab 100644
--- a/include/asm-arm/arch-rpc/entry-macro.S
+++ b/include/asm-arm/arch-rpc/entry-macro.S
@@ -1,3 +1,8 @@
 #include <asm/hardware.h>
 #include <asm/hardware/entry-macro-iomd.S>
+	.macro  get_irqnr_preamble, base, tmp
+	.endm
+
+	.macro  arch_ret_to_user, tmp1, tmp2
+	.endm
 
diff --git a/include/asm-arm/arch-s3c2410/entry-macro.S b/include/asm-arm/arch-s3c2410/entry-macro.S
index 1eb4e6b8d249c70ed2a53be4c7252aed5dbaf23c..bbec0a8ff158649b8d828f3aa97e44ee811bcb88 100644
--- a/include/asm-arm/arch-s3c2410/entry-macro.S
+++ b/include/asm-arm/arch-s3c2410/entry-macro.S
@@ -22,6 +22,12 @@
 #include <asm/hardware.h>
 #include <asm/irq.h>
 
+	.macro  get_irqnr_preamble, base, tmp
+	.endm
+
+	.macro  arch_ret_to_user, tmp1, tmp2
+	.endm
+
 	.macro	get_irqnr_and_base, irqnr, irqstat, base, tmp
 
 		mov	\base, #S3C24XX_VA_IRQ
diff --git a/include/asm-arm/arch-sa1100/entry-macro.S b/include/asm-arm/arch-sa1100/entry-macro.S
index 51fb50ce11690eb4dc48fad0108deb53e234af82..028967629340b60164cc98061b78443703a7e50c 100644
--- a/include/asm-arm/arch-sa1100/entry-macro.S
+++ b/include/asm-arm/arch-sa1100/entry-macro.S
@@ -11,6 +11,12 @@
  		.macro	disable_fiq
 		.endm
 
+		.macro  get_irqnr_preamble, base, tmp
+		.endm
+
+		.macro  arch_ret_to_user, tmp1, tmp2
+		.endm
+
 		.macro	get_irqnr_and_base, irqnr, irqstat, base, tmp
 		mov	r4, #0xfa000000			@ ICIP = 0xfa050000
 		add	r4, r4, #0x00050000
diff --git a/include/asm-arm/arch-shark/entry-macro.S b/include/asm-arm/arch-shark/entry-macro.S
index a924f27fb8d98c7161a26f90aae9f4dc0808c4b2..82463f30f3dfc7e02dcbbafd270e352aa1c1a048 100644
--- a/include/asm-arm/arch-shark/entry-macro.S
+++ b/include/asm-arm/arch-shark/entry-macro.S
@@ -10,6 +10,12 @@
 		.macro	disable_fiq
 		.endm
 
+		.macro  get_irqnr_preamble, base, tmp
+		.endm
+
+		.macro  arch_ret_to_user, tmp1, tmp2
+		.endm
+
 		.macro	get_irqnr_and_base, irqnr, irqstat, base, tmp
 		mov	r4, #0xe0000000
 
diff --git a/include/asm-arm/arch-versatile/entry-macro.S b/include/asm-arm/arch-versatile/entry-macro.S
index feff771c0a0a950630cbef18d231da9c5d665d47..0fae002637a06c832db730c9796c229d4dfdee24 100644
--- a/include/asm-arm/arch-versatile/entry-macro.S
+++ b/include/asm-arm/arch-versatile/entry-macro.S
@@ -13,6 +13,12 @@
 		.macro	disable_fiq
 		.endm
 
+		.macro  get_irqnr_preamble, base, tmp
+		.endm
+
+		.macro  arch_ret_to_user, tmp1, tmp2
+		.endm
+
 		.macro	get_irqnr_and_base, irqnr, irqstat, base, tmp
 		ldr	\base, =IO_ADDRESS(VERSATILE_VIC_BASE)
 		ldr	\irqstat, [\base, #VIC_IRQ_STATUS]	@ get masked status
diff --git a/include/asm-arm/hardware/iop3xx.h b/include/asm-arm/hardware/iop3xx.h
index c91b546e20ef8040419fbd86cb9170192581d915..15141a9caca8352a641117341caa042af3cedff2 100644
--- a/include/asm-arm/hardware/iop3xx.h
+++ b/include/asm-arm/hardware/iop3xx.h
@@ -188,14 +188,10 @@ extern void gpio_line_set(int line, int value);
 #define IOP3XX_TU_TRR1		(volatile u32 *)IOP3XX_TIMER_REG(0x0014)
 #define IOP3XX_TU_TISR		(volatile u32 *)IOP3XX_TIMER_REG(0x0018)
 #define IOP3XX_TU_WDTCR		(volatile u32 *)IOP3XX_TIMER_REG(0x001c)
-#define IOP3XX_TMR_TC		0x01
-#define IOP3XX_TMR_EN		0x02
-#define IOP3XX_TMR_RELOAD	0x04
-#define IOP3XX_TMR_PRIVILEGED	0x09
-#define IOP3XX_TMR_RATIO_1_1	0x00
-#define IOP3XX_TMR_RATIO_4_1	0x10
-#define IOP3XX_TMR_RATIO_8_1	0x20
-#define IOP3XX_TMR_RATIO_16_1	0x30
+#define IOP_TMR_EN	    0x02
+#define IOP_TMR_RELOAD	    0x04
+#define IOP_TMR_PRIVILEGED 0x08
+#define IOP_TMR_RATIO_1_1  0x00
 
 /* Application accelerator unit  */
 #define IOP3XX_AAU_ACR		(volatile u32 *)IOP3XX_REG_ADDR(0x0800)
@@ -276,40 +272,52 @@ extern void gpio_line_set(int line, int value);
 
 #ifndef __ASSEMBLY__
 void iop3xx_map_io(void);
-void iop3xx_init_time(unsigned long);
-unsigned long iop3xx_gettimeoffset(void);
 void iop_init_cp6_handler(void);
+void iop_init_time(unsigned long tickrate);
+unsigned long iop_gettimeoffset(void);
 
-extern struct platform_device iop3xx_i2c0_device;
-extern struct platform_device iop3xx_i2c1_device;
+static inline void write_tmr0(u32 val)
+{
+	asm volatile("mcr p6, 0, %0, c0, c1, 0" : : "r" (val));
+}
+
+static inline void write_tmr1(u32 val)
+{
+	asm volatile("mcr p6, 0, %0, c1, c1, 0" : : "r" (val));
+}
+
+static inline u32 read_tcr0(void)
+{
+	u32 val;
+	asm volatile("mrc p6, 0, %0, c2, c1, 0" : "=r" (val));
+	return val;
+}
 
-extern inline void iop3xx_cp6_enable(void)
+static inline u32 read_tcr1(void)
 {
-	u32 temp;
+	u32 val;
+	asm volatile("mrc p6, 0, %0, c3, c1, 0" : "=r" (val));
+	return val;
+}
 
-	asm volatile (
-		"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\n\t"
-		: "=r" (temp) );
+static inline void write_trr0(u32 val)
+{
+	asm volatile("mcr p6, 0, %0, c4, c1, 0" : : "r" (val));
 }
 
-extern inline void iop3xx_cp6_disable(void)
+static inline void write_trr1(u32 val)
 {
-	u32 temp;
+	asm volatile("mcr p6, 0, %0, c5, c1, 0" : : "r" (val));
+}
 
-	asm volatile (
-		"mrc	p15, 0, %0, c15, c1, 0\n\t"
-		"bic	%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\n\t"
-		: "=r" (temp) );
+static inline void write_tisr(u32 val)
+{
+	asm volatile("mcr p6, 0, %0, c6, c1, 0" : : "r" (val));
 }
+
+extern struct platform_device iop3xx_i2c0_device;
+extern struct platform_device iop3xx_i2c1_device;
+
 #endif