clk-vco-pll.c

  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 */
 66static 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
 81static 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
113static int clk_pll_determine_rate(struct clk_hw *hw,
114				  struct clk_rate_request *req)
115{
116	int unused;
117
118	req->rate = clk_pll_round_rate_index(hw, req->rate,
119					     &req->best_parent_rate, &unused);
120
121	return 0;
122}
123
124static unsigned long clk_pll_recalc_rate(struct clk_hw *hw, unsigned long
125		parent_rate)
126{
127	struct clk_pll *pll = to_clk_pll(hw);
128	unsigned long flags = 0;
129	unsigned int p;
130
131	if (pll->vco->lock)
132		spin_lock_irqsave(pll->vco->lock, flags);
133
134	p = readl_relaxed(pll->vco->cfg_reg);
135
136	if (pll->vco->lock)
137		spin_unlock_irqrestore(pll->vco->lock, flags);
138
139	p = (p >> PLL_DIV_P_SHIFT) & PLL_DIV_P_MASK;
140
141	return parent_rate / (1 << p);
142}
143
144static int clk_pll_set_rate(struct clk_hw *hw, unsigned long drate,
145				unsigned long prate)
146{
147	struct clk_pll *pll = to_clk_pll(hw);
148	struct pll_rate_tbl *rtbl = pll->vco->rtbl;
149	unsigned long flags = 0, val;
150	int i = 0;
151
152	clk_pll_round_rate_index(hw, drate, NULL, &i);
153
154	if (pll->vco->lock)
155		spin_lock_irqsave(pll->vco->lock, flags);
156
157	val = readl_relaxed(pll->vco->cfg_reg);
158	val &= ~(PLL_DIV_P_MASK << PLL_DIV_P_SHIFT);
159	val |= (rtbl[i].p & PLL_DIV_P_MASK) << PLL_DIV_P_SHIFT;
160	writel_relaxed(val, pll->vco->cfg_reg);
161
162	if (pll->vco->lock)
163		spin_unlock_irqrestore(pll->vco->lock, flags);
164
165	return 0;
166}
167
168static const struct clk_ops clk_pll_ops = {
169	.recalc_rate = clk_pll_recalc_rate,
170	.determine_rate = clk_pll_determine_rate,
171	.set_rate = clk_pll_set_rate,
172};
173
174static inline unsigned long vco_calc_rate(struct clk_hw *hw,
175		unsigned long prate, int index)
176{
177	struct clk_vco *vco = to_clk_vco(hw);
178
179	return pll_calc_rate(vco->rtbl, prate, index, NULL);
180}
181
182static int clk_vco_determine_rate(struct clk_hw *hw,
183				  struct clk_rate_request *req)
184{
185	struct clk_vco *vco = to_clk_vco(hw);
186	int unused;
187
188	req->rate = clk_round_rate_index(hw, req->rate, req->best_parent_rate,
189					 vco_calc_rate, vco->rtbl_cnt, &unused);
190
191	return 0;
192}
193
194static unsigned long clk_vco_recalc_rate(struct clk_hw *hw,
195		unsigned long parent_rate)
196{
197	struct clk_vco *vco = to_clk_vco(hw);
198	unsigned long flags = 0;
199	unsigned int num = 2, den = 0, val, mode = 0;
200
201	if (vco->lock)
202		spin_lock_irqsave(vco->lock, flags);
203
204	mode = (readl_relaxed(vco->mode_reg) >> PLL_MODE_SHIFT) & PLL_MODE_MASK;
205
206	val = readl_relaxed(vco->cfg_reg);
207
208	if (vco->lock)
209		spin_unlock_irqrestore(vco->lock, flags);
210
211	den = (val >> PLL_DIV_N_SHIFT) & PLL_DIV_N_MASK;
212
213	/* calculate numerator & denominator */
214	if (!mode) {
215		/* Normal mode */
216		num *= (val >> PLL_NORM_FDBK_M_SHIFT) & PLL_NORM_FDBK_M_MASK;
217	} else {
218		/* Dithered mode */
219		num *= (val >> PLL_DITH_FDBK_M_SHIFT) & PLL_DITH_FDBK_M_MASK;
220		den *= 256;
221	}
222
223	if (!den) {
224		WARN(1, "%s: denominator can't be zero\n", __func__);
225		return 0;
226	}
227
228	return (((parent_rate / 10000) * num) / den) * 10000;
229}
230
231/* Configures new clock rate of vco */
232static int clk_vco_set_rate(struct clk_hw *hw, unsigned long drate,
233				unsigned long prate)
234{
235	struct clk_vco *vco = to_clk_vco(hw);
236	struct pll_rate_tbl *rtbl = vco->rtbl;
237	unsigned long flags = 0, val;
238	int i;
239
240	clk_round_rate_index(hw, drate, prate, vco_calc_rate, vco->rtbl_cnt,
241			&i);
242
243	if (vco->lock)
244		spin_lock_irqsave(vco->lock, flags);
245
246	val = readl_relaxed(vco->mode_reg);
247	val &= ~(PLL_MODE_MASK << PLL_MODE_SHIFT);
248	val |= (rtbl[i].mode & PLL_MODE_MASK) << PLL_MODE_SHIFT;
249	writel_relaxed(val, vco->mode_reg);
250
251	val = readl_relaxed(vco->cfg_reg);
252	val &= ~(PLL_DIV_N_MASK << PLL_DIV_N_SHIFT);
253	val |= (rtbl[i].n & PLL_DIV_N_MASK) << PLL_DIV_N_SHIFT;
254
255	val &= ~(PLL_DITH_FDBK_M_MASK << PLL_DITH_FDBK_M_SHIFT);
256	if (rtbl[i].mode)
257		val |= (rtbl[i].m & PLL_DITH_FDBK_M_MASK) <<
258			PLL_DITH_FDBK_M_SHIFT;
259	else
260		val |= (rtbl[i].m & PLL_NORM_FDBK_M_MASK) <<
261			PLL_NORM_FDBK_M_SHIFT;
262
263	writel_relaxed(val, vco->cfg_reg);
264
265	if (vco->lock)
266		spin_unlock_irqrestore(vco->lock, flags);
267
268	return 0;
269}
270
271static const struct clk_ops clk_vco_ops = {
272	.recalc_rate = clk_vco_recalc_rate,
273	.determine_rate = clk_vco_determine_rate,
274	.set_rate = clk_vco_set_rate,
275};
276
277struct clk *clk_register_vco_pll(const char *vco_name, const char *pll_name,
278		const char *vco_gate_name, const char *parent_name,
279		unsigned long flags, void __iomem *mode_reg, void __iomem
280		*cfg_reg, struct pll_rate_tbl *rtbl, u8 rtbl_cnt,
281		spinlock_t *lock, struct clk **pll_clk,
282		struct clk **vco_gate_clk)
283{
284	struct clk_vco *vco;
285	struct clk_pll *pll;
286	struct clk *vco_clk, *tpll_clk, *tvco_gate_clk;
287	struct clk_init_data vco_init, pll_init;
288	const char **vco_parent_name;
289
290	if (!vco_name || !pll_name || !parent_name || !mode_reg || !cfg_reg ||
291			!rtbl || !rtbl_cnt) {
292		pr_err("Invalid arguments passed");
293		return ERR_PTR(-EINVAL);
294	}
295
296	vco = kzalloc(sizeof(*vco), GFP_KERNEL);
297	if (!vco)
298		return ERR_PTR(-ENOMEM);
299
300	pll = kzalloc(sizeof(*pll), GFP_KERNEL);
301	if (!pll)
302		goto free_vco;
303
304	/* struct clk_vco assignments */
305	vco->mode_reg = mode_reg;
306	vco->cfg_reg = cfg_reg;
307	vco->rtbl = rtbl;
308	vco->rtbl_cnt = rtbl_cnt;
309	vco->lock = lock;
310	vco->hw.init = &vco_init;
311
312	pll->vco = vco;
313	pll->hw.init = &pll_init;
314
315	if (vco_gate_name) {
316		tvco_gate_clk = clk_register_gate(NULL, vco_gate_name,
317				parent_name, 0, mode_reg, PLL_ENABLE, 0, lock);
318		if (IS_ERR_OR_NULL(tvco_gate_clk))
319			goto free_pll;
320
321		if (vco_gate_clk)
322			*vco_gate_clk = tvco_gate_clk;
323		vco_parent_name = &vco_gate_name;
324	} else {
325		vco_parent_name = &parent_name;
326	}
327
328	vco_init.name = vco_name;
329	vco_init.ops = &clk_vco_ops;
330	vco_init.flags = flags;
331	vco_init.parent_names = vco_parent_name;
332	vco_init.num_parents = 1;
333
334	pll_init.name = pll_name;
335	pll_init.ops = &clk_pll_ops;
336	pll_init.flags = CLK_SET_RATE_PARENT;
337	pll_init.parent_names = &vco_name;
338	pll_init.num_parents = 1;
339
340	vco_clk = clk_register(NULL, &vco->hw);
341	if (IS_ERR_OR_NULL(vco_clk))
342		goto free_pll;
343
344	tpll_clk = clk_register(NULL, &pll->hw);
345	if (IS_ERR_OR_NULL(tpll_clk))
346		goto free_pll;
347
348	if (pll_clk)
349		*pll_clk = tpll_clk;
350
351	return vco_clk;
352
353free_pll:
354	kfree(pll);
355free_vco:
356	kfree(vco);
357
358	pr_err("Failed to register vco pll clock\n");
359
360	return ERR_PTR(-ENOMEM);
361}