1 | // SPDX-License-Identifier: GPL-2.0-only |
2 | /* |
3 | * Copyright (C) 2012 ST Microelectronics |
4 | * Viresh Kumar <vireshk@kernel.org> |
5 | * |
6 | * VCO-PLL clock implementation |
7 | */ |
8 | |
9 | #define pr_fmt(fmt) "clk-vco-pll: " fmt |
10 | |
11 | #include <linux/clk-provider.h> |
12 | #include <linux/slab.h> |
13 | #include <linux/io.h> |
14 | #include <linux/err.h> |
15 | #include "clk.h" |
16 | |
17 | /* |
18 | * DOC: VCO-PLL clock |
19 | * |
20 | * VCO and PLL rate are derived from following equations: |
21 | * |
22 | * In normal mode |
23 | * vco = (2 * M[15:8] * Fin)/N |
24 | * |
25 | * In Dithered mode |
26 | * vco = (2 * M[15:0] * Fin)/(256 * N) |
27 | * |
28 | * pll_rate = pll/2^p |
29 | * |
30 | * vco and pll are very closely bound to each other, "vco needs to program: |
31 | * mode, m & n" and "pll needs to program p", both share common enable/disable |
32 | * logic. |
33 | * |
34 | * clk_register_vco_pll() registers instances of both vco & pll. |
35 | * CLK_SET_RATE_PARENT flag is forced for pll, as it will always pass its |
36 | * set_rate to vco. A single rate table exists for both the clocks, which |
37 | * configures m, n and p. |
38 | */ |
39 | |
40 | /* PLL_CTR register masks */ |
41 | #define PLL_MODE_NORMAL 0 |
42 | #define PLL_MODE_FRACTION 1 |
43 | #define PLL_MODE_DITH_DSM 2 |
44 | #define PLL_MODE_DITH_SSM 3 |
45 | #define PLL_MODE_MASK 3 |
46 | #define PLL_MODE_SHIFT 3 |
47 | #define PLL_ENABLE 2 |
48 | |
49 | #define PLL_LOCK_SHIFT 0 |
50 | #define PLL_LOCK_MASK 1 |
51 | |
52 | /* PLL FRQ register masks */ |
53 | #define PLL_NORM_FDBK_M_MASK 0xFF |
54 | #define PLL_NORM_FDBK_M_SHIFT 24 |
55 | #define PLL_DITH_FDBK_M_MASK 0xFFFF |
56 | #define PLL_DITH_FDBK_M_SHIFT 16 |
57 | #define PLL_DIV_P_MASK 0x7 |
58 | #define PLL_DIV_P_SHIFT 8 |
59 | #define PLL_DIV_N_MASK 0xFF |
60 | #define PLL_DIV_N_SHIFT 0 |
61 | |
62 | #define to_clk_vco(_hw) container_of(_hw, struct clk_vco, hw) |
63 | #define to_clk_pll(_hw) container_of(_hw, struct clk_pll, hw) |
64 | |
65 | /* Calculates pll clk rate for specific value of mode, m, n and p */ |
66 | static unsigned long pll_calc_rate(struct pll_rate_tbl *rtbl, |
67 | unsigned long prate, int index, unsigned long *pll_rate) |
68 | { |
69 | unsigned long rate = prate; |
70 | unsigned int mode; |
71 | |
72 | mode = rtbl[index].mode ? 256 : 1; |
73 | rate = (((2 * rate / 10000) * rtbl[index].m) / (mode * rtbl[index].n)); |
74 | |
75 | if (pll_rate) |
76 | *pll_rate = (rate / (1 << rtbl[index].p)) * 10000; |
77 | |
78 | return rate * 10000; |
79 | } |
80 | |
81 | static long clk_pll_round_rate_index(struct clk_hw *hw, unsigned long drate, |
82 | unsigned long *prate, int *index) |
83 | { |
84 | struct clk_pll *pll = to_clk_pll(hw); |
85 | unsigned long prev_rate, vco_prev_rate, rate = 0; |
86 | unsigned long vco_parent_rate = |
87 | clk_hw_get_rate(hw: clk_hw_get_parent(hw: clk_hw_get_parent(hw))); |
88 | |
89 | if (!prate) { |
90 | pr_err("%s: prate is must for pll clk\n" , __func__); |
91 | return -EINVAL; |
92 | } |
93 | |
94 | for (*index = 0; *index < pll->vco->rtbl_cnt; (*index)++) { |
95 | prev_rate = rate; |
96 | vco_prev_rate = *prate; |
97 | *prate = pll_calc_rate(rtbl: pll->vco->rtbl, prate: vco_parent_rate, index: *index, |
98 | pll_rate: &rate); |
99 | if (drate < rate) { |
100 | /* previous clock was best */ |
101 | if (*index) { |
102 | rate = prev_rate; |
103 | *prate = vco_prev_rate; |
104 | (*index)--; |
105 | } |
106 | break; |
107 | } |
108 | } |
109 | |
110 | return rate; |
111 | } |
112 | |
113 | static long clk_pll_round_rate(struct clk_hw *hw, unsigned long drate, |
114 | unsigned long *prate) |
115 | { |
116 | int unused; |
117 | |
118 | return clk_pll_round_rate_index(hw, drate, prate, index: &unused); |
119 | } |
120 | |
121 | static unsigned long clk_pll_recalc_rate(struct clk_hw *hw, unsigned long |
122 | parent_rate) |
123 | { |
124 | struct clk_pll *pll = to_clk_pll(hw); |
125 | unsigned long flags = 0; |
126 | unsigned int p; |
127 | |
128 | if (pll->vco->lock) |
129 | spin_lock_irqsave(pll->vco->lock, flags); |
130 | |
131 | p = readl_relaxed(pll->vco->cfg_reg); |
132 | |
133 | if (pll->vco->lock) |
134 | spin_unlock_irqrestore(lock: pll->vco->lock, flags); |
135 | |
136 | p = (p >> PLL_DIV_P_SHIFT) & PLL_DIV_P_MASK; |
137 | |
138 | return parent_rate / (1 << p); |
139 | } |
140 | |
141 | static int clk_pll_set_rate(struct clk_hw *hw, unsigned long drate, |
142 | unsigned long prate) |
143 | { |
144 | struct clk_pll *pll = to_clk_pll(hw); |
145 | struct pll_rate_tbl *rtbl = pll->vco->rtbl; |
146 | unsigned long flags = 0, val; |
147 | int i = 0; |
148 | |
149 | clk_pll_round_rate_index(hw, drate, NULL, index: &i); |
150 | |
151 | if (pll->vco->lock) |
152 | spin_lock_irqsave(pll->vco->lock, flags); |
153 | |
154 | val = readl_relaxed(pll->vco->cfg_reg); |
155 | val &= ~(PLL_DIV_P_MASK << PLL_DIV_P_SHIFT); |
156 | val |= (rtbl[i].p & PLL_DIV_P_MASK) << PLL_DIV_P_SHIFT; |
157 | writel_relaxed(val, pll->vco->cfg_reg); |
158 | |
159 | if (pll->vco->lock) |
160 | spin_unlock_irqrestore(lock: pll->vco->lock, flags); |
161 | |
162 | return 0; |
163 | } |
164 | |
165 | static const struct clk_ops clk_pll_ops = { |
166 | .recalc_rate = clk_pll_recalc_rate, |
167 | .round_rate = clk_pll_round_rate, |
168 | .set_rate = clk_pll_set_rate, |
169 | }; |
170 | |
171 | static inline unsigned long vco_calc_rate(struct clk_hw *hw, |
172 | unsigned long prate, int index) |
173 | { |
174 | struct clk_vco *vco = to_clk_vco(hw); |
175 | |
176 | return pll_calc_rate(rtbl: vco->rtbl, prate, index, NULL); |
177 | } |
178 | |
179 | static long clk_vco_round_rate(struct clk_hw *hw, unsigned long drate, |
180 | unsigned long *prate) |
181 | { |
182 | struct clk_vco *vco = to_clk_vco(hw); |
183 | int unused; |
184 | |
185 | return clk_round_rate_index(hw, drate, parent_rate: *prate, calc_rate: vco_calc_rate, |
186 | rtbl_cnt: vco->rtbl_cnt, index: &unused); |
187 | } |
188 | |
189 | static unsigned long clk_vco_recalc_rate(struct clk_hw *hw, |
190 | unsigned long parent_rate) |
191 | { |
192 | struct clk_vco *vco = to_clk_vco(hw); |
193 | unsigned long flags = 0; |
194 | unsigned int num = 2, den = 0, val, mode = 0; |
195 | |
196 | if (vco->lock) |
197 | spin_lock_irqsave(vco->lock, flags); |
198 | |
199 | mode = (readl_relaxed(vco->mode_reg) >> PLL_MODE_SHIFT) & PLL_MODE_MASK; |
200 | |
201 | val = readl_relaxed(vco->cfg_reg); |
202 | |
203 | if (vco->lock) |
204 | spin_unlock_irqrestore(lock: vco->lock, flags); |
205 | |
206 | den = (val >> PLL_DIV_N_SHIFT) & PLL_DIV_N_MASK; |
207 | |
208 | /* calculate numerator & denominator */ |
209 | if (!mode) { |
210 | /* Normal mode */ |
211 | num *= (val >> PLL_NORM_FDBK_M_SHIFT) & PLL_NORM_FDBK_M_MASK; |
212 | } else { |
213 | /* Dithered mode */ |
214 | num *= (val >> PLL_DITH_FDBK_M_SHIFT) & PLL_DITH_FDBK_M_MASK; |
215 | den *= 256; |
216 | } |
217 | |
218 | if (!den) { |
219 | WARN(1, "%s: denominator can't be zero\n" , __func__); |
220 | return 0; |
221 | } |
222 | |
223 | return (((parent_rate / 10000) * num) / den) * 10000; |
224 | } |
225 | |
226 | /* Configures new clock rate of vco */ |
227 | static int clk_vco_set_rate(struct clk_hw *hw, unsigned long drate, |
228 | unsigned long prate) |
229 | { |
230 | struct clk_vco *vco = to_clk_vco(hw); |
231 | struct pll_rate_tbl *rtbl = vco->rtbl; |
232 | unsigned long flags = 0, val; |
233 | int i; |
234 | |
235 | clk_round_rate_index(hw, drate, parent_rate: prate, calc_rate: vco_calc_rate, rtbl_cnt: vco->rtbl_cnt, |
236 | index: &i); |
237 | |
238 | if (vco->lock) |
239 | spin_lock_irqsave(vco->lock, flags); |
240 | |
241 | val = readl_relaxed(vco->mode_reg); |
242 | val &= ~(PLL_MODE_MASK << PLL_MODE_SHIFT); |
243 | val |= (rtbl[i].mode & PLL_MODE_MASK) << PLL_MODE_SHIFT; |
244 | writel_relaxed(val, vco->mode_reg); |
245 | |
246 | val = readl_relaxed(vco->cfg_reg); |
247 | val &= ~(PLL_DIV_N_MASK << PLL_DIV_N_SHIFT); |
248 | val |= (rtbl[i].n & PLL_DIV_N_MASK) << PLL_DIV_N_SHIFT; |
249 | |
250 | val &= ~(PLL_DITH_FDBK_M_MASK << PLL_DITH_FDBK_M_SHIFT); |
251 | if (rtbl[i].mode) |
252 | val |= (rtbl[i].m & PLL_DITH_FDBK_M_MASK) << |
253 | PLL_DITH_FDBK_M_SHIFT; |
254 | else |
255 | val |= (rtbl[i].m & PLL_NORM_FDBK_M_MASK) << |
256 | PLL_NORM_FDBK_M_SHIFT; |
257 | |
258 | writel_relaxed(val, vco->cfg_reg); |
259 | |
260 | if (vco->lock) |
261 | spin_unlock_irqrestore(lock: vco->lock, flags); |
262 | |
263 | return 0; |
264 | } |
265 | |
266 | static const struct clk_ops clk_vco_ops = { |
267 | .recalc_rate = clk_vco_recalc_rate, |
268 | .round_rate = clk_vco_round_rate, |
269 | .set_rate = clk_vco_set_rate, |
270 | }; |
271 | |
272 | struct clk *clk_register_vco_pll(const char *vco_name, const char *pll_name, |
273 | const char *vco_gate_name, const char *parent_name, |
274 | unsigned long flags, void __iomem *mode_reg, void __iomem |
275 | *cfg_reg, struct pll_rate_tbl *rtbl, u8 rtbl_cnt, |
276 | spinlock_t *lock, struct clk **pll_clk, |
277 | struct clk **vco_gate_clk) |
278 | { |
279 | struct clk_vco *vco; |
280 | struct clk_pll *pll; |
281 | struct clk *vco_clk, *tpll_clk, *tvco_gate_clk; |
282 | struct clk_init_data vco_init, pll_init; |
283 | const char **vco_parent_name; |
284 | |
285 | if (!vco_name || !pll_name || !parent_name || !mode_reg || !cfg_reg || |
286 | !rtbl || !rtbl_cnt) { |
287 | pr_err("Invalid arguments passed" ); |
288 | return ERR_PTR(error: -EINVAL); |
289 | } |
290 | |
291 | vco = kzalloc(size: sizeof(*vco), GFP_KERNEL); |
292 | if (!vco) |
293 | return ERR_PTR(error: -ENOMEM); |
294 | |
295 | pll = kzalloc(size: sizeof(*pll), GFP_KERNEL); |
296 | if (!pll) |
297 | goto free_vco; |
298 | |
299 | /* struct clk_vco assignments */ |
300 | vco->mode_reg = mode_reg; |
301 | vco->cfg_reg = cfg_reg; |
302 | vco->rtbl = rtbl; |
303 | vco->rtbl_cnt = rtbl_cnt; |
304 | vco->lock = lock; |
305 | vco->hw.init = &vco_init; |
306 | |
307 | pll->vco = vco; |
308 | pll->hw.init = &pll_init; |
309 | |
310 | if (vco_gate_name) { |
311 | tvco_gate_clk = clk_register_gate(NULL, name: vco_gate_name, |
312 | parent_name, flags: 0, reg: mode_reg, PLL_ENABLE, clk_gate_flags: 0, lock); |
313 | if (IS_ERR_OR_NULL(ptr: tvco_gate_clk)) |
314 | goto free_pll; |
315 | |
316 | if (vco_gate_clk) |
317 | *vco_gate_clk = tvco_gate_clk; |
318 | vco_parent_name = &vco_gate_name; |
319 | } else { |
320 | vco_parent_name = &parent_name; |
321 | } |
322 | |
323 | vco_init.name = vco_name; |
324 | vco_init.ops = &clk_vco_ops; |
325 | vco_init.flags = flags; |
326 | vco_init.parent_names = vco_parent_name; |
327 | vco_init.num_parents = 1; |
328 | |
329 | pll_init.name = pll_name; |
330 | pll_init.ops = &clk_pll_ops; |
331 | pll_init.flags = CLK_SET_RATE_PARENT; |
332 | pll_init.parent_names = &vco_name; |
333 | pll_init.num_parents = 1; |
334 | |
335 | vco_clk = clk_register(NULL, hw: &vco->hw); |
336 | if (IS_ERR_OR_NULL(ptr: vco_clk)) |
337 | goto free_pll; |
338 | |
339 | tpll_clk = clk_register(NULL, hw: &pll->hw); |
340 | if (IS_ERR_OR_NULL(ptr: tpll_clk)) |
341 | goto free_pll; |
342 | |
343 | if (pll_clk) |
344 | *pll_clk = tpll_clk; |
345 | |
346 | return vco_clk; |
347 | |
348 | free_pll: |
349 | kfree(objp: pll); |
350 | free_vco: |
351 | kfree(objp: vco); |
352 | |
353 | pr_err("Failed to register vco pll clock\n" ); |
354 | |
355 | return ERR_PTR(error: -ENOMEM); |
356 | } |
357 | |