1 /*
2  * Copyright (C) 2014 STMicroelectronics (R&D) Limited
3  *
4  * This program is free software; you can redistribute it and/or modify
5  * it under the terms of the GNU General Public License as published by
6  * the Free Software Foundation; either version 2 of the License, or
7  * (at your option) any later version.
8  *
9  */
10 
11 /*
12  * Authors:
13  * Stephen Gallimore <stephen.gallimore@st.com>,
14  * Pankaj Dev <pankaj.dev@st.com>.
15  */
16 
17 #include <linux/slab.h>
18 #include <linux/of_address.h>
19 #include <linux/clk.h>
20 #include <linux/clk-provider.h>
21 #include <linux/iopoll.h>
22 
23 #include "clkgen.h"
24 
25 static DEFINE_SPINLOCK(clkgena_c32_odf_lock);
26 DEFINE_SPINLOCK(clkgen_a9_lock);
27 
28 /*
29  * PLL configuration register bits for PLL3200 C32
30  */
31 #define C32_NDIV_MASK (0xff)
32 #define C32_IDF_MASK (0x7)
33 #define C32_ODF_MASK (0x3f)
34 #define C32_LDF_MASK (0x7f)
35 #define C32_CP_MASK (0x1f)
36 
37 #define C32_MAX_ODFS (4)
38 
39 /*
40  * PLL configuration register bits for PLL4600 C28
41  */
42 #define C28_NDIV_MASK (0xff)
43 #define C28_IDF_MASK (0x7)
44 #define C28_ODF_MASK (0x3f)
45 
46 struct clkgen_pll_data {
47 	struct clkgen_field pdn_status;
48 	struct clkgen_field pdn_ctrl;
49 	struct clkgen_field locked_status;
50 	struct clkgen_field mdiv;
51 	struct clkgen_field ndiv;
52 	struct clkgen_field pdiv;
53 	struct clkgen_field idf;
54 	struct clkgen_field ldf;
55 	struct clkgen_field cp;
56 	unsigned int num_odfs;
57 	struct clkgen_field odf[C32_MAX_ODFS];
58 	struct clkgen_field odf_gate[C32_MAX_ODFS];
59 	bool switch2pll_en;
60 	struct clkgen_field switch2pll;
61 	spinlock_t *lock;
62 	const struct clk_ops *ops;
63 };
64 
65 static const struct clk_ops stm_pll3200c32_ops;
66 static const struct clk_ops stm_pll3200c32_a9_ops;
67 static const struct clk_ops stm_pll4600c28_ops;
68 
69 static const struct clkgen_pll_data st_pll3200c32_407_a0 = {
70 	/* 407 A0 */
71 	.pdn_status	= CLKGEN_FIELD(0x2a0,	0x1,			8),
72 	.pdn_ctrl	= CLKGEN_FIELD(0x2a0,	0x1,			8),
73 	.locked_status	= CLKGEN_FIELD(0x2a0,	0x1,			24),
74 	.ndiv		= CLKGEN_FIELD(0x2a4,	C32_NDIV_MASK,		16),
75 	.idf		= CLKGEN_FIELD(0x2a4,	C32_IDF_MASK,		0x0),
76 	.num_odfs = 1,
77 	.odf		= { CLKGEN_FIELD(0x2b4, C32_ODF_MASK,		0) },
78 	.odf_gate	= { CLKGEN_FIELD(0x2b4,	0x1,			6) },
79 	.ops		= &stm_pll3200c32_ops,
80 };
81 
82 static const struct clkgen_pll_data st_pll3200c32_cx_0 = {
83 	/* 407 C0 PLL0 */
84 	.pdn_status	= CLKGEN_FIELD(0x2a0,	0x1,			8),
85 	.pdn_ctrl	= CLKGEN_FIELD(0x2a0,	0x1,			8),
86 	.locked_status	= CLKGEN_FIELD(0x2a0,	0x1,			24),
87 	.ndiv		= CLKGEN_FIELD(0x2a4,	C32_NDIV_MASK,		16),
88 	.idf		= CLKGEN_FIELD(0x2a4,	C32_IDF_MASK,		0x0),
89 	.num_odfs = 1,
90 	.odf		= { CLKGEN_FIELD(0x2b4, C32_ODF_MASK,		0) },
91 	.odf_gate	= { CLKGEN_FIELD(0x2b4, 0x1,			6) },
92 	.ops		= &stm_pll3200c32_ops,
93 };
94 
95 static const struct clkgen_pll_data st_pll3200c32_cx_1 = {
96 	/* 407 C0 PLL1 */
97 	.pdn_status	= CLKGEN_FIELD(0x2c8,	0x1,			8),
98 	.pdn_ctrl	= CLKGEN_FIELD(0x2c8,	0x1,			8),
99 	.locked_status	= CLKGEN_FIELD(0x2c8,	0x1,			24),
100 	.ndiv		= CLKGEN_FIELD(0x2cc,	C32_NDIV_MASK,		16),
101 	.idf		= CLKGEN_FIELD(0x2cc,	C32_IDF_MASK,		0x0),
102 	.num_odfs = 1,
103 	.odf		= { CLKGEN_FIELD(0x2dc, C32_ODF_MASK,		0) },
104 	.odf_gate	= { CLKGEN_FIELD(0x2dc, 0x1,			6) },
105 	.ops		= &stm_pll3200c32_ops,
106 };
107 
108 static const struct clkgen_pll_data st_pll3200c32_407_a9 = {
109 	/* 407 A9 */
110 	.pdn_status	= CLKGEN_FIELD(0x1a8,	0x1,			0),
111 	.pdn_ctrl	= CLKGEN_FIELD(0x1a8,	0x1,			0),
112 	.locked_status	= CLKGEN_FIELD(0x87c,	0x1,			0),
113 	.ndiv		= CLKGEN_FIELD(0x1b0,	C32_NDIV_MASK,		0),
114 	.idf		= CLKGEN_FIELD(0x1a8,	C32_IDF_MASK,		25),
115 	.num_odfs = 1,
116 	.odf		= { CLKGEN_FIELD(0x1b0, C32_ODF_MASK,		8) },
117 	.odf_gate	= { CLKGEN_FIELD(0x1ac, 0x1,			28) },
118 	.switch2pll_en	= true,
119 	.cp		= CLKGEN_FIELD(0x1a8,	C32_CP_MASK,		1),
120 	.switch2pll	= CLKGEN_FIELD(0x1a4,	0x1,			1),
121 	.lock = &clkgen_a9_lock,
122 	.ops		= &stm_pll3200c32_a9_ops,
123 };
124 
125 static struct clkgen_pll_data st_pll4600c28_418_a9 = {
126 	/* 418 A9 */
127 	.pdn_status	= CLKGEN_FIELD(0x1a8,	0x1,			0),
128 	.pdn_ctrl	= CLKGEN_FIELD(0x1a8,	0x1,			0),
129 	.locked_status	= CLKGEN_FIELD(0x87c,	0x1,			0),
130 	.ndiv		= CLKGEN_FIELD(0x1b0,	C28_NDIV_MASK,		0),
131 	.idf		= CLKGEN_FIELD(0x1a8,	C28_IDF_MASK,		25),
132 	.num_odfs = 1,
133 	.odf		= { CLKGEN_FIELD(0x1b0, C28_ODF_MASK,		8) },
134 	.odf_gate	= { CLKGEN_FIELD(0x1ac, 0x1,			28) },
135 	.switch2pll_en	= true,
136 	.switch2pll	= CLKGEN_FIELD(0x1a4,	0x1,			1),
137 	.lock		= &clkgen_a9_lock,
138 	.ops		= &stm_pll4600c28_ops,
139 };
140 
141 /**
142  * DOC: Clock Generated by PLL, rate set and enabled by bootloader
143  *
144  * Traits of this clock:
145  * prepare - clk_(un)prepare only ensures parent is (un)prepared
146  * enable - clk_enable/disable only ensures parent is enabled
147  * rate - rate is fixed. No clk_set_rate support
148  * parent - fixed parent.  No clk_set_parent support
149  */
150 
151 /**
152  * PLL clock that is integrated in the ClockGenA instances on the STiH415
153  * and STiH416.
154  *
155  * @hw: handle between common and hardware-specific interfaces.
156  * @type: PLL instance type.
157  * @regs_base: base of the PLL configuration register(s).
158  *
159  */
160 struct clkgen_pll {
161 	struct clk_hw		hw;
162 	struct clkgen_pll_data	*data;
163 	void __iomem		*regs_base;
164 	spinlock_t	*lock;
165 
166 	u32 ndiv;
167 	u32 idf;
168 	u32 odf;
169 	u32 cp;
170 };
171 
172 #define to_clkgen_pll(_hw) container_of(_hw, struct clkgen_pll, hw)
173 
174 struct stm_pll {
175 	unsigned long mdiv;
176 	unsigned long ndiv;
177 	unsigned long pdiv;
178 	unsigned long odf;
179 	unsigned long idf;
180 	unsigned long ldf;
181 	unsigned long cp;
182 };
183 
clkgen_pll_is_locked(struct clk_hw * hw)184 static int clkgen_pll_is_locked(struct clk_hw *hw)
185 {
186 	struct clkgen_pll *pll = to_clkgen_pll(hw);
187 	u32 locked = CLKGEN_READ(pll, locked_status);
188 
189 	return !!locked;
190 }
191 
clkgen_pll_is_enabled(struct clk_hw * hw)192 static int clkgen_pll_is_enabled(struct clk_hw *hw)
193 {
194 	struct clkgen_pll *pll = to_clkgen_pll(hw);
195 	u32 poweroff = CLKGEN_READ(pll, pdn_status);
196 	return !poweroff;
197 }
198 
__clkgen_pll_enable(struct clk_hw * hw)199 static int __clkgen_pll_enable(struct clk_hw *hw)
200 {
201 	struct clkgen_pll *pll = to_clkgen_pll(hw);
202 	void __iomem *base =  pll->regs_base;
203 	struct clkgen_field *field = &pll->data->locked_status;
204 	int ret = 0;
205 	u32 reg;
206 
207 	if (clkgen_pll_is_enabled(hw))
208 		return 0;
209 
210 	CLKGEN_WRITE(pll, pdn_ctrl, 0);
211 
212 	ret = readl_relaxed_poll_timeout(base + field->offset, reg,
213 			!!((reg >> field->shift) & field->mask),  0, 10000);
214 
215 	if (!ret) {
216 		if (pll->data->switch2pll_en)
217 			CLKGEN_WRITE(pll, switch2pll, 0);
218 
219 		pr_debug("%s:%s enabled\n", __clk_get_name(hw->clk), __func__);
220 	}
221 
222 	return ret;
223 }
224 
clkgen_pll_enable(struct clk_hw * hw)225 static int clkgen_pll_enable(struct clk_hw *hw)
226 {
227 	struct clkgen_pll *pll = to_clkgen_pll(hw);
228 	unsigned long flags = 0;
229 	int ret = 0;
230 
231 	if (pll->lock)
232 		spin_lock_irqsave(pll->lock, flags);
233 
234 	ret = __clkgen_pll_enable(hw);
235 
236 	if (pll->lock)
237 		spin_unlock_irqrestore(pll->lock, flags);
238 
239 	return ret;
240 }
241 
__clkgen_pll_disable(struct clk_hw * hw)242 static void __clkgen_pll_disable(struct clk_hw *hw)
243 {
244 	struct clkgen_pll *pll = to_clkgen_pll(hw);
245 
246 	if (!clkgen_pll_is_enabled(hw))
247 		return;
248 
249 	if (pll->data->switch2pll_en)
250 		CLKGEN_WRITE(pll, switch2pll, 1);
251 
252 	CLKGEN_WRITE(pll, pdn_ctrl, 1);
253 
254 	pr_debug("%s:%s disabled\n", __clk_get_name(hw->clk), __func__);
255 }
256 
clkgen_pll_disable(struct clk_hw * hw)257 static void clkgen_pll_disable(struct clk_hw *hw)
258 {
259 	struct clkgen_pll *pll = to_clkgen_pll(hw);
260 	unsigned long flags = 0;
261 
262 	if (pll->lock)
263 		spin_lock_irqsave(pll->lock, flags);
264 
265 	__clkgen_pll_disable(hw);
266 
267 	if (pll->lock)
268 		spin_unlock_irqrestore(pll->lock, flags);
269 }
270 
clk_pll3200c32_get_params(unsigned long input,unsigned long output,struct stm_pll * pll)271 static int clk_pll3200c32_get_params(unsigned long input, unsigned long output,
272 			  struct stm_pll *pll)
273 {
274 	unsigned long i, n;
275 	unsigned long deviation = ~0;
276 	unsigned long new_freq;
277 	long new_deviation;
278 	/* Charge pump table: highest ndiv value for cp=6 to 25 */
279 	static const unsigned char cp_table[] = {
280 		48, 56, 64, 72, 80, 88, 96, 104, 112, 120,
281 		128, 136, 144, 152, 160, 168, 176, 184, 192
282 	};
283 
284 	/* Output clock range: 800Mhz to 1600Mhz */
285 	if (output < 800000000 || output > 1600000000)
286 		return -EINVAL;
287 
288 	input /= 1000;
289 	output /= 1000;
290 
291 	for (i = 1; i <= 7 && deviation; i++) {
292 		n = i * output / (2 * input);
293 
294 		/* Checks */
295 		if (n < 8)
296 			continue;
297 		if (n > 200)
298 			break;
299 
300 		new_freq = (input * 2 * n) / i;
301 
302 		new_deviation = abs(new_freq - output);
303 
304 		if (!new_deviation || new_deviation < deviation) {
305 			pll->idf  = i;
306 			pll->ndiv = n;
307 			deviation = new_deviation;
308 		}
309 	}
310 
311 	if (deviation == ~0) /* No solution found */
312 		return -EINVAL;
313 
314 	/* Computing recommended charge pump value */
315 	for (pll->cp = 6; pll->ndiv > cp_table[pll->cp-6]; (pll->cp)++)
316 		;
317 
318 	return 0;
319 }
320 
clk_pll3200c32_get_rate(unsigned long input,struct stm_pll * pll,unsigned long * rate)321 static int clk_pll3200c32_get_rate(unsigned long input, struct stm_pll *pll,
322 			unsigned long *rate)
323 {
324 	if (!pll->idf)
325 		pll->idf = 1;
326 
327 	*rate = ((2 * (input / 1000) * pll->ndiv) / pll->idf) * 1000;
328 
329 	return 0;
330 }
331 
recalc_stm_pll3200c32(struct clk_hw * hw,unsigned long parent_rate)332 static unsigned long recalc_stm_pll3200c32(struct clk_hw *hw,
333 		unsigned long parent_rate)
334 {
335 	struct clkgen_pll *pll = to_clkgen_pll(hw);
336 	unsigned long ndiv, idf;
337 	unsigned long rate = 0;
338 
339 	if (!clkgen_pll_is_enabled(hw) || !clkgen_pll_is_locked(hw))
340 		return 0;
341 
342 	ndiv = CLKGEN_READ(pll, ndiv);
343 	idf = CLKGEN_READ(pll, idf);
344 
345 	if (idf)
346 		/* Note: input is divided to avoid overflow */
347 		rate = ((2 * (parent_rate/1000) * ndiv) / idf) * 1000;
348 
349 	pr_debug("%s:%s rate %lu\n", clk_hw_get_name(hw), __func__, rate);
350 
351 	return rate;
352 }
353 
round_rate_stm_pll3200c32(struct clk_hw * hw,unsigned long rate,unsigned long * prate)354 static long round_rate_stm_pll3200c32(struct clk_hw *hw, unsigned long rate,
355 		unsigned long *prate)
356 {
357 	struct stm_pll params;
358 
359 	if (!clk_pll3200c32_get_params(*prate, rate, &params))
360 		clk_pll3200c32_get_rate(*prate, &params, &rate);
361 	else {
362 		pr_debug("%s: %s rate %ld Invalid\n", __func__,
363 			 __clk_get_name(hw->clk), rate);
364 		return 0;
365 	}
366 
367 	pr_debug("%s: %s new rate %ld [ndiv=%u] [idf=%u]\n",
368 		 __func__, __clk_get_name(hw->clk),
369 		 rate, (unsigned int)params.ndiv,
370 		 (unsigned int)params.idf);
371 
372 	return rate;
373 }
374 
set_rate_stm_pll3200c32(struct clk_hw * hw,unsigned long rate,unsigned long parent_rate)375 static int set_rate_stm_pll3200c32(struct clk_hw *hw, unsigned long rate,
376 				unsigned long parent_rate)
377 {
378 	struct clkgen_pll *pll = to_clkgen_pll(hw);
379 	struct stm_pll params;
380 	long hwrate = 0;
381 	unsigned long flags = 0;
382 
383 	if (!rate || !parent_rate)
384 		return -EINVAL;
385 
386 	if (!clk_pll3200c32_get_params(parent_rate, rate, &params))
387 		clk_pll3200c32_get_rate(parent_rate, &params, &hwrate);
388 
389 	pr_debug("%s: %s new rate %ld [ndiv=0x%x] [idf=0x%x]\n",
390 		 __func__, __clk_get_name(hw->clk),
391 		 hwrate, (unsigned int)params.ndiv,
392 		 (unsigned int)params.idf);
393 
394 	if (!hwrate)
395 		return -EINVAL;
396 
397 	pll->ndiv = params.ndiv;
398 	pll->idf = params.idf;
399 	pll->cp = params.cp;
400 
401 	__clkgen_pll_disable(hw);
402 
403 	if (pll->lock)
404 		spin_lock_irqsave(pll->lock, flags);
405 
406 	CLKGEN_WRITE(pll, ndiv, pll->ndiv);
407 	CLKGEN_WRITE(pll, idf, pll->idf);
408 	CLKGEN_WRITE(pll, cp, pll->cp);
409 
410 	if (pll->lock)
411 		spin_unlock_irqrestore(pll->lock, flags);
412 
413 	__clkgen_pll_enable(hw);
414 
415 	return 0;
416 }
417 
418 /* PLL output structure
419  * FVCO >> /2 >> FVCOBY2 (no output)
420  *                 |> Divider (ODF) >> PHI
421  *
422  * FVCOby2 output = (input * 2 * NDIV) / IDF (assuming FRAC_CONTROL==L)
423  *
424  * Rules:
425  *   4Mhz <= INFF input <= 350Mhz
426  *   4Mhz <= INFIN (INFF / IDF) <= 50Mhz
427  *   19.05Mhz <= FVCOby2 output (PHI w ODF=1) <= 3000Mhz
428  *   1 <= i (register/dec value for IDF) <= 7
429  *   8 <= n (register/dec value for NDIV) <= 246
430  */
431 
clk_pll4600c28_get_params(unsigned long input,unsigned long output,struct stm_pll * pll)432 static int clk_pll4600c28_get_params(unsigned long input, unsigned long output,
433 			  struct stm_pll *pll)
434 {
435 
436 	unsigned long i, infin, n;
437 	unsigned long deviation = ~0;
438 	unsigned long new_freq, new_deviation;
439 
440 	/* Output clock range: 19Mhz to 3000Mhz */
441 	if (output < 19000000 || output > 3000000000u)
442 		return -EINVAL;
443 
444 	/* For better jitter, IDF should be smallest and NDIV must be maximum */
445 	for (i = 1; i <= 7 && deviation; i++) {
446 		/* INFIN checks */
447 		infin = input / i;
448 		if (infin < 4000000 || infin > 50000000)
449 			continue;	/* Invalid case */
450 
451 		n = output / (infin * 2);
452 		if (n < 8 || n > 246)
453 			continue;	/* Invalid case */
454 		if (n < 246)
455 			n++;	/* To work around 'y' when n=x.y */
456 
457 		for (; n >= 8 && deviation; n--) {
458 			new_freq = infin * 2 * n;
459 			if (new_freq < output)
460 				break;	/* Optimization: shorting loop */
461 
462 			new_deviation = new_freq - output;
463 			if (!new_deviation || new_deviation < deviation) {
464 				pll->idf  = i;
465 				pll->ndiv = n;
466 				deviation = new_deviation;
467 			}
468 		}
469 	}
470 
471 	if (deviation == ~0) /* No solution found */
472 		return -EINVAL;
473 
474 	return 0;
475 }
476 
clk_pll4600c28_get_rate(unsigned long input,struct stm_pll * pll,unsigned long * rate)477 static int clk_pll4600c28_get_rate(unsigned long input, struct stm_pll *pll,
478 			unsigned long *rate)
479 {
480 	if (!pll->idf)
481 		pll->idf = 1;
482 
483 	*rate = (input / pll->idf) * 2 * pll->ndiv;
484 
485 	return 0;
486 }
487 
recalc_stm_pll4600c28(struct clk_hw * hw,unsigned long parent_rate)488 static unsigned long recalc_stm_pll4600c28(struct clk_hw *hw,
489 				    unsigned long parent_rate)
490 {
491 	struct clkgen_pll *pll = to_clkgen_pll(hw);
492 	struct stm_pll params;
493 	unsigned long rate;
494 
495 	if (!clkgen_pll_is_enabled(hw) || !clkgen_pll_is_locked(hw))
496 		return 0;
497 
498 	params.ndiv = CLKGEN_READ(pll, ndiv);
499 	params.idf = CLKGEN_READ(pll, idf);
500 
501 	clk_pll4600c28_get_rate(parent_rate, &params, &rate);
502 
503 	pr_debug("%s:%s rate %lu\n", __clk_get_name(hw->clk), __func__, rate);
504 
505 	return rate;
506 }
507 
round_rate_stm_pll4600c28(struct clk_hw * hw,unsigned long rate,unsigned long * prate)508 static long round_rate_stm_pll4600c28(struct clk_hw *hw, unsigned long rate,
509 				      unsigned long *prate)
510 {
511 	struct stm_pll params;
512 
513 	if (!clk_pll4600c28_get_params(*prate, rate, &params)) {
514 		clk_pll4600c28_get_rate(*prate, &params, &rate);
515 	} else {
516 		pr_debug("%s: %s rate %ld Invalid\n", __func__,
517 			 __clk_get_name(hw->clk), rate);
518 		return 0;
519 	}
520 
521 	pr_debug("%s: %s new rate %ld [ndiv=%u] [idf=%u]\n",
522 		 __func__, __clk_get_name(hw->clk),
523 		 rate, (unsigned int)params.ndiv,
524 		 (unsigned int)params.idf);
525 
526 	return rate;
527 }
528 
set_rate_stm_pll4600c28(struct clk_hw * hw,unsigned long rate,unsigned long parent_rate)529 static int set_rate_stm_pll4600c28(struct clk_hw *hw, unsigned long rate,
530 				   unsigned long parent_rate)
531 {
532 	struct clkgen_pll *pll = to_clkgen_pll(hw);
533 	struct stm_pll params;
534 	long hwrate;
535 	unsigned long flags = 0;
536 
537 	if (!rate || !parent_rate)
538 		return -EINVAL;
539 
540 	if (!clk_pll4600c28_get_params(parent_rate, rate, &params)) {
541 		clk_pll4600c28_get_rate(parent_rate, &params, &hwrate);
542 	} else {
543 		pr_debug("%s: %s rate %ld Invalid\n", __func__,
544 			 __clk_get_name(hw->clk), rate);
545 		return -EINVAL;
546 	}
547 
548 	pr_debug("%s: %s new rate %ld [ndiv=0x%x] [idf=0x%x]\n",
549 		 __func__, __clk_get_name(hw->clk),
550 		 hwrate, (unsigned int)params.ndiv,
551 		 (unsigned int)params.idf);
552 
553 	if (!hwrate)
554 		return -EINVAL;
555 
556 	pll->ndiv = params.ndiv;
557 	pll->idf = params.idf;
558 
559 	__clkgen_pll_disable(hw);
560 
561 	if (pll->lock)
562 		spin_lock_irqsave(pll->lock, flags);
563 
564 	CLKGEN_WRITE(pll, ndiv, pll->ndiv);
565 	CLKGEN_WRITE(pll, idf, pll->idf);
566 
567 	if (pll->lock)
568 		spin_unlock_irqrestore(pll->lock, flags);
569 
570 	__clkgen_pll_enable(hw);
571 
572 	return 0;
573 }
574 
575 static const struct clk_ops stm_pll3200c32_ops = {
576 	.enable		= clkgen_pll_enable,
577 	.disable	= clkgen_pll_disable,
578 	.is_enabled	= clkgen_pll_is_enabled,
579 	.recalc_rate	= recalc_stm_pll3200c32,
580 };
581 
582 static const struct clk_ops stm_pll3200c32_a9_ops = {
583 	.enable		= clkgen_pll_enable,
584 	.disable	= clkgen_pll_disable,
585 	.is_enabled	= clkgen_pll_is_enabled,
586 	.recalc_rate	= recalc_stm_pll3200c32,
587 	.round_rate	= round_rate_stm_pll3200c32,
588 	.set_rate	= set_rate_stm_pll3200c32,
589 };
590 
591 static const struct clk_ops stm_pll4600c28_ops = {
592 	.enable		= clkgen_pll_enable,
593 	.disable	= clkgen_pll_disable,
594 	.is_enabled	= clkgen_pll_is_enabled,
595 	.recalc_rate	= recalc_stm_pll4600c28,
596 	.round_rate	= round_rate_stm_pll4600c28,
597 	.set_rate	= set_rate_stm_pll4600c28,
598 };
599 
clkgen_pll_register(const char * parent_name,struct clkgen_pll_data * pll_data,void __iomem * reg,unsigned long pll_flags,const char * clk_name,spinlock_t * lock)600 static struct clk * __init clkgen_pll_register(const char *parent_name,
601 				struct clkgen_pll_data	*pll_data,
602 				void __iomem *reg, unsigned long pll_flags,
603 				const char *clk_name, spinlock_t *lock)
604 {
605 	struct clkgen_pll *pll;
606 	struct clk *clk;
607 	struct clk_init_data init;
608 
609 	pll = kzalloc(sizeof(*pll), GFP_KERNEL);
610 	if (!pll)
611 		return ERR_PTR(-ENOMEM);
612 
613 	init.name = clk_name;
614 	init.ops = pll_data->ops;
615 
616 	init.flags = pll_flags | CLK_IS_BASIC | CLK_GET_RATE_NOCACHE;
617 	init.parent_names = &parent_name;
618 	init.num_parents  = 1;
619 
620 	pll->data = pll_data;
621 	pll->regs_base = reg;
622 	pll->hw.init = &init;
623 	pll->lock = lock;
624 
625 	clk = clk_register(NULL, &pll->hw);
626 	if (IS_ERR(clk)) {
627 		kfree(pll);
628 		return clk;
629 	}
630 
631 	pr_debug("%s: parent %s rate %lu\n",
632 			__clk_get_name(clk),
633 			__clk_get_name(clk_get_parent(clk)),
634 			clk_get_rate(clk));
635 
636 	return clk;
637 }
638 
clkgen_get_register_base(struct device_node * np)639 static void __iomem * __init clkgen_get_register_base(
640 				struct device_node *np)
641 {
642 	struct device_node *pnode;
643 	void __iomem *reg = NULL;
644 
645 	pnode = of_get_parent(np);
646 	if (!pnode)
647 		return NULL;
648 
649 	reg = of_iomap(pnode, 0);
650 
651 	of_node_put(pnode);
652 	return reg;
653 }
654 
clkgen_odf_register(const char * parent_name,void __iomem * reg,struct clkgen_pll_data * pll_data,unsigned long pll_flags,int odf,spinlock_t * odf_lock,const char * odf_name)655 static struct clk * __init clkgen_odf_register(const char *parent_name,
656 					       void __iomem *reg,
657 					       struct clkgen_pll_data *pll_data,
658 					       unsigned long pll_flags, int odf,
659 					       spinlock_t *odf_lock,
660 					       const char *odf_name)
661 {
662 	struct clk *clk;
663 	unsigned long flags;
664 	struct clk_gate *gate;
665 	struct clk_divider *div;
666 
667 	flags = pll_flags | CLK_GET_RATE_NOCACHE | CLK_SET_RATE_PARENT;
668 
669 	gate = kzalloc(sizeof(*gate), GFP_KERNEL);
670 	if (!gate)
671 		return ERR_PTR(-ENOMEM);
672 
673 	gate->flags = CLK_GATE_SET_TO_DISABLE;
674 	gate->reg = reg + pll_data->odf_gate[odf].offset;
675 	gate->bit_idx = pll_data->odf_gate[odf].shift;
676 	gate->lock = odf_lock;
677 
678 	div = kzalloc(sizeof(*div), GFP_KERNEL);
679 	if (!div) {
680 		kfree(gate);
681 		return ERR_PTR(-ENOMEM);
682 	}
683 
684 	div->flags = CLK_DIVIDER_ONE_BASED | CLK_DIVIDER_ALLOW_ZERO;
685 	div->reg = reg + pll_data->odf[odf].offset;
686 	div->shift = pll_data->odf[odf].shift;
687 	div->width = fls(pll_data->odf[odf].mask);
688 	div->lock = odf_lock;
689 
690 	clk = clk_register_composite(NULL, odf_name, &parent_name, 1,
691 				     NULL, NULL,
692 				     &div->hw, &clk_divider_ops,
693 				     &gate->hw, &clk_gate_ops,
694 				     flags);
695 	if (IS_ERR(clk))
696 		return clk;
697 
698 	pr_debug("%s: parent %s rate %lu\n",
699 			__clk_get_name(clk),
700 			__clk_get_name(clk_get_parent(clk)),
701 			clk_get_rate(clk));
702 	return clk;
703 }
704 
705 
clkgen_c32_pll_setup(struct device_node * np,struct clkgen_pll_data * data)706 static void __init clkgen_c32_pll_setup(struct device_node *np,
707 		struct clkgen_pll_data *data)
708 {
709 	struct clk *clk;
710 	const char *parent_name, *pll_name;
711 	void __iomem *pll_base;
712 	int num_odfs, odf;
713 	struct clk_onecell_data *clk_data;
714 	unsigned long pll_flags = 0;
715 
716 
717 	parent_name = of_clk_get_parent_name(np, 0);
718 	if (!parent_name)
719 		return;
720 
721 	pll_base = clkgen_get_register_base(np);
722 	if (!pll_base)
723 		return;
724 
725 	of_clk_detect_critical(np, 0, &pll_flags);
726 
727 	clk = clkgen_pll_register(parent_name, data, pll_base, pll_flags,
728 				  np->name, data->lock);
729 	if (IS_ERR(clk))
730 		return;
731 
732 	pll_name = __clk_get_name(clk);
733 
734 	num_odfs = data->num_odfs;
735 
736 	clk_data = kzalloc(sizeof(*clk_data), GFP_KERNEL);
737 	if (!clk_data)
738 		return;
739 
740 	clk_data->clk_num = num_odfs;
741 	clk_data->clks = kcalloc(clk_data->clk_num, sizeof(struct clk *),
742 				 GFP_KERNEL);
743 
744 	if (!clk_data->clks)
745 		goto err;
746 
747 	for (odf = 0; odf < num_odfs; odf++) {
748 		struct clk *clk;
749 		const char *clk_name;
750 		unsigned long odf_flags = 0;
751 
752 		if (of_property_read_string_index(np, "clock-output-names",
753 						  odf, &clk_name))
754 			return;
755 
756 		of_clk_detect_critical(np, odf, &odf_flags);
757 
758 		clk = clkgen_odf_register(pll_name, pll_base, data, odf_flags,
759 				odf, &clkgena_c32_odf_lock, clk_name);
760 		if (IS_ERR(clk))
761 			goto err;
762 
763 		clk_data->clks[odf] = clk;
764 	}
765 
766 	of_clk_add_provider(np, of_clk_src_onecell_get, clk_data);
767 	return;
768 
769 err:
770 	kfree(pll_name);
771 	kfree(clk_data->clks);
772 	kfree(clk_data);
773 }
clkgen_c32_pll0_setup(struct device_node * np)774 static void __init clkgen_c32_pll0_setup(struct device_node *np)
775 {
776 	clkgen_c32_pll_setup(np,
777 			(struct clkgen_pll_data *) &st_pll3200c32_cx_0);
778 }
779 CLK_OF_DECLARE(c32_pll0, "st,clkgen-pll0", clkgen_c32_pll0_setup);
780 
clkgen_c32_pll1_setup(struct device_node * np)781 static void __init clkgen_c32_pll1_setup(struct device_node *np)
782 {
783 	clkgen_c32_pll_setup(np,
784 			(struct clkgen_pll_data *) &st_pll3200c32_cx_1);
785 }
786 CLK_OF_DECLARE(c32_pll1, "st,clkgen-pll1", clkgen_c32_pll1_setup);
787 
clkgen_c32_plla9_setup(struct device_node * np)788 static void __init clkgen_c32_plla9_setup(struct device_node *np)
789 {
790 	clkgen_c32_pll_setup(np,
791 			(struct clkgen_pll_data *) &st_pll3200c32_407_a9);
792 }
793 CLK_OF_DECLARE(c32_plla9, "st,stih407-clkgen-plla9", clkgen_c32_plla9_setup);
794 
clkgen_c28_plla9_setup(struct device_node * np)795 static void __init clkgen_c28_plla9_setup(struct device_node *np)
796 {
797 	clkgen_c32_pll_setup(np,
798 			(struct clkgen_pll_data *) &st_pll4600c28_418_a9);
799 }
800 CLK_OF_DECLARE(c28_plla9, "st,stih418-clkgen-plla9", clkgen_c28_plla9_setup);
801