Merge tag 'armsoc-drivers' of git://git.kernel.org/pub/scm/linux/kernel/git/arm/arm-soc
[linux-2.6-microblaze.git] / drivers / clk / clk-multiplier.c
1 // SPDX-License-Identifier: GPL-2.0
2 /*
3  * Copyright (C) 2015 Maxime Ripard <maxime.ripard@free-electrons.com>
4  */
5
6 #include <linux/bitops.h>
7 #include <linux/clk-provider.h>
8 #include <linux/err.h>
9 #include <linux/export.h>
10 #include <linux/kernel.h>
11 #include <linux/of.h>
12 #include <linux/slab.h>
13
14 static unsigned long __get_mult(struct clk_multiplier *mult,
15                                 unsigned long rate,
16                                 unsigned long parent_rate)
17 {
18         if (mult->flags & CLK_MULTIPLIER_ROUND_CLOSEST)
19                 return DIV_ROUND_CLOSEST(rate, parent_rate);
20
21         return rate / parent_rate;
22 }
23
24 static unsigned long clk_multiplier_recalc_rate(struct clk_hw *hw,
25                                                 unsigned long parent_rate)
26 {
27         struct clk_multiplier *mult = to_clk_multiplier(hw);
28         unsigned long val;
29
30         val = clk_readl(mult->reg) >> mult->shift;
31         val &= GENMASK(mult->width - 1, 0);
32
33         if (!val && mult->flags & CLK_MULTIPLIER_ZERO_BYPASS)
34                 val = 1;
35
36         return parent_rate * val;
37 }
38
39 static bool __is_best_rate(unsigned long rate, unsigned long new,
40                            unsigned long best, unsigned long flags)
41 {
42         if (flags & CLK_MULTIPLIER_ROUND_CLOSEST)
43                 return abs(rate - new) < abs(rate - best);
44
45         return new >= rate && new < best;
46 }
47
48 static unsigned long __bestmult(struct clk_hw *hw, unsigned long rate,
49                                 unsigned long *best_parent_rate,
50                                 u8 width, unsigned long flags)
51 {
52         struct clk_multiplier *mult = to_clk_multiplier(hw);
53         unsigned long orig_parent_rate = *best_parent_rate;
54         unsigned long parent_rate, current_rate, best_rate = ~0;
55         unsigned int i, bestmult = 0;
56         unsigned int maxmult = (1 << width) - 1;
57
58         if (!(clk_hw_get_flags(hw) & CLK_SET_RATE_PARENT)) {
59                 bestmult = rate / orig_parent_rate;
60
61                 /* Make sure we don't end up with a 0 multiplier */
62                 if ((bestmult == 0) &&
63                     !(mult->flags & CLK_MULTIPLIER_ZERO_BYPASS))
64                         bestmult = 1;
65
66                 /* Make sure we don't overflow the multiplier */
67                 if (bestmult > maxmult)
68                         bestmult = maxmult;
69
70                 return bestmult;
71         }
72
73         for (i = 1; i < maxmult; i++) {
74                 if (rate == orig_parent_rate * i) {
75                         /*
76                          * This is the best case for us if we have a
77                          * perfect match without changing the parent
78                          * rate.
79                          */
80                         *best_parent_rate = orig_parent_rate;
81                         return i;
82                 }
83
84                 parent_rate = clk_hw_round_rate(clk_hw_get_parent(hw),
85                                                 rate / i);
86                 current_rate = parent_rate * i;
87
88                 if (__is_best_rate(rate, current_rate, best_rate, flags)) {
89                         bestmult = i;
90                         best_rate = current_rate;
91                         *best_parent_rate = parent_rate;
92                 }
93         }
94
95         return bestmult;
96 }
97
98 static long clk_multiplier_round_rate(struct clk_hw *hw, unsigned long rate,
99                                   unsigned long *parent_rate)
100 {
101         struct clk_multiplier *mult = to_clk_multiplier(hw);
102         unsigned long factor = __bestmult(hw, rate, parent_rate,
103                                           mult->width, mult->flags);
104
105         return *parent_rate * factor;
106 }
107
108 static int clk_multiplier_set_rate(struct clk_hw *hw, unsigned long rate,
109                                unsigned long parent_rate)
110 {
111         struct clk_multiplier *mult = to_clk_multiplier(hw);
112         unsigned long factor = __get_mult(mult, rate, parent_rate);
113         unsigned long flags = 0;
114         unsigned long val;
115
116         if (mult->lock)
117                 spin_lock_irqsave(mult->lock, flags);
118         else
119                 __acquire(mult->lock);
120
121         val = clk_readl(mult->reg);
122         val &= ~GENMASK(mult->width + mult->shift - 1, mult->shift);
123         val |= factor << mult->shift;
124         clk_writel(val, mult->reg);
125
126         if (mult->lock)
127                 spin_unlock_irqrestore(mult->lock, flags);
128         else
129                 __release(mult->lock);
130
131         return 0;
132 }
133
134 const struct clk_ops clk_multiplier_ops = {
135         .recalc_rate    = clk_multiplier_recalc_rate,
136         .round_rate     = clk_multiplier_round_rate,
137         .set_rate       = clk_multiplier_set_rate,
138 };
139 EXPORT_SYMBOL_GPL(clk_multiplier_ops);