1 files changed, 20 insertions, 5 deletions

diff --git a/arch/arm/mach-msm/timer.c b/arch/arm/mach-msm/timer.c
index 405e8a925202..9f3671a43314 100644
--- a/arch/arm/mach-msm/timer.c
+++ b/arch/arm/mach-msm/timer.c
@@ -81,11 +81,20 @@ enum {
 
 static struct msm_clock msm_clocks[];
 
+static struct msm_clock *clockevent_to_clock(struct clock_event_device *evt);
+
 static irqreturn_t msm_timer_interrupt(int irq, void *dev_id)
 {
         struct clock_event_device *evt = *(struct clock_event_device **)dev_id;
         if (evt->event_handler == NULL)
                 return IRQ_HANDLED;
+        /* Stop the timer tick */
+        if (evt->mode == CLOCK_EVT_MODE_ONESHOT) {
+                struct msm_clock *clock = clockevent_to_clock(evt);
+                u32 ctrl = readl_relaxed(clock->regbase + TIMER_ENABLE);
+                ctrl &= ~TIMER_ENABLE_EN;
+                writel_relaxed(ctrl, clock->regbase + TIMER_ENABLE);
+        }
         evt->event_handler(evt);
         return IRQ_HANDLED;
 }
@@ -118,10 +127,12 @@ static int msm_timer_set_next_event(unsigned long cycles,
                                     struct clock_event_device *evt)
 {
         struct msm_clock *clock = clockevent_to_clock(evt);
-        uint32_t now = readl(clock->local_counter);
-        uint32_t alarm = now + (cycles << clock->shift);
+        u32 match = cycles << clock->shift;
+        u32 ctrl = readl_relaxed(clock->regbase + TIMER_ENABLE);
 
-        writel(alarm, clock->regbase + TIMER_MATCH_VAL);
+        writel_relaxed(0, clock->regbase + TIMER_CLEAR);
+        writel_relaxed(match, clock->regbase + TIMER_MATCH_VAL);
+        writel_relaxed(ctrl | TIMER_ENABLE_EN, clock->regbase + TIMER_ENABLE);
         return 0;
 }
 
@@ -129,19 +140,23 @@ static void msm_timer_set_mode(enum clock_event_mode mode,
                               struct clock_event_device *evt)
 {
         struct msm_clock *clock = clockevent_to_clock(evt);
+        u32 ctrl;
+
+        ctrl = readl_relaxed(clock->regbase + TIMER_ENABLE);
+        ctrl &= ~(TIMER_ENABLE_EN | TIMER_ENABLE_CLR_ON_MATCH_EN);
 
         switch (mode) {
         case CLOCK_EVT_MODE_RESUME:
         case CLOCK_EVT_MODE_PERIODIC:
                 break;
         case CLOCK_EVT_MODE_ONESHOT:
-                writel(TIMER_ENABLE_EN, clock->regbase + TIMER_ENABLE);
+                /* Timer is enabled in set_next_event */
                 break;
         case CLOCK_EVT_MODE_UNUSED:
         case CLOCK_EVT_MODE_SHUTDOWN:
-                writel(0, clock->regbase + TIMER_ENABLE);
                 break;
         }
+        writel_relaxed(ctrl, clock->regbase + TIMER_ENABLE);
 }
 
 static struct msm_clock msm_clocks[] = {