1 files changed, 32 insertions, 16 deletions

diff --git a/drivers/clocksource/sun4i_timer.c b/drivers/clocksource/sun4i_timer.c
index 7123f654ce71..dd78b63f3a24 100644
--- a/drivers/clocksource/sun4i_timer.c
+++ b/drivers/clocksource/sun4i_timer.c
@@ -52,24 +52,46 @@ static void sun4i_clkevt_sync(void)
                 cpu_relax();
 }
 
+static void sun4i_clkevt_time_stop(u8 timer)
+{
+        u32 val = readl(timer_base + TIMER_CTL_REG(timer));
+        writel(val & ~TIMER_CTL_ENABLE, timer_base + TIMER_CTL_REG(timer));
+        sun4i_clkevt_sync();
+}
+
+static void sun4i_clkevt_time_setup(u8 timer, unsigned long delay)
+{
+        writel(delay, timer_base + TIMER_INTVAL_REG(timer));
+}
+
+static void sun4i_clkevt_time_start(u8 timer, bool periodic)
+{
+        u32 val = readl(timer_base + TIMER_CTL_REG(timer));
+
+        if (periodic)
+                val &= ~TIMER_CTL_ONESHOT;
+        else
+                val |= TIMER_CTL_ONESHOT;
+
+        writel(val | TIMER_CTL_ENABLE, timer_base + TIMER_CTL_REG(timer));
+}
+
 static void sun4i_clkevt_mode(enum clock_event_mode mode,
                               struct clock_event_device *clk)
 {
-        u32 u = readl(timer_base + TIMER_CTL_REG(0));
-
         switch (mode) {
         case CLOCK_EVT_MODE_PERIODIC:
-                u &= ~(TIMER_CTL_ONESHOT);
-                writel(u | TIMER_CTL_ENABLE, timer_base + TIMER_CTL_REG(0));
+                sun4i_clkevt_time_stop(0);
+                sun4i_clkevt_time_start(0, true);
                 break;
-
         case CLOCK_EVT_MODE_ONESHOT:
-                writel(u | TIMER_CTL_ONESHOT, timer_base + TIMER_CTL_REG(0));
+                sun4i_clkevt_time_stop(0);
+                sun4i_clkevt_time_start(0, false);
                 break;
         case CLOCK_EVT_MODE_UNUSED:
         case CLOCK_EVT_MODE_SHUTDOWN:
         default:
-                writel(u & ~(TIMER_CTL_ENABLE), timer_base + TIMER_CTL_REG(0));
+                sun4i_clkevt_time_stop(0);
                 break;
         }
 }
@@ -77,15 +99,9 @@ static void sun4i_clkevt_mode(enum clock_event_mode mode,
 static int sun4i_clkevt_next_event(unsigned long evt,
                                    struct clock_event_device *unused)
 {
-        u32 val = readl(timer_base + TIMER_CTL_REG(0));
-        writel(val & ~TIMER_CTL_ENABLE, timer_base + TIMER_CTL_REG(0));
-        sun4i_clkevt_sync();
-
-        writel(evt, timer_base + TIMER_INTVAL_REG(0));
-
-        val = readl(timer_base + TIMER_CTL_REG(0));
-        writel(val | TIMER_CTL_ENABLE | TIMER_CTL_AUTORELOAD,
-               timer_base + TIMER_CTL_REG(0));
+        sun4i_clkevt_time_stop(0);
+        sun4i_clkevt_time_setup(0, evt);
+        sun4i_clkevt_time_start(0, false);
 
         return 0;
 }