1 files changed, 82 insertions, 25 deletions

diff --git a/drivers/clocksource/timer-stm32.c b/drivers/clocksource/timer-stm32.c
index 882037f1d8d6..0d37f1a1994e 100644
--- a/drivers/clocksource/timer-stm32.c
+++ b/drivers/clocksource/timer-stm32.c
@@ -44,6 +44,42 @@
 #define TIM_PSC_MAX     USHRT_MAX
 #define TIM_PSC_CLKRATE 10000
 
+struct stm32_timer_private {
+        int bits;
+};
+
+/**
+ * stm32_timer_of_bits_set - set accessor helper
+ * @to: a timer_of structure pointer
+ * @bits: the number of bits (16 or 32)
+ *
+ * Accessor helper to set the number of bits in the timer-of private
+ * structure.
+ *
+ */
+static void stm32_timer_of_bits_set(struct timer_of *to, int bits)
+{
+        struct stm32_timer_private *pd = to->private_data;
+
+        pd->bits = bits;
+}
+
+/**
+ * stm32_timer_of_bits_get - get accessor helper
+ * @to: a timer_of structure pointer
+ *
+ * Accessor helper to get the number of bits in the timer-of private
+ * structure.
+ *
+ * Returns an integer corresponding to the number of bits.
+ */
+static int stm32_timer_of_bits_get(struct timer_of *to)
+{
+        struct stm32_timer_private *pd = to->private_data;
+
+        return pd->bits;
+}
+
 static void stm32_clock_event_disable(struct timer_of *to)
 {
         writel_relaxed(0, timer_of_base(to) + TIM_DIER);
@@ -124,35 +160,31 @@ static irqreturn_t stm32_clock_event_handler(int irq, void *dev_id)
  * is 32 bits wide, the result will be UINT_MAX, otherwise it will
  * be truncated by the 16-bit register to USHRT_MAX.
  *
- * Returns UINT_MAX if the timer is 32 bits wide, USHRT_MAX if it is a
- * 16 bits wide.
  */
-static u32 __init stm32_timer_width(struct timer_of *to)
+static void __init stm32_timer_set_width(struct timer_of *to)
 {
+        u32 width;
+
         writel_relaxed(UINT_MAX, timer_of_base(to) + TIM_ARR);
 
-        return readl_relaxed(timer_of_base(to) + TIM_ARR);
+        width = readl_relaxed(timer_of_base(to) + TIM_ARR);
+
+        stm32_timer_of_bits_set(to, width == UINT_MAX ? 32 : 16);
 }
 
-static void __init stm32_clockevent_init(struct timer_of *to)
+/**
+ * stm32_timer_set_prescaler - Compute and set the prescaler register
+ * @to: a pointer to a timer-of structure
+ *
+ * Depending on the timer width, compute the prescaler to always
+ * target a 10MHz timer rate for 16 bits. 32-bit timers are
+ * considered precise and long enough to not use the prescaler.
+ */
+static void __init stm32_timer_set_prescaler(struct timer_of *to)
 {
-        u32 width = 0;
-        int prescaler;
+        int prescaler = 1;
 
-        to->clkevt.name = to->np->full_name;
-        to->clkevt.features = CLOCK_EVT_FEAT_PERIODIC;
-        to->clkevt.features = CLOCK_EVT_FEAT_PERIODIC | CLOCK_EVT_FEAT_ONESHOT;
-        to->clkevt.set_state_shutdown = stm32_clock_event_shutdown;
-        to->clkevt.set_state_periodic = stm32_clock_event_set_periodic;
-        to->clkevt.set_state_oneshot = stm32_clock_event_set_oneshot;
-        to->clkevt.tick_resume = stm32_clock_event_shutdown;
-        to->clkevt.set_next_event = stm32_clock_event_set_next_event;
-
-        width = stm32_timer_width(to);
-        if (width == UINT_MAX) {
-                prescaler = 1;
-                to->clkevt.rating = 250;
-        } else {
+        if (stm32_timer_of_bits_get(to) != 32) {
                 prescaler = DIV_ROUND_CLOSEST(timer_of_rate(to),
                                               TIM_PSC_CLKRATE);
                 /*
@@ -161,7 +193,6 @@ static void __init stm32_clockevent_init(struct timer_of *to)
                  * this case.
                  */
                 prescaler = prescaler < TIM_PSC_MAX ? prescaler : TIM_PSC_MAX;
-                to->clkevt.rating = 100;
         }
 
         writel_relaxed(prescaler - 1, timer_of_base(to) + TIM_PSC);
@@ -171,12 +202,26 @@ static void __init stm32_clockevent_init(struct timer_of *to)
         /* Adjust rate and period given the prescaler value */
         to->of_clk.rate = DIV_ROUND_CLOSEST(to->of_clk.rate, prescaler);
         to->of_clk.period = DIV_ROUND_UP(to->of_clk.rate, HZ);
+}
+
+static void __init stm32_clockevent_init(struct timer_of *to)
+{
+        u32 bits = stm32_timer_of_bits_get(to);
 
-        clockevents_config_and_register(&to->clkevt,
-                                        timer_of_rate(to), 0x1, width);
+        to->clkevt.name = to->np->full_name;
+        to->clkevt.features = CLOCK_EVT_FEAT_PERIODIC | CLOCK_EVT_FEAT_ONESHOT;
+        to->clkevt.set_state_shutdown = stm32_clock_event_shutdown;
+        to->clkevt.set_state_periodic = stm32_clock_event_set_periodic;
+        to->clkevt.set_state_oneshot = stm32_clock_event_set_oneshot;
+        to->clkevt.tick_resume = stm32_clock_event_shutdown;
+        to->clkevt.set_next_event = stm32_clock_event_set_next_event;
+        to->clkevt.rating = bits == 32 ? 250 : 100;
+
+        clockevents_config_and_register(&to->clkevt, timer_of_rate(to), 0x1,
+                                        (1 <<  bits) - 1);
 
         pr_info("%pOF: STM32 clockevent driver initialized (%d bits)\n",
-                to->np, width == UINT_MAX ? 32 : 16);
+                to->np, bits);
 }
 
 static int __init stm32_timer_init(struct device_node *node)
@@ -196,14 +241,26 @@ static int __init stm32_timer_init(struct device_node *node)
         if (ret)
                 goto err;
 
+        to->private_data = kzalloc(sizeof(struct stm32_timer_private),
+                                   GFP_KERNEL);
+        if (!to->private_data)
+                goto deinit;
+
         rstc = of_reset_control_get(node, NULL);
         if (!IS_ERR(rstc)) {
                 reset_control_assert(rstc);
                 reset_control_deassert(rstc);
         }
 
+        stm32_timer_set_width(to);
+
+        stm32_timer_set_prescaler(to);
+
         stm32_clockevent_init(to);
         return 0;
+
+deinit:
+        timer_of_cleanup(to);
 err:
         kfree(to);
         return ret;