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 */
44 struct 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
76 struct 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
sg2042_pll_ctrl_encode(struct sg2042_pll_ctrl * ctrl)89 static 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
sg2042_pll_ctrl_decode(unsigned int reg_value,struct sg2042_pll_ctrl * ctrl)97 static 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
sg2042_pll_enable(struct sg2042_pll_clock * pll,bool en)106 static 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 */
sg2042_pll_recalc_rate(unsigned int reg_value,unsigned long parent_rate)148 static 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 = parent_rate * ctrl_table.fbdiv;
157 denominator = ctrl_table.refdiv * ctrl_table.postdiv1 * ctrl_table.postdiv2;
158 do_div(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 */
sg2042_pll_get_postdiv_1_2(unsigned long rate,unsigned long prate,unsigned int fbdiv,unsigned int refdiv,unsigned int * postdiv1,unsigned int * postdiv2)186 static 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 do_div(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 */
sg2042_get_pll_ctl_setting(struct sg2042_pll_ctrl * best,unsigned long req_rate,unsigned long parent_rate)252 static 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 */
sg2042_clk_pll_recalc_rate(struct clk_hw * hw,unsigned long parent_rate)334 static 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
sg2042_clk_pll_round_rate(struct clk_hw * hw,unsigned long req_rate,unsigned long * prate)349 static long sg2042_clk_pll_round_rate(struct clk_hw *hw,
350 unsigned long req_rate,
351 unsigned long *prate)
352 {
353 struct sg2042_pll_ctrl pctrl_table;
354 unsigned int value;
355 long proper_rate;
356 int ret;
357
358 ret = sg2042_get_pll_ctl_setting(&pctrl_table, req_rate, *prate);
359 if (ret) {
360 proper_rate = 0;
361 goto out;
362 }
363
364 value = sg2042_pll_ctrl_encode(&pctrl_table);
365 proper_rate = (long)sg2042_pll_recalc_rate(value, *prate);
366
367 out:
368 pr_debug("--> %s: pll_round_rate: val = %ld\n",
369 clk_hw_get_name(hw), proper_rate);
370 return proper_rate;
371 }
372
sg2042_clk_pll_determine_rate(struct clk_hw * hw,struct clk_rate_request * req)373 static int sg2042_clk_pll_determine_rate(struct clk_hw *hw,
374 struct clk_rate_request *req)
375 {
376 req->rate = sg2042_clk_pll_round_rate(hw, min(req->rate, req->max_rate),
377 &req->best_parent_rate);
378 pr_debug("--> %s: pll_determine_rate: val = %ld\n",
379 clk_hw_get_name(hw), req->rate);
380 return 0;
381 }
382
sg2042_clk_pll_set_rate(struct clk_hw * hw,unsigned long rate,unsigned long parent_rate)383 static int sg2042_clk_pll_set_rate(struct clk_hw *hw,
384 unsigned long rate,
385 unsigned long parent_rate)
386 {
387 struct sg2042_pll_clock *pll = to_sg2042_pll_clk(hw);
388 struct sg2042_pll_ctrl pctrl_table;
389 unsigned long flags;
390 u32 value = 0;
391 int ret;
392
393 spin_lock_irqsave(pll->lock, flags);
394
395 sg2042_pll_enable(pll, 0);
396
397 ret = sg2042_get_pll_ctl_setting(&pctrl_table, rate, parent_rate);
398 if (ret) {
399 pr_warn("%s: Can't find a proper pll setting\n", pll->hw.init->name);
400 goto out;
401 }
402
403 value = sg2042_pll_ctrl_encode(&pctrl_table);
404
405 /* write the value to top register */
406 writel(value, pll->base + pll->offset_ctrl);
407
408 out:
409 sg2042_pll_enable(pll, 1);
410
411 spin_unlock_irqrestore(pll->lock, flags);
412
413 pr_debug("--> %s: pll_set_rate: val = 0x%x\n",
414 clk_hw_get_name(hw), value);
415 return ret;
416 }
417
418 static const struct clk_ops sg2042_clk_pll_ops = {
419 .recalc_rate = sg2042_clk_pll_recalc_rate,
420 .round_rate = sg2042_clk_pll_round_rate,
421 .determine_rate = sg2042_clk_pll_determine_rate,
422 .set_rate = sg2042_clk_pll_set_rate,
423 };
424
425 static const struct clk_ops sg2042_clk_pll_ro_ops = {
426 .recalc_rate = sg2042_clk_pll_recalc_rate,
427 .round_rate = sg2042_clk_pll_round_rate,
428 };
429
430 /*
431 * Clock initialization macro naming rules:
432 * FW: use CLK_HW_INIT_FW_NAME
433 * RO: means Read-Only
434 */
435 #define SG2042_PLL_FW(_id, _name, _parent, _r_ctrl, _shift) \
436 { \
437 .id = _id, \
438 .hw.init = CLK_HW_INIT_FW_NAME( \
439 _name, \
440 _parent, \
441 &sg2042_clk_pll_ops, \
442 CLK_GET_RATE_NOCACHE | CLK_GET_ACCURACY_NOCACHE),\
443 .offset_ctrl = _r_ctrl, \
444 .shift_status_lock = 8 + (_shift), \
445 .shift_status_updating = _shift, \
446 .shift_enable = _shift, \
447 }
448
449 #define SG2042_PLL_FW_RO(_id, _name, _parent, _r_ctrl, _shift) \
450 { \
451 .id = _id, \
452 .hw.init = CLK_HW_INIT_FW_NAME( \
453 _name, \
454 _parent, \
455 &sg2042_clk_pll_ro_ops, \
456 CLK_GET_RATE_NOCACHE | CLK_GET_ACCURACY_NOCACHE),\
457 .offset_ctrl = _r_ctrl, \
458 .shift_status_lock = 8 + (_shift), \
459 .shift_status_updating = _shift, \
460 .shift_enable = _shift, \
461 }
462
463 static struct sg2042_pll_clock sg2042_pll_clks[] = {
464 SG2042_PLL_FW(MPLL_CLK, "mpll_clock", "cgi_main", R_MPLL_CONTROL, 0),
465 SG2042_PLL_FW_RO(FPLL_CLK, "fpll_clock", "cgi_main", R_FPLL_CONTROL, 3),
466 SG2042_PLL_FW_RO(DPLL0_CLK, "dpll0_clock", "cgi_dpll0", R_DPLL0_CONTROL, 4),
467 SG2042_PLL_FW_RO(DPLL1_CLK, "dpll1_clock", "cgi_dpll1", R_DPLL1_CONTROL, 5),
468 };
469
470 static DEFINE_SPINLOCK(sg2042_clk_lock);
471
sg2042_clk_register_plls(struct device * dev,struct sg2042_clk_data * clk_data,struct sg2042_pll_clock pll_clks[],int num_pll_clks)472 static int sg2042_clk_register_plls(struct device *dev,
473 struct sg2042_clk_data *clk_data,
474 struct sg2042_pll_clock pll_clks[],
475 int num_pll_clks)
476 {
477 struct sg2042_pll_clock *pll;
478 struct clk_hw *hw;
479 int i, ret = 0;
480
481 for (i = 0; i < num_pll_clks; i++) {
482 pll = &pll_clks[i];
483 /* assign these for ops usage during registration */
484 pll->base = clk_data->iobase;
485 pll->lock = &sg2042_clk_lock;
486
487 hw = &pll->hw;
488 ret = devm_clk_hw_register(dev, hw);
489 if (ret) {
490 pr_err("failed to register clock %s\n", pll->hw.init->name);
491 break;
492 }
493
494 clk_data->onecell_data.hws[pll->id] = hw;
495 }
496
497 return ret;
498 }
499
sg2042_init_clkdata(struct platform_device * pdev,int num_clks,struct sg2042_clk_data ** pp_clk_data)500 static int sg2042_init_clkdata(struct platform_device *pdev,
501 int num_clks,
502 struct sg2042_clk_data **pp_clk_data)
503 {
504 struct sg2042_clk_data *clk_data;
505
506 clk_data = devm_kzalloc(&pdev->dev,
507 struct_size(clk_data, onecell_data.hws, num_clks),
508 GFP_KERNEL);
509 if (!clk_data)
510 return -ENOMEM;
511
512 clk_data->iobase = devm_platform_ioremap_resource(pdev, 0);
513 if (WARN_ON(IS_ERR(clk_data->iobase)))
514 return PTR_ERR(clk_data->iobase);
515
516 clk_data->onecell_data.num = num_clks;
517
518 *pp_clk_data = clk_data;
519
520 return 0;
521 }
522
sg2042_pll_probe(struct platform_device * pdev)523 static int sg2042_pll_probe(struct platform_device *pdev)
524 {
525 struct sg2042_clk_data *clk_data = NULL;
526 int num_clks;
527 int ret;
528
529 num_clks = ARRAY_SIZE(sg2042_pll_clks);
530
531 ret = sg2042_init_clkdata(pdev, num_clks, &clk_data);
532 if (ret)
533 goto error_out;
534
535 ret = sg2042_clk_register_plls(&pdev->dev, clk_data, sg2042_pll_clks,
536 num_clks);
537 if (ret)
538 goto error_out;
539
540 return devm_of_clk_add_hw_provider(&pdev->dev,
541 of_clk_hw_onecell_get,
542 &clk_data->onecell_data);
543
544 error_out:
545 pr_err("%s failed error number %d\n", __func__, ret);
546 return ret;
547 }
548
549 static const struct of_device_id sg2042_pll_match[] = {
550 { .compatible = "sophgo,sg2042-pll" },
551 { /* sentinel */ }
552 };
553 MODULE_DEVICE_TABLE(of, sg2042_pll_match);
554
555 static struct platform_driver sg2042_pll_driver = {
556 .probe = sg2042_pll_probe,
557 .driver = {
558 .name = "clk-sophgo-sg2042-pll",
559 .of_match_table = sg2042_pll_match,
560 .suppress_bind_attrs = true,
561 },
562 };
563 module_platform_driver(sg2042_pll_driver);
564
565 MODULE_AUTHOR("Chen Wang");
566 MODULE_DESCRIPTION("Sophgo SG2042 pll clock driver");
567 MODULE_LICENSE("GPL");
568