2 files changed, 136 insertions, 61 deletions

diff --git a/arch/arm/plat-omap/dmtimer.c b/arch/arm/plat-omap/dmtimer.c
index dfdc3b2e3201..7c5cb4e128f2 100644
--- a/arch/arm/plat-omap/dmtimer.c
+++ b/arch/arm/plat-omap/dmtimer.c
@@ -170,11 +170,7 @@ static spinlock_t dm_timer_lock;
  */
 static inline u32 omap_dm_timer_read_reg(struct omap_dm_timer *timer, u32 reg)
 {
-        if (timer->posted)
-                while (readl(timer->io_base + (OMAP_TIMER_WRITE_PEND_REG & 0xff))
-                                & (reg >> WPSHIFT))
-                        cpu_relax();
-        return readl(timer->io_base + (reg & 0xff));
+        return __omap_dm_timer_read(timer->io_base, reg, timer->posted);
 }
 
 /*
@@ -186,11 +182,7 @@ static inline u32 omap_dm_timer_read_reg(struct omap_dm_timer *timer, u32 reg)
 static void omap_dm_timer_write_reg(struct omap_dm_timer *timer, u32 reg,
                                                 u32 value)
 {
-        if (timer->posted)
-                while (readl(timer->io_base + (OMAP_TIMER_WRITE_PEND_REG & 0xff))
-                                & (reg >> WPSHIFT))
-                        cpu_relax();
-        writel(value, timer->io_base + (reg & 0xff));
+        __omap_dm_timer_write(timer->io_base, reg, value, timer->posted);
 }
 
 static void omap_dm_timer_wait_for_reset(struct omap_dm_timer *timer)
@@ -209,7 +201,7 @@ static void omap_dm_timer_wait_for_reset(struct omap_dm_timer *timer)
 
 static void omap_dm_timer_reset(struct omap_dm_timer *timer)
 {
-        u32 l;
+        int autoidle = 0, wakeup = 0;
 
         if (!cpu_class_is_omap2() || timer != &dm_timers[0]) {
                 omap_dm_timer_write_reg(timer, OMAP_TIMER_IF_CTRL_REG, 0x06);
@@ -217,28 +209,21 @@ static void omap_dm_timer_reset(struct omap_dm_timer *timer)
         }
         omap_dm_timer_set_source(timer, OMAP_TIMER_SRC_32_KHZ);
 
-        l = omap_dm_timer_read_reg(timer, OMAP_TIMER_OCP_CFG_REG);
-        l |= 0x02 << 3;  /* Set to smart-idle mode */
-        l |= 0x2 << 8;   /* Set clock activity to perserve f-clock on idle */
-
         /* Enable autoidle on OMAP2 / OMAP3 */
         if (cpu_is_omap24xx() || cpu_is_omap34xx())
-                l |= 0x1 << 0;
+                autoidle = 1;
 
         /*
          * Enable wake-up on OMAP2 CPUs.
          */
         if (cpu_class_is_omap2())
-                l |= 1 << 2;
-        omap_dm_timer_write_reg(timer, OMAP_TIMER_OCP_CFG_REG, l);
+                wakeup = 1;
 
-        /* Match hardware reset default of posted mode */
-        omap_dm_timer_write_reg(timer, OMAP_TIMER_IF_CTRL_REG,
-                        OMAP_TIMER_CTRL_POSTED);
+        __omap_dm_timer_reset(timer->io_base, autoidle, wakeup);
         timer->posted = 1;
 }
 
-static void omap_dm_timer_prepare(struct omap_dm_timer *timer)
+void omap_dm_timer_prepare(struct omap_dm_timer *timer)
 {
         omap_dm_timer_enable(timer);
         omap_dm_timer_reset(timer);
@@ -410,25 +395,13 @@ EXPORT_SYMBOL_GPL(omap_dm_timer_start);
 
 void omap_dm_timer_stop(struct omap_dm_timer *timer)
 {
-        u32 l;
+        unsigned long rate = 0;
 
-        l = omap_dm_timer_read_reg(timer, OMAP_TIMER_CTRL_REG);
-        if (l & OMAP_TIMER_CTRL_ST) {
-                l &= ~0x1;
-                omap_dm_timer_write_reg(timer, OMAP_TIMER_CTRL_REG, l);
 #ifdef CONFIG_ARCH_OMAP2PLUS
-                /* Readback to make sure write has completed */
-                omap_dm_timer_read_reg(timer, OMAP_TIMER_CTRL_REG);
-                 /*
-                  * Wait for functional clock period x 3.5 to make sure that
-                  * timer is stopped
-                  */
-                udelay(3500000 / clk_get_rate(timer->fclk) + 1);
+        rate = clk_get_rate(timer->fclk);
 #endif
-        }
-        /* Ack possibly pending interrupt */
-        omap_dm_timer_write_reg(timer, OMAP_TIMER_STAT_REG,
-                        OMAP_TIMER_INT_OVERFLOW);
+
+        __omap_dm_timer_stop(timer->io_base, timer->posted, rate);
 }
 EXPORT_SYMBOL_GPL(omap_dm_timer_stop);
 
@@ -451,22 +424,11 @@ EXPORT_SYMBOL_GPL(omap_dm_timer_set_source);
 
 int omap_dm_timer_set_source(struct omap_dm_timer *timer, int source)
 {
-        int ret = -EINVAL;
-
         if (source < 0 || source >= 3)
                 return -EINVAL;
 
-        clk_disable(timer->fclk);
-        ret = clk_set_parent(timer->fclk, dm_source_clocks[source]);
-        clk_enable(timer->fclk);
-
-        /*
-         * When the functional clock disappears, too quick writes seem
-         * to cause an abort. XXX Is this still necessary?
-         */
-        __delay(300000);
-
-        return ret;
+        return __omap_dm_timer_set_source(timer->fclk,
+                                                dm_source_clocks[source]);
 }
 EXPORT_SYMBOL_GPL(omap_dm_timer_set_source);
 
@@ -504,8 +466,7 @@ void omap_dm_timer_set_load_start(struct omap_dm_timer *timer, int autoreload,
         }
         l |= OMAP_TIMER_CTRL_ST;
 
-        omap_dm_timer_write_reg(timer, OMAP_TIMER_COUNTER_REG, load);
-        omap_dm_timer_write_reg(timer, OMAP_TIMER_CTRL_REG, l);
+        __omap_dm_timer_load_start(timer->io_base, l, load, timer->posted);
 }
 EXPORT_SYMBOL_GPL(omap_dm_timer_set_load_start);
 
@@ -558,8 +519,7 @@ EXPORT_SYMBOL_GPL(omap_dm_timer_set_prescaler);
 void omap_dm_timer_set_int_enable(struct omap_dm_timer *timer,
                                   unsigned int value)
 {
-        omap_dm_timer_write_reg(timer, OMAP_TIMER_INT_EN_REG, value);
-        omap_dm_timer_write_reg(timer, OMAP_TIMER_WAKEUP_EN_REG, value);
+        __omap_dm_timer_int_enable(timer->io_base, value);
 }
 EXPORT_SYMBOL_GPL(omap_dm_timer_set_int_enable);
 
@@ -575,17 +535,13 @@ EXPORT_SYMBOL_GPL(omap_dm_timer_read_status);
 
 void omap_dm_timer_write_status(struct omap_dm_timer *timer, unsigned int value)
 {
-        omap_dm_timer_write_reg(timer, OMAP_TIMER_STAT_REG, value);
+        __omap_dm_timer_write_status(timer->io_base, value);
 }
 EXPORT_SYMBOL_GPL(omap_dm_timer_write_status);
 
 unsigned int omap_dm_timer_read_counter(struct omap_dm_timer *timer)
 {
-        unsigned int l;
-
-        l = omap_dm_timer_read_reg(timer, OMAP_TIMER_COUNTER_REG);
-
-        return l;
+        return __omap_dm_timer_read_counter(timer->io_base, timer->posted);
 }
 EXPORT_SYMBOL_GPL(omap_dm_timer_read_counter);
 
diff --git a/arch/arm/plat-omap/include/plat/dmtimer.h b/arch/arm/plat-omap/include/plat/dmtimer.h
index 32031056165d..54664a7498af 100644
--- a/arch/arm/plat-omap/include/plat/dmtimer.h
+++ b/arch/arm/plat-omap/include/plat/dmtimer.h
@@ -32,6 +32,9 @@
  * 675 Mass Ave, Cambridge, MA 02139, USA.
  */
 
+#include <linux/clk.h>
+#include <linux/delay.h>
+
 #ifndef __ASM_ARCH_DMTIMER_H
 #define __ASM_ARCH_DMTIMER_H
 
@@ -218,4 +221,120 @@ struct omap_dm_timer {
         unsigned posted:1;
 };
 
+void omap_dm_timer_prepare(struct omap_dm_timer *timer);
+
+static inline u32 __omap_dm_timer_read(void __iomem *base, u32 reg,
+                                                int posted)
+{
+        if (posted)
+                while (__raw_readl(base + (OMAP_TIMER_WRITE_PEND_REG & 0xff))
+                                & (reg >> WPSHIFT))
+                        cpu_relax();
+
+        return __raw_readl(base + (reg & 0xff));
+}
+
+static inline void __omap_dm_timer_write(void __iomem *base, u32 reg, u32 val,
+                                                int posted)
+{
+        if (posted)
+                while (__raw_readl(base + (OMAP_TIMER_WRITE_PEND_REG & 0xff))
+                                & (reg >> WPSHIFT))
+                        cpu_relax();
+
+        __raw_writel(val, base + (reg & 0xff));
+}
+
+/* Assumes the source clock has been set by caller */
+static inline void __omap_dm_timer_reset(void __iomem *base, int autoidle,
+                                                int wakeup)
+{
+        u32 l;
+
+        l = __omap_dm_timer_read(base, OMAP_TIMER_OCP_CFG_REG, 0);
+        l |= 0x02 << 3;  /* Set to smart-idle mode */
+        l |= 0x2 << 8;   /* Set clock activity to perserve f-clock on idle */
+
+        if (autoidle)
+                l |= 0x1 << 0;
+
+        if (wakeup)
+                l |= 1 << 2;
+
+        __omap_dm_timer_write(base, OMAP_TIMER_OCP_CFG_REG, l, 0);
+
+        /* Match hardware reset default of posted mode */
+        __omap_dm_timer_write(base, OMAP_TIMER_IF_CTRL_REG,
+                                        OMAP_TIMER_CTRL_POSTED, 0);
+}
+
+static inline int __omap_dm_timer_set_source(struct clk *timer_fck,
+                                                struct clk *parent)
+{
+        int ret;
+
+        clk_disable(timer_fck);
+        ret = clk_set_parent(timer_fck, parent);
+        clk_enable(timer_fck);
+
+        /*
+         * When the functional clock disappears, too quick writes seem
+         * to cause an abort. XXX Is this still necessary?
+         */
+        __delay(300000);
+
+        return ret;
+}
+
+static inline void __omap_dm_timer_stop(void __iomem *base, int posted,
+                                                unsigned long rate)
+{
+        u32 l;
+
+        l = __omap_dm_timer_read(base, OMAP_TIMER_CTRL_REG, posted);
+        if (l & OMAP_TIMER_CTRL_ST) {
+                l &= ~0x1;
+                __omap_dm_timer_write(base, OMAP_TIMER_CTRL_REG, l, posted);
+#ifdef CONFIG_ARCH_OMAP2PLUS
+                /* Readback to make sure write has completed */
+                __omap_dm_timer_read(base, OMAP_TIMER_CTRL_REG, posted);
+                /*
+                 * Wait for functional clock period x 3.5 to make sure that
+                 * timer is stopped
+                 */
+                udelay(3500000 / rate + 1);
+#endif
+        }
+
+        /* Ack possibly pending interrupt */
+        __omap_dm_timer_write(base, OMAP_TIMER_STAT_REG,
+                                        OMAP_TIMER_INT_OVERFLOW, 0);
+}
+
+static inline void __omap_dm_timer_load_start(void __iomem *base, u32 ctrl,
+                                                unsigned int load, int posted)
+{
+        __omap_dm_timer_write(base, OMAP_TIMER_COUNTER_REG, load, posted);
+        __omap_dm_timer_write(base, OMAP_TIMER_CTRL_REG, ctrl, posted);
+}
+
+static inline void __omap_dm_timer_int_enable(void __iomem *base,
+                                                unsigned int value)
+{
+        __omap_dm_timer_write(base, OMAP_TIMER_INT_EN_REG, value, 0);
+        __omap_dm_timer_write(base, OMAP_TIMER_WAKEUP_EN_REG, value, 0);
+}
+
+static inline unsigned int __omap_dm_timer_read_counter(void __iomem *base,
+                                                        int posted)
+{
+        return __omap_dm_timer_read(base, OMAP_TIMER_COUNTER_REG, posted);
+}
+
+static inline void __omap_dm_timer_write_status(void __iomem *base,
+                                                unsigned int value)
+{
+        __omap_dm_timer_write(base, OMAP_TIMER_STAT_REG, value, 0);
+}
+
 #endif /* __ASM_ARCH_DMTIMER_H */