clk-sg2042-pll.c

  1// SPDX-License-Identifier: GPL-2.0
  2/*
  3 * Sophgo SG2042 PLL clock Driver
  4 *
  5 * Copyright (C) 2024 Sophgo Technology Inc.
  6 * Copyright (C) 2024 Chen Wang <unicorn_wang@outlook.com>
  7 */
  8
  9#include <linux/array_size.h>
 10#include <linux/bitfield.h>
 11#include <linux/bits.h>
 12#include <linux/clk-provider.h>
 13#include <linux/io.h>
 14#include <linux/iopoll.h>
 15#include <linux/platform_device.h>
 16#include <asm/div64.h>
 17
 18#include <dt-bindings/clock/sophgo,sg2042-pll.h>
 19
 20#include "clk-sg2042.h"
 21
 22/* Registers defined in SYS_CTRL */
 23#define R_PLL_BEGIN		0xC0
 24#define R_PLL_STAT		(0xC0 - R_PLL_BEGIN)
 25#define R_PLL_CLKEN_CONTROL	(0xC4 - R_PLL_BEGIN)
 26#define R_MPLL_CONTROL		(0xE8 - R_PLL_BEGIN)
 27#define R_FPLL_CONTROL		(0xF4 - R_PLL_BEGIN)
 28#define R_DPLL0_CONTROL		(0xF8 - R_PLL_BEGIN)
 29#define R_DPLL1_CONTROL		(0xFC - R_PLL_BEGIN)
 30
 31/**
 32 * struct sg2042_pll_clock - PLL clock
 33 * @hw:				clk_hw for initialization
 34 * @id:				used to map clk_onecell_data
 35 * @base:			used for readl/writel.
 36 *				**NOTE**: PLL registers are all in SYS_CTRL!
 37 * @lock:			spinlock to protect register access, modification
 38 *				of frequency can only be served one at the time.
 39 * @offset_ctrl:		offset of pll control registers
 40 * @shift_status_lock:		shift of XXX_LOCK in pll status register
 41 * @shift_status_updating:	shift of UPDATING_XXX in pll status register
 42 * @shift_enable:		shift of XXX_CLK_EN in pll enable register
 43 */
 44struct sg2042_pll_clock {
 45	struct clk_hw hw;
 46
 47	unsigned int id;
 48	void __iomem *base;
 49	/* protect register access */
 50	spinlock_t *lock;
 51
 52	u32 offset_ctrl;
 53	u8 shift_status_lock;
 54	u8 shift_status_updating;
 55	u8 shift_enable;
 56};
 57
 58#define to_sg2042_pll_clk(_hw) container_of(_hw, struct sg2042_pll_clock, hw)
 59
 60#define KHZ 1000UL
 61#define MHZ (KHZ * KHZ)
 62
 63#define REFDIV_MIN 1
 64#define REFDIV_MAX 63
 65#define FBDIV_MIN 16
 66#define FBDIV_MAX 320
 67
 68#define PLL_FREF_SG2042 (25 * MHZ)
 69
 70#define PLL_FOUTPOSTDIV_MIN (16 * MHZ)
 71#define PLL_FOUTPOSTDIV_MAX (3200 * MHZ)
 72
 73#define PLL_FOUTVCO_MIN (800 * MHZ)
 74#define PLL_FOUTVCO_MAX (3200 * MHZ)
 75
 76struct sg2042_pll_ctrl {
 77	unsigned long freq;
 78	unsigned int fbdiv;
 79	unsigned int postdiv1;
 80	unsigned int postdiv2;
 81	unsigned int refdiv;
 82};
 83
 84#define PLLCTRL_FBDIV_MASK	GENMASK(27, 16)
 85#define PLLCTRL_POSTDIV2_MASK	GENMASK(14, 12)
 86#define PLLCTRL_POSTDIV1_MASK	GENMASK(10, 8)
 87#define PLLCTRL_REFDIV_MASK	GENMASK(5, 0)
 88
 89static inline u32 sg2042_pll_ctrl_encode(struct sg2042_pll_ctrl *ctrl)
 90{
 91	return FIELD_PREP(PLLCTRL_FBDIV_MASK, ctrl->fbdiv) |
 92	       FIELD_PREP(PLLCTRL_POSTDIV2_MASK, ctrl->postdiv2) |
 93	       FIELD_PREP(PLLCTRL_POSTDIV1_MASK, ctrl->postdiv1) |
 94	       FIELD_PREP(PLLCTRL_REFDIV_MASK, ctrl->refdiv);
 95}
 96
 97static inline void sg2042_pll_ctrl_decode(unsigned int reg_value,
 98					  struct sg2042_pll_ctrl *ctrl)
 99{
100	ctrl->fbdiv = FIELD_GET(PLLCTRL_FBDIV_MASK, reg_value);
101	ctrl->refdiv = FIELD_GET(PLLCTRL_REFDIV_MASK, reg_value);
102	ctrl->postdiv1 = FIELD_GET(PLLCTRL_POSTDIV1_MASK, reg_value);
103	ctrl->postdiv2 = FIELD_GET(PLLCTRL_POSTDIV2_MASK, reg_value);
104}
105
106static inline void sg2042_pll_enable(struct sg2042_pll_clock *pll, bool en)
107{
108	u32 value;
109
110	if (en) {
111		/* wait pll lock */
112		if (readl_poll_timeout_atomic(pll->base + R_PLL_STAT,
113					      value,
114					      ((value >> pll->shift_status_lock) & 0x1),
115					      0,
116					      100000))
117			pr_warn("%s not locked\n", pll->hw.init->name);
118
119		/* wait pll updating */
120		if (readl_poll_timeout_atomic(pll->base + R_PLL_STAT,
121					      value,
122					      !((value >> pll->shift_status_updating) & 0x1),
123					      0,
124					      100000))
125			pr_warn("%s still updating\n", pll->hw.init->name);
126
127		/* enable pll */
128		value = readl(pll->base + R_PLL_CLKEN_CONTROL);
129		writel(value | (1 << pll->shift_enable), pll->base + R_PLL_CLKEN_CONTROL);
130	} else {
131		/* disable pll */
132		value = readl(pll->base + R_PLL_CLKEN_CONTROL);
133		writel(value & (~(1 << pll->shift_enable)), pll->base + R_PLL_CLKEN_CONTROL);
134	}
135}
136
137/**
138 * sg2042_pll_recalc_rate() - Calculate rate for plls
139 * @reg_value: current register value
140 * @parent_rate: parent frequency
141 *
142 * This function is used to calculate below "rate" in equation
143 * rate = (parent_rate/REFDIV) x FBDIV/POSTDIV1/POSTDIV2
144 *      = (parent_rate x FBDIV) / (REFDIV x POSTDIV1 x POSTDIV2)
145 *
146 * Return: The rate calculated.
147 */
148static unsigned long sg2042_pll_recalc_rate(unsigned int reg_value,
149					    unsigned long parent_rate)
150{
151	struct sg2042_pll_ctrl ctrl_table;
152	u64 numerator, denominator;
153
154	sg2042_pll_ctrl_decode(reg_value, &ctrl_table);
155
156	numerator = (u64)parent_rate * ctrl_table.fbdiv;
157	denominator = ctrl_table.refdiv * ctrl_table.postdiv1 * ctrl_table.postdiv2;
158	numerator =  div64_u64(numerator, denominator);
159	return numerator;
160}
161
162/**
163 * sg2042_pll_get_postdiv_1_2() - Based on input rate/prate/fbdiv/refdiv,
164 * look up the postdiv1_2 table to get the closest postdiiv combination.
165 * @rate: FOUTPOSTDIV
166 * @prate: parent rate, i.e. FREF
167 * @fbdiv: FBDIV
168 * @refdiv: REFDIV
169 * @postdiv1: POSTDIV1, output
170 * @postdiv2: POSTDIV2, output
171 *
172 * postdiv1_2 contains all the possible combination lists of POSTDIV1 and POSTDIV2
173 * for example:
174 * postdiv1_2[0] = {2, 4, 8}, where div1 = 2, div2 = 4 , div1 * div2 = 8
175 *
176 * See TRM:
177 * FOUTPOSTDIV = FREF * FBDIV / REFDIV / (POSTDIV1 * POSTDIV2)
178 * So we get following formula to get POSTDIV1 and POSTDIV2:
179 * POSTDIV = (prate/REFDIV) x FBDIV/rate
180 * above POSTDIV = POSTDIV1*POSTDIV2
181 *
182 * Return:
183 * %0 - OK
184 * %-EINVAL - invalid argument, which means Failed to get the postdivs.
185 */
186static int sg2042_pll_get_postdiv_1_2(unsigned long rate,
187				      unsigned long prate,
188				      unsigned int fbdiv,
189				      unsigned int refdiv,
190				      unsigned int *postdiv1,
191				      unsigned int *postdiv2)
192{
193	int index;
194	u64 tmp0;
195
196	/* POSTDIV_RESULT_INDEX point to 3rd element in the array postdiv1_2 */
197	#define	POSTDIV_RESULT_INDEX	2
198
199	static const int postdiv1_2[][3] = {
200		{2, 4,  8}, {3, 3,  9}, {2, 5, 10}, {2, 6, 12},
201		{2, 7, 14}, {3, 5, 15}, {4, 4, 16}, {3, 6, 18},
202		{4, 5, 20}, {3, 7, 21}, {4, 6, 24}, {5, 5, 25},
203		{4, 7, 28}, {5, 6, 30}, {5, 7, 35}, {6, 6, 36},
204		{6, 7, 42}, {7, 7, 49}
205	};
206
207	/* prate/REFDIV and result save to tmp0 */
208	tmp0 = prate;
209	do_div(tmp0, refdiv);
210
211	/* ((prate/REFDIV) x FBDIV) and result save to tmp0 */
212	tmp0 *= fbdiv;
213
214	/* ((prate/REFDIV) x FBDIV)/rate and result save to tmp0 */
215	tmp0 = div64_ul(tmp0, rate);
216
217	/* tmp0 is POSTDIV1*POSTDIV2, now we calculate div1 and div2 value */
218	if (tmp0 <= 7) {
219		/* (div1 * div2) <= 7, no need to use array search */
220		*postdiv1 = tmp0;
221		*postdiv2 = 1;
222		return 0;
223	}
224
225	/* (div1 * div2) > 7, use array search */
226	for (index = 0; index < ARRAY_SIZE(postdiv1_2); index++) {
227		if (tmp0 > postdiv1_2[index][POSTDIV_RESULT_INDEX]) {
228			continue;
229		} else {
230			/* found it */
231			*postdiv1 = postdiv1_2[index][1];
232			*postdiv2 = postdiv1_2[index][0];
233			return 0;
234		}
235	}
236	pr_warn("%s can not find in postdiv array!\n", __func__);
237	return -EINVAL;
238}
239
240/**
241 * sg2042_get_pll_ctl_setting() - Based on the given FOUTPISTDIV and the input
242 * FREF to calculate the REFDIV/FBDIV/PSTDIV1/POSTDIV2 combination for pllctrl
243 * register.
244 * @req_rate: expected output clock rate, i.e. FOUTPISTDIV
245 * @parent_rate: input parent clock rate, i.e. FREF
246 * @best: output to hold calculated combination of REFDIV/FBDIV/PSTDIV1/POSTDIV2
247 *
248 * Return:
249 * %0 - OK
250 * %-EINVAL - invalid argument
251 */
252static int sg2042_get_pll_ctl_setting(struct sg2042_pll_ctrl *best,
253				      unsigned long req_rate,
254				      unsigned long parent_rate)
255{
256	unsigned int fbdiv, refdiv, postdiv1, postdiv2;
257	unsigned long foutpostdiv;
258	u64 foutvco;
259	int ret;
260	u64 tmp;
261
262	if (parent_rate != PLL_FREF_SG2042) {
263		pr_err("INVALID FREF: %ld\n", parent_rate);
264		return -EINVAL;
265	}
266
267	if (req_rate < PLL_FOUTPOSTDIV_MIN || req_rate > PLL_FOUTPOSTDIV_MAX) {
268		pr_alert("INVALID FOUTPOSTDIV: %ld\n", req_rate);
269		return -EINVAL;
270	}
271
272	memset(best, 0, sizeof(struct sg2042_pll_ctrl));
273
274	for (refdiv = REFDIV_MIN; refdiv < REFDIV_MAX + 1; refdiv++) {
275		/* required by hardware: FREF/REFDIV must > 10 */
276		tmp = parent_rate;
277		do_div(tmp, refdiv);
278		if (tmp <= 10)
279			continue;
280
281		for (fbdiv = FBDIV_MIN; fbdiv < FBDIV_MAX + 1; fbdiv++) {
282			/*
283			 * FOUTVCO = FREF*FBDIV/REFDIV validation
284			 * required by hardware, FOUTVCO must [800MHz, 3200MHz]
285			 */
286			foutvco = parent_rate * fbdiv;
287			do_div(foutvco, refdiv);
288			if (foutvco < PLL_FOUTVCO_MIN || foutvco > PLL_FOUTVCO_MAX)
289				continue;
290
291			ret = sg2042_pll_get_postdiv_1_2(req_rate, parent_rate,
292							 fbdiv, refdiv,
293							 &postdiv1, &postdiv2);
294			if (ret)
295				continue;
296
297			/*
298			 * FOUTPOSTDIV = FREF*FBDIV/REFDIV/(POSTDIV1*POSTDIV2)
299			 *             = FOUTVCO/(POSTDIV1*POSTDIV2)
300			 */
301			tmp = foutvco;
302			do_div(tmp, (postdiv1 * postdiv2));
303			foutpostdiv = (unsigned long)tmp;
304			/* Iterative to approach the expected value */
305			if (abs_diff(foutpostdiv, req_rate) < abs_diff(best->freq, req_rate)) {
306				best->freq = foutpostdiv;
307				best->refdiv = refdiv;
308				best->fbdiv = fbdiv;
309				best->postdiv1 = postdiv1;
310				best->postdiv2 = postdiv2;
311				if (foutpostdiv == req_rate)
312					return 0;
313			}
314			continue;
315		}
316	}
317
318	if (best->freq == 0)
319		return -EINVAL;
320	else
321		return 0;
322}
323
324/**
325 * sg2042_clk_pll_recalc_rate() - recalc_rate callback for pll clks
326 * @hw: ccf use to hook get sg2042_pll_clock
327 * @parent_rate: parent rate
328 *
329 * The is function will be called through clk_get_rate
330 * and return current rate after decoding reg value
331 *
332 * Return: Current rate recalculated.
333 */
334static unsigned long sg2042_clk_pll_recalc_rate(struct clk_hw *hw,
335						unsigned long parent_rate)
336{
337	struct sg2042_pll_clock *pll = to_sg2042_pll_clk(hw);
338	unsigned long rate;
339	u32 value;
340
341	value = readl(pll->base + pll->offset_ctrl);
342	rate = sg2042_pll_recalc_rate(value, parent_rate);
343
344	pr_debug("--> %s: pll_recalc_rate: val = %ld\n",
345		 clk_hw_get_name(hw), rate);
346	return rate;
347}
348
349static int sg2042_clk_pll_determine_rate(struct clk_hw *hw,
350					 struct clk_rate_request *req)
351{
352	struct sg2042_pll_ctrl pctrl_table;
353	unsigned int value;
354	long proper_rate;
355	int ret;
356
357	ret = sg2042_get_pll_ctl_setting(&pctrl_table,
358					 min(req->rate, req->max_rate),
359					 req->best_parent_rate);
360	if (ret) {
361		proper_rate = 0;
362		goto out;
363	}
364
365	value = sg2042_pll_ctrl_encode(&pctrl_table);
366	proper_rate = (long)sg2042_pll_recalc_rate(value, req->best_parent_rate);
367
368out:
369	pr_debug("--> %s: pll_determine_rate: val = %ld\n",
370		 clk_hw_get_name(hw), proper_rate);
371	req->rate = proper_rate;
372
373	return 0;
374}
375
376static int sg2042_clk_pll_set_rate(struct clk_hw *hw,
377				   unsigned long rate,
378				   unsigned long parent_rate)
379{
380	struct sg2042_pll_clock *pll = to_sg2042_pll_clk(hw);
381	struct sg2042_pll_ctrl pctrl_table;
382	unsigned long flags;
383	u32 value = 0;
384	int ret;
385
386	spin_lock_irqsave(pll->lock, flags);
387
388	sg2042_pll_enable(pll, 0);
389
390	ret = sg2042_get_pll_ctl_setting(&pctrl_table, rate, parent_rate);
391	if (ret) {
392		pr_warn("%s: Can't find a proper pll setting\n", pll->hw.init->name);
393		goto out;
394	}
395
396	value = sg2042_pll_ctrl_encode(&pctrl_table);
397
398	/* write the value to top register */
399	writel(value, pll->base + pll->offset_ctrl);
400
401out:
402	sg2042_pll_enable(pll, 1);
403
404	spin_unlock_irqrestore(pll->lock, flags);
405
406	pr_debug("--> %s: pll_set_rate: val = 0x%x\n",
407		 clk_hw_get_name(hw), value);
408	return ret;
409}
410
411static const struct clk_ops sg2042_clk_pll_ops = {
412	.recalc_rate = sg2042_clk_pll_recalc_rate,
413	.determine_rate = sg2042_clk_pll_determine_rate,
414	.set_rate = sg2042_clk_pll_set_rate,
415};
416
417static const struct clk_ops sg2042_clk_pll_ro_ops = {
418	.recalc_rate = sg2042_clk_pll_recalc_rate,
419	.determine_rate = sg2042_clk_pll_determine_rate,
420};
421
422/*
423 * Clock initialization macro naming rules:
424 * FW: use CLK_HW_INIT_FW_NAME
425 * RO: means Read-Only
426 */
427#define SG2042_PLL_FW(_id, _name, _parent, _r_ctrl, _shift)		\
428	{								\
429		.id = _id,						\
430		.hw.init = CLK_HW_INIT_FW_NAME(				\
431				_name,					\
432				_parent,				\
433				&sg2042_clk_pll_ops,			\
434				CLK_GET_RATE_NOCACHE | CLK_GET_ACCURACY_NOCACHE),\
435		.offset_ctrl = _r_ctrl,					\
436		.shift_status_lock = 8 + (_shift),			\
437		.shift_status_updating = _shift,			\
438		.shift_enable = _shift,					\
439	}
440
441#define SG2042_PLL_FW_RO(_id, _name, _parent, _r_ctrl, _shift)		\
442	{								\
443		.id = _id,						\
444		.hw.init = CLK_HW_INIT_FW_NAME(				\
445				_name,					\
446				_parent,				\
447				&sg2042_clk_pll_ro_ops,			\
448				CLK_GET_RATE_NOCACHE | CLK_GET_ACCURACY_NOCACHE),\
449		.offset_ctrl = _r_ctrl,					\
450		.shift_status_lock = 8 + (_shift),			\
451		.shift_status_updating = _shift,			\
452		.shift_enable = _shift,					\
453	}
454
455static struct sg2042_pll_clock sg2042_pll_clks[] = {
456	SG2042_PLL_FW(MPLL_CLK, "mpll_clock", "cgi_main", R_MPLL_CONTROL, 0),
457	SG2042_PLL_FW_RO(FPLL_CLK, "fpll_clock", "cgi_main", R_FPLL_CONTROL, 3),
458	SG2042_PLL_FW_RO(DPLL0_CLK, "dpll0_clock", "cgi_dpll0", R_DPLL0_CONTROL, 4),
459	SG2042_PLL_FW_RO(DPLL1_CLK, "dpll1_clock", "cgi_dpll1", R_DPLL1_CONTROL, 5),
460};
461
462static DEFINE_SPINLOCK(sg2042_clk_lock);
463
464static int sg2042_clk_register_plls(struct device *dev,
465				    struct sg2042_clk_data *clk_data,
466				    struct sg2042_pll_clock pll_clks[],
467				    int num_pll_clks)
468{
469	struct sg2042_pll_clock *pll;
470	struct clk_hw *hw;
471	int i, ret = 0;
472
473	for (i = 0; i < num_pll_clks; i++) {
474		pll = &pll_clks[i];
475		/* assign these for ops usage during registration */
476		pll->base = clk_data->iobase;
477		pll->lock = &sg2042_clk_lock;
478
479		hw = &pll->hw;
480		ret = devm_clk_hw_register(dev, hw);
481		if (ret) {
482			pr_err("failed to register clock %s\n", pll->hw.init->name);
483			break;
484		}
485
486		clk_data->onecell_data.hws[pll->id] = hw;
487	}
488
489	return ret;
490}
491
492static int sg2042_init_clkdata(struct platform_device *pdev,
493			       int num_clks,
494			       struct sg2042_clk_data **pp_clk_data)
495{
496	struct sg2042_clk_data *clk_data;
497
498	clk_data = devm_kzalloc(&pdev->dev,
499				struct_size(clk_data, onecell_data.hws, num_clks),
500				GFP_KERNEL);
501	if (!clk_data)
502		return -ENOMEM;
503
504	clk_data->iobase = devm_platform_ioremap_resource(pdev, 0);
505	if (WARN_ON(IS_ERR(clk_data->iobase)))
506		return PTR_ERR(clk_data->iobase);
507
508	clk_data->onecell_data.num = num_clks;
509
510	*pp_clk_data = clk_data;
511
512	return 0;
513}
514
515static int sg2042_pll_probe(struct platform_device *pdev)
516{
517	struct sg2042_clk_data *clk_data = NULL;
518	int num_clks;
519	int ret;
520
521	num_clks = ARRAY_SIZE(sg2042_pll_clks);
522
523	ret = sg2042_init_clkdata(pdev, num_clks, &clk_data);
524	if (ret)
525		goto error_out;
526
527	ret = sg2042_clk_register_plls(&pdev->dev, clk_data, sg2042_pll_clks,
528				       num_clks);
529	if (ret)
530		goto error_out;
531
532	return devm_of_clk_add_hw_provider(&pdev->dev,
533					   of_clk_hw_onecell_get,
534					   &clk_data->onecell_data);
535
536error_out:
537	pr_err("%s failed error number %d\n", __func__, ret);
538	return ret;
539}
540
541static const struct of_device_id sg2042_pll_match[] = {
542	{ .compatible = "sophgo,sg2042-pll" },
543	{ /* sentinel */ }
544};
545MODULE_DEVICE_TABLE(of, sg2042_pll_match);
546
547static struct platform_driver sg2042_pll_driver = {
548	.probe = sg2042_pll_probe,
549	.driver = {
550		.name = "clk-sophgo-sg2042-pll",
551		.of_match_table = sg2042_pll_match,
552		.suppress_bind_attrs = true,
553	},
554};
555module_platform_driver(sg2042_pll_driver);
556
557MODULE_AUTHOR("Chen Wang");
558MODULE_DESCRIPTION("Sophgo SG2042 pll clock driver");
559MODULE_LICENSE("GPL");