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