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 */
pll_calc_rate(struct pll_rate_tbl * rtbl,unsigned long prate,int index,unsigned long * pll_rate)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 
clk_pll_round_rate_index(struct clk_hw * hw,unsigned long drate,unsigned long * prate,int * index)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(clk_hw_get_parent(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(pll->vco->rtbl, vco_parent_rate, *index,
98 				&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 
clk_pll_round_rate(struct clk_hw * hw,unsigned long drate,unsigned long * prate)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, &unused);
119 }
120 
clk_pll_recalc_rate(struct clk_hw * hw,unsigned long parent_rate)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(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 
clk_pll_set_rate(struct clk_hw * hw,unsigned long drate,unsigned long prate)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, &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(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 
vco_calc_rate(struct clk_hw * hw,unsigned long prate,int index)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(vco->rtbl, prate, index, NULL);
177 }
178 
clk_vco_round_rate(struct clk_hw * hw,unsigned long drate,unsigned long * prate)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, *prate, vco_calc_rate,
186 			vco->rtbl_cnt, &unused);
187 }
188 
clk_vco_recalc_rate(struct clk_hw * hw,unsigned long parent_rate)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(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 */
clk_vco_set_rate(struct clk_hw * hw,unsigned long drate,unsigned long prate)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, prate, vco_calc_rate, vco->rtbl_cnt,
236 			&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(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 
clk_register_vco_pll(const char * vco_name,const char * pll_name,const char * vco_gate_name,const char * parent_name,unsigned long flags,void __iomem * mode_reg,void __iomem * cfg_reg,struct pll_rate_tbl * rtbl,u8 rtbl_cnt,spinlock_t * lock,struct clk ** pll_clk,struct clk ** vco_gate_clk)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(-EINVAL);
289 	}
290 
291 	vco = kzalloc(sizeof(*vco), GFP_KERNEL);
292 	if (!vco)
293 		return ERR_PTR(-ENOMEM);
294 
295 	pll = kzalloc(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, vco_gate_name,
312 				parent_name, 0, mode_reg, PLL_ENABLE, 0, lock);
313 		if (IS_ERR_OR_NULL(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, &vco->hw);
336 	if (IS_ERR_OR_NULL(vco_clk))
337 		goto free_pll;
338 
339 	tpll_clk = clk_register(NULL, &pll->hw);
340 	if (IS_ERR_OR_NULL(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(pll);
350 free_vco:
351 	kfree(vco);
352 
353 	pr_err("Failed to register vco pll clock\n");
354 
355 	return ERR_PTR(-ENOMEM);
356 }
357