1 // SPDX-License-Identifier: GPL-2.0-only
2 /*
3  * Purna Chandra Mandal,<purna.mandal@microchip.com>
4  * Copyright (C) 2015 Microchip Technology Inc.  All rights reserved.
5  */
6 #include <linux/clk-provider.h>
7 #include <linux/delay.h>
8 #include <linux/device.h>
9 #include <linux/interrupt.h>
10 #include <linux/io.h>
11 #include <linux/iopoll.h>
12 #include <asm/mach-pic32/pic32.h>
13 #include <asm/traps.h>
14 
15 #include "clk-core.h"
16 
17 /* OSCCON Reg fields */
18 #define OSC_CUR_MASK		0x07
19 #define OSC_CUR_SHIFT		12
20 #define OSC_NEW_MASK		0x07
21 #define OSC_NEW_SHIFT		8
22 #define OSC_SWEN		BIT(0)
23 
24 /* SPLLCON Reg fields */
25 #define PLL_RANGE_MASK		0x07
26 #define PLL_RANGE_SHIFT		0
27 #define PLL_ICLK_MASK		0x01
28 #define PLL_ICLK_SHIFT		7
29 #define PLL_IDIV_MASK		0x07
30 #define PLL_IDIV_SHIFT		8
31 #define PLL_ODIV_MASK		0x07
32 #define PLL_ODIV_SHIFT		24
33 #define PLL_MULT_MASK		0x7F
34 #define PLL_MULT_SHIFT		16
35 #define PLL_MULT_MAX		128
36 #define PLL_ODIV_MIN		1
37 #define PLL_ODIV_MAX		5
38 
39 /* Peripheral Bus Clock Reg Fields */
40 #define PB_DIV_MASK		0x7f
41 #define PB_DIV_SHIFT		0
42 #define PB_DIV_READY		BIT(11)
43 #define PB_DIV_ENABLE		BIT(15)
44 #define PB_DIV_MAX		128
45 #define PB_DIV_MIN		0
46 
47 /* Reference Oscillator Control Reg fields */
48 #define REFO_SEL_MASK		0x0f
49 #define REFO_SEL_SHIFT		0
50 #define REFO_ACTIVE		BIT(8)
51 #define REFO_DIVSW_EN		BIT(9)
52 #define REFO_OE			BIT(12)
53 #define REFO_ON			BIT(15)
54 #define REFO_DIV_SHIFT		16
55 #define REFO_DIV_MASK		0x7fff
56 
57 /* Reference Oscillator Trim Register Fields */
58 #define REFO_TRIM_REG		0x10
59 #define REFO_TRIM_MASK		0x1ff
60 #define REFO_TRIM_SHIFT		23
61 #define REFO_TRIM_MAX		511
62 
63 /* Mux Slew Control Register fields */
64 #define SLEW_BUSY		BIT(0)
65 #define SLEW_DOWNEN		BIT(1)
66 #define SLEW_UPEN		BIT(2)
67 #define SLEW_DIV		0x07
68 #define SLEW_DIV_SHIFT		8
69 #define SLEW_SYSDIV		0x0f
70 #define SLEW_SYSDIV_SHIFT	20
71 
72 /* Clock Poll Timeout */
73 #define LOCK_TIMEOUT_US         USEC_PER_MSEC
74 
75 /* SoC specific clock needed during SPLL clock rate switch */
76 static struct clk_hw *pic32_sclk_hw;
77 
78 /* add instruction pipeline delay while CPU clock is in-transition. */
79 #define cpu_nop5()			\
80 do {					\
81 	__asm__ __volatile__("nop");	\
82 	__asm__ __volatile__("nop");	\
83 	__asm__ __volatile__("nop");	\
84 	__asm__ __volatile__("nop");	\
85 	__asm__ __volatile__("nop");	\
86 } while (0)
87 
88 /* Perpheral bus clocks */
89 struct pic32_periph_clk {
90 	struct clk_hw hw;
91 	void __iomem *ctrl_reg;
92 	struct pic32_clk_common *core;
93 };
94 
95 #define clkhw_to_pbclk(_hw)	container_of(_hw, struct pic32_periph_clk, hw)
96 
pbclk_is_enabled(struct clk_hw * hw)97 static int pbclk_is_enabled(struct clk_hw *hw)
98 {
99 	struct pic32_periph_clk *pb = clkhw_to_pbclk(hw);
100 
101 	return readl(pb->ctrl_reg) & PB_DIV_ENABLE;
102 }
103 
pbclk_enable(struct clk_hw * hw)104 static int pbclk_enable(struct clk_hw *hw)
105 {
106 	struct pic32_periph_clk *pb = clkhw_to_pbclk(hw);
107 
108 	writel(PB_DIV_ENABLE, PIC32_SET(pb->ctrl_reg));
109 	return 0;
110 }
111 
pbclk_disable(struct clk_hw * hw)112 static void pbclk_disable(struct clk_hw *hw)
113 {
114 	struct pic32_periph_clk *pb = clkhw_to_pbclk(hw);
115 
116 	writel(PB_DIV_ENABLE, PIC32_CLR(pb->ctrl_reg));
117 }
118 
calc_best_divided_rate(unsigned long rate,unsigned long parent_rate,u32 divider_max,u32 divider_min)119 static unsigned long calc_best_divided_rate(unsigned long rate,
120 					    unsigned long parent_rate,
121 					    u32 divider_max,
122 					    u32 divider_min)
123 {
124 	unsigned long divided_rate, divided_rate_down, best_rate;
125 	unsigned long div, div_up;
126 
127 	/* eq. clk_rate = parent_rate / divider.
128 	 *
129 	 * Find best divider to produce closest of target divided rate.
130 	 */
131 	div = parent_rate / rate;
132 	div = clamp_val(div, divider_min, divider_max);
133 	div_up = clamp_val(div + 1, divider_min, divider_max);
134 
135 	divided_rate = parent_rate / div;
136 	divided_rate_down = parent_rate / div_up;
137 	if (abs(rate - divided_rate_down) < abs(rate - divided_rate))
138 		best_rate = divided_rate_down;
139 	else
140 		best_rate = divided_rate;
141 
142 	return best_rate;
143 }
144 
pbclk_read_pbdiv(struct pic32_periph_clk * pb)145 static inline u32 pbclk_read_pbdiv(struct pic32_periph_clk *pb)
146 {
147 	return ((readl(pb->ctrl_reg) >> PB_DIV_SHIFT) & PB_DIV_MASK) + 1;
148 }
149 
pbclk_recalc_rate(struct clk_hw * hw,unsigned long parent_rate)150 static unsigned long pbclk_recalc_rate(struct clk_hw *hw,
151 				       unsigned long parent_rate)
152 {
153 	struct pic32_periph_clk *pb = clkhw_to_pbclk(hw);
154 
155 	return parent_rate / pbclk_read_pbdiv(pb);
156 }
157 
pbclk_round_rate(struct clk_hw * hw,unsigned long rate,unsigned long * parent_rate)158 static long pbclk_round_rate(struct clk_hw *hw, unsigned long rate,
159 			     unsigned long *parent_rate)
160 {
161 	return calc_best_divided_rate(rate, *parent_rate,
162 				      PB_DIV_MAX, PB_DIV_MIN);
163 }
164 
pbclk_set_rate(struct clk_hw * hw,unsigned long rate,unsigned long parent_rate)165 static int pbclk_set_rate(struct clk_hw *hw, unsigned long rate,
166 			  unsigned long parent_rate)
167 {
168 	struct pic32_periph_clk *pb = clkhw_to_pbclk(hw);
169 	unsigned long flags;
170 	u32 v, div;
171 	int err;
172 
173 	/* check & wait for DIV_READY */
174 	err = readl_poll_timeout(pb->ctrl_reg, v, v & PB_DIV_READY,
175 				 1, LOCK_TIMEOUT_US);
176 	if (err)
177 		return err;
178 
179 	/* calculate clkdiv and best rate */
180 	div = DIV_ROUND_CLOSEST(parent_rate, rate);
181 
182 	spin_lock_irqsave(&pb->core->reg_lock, flags);
183 
184 	/* apply new div */
185 	v = readl(pb->ctrl_reg);
186 	v &= ~PB_DIV_MASK;
187 	v |= (div - 1);
188 
189 	pic32_syskey_unlock();
190 
191 	writel(v, pb->ctrl_reg);
192 
193 	spin_unlock_irqrestore(&pb->core->reg_lock, flags);
194 
195 	/* wait again for DIV_READY */
196 	err = readl_poll_timeout(pb->ctrl_reg, v, v & PB_DIV_READY,
197 				 1, LOCK_TIMEOUT_US);
198 	if (err)
199 		return err;
200 
201 	/* confirm that new div is applied correctly */
202 	return (pbclk_read_pbdiv(pb) == div) ? 0 : -EBUSY;
203 }
204 
205 const struct clk_ops pic32_pbclk_ops = {
206 	.enable		= pbclk_enable,
207 	.disable	= pbclk_disable,
208 	.is_enabled	= pbclk_is_enabled,
209 	.recalc_rate	= pbclk_recalc_rate,
210 	.round_rate	= pbclk_round_rate,
211 	.set_rate	= pbclk_set_rate,
212 };
213 
pic32_periph_clk_register(const struct pic32_periph_clk_data * desc,struct pic32_clk_common * core)214 struct clk *pic32_periph_clk_register(const struct pic32_periph_clk_data *desc,
215 				      struct pic32_clk_common *core)
216 {
217 	struct pic32_periph_clk *pbclk;
218 	struct clk *clk;
219 
220 	pbclk = devm_kzalloc(core->dev, sizeof(*pbclk), GFP_KERNEL);
221 	if (!pbclk)
222 		return ERR_PTR(-ENOMEM);
223 
224 	pbclk->hw.init = &desc->init_data;
225 	pbclk->core = core;
226 	pbclk->ctrl_reg = desc->ctrl_reg + core->iobase;
227 
228 	clk = devm_clk_register(core->dev, &pbclk->hw);
229 	if (IS_ERR(clk)) {
230 		dev_err(core->dev, "%s: clk_register() failed\n", __func__);
231 		devm_kfree(core->dev, pbclk);
232 	}
233 
234 	return clk;
235 }
236 
237 /* Reference oscillator operations */
238 struct pic32_ref_osc {
239 	struct clk_hw hw;
240 	void __iomem *ctrl_reg;
241 	const u32 *parent_map;
242 	struct pic32_clk_common *core;
243 };
244 
245 #define clkhw_to_refosc(_hw)	container_of(_hw, struct pic32_ref_osc, hw)
246 
roclk_is_enabled(struct clk_hw * hw)247 static int roclk_is_enabled(struct clk_hw *hw)
248 {
249 	struct pic32_ref_osc *refo = clkhw_to_refosc(hw);
250 
251 	return readl(refo->ctrl_reg) & REFO_ON;
252 }
253 
roclk_enable(struct clk_hw * hw)254 static int roclk_enable(struct clk_hw *hw)
255 {
256 	struct pic32_ref_osc *refo = clkhw_to_refosc(hw);
257 
258 	writel(REFO_ON | REFO_OE, PIC32_SET(refo->ctrl_reg));
259 	return 0;
260 }
261 
roclk_disable(struct clk_hw * hw)262 static void roclk_disable(struct clk_hw *hw)
263 {
264 	struct pic32_ref_osc *refo = clkhw_to_refosc(hw);
265 
266 	writel(REFO_ON | REFO_OE, PIC32_CLR(refo->ctrl_reg));
267 }
268 
roclk_init(struct clk_hw * hw)269 static int roclk_init(struct clk_hw *hw)
270 {
271 	/* initialize clock in disabled state */
272 	roclk_disable(hw);
273 
274 	return 0;
275 }
276 
roclk_get_parent(struct clk_hw * hw)277 static u8 roclk_get_parent(struct clk_hw *hw)
278 {
279 	struct pic32_ref_osc *refo = clkhw_to_refosc(hw);
280 	u32 v, i;
281 
282 	v = (readl(refo->ctrl_reg) >> REFO_SEL_SHIFT) & REFO_SEL_MASK;
283 
284 	if (!refo->parent_map)
285 		return v;
286 
287 	for (i = 0; i < clk_hw_get_num_parents(hw); i++)
288 		if (refo->parent_map[i] == v)
289 			return i;
290 
291 	return -EINVAL;
292 }
293 
roclk_calc_rate(unsigned long parent_rate,u32 rodiv,u32 rotrim)294 static unsigned long roclk_calc_rate(unsigned long parent_rate,
295 				     u32 rodiv, u32 rotrim)
296 {
297 	u64 rate64;
298 
299 	/* fout = fin / [2 * {div + (trim / 512)}]
300 	 *	= fin * 512 / [1024 * div + 2 * trim]
301 	 *	= fin * 256 / (512 * div + trim)
302 	 *	= (fin << 8) / ((div << 9) + trim)
303 	 */
304 	if (rotrim) {
305 		rodiv = (rodiv << 9) + rotrim;
306 		rate64 = parent_rate;
307 		rate64 <<= 8;
308 		do_div(rate64, rodiv);
309 	} else if (rodiv) {
310 		rate64 = parent_rate / (rodiv << 1);
311 	} else {
312 		rate64 = parent_rate;
313 	}
314 	return rate64;
315 }
316 
roclk_calc_div_trim(unsigned long rate,unsigned long parent_rate,u32 * rodiv_p,u32 * rotrim_p)317 static void roclk_calc_div_trim(unsigned long rate,
318 				unsigned long parent_rate,
319 				u32 *rodiv_p, u32 *rotrim_p)
320 {
321 	u32 div, rotrim, rodiv;
322 	u64 frac;
323 
324 	/* Find integer approximation of floating-point arithmetic.
325 	 *      fout = fin / [2 * {rodiv + (rotrim / 512)}] ... (1)
326 	 * i.e. fout = fin / 2 * DIV
327 	 *      whereas DIV = rodiv + (rotrim / 512)
328 	 *
329 	 * Since kernel does not perform floating-point arithmatic so
330 	 * (rotrim/512) will be zero. And DIV & rodiv will result same.
331 	 *
332 	 * ie. fout = (fin * 256) / [(512 * rodiv) + rotrim]  ... from (1)
333 	 * ie. rotrim = ((fin * 256) / fout) - (512 * DIV)
334 	 */
335 	if (parent_rate <= rate) {
336 		div = 0;
337 		frac = 0;
338 		rodiv = 0;
339 		rotrim = 0;
340 	} else {
341 		div = parent_rate / (rate << 1);
342 		frac = parent_rate;
343 		frac <<= 8;
344 		do_div(frac, rate);
345 		frac -= (u64)(div << 9);
346 
347 		rodiv = (div > REFO_DIV_MASK) ? REFO_DIV_MASK : div;
348 		rotrim = (frac >= REFO_TRIM_MAX) ? REFO_TRIM_MAX : frac;
349 	}
350 
351 	if (rodiv_p)
352 		*rodiv_p = rodiv;
353 
354 	if (rotrim_p)
355 		*rotrim_p = rotrim;
356 }
357 
roclk_recalc_rate(struct clk_hw * hw,unsigned long parent_rate)358 static unsigned long roclk_recalc_rate(struct clk_hw *hw,
359 				       unsigned long parent_rate)
360 {
361 	struct pic32_ref_osc *refo = clkhw_to_refosc(hw);
362 	u32 v, rodiv, rotrim;
363 
364 	/* get rodiv */
365 	v = readl(refo->ctrl_reg);
366 	rodiv = (v >> REFO_DIV_SHIFT) & REFO_DIV_MASK;
367 
368 	/* get trim */
369 	v = readl(refo->ctrl_reg + REFO_TRIM_REG);
370 	rotrim = (v >> REFO_TRIM_SHIFT) & REFO_TRIM_MASK;
371 
372 	return roclk_calc_rate(parent_rate, rodiv, rotrim);
373 }
374 
roclk_round_rate(struct clk_hw * hw,unsigned long rate,unsigned long * parent_rate)375 static long roclk_round_rate(struct clk_hw *hw, unsigned long rate,
376 			     unsigned long *parent_rate)
377 {
378 	u32 rotrim, rodiv;
379 
380 	/* calculate dividers for new rate */
381 	roclk_calc_div_trim(rate, *parent_rate, &rodiv, &rotrim);
382 
383 	/* caclulate new rate (rounding) based on new rodiv & rotrim */
384 	return roclk_calc_rate(*parent_rate, rodiv, rotrim);
385 }
386 
roclk_determine_rate(struct clk_hw * hw,struct clk_rate_request * req)387 static int roclk_determine_rate(struct clk_hw *hw,
388 				struct clk_rate_request *req)
389 {
390 	struct clk_hw *parent_clk, *best_parent_clk = NULL;
391 	unsigned int i, delta, best_delta = -1;
392 	unsigned long parent_rate, best_parent_rate = 0;
393 	unsigned long best = 0, nearest_rate;
394 
395 	/* find a parent which can generate nearest clkrate >= rate */
396 	for (i = 0; i < clk_hw_get_num_parents(hw); i++) {
397 		/* get parent */
398 		parent_clk = clk_hw_get_parent_by_index(hw, i);
399 		if (!parent_clk)
400 			continue;
401 
402 		/* skip if parent runs slower than target rate */
403 		parent_rate = clk_hw_get_rate(parent_clk);
404 		if (req->rate > parent_rate)
405 			continue;
406 
407 		nearest_rate = roclk_round_rate(hw, req->rate, &parent_rate);
408 		delta = abs(nearest_rate - req->rate);
409 		if ((nearest_rate >= req->rate) && (delta < best_delta)) {
410 			best_parent_clk = parent_clk;
411 			best_parent_rate = parent_rate;
412 			best = nearest_rate;
413 			best_delta = delta;
414 
415 			if (delta == 0)
416 				break;
417 		}
418 	}
419 
420 	/* if no match found, retain old rate */
421 	if (!best_parent_clk) {
422 		pr_err("%s:%s, no parent found for rate %lu.\n",
423 		       __func__, clk_hw_get_name(hw), req->rate);
424 		return clk_hw_get_rate(hw);
425 	}
426 
427 	pr_debug("%s,rate %lu, best_parent(%s, %lu), best %lu, delta %d\n",
428 		 clk_hw_get_name(hw), req->rate,
429 		 clk_hw_get_name(best_parent_clk), best_parent_rate,
430 		 best, best_delta);
431 
432 	if (req->best_parent_rate)
433 		req->best_parent_rate = best_parent_rate;
434 
435 	if (req->best_parent_hw)
436 		req->best_parent_hw = best_parent_clk;
437 
438 	return best;
439 }
440 
roclk_set_parent(struct clk_hw * hw,u8 index)441 static int roclk_set_parent(struct clk_hw *hw, u8 index)
442 {
443 	struct pic32_ref_osc *refo = clkhw_to_refosc(hw);
444 	unsigned long flags;
445 	u32 v;
446 	int err;
447 
448 	if (refo->parent_map)
449 		index = refo->parent_map[index];
450 
451 	/* wait until ACTIVE bit is zero or timeout */
452 	err = readl_poll_timeout(refo->ctrl_reg, v, !(v & REFO_ACTIVE),
453 				 1, LOCK_TIMEOUT_US);
454 	if (err) {
455 		pr_err("%s: poll failed, clk active\n", clk_hw_get_name(hw));
456 		return err;
457 	}
458 
459 	spin_lock_irqsave(&refo->core->reg_lock, flags);
460 
461 	pic32_syskey_unlock();
462 
463 	/* calculate & apply new */
464 	v = readl(refo->ctrl_reg);
465 	v &= ~(REFO_SEL_MASK << REFO_SEL_SHIFT);
466 	v |= index << REFO_SEL_SHIFT;
467 
468 	writel(v, refo->ctrl_reg);
469 
470 	spin_unlock_irqrestore(&refo->core->reg_lock, flags);
471 
472 	return 0;
473 }
474 
roclk_set_rate_and_parent(struct clk_hw * hw,unsigned long rate,unsigned long parent_rate,u8 index)475 static int roclk_set_rate_and_parent(struct clk_hw *hw,
476 				     unsigned long rate,
477 				     unsigned long parent_rate,
478 				     u8 index)
479 {
480 	struct pic32_ref_osc *refo = clkhw_to_refosc(hw);
481 	unsigned long flags;
482 	u32 trim, rodiv, v;
483 	int err;
484 
485 	/* calculate new rodiv & rotrim for new rate */
486 	roclk_calc_div_trim(rate, parent_rate, &rodiv, &trim);
487 
488 	pr_debug("parent_rate = %lu, rate = %lu, div = %d, trim = %d\n",
489 		 parent_rate, rate, rodiv, trim);
490 
491 	/* wait till source change is active */
492 	err = readl_poll_timeout(refo->ctrl_reg, v,
493 				 !(v & (REFO_ACTIVE | REFO_DIVSW_EN)),
494 				 1, LOCK_TIMEOUT_US);
495 	if (err) {
496 		pr_err("%s: poll timedout, clock is still active\n", __func__);
497 		return err;
498 	}
499 
500 	spin_lock_irqsave(&refo->core->reg_lock, flags);
501 	v = readl(refo->ctrl_reg);
502 
503 	pic32_syskey_unlock();
504 
505 	/* apply parent, if required */
506 	if (refo->parent_map)
507 		index = refo->parent_map[index];
508 
509 	v &= ~(REFO_SEL_MASK << REFO_SEL_SHIFT);
510 	v |= index << REFO_SEL_SHIFT;
511 
512 	/* apply RODIV */
513 	v &= ~(REFO_DIV_MASK << REFO_DIV_SHIFT);
514 	v |= rodiv << REFO_DIV_SHIFT;
515 	writel(v, refo->ctrl_reg);
516 
517 	/* apply ROTRIM */
518 	v = readl(refo->ctrl_reg + REFO_TRIM_REG);
519 	v &= ~(REFO_TRIM_MASK << REFO_TRIM_SHIFT);
520 	v |= trim << REFO_TRIM_SHIFT;
521 	writel(v, refo->ctrl_reg + REFO_TRIM_REG);
522 
523 	/* enable & activate divider switching */
524 	writel(REFO_ON | REFO_DIVSW_EN, PIC32_SET(refo->ctrl_reg));
525 
526 	/* wait till divswen is in-progress */
527 	err = readl_poll_timeout_atomic(refo->ctrl_reg, v, !(v & REFO_DIVSW_EN),
528 					1, LOCK_TIMEOUT_US);
529 	/* leave the clk gated as it was */
530 	writel(REFO_ON, PIC32_CLR(refo->ctrl_reg));
531 
532 	spin_unlock_irqrestore(&refo->core->reg_lock, flags);
533 
534 	return err;
535 }
536 
roclk_set_rate(struct clk_hw * hw,unsigned long rate,unsigned long parent_rate)537 static int roclk_set_rate(struct clk_hw *hw, unsigned long rate,
538 			  unsigned long parent_rate)
539 {
540 	u8 index = roclk_get_parent(hw);
541 
542 	return roclk_set_rate_and_parent(hw, rate, parent_rate, index);
543 }
544 
545 const struct clk_ops pic32_roclk_ops = {
546 	.enable			= roclk_enable,
547 	.disable		= roclk_disable,
548 	.is_enabled		= roclk_is_enabled,
549 	.get_parent		= roclk_get_parent,
550 	.set_parent		= roclk_set_parent,
551 	.determine_rate		= roclk_determine_rate,
552 	.recalc_rate		= roclk_recalc_rate,
553 	.set_rate_and_parent	= roclk_set_rate_and_parent,
554 	.set_rate		= roclk_set_rate,
555 	.init			= roclk_init,
556 };
557 
pic32_refo_clk_register(const struct pic32_ref_osc_data * data,struct pic32_clk_common * core)558 struct clk *pic32_refo_clk_register(const struct pic32_ref_osc_data *data,
559 				    struct pic32_clk_common *core)
560 {
561 	struct pic32_ref_osc *refo;
562 	struct clk *clk;
563 
564 	refo = devm_kzalloc(core->dev, sizeof(*refo), GFP_KERNEL);
565 	if (!refo)
566 		return ERR_PTR(-ENOMEM);
567 
568 	refo->core = core;
569 	refo->hw.init = &data->init_data;
570 	refo->ctrl_reg = data->ctrl_reg + core->iobase;
571 	refo->parent_map = data->parent_map;
572 
573 	clk = devm_clk_register(core->dev, &refo->hw);
574 	if (IS_ERR(clk))
575 		dev_err(core->dev, "%s: clk_register() failed\n", __func__);
576 
577 	return clk;
578 }
579 
580 struct pic32_sys_pll {
581 	struct clk_hw hw;
582 	void __iomem *ctrl_reg;
583 	void __iomem *status_reg;
584 	u32 lock_mask;
585 	u32 idiv; /* PLL iclk divider, treated fixed */
586 	struct pic32_clk_common *core;
587 };
588 
589 #define clkhw_to_spll(_hw)	container_of(_hw, struct pic32_sys_pll, hw)
590 
spll_odiv_to_divider(u32 odiv)591 static inline u32 spll_odiv_to_divider(u32 odiv)
592 {
593 	odiv = clamp_val(odiv, PLL_ODIV_MIN, PLL_ODIV_MAX);
594 
595 	return 1 << odiv;
596 }
597 
spll_calc_mult_div(struct pic32_sys_pll * pll,unsigned long rate,unsigned long parent_rate,u32 * mult_p,u32 * odiv_p)598 static unsigned long spll_calc_mult_div(struct pic32_sys_pll *pll,
599 					unsigned long rate,
600 					unsigned long parent_rate,
601 					u32 *mult_p, u32 *odiv_p)
602 {
603 	u32 mul, div, best_mul = 1, best_div = 1;
604 	unsigned long new_rate, best_rate = rate;
605 	unsigned int best_delta = -1, delta, match_found = 0;
606 	u64 rate64;
607 
608 	parent_rate /= pll->idiv;
609 
610 	for (mul = 1; mul <= PLL_MULT_MAX; mul++) {
611 		for (div = PLL_ODIV_MIN; div <= PLL_ODIV_MAX; div++) {
612 			rate64 = parent_rate;
613 			rate64 *= mul;
614 			do_div(rate64, 1 << div);
615 			new_rate = rate64;
616 			delta = abs(rate - new_rate);
617 			if ((new_rate >= rate) && (delta < best_delta)) {
618 				best_delta = delta;
619 				best_rate = new_rate;
620 				best_mul = mul;
621 				best_div = div;
622 				match_found = 1;
623 			}
624 		}
625 	}
626 
627 	if (!match_found) {
628 		pr_warn("spll: no match found\n");
629 		return 0;
630 	}
631 
632 	pr_debug("rate %lu, par_rate %lu/mult %u, div %u, best_rate %lu\n",
633 		 rate, parent_rate, best_mul, best_div, best_rate);
634 
635 	if (mult_p)
636 		*mult_p = best_mul - 1;
637 
638 	if (odiv_p)
639 		*odiv_p = best_div;
640 
641 	return best_rate;
642 }
643 
spll_clk_recalc_rate(struct clk_hw * hw,unsigned long parent_rate)644 static unsigned long spll_clk_recalc_rate(struct clk_hw *hw,
645 					  unsigned long parent_rate)
646 {
647 	struct pic32_sys_pll *pll = clkhw_to_spll(hw);
648 	unsigned long pll_in_rate;
649 	u32 mult, odiv, div, v;
650 	u64 rate64;
651 
652 	v = readl(pll->ctrl_reg);
653 	odiv = ((v >> PLL_ODIV_SHIFT) & PLL_ODIV_MASK);
654 	mult = ((v >> PLL_MULT_SHIFT) & PLL_MULT_MASK) + 1;
655 	div = spll_odiv_to_divider(odiv);
656 
657 	/* pll_in_rate = parent_rate / idiv
658 	 * pll_out_rate = pll_in_rate * mult / div;
659 	 */
660 	pll_in_rate = parent_rate / pll->idiv;
661 	rate64 = pll_in_rate;
662 	rate64 *= mult;
663 	do_div(rate64, div);
664 
665 	return rate64;
666 }
667 
spll_clk_round_rate(struct clk_hw * hw,unsigned long rate,unsigned long * parent_rate)668 static long spll_clk_round_rate(struct clk_hw *hw, unsigned long rate,
669 				unsigned long *parent_rate)
670 {
671 	struct pic32_sys_pll *pll = clkhw_to_spll(hw);
672 
673 	return spll_calc_mult_div(pll, rate, *parent_rate, NULL, NULL);
674 }
675 
spll_clk_set_rate(struct clk_hw * hw,unsigned long rate,unsigned long parent_rate)676 static int spll_clk_set_rate(struct clk_hw *hw, unsigned long rate,
677 			     unsigned long parent_rate)
678 {
679 	struct pic32_sys_pll *pll = clkhw_to_spll(hw);
680 	unsigned long ret, flags;
681 	u32 mult, odiv, v;
682 	int err;
683 
684 	ret = spll_calc_mult_div(pll, rate, parent_rate, &mult, &odiv);
685 	if (!ret)
686 		return -EINVAL;
687 
688 	/*
689 	 * We can't change SPLL counters when it is in-active use
690 	 * by SYSCLK. So check before applying new counters/rate.
691 	 */
692 
693 	/* Is spll_clk active parent of sys_clk ? */
694 	if (unlikely(clk_hw_get_parent(pic32_sclk_hw) == hw)) {
695 		pr_err("%s: failed, clk in-use\n", __func__);
696 		return -EBUSY;
697 	}
698 
699 	spin_lock_irqsave(&pll->core->reg_lock, flags);
700 
701 	/* apply new multiplier & divisor */
702 	v = readl(pll->ctrl_reg);
703 	v &= ~(PLL_MULT_MASK << PLL_MULT_SHIFT);
704 	v &= ~(PLL_ODIV_MASK << PLL_ODIV_SHIFT);
705 	v |= (mult << PLL_MULT_SHIFT) | (odiv << PLL_ODIV_SHIFT);
706 
707 	/* sys unlock before write */
708 	pic32_syskey_unlock();
709 
710 	writel(v, pll->ctrl_reg);
711 	cpu_relax();
712 
713 	/* insert few nops (5-stage) to ensure CPU does not hang */
714 	cpu_nop5();
715 	cpu_nop5();
716 
717 	/* Wait until PLL is locked (maximum 100 usecs). */
718 	err = readl_poll_timeout_atomic(pll->status_reg, v,
719 					v & pll->lock_mask, 1, 100);
720 	spin_unlock_irqrestore(&pll->core->reg_lock, flags);
721 
722 	return err;
723 }
724 
725 /* SPLL clock operation */
726 const struct clk_ops pic32_spll_ops = {
727 	.recalc_rate	= spll_clk_recalc_rate,
728 	.round_rate	= spll_clk_round_rate,
729 	.set_rate	= spll_clk_set_rate,
730 };
731 
pic32_spll_clk_register(const struct pic32_sys_pll_data * data,struct pic32_clk_common * core)732 struct clk *pic32_spll_clk_register(const struct pic32_sys_pll_data *data,
733 				    struct pic32_clk_common *core)
734 {
735 	struct pic32_sys_pll *spll;
736 	struct clk *clk;
737 
738 	spll = devm_kzalloc(core->dev, sizeof(*spll), GFP_KERNEL);
739 	if (!spll)
740 		return ERR_PTR(-ENOMEM);
741 
742 	spll->core = core;
743 	spll->hw.init = &data->init_data;
744 	spll->ctrl_reg = data->ctrl_reg + core->iobase;
745 	spll->status_reg = data->status_reg + core->iobase;
746 	spll->lock_mask = data->lock_mask;
747 
748 	/* cache PLL idiv; PLL driver uses it as constant.*/
749 	spll->idiv = (readl(spll->ctrl_reg) >> PLL_IDIV_SHIFT) & PLL_IDIV_MASK;
750 	spll->idiv += 1;
751 
752 	clk = devm_clk_register(core->dev, &spll->hw);
753 	if (IS_ERR(clk))
754 		dev_err(core->dev, "sys_pll: clk_register() failed\n");
755 
756 	return clk;
757 }
758 
759 /* System mux clock(aka SCLK) */
760 
761 struct pic32_sys_clk {
762 	struct clk_hw hw;
763 	void __iomem *mux_reg;
764 	void __iomem *slew_reg;
765 	u32 slew_div;
766 	const u32 *parent_map;
767 	struct pic32_clk_common *core;
768 };
769 
770 #define clkhw_to_sys_clk(_hw)	container_of(_hw, struct pic32_sys_clk, hw)
771 
sclk_get_rate(struct clk_hw * hw,unsigned long parent_rate)772 static unsigned long sclk_get_rate(struct clk_hw *hw, unsigned long parent_rate)
773 {
774 	struct pic32_sys_clk *sclk = clkhw_to_sys_clk(hw);
775 	u32 div;
776 
777 	div = (readl(sclk->slew_reg) >> SLEW_SYSDIV_SHIFT) & SLEW_SYSDIV;
778 	div += 1; /* sys-div to divider */
779 
780 	return parent_rate / div;
781 }
782 
sclk_round_rate(struct clk_hw * hw,unsigned long rate,unsigned long * parent_rate)783 static long sclk_round_rate(struct clk_hw *hw, unsigned long rate,
784 			    unsigned long *parent_rate)
785 {
786 	return calc_best_divided_rate(rate, *parent_rate, SLEW_SYSDIV, 1);
787 }
788 
sclk_set_rate(struct clk_hw * hw,unsigned long rate,unsigned long parent_rate)789 static int sclk_set_rate(struct clk_hw *hw,
790 			 unsigned long rate, unsigned long parent_rate)
791 {
792 	struct pic32_sys_clk *sclk = clkhw_to_sys_clk(hw);
793 	unsigned long flags;
794 	u32 v, div;
795 	int err;
796 
797 	div = parent_rate / rate;
798 
799 	spin_lock_irqsave(&sclk->core->reg_lock, flags);
800 
801 	/* apply new div */
802 	v = readl(sclk->slew_reg);
803 	v &= ~(SLEW_SYSDIV << SLEW_SYSDIV_SHIFT);
804 	v |= (div - 1) << SLEW_SYSDIV_SHIFT;
805 
806 	pic32_syskey_unlock();
807 
808 	writel(v, sclk->slew_reg);
809 
810 	/* wait until BUSY is cleared */
811 	err = readl_poll_timeout_atomic(sclk->slew_reg, v,
812 					!(v & SLEW_BUSY), 1, LOCK_TIMEOUT_US);
813 
814 	spin_unlock_irqrestore(&sclk->core->reg_lock, flags);
815 
816 	return err;
817 }
818 
sclk_get_parent(struct clk_hw * hw)819 static u8 sclk_get_parent(struct clk_hw *hw)
820 {
821 	struct pic32_sys_clk *sclk = clkhw_to_sys_clk(hw);
822 	u32 i, v;
823 
824 	v = (readl(sclk->mux_reg) >> OSC_CUR_SHIFT) & OSC_CUR_MASK;
825 
826 	if (!sclk->parent_map)
827 		return v;
828 
829 	for (i = 0; i < clk_hw_get_num_parents(hw); i++)
830 		if (sclk->parent_map[i] == v)
831 			return i;
832 	return -EINVAL;
833 }
834 
sclk_set_parent(struct clk_hw * hw,u8 index)835 static int sclk_set_parent(struct clk_hw *hw, u8 index)
836 {
837 	struct pic32_sys_clk *sclk = clkhw_to_sys_clk(hw);
838 	unsigned long flags;
839 	u32 nosc, cosc, v;
840 	int err;
841 
842 	spin_lock_irqsave(&sclk->core->reg_lock, flags);
843 
844 	/* find new_osc */
845 	nosc = sclk->parent_map ? sclk->parent_map[index] : index;
846 
847 	/* set new parent */
848 	v = readl(sclk->mux_reg);
849 	v &= ~(OSC_NEW_MASK << OSC_NEW_SHIFT);
850 	v |= nosc << OSC_NEW_SHIFT;
851 
852 	pic32_syskey_unlock();
853 
854 	writel(v, sclk->mux_reg);
855 
856 	/* initate switch */
857 	writel(OSC_SWEN, PIC32_SET(sclk->mux_reg));
858 	cpu_relax();
859 
860 	/* add nop to flush pipeline (as cpu_clk is in-flux) */
861 	cpu_nop5();
862 
863 	/* wait for SWEN bit to clear */
864 	err = readl_poll_timeout_atomic(sclk->slew_reg, v,
865 					!(v & OSC_SWEN), 1, LOCK_TIMEOUT_US);
866 
867 	spin_unlock_irqrestore(&sclk->core->reg_lock, flags);
868 
869 	/*
870 	 * SCLK clock-switching logic might reject a clock switching request
871 	 * if pre-requisites (like new clk_src not present or unstable) are
872 	 * not met.
873 	 * So confirm before claiming success.
874 	 */
875 	cosc = (readl(sclk->mux_reg) >> OSC_CUR_SHIFT) & OSC_CUR_MASK;
876 	if (cosc != nosc) {
877 		pr_err("%s: err, failed to set_parent() to %d, current %d\n",
878 		       clk_hw_get_name(hw), nosc, cosc);
879 		err = -EBUSY;
880 	}
881 
882 	return err;
883 }
884 
sclk_init(struct clk_hw * hw)885 static int sclk_init(struct clk_hw *hw)
886 {
887 	struct pic32_sys_clk *sclk = clkhw_to_sys_clk(hw);
888 	unsigned long flags;
889 	u32 v;
890 
891 	/* Maintain reference to this clk, required in spll_clk_set_rate() */
892 	pic32_sclk_hw = hw;
893 
894 	/* apply slew divider on both up and down scaling */
895 	if (sclk->slew_div) {
896 		spin_lock_irqsave(&sclk->core->reg_lock, flags);
897 		v = readl(sclk->slew_reg);
898 		v &= ~(SLEW_DIV << SLEW_DIV_SHIFT);
899 		v |= sclk->slew_div << SLEW_DIV_SHIFT;
900 		v |= SLEW_DOWNEN | SLEW_UPEN;
901 		writel(v, sclk->slew_reg);
902 		spin_unlock_irqrestore(&sclk->core->reg_lock, flags);
903 	}
904 
905 	return 0;
906 }
907 
908 /* sclk with post-divider */
909 const struct clk_ops pic32_sclk_ops = {
910 	.get_parent	= sclk_get_parent,
911 	.set_parent	= sclk_set_parent,
912 	.round_rate	= sclk_round_rate,
913 	.set_rate	= sclk_set_rate,
914 	.recalc_rate	= sclk_get_rate,
915 	.init		= sclk_init,
916 	.determine_rate = __clk_mux_determine_rate,
917 };
918 
919 /* sclk with no slew and no post-divider */
920 const struct clk_ops pic32_sclk_no_div_ops = {
921 	.get_parent	= sclk_get_parent,
922 	.set_parent	= sclk_set_parent,
923 	.init		= sclk_init,
924 	.determine_rate = __clk_mux_determine_rate,
925 };
926 
pic32_sys_clk_register(const struct pic32_sys_clk_data * data,struct pic32_clk_common * core)927 struct clk *pic32_sys_clk_register(const struct pic32_sys_clk_data *data,
928 				   struct pic32_clk_common *core)
929 {
930 	struct pic32_sys_clk *sclk;
931 	struct clk *clk;
932 
933 	sclk = devm_kzalloc(core->dev, sizeof(*sclk), GFP_KERNEL);
934 	if (!sclk)
935 		return ERR_PTR(-ENOMEM);
936 
937 	sclk->core = core;
938 	sclk->hw.init = &data->init_data;
939 	sclk->mux_reg = data->mux_reg + core->iobase;
940 	sclk->slew_reg = data->slew_reg + core->iobase;
941 	sclk->slew_div = data->slew_div;
942 	sclk->parent_map = data->parent_map;
943 
944 	clk = devm_clk_register(core->dev, &sclk->hw);
945 	if (IS_ERR(clk))
946 		dev_err(core->dev, "%s: clk register failed\n", __func__);
947 
948 	return clk;
949 }
950 
951 /* secondary oscillator */
952 struct pic32_sec_osc {
953 	struct clk_hw hw;
954 	void __iomem *enable_reg;
955 	void __iomem *status_reg;
956 	u32 enable_mask;
957 	u32 status_mask;
958 	unsigned long fixed_rate;
959 	struct pic32_clk_common *core;
960 };
961 
962 #define clkhw_to_sosc(_hw)	container_of(_hw, struct pic32_sec_osc, hw)
sosc_clk_enable(struct clk_hw * hw)963 static int sosc_clk_enable(struct clk_hw *hw)
964 {
965 	struct pic32_sec_osc *sosc = clkhw_to_sosc(hw);
966 	u32 v;
967 
968 	/* enable SOSC */
969 	pic32_syskey_unlock();
970 	writel(sosc->enable_mask, PIC32_SET(sosc->enable_reg));
971 
972 	/* wait till warm-up period expires or ready-status is updated */
973 	return readl_poll_timeout_atomic(sosc->status_reg, v,
974 					 v & sosc->status_mask, 1, 100);
975 }
976 
sosc_clk_disable(struct clk_hw * hw)977 static void sosc_clk_disable(struct clk_hw *hw)
978 {
979 	struct pic32_sec_osc *sosc = clkhw_to_sosc(hw);
980 
981 	pic32_syskey_unlock();
982 	writel(sosc->enable_mask, PIC32_CLR(sosc->enable_reg));
983 }
984 
sosc_clk_is_enabled(struct clk_hw * hw)985 static int sosc_clk_is_enabled(struct clk_hw *hw)
986 {
987 	struct pic32_sec_osc *sosc = clkhw_to_sosc(hw);
988 	u32 enabled, ready;
989 
990 	/* check enabled and ready status */
991 	enabled = readl(sosc->enable_reg) & sosc->enable_mask;
992 	ready = readl(sosc->status_reg) & sosc->status_mask;
993 
994 	return enabled && ready;
995 }
996 
sosc_clk_calc_rate(struct clk_hw * hw,unsigned long parent_rate)997 static unsigned long sosc_clk_calc_rate(struct clk_hw *hw,
998 					unsigned long parent_rate)
999 {
1000 	return clkhw_to_sosc(hw)->fixed_rate;
1001 }
1002 
1003 const struct clk_ops pic32_sosc_ops = {
1004 	.enable = sosc_clk_enable,
1005 	.disable = sosc_clk_disable,
1006 	.is_enabled = sosc_clk_is_enabled,
1007 	.recalc_rate = sosc_clk_calc_rate,
1008 };
1009 
pic32_sosc_clk_register(const struct pic32_sec_osc_data * data,struct pic32_clk_common * core)1010 struct clk *pic32_sosc_clk_register(const struct pic32_sec_osc_data *data,
1011 				    struct pic32_clk_common *core)
1012 {
1013 	struct pic32_sec_osc *sosc;
1014 
1015 	sosc = devm_kzalloc(core->dev, sizeof(*sosc), GFP_KERNEL);
1016 	if (!sosc)
1017 		return ERR_PTR(-ENOMEM);
1018 
1019 	sosc->core = core;
1020 	sosc->hw.init = &data->init_data;
1021 	sosc->fixed_rate = data->fixed_rate;
1022 	sosc->enable_mask = data->enable_mask;
1023 	sosc->status_mask = data->status_mask;
1024 	sosc->enable_reg = data->enable_reg + core->iobase;
1025 	sosc->status_reg = data->status_reg + core->iobase;
1026 
1027 	return devm_clk_register(core->dev, &sosc->hw);
1028 }
1029