1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41#include <linux/clk-provider.h>
42#include <linux/io.h>
43#include <linux/module.h>
44#include <linux/device.h>
45#include <linux/slab.h>
46#include <linux/rational.h>
47
48#include "clk-fractional-divider.h"
49
50static inline u32 clk_fd_readl(struct clk_fractional_divider *fd)
51{
52 if (fd->flags & CLK_FRAC_DIVIDER_BIG_ENDIAN)
53 return ioread32be(fd->reg);
54
55 return readl(fd->reg);
56}
57
58static inline void clk_fd_writel(struct clk_fractional_divider *fd, u32 val)
59{
60 if (fd->flags & CLK_FRAC_DIVIDER_BIG_ENDIAN)
61 iowrite32be(val, fd->reg);
62 else
63 writel(val, fd->reg);
64}
65
66static unsigned long clk_fd_recalc_rate(struct clk_hw *hw,
67 unsigned long parent_rate)
68{
69 struct clk_fractional_divider *fd = to_clk_fd(hw);
70 unsigned long flags = 0;
71 unsigned long m, n;
72 u32 val;
73 u64 ret;
74
75 if (fd->lock)
76 spin_lock_irqsave(fd->lock, flags);
77 else
78 __acquire(fd->lock);
79
80 val = clk_fd_readl(fd);
81
82 if (fd->lock)
83 spin_unlock_irqrestore(fd->lock, flags);
84 else
85 __release(fd->lock);
86
87 m = (val & fd->mmask) >> fd->mshift;
88 n = (val & fd->nmask) >> fd->nshift;
89
90 if (fd->flags & CLK_FRAC_DIVIDER_ZERO_BASED) {
91 m++;
92 n++;
93 }
94
95 if (!n || !m)
96 return parent_rate;
97
98 ret = (u64)parent_rate * m;
99 do_div(ret, n);
100
101 return ret;
102}
103
104void clk_fractional_divider_general_approximation(struct clk_hw *hw,
105 unsigned long rate,
106 unsigned long *parent_rate,
107 unsigned long *m, unsigned long *n)
108{
109 struct clk_fractional_divider *fd = to_clk_fd(hw);
110
111
112
113
114
115
116
117
118 if (fd->flags & CLK_FRAC_DIVIDER_POWER_OF_TWO_PS) {
119 unsigned long scale = fls_long(*parent_rate / rate - 1);
120
121 if (scale > fd->nwidth)
122 rate <<= scale - fd->nwidth;
123 }
124
125 rational_best_approximation(rate, *parent_rate,
126 GENMASK(fd->mwidth - 1, 0), GENMASK(fd->nwidth - 1, 0),
127 m, n);
128}
129
130static long clk_fd_round_rate(struct clk_hw *hw, unsigned long rate,
131 unsigned long *parent_rate)
132{
133 struct clk_fractional_divider *fd = to_clk_fd(hw);
134 unsigned long m, n;
135 u64 ret;
136
137 if (!rate || (!clk_hw_can_set_rate_parent(hw) && rate >= *parent_rate))
138 return *parent_rate;
139
140 if (fd->approximation)
141 fd->approximation(hw, rate, parent_rate, &m, &n);
142 else
143 clk_fractional_divider_general_approximation(hw, rate, parent_rate, &m, &n);
144
145 ret = (u64)*parent_rate * m;
146 do_div(ret, n);
147
148 return ret;
149}
150
151static int clk_fd_set_rate(struct clk_hw *hw, unsigned long rate,
152 unsigned long parent_rate)
153{
154 struct clk_fractional_divider *fd = to_clk_fd(hw);
155 unsigned long flags = 0;
156 unsigned long m, n;
157 u32 val;
158
159 rational_best_approximation(rate, parent_rate,
160 GENMASK(fd->mwidth - 1, 0), GENMASK(fd->nwidth - 1, 0),
161 &m, &n);
162
163 if (fd->flags & CLK_FRAC_DIVIDER_ZERO_BASED) {
164 m--;
165 n--;
166 }
167
168 if (fd->lock)
169 spin_lock_irqsave(fd->lock, flags);
170 else
171 __acquire(fd->lock);
172
173 val = clk_fd_readl(fd);
174 val &= ~(fd->mmask | fd->nmask);
175 val |= (m << fd->mshift) | (n << fd->nshift);
176 clk_fd_writel(fd, val);
177
178 if (fd->lock)
179 spin_unlock_irqrestore(fd->lock, flags);
180 else
181 __release(fd->lock);
182
183 return 0;
184}
185
186const struct clk_ops clk_fractional_divider_ops = {
187 .recalc_rate = clk_fd_recalc_rate,
188 .round_rate = clk_fd_round_rate,
189 .set_rate = clk_fd_set_rate,
190};
191EXPORT_SYMBOL_GPL(clk_fractional_divider_ops);
192
193struct clk_hw *clk_hw_register_fractional_divider(struct device *dev,
194 const char *name, const char *parent_name, unsigned long flags,
195 void __iomem *reg, u8 mshift, u8 mwidth, u8 nshift, u8 nwidth,
196 u8 clk_divider_flags, spinlock_t *lock)
197{
198 struct clk_fractional_divider *fd;
199 struct clk_init_data init;
200 struct clk_hw *hw;
201 int ret;
202
203 fd = kzalloc(sizeof(*fd), GFP_KERNEL);
204 if (!fd)
205 return ERR_PTR(-ENOMEM);
206
207 init.name = name;
208 init.ops = &clk_fractional_divider_ops;
209 init.flags = flags;
210 init.parent_names = parent_name ? &parent_name : NULL;
211 init.num_parents = parent_name ? 1 : 0;
212
213 fd->reg = reg;
214 fd->mshift = mshift;
215 fd->mwidth = mwidth;
216 fd->mmask = GENMASK(mwidth - 1, 0) << mshift;
217 fd->nshift = nshift;
218 fd->nwidth = nwidth;
219 fd->nmask = GENMASK(nwidth - 1, 0) << nshift;
220 fd->flags = clk_divider_flags;
221 fd->lock = lock;
222 fd->hw.init = &init;
223
224 hw = &fd->hw;
225 ret = clk_hw_register(dev, hw);
226 if (ret) {
227 kfree(fd);
228 hw = ERR_PTR(ret);
229 }
230
231 return hw;
232}
233EXPORT_SYMBOL_GPL(clk_hw_register_fractional_divider);
234
235struct clk *clk_register_fractional_divider(struct device *dev,
236 const char *name, const char *parent_name, unsigned long flags,
237 void __iomem *reg, u8 mshift, u8 mwidth, u8 nshift, u8 nwidth,
238 u8 clk_divider_flags, spinlock_t *lock)
239{
240 struct clk_hw *hw;
241
242 hw = clk_hw_register_fractional_divider(dev, name, parent_name, flags,
243 reg, mshift, mwidth, nshift, nwidth, clk_divider_flags,
244 lock);
245 if (IS_ERR(hw))
246 return ERR_CAST(hw);
247 return hw->clk;
248}
249EXPORT_SYMBOL_GPL(clk_register_fractional_divider);
250
251void clk_hw_unregister_fractional_divider(struct clk_hw *hw)
252{
253 struct clk_fractional_divider *fd;
254
255 fd = to_clk_fd(hw);
256
257 clk_hw_unregister(hw);
258 kfree(fd);
259}
260