[mirror_ubuntu-jammy-kernel.git] / drivers / clk / at91 / clk-master.c

// SPDX-License-Identifier: GPL-2.0-or-later
/*
 *  Copyright (C) 2013 Boris BREZILLON <b.brezillon@overkiz.com>
 */

#include <linux/clk-provider.h>
#include <linux/clkdev.h>
#include <linux/clk/at91_pmc.h>
#include <linux/of.h>
#include <linux/mfd/syscon.h>
#include <linux/regmap.h>

#include "pmc.h"

#define MASTER_PRES_MASK	0x7
#define MASTER_PRES_MAX		MASTER_PRES_MASK
#define MASTER_DIV_SHIFT	8
#define MASTER_DIV_MASK		0x3

#define PMC_MCR			0x30
#define PMC_MCR_ID_MSK		GENMASK(3, 0)
#define PMC_MCR_CMD		BIT(7)
#define PMC_MCR_DIV		GENMASK(10, 8)
#define PMC_MCR_CSS		GENMASK(20, 16)
#define PMC_MCR_CSS_SHIFT	(16)
#define PMC_MCR_EN		BIT(28)

#define PMC_MCR_ID(x)		((x) & PMC_MCR_ID_MSK)

#define MASTER_MAX_ID		4

#define to_clk_master(hw) container_of(hw, struct clk_master, hw)

struct clk_master {
	struct clk_hw hw;
	struct regmap *regmap;
	spinlock_t *lock;
	const struct clk_master_layout *layout;
	const struct clk_master_characteristics *characteristics;
	u32 *mux_table;
	u32 mckr;
	int chg_pid;
	u8 id;
	u8 parent;
	u8 div;
};

static inline bool clk_master_ready(struct clk_master *master)
{
	unsigned int bit = master->id ? AT91_PMC_MCKXRDY : AT91_PMC_MCKRDY;
	unsigned int status;

	regmap_read(master->regmap, AT91_PMC_SR, &status);

	return !!(status & bit);
}

static int clk_master_prepare(struct clk_hw *hw)
{
	struct clk_master *master = to_clk_master(hw);

	while (!clk_master_ready(master))
		cpu_relax();

	return 0;
}

static int clk_master_is_prepared(struct clk_hw *hw)
{
	struct clk_master *master = to_clk_master(hw);

	return clk_master_ready(master);
}

static unsigned long clk_master_recalc_rate(struct clk_hw *hw,
					    unsigned long parent_rate)
{
	u8 pres;
	u8 div;
	unsigned long rate = parent_rate;
	struct clk_master *master = to_clk_master(hw);
	const struct clk_master_layout *layout = master->layout;
	const struct clk_master_characteristics *characteristics =
						master->characteristics;
	unsigned int mckr;

	regmap_read(master->regmap, master->layout->offset, &mckr);
	mckr &= layout->mask;

	pres = (mckr >> layout->pres_shift) & MASTER_PRES_MASK;
	div = (mckr >> MASTER_DIV_SHIFT) & MASTER_DIV_MASK;

	if (characteristics->have_div3_pres && pres == MASTER_PRES_MAX)
		rate /= 3;
	else
		rate >>= pres;

	rate /= characteristics->divisors[div];

	if (rate < characteristics->output.min)
		pr_warn("master clk is underclocked");
	else if (rate > characteristics->output.max)
		pr_warn("master clk is overclocked");

	return rate;
}

static u8 clk_master_get_parent(struct clk_hw *hw)
{
	struct clk_master *master = to_clk_master(hw);
	unsigned int mckr;

	regmap_read(master->regmap, master->layout->offset, &mckr);

	return mckr & AT91_PMC_CSS;
}

static const struct clk_ops master_ops = {
	.prepare = clk_master_prepare,
	.is_prepared = clk_master_is_prepared,
	.recalc_rate = clk_master_recalc_rate,
	.get_parent = clk_master_get_parent,
};

struct clk_hw * __init
at91_clk_register_master(struct regmap *regmap,
		const char *name, int num_parents,
		const char **parent_names,
		const struct clk_master_layout *layout,
		const struct clk_master_characteristics *characteristics)
{
	struct clk_master *master;
	struct clk_init_data init;
	struct clk_hw *hw;
	int ret;

	if (!name || !num_parents || !parent_names)
		return ERR_PTR(-EINVAL);

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

	init.name = name;
	init.ops = &master_ops;
	init.parent_names = parent_names;
	init.num_parents = num_parents;
	init.flags = 0;

	master->hw.init = &init;
	master->layout = layout;
	master->characteristics = characteristics;
	master->regmap = regmap;

	hw = &master->hw;
	ret = clk_hw_register(NULL, &master->hw);
	if (ret) {
		kfree(master);
		hw = ERR_PTR(ret);
	}

	return hw;
}

static unsigned long
clk_sama7g5_master_recalc_rate(struct clk_hw *hw,
			       unsigned long parent_rate)
{
	struct clk_master *master = to_clk_master(hw);

	return DIV_ROUND_CLOSEST_ULL(parent_rate, (1 << master->div));
}

static void clk_sama7g5_master_best_diff(struct clk_rate_request *req,
					 struct clk_hw *parent,
					 unsigned long parent_rate,
					 long *best_rate,
					 long *best_diff,
					 u32 div)
{
	unsigned long tmp_rate, tmp_diff;

	if (div == MASTER_PRES_MAX)
		tmp_rate = parent_rate / 3;
	else
		tmp_rate = parent_rate >> div;

	tmp_diff = abs(req->rate - tmp_rate);

	if (*best_diff < 0 || *best_diff >= tmp_diff) {
		*best_rate = tmp_rate;
		*best_diff = tmp_diff;
		req->best_parent_rate = parent_rate;
		req->best_parent_hw = parent;
	}
}

static int clk_sama7g5_master_determine_rate(struct clk_hw *hw,
					     struct clk_rate_request *req)
{
	struct clk_master *master = to_clk_master(hw);
	struct clk_rate_request req_parent = *req;
	struct clk_hw *parent;
	long best_rate = LONG_MIN, best_diff = LONG_MIN;
	unsigned long parent_rate;
	unsigned int div, i;

	/* First: check the dividers of MCR. */
	for (i = 0; i < clk_hw_get_num_parents(hw); i++) {
		parent = clk_hw_get_parent_by_index(hw, i);
		if (!parent)
			continue;

		parent_rate = clk_hw_get_rate(parent);
		if (!parent_rate)
			continue;

		for (div = 0; div < MASTER_PRES_MAX + 1; div++) {
			clk_sama7g5_master_best_diff(req, parent, parent_rate,
						     &best_rate, &best_diff,
						     div);
			if (!best_diff)
				break;
		}

		if (!best_diff)
			break;
	}

	/* Second: try to request rate form changeable parent. */
	if (master->chg_pid < 0)
		goto end;

	parent = clk_hw_get_parent_by_index(hw, master->chg_pid);
	if (!parent)
		goto end;

	for (div = 0; div < MASTER_PRES_MAX + 1; div++) {
		if (div == MASTER_PRES_MAX)
			req_parent.rate = req->rate * 3;
		else
			req_parent.rate = req->rate << div;

		if (__clk_determine_rate(parent, &req_parent))
			continue;

		clk_sama7g5_master_best_diff(req, parent, req_parent.rate,
					     &best_rate, &best_diff, div);

		if (!best_diff)
			break;
	}

end:
	pr_debug("MCK: %s, best_rate = %ld, parent clk: %s @ %ld\n",
		 __func__, best_rate,
		 __clk_get_name((req->best_parent_hw)->clk),
		req->best_parent_rate);

	if (best_rate < 0)
		return -EINVAL;

	req->rate = best_rate;

	return 0;
}

static u8 clk_sama7g5_master_get_parent(struct clk_hw *hw)
{
	struct clk_master *master = to_clk_master(hw);
	unsigned long flags;
	u8 index;

	spin_lock_irqsave(master->lock, flags);
	index = clk_mux_val_to_index(&master->hw, master->mux_table, 0,
				     master->parent);
	spin_unlock_irqrestore(master->lock, flags);

	return index;
}

static int clk_sama7g5_master_set_parent(struct clk_hw *hw, u8 index)
{
	struct clk_master *master = to_clk_master(hw);
	unsigned long flags;

	if (index >= clk_hw_get_num_parents(hw))
		return -EINVAL;

	spin_lock_irqsave(master->lock, flags);
	master->parent = clk_mux_index_to_val(master->mux_table, 0, index);
	spin_unlock_irqrestore(master->lock, flags);

	return 0;
}

static int clk_sama7g5_master_enable(struct clk_hw *hw)
{
	struct clk_master *master = to_clk_master(hw);
	unsigned long flags;
	unsigned int val, cparent;

	spin_lock_irqsave(master->lock, flags);

	regmap_write(master->regmap, PMC_MCR, PMC_MCR_ID(master->id));
	regmap_read(master->regmap, PMC_MCR, &val);
	regmap_update_bits(master->regmap, PMC_MCR,
			   PMC_MCR_EN | PMC_MCR_CSS | PMC_MCR_DIV |
			   PMC_MCR_CMD | PMC_MCR_ID_MSK,
			   PMC_MCR_EN | (master->parent << PMC_MCR_CSS_SHIFT) |
			   (master->div << MASTER_DIV_SHIFT) |
			   PMC_MCR_CMD | PMC_MCR_ID(master->id));

	cparent = (val & PMC_MCR_CSS) >> PMC_MCR_CSS_SHIFT;

	/* Wait here only if parent is being changed. */
	while ((cparent != master->parent) && !clk_master_ready(master))
		cpu_relax();

	spin_unlock_irqrestore(master->lock, flags);

	return 0;
}

static void clk_sama7g5_master_disable(struct clk_hw *hw)
{
	struct clk_master *master = to_clk_master(hw);
	unsigned long flags;

	spin_lock_irqsave(master->lock, flags);

	regmap_write(master->regmap, PMC_MCR, master->id);
	regmap_update_bits(master->regmap, PMC_MCR,
			   PMC_MCR_EN | PMC_MCR_CMD | PMC_MCR_ID_MSK,
			   PMC_MCR_CMD | PMC_MCR_ID(master->id));

	spin_unlock_irqrestore(master->lock, flags);
}

static int clk_sama7g5_master_is_enabled(struct clk_hw *hw)
{
	struct clk_master *master = to_clk_master(hw);
	unsigned long flags;
	unsigned int val;

	spin_lock_irqsave(master->lock, flags);

	regmap_write(master->regmap, PMC_MCR, master->id);
	regmap_read(master->regmap, PMC_MCR, &val);

	spin_unlock_irqrestore(master->lock, flags);

	return !!(val & PMC_MCR_EN);
}

static int clk_sama7g5_master_set_rate(struct clk_hw *hw, unsigned long rate,
				       unsigned long parent_rate)
{
	struct clk_master *master = to_clk_master(hw);
	unsigned long div, flags;

	div = DIV_ROUND_CLOSEST(parent_rate, rate);
	if ((div > (1 << (MASTER_PRES_MAX - 1))) || (div & (div - 1)))
		return -EINVAL;

	if (div == 3)
		div = MASTER_PRES_MAX;
	else
		div = ffs(div) - 1;

	spin_lock_irqsave(master->lock, flags);
	master->div = div;
	spin_unlock_irqrestore(master->lock, flags);

	return 0;
}

static const struct clk_ops sama7g5_master_ops = {
	.enable = clk_sama7g5_master_enable,
	.disable = clk_sama7g5_master_disable,
	.is_enabled = clk_sama7g5_master_is_enabled,
	.recalc_rate = clk_sama7g5_master_recalc_rate,
	.determine_rate = clk_sama7g5_master_determine_rate,
	.set_rate = clk_sama7g5_master_set_rate,
	.get_parent = clk_sama7g5_master_get_parent,
	.set_parent = clk_sama7g5_master_set_parent,
};

struct clk_hw * __init
at91_clk_sama7g5_register_master(struct regmap *regmap,
				 const char *name, int num_parents,
				 const char **parent_names,
				 u32 *mux_table,
				 spinlock_t *lock, u8 id,
				 bool critical, int chg_pid)
{
	struct clk_master *master;
	struct clk_hw *hw;
	struct clk_init_data init;
	unsigned long flags;
	unsigned int val;
	int ret;

	if (!name || !num_parents || !parent_names || !mux_table ||
	    !lock || id > MASTER_MAX_ID)
		return ERR_PTR(-EINVAL);

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

	init.name = name;
	init.ops = &sama7g5_master_ops;
	init.parent_names = parent_names;
	init.num_parents = num_parents;
	init.flags = CLK_SET_RATE_GATE | CLK_SET_PARENT_GATE;
	if (chg_pid >= 0)
		init.flags |= CLK_SET_RATE_PARENT;
	if (critical)
		init.flags |= CLK_IS_CRITICAL;

	master->hw.init = &init;
	master->regmap = regmap;
	master->id = id;
	master->chg_pid = chg_pid;
	master->lock = lock;
	master->mux_table = mux_table;

	spin_lock_irqsave(master->lock, flags);
	regmap_write(master->regmap, PMC_MCR, master->id);
	regmap_read(master->regmap, PMC_MCR, &val);
	master->parent = (val & PMC_MCR_CSS) >> PMC_MCR_CSS_SHIFT;
	master->div = (val & PMC_MCR_DIV) >> MASTER_DIV_SHIFT;
	spin_unlock_irqrestore(master->lock, flags);

	hw = &master->hw;
	ret = clk_hw_register(NULL, &master->hw);
	if (ret) {
		kfree(master);
		hw = ERR_PTR(ret);
	}

	return hw;
}

const struct clk_master_layout at91rm9200_master_layout = {
	.mask = 0x31F,
	.pres_shift = 2,
	.offset = AT91_PMC_MCKR,
};

const struct clk_master_layout at91sam9x5_master_layout = {
	.mask = 0x373,
	.pres_shift = 4,
	.offset = AT91_PMC_MCKR,
};
Commit	Line	Data
2874c5fd	1	// SPDX-License-Identifier: GPL-2.0-or-later
e442d234 BB	2	/*
e442d234 BB	3	* Copyright (C) 2013 Boris BREZILLON <b.brezillon@overkiz.com>
e442d234 BB	4	*/
	5
	6	#include <linux/clk-provider.h>
	7	#include <linux/clkdev.h>
	8	#include <linux/clk/at91_pmc.h>
	9	#include <linux/of.h>
1bdf0232 BB	10	#include <linux/mfd/syscon.h>
1bdf0232 BB	11	#include <linux/regmap.h>
e442d234 BB	12
	13	#include "pmc.h"
	14
e442d234 BB	15	#define MASTER_PRES_MASK 0x7
	16	#define MASTER_PRES_MAX MASTER_PRES_MASK
	17	#define MASTER_DIV_SHIFT 8
	18	#define MASTER_DIV_MASK 0x3
	19
75c88143 CB	20	#define PMC_MCR 0x30
	21	#define PMC_MCR_ID_MSK GENMASK(3, 0)
	22	#define PMC_MCR_CMD BIT(7)
	23	#define PMC_MCR_DIV GENMASK(10, 8)
	24	#define PMC_MCR_CSS GENMASK(20, 16)
	25	#define PMC_MCR_CSS_SHIFT (16)
	26	#define PMC_MCR_EN BIT(28)
	27
	28	#define PMC_MCR_ID(x) ((x) & PMC_MCR_ID_MSK)
	29
	30	#define MASTER_MAX_ID 4
	31
e442d234 BB	32	#define to_clk_master(hw) container_of(hw, struct clk_master, hw)
	33
	34	struct clk_master {
	35	struct clk_hw hw;
1bdf0232	36	struct regmap *regmap;
75c88143	37	spinlock_t *lock;
e442d234 BB	38	const struct clk_master_layout *layout;
e442d234 BB	39	const struct clk_master_characteristics *characteristics;
75c88143	40	u32 *mux_table;
e5be5370	41	u32 mckr;
75c88143 CB	42	int chg_pid;
	43	u8 id;
	44	u8 parent;
	45	u8 div;
e442d234 BB	46	};
e442d234 BB	47
75c88143	48	static inline bool clk_master_ready(struct clk_master *master)
1bdf0232	49	{
75c88143	50	unsigned int bit = master->id ? AT91_PMC_MCKXRDY : AT91_PMC_MCKRDY;
1bdf0232 BB	51	unsigned int status;
1bdf0232 BB	52
75c88143	53	regmap_read(master->regmap, AT91_PMC_SR, &status);
1bdf0232	54
75c88143	55	return !!(status & bit);
1bdf0232 BB	56	}
1bdf0232 BB	57
e442d234 BB	58	static int clk_master_prepare(struct clk_hw *hw)
	59	{
	60	struct clk_master *master = to_clk_master(hw);
e442d234	61
75c88143	62	while (!clk_master_ready(master))
99a81706	63	cpu_relax();
e442d234 BB	64
	65	return 0;
	66	}
	67
	68	static int clk_master_is_prepared(struct clk_hw *hw)
	69	{
	70	struct clk_master *master = to_clk_master(hw);
	71
75c88143	72	return clk_master_ready(master);
e442d234 BB	73	}
	74
	75	static unsigned long clk_master_recalc_rate(struct clk_hw *hw,
	76	unsigned long parent_rate)
	77	{
	78	u8 pres;
	79	u8 div;
	80	unsigned long rate = parent_rate;
	81	struct clk_master *master = to_clk_master(hw);
e442d234 BB	82	const struct clk_master_layout *layout = master->layout;
	83	const struct clk_master_characteristics *characteristics =
	84	master->characteristics;
1bdf0232	85	unsigned int mckr;
e442d234	86
e5be5370	87	regmap_read(master->regmap, master->layout->offset, &mckr);
1bdf0232	88	mckr &= layout->mask;
e442d234	89
1bdf0232 BB	90	pres = (mckr >> layout->pres_shift) & MASTER_PRES_MASK;
1bdf0232 BB	91	div = (mckr >> MASTER_DIV_SHIFT) & MASTER_DIV_MASK;
e442d234 BB	92
	93	if (characteristics->have_div3_pres && pres == MASTER_PRES_MAX)
	94	rate /= 3;
	95	else
	96	rate >>= pres;
	97
	98	rate /= characteristics->divisors[div];
	99
	100	if (rate < characteristics->output.min)
	101	pr_warn("master clk is underclocked");
	102	else if (rate > characteristics->output.max)
	103	pr_warn("master clk is overclocked");
	104
	105	return rate;
	106	}
	107
	108	static u8 clk_master_get_parent(struct clk_hw *hw)
	109	{
	110	struct clk_master *master = to_clk_master(hw);
1bdf0232 BB	111	unsigned int mckr;
1bdf0232 BB	112
e5be5370	113	regmap_read(master->regmap, master->layout->offset, &mckr);
e442d234	114
1bdf0232	115	return mckr & AT91_PMC_CSS;
e442d234 BB	116	}
	117
	118	static const struct clk_ops master_ops = {
	119	.prepare = clk_master_prepare,
	120	.is_prepared = clk_master_is_prepared,
	121	.recalc_rate = clk_master_recalc_rate,
	122	.get_parent = clk_master_get_parent,
	123	};
	124
b2e39dc0	125	struct clk_hw * __init
99a81706	126	at91_clk_register_master(struct regmap *regmap,
e442d234 BB	127	const char *name, int num_parents,
	128	const char **parent_names,
	129	const struct clk_master_layout *layout,
	130	const struct clk_master_characteristics *characteristics)
	131	{
e442d234	132	struct clk_master *master;
e442d234	133	struct clk_init_data init;
f5644f10 SB	134	struct clk_hw *hw;
f5644f10 SB	135	int ret;
e442d234	136
1bdf0232	137	if (!name \|\| !num_parents \|\| !parent_names)
e442d234 BB	138	return ERR_PTR(-EINVAL);
	139
	140	master = kzalloc(sizeof(*master), GFP_KERNEL);
	141	if (!master)
	142	return ERR_PTR(-ENOMEM);
	143
	144	init.name = name;
	145	init.ops = &master_ops;
	146	init.parent_names = parent_names;
	147	init.num_parents = num_parents;
	148	init.flags = 0;
	149
	150	master->hw.init = &init;
	151	master->layout = layout;
	152	master->characteristics = characteristics;
1bdf0232	153	master->regmap = regmap;
e442d234	154
f5644f10 SB	155	hw = &master->hw;
	156	ret = clk_hw_register(NULL, &master->hw);
	157	if (ret) {
e442d234	158	kfree(master);
f5644f10	159	hw = ERR_PTR(ret);
c76a024e	160	}
e442d234	161
f5644f10	162	return hw;
e442d234 BB	163	}
e442d234 BB	164
75c88143 CB	165	static unsigned long
	166	clk_sama7g5_master_recalc_rate(struct clk_hw *hw,
	167	unsigned long parent_rate)
	168	{
	169	struct clk_master *master = to_clk_master(hw);
	170
	171	return DIV_ROUND_CLOSEST_ULL(parent_rate, (1 << master->div));
	172	}
	173
	174	static void clk_sama7g5_master_best_diff(struct clk_rate_request *req,
	175	struct clk_hw *parent,
	176	unsigned long parent_rate,
	177	long *best_rate,
	178	long *best_diff,
	179	u32 div)
	180	{
	181	unsigned long tmp_rate, tmp_diff;
	182
	183	if (div == MASTER_PRES_MAX)
	184	tmp_rate = parent_rate / 3;
	185	else
	186	tmp_rate = parent_rate >> div;
	187
	188	tmp_diff = abs(req->rate - tmp_rate);
	189
	190	if (best_diff < 0 \|\| best_diff >= tmp_diff) {
	191	*best_rate = tmp_rate;
	192	*best_diff = tmp_diff;
	193	req->best_parent_rate = parent_rate;
	194	req->best_parent_hw = parent;
	195	}
	196	}
	197
	198	static int clk_sama7g5_master_determine_rate(struct clk_hw *hw,
	199	struct clk_rate_request *req)
	200	{
	201	struct clk_master *master = to_clk_master(hw);
	202	struct clk_rate_request req_parent = *req;
	203	struct clk_hw *parent;
	204	long best_rate = LONG_MIN, best_diff = LONG_MIN;
	205	unsigned long parent_rate;
	206	unsigned int div, i;
	207
	208	/* First: check the dividers of MCR. */
	209	for (i = 0; i < clk_hw_get_num_parents(hw); i++) {
	210	parent = clk_hw_get_parent_by_index(hw, i);
	211	if (!parent)
	212	continue;
	213
	214	parent_rate = clk_hw_get_rate(parent);
	215	if (!parent_rate)
	216	continue;
	217
	218	for (div = 0; div < MASTER_PRES_MAX + 1; div++) {
	219	clk_sama7g5_master_best_diff(req, parent, parent_rate,
	220	&best_rate, &best_diff,
	221	div);
	222	if (!best_diff)
	223	break;
	224	}
	225
	226	if (!best_diff)
	227	break;
	228	}
229
230	/* Second: try to request rate form changeable parent. */
231	if (master->chg_pid < 0)
232	goto end;
233
234	parent = clk_hw_get_parent_by_index(hw, master->chg_pid);
235	if (!parent)
236	goto end;
237
238	for (div = 0; div < MASTER_PRES_MAX + 1; div++) {
239	if (div == MASTER_PRES_MAX)
240	req_parent.rate = req->rate * 3;
241	else
242	req_parent.rate = req->rate << div;
243
244	if (__clk_determine_rate(parent, &req_parent))
245	continue;
246
247	clk_sama7g5_master_best_diff(req, parent, req_parent.rate,
248	&best_rate, &best_diff, div);
249
250	if (!best_diff)
251	break;
252	}
253
254	end:
255	pr_debug("MCK: %s, best_rate = %ld, parent clk: %s @ %ld\n",
256	__func__, best_rate,
257	__clk_get_name((req->best_parent_hw)->clk),
258	req->best_parent_rate);
259
260	if (best_rate < 0)
261	return -EINVAL;
262
263	req->rate = best_rate;
264
265	return 0;
266	}
267
268	static u8 clk_sama7g5_master_get_parent(struct clk_hw *hw)
269	{
270	struct clk_master *master = to_clk_master(hw);
271	unsigned long flags;
272	u8 index;
273
274	spin_lock_irqsave(master->lock, flags);
275	index = clk_mux_val_to_index(&master->hw, master->mux_table, 0,
276	master->parent);
277	spin_unlock_irqrestore(master->lock, flags);
278
279	return index;
280	}
281
282	static int clk_sama7g5_master_set_parent(struct clk_hw *hw, u8 index)
283	{
284	struct clk_master *master = to_clk_master(hw);
285	unsigned long flags;
286
287	if (index >= clk_hw_get_num_parents(hw))
288	return -EINVAL;
289
290	spin_lock_irqsave(master->lock, flags);
291	master->parent = clk_mux_index_to_val(master->mux_table, 0, index);
292	spin_unlock_irqrestore(master->lock, flags);
293
294	return 0;
295	}
296
297	static int clk_sama7g5_master_enable(struct clk_hw *hw)
298	{
299	struct clk_master *master = to_clk_master(hw);
300	unsigned long flags;
301	unsigned int val, cparent;
302
303	spin_lock_irqsave(master->lock, flags);
304
305	regmap_write(master->regmap, PMC_MCR, PMC_MCR_ID(master->id));
306	regmap_read(master->regmap, PMC_MCR, &val);
307	regmap_update_bits(master->regmap, PMC_MCR,
308	PMC_MCR_EN \| PMC_MCR_CSS \| PMC_MCR_DIV \|
309	PMC_MCR_CMD \| PMC_MCR_ID_MSK,
310	PMC_MCR_EN \| (master->parent << PMC_MCR_CSS_SHIFT) \|
311	(master->div << MASTER_DIV_SHIFT) \|
312	PMC_MCR_CMD \| PMC_MCR_ID(master->id));
313
314	cparent = (val & PMC_MCR_CSS) >> PMC_MCR_CSS_SHIFT;
315
316	/* Wait here only if parent is being changed. */
317	while ((cparent != master->parent) && !clk_master_ready(master))
318	cpu_relax();
319
320	spin_unlock_irqrestore(master->lock, flags);
321
322	return 0;
323	}
324
325	static void clk_sama7g5_master_disable(struct clk_hw *hw)
326	{
327	struct clk_master *master = to_clk_master(hw);
328	unsigned long flags;
329
330	spin_lock_irqsave(master->lock, flags);
331
332	regmap_write(master->regmap, PMC_MCR, master->id);
333	regmap_update_bits(master->regmap, PMC_MCR,
334	PMC_MCR_EN \| PMC_MCR_CMD \| PMC_MCR_ID_MSK,
335	PMC_MCR_CMD \| PMC_MCR_ID(master->id));
336
337	spin_unlock_irqrestore(master->lock, flags);
338	}
339
340	static int clk_sama7g5_master_is_enabled(struct clk_hw *hw)
341	{
342	struct clk_master *master = to_clk_master(hw);
343	unsigned long flags;
344	unsigned int val;
345
346	spin_lock_irqsave(master->lock, flags);
347
348	regmap_write(master->regmap, PMC_MCR, master->id);
349	regmap_read(master->regmap, PMC_MCR, &val);
350
351	spin_unlock_irqrestore(master->lock, flags);
352
353	return !!(val & PMC_MCR_EN);
354	}
355
356	static int clk_sama7g5_master_set_rate(struct clk_hw *hw, unsigned long rate,
357	unsigned long parent_rate)
358	{
359	struct clk_master *master = to_clk_master(hw);
360	unsigned long div, flags;
361
362	div = DIV_ROUND_CLOSEST(parent_rate, rate);
363	if ((div > (1 << (MASTER_PRES_MAX - 1))) \|\| (div & (div - 1)))
364	return -EINVAL;
365
366	if (div == 3)
367	div = MASTER_PRES_MAX;
368	else
369	div = ffs(div) - 1;
370
371	spin_lock_irqsave(master->lock, flags);
372	master->div = div;
373	spin_unlock_irqrestore(master->lock, flags);
374
375	return 0;
376	}
377
378	static const struct clk_ops sama7g5_master_ops = {
379	.enable = clk_sama7g5_master_enable,
380	.disable = clk_sama7g5_master_disable,
381	.is_enabled = clk_sama7g5_master_is_enabled,
382	.recalc_rate = clk_sama7g5_master_recalc_rate,
383	.determine_rate = clk_sama7g5_master_determine_rate,
384	.set_rate = clk_sama7g5_master_set_rate,
385	.get_parent = clk_sama7g5_master_get_parent,
386	.set_parent = clk_sama7g5_master_set_parent,
387	};
388
389	struct clk_hw * __init
390	at91_clk_sama7g5_register_master(struct regmap *regmap,
391	const char *name, int num_parents,
392	const char **parent_names,
393	u32 *mux_table,
394	spinlock_t *lock, u8 id,
395	bool critical, int chg_pid)
396	{
397	struct clk_master *master;
398	struct clk_hw *hw;
399	struct clk_init_data init;
400	unsigned long flags;
401	unsigned int val;
402	int ret;
403
404	if (!name \|\| !num_parents \|\| !parent_names \|\| !mux_table \|\|
405	!lock \|\| id > MASTER_MAX_ID)
406	return ERR_PTR(-EINVAL);
407
408	master = kzalloc(sizeof(*master), GFP_KERNEL);
409	if (!master)
410	return ERR_PTR(-ENOMEM);
411
412	init.name = name;
413	init.ops = &sama7g5_master_ops;
414	init.parent_names = parent_names;
415	init.num_parents = num_parents;
416	init.flags = CLK_SET_RATE_GATE \| CLK_SET_PARENT_GATE;
417	if (chg_pid >= 0)
418	init.flags \|= CLK_SET_RATE_PARENT;
419	if (critical)
420	init.flags \|= CLK_IS_CRITICAL;
421
422	master->hw.init = &init;
423	master->regmap = regmap;
424	master->id = id;
425	master->chg_pid = chg_pid;
426	master->lock = lock;
427	master->mux_table = mux_table;
428
429	spin_lock_irqsave(master->lock, flags);
430	regmap_write(master->regmap, PMC_MCR, master->id);
431	regmap_read(master->regmap, PMC_MCR, &val);
432	master->parent = (val & PMC_MCR_CSS) >> PMC_MCR_CSS_SHIFT;
433	master->div = (val & PMC_MCR_DIV) >> MASTER_DIV_SHIFT;
434	spin_unlock_irqrestore(master->lock, flags);
435
436	hw = &master->hw;
437	ret = clk_hw_register(NULL, &master->hw);
438	if (ret) {
439	kfree(master);
440	hw = ERR_PTR(ret);
441	}
442
443	return hw;
444	}
445
b2e39dc0	446	const struct clk_master_layout at91rm9200_master_layout = {
e442d234 BB	447	.mask = 0x31F,
e442d234 BB	448	.pres_shift = 2,
e5be5370	449	.offset = AT91_PMC_MCKR,
e442d234 BB	450	};
e442d234 BB	451
b2e39dc0	452	const struct clk_master_layout at91sam9x5_master_layout = {
e442d234 BB	453	.mask = 0x373,
e442d234 BB	454	.pres_shift = 4,
e5be5370	455	.offset = AT91_PMC_MCKR,
e442d234	456	};