drivers: sam: add at91_pll clock driver

Add driver for PLL.

Acked-by: Etienne Carriere <etienne.carriere@linaro.org>
Acked-by: Nicolas Ferre <nicolas.ferre@microchip.com>
Acked-by: Boris Brezillon <bbrezillon@kernel.org>
Signed-off-by: Clément Léger <clement.leger@bootlin.com>

core/drivers/clk/sam/at91_pll.c

@@ -0,0 +1,306 @@
+// SPDX-License-Identifier: GPL-2.0+ or BSD-3-Clause
+/*
+ *  Copyright (C) 2013 Boris BREZILLON <b.brezillon@overkiz.com>
+ *  Copyright (C) 2021 Microchip
+ */
+
+#include <io.h>
+#include <kernel/delay.h>
+#include <kernel/panic.h>
+#include <util.h>
+#include <mm/core_memprot.h>
+#include <types_ext.h>
+
+#include "at91_clk.h"
+
+#define PLL_STATUS_MASK(id)	BIT(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) \
+	({ \
+		typeof(layout) __layout = 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
+
+struct clk_pll {
+	vaddr_t base;
+	uint8_t id;
+	uint8_t div;
+	uint8_t range;
+	uint16_t mul;
+	const struct clk_pll_layout *layout;
+	const struct clk_pll_charac *charac;
+};
+
+static bool clk_pll_ready(vaddr_t base, int id)
+{
+	unsigned int status = io_read32(base + AT91_PMC_SR);
+
+	return status & PLL_STATUS_MASK(id);
+}
+
+static TEE_Result clk_pll_enable(struct clk *clk)
+{
+	struct clk_pll *pll = clk->priv;
+	const struct clk_pll_layout *layout = pll->layout;
+	const struct clk_pll_charac *charac = pll->charac;
+	uint8_t id = pll->id;
+	uint32_t mask = PLL_STATUS_MASK(id);
+	int offset = PLL_REG(id);
+	uint8_t out = 0;
+	unsigned int pllr = 0;
+	unsigned int status = 0;
+	uint8_t div = 0;
+	uint16_t mul = 0;
+
+	pllr = io_read32(pll->base + offset);
+	div = PLL_DIV(pllr);
+	mul = PLL_MUL(pllr, layout);
+
+	status = io_read32(pll->base + AT91_PMC_SR);
+	if ((status & mask) &&
+	    (div == pll->div && mul == pll->mul))
+		return TEE_SUCCESS;
+
+	if (charac->out)
+		out = charac->out[pll->range];
+
+	if (charac->icpll)
+		io_clrsetbits32(pll->base + AT91_PMC_PLLICPR, PLL_ICPR_MASK(id),
+				charac->icpll[pll->range] <<
+				PLL_ICPR_SHIFT(id));
+
+	io_clrsetbits32(pll->base + 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(pll->base, pll->id))
+		;
+
+	return TEE_SUCCESS;
+}
+
+static void clk_pll_disable(struct clk *clk)
+{
+	struct clk_pll *pll = clk->priv;
+	unsigned int mask = pll->layout->pllr_mask;
+
+	io_clrsetbits32(pll->base + PLL_REG(pll->id), mask, ~mask);
+}
+
+static unsigned long clk_pll_get_rate(struct clk *clk,
+				      unsigned long parent_rate)
+{
+	struct clk_pll *pll = clk->priv;
+
+	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,
+				     uint32_t *div, uint32_t *mul,
+				     uint32_t *index)
+{
+	const struct clk_pll_layout *layout = pll->layout;
+	const struct clk_pll_charac *charac = pll->charac;
+	unsigned long bestremainder = ULONG_MAX;
+	unsigned long maxdiv = 1;
+	unsigned long mindiv = 1;
+	unsigned long tmpdiv = 1;
+	long bestrate = -1;
+	unsigned long bestdiv = 0;
+	unsigned long bestmul = 0;
+	int i = 0;
+
+	/* Check if parent_rate is a valid input rate */
+	if (parent_rate < charac->input.min)
+		return -1;
+
+	/*
+	 * 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 charac
+	 * of the PLL blocks.
+	 */
+	mindiv = (parent_rate * PLL_MUL_MIN) / rate;
+	if (!mindiv)
+		mindiv = 1;
+
+	if (parent_rate > charac->input.max) {
+		tmpdiv = DIV_ROUND_UP(parent_rate, charac->input.max);
+		if (tmpdiv > PLL_DIV_MAX)
+			return -1;
+
+		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 = 0;
+		unsigned long tmprate = 0;
+		unsigned long tmpmul = 0;
+
+		/*
+		 * Calculate the multiplier associated with the current
+		 * divider that provide the closest rate to the requested one.
+		 */
+		tmpmul = UDIV_ROUND_NEAREST(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 < charac->num_output; i++) {
+		if (bestrate >= (long)charac->output[i].min &&
+		    bestrate <= (long)charac->output[i].max)
+			break;
+	}
+
+	if (i >= charac->num_output)
+		return -1;
+
+	if (div)
+		*div = bestdiv;
+	if (mul)
+		*mul = bestmul - 1;
+	if (index)
+		*index = i;
+
+	return bestrate;
+}
+
+static TEE_Result clk_pll_set_rate(struct clk *clk, unsigned long rate,
+				   unsigned long parent_rate)
+{
+	struct clk_pll *pll = clk->priv;
+	long ret = -1;
+	uint32_t div = 1;
+	uint32_t mul = 0;
+	uint32_t index = 0;
+
+	ret = clk_pll_get_best_div_mul(pll, rate, parent_rate,
+				       &div, &mul, &index);
+	if (ret < 0)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	pll->range = index;
+	pll->div = div;
+	pll->mul = mul;
+
+	return TEE_SUCCESS;
+}
+
+static const struct clk_ops pll_ops = {
+	.enable = clk_pll_enable,
+	.disable = clk_pll_disable,
+	.get_rate = clk_pll_get_rate,
+	.set_rate = clk_pll_set_rate,
+};
+
+struct clk *
+at91_clk_register_pll(struct pmc_data *pmc, const char *name,
+		      struct clk *parent, uint8_t id,
+		      const struct clk_pll_layout *layout,
+		      const struct clk_pll_charac *charac)
+{
+	struct clk *clk = NULL;
+	struct clk_pll *pll = NULL;
+	int offset = PLL_REG(id);
+	uint32_t pllr = 0;
+
+	if (!name || !parent)
+		return NULL;
+
+	clk = clk_alloc(name, &pll_ops, &parent, 1);
+	if (!clk)
+		return NULL;
+
+	if (id > PLL_MAX_ID)
+		return NULL;
+
+	pll = calloc(1, sizeof(*pll));
+	if (!pll) {
+		clk_free(clk);
+		return NULL;
+	}
+
+	pll->id = id;
+	pll->layout = layout;
+	pll->charac = charac;
+	pll->base = pmc->base;
+	pllr = io_read32(pmc->base + offset);
+	pll->div = PLL_DIV(pllr);
+	pll->mul = PLL_MUL(pllr, layout);
+
+	clk->flags = CLK_SET_RATE_GATE;
+	clk->priv = pll;
+
+	if (clk_register(clk)) {
+		clk_free(clk);
+		free(pll);
+		return NULL;
+	}
+
+	return clk;
+}
+
+const struct clk_pll_layout sama5d3_pll_layout = {
+	.pllr_mask = 0x1FFFFFF,
+	.mul_shift = 18,
+	.mul_mask = 0x7F,
+};