xref: /openbmc/linux/drivers/clk/st/clkgen-mux.c (revision 0edbfea5)
1 /*
2  * clkgen-mux.c: ST GEN-MUX Clock driver
3  *
4  * Copyright (C) 2014 STMicroelectronics (R&D) Limited
5  *
6  * Authors: Stephen Gallimore <stephen.gallimore@st.com>
7  *	    Pankaj Dev <pankaj.dev@st.com>
8  *
9  * This program is free software; you can redistribute it and/or modify
10  * it under the terms of the GNU General Public License as published by
11  * the Free Software Foundation; either version 2 of the License, or
12  * (at your option) any later version.
13  *
14  */
15 
16 #include <linux/slab.h>
17 #include <linux/of_address.h>
18 #include <linux/clk.h>
19 #include <linux/clk-provider.h>
20 #include "clkgen.h"
21 
22 static DEFINE_SPINLOCK(clkgena_divmux_lock);
23 static DEFINE_SPINLOCK(clkgenf_lock);
24 
25 static const char ** __init clkgen_mux_get_parents(struct device_node *np,
26 						       int *num_parents)
27 {
28 	const char **parents;
29 	unsigned int nparents;
30 
31 	nparents = of_clk_get_parent_count(np);
32 	if (WARN_ON(!nparents))
33 		return ERR_PTR(-EINVAL);
34 
35 	parents = kcalloc(nparents, sizeof(const char *), GFP_KERNEL);
36 	if (!parents)
37 		return ERR_PTR(-ENOMEM);
38 
39 	*num_parents = of_clk_parent_fill(np, parents, nparents);
40 	return parents;
41 }
42 
43 /**
44  * DOC: Clock mux with a programmable divider on each of its three inputs.
45  *      The mux has an input setting which effectively gates its output.
46  *
47  * Traits of this clock:
48  * prepare - clk_(un)prepare only ensures parent is (un)prepared
49  * enable - clk_enable and clk_disable are functional & control gating
50  * rate - set rate is supported
51  * parent - set/get parent
52  */
53 
54 #define NUM_INPUTS 3
55 
56 struct clkgena_divmux {
57 	struct clk_hw hw;
58 	/* Subclassed mux and divider structures */
59 	struct clk_mux mux;
60 	struct clk_divider div[NUM_INPUTS];
61 	/* Enable/running feedback register bits for each input */
62 	void __iomem *feedback_reg[NUM_INPUTS];
63 	int feedback_bit_idx;
64 
65 	u8              muxsel;
66 };
67 
68 #define to_clkgena_divmux(_hw) container_of(_hw, struct clkgena_divmux, hw)
69 
70 struct clkgena_divmux_data {
71 	int num_outputs;
72 	int mux_offset;
73 	int mux_offset2;
74 	int mux_start_bit;
75 	int div_offsets[NUM_INPUTS];
76 	int fb_offsets[NUM_INPUTS];
77 	int fb_start_bit_idx;
78 };
79 
80 #define CKGAX_CLKOPSRC_SWITCH_OFF 0x3
81 
82 static int clkgena_divmux_is_running(struct clkgena_divmux *mux)
83 {
84 	u32 regval = readl(mux->feedback_reg[mux->muxsel]);
85 	u32 running = regval & BIT(mux->feedback_bit_idx);
86 	return !!running;
87 }
88 
89 static int clkgena_divmux_enable(struct clk_hw *hw)
90 {
91 	struct clkgena_divmux *genamux = to_clkgena_divmux(hw);
92 	struct clk_hw *mux_hw = &genamux->mux.hw;
93 	unsigned long timeout;
94 	int ret = 0;
95 
96 	__clk_hw_set_clk(mux_hw, hw);
97 
98 	ret = clk_mux_ops.set_parent(mux_hw, genamux->muxsel);
99 	if (ret)
100 		return ret;
101 
102 	timeout = jiffies + msecs_to_jiffies(10);
103 
104 	while (!clkgena_divmux_is_running(genamux)) {
105 		if (time_after(jiffies, timeout))
106 			return -ETIMEDOUT;
107 		cpu_relax();
108 	}
109 
110 	return 0;
111 }
112 
113 static void clkgena_divmux_disable(struct clk_hw *hw)
114 {
115 	struct clkgena_divmux *genamux = to_clkgena_divmux(hw);
116 	struct clk_hw *mux_hw = &genamux->mux.hw;
117 
118 	__clk_hw_set_clk(mux_hw, hw);
119 
120 	clk_mux_ops.set_parent(mux_hw, CKGAX_CLKOPSRC_SWITCH_OFF);
121 }
122 
123 static int clkgena_divmux_is_enabled(struct clk_hw *hw)
124 {
125 	struct clkgena_divmux *genamux = to_clkgena_divmux(hw);
126 	struct clk_hw *mux_hw = &genamux->mux.hw;
127 
128 	__clk_hw_set_clk(mux_hw, hw);
129 
130 	return (s8)clk_mux_ops.get_parent(mux_hw) > 0;
131 }
132 
133 static u8 clkgena_divmux_get_parent(struct clk_hw *hw)
134 {
135 	struct clkgena_divmux *genamux = to_clkgena_divmux(hw);
136 	struct clk_hw *mux_hw = &genamux->mux.hw;
137 
138 	__clk_hw_set_clk(mux_hw, hw);
139 
140 	genamux->muxsel = clk_mux_ops.get_parent(mux_hw);
141 	if ((s8)genamux->muxsel < 0) {
142 		pr_debug("%s: %s: Invalid parent, setting to default.\n",
143 		      __func__, clk_hw_get_name(hw));
144 		genamux->muxsel = 0;
145 	}
146 
147 	return genamux->muxsel;
148 }
149 
150 static int clkgena_divmux_set_parent(struct clk_hw *hw, u8 index)
151 {
152 	struct clkgena_divmux *genamux = to_clkgena_divmux(hw);
153 
154 	if (index >= CKGAX_CLKOPSRC_SWITCH_OFF)
155 		return -EINVAL;
156 
157 	genamux->muxsel = index;
158 
159 	/*
160 	 * If the mux is already enabled, call enable directly to set the
161 	 * new mux position and wait for it to start running again. Otherwise
162 	 * do nothing.
163 	 */
164 	if (clkgena_divmux_is_enabled(hw))
165 		clkgena_divmux_enable(hw);
166 
167 	return 0;
168 }
169 
170 static unsigned long clkgena_divmux_recalc_rate(struct clk_hw *hw,
171 		unsigned long parent_rate)
172 {
173 	struct clkgena_divmux *genamux = to_clkgena_divmux(hw);
174 	struct clk_hw *div_hw = &genamux->div[genamux->muxsel].hw;
175 
176 	__clk_hw_set_clk(div_hw, hw);
177 
178 	return clk_divider_ops.recalc_rate(div_hw, parent_rate);
179 }
180 
181 static int clkgena_divmux_set_rate(struct clk_hw *hw, unsigned long rate,
182 				unsigned long parent_rate)
183 {
184 	struct clkgena_divmux *genamux = to_clkgena_divmux(hw);
185 	struct clk_hw *div_hw = &genamux->div[genamux->muxsel].hw;
186 
187 	__clk_hw_set_clk(div_hw, hw);
188 
189 	return clk_divider_ops.set_rate(div_hw, rate, parent_rate);
190 }
191 
192 static long clkgena_divmux_round_rate(struct clk_hw *hw, unsigned long rate,
193 				   unsigned long *prate)
194 {
195 	struct clkgena_divmux *genamux = to_clkgena_divmux(hw);
196 	struct clk_hw *div_hw = &genamux->div[genamux->muxsel].hw;
197 
198 	__clk_hw_set_clk(div_hw, hw);
199 
200 	return clk_divider_ops.round_rate(div_hw, rate, prate);
201 }
202 
203 static const struct clk_ops clkgena_divmux_ops = {
204 	.enable = clkgena_divmux_enable,
205 	.disable = clkgena_divmux_disable,
206 	.is_enabled = clkgena_divmux_is_enabled,
207 	.get_parent = clkgena_divmux_get_parent,
208 	.set_parent = clkgena_divmux_set_parent,
209 	.round_rate = clkgena_divmux_round_rate,
210 	.recalc_rate = clkgena_divmux_recalc_rate,
211 	.set_rate = clkgena_divmux_set_rate,
212 };
213 
214 /**
215  * clk_register_genamux - register a genamux clock with the clock framework
216  */
217 static struct clk * __init clk_register_genamux(const char *name,
218 				const char **parent_names, u8 num_parents,
219 				void __iomem *reg,
220 				const struct clkgena_divmux_data *muxdata,
221 				u32 idx)
222 {
223 	/*
224 	 * Fixed constants across all ClockgenA variants
225 	 */
226 	const int mux_width = 2;
227 	const int divider_width = 5;
228 	struct clkgena_divmux *genamux;
229 	struct clk *clk;
230 	struct clk_init_data init;
231 	int i;
232 
233 	genamux = kzalloc(sizeof(*genamux), GFP_KERNEL);
234 	if (!genamux)
235 		return ERR_PTR(-ENOMEM);
236 
237 	init.name = name;
238 	init.ops = &clkgena_divmux_ops;
239 	init.flags = CLK_IS_BASIC | CLK_GET_RATE_NOCACHE;
240 	init.parent_names = parent_names;
241 	init.num_parents = num_parents;
242 
243 	genamux->mux.lock  = &clkgena_divmux_lock;
244 	genamux->mux.mask = BIT(mux_width) - 1;
245 	genamux->mux.shift = muxdata->mux_start_bit + (idx * mux_width);
246 	if (genamux->mux.shift > 31) {
247 		/*
248 		 * We have spilled into the second mux register so
249 		 * adjust the register address and the bit shift accordingly
250 		 */
251 		genamux->mux.reg = reg + muxdata->mux_offset2;
252 		genamux->mux.shift -= 32;
253 	} else {
254 		genamux->mux.reg   = reg + muxdata->mux_offset;
255 	}
256 
257 	for (i = 0; i < NUM_INPUTS; i++) {
258 		/*
259 		 * Divider config for each input
260 		 */
261 		void __iomem *divbase = reg + muxdata->div_offsets[i];
262 		genamux->div[i].width = divider_width;
263 		genamux->div[i].reg = divbase + (idx * sizeof(u32));
264 
265 		/*
266 		 * Mux enabled/running feedback register for each input.
267 		 */
268 		genamux->feedback_reg[i] = reg + muxdata->fb_offsets[i];
269 	}
270 
271 	genamux->feedback_bit_idx = muxdata->fb_start_bit_idx + idx;
272 	genamux->hw.init = &init;
273 
274 	clk = clk_register(NULL, &genamux->hw);
275 	if (IS_ERR(clk)) {
276 		kfree(genamux);
277 		goto err;
278 	}
279 
280 	pr_debug("%s: parent %s rate %lu\n",
281 			__clk_get_name(clk),
282 			__clk_get_name(clk_get_parent(clk)),
283 			clk_get_rate(clk));
284 err:
285 	return clk;
286 }
287 
288 static struct clkgena_divmux_data st_divmux_c65hs = {
289 	.num_outputs = 4,
290 	.mux_offset = 0x14,
291 	.mux_start_bit = 0,
292 	.div_offsets = { 0x800, 0x900, 0xb00 },
293 	.fb_offsets = { 0x18, 0x1c, 0x20 },
294 	.fb_start_bit_idx = 0,
295 };
296 
297 static struct clkgena_divmux_data st_divmux_c65ls = {
298 	.num_outputs = 14,
299 	.mux_offset = 0x14,
300 	.mux_offset2 = 0x24,
301 	.mux_start_bit = 8,
302 	.div_offsets = { 0x810, 0xa10, 0xb10 },
303 	.fb_offsets = { 0x18, 0x1c, 0x20 },
304 	.fb_start_bit_idx = 4,
305 };
306 
307 static struct clkgena_divmux_data st_divmux_c32odf0 = {
308 	.num_outputs = 8,
309 	.mux_offset = 0x1c,
310 	.mux_start_bit = 0,
311 	.div_offsets = { 0x800, 0x900, 0xa60 },
312 	.fb_offsets = { 0x2c, 0x24, 0x28 },
313 	.fb_start_bit_idx = 0,
314 };
315 
316 static struct clkgena_divmux_data st_divmux_c32odf1 = {
317 	.num_outputs = 8,
318 	.mux_offset = 0x1c,
319 	.mux_start_bit = 16,
320 	.div_offsets = { 0x820, 0x980, 0xa80 },
321 	.fb_offsets = { 0x2c, 0x24, 0x28 },
322 	.fb_start_bit_idx = 8,
323 };
324 
325 static struct clkgena_divmux_data st_divmux_c32odf2 = {
326 	.num_outputs = 8,
327 	.mux_offset = 0x20,
328 	.mux_start_bit = 0,
329 	.div_offsets = { 0x840, 0xa20, 0xb10 },
330 	.fb_offsets = { 0x2c, 0x24, 0x28 },
331 	.fb_start_bit_idx = 16,
332 };
333 
334 static struct clkgena_divmux_data st_divmux_c32odf3 = {
335 	.num_outputs = 8,
336 	.mux_offset = 0x20,
337 	.mux_start_bit = 16,
338 	.div_offsets = { 0x860, 0xa40, 0xb30 },
339 	.fb_offsets = { 0x2c, 0x24, 0x28 },
340 	.fb_start_bit_idx = 24,
341 };
342 
343 static const struct of_device_id clkgena_divmux_of_match[] = {
344 	{
345 		.compatible = "st,clkgena-divmux-c65-hs",
346 		.data = &st_divmux_c65hs,
347 	},
348 	{
349 		.compatible = "st,clkgena-divmux-c65-ls",
350 		.data = &st_divmux_c65ls,
351 	},
352 	{
353 		.compatible = "st,clkgena-divmux-c32-odf0",
354 		.data = &st_divmux_c32odf0,
355 	},
356 	{
357 		.compatible = "st,clkgena-divmux-c32-odf1",
358 		.data = &st_divmux_c32odf1,
359 	},
360 	{
361 		.compatible = "st,clkgena-divmux-c32-odf2",
362 		.data = &st_divmux_c32odf2,
363 	},
364 	{
365 		.compatible = "st,clkgena-divmux-c32-odf3",
366 		.data = &st_divmux_c32odf3,
367 	},
368 	{}
369 };
370 
371 static void __iomem * __init clkgen_get_register_base(struct device_node *np)
372 {
373 	struct device_node *pnode;
374 	void __iomem *reg;
375 
376 	pnode = of_get_parent(np);
377 	if (!pnode)
378 		return NULL;
379 
380 	reg = of_iomap(pnode, 0);
381 
382 	of_node_put(pnode);
383 	return reg;
384 }
385 
386 static void __init st_of_clkgena_divmux_setup(struct device_node *np)
387 {
388 	const struct of_device_id *match;
389 	const struct clkgena_divmux_data *data;
390 	struct clk_onecell_data *clk_data;
391 	void __iomem *reg;
392 	const char **parents;
393 	int num_parents = 0, i;
394 
395 	match = of_match_node(clkgena_divmux_of_match, np);
396 	if (WARN_ON(!match))
397 		return;
398 
399 	data = match->data;
400 
401 	reg = clkgen_get_register_base(np);
402 	if (!reg)
403 		return;
404 
405 	parents = clkgen_mux_get_parents(np, &num_parents);
406 	if (IS_ERR(parents))
407 		goto err_parents;
408 
409 	clk_data = kzalloc(sizeof(*clk_data), GFP_KERNEL);
410 	if (!clk_data)
411 		goto err_alloc;
412 
413 	clk_data->clk_num = data->num_outputs;
414 	clk_data->clks = kcalloc(clk_data->clk_num, sizeof(struct clk *),
415 				 GFP_KERNEL);
416 
417 	if (!clk_data->clks)
418 		goto err_alloc_clks;
419 
420 	for (i = 0; i < clk_data->clk_num; i++) {
421 		struct clk *clk;
422 		const char *clk_name;
423 
424 		if (of_property_read_string_index(np, "clock-output-names",
425 						  i, &clk_name))
426 			break;
427 
428 		/*
429 		 * If we read an empty clock name then the output is unused
430 		 */
431 		if (*clk_name == '\0')
432 			continue;
433 
434 		clk = clk_register_genamux(clk_name, parents, num_parents,
435 					   reg, data, i);
436 
437 		if (IS_ERR(clk))
438 			goto err;
439 
440 		clk_data->clks[i] = clk;
441 	}
442 
443 	kfree(parents);
444 
445 	of_clk_add_provider(np, of_clk_src_onecell_get, clk_data);
446 	return;
447 err:
448 	kfree(clk_data->clks);
449 err_alloc_clks:
450 	kfree(clk_data);
451 err_alloc:
452 	kfree(parents);
453 err_parents:
454 	iounmap(reg);
455 }
456 CLK_OF_DECLARE(clkgenadivmux, "st,clkgena-divmux", st_of_clkgena_divmux_setup);
457 
458 struct clkgena_prediv_data {
459 	u32 offset;
460 	u8 shift;
461 	struct clk_div_table *table;
462 };
463 
464 static struct clk_div_table prediv_table16[] = {
465 	{ .val = 0, .div = 1 },
466 	{ .val = 1, .div = 16 },
467 	{ .div = 0 },
468 };
469 
470 static struct clkgena_prediv_data prediv_c65_data = {
471 	.offset = 0x4c,
472 	.shift = 31,
473 	.table = prediv_table16,
474 };
475 
476 static struct clkgena_prediv_data prediv_c32_data = {
477 	.offset = 0x50,
478 	.shift = 1,
479 	.table = prediv_table16,
480 };
481 
482 static const struct of_device_id clkgena_prediv_of_match[] = {
483 	{ .compatible = "st,clkgena-prediv-c65", .data = &prediv_c65_data },
484 	{ .compatible = "st,clkgena-prediv-c32", .data = &prediv_c32_data },
485 	{}
486 };
487 
488 static void __init st_of_clkgena_prediv_setup(struct device_node *np)
489 {
490 	const struct of_device_id *match;
491 	void __iomem *reg;
492 	const char *parent_name, *clk_name;
493 	struct clk *clk;
494 	const struct clkgena_prediv_data *data;
495 
496 	match = of_match_node(clkgena_prediv_of_match, np);
497 	if (!match) {
498 		pr_err("%s: No matching data\n", __func__);
499 		return;
500 	}
501 
502 	data = match->data;
503 
504 	reg = clkgen_get_register_base(np);
505 	if (!reg)
506 		return;
507 
508 	parent_name = of_clk_get_parent_name(np, 0);
509 	if (!parent_name)
510 		goto err;
511 
512 	if (of_property_read_string_index(np, "clock-output-names",
513 					  0, &clk_name))
514 		goto err;
515 
516 	clk = clk_register_divider_table(NULL, clk_name, parent_name,
517 					 CLK_GET_RATE_NOCACHE,
518 					 reg + data->offset, data->shift, 1,
519 					 0, data->table, NULL);
520 	if (IS_ERR(clk))
521 		goto err;
522 
523 	of_clk_add_provider(np, of_clk_src_simple_get, clk);
524 	pr_debug("%s: parent %s rate %u\n",
525 		__clk_get_name(clk),
526 		__clk_get_name(clk_get_parent(clk)),
527 		(unsigned int)clk_get_rate(clk));
528 
529 	return;
530 err:
531 	iounmap(reg);
532 }
533 CLK_OF_DECLARE(clkgenaprediv, "st,clkgena-prediv", st_of_clkgena_prediv_setup);
534 
535 struct clkgen_mux_data {
536 	u32 offset;
537 	u8 shift;
538 	u8 width;
539 	spinlock_t *lock;
540 	unsigned long clk_flags;
541 	u8 mux_flags;
542 };
543 
544 static struct clkgen_mux_data clkgen_mux_c_vcc_hd_416 = {
545 	.offset = 0,
546 	.shift = 0,
547 	.width = 1,
548 };
549 
550 static struct clkgen_mux_data clkgen_mux_f_vcc_fvdp_416 = {
551 	.offset = 0,
552 	.shift = 0,
553 	.width = 1,
554 };
555 
556 static struct clkgen_mux_data clkgen_mux_f_vcc_hva_416 = {
557 	.offset = 0,
558 	.shift = 0,
559 	.width = 1,
560 };
561 
562 static struct clkgen_mux_data clkgen_mux_f_vcc_hd_416 = {
563 	.offset = 0,
564 	.shift = 16,
565 	.width = 1,
566 	.lock = &clkgenf_lock,
567 };
568 
569 static struct clkgen_mux_data clkgen_mux_c_vcc_sd_416 = {
570 	.offset = 0,
571 	.shift = 17,
572 	.width = 1,
573 	.lock = &clkgenf_lock,
574 };
575 
576 static struct clkgen_mux_data stih415_a9_mux_data = {
577 	.offset = 0,
578 	.shift = 1,
579 	.width = 2,
580 	.lock = &clkgen_a9_lock,
581 };
582 static struct clkgen_mux_data stih416_a9_mux_data = {
583 	.offset = 0,
584 	.shift = 0,
585 	.width = 2,
586 };
587 static struct clkgen_mux_data stih407_a9_mux_data = {
588 	.offset = 0x1a4,
589 	.shift = 0,
590 	.width = 2,
591 	.lock = &clkgen_a9_lock,
592 };
593 
594 static const struct of_device_id mux_of_match[] = {
595 	{
596 		.compatible = "st,stih416-clkgenc-vcc-hd",
597 		.data = &clkgen_mux_c_vcc_hd_416,
598 	},
599 	{
600 		.compatible = "st,stih416-clkgenf-vcc-fvdp",
601 		.data = &clkgen_mux_f_vcc_fvdp_416,
602 	},
603 	{
604 		.compatible = "st,stih416-clkgenf-vcc-hva",
605 		.data = &clkgen_mux_f_vcc_hva_416,
606 	},
607 	{
608 		.compatible = "st,stih416-clkgenf-vcc-hd",
609 		.data = &clkgen_mux_f_vcc_hd_416,
610 	},
611 	{
612 		.compatible = "st,stih416-clkgenf-vcc-sd",
613 		.data = &clkgen_mux_c_vcc_sd_416,
614 	},
615 	{
616 		.compatible = "st,stih415-clkgen-a9-mux",
617 		.data = &stih415_a9_mux_data,
618 	},
619 	{
620 		.compatible = "st,stih416-clkgen-a9-mux",
621 		.data = &stih416_a9_mux_data,
622 	},
623 	{
624 		.compatible = "st,stih407-clkgen-a9-mux",
625 		.data = &stih407_a9_mux_data,
626 	},
627 	{}
628 };
629 
630 static void __init st_of_clkgen_mux_setup(struct device_node *np)
631 {
632 	const struct of_device_id *match;
633 	struct clk *clk;
634 	void __iomem *reg;
635 	const char **parents;
636 	int num_parents;
637 	const struct clkgen_mux_data *data;
638 
639 	match = of_match_node(mux_of_match, np);
640 	if (!match) {
641 		pr_err("%s: No matching data\n", __func__);
642 		return;
643 	}
644 
645 	data = match->data;
646 
647 	reg = of_iomap(np, 0);
648 	if (!reg) {
649 		pr_err("%s: Failed to get base address\n", __func__);
650 		return;
651 	}
652 
653 	parents = clkgen_mux_get_parents(np, &num_parents);
654 	if (IS_ERR(parents)) {
655 		pr_err("%s: Failed to get parents (%ld)\n",
656 				__func__, PTR_ERR(parents));
657 		goto err_parents;
658 	}
659 
660 	clk = clk_register_mux(NULL, np->name, parents, num_parents,
661 				data->clk_flags | CLK_SET_RATE_PARENT,
662 				reg + data->offset,
663 				data->shift, data->width, data->mux_flags,
664 				data->lock);
665 	if (IS_ERR(clk))
666 		goto err;
667 
668 	pr_debug("%s: parent %s rate %u\n",
669 			__clk_get_name(clk),
670 			__clk_get_name(clk_get_parent(clk)),
671 			(unsigned int)clk_get_rate(clk));
672 
673 	kfree(parents);
674 	of_clk_add_provider(np, of_clk_src_simple_get, clk);
675 	return;
676 
677 err:
678 	kfree(parents);
679 err_parents:
680 	iounmap(reg);
681 }
682 CLK_OF_DECLARE(clkgen_mux, "st,clkgen-mux", st_of_clkgen_mux_setup);
683 
684 #define VCC_MAX_CHANNELS 16
685 
686 #define VCC_GATE_OFFSET 0x0
687 #define VCC_MUX_OFFSET 0x4
688 #define VCC_DIV_OFFSET 0x8
689 
690 struct clkgen_vcc_data {
691 	spinlock_t *lock;
692 	unsigned long clk_flags;
693 };
694 
695 static struct clkgen_vcc_data st_clkgenc_vcc_416 = {
696 	.clk_flags = CLK_SET_RATE_PARENT,
697 };
698 
699 static struct clkgen_vcc_data st_clkgenf_vcc_416 = {
700 	.lock = &clkgenf_lock,
701 };
702 
703 static const struct of_device_id vcc_of_match[] = {
704 	{ .compatible = "st,stih416-clkgenc", .data = &st_clkgenc_vcc_416 },
705 	{ .compatible = "st,stih416-clkgenf", .data = &st_clkgenf_vcc_416 },
706 	{}
707 };
708 
709 static void __init st_of_clkgen_vcc_setup(struct device_node *np)
710 {
711 	const struct of_device_id *match;
712 	void __iomem *reg;
713 	const char **parents;
714 	int num_parents, i;
715 	struct clk_onecell_data *clk_data;
716 	const struct clkgen_vcc_data *data;
717 
718 	match = of_match_node(vcc_of_match, np);
719 	if (WARN_ON(!match))
720 		return;
721 	data = match->data;
722 
723 	reg = of_iomap(np, 0);
724 	if (!reg)
725 		return;
726 
727 	parents = clkgen_mux_get_parents(np, &num_parents);
728 	if (IS_ERR(parents))
729 		goto err_parents;
730 
731 	clk_data = kzalloc(sizeof(*clk_data), GFP_KERNEL);
732 	if (!clk_data)
733 		goto err_alloc;
734 
735 	clk_data->clk_num = VCC_MAX_CHANNELS;
736 	clk_data->clks = kcalloc(clk_data->clk_num, sizeof(struct clk *),
737 				 GFP_KERNEL);
738 
739 	if (!clk_data->clks)
740 		goto err_alloc_clks;
741 
742 	for (i = 0; i < clk_data->clk_num; i++) {
743 		struct clk *clk;
744 		const char *clk_name;
745 		struct clk_gate *gate;
746 		struct clk_divider *div;
747 		struct clk_mux *mux;
748 
749 		if (of_property_read_string_index(np, "clock-output-names",
750 						  i, &clk_name))
751 			break;
752 
753 		/*
754 		 * If we read an empty clock name then the output is unused
755 		 */
756 		if (*clk_name == '\0')
757 			continue;
758 
759 		gate = kzalloc(sizeof(*gate), GFP_KERNEL);
760 		if (!gate)
761 			goto err;
762 
763 		div = kzalloc(sizeof(*div), GFP_KERNEL);
764 		if (!div) {
765 			kfree(gate);
766 			goto err;
767 		}
768 
769 		mux = kzalloc(sizeof(*mux), GFP_KERNEL);
770 		if (!mux) {
771 			kfree(gate);
772 			kfree(div);
773 			goto err;
774 		}
775 
776 		gate->reg = reg + VCC_GATE_OFFSET;
777 		gate->bit_idx = i;
778 		gate->flags = CLK_GATE_SET_TO_DISABLE;
779 		gate->lock = data->lock;
780 
781 		div->reg = reg + VCC_DIV_OFFSET;
782 		div->shift = 2 * i;
783 		div->width = 2;
784 		div->flags = CLK_DIVIDER_POWER_OF_TWO |
785 			CLK_DIVIDER_ROUND_CLOSEST;
786 
787 		mux->reg = reg + VCC_MUX_OFFSET;
788 		mux->shift = 2 * i;
789 		mux->mask = 0x3;
790 
791 		clk = clk_register_composite(NULL, clk_name, parents,
792 					     num_parents,
793 					     &mux->hw, &clk_mux_ops,
794 					     &div->hw, &clk_divider_ops,
795 					     &gate->hw, &clk_gate_ops,
796 					     data->clk_flags |
797 					     CLK_GET_RATE_NOCACHE);
798 		if (IS_ERR(clk)) {
799 			kfree(gate);
800 			kfree(div);
801 			kfree(mux);
802 			goto err;
803 		}
804 
805 		pr_debug("%s: parent %s rate %u\n",
806 			__clk_get_name(clk),
807 			__clk_get_name(clk_get_parent(clk)),
808 			(unsigned int)clk_get_rate(clk));
809 
810 		clk_data->clks[i] = clk;
811 	}
812 
813 	kfree(parents);
814 
815 	of_clk_add_provider(np, of_clk_src_onecell_get, clk_data);
816 	return;
817 
818 err:
819 	for (i = 0; i < clk_data->clk_num; i++) {
820 		struct clk_composite *composite;
821 
822 		if (!clk_data->clks[i])
823 			continue;
824 
825 		composite = to_clk_composite(__clk_get_hw(clk_data->clks[i]));
826 		kfree(to_clk_gate(composite->gate_hw));
827 		kfree(to_clk_divider(composite->rate_hw));
828 		kfree(to_clk_mux(composite->mux_hw));
829 	}
830 
831 	kfree(clk_data->clks);
832 err_alloc_clks:
833 	kfree(clk_data);
834 err_alloc:
835 	kfree(parents);
836 err_parents:
837 	iounmap(reg);
838 }
839 CLK_OF_DECLARE(clkgen_vcc, "st,clkgen-vcc", st_of_clkgen_vcc_setup);
840