clkgen-mux.c 18.8 KB
Newer Older
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17
/*
 * clkgen-mux.c: ST GEN-MUX Clock driver
 *
 * Copyright (C) 2014 STMicroelectronics (R&D) Limited
 *
 * Authors: Stephen Gallimore <stephen.gallimore@st.com>
 *	    Pankaj Dev <pankaj.dev@st.com>
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 */

#include <linux/slab.h>
#include <linux/of_address.h>
Stephen Boyd's avatar
Stephen Boyd committed
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 26 27 28

static const char ** __init clkgen_mux_get_parents(struct device_node *np,
						       int *num_parents)
{
	const char **parents;
29
	unsigned int nparents;
30

31
	nparents = of_clk_get_parent_count(np);
32
	if (WARN_ON(!nparents))
33 34
		return ERR_PTR(-EINVAL);

35
	parents = kcalloc(nparents, sizeof(const char *), GFP_KERNEL);
36 37 38
	if (!parents)
		return ERR_PTR(-ENOMEM);

39
	*num_parents = of_clk_parent_fill(np, parents, nparents);
40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95
	return parents;
}

/**
 * DOC: Clock mux with a programmable divider on each of its three inputs.
 *      The mux has an input setting which effectively gates its output.
 *
 * Traits of this clock:
 * prepare - clk_(un)prepare only ensures parent is (un)prepared
 * enable - clk_enable and clk_disable are functional & control gating
 * rate - set rate is supported
 * parent - set/get parent
 */

#define NUM_INPUTS 3

struct clkgena_divmux {
	struct clk_hw hw;
	/* Subclassed mux and divider structures */
	struct clk_mux mux;
	struct clk_divider div[NUM_INPUTS];
	/* Enable/running feedback register bits for each input */
	void __iomem *feedback_reg[NUM_INPUTS];
	int feedback_bit_idx;

	u8              muxsel;
};

#define to_clkgena_divmux(_hw) container_of(_hw, struct clkgena_divmux, hw)

struct clkgena_divmux_data {
	int num_outputs;
	int mux_offset;
	int mux_offset2;
	int mux_start_bit;
	int div_offsets[NUM_INPUTS];
	int fb_offsets[NUM_INPUTS];
	int fb_start_bit_idx;
};

#define CKGAX_CLKOPSRC_SWITCH_OFF 0x3

static int clkgena_divmux_is_running(struct clkgena_divmux *mux)
{
	u32 regval = readl(mux->feedback_reg[mux->muxsel]);
	u32 running = regval & BIT(mux->feedback_bit_idx);
	return !!running;
}

static int clkgena_divmux_enable(struct clk_hw *hw)
{
	struct clkgena_divmux *genamux = to_clkgena_divmux(hw);
	struct clk_hw *mux_hw = &genamux->mux.hw;
	unsigned long timeout;
	int ret = 0;

96
	__clk_hw_set_clk(mux_hw, hw);
97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117

	ret = clk_mux_ops.set_parent(mux_hw, genamux->muxsel);
	if (ret)
		return ret;

	timeout = jiffies + msecs_to_jiffies(10);

	while (!clkgena_divmux_is_running(genamux)) {
		if (time_after(jiffies, timeout))
			return -ETIMEDOUT;
		cpu_relax();
	}

	return 0;
}

static void clkgena_divmux_disable(struct clk_hw *hw)
{
	struct clkgena_divmux *genamux = to_clkgena_divmux(hw);
	struct clk_hw *mux_hw = &genamux->mux.hw;

118
	__clk_hw_set_clk(mux_hw, hw);
119 120 121 122 123 124 125 126 127

	clk_mux_ops.set_parent(mux_hw, CKGAX_CLKOPSRC_SWITCH_OFF);
}

static int clkgena_divmux_is_enabled(struct clk_hw *hw)
{
	struct clkgena_divmux *genamux = to_clkgena_divmux(hw);
	struct clk_hw *mux_hw = &genamux->mux.hw;

128
	__clk_hw_set_clk(mux_hw, hw);
129 130 131 132

	return (s8)clk_mux_ops.get_parent(mux_hw) > 0;
}

133
static u8 clkgena_divmux_get_parent(struct clk_hw *hw)
134 135 136 137
{
	struct clkgena_divmux *genamux = to_clkgena_divmux(hw);
	struct clk_hw *mux_hw = &genamux->mux.hw;

138
	__clk_hw_set_clk(mux_hw, hw);
139 140 141 142

	genamux->muxsel = clk_mux_ops.get_parent(mux_hw);
	if ((s8)genamux->muxsel < 0) {
		pr_debug("%s: %s: Invalid parent, setting to default.\n",
143
		      __func__, clk_hw_get_name(hw));
144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169
		genamux->muxsel = 0;
	}

	return genamux->muxsel;
}

static int clkgena_divmux_set_parent(struct clk_hw *hw, u8 index)
{
	struct clkgena_divmux *genamux = to_clkgena_divmux(hw);

	if (index >= CKGAX_CLKOPSRC_SWITCH_OFF)
		return -EINVAL;

	genamux->muxsel = index;

	/*
	 * If the mux is already enabled, call enable directly to set the
	 * new mux position and wait for it to start running again. Otherwise
	 * do nothing.
	 */
	if (clkgena_divmux_is_enabled(hw))
		clkgena_divmux_enable(hw);

	return 0;
}

170
static unsigned long clkgena_divmux_recalc_rate(struct clk_hw *hw,
171 172 173 174 175
		unsigned long parent_rate)
{
	struct clkgena_divmux *genamux = to_clkgena_divmux(hw);
	struct clk_hw *div_hw = &genamux->div[genamux->muxsel].hw;

176
	__clk_hw_set_clk(div_hw, hw);
177 178 179 180 181 182 183 184 185 186

	return clk_divider_ops.recalc_rate(div_hw, parent_rate);
}

static int clkgena_divmux_set_rate(struct clk_hw *hw, unsigned long rate,
				unsigned long parent_rate)
{
	struct clkgena_divmux *genamux = to_clkgena_divmux(hw);
	struct clk_hw *div_hw = &genamux->div[genamux->muxsel].hw;

187
	__clk_hw_set_clk(div_hw, hw);
188 189 190 191 192 193 194 195 196 197

	return clk_divider_ops.set_rate(div_hw, rate, parent_rate);
}

static long clkgena_divmux_round_rate(struct clk_hw *hw, unsigned long rate,
				   unsigned long *prate)
{
	struct clkgena_divmux *genamux = to_clkgena_divmux(hw);
	struct clk_hw *div_hw = &genamux->div[genamux->muxsel].hw;

198
	__clk_hw_set_clk(div_hw, hw);
199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216

	return clk_divider_ops.round_rate(div_hw, rate, prate);
}

static const struct clk_ops clkgena_divmux_ops = {
	.enable = clkgena_divmux_enable,
	.disable = clkgena_divmux_disable,
	.is_enabled = clkgena_divmux_is_enabled,
	.get_parent = clkgena_divmux_get_parent,
	.set_parent = clkgena_divmux_set_parent,
	.round_rate = clkgena_divmux_round_rate,
	.recalc_rate = clkgena_divmux_recalc_rate,
	.set_rate = clkgena_divmux_set_rate,
};

/**
 * clk_register_genamux - register a genamux clock with the clock framework
 */
217
static struct clk * __init clk_register_genamux(const char *name,
218 219 220 221 222 223 224 225 226 227 228 229 230 231 232 233 234 235 236 237 238
				const char **parent_names, u8 num_parents,
				void __iomem *reg,
				const struct clkgena_divmux_data *muxdata,
				u32 idx)
{
	/*
	 * Fixed constants across all ClockgenA variants
	 */
	const int mux_width = 2;
	const int divider_width = 5;
	struct clkgena_divmux *genamux;
	struct clk *clk;
	struct clk_init_data init;
	int i;

	genamux = kzalloc(sizeof(*genamux), GFP_KERNEL);
	if (!genamux)
		return ERR_PTR(-ENOMEM);

	init.name = name;
	init.ops = &clkgena_divmux_ops;
239
	init.flags = CLK_IS_BASIC | CLK_GET_RATE_NOCACHE;
240 241 242 243 244 245 246 247 248 249 250 251 252 253 254 255 256 257 258 259 260 261 262 263 264 265 266 267 268 269 270 271 272 273 274 275 276 277 278 279 280 281 282 283 284 285 286 287 288 289 290 291 292 293 294 295 296 297 298 299 300 301 302 303 304 305 306 307 308 309 310 311 312 313 314 315 316 317 318 319 320 321 322 323 324 325 326 327 328 329 330 331 332 333 334 335 336 337 338 339 340 341 342
	init.parent_names = parent_names;
	init.num_parents = num_parents;

	genamux->mux.lock  = &clkgena_divmux_lock;
	genamux->mux.mask = BIT(mux_width) - 1;
	genamux->mux.shift = muxdata->mux_start_bit + (idx * mux_width);
	if (genamux->mux.shift > 31) {
		/*
		 * We have spilled into the second mux register so
		 * adjust the register address and the bit shift accordingly
		 */
		genamux->mux.reg = reg + muxdata->mux_offset2;
		genamux->mux.shift -= 32;
	} else {
		genamux->mux.reg   = reg + muxdata->mux_offset;
	}

	for (i = 0; i < NUM_INPUTS; i++) {
		/*
		 * Divider config for each input
		 */
		void __iomem *divbase = reg + muxdata->div_offsets[i];
		genamux->div[i].width = divider_width;
		genamux->div[i].reg = divbase + (idx * sizeof(u32));

		/*
		 * Mux enabled/running feedback register for each input.
		 */
		genamux->feedback_reg[i] = reg + muxdata->fb_offsets[i];
	}

	genamux->feedback_bit_idx = muxdata->fb_start_bit_idx + idx;
	genamux->hw.init = &init;

	clk = clk_register(NULL, &genamux->hw);
	if (IS_ERR(clk)) {
		kfree(genamux);
		goto err;
	}

	pr_debug("%s: parent %s rate %lu\n",
			__clk_get_name(clk),
			__clk_get_name(clk_get_parent(clk)),
			clk_get_rate(clk));
err:
	return clk;
}

static struct clkgena_divmux_data st_divmux_c65hs = {
	.num_outputs = 4,
	.mux_offset = 0x14,
	.mux_start_bit = 0,
	.div_offsets = { 0x800, 0x900, 0xb00 },
	.fb_offsets = { 0x18, 0x1c, 0x20 },
	.fb_start_bit_idx = 0,
};

static struct clkgena_divmux_data st_divmux_c65ls = {
	.num_outputs = 14,
	.mux_offset = 0x14,
	.mux_offset2 = 0x24,
	.mux_start_bit = 8,
	.div_offsets = { 0x810, 0xa10, 0xb10 },
	.fb_offsets = { 0x18, 0x1c, 0x20 },
	.fb_start_bit_idx = 4,
};

static struct clkgena_divmux_data st_divmux_c32odf0 = {
	.num_outputs = 8,
	.mux_offset = 0x1c,
	.mux_start_bit = 0,
	.div_offsets = { 0x800, 0x900, 0xa60 },
	.fb_offsets = { 0x2c, 0x24, 0x28 },
	.fb_start_bit_idx = 0,
};

static struct clkgena_divmux_data st_divmux_c32odf1 = {
	.num_outputs = 8,
	.mux_offset = 0x1c,
	.mux_start_bit = 16,
	.div_offsets = { 0x820, 0x980, 0xa80 },
	.fb_offsets = { 0x2c, 0x24, 0x28 },
	.fb_start_bit_idx = 8,
};

static struct clkgena_divmux_data st_divmux_c32odf2 = {
	.num_outputs = 8,
	.mux_offset = 0x20,
	.mux_start_bit = 0,
	.div_offsets = { 0x840, 0xa20, 0xb10 },
	.fb_offsets = { 0x2c, 0x24, 0x28 },
	.fb_start_bit_idx = 16,
};

static struct clkgena_divmux_data st_divmux_c32odf3 = {
	.num_outputs = 8,
	.mux_offset = 0x20,
	.mux_start_bit = 16,
	.div_offsets = { 0x860, 0xa40, 0xb30 },
	.fb_offsets = { 0x2c, 0x24, 0x28 },
	.fb_start_bit_idx = 24,
};

343
static const struct of_device_id clkgena_divmux_of_match[] = {
344 345 346 347 348 349 350 351 352 353 354 355 356 357 358 359 360 361 362 363 364 365 366 367 368 369 370
	{
		.compatible = "st,clkgena-divmux-c65-hs",
		.data = &st_divmux_c65hs,
	},
	{
		.compatible = "st,clkgena-divmux-c65-ls",
		.data = &st_divmux_c65ls,
	},
	{
		.compatible = "st,clkgena-divmux-c32-odf0",
		.data = &st_divmux_c32odf0,
	},
	{
		.compatible = "st,clkgena-divmux-c32-odf1",
		.data = &st_divmux_c32odf1,
	},
	{
		.compatible = "st,clkgena-divmux-c32-odf2",
		.data = &st_divmux_c32odf2,
	},
	{
		.compatible = "st,clkgena-divmux-c32-odf3",
		.data = &st_divmux_c32odf3,
	},
	{}
};

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 377 378 379 380 381 382 383 384 385

	pnode = of_get_parent(np);
	if (!pnode)
		return NULL;

	reg = of_iomap(pnode, 0);

	of_node_put(pnode);
	return reg;
}

386
static void __init st_of_clkgena_divmux_setup(struct device_node *np)
387 388 389 390 391 392 393 394 395 396 397 398
{
	const struct of_device_id *match;
	const struct clkgena_divmux_data *data;
	struct clk_onecell_data *clk_data;
	void __iomem *reg;
	const char **parents;
	int num_parents = 0, i;

	match = of_match_node(clkgena_divmux_of_match, np);
	if (WARN_ON(!match))
		return;

399
	data = match->data;
400 401 402 403 404 405 406

	reg = clkgen_get_register_base(np);
	if (!reg)
		return;

	parents = clkgen_mux_get_parents(np, &num_parents);
	if (IS_ERR(parents))
407
		goto err_parents;
408 409 410

	clk_data = kzalloc(sizeof(*clk_data), GFP_KERNEL);
	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 416 417
				 GFP_KERNEL);

	if (!clk_data->clks)
418
		goto err_alloc_clks;
419 420 421 422 423 424 425 426 427 428 429 430 431 432 433 434 435 436 437 438 439 440 441 442 443 444 445 446 447

	for (i = 0; i < clk_data->clk_num; i++) {
		struct clk *clk;
		const char *clk_name;

		if (of_property_read_string_index(np, "clock-output-names",
						  i, &clk_name))
			break;

		/*
		 * If we read an empty clock name then the output is unused
		 */
		if (*clk_name == '\0')
			continue;

		clk = clk_register_genamux(clk_name, parents, num_parents,
					   reg, data, i);

		if (IS_ERR(clk))
			goto err;

		clk_data->clks[i] = clk;
	}

	kfree(parents);

	of_clk_add_provider(np, of_clk_src_onecell_get, clk_data);
	return;
err:
448 449
	kfree(clk_data->clks);
err_alloc_clks:
450
	kfree(clk_data);
451
err_alloc:
452
	kfree(parents);
453 454
err_parents:
	iounmap(reg);
455 456 457 458 459 460 461 462 463 464 465 466 467 468 469 470 471 472 473 474 475 476 477 478 479 480 481
}
CLK_OF_DECLARE(clkgenadivmux, "st,clkgena-divmux", st_of_clkgena_divmux_setup);

struct clkgena_prediv_data {
	u32 offset;
	u8 shift;
	struct clk_div_table *table;
};

static struct clk_div_table prediv_table16[] = {
	{ .val = 0, .div = 1 },
	{ .val = 1, .div = 16 },
	{ .div = 0 },
};

static struct clkgena_prediv_data prediv_c65_data = {
	.offset = 0x4c,
	.shift = 31,
	.table = prediv_table16,
};

static struct clkgena_prediv_data prediv_c32_data = {
	.offset = 0x50,
	.shift = 1,
	.table = prediv_table16,
};

482
static const struct of_device_id clkgena_prediv_of_match[] = {
483 484 485 486 487
	{ .compatible = "st,clkgena-prediv-c65", .data = &prediv_c65_data },
	{ .compatible = "st,clkgena-prediv-c32", .data = &prediv_c32_data },
	{}
};

488
static void __init st_of_clkgena_prediv_setup(struct device_node *np)
489 490 491 492 493
{
	const struct of_device_id *match;
	void __iomem *reg;
	const char *parent_name, *clk_name;
	struct clk *clk;
494
	const struct clkgena_prediv_data *data;
495 496 497 498 499 500 501

	match = of_match_node(clkgena_prediv_of_match, np);
	if (!match) {
		pr_err("%s: No matching data\n", __func__);
		return;
	}

502
	data = match->data;
503 504 505 506 507 508 509

	reg = clkgen_get_register_base(np);
	if (!reg)
		return;

	parent_name = of_clk_get_parent_name(np, 0);
	if (!parent_name)
510
		goto err;
511 512 513

	if (of_property_read_string_index(np, "clock-output-names",
					  0, &clk_name))
514
		goto err;
515

516 517
	clk = clk_register_divider_table(NULL, clk_name, parent_name,
					 CLK_GET_RATE_NOCACHE,
518 519 520
					 reg + data->offset, data->shift, 1,
					 0, data->table, NULL);
	if (IS_ERR(clk))
521
		goto err;
522 523 524 525 526 527 528 529

	of_clk_add_provider(np, of_clk_src_simple_get, clk);
	pr_debug("%s: parent %s rate %u\n",
		__clk_get_name(clk),
		__clk_get_name(clk_get_parent(clk)),
		(unsigned int)clk_get_rate(clk));

	return;
530 531
err:
	iounmap(reg);
532 533
}
CLK_OF_DECLARE(clkgenaprediv, "st,clkgena-prediv", st_of_clkgena_prediv_setup);
534 535 536 537 538 539 540 541 542 543 544 545 546 547 548 549 550 551 552 553 554 555 556 557 558 559 560 561 562 563 564 565 566 567 568 569 570 571 572 573 574 575

struct clkgen_mux_data {
	u32 offset;
	u8 shift;
	u8 width;
	spinlock_t *lock;
	unsigned long clk_flags;
	u8 mux_flags;
};

static struct clkgen_mux_data clkgen_mux_c_vcc_hd_416 = {
	.offset = 0,
	.shift = 0,
	.width = 1,
};

static struct clkgen_mux_data clkgen_mux_f_vcc_fvdp_416 = {
	.offset = 0,
	.shift = 0,
	.width = 1,
};

static struct clkgen_mux_data clkgen_mux_f_vcc_hva_416 = {
	.offset = 0,
	.shift = 0,
	.width = 1,
};

static struct clkgen_mux_data clkgen_mux_f_vcc_hd_416 = {
	.offset = 0,
	.shift = 16,
	.width = 1,
	.lock = &clkgenf_lock,
};

static struct clkgen_mux_data clkgen_mux_c_vcc_sd_416 = {
	.offset = 0,
	.shift = 17,
	.width = 1,
	.lock = &clkgenf_lock,
};

576 577 578 579
static struct clkgen_mux_data stih415_a9_mux_data = {
	.offset = 0,
	.shift = 1,
	.width = 2,
580
	.lock = &clkgen_a9_lock,
581 582 583 584 585 586
};
static struct clkgen_mux_data stih416_a9_mux_data = {
	.offset = 0,
	.shift = 0,
	.width = 2,
};
587 588
static struct clkgen_mux_data stih407_a9_mux_data = {
	.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 597 598 599 600 601 602 603 604 605 606 607 608 609 610 611 612 613 614
	{
		.compatible = "st,stih416-clkgenc-vcc-hd",
		.data = &clkgen_mux_c_vcc_hd_416,
	},
	{
		.compatible = "st,stih416-clkgenf-vcc-fvdp",
		.data = &clkgen_mux_f_vcc_fvdp_416,
	},
	{
		.compatible = "st,stih416-clkgenf-vcc-hva",
		.data = &clkgen_mux_f_vcc_hva_416,
	},
	{
		.compatible = "st,stih416-clkgenf-vcc-hd",
		.data = &clkgen_mux_f_vcc_hd_416,
	},
	{
		.compatible = "st,stih416-clkgenf-vcc-sd",
		.data = &clkgen_mux_c_vcc_sd_416,
	},
615 616 617 618 619 620 621 622
	{
		.compatible = "st,stih415-clkgen-a9-mux",
		.data = &stih415_a9_mux_data,
	},
	{
		.compatible = "st,stih416-clkgen-a9-mux",
		.data = &stih416_a9_mux_data,
	},
623 624 625 626
	{
		.compatible = "st,stih407-clkgen-a9-mux",
		.data = &stih407_a9_mux_data,
	},
627 628 629
	{}
};

630
static void __init st_of_clkgen_mux_setup(struct device_node *np)
631 632 633 634 635 636
{
	const struct of_device_id *match;
	struct clk *clk;
	void __iomem *reg;
	const char **parents;
	int num_parents;
637
	const struct clkgen_mux_data *data;
638 639 640 641 642 643 644

	match = of_match_node(mux_of_match, np);
	if (!match) {
		pr_err("%s: No matching data\n", __func__);
		return;
	}

645
	data = match->data;
646 647 648 649 650 651 652 653 654 655 656

	reg = of_iomap(np, 0);
	if (!reg) {
		pr_err("%s: Failed to get base address\n", __func__);
		return;
	}

	parents = clkgen_mux_get_parents(np, &num_parents);
	if (IS_ERR(parents)) {
		pr_err("%s: Failed to get parents (%ld)\n",
				__func__, PTR_ERR(parents));
657
		goto err_parents;
658 659 660 661 662 663 664 665 666 667 668 669 670 671 672
	}

	clk = clk_register_mux(NULL, np->name, parents, num_parents,
				data->clk_flags | CLK_SET_RATE_PARENT,
				reg + data->offset,
				data->shift, data->width, data->mux_flags,
				data->lock);
	if (IS_ERR(clk))
		goto err;

	pr_debug("%s: parent %s rate %u\n",
			__clk_get_name(clk),
			__clk_get_name(clk_get_parent(clk)),
			(unsigned int)clk_get_rate(clk));

673
	kfree(parents);
674
	of_clk_add_provider(np, of_clk_src_simple_get, clk);
675
	return;
676 677 678

err:
	kfree(parents);
679 680
err_parents:
	iounmap(reg);
681 682 683 684 685 686 687 688 689 690 691 692 693 694 695 696 697 698 699 700 701 702
}
CLK_OF_DECLARE(clkgen_mux, "st,clkgen-mux", st_of_clkgen_mux_setup);

#define VCC_MAX_CHANNELS 16

#define VCC_GATE_OFFSET 0x0
#define VCC_MUX_OFFSET 0x4
#define VCC_DIV_OFFSET 0x8

struct clkgen_vcc_data {
	spinlock_t *lock;
	unsigned long clk_flags;
};

static struct clkgen_vcc_data st_clkgenc_vcc_416 = {
	.clk_flags = CLK_SET_RATE_PARENT,
};

static struct clkgen_vcc_data st_clkgenf_vcc_416 = {
	.lock = &clkgenf_lock,
};

703
static const struct of_device_id vcc_of_match[] = {
704 705 706 707 708
	{ .compatible = "st,stih416-clkgenc", .data = &st_clkgenc_vcc_416 },
	{ .compatible = "st,stih416-clkgenf", .data = &st_clkgenf_vcc_416 },
	{}
};

709
static void __init st_of_clkgen_vcc_setup(struct device_node *np)
710 711 712 713 714 715
{
	const struct of_device_id *match;
	void __iomem *reg;
	const char **parents;
	int num_parents, i;
	struct clk_onecell_data *clk_data;
716
	const struct clkgen_vcc_data *data;
717 718 719 720

	match = of_match_node(vcc_of_match, np);
	if (WARN_ON(!match))
		return;
721
	data = match->data;
722 723 724 725 726 727 728

	reg = of_iomap(np, 0);
	if (!reg)
		return;

	parents = clkgen_mux_get_parents(np, &num_parents);
	if (IS_ERR(parents))
729
		goto err_parents;
730 731 732

	clk_data = kzalloc(sizeof(*clk_data), GFP_KERNEL);
	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 738 739
				 GFP_KERNEL);

	if (!clk_data->clks)
740
		goto err_alloc_clks;
741 742 743 744 745 746 747 748 749 750 751 752 753 754 755 756 757 758

	for (i = 0; i < clk_data->clk_num; i++) {
		struct clk *clk;
		const char *clk_name;
		struct clk_gate *gate;
		struct clk_divider *div;
		struct clk_mux *mux;

		if (of_property_read_string_index(np, "clock-output-names",
						  i, &clk_name))
			break;

		/*
		 * If we read an empty clock name then the output is unused
		 */
		if (*clk_name == '\0')
			continue;

759
		gate = kzalloc(sizeof(*gate), GFP_KERNEL);
760
		if (!gate)
761
			goto err;
762

763
		div = kzalloc(sizeof(*div), GFP_KERNEL);
764 765
		if (!div) {
			kfree(gate);
766
			goto err;
767 768
		}

769
		mux = kzalloc(sizeof(*mux), GFP_KERNEL);
770 771 772
		if (!mux) {
			kfree(gate);
			kfree(div);
773
			goto err;
774 775 776 777 778 779 780 781 782 783
		}

		gate->reg = reg + VCC_GATE_OFFSET;
		gate->bit_idx = i;
		gate->flags = CLK_GATE_SET_TO_DISABLE;
		gate->lock = data->lock;

		div->reg = reg + VCC_DIV_OFFSET;
		div->shift = 2 * i;
		div->width = 2;
784 785
		div->flags = CLK_DIVIDER_POWER_OF_TWO |
			CLK_DIVIDER_ROUND_CLOSEST;
786 787 788 789 790 791 792 793 794 795

		mux->reg = reg + VCC_MUX_OFFSET;
		mux->shift = 2 * i;
		mux->mask = 0x3;

		clk = clk_register_composite(NULL, clk_name, parents,
					     num_parents,
					     &mux->hw, &clk_mux_ops,
					     &div->hw, &clk_divider_ops,
					     &gate->hw, &clk_gate_ops,
796 797
					     data->clk_flags |
					     CLK_GET_RATE_NOCACHE);
798 799 800 801 802 803 804 805 806 807 808 809 810 811 812 813 814 815 816 817 818 819 820 821 822 823 824
		if (IS_ERR(clk)) {
			kfree(gate);
			kfree(div);
			kfree(mux);
			goto err;
		}

		pr_debug("%s: parent %s rate %u\n",
			__clk_get_name(clk),
			__clk_get_name(clk_get_parent(clk)),
			(unsigned int)clk_get_rate(clk));

		clk_data->clks[i] = clk;
	}

	kfree(parents);

	of_clk_add_provider(np, of_clk_src_onecell_get, clk_data);
	return;

err:
	for (i = 0; i < clk_data->clk_num; i++) {
		struct clk_composite *composite;

		if (!clk_data->clks[i])
			continue;

825 826 827 828
		composite = to_clk_composite(__clk_get_hw(clk_data->clks[i]));
		kfree(to_clk_gate(composite->gate_hw));
		kfree(to_clk_divider(composite->rate_hw));
		kfree(to_clk_mux(composite->mux_hw));
829 830
	}

831 832
	kfree(clk_data->clks);
err_alloc_clks:
833
	kfree(clk_data);
834
err_alloc:
835
	kfree(parents);
836 837
err_parents:
	iounmap(reg);
838 839
}
CLK_OF_DECLARE(clkgen_vcc, "st,clkgen-vcc", st_of_clkgen_vcc_setup);