Apply preempt_rt patch-4.9-rt1.patch.xz

[zynq/linux.git] / drivers / clocksource / tcb_clksrc.c

diff --git a/drivers/clocksource/tcb_clksrc.c b/drivers/clocksource/tcb_clksrc.c
index 4da2af9694a23b267cebdfa52da624e9f43b61fe..5b6f57f500b882894fb194f7108e1a03a3dc7ca1 100644 (file)
--- a/drivers/clocksource/tcb_clksrc.c
+++ b/drivers/clocksource/tcb_clksrc.c
@@ -23,8 +23,7 @@
  *     this 32 bit free-running counter. the second channel is not used.
  *
  *   - The third channel may be used to provide a 16-bit clockevent
- *     source, used in either periodic or oneshot mode.  This runs
- *     at 32 KiHZ, and can handle delays of up to two seconds.
+ *     source, used in either periodic or oneshot mode.
  *
  * A boot clocksource and clockevent source are also currently needed,
  * unless the relevant platforms (ARM/AT91, AVR32/AT32) are changed so
@@ -74,6 +73,8 @@ static struct clocksource clksrc = {
 struct tc_clkevt_device {
        struct clock_event_device       clkevt;
        struct clk                      *clk;
+       bool                            clk_enabled;
+       u32                             freq;
        void __iomem                    *regs;
 };
 
@@ -82,15 +83,26 @@ static struct tc_clkevt_device *to_tc_clkevt(struct clock_event_device *clkevt)
        return container_of(clkevt, struct tc_clkevt_device, clkevt);
 }
 
-/* For now, we always use the 32K clock ... this optimizes for NO_HZ,
- * because using one of the divided clocks would usually mean the
- * tick rate can never be less than several dozen Hz (vs 0.5 Hz).
- *
- * A divided clock could be good for high resolution timers, since
- * 30.5 usec resolution can seem "low".
- */
 static u32 timer_clock;
 
+static void tc_clk_disable(struct clock_event_device *d)
+{
+       struct tc_clkevt_device *tcd = to_tc_clkevt(d);
+
+       clk_disable(tcd->clk);
+       tcd->clk_enabled = false;
+}
+
+static void tc_clk_enable(struct clock_event_device *d)
+{
+       struct tc_clkevt_device *tcd = to_tc_clkevt(d);
+
+       if (tcd->clk_enabled)
+               return;
+       clk_enable(tcd->clk);
+       tcd->clk_enabled = true;
+}
+
 static int tc_shutdown(struct clock_event_device *d)
 {
        struct tc_clkevt_device *tcd = to_tc_clkevt(d);
@@ -98,8 +110,14 @@ static int tc_shutdown(struct clock_event_device *d)
 
        __raw_writel(0xff, regs + ATMEL_TC_REG(2, IDR));
        __raw_writel(ATMEL_TC_CLKDIS, regs + ATMEL_TC_REG(2, CCR));
+       return 0;
+}
+
+static int tc_shutdown_clk_off(struct clock_event_device *d)
+{
+       tc_shutdown(d);
        if (!clockevent_state_detached(d))
-               clk_disable(tcd->clk);
+               tc_clk_disable(d);
 
        return 0;
 }
@@ -112,9 +130,9 @@ static int tc_set_oneshot(struct clock_event_device *d)
        if (clockevent_state_oneshot(d) || clockevent_state_periodic(d))
                tc_shutdown(d);
 
-       clk_enable(tcd->clk);
+       tc_clk_enable(d);
 
-       /* slow clock, count up to RC, then irq and stop */
+       /* count up to RC, then irq and stop */
        __raw_writel(timer_clock | ATMEL_TC_CPCSTOP | ATMEL_TC_WAVE |
                     ATMEL_TC_WAVESEL_UP_AUTO, regs + ATMEL_TC_REG(2, CMR));
        __raw_writel(ATMEL_TC_CPCS, regs + ATMEL_TC_REG(2, IER));
@@ -134,12 +152,12 @@ static int tc_set_periodic(struct clock_event_device *d)
        /* By not making the gentime core emulate periodic mode on top
         * of oneshot, we get lower overhead and improved accuracy.
         */
-       clk_enable(tcd->clk);
+       tc_clk_enable(d);
 
-       /* slow clock, count up to RC, then irq and restart */
+       /* count up to RC, then irq and restart */
        __raw_writel(timer_clock | ATMEL_TC_WAVE | ATMEL_TC_WAVESEL_UP_AUTO,
                     regs + ATMEL_TC_REG(2, CMR));
-       __raw_writel((32768 + HZ / 2) / HZ, tcaddr + ATMEL_TC_REG(2, RC));
+       __raw_writel((tcd->freq + HZ / 2) / HZ, tcaddr + ATMEL_TC_REG(2, RC));
 
        /* Enable clock and interrupts on RC compare */
        __raw_writel(ATMEL_TC_CPCS, regs + ATMEL_TC_REG(2, IER));
@@ -166,9 +184,13 @@ static struct tc_clkevt_device clkevt = {
                .features               = CLOCK_EVT_FEAT_PERIODIC |
                                          CLOCK_EVT_FEAT_ONESHOT,
                /* Should be lower than at91rm9200's system timer */
+#ifdef CONFIG_ATMEL_TCB_CLKSRC_USE_SLOW_CLOCK
                .rating                 = 125,
+#else
+               .rating                 = 200,
+#endif
                .set_next_event         = tc_next_event,
-               .set_state_shutdown     = tc_shutdown,
+               .set_state_shutdown     = tc_shutdown_clk_off,
                .set_state_periodic     = tc_set_periodic,
                .set_state_oneshot      = tc_set_oneshot,
        },
@@ -188,8 +210,9 @@ static irqreturn_t ch2_irq(int irq, void *handle)
        return IRQ_NONE;
 }
 
-static int __init setup_clkevents(struct atmel_tc *tc, int clk32k_divisor_idx)
+static int __init setup_clkevents(struct atmel_tc *tc, int divisor_idx)
 {
+       unsigned divisor = atmel_tc_divisors[divisor_idx];
        int ret;
        struct clk *t2_clk = tc->clk[2];
        int irq = tc->irq[2];
@@ -210,7 +233,11 @@ static int __init setup_clkevents(struct atmel_tc *tc, int clk32k_divisor_idx)
        clkevt.regs = tc->regs;
        clkevt.clk = t2_clk;
 
-       timer_clock = clk32k_divisor_idx;
+       timer_clock = divisor_idx;
+       if (!divisor)
+               clkevt.freq = 32768;
+       else
+               clkevt.freq = clk_get_rate(t2_clk) / divisor;
 
        clkevt.clkevt.cpumask = cpumask_of(0);
 
@@ -221,7 +248,7 @@ static int __init setup_clkevents(struct atmel_tc *tc, int clk32k_divisor_idx)
                return ret;
        }
 
-       clockevents_config_and_register(&clkevt.clkevt, 32768, 1, 0xffff);
+       clockevents_config_and_register(&clkevt.clkevt, clkevt.freq, 1, 0xffff);
 
        return ret;
 }
@@ -358,7 +385,11 @@ static int __init tcb_clksrc_init(void)
                goto err_disable_t1;
 
        /* channel 2:  periodic and oneshot timer support */
+#ifdef CONFIG_ATMEL_TCB_CLKSRC_USE_SLOW_CLOCK
        ret = setup_clkevents(tc, clk32k_divisor_idx);
+#else
+       ret = setup_clkevents(tc, best_divisor_idx);
+#endif
        if (ret)
                goto err_unregister_clksrc;