[linux-block.git] / drivers / clk / at91 / clk-pll.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 PLL_STATUS_MASK(id)	(1 << (1 + (id)))
#define PLL_REG(id)		(AT91_CKGR_PLLAR + ((id) * 4))
#define PLL_DIV_MASK		0xff
#define PLL_DIV_MAX		PLL_DIV_MASK
#define PLL_DIV(reg)		((reg) & PLL_DIV_MASK)
#define PLL_MUL(reg, layout)	(((reg) >> (layout)->mul_shift) & \
				 (layout)->mul_mask)
#define PLL_MUL_MIN		2
#define PLL_MUL_MASK(layout)	((layout)->mul_mask)
#define PLL_MUL_MAX(layout)	(PLL_MUL_MASK(layout) + 1)
#define PLL_ICPR_SHIFT(id)	((id) * 16)
#define PLL_ICPR_MASK(id)	(0xffff << PLL_ICPR_SHIFT(id))
#define PLL_MAX_COUNT		0x3f
#define PLL_COUNT_SHIFT		8
#define PLL_OUT_SHIFT		14
#define PLL_MAX_ID		1

#define to_clk_pll(hw) container_of(hw, struct clk_pll, hw)

struct clk_pll {
	struct clk_hw hw;
	struct regmap *regmap;
	u8 id;
	u8 div;
	u8 range;
	u16 mul;
	const struct clk_pll_layout *layout;
	const struct clk_pll_characteristics *characteristics;
	struct at91_clk_pms pms;
};

static inline bool clk_pll_ready(struct regmap *regmap, int id)
{
	unsigned int status;

	regmap_read(regmap, AT91_PMC_SR, &status);

	return status & PLL_STATUS_MASK(id) ? 1 : 0;
}

static int clk_pll_prepare(struct clk_hw *hw)
{
	struct clk_pll *pll = to_clk_pll(hw);
	struct regmap *regmap = pll->regmap;
	const struct clk_pll_layout *layout = pll->layout;
	const struct clk_pll_characteristics *characteristics =
							pll->characteristics;
	u8 id = pll->id;
	u32 mask = PLL_STATUS_MASK(id);
	int offset = PLL_REG(id);
	u8 out = 0;
	unsigned int pllr;
	unsigned int status;
	u8 div;
	u16 mul;

	regmap_read(regmap, offset, &pllr);
	div = PLL_DIV(pllr);
	mul = PLL_MUL(pllr, layout);

	regmap_read(regmap, AT91_PMC_SR, &status);
	if ((status & mask) &&
	    (div == pll->div && mul == pll->mul))
		return 0;

	if (characteristics->out)
		out = characteristics->out[pll->range];

	if (characteristics->icpll)
		regmap_update_bits(regmap, AT91_PMC_PLLICPR, PLL_ICPR_MASK(id),
			characteristics->icpll[pll->range] << PLL_ICPR_SHIFT(id));

	regmap_update_bits(regmap, offset, layout->pllr_mask,
			pll->div | (PLL_MAX_COUNT << PLL_COUNT_SHIFT) |
			(out << PLL_OUT_SHIFT) |
			((pll->mul & layout->mul_mask) << layout->mul_shift));

	while (!clk_pll_ready(regmap, pll->id))
		cpu_relax();

	return 0;
}

static int clk_pll_is_prepared(struct clk_hw *hw)
{
	struct clk_pll *pll = to_clk_pll(hw);

	return clk_pll_ready(pll->regmap, pll->id);
}

static void clk_pll_unprepare(struct clk_hw *hw)
{
	struct clk_pll *pll = to_clk_pll(hw);
	unsigned int mask = pll->layout->pllr_mask;

	regmap_update_bits(pll->regmap, PLL_REG(pll->id), mask, ~mask);
}

static unsigned long clk_pll_recalc_rate(struct clk_hw *hw,
					 unsigned long parent_rate)
{
	struct clk_pll *pll = to_clk_pll(hw);

	if (!pll->div || !pll->mul)
		return 0;

	return (parent_rate / pll->div) * (pll->mul + 1);
}

static long clk_pll_get_best_div_mul(struct clk_pll *pll, unsigned long rate,
				     unsigned long parent_rate,
				     u32 *div, u32 *mul,
				     u32 *index) {
	const struct clk_pll_layout *layout = pll->layout;
	const struct clk_pll_characteristics *characteristics =
							pll->characteristics;
	unsigned long bestremainder = ULONG_MAX;
	unsigned long maxdiv, mindiv, tmpdiv;
	long bestrate = -ERANGE;
	unsigned long bestdiv;
	unsigned long bestmul;
	int i = 0;

	/* Check if parent_rate is a valid input rate */
	if (parent_rate < characteristics->input.min)
		return -ERANGE;

	/*
	 * Calculate minimum divider based on the minimum multiplier, the
	 * parent_rate and the requested rate.
	 * Should always be 2 according to the input and output characteristics
	 * of the PLL blocks.
	 */
	mindiv = (parent_rate * PLL_MUL_MIN) / rate;
	if (!mindiv)
		mindiv = 1;

	if (parent_rate > characteristics->input.max) {
		tmpdiv = DIV_ROUND_UP(parent_rate, characteristics->input.max);
		if (tmpdiv > PLL_DIV_MAX)
			return -ERANGE;

		if (tmpdiv > mindiv)
			mindiv = tmpdiv;
	}

	/*
	 * Calculate the maximum divider which is limited by PLL register
	 * layout (limited by the MUL or DIV field size).
	 */
	maxdiv = DIV_ROUND_UP(parent_rate * PLL_MUL_MAX(layout), rate);
	if (maxdiv > PLL_DIV_MAX)
		maxdiv = PLL_DIV_MAX;

	/*
	 * Iterate over the acceptable divider values to find the best
	 * divider/multiplier pair (the one that generates the closest
	 * rate to the requested one).
	 */
	for (tmpdiv = mindiv; tmpdiv <= maxdiv; tmpdiv++) {
		unsigned long remainder;
		unsigned long tmprate;
		unsigned long tmpmul;

		/*
		 * Calculate the multiplier associated with the current
		 * divider that provide the closest rate to the requested one.
		 */
		tmpmul = DIV_ROUND_CLOSEST(rate, parent_rate / tmpdiv);
		tmprate = (parent_rate / tmpdiv) * tmpmul;
		if (tmprate > rate)
			remainder = tmprate - rate;
		else
			remainder = rate - tmprate;

		/*
		 * Compare the remainder with the best remainder found until
		 * now and elect a new best multiplier/divider pair if the
		 * current remainder is smaller than the best one.
		 */
		if (remainder < bestremainder) {
			bestremainder = remainder;
			bestdiv = tmpdiv;
			bestmul = tmpmul;
			bestrate = tmprate;
		}

		/*
		 * We've found a perfect match!
		 * Stop searching now and use this multiplier/divider pair.
		 */
		if (!remainder)
			break;
	}

	/* We haven't found any multiplier/divider pair => return -ERANGE */
	if (bestrate < 0)
		return bestrate;

	/* Check if bestrate is a valid output rate  */
	for (i = 0; i < characteristics->num_output; i++) {
		if (bestrate >= characteristics->output[i].min &&
		    bestrate <= characteristics->output[i].max)
			break;
	}

	if (i >= characteristics->num_output)
		return -ERANGE;

	if (div)
		*div = bestdiv;
	if (mul)
		*mul = bestmul - 1;
	if (index)
		*index = i;

	return bestrate;
}

static long clk_pll_round_rate(struct clk_hw *hw, unsigned long rate,
					unsigned long *parent_rate)
{
	struct clk_pll *pll = to_clk_pll(hw);

	return clk_pll_get_best_div_mul(pll, rate, *parent_rate,
					NULL, NULL, NULL);
}

static int clk_pll_set_rate(struct clk_hw *hw, unsigned long rate,
			    unsigned long parent_rate)
{
	struct clk_pll *pll = to_clk_pll(hw);
	long ret;
	u32 div;
	u32 mul;
	u32 index;

	ret = clk_pll_get_best_div_mul(pll, rate, parent_rate,
				       &div, &mul, &index);
	if (ret < 0)
		return ret;

	pll->range = index;
	pll->div = div;
	pll->mul = mul;

	return 0;
}

static int clk_pll_save_context(struct clk_hw *hw)
{
	struct clk_pll *pll = to_clk_pll(hw);
	struct clk_hw *parent_hw = clk_hw_get_parent(hw);

	pll->pms.parent_rate = clk_hw_get_rate(parent_hw);
	pll->pms.rate = clk_pll_recalc_rate(&pll->hw, pll->pms.parent_rate);
	pll->pms.status = clk_pll_ready(pll->regmap, PLL_REG(pll->id));

	return 0;
}

static void clk_pll_restore_context(struct clk_hw *hw)
{
	struct clk_pll *pll = to_clk_pll(hw);
	unsigned long calc_rate;
	unsigned int pllr, pllr_out, pllr_count;
	u8 out = 0;

	if (pll->characteristics->out)
		out = pll->characteristics->out[pll->range];

	regmap_read(pll->regmap, PLL_REG(pll->id), &pllr);

	calc_rate = (pll->pms.parent_rate / PLL_DIV(pllr)) *
		     (PLL_MUL(pllr, pll->layout) + 1);
	pllr_count = (pllr >> PLL_COUNT_SHIFT) & PLL_MAX_COUNT;
	pllr_out = (pllr >> PLL_OUT_SHIFT) & out;

	if (pll->pms.rate != calc_rate ||
	    pll->pms.status != clk_pll_ready(pll->regmap, PLL_REG(pll->id)) ||
	    pllr_count != PLL_MAX_COUNT ||
	    (out && pllr_out != out))
		pr_warn("PLLAR was not configured properly by firmware\n");
}

static const struct clk_ops pll_ops = {
	.prepare = clk_pll_prepare,
	.unprepare = clk_pll_unprepare,
	.is_prepared = clk_pll_is_prepared,
	.recalc_rate = clk_pll_recalc_rate,
	.round_rate = clk_pll_round_rate,
	.set_rate = clk_pll_set_rate,
	.save_context = clk_pll_save_context,
	.restore_context = clk_pll_restore_context,
};

struct clk_hw * __init
at91_clk_register_pll(struct regmap *regmap, const char *name,
		      const char *parent_name, u8 id,
		      const struct clk_pll_layout *layout,
		      const struct clk_pll_characteristics *characteristics)
{
	struct clk_pll *pll;
	struct clk_hw *hw;
	struct clk_init_data init;
	int offset = PLL_REG(id);
	unsigned int pllr;
	int ret;

	if (id > PLL_MAX_ID)
		return ERR_PTR(-EINVAL);

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

	init.name = name;
	init.ops = &pll_ops;
	init.parent_names = &parent_name;
	init.num_parents = 1;
	init.flags = CLK_SET_RATE_GATE;

	pll->id = id;
	pll->hw.init = &init;
	pll->layout = layout;
	pll->characteristics = characteristics;
	pll->regmap = regmap;
	regmap_read(regmap, offset, &pllr);
	pll->div = PLL_DIV(pllr);
	pll->mul = PLL_MUL(pllr, layout);

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

	return hw;
}


const struct clk_pll_layout at91rm9200_pll_layout = {
	.pllr_mask = 0x7FFFFFF,
	.mul_shift = 16,
	.mul_mask = 0x7FF,
};

const struct clk_pll_layout at91sam9g45_pll_layout = {
	.pllr_mask = 0xFFFFFF,
	.mul_shift = 16,
	.mul_mask = 0xFF,
};

const struct clk_pll_layout at91sam9g20_pllb_layout = {
	.pllr_mask = 0x3FFFFF,
	.mul_shift = 16,
	.mul_mask = 0x3F,
};

const struct clk_pll_layout sama5d3_pll_layout = {
	.pllr_mask = 0x1FFFFFF,
	.mul_shift = 18,
	.mul_mask = 0x7F,
};
Commit	Line	Data
2874c5fd	1	// SPDX-License-Identifier: GPL-2.0-or-later
1a748d2b BB	2	/*
1a748d2b BB	3	* Copyright (C) 2013 Boris BREZILLON <b.brezillon@overkiz.com>
1a748d2b 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>
1a748d2b BB	12
	13	#include "pmc.h"
	14
	15	#define PLL_STATUS_MASK(id) (1 << (1 + (id)))
	16	#define PLL_REG(id) (AT91_CKGR_PLLAR + ((id) * 4))
	17	#define PLL_DIV_MASK 0xff
	18	#define PLL_DIV_MAX PLL_DIV_MASK
	19	#define PLL_DIV(reg) ((reg) & PLL_DIV_MASK)
	20	#define PLL_MUL(reg, layout) (((reg) >> (layout)->mul_shift) & \
	21	(layout)->mul_mask)
3ef9dd2b BB	22	#define PLL_MUL_MIN 2
	23	#define PLL_MUL_MASK(layout) ((layout)->mul_mask)
	24	#define PLL_MUL_MAX(layout) (PLL_MUL_MASK(layout) + 1)
1a748d2b BB	25	#define PLL_ICPR_SHIFT(id) ((id) * 16)
1a748d2b BB	26	#define PLL_ICPR_MASK(id) (0xffff << PLL_ICPR_SHIFT(id))
078a3eb5	27	#define PLL_MAX_COUNT 0x3f
1a748d2b BB	28	#define PLL_COUNT_SHIFT 8
	29	#define PLL_OUT_SHIFT 14
	30	#define PLL_MAX_ID 1
	31
1a748d2b BB	32	#define to_clk_pll(hw) container_of(hw, struct clk_pll, hw)
	33
	34	struct clk_pll {
	35	struct clk_hw hw;
1bdf0232	36	struct regmap *regmap;
1a748d2b BB	37	u8 id;
	38	u8 div;
	39	u8 range;
	40	u16 mul;
	41	const struct clk_pll_layout *layout;
	42	const struct clk_pll_characteristics *characteristics;
36971566	43	struct at91_clk_pms pms;
1a748d2b BB	44	};
1a748d2b BB	45
1bdf0232 BB	46	static inline bool clk_pll_ready(struct regmap *regmap, int id)
	47	{
	48	unsigned int status;
	49
	50	regmap_read(regmap, AT91_PMC_SR, &status);
	51
	52	return status & PLL_STATUS_MASK(id) ? 1 : 0;
	53	}
	54
1a748d2b BB	55	static int clk_pll_prepare(struct clk_hw *hw)
	56	{
	57	struct clk_pll *pll = to_clk_pll(hw);
1bdf0232	58	struct regmap *regmap = pll->regmap;
1a748d2b	59	const struct clk_pll_layout *layout = pll->layout;
e442d234 BB	60	const struct clk_pll_characteristics *characteristics =
e442d234 BB	61	pll->characteristics;
1a748d2b BB	62	u8 id = pll->id;
	63	u32 mask = PLL_STATUS_MASK(id);
	64	int offset = PLL_REG(id);
	65	u8 out = 0;
1bdf0232 BB	66	unsigned int pllr;
1bdf0232 BB	67	unsigned int status;
1a748d2b BB	68	u8 div;
	69	u16 mul;
	70
1bdf0232	71	regmap_read(regmap, offset, &pllr);
1a748d2b BB	72	div = PLL_DIV(pllr);
	73	mul = PLL_MUL(pllr, layout);
	74
1bdf0232 BB	75	regmap_read(regmap, AT91_PMC_SR, &status);
1bdf0232 BB	76	if ((status & mask) &&
1a748d2b BB	77	(div == pll->div && mul == pll->mul))
	78	return 0;
	79
	80	if (characteristics->out)
	81	out = characteristics->out[pll->range];
1a748d2b	82
1bdf0232 BB	83	if (characteristics->icpll)
	84	regmap_update_bits(regmap, AT91_PMC_PLLICPR, PLL_ICPR_MASK(id),
	85	characteristics->icpll[pll->range] << PLL_ICPR_SHIFT(id));
1a748d2b	86
1bdf0232 BB	87	regmap_update_bits(regmap, offset, layout->pllr_mask,
	88	pll->div \| (PLL_MAX_COUNT << PLL_COUNT_SHIFT) \|
	89	(out << PLL_OUT_SHIFT) \|
	90	((pll->mul & layout->mul_mask) << layout->mul_shift));
	91
99a81706 AB	92	while (!clk_pll_ready(regmap, pll->id))
99a81706 AB	93	cpu_relax();
1a748d2b BB	94
	95	return 0;
	96	}
	97
	98	static int clk_pll_is_prepared(struct clk_hw *hw)
	99	{
	100	struct clk_pll *pll = to_clk_pll(hw);
1a748d2b	101
1bdf0232	102	return clk_pll_ready(pll->regmap, pll->id);
1a748d2b BB	103	}
	104
	105	static void clk_pll_unprepare(struct clk_hw *hw)
	106	{
	107	struct clk_pll *pll = to_clk_pll(hw);
1bdf0232	108	unsigned int mask = pll->layout->pllr_mask;
1a748d2b	109
1bdf0232	110	regmap_update_bits(pll->regmap, PLL_REG(pll->id), mask, ~mask);
1a748d2b BB	111	}
	112
	113	static unsigned long clk_pll_recalc_rate(struct clk_hw *hw,
	114	unsigned long parent_rate)
	115	{
	116	struct clk_pll *pll = to_clk_pll(hw);
1a748d2b	117
0f5cb0e6 RW	118	if (!pll->div \|\| !pll->mul)
	119	return 0;
	120
a982e45d	121	return (parent_rate / pll->div) * (pll->mul + 1);
1a748d2b BB	122	}
	123
	124	static long clk_pll_get_best_div_mul(struct clk_pll *pll, unsigned long rate,
	125	unsigned long parent_rate,
	126	u32 div, u32 mul,
	127	u32 *index) {
1a748d2b BB	128	const struct clk_pll_layout *layout = pll->layout;
	129	const struct clk_pll_characteristics *characteristics =
	130	pll->characteristics;
3ef9dd2b BB	131	unsigned long bestremainder = ULONG_MAX;
	132	unsigned long maxdiv, mindiv, tmpdiv;
	133	long bestrate = -ERANGE;
	134	unsigned long bestdiv;
	135	unsigned long bestmul;
	136	int i = 0;
1a748d2b	137
3ef9dd2b	138	/* Check if parent_rate is a valid input rate */
6c7b03e1	139	if (parent_rate < characteristics->input.min)
1a748d2b BB	140	return -ERANGE;
1a748d2b BB	141
3ef9dd2b BB	142	/*
	143	* Calculate minimum divider based on the minimum multiplier, the
	144	* parent_rate and the requested rate.
	145	* Should always be 2 according to the input and output characteristics
	146	* of the PLL blocks.
	147	*/
	148	mindiv = (parent_rate * PLL_MUL_MIN) / rate;
	149	if (!mindiv)
	150	mindiv = 1;
	151
6c7b03e1 BB	152	if (parent_rate > characteristics->input.max) {
	153	tmpdiv = DIV_ROUND_UP(parent_rate, characteristics->input.max);
	154	if (tmpdiv > PLL_DIV_MAX)
	155	return -ERANGE;
	156
	157	if (tmpdiv > mindiv)
	158	mindiv = tmpdiv;
	159	}
	160
3ef9dd2b BB	161	/*
	162	* Calculate the maximum divider which is limited by PLL register
	163	* layout (limited by the MUL or DIV field size).
	164	*/
	165	maxdiv = DIV_ROUND_UP(parent_rate * PLL_MUL_MAX(layout), rate);
	166	if (maxdiv > PLL_DIV_MAX)
	167	maxdiv = PLL_DIV_MAX;
	168
	169	/*
	170	* Iterate over the acceptable divider values to find the best
	171	* divider/multiplier pair (the one that generates the closest
	172	* rate to the requested one).
	173	*/
	174	for (tmpdiv = mindiv; tmpdiv <= maxdiv; tmpdiv++) {
	175	unsigned long remainder;
	176	unsigned long tmprate;
	177	unsigned long tmpmul;
	178
	179	/*
	180	* Calculate the multiplier associated with the current
	181	* divider that provide the closest rate to the requested one.
	182	*/
	183	tmpmul = DIV_ROUND_CLOSEST(rate, parent_rate / tmpdiv);
	184	tmprate = (parent_rate / tmpdiv) * tmpmul;
	185	if (tmprate > rate)
	186	remainder = tmprate - rate;
	187	else
	188	remainder = rate - tmprate;
	189
	190	/*
	191	* Compare the remainder with the best remainder found until
	192	* now and elect a new best multiplier/divider pair if the
	193	* current remainder is smaller than the best one.
	194	*/
1a748d2b BB	195	if (remainder < bestremainder) {
1a748d2b BB	196	bestremainder = remainder;
1a748d2b	197	bestdiv = tmpdiv;
3ef9dd2b BB	198	bestmul = tmpmul;
3ef9dd2b BB	199	bestrate = tmprate;
1a748d2b BB	200	}
1a748d2b BB	201
3ef9dd2b BB	202	/*
	203	* We've found a perfect match!
	204	* Stop searching now and use this multiplier/divider pair.
	205	*/
1a748d2b BB	206	if (!remainder)
	207	break;
	208	}
	209
3ef9dd2b BB	210	/* We haven't found any multiplier/divider pair => return -ERANGE */
	211	if (bestrate < 0)
	212	return bestrate;
	213
	214	/* Check if bestrate is a valid output rate */
	215	for (i = 0; i < characteristics->num_output; i++) {
	216	if (bestrate >= characteristics->output[i].min &&
	217	bestrate <= characteristics->output[i].max)
	218	break;
	219	}
	220
	221	if (i >= characteristics->num_output)
	222	return -ERANGE;
1a748d2b BB	223
	224	if (div)
	225	*div = bestdiv;
	226	if (mul)
3ef9dd2b	227	*mul = bestmul - 1;
1a748d2b BB	228	if (index)
	229	*index = i;
	230
3ef9dd2b	231	return bestrate;
1a748d2b BB	232	}
	233
	234	static long clk_pll_round_rate(struct clk_hw *hw, unsigned long rate,
	235	unsigned long *parent_rate)
	236	{
	237	struct clk_pll *pll = to_clk_pll(hw);
	238
	239	return clk_pll_get_best_div_mul(pll, rate, *parent_rate,
	240	NULL, NULL, NULL);
	241	}
	242
	243	static int clk_pll_set_rate(struct clk_hw *hw, unsigned long rate,
	244	unsigned long parent_rate)
	245	{
	246	struct clk_pll *pll = to_clk_pll(hw);
1a748d2b BB	247	long ret;
	248	u32 div;
	249	u32 mul;
	250	u32 index;
1a748d2b BB	251
	252	ret = clk_pll_get_best_div_mul(pll, rate, parent_rate,
	253	&div, &mul, &index);
	254	if (ret < 0)
	255	return ret;
	256
	257	pll->range = index;
	258	pll->div = div;
	259	pll->mul = mul;
	260
	261	return 0;
	262	}
	263
36971566 CB	264	static int clk_pll_save_context(struct clk_hw *hw)
	265	{
	266	struct clk_pll *pll = to_clk_pll(hw);
	267	struct clk_hw *parent_hw = clk_hw_get_parent(hw);
	268
	269	pll->pms.parent_rate = clk_hw_get_rate(parent_hw);
	270	pll->pms.rate = clk_pll_recalc_rate(&pll->hw, pll->pms.parent_rate);
	271	pll->pms.status = clk_pll_ready(pll->regmap, PLL_REG(pll->id));
	272
	273	return 0;
	274	}
	275
	276	static void clk_pll_restore_context(struct clk_hw *hw)
	277	{
	278	struct clk_pll *pll = to_clk_pll(hw);
	279	unsigned long calc_rate;
	280	unsigned int pllr, pllr_out, pllr_count;
	281	u8 out = 0;
	282
	283	if (pll->characteristics->out)
	284	out = pll->characteristics->out[pll->range];
	285
	286	regmap_read(pll->regmap, PLL_REG(pll->id), &pllr);
	287
	288	calc_rate = (pll->pms.parent_rate / PLL_DIV(pllr)) *
	289	(PLL_MUL(pllr, pll->layout) + 1);
	290	pllr_count = (pllr >> PLL_COUNT_SHIFT) & PLL_MAX_COUNT;
	291	pllr_out = (pllr >> PLL_OUT_SHIFT) & out;
	292
	293	if (pll->pms.rate != calc_rate \|\|
	294	pll->pms.status != clk_pll_ready(pll->regmap, PLL_REG(pll->id)) \|\|
	295	pllr_count != PLL_MAX_COUNT \|\|
	296	(out && pllr_out != out))
	297	pr_warn("PLLAR was not configured properly by firmware\n");
	298	}
	299
1a748d2b BB	300	static const struct clk_ops pll_ops = {
	301	.prepare = clk_pll_prepare,
	302	.unprepare = clk_pll_unprepare,
	303	.is_prepared = clk_pll_is_prepared,
	304	.recalc_rate = clk_pll_recalc_rate,
	305	.round_rate = clk_pll_round_rate,
	306	.set_rate = clk_pll_set_rate,
36971566 CB	307	.save_context = clk_pll_save_context,
36971566 CB	308	.restore_context = clk_pll_restore_context,
1a748d2b BB	309	};
1a748d2b BB	310
b2e39dc0	311	struct clk_hw * __init
99a81706	312	at91_clk_register_pll(struct regmap regmap, const char name,
1a748d2b BB	313	const char *parent_name, u8 id,
	314	const struct clk_pll_layout *layout,
	315	const struct clk_pll_characteristics *characteristics)
	316	{
	317	struct clk_pll *pll;
f5644f10	318	struct clk_hw *hw;
1a748d2b	319	struct clk_init_data init;
1a748d2b	320	int offset = PLL_REG(id);
1bdf0232	321	unsigned int pllr;
f5644f10	322	int ret;
1a748d2b BB	323
	324	if (id > PLL_MAX_ID)
	325	return ERR_PTR(-EINVAL);
	326
	327	pll = kzalloc(sizeof(*pll), GFP_KERNEL);
	328	if (!pll)
	329	return ERR_PTR(-ENOMEM);
	330
	331	init.name = name;
	332	init.ops = &pll_ops;
	333	init.parent_names = &parent_name;
	334	init.num_parents = 1;
	335	init.flags = CLK_SET_RATE_GATE;
	336
	337	pll->id = id;
	338	pll->hw.init = &init;
	339	pll->layout = layout;
	340	pll->characteristics = characteristics;
1bdf0232	341	pll->regmap = regmap;
1bdf0232 BB	342	regmap_read(regmap, offset, &pllr);
	343	pll->div = PLL_DIV(pllr);
	344	pll->mul = PLL_MUL(pllr, layout);
1a748d2b	345
f5644f10 SB	346	hw = &pll->hw;
	347	ret = clk_hw_register(NULL, &pll->hw);
	348	if (ret) {
1a748d2b	349	kfree(pll);
f5644f10	350	hw = ERR_PTR(ret);
c76a024e	351	}
1a748d2b	352
f5644f10	353	return hw;
1a748d2b BB	354	}
	355
	356
b2e39dc0	357	const struct clk_pll_layout at91rm9200_pll_layout = {
1a748d2b BB	358	.pllr_mask = 0x7FFFFFF,
	359	.mul_shift = 16,
	360	.mul_mask = 0x7FF,
	361	};
	362
b2e39dc0	363	const struct clk_pll_layout at91sam9g45_pll_layout = {
1a748d2b BB	364	.pllr_mask = 0xFFFFFF,
	365	.mul_shift = 16,
	366	.mul_mask = 0xFF,
	367	};
	368
b2e39dc0	369	const struct clk_pll_layout at91sam9g20_pllb_layout = {
1a748d2b BB	370	.pllr_mask = 0x3FFFFF,
	371	.mul_shift = 16,
	372	.mul_mask = 0x3F,
	373	};
	374
b2e39dc0	375	const struct clk_pll_layout sama5d3_pll_layout = {
1a748d2b BB	376	.pllr_mask = 0x1FFFFFF,
	377	.mul_shift = 18,
	378	.mul_mask = 0x7F,
	379	};