[PATCH v2 3/5] clk: at91: pmc: export clock setup to pmc

[Claudiu Beznea]

Clock setup was intended for setting clocks at boot time on SAMA7G5,
e.g. for root clocks like PLLs, that were used to feed IPs needed alive
in u-boot (e.g. Ethernet clock feed by a PLL). Export this functionality
to all at91 clocks as it may be necessary on other SoCs.

Signed-off-by: Claudiu Beznea <claudiu.beznea at microchip.com>
---
 drivers/clk/at91/pmc.c     | 42 +++++++++++++++++++++++++++++++++
 drivers/clk/at91/pmc.h     | 16 +++++++++++++
 drivers/clk/at91/sama7g5.c | 48 +++++---------------------------------
 3 files changed, 64 insertions(+), 42 deletions(-)

diff --git a/drivers/clk/at91/pmc.c b/drivers/clk/at91/pmc.c
index 270892517a90..87d2069d89ce 100644
--- a/drivers/clk/at91/pmc.c
+++ b/drivers/clk/at91/pmc.c
@@ -120,3 +120,45 @@ int at91_clk_mux_index_to_val(const u32 *table, u32 num_parents, u32 index)
 
 	return table[index];
 }
+
+int at91_clk_setup(const struct pmc_clk_setup *setup, int size)
+{
+	struct clk *c, *parent;
+	int i, ret;
+
+	if (!size)
+		return 0;
+
+	if (!setup)
+		return -EINVAL;
+
+	for (i = 0; i < size; i++) {
+		ret = clk_get_by_id(setup[i].cid, &c);
+		if (ret)
+			return ret;
+
+		if (setup[i].pid) {
+			ret = clk_get_by_id(setup[i].pid, &parent);
+			if (ret)
+				return ret;
+
+			ret = clk_set_parent(c, parent);
+			if (ret)
+				return ret;
+
+			if (setup[i].prate) {
+				ret = clk_set_rate(parent, setup[i].prate);
+				if (ret < 0)
+					return ret;
+			}
+		}
+
+		if (setup[i].rate) {
+			ret = clk_set_rate(c, setup[i].rate);
+			if (ret < 0)
+				return ret;
+		}
+	}
+
+	return 0;
+}
diff --git a/drivers/clk/at91/pmc.h b/drivers/clk/at91/pmc.h
index 17793b8802ad..ff464522aa09 100644
--- a/drivers/clk/at91/pmc.h
+++ b/drivers/clk/at91/pmc.h
@@ -77,6 +77,20 @@ struct clk_usbck_layout {
 	u32 usbdiv_mask;
 };
 
+/**
+ * Clock setup description
+ * @cid:	clock id corresponding to clock subsystem
+ * @pid:	parent clock id corresponding to clock subsystem
+ * @rate:	clock rate
+ * @prate:	parent rate
+ */
+struct pmc_clk_setup {
+	unsigned int cid;
+	unsigned int pid;
+	unsigned long rate;
+	unsigned long prate;
+};
+
 extern const struct clk_programmable_layout at91rm9200_programmable_layout;
 extern const struct clk_programmable_layout at91sam9g45_programmable_layout;
 extern const struct clk_programmable_layout at91sam9x5_programmable_layout;
@@ -160,4 +174,6 @@ void pmc_write(void __iomem *base, unsigned int off, unsigned int val);
 void pmc_update_bits(void __iomem *base, unsigned int off, unsigned int mask,
 			unsigned int bits);
 
+int at91_clk_setup(const struct pmc_clk_setup *setup, int size);
+
 #endif
diff --git a/drivers/clk/at91/sama7g5.c b/drivers/clk/at91/sama7g5.c
index d1ec3c82b547..8bd9c141566d 100644
--- a/drivers/clk/at91/sama7g5.c
+++ b/drivers/clk/at91/sama7g5.c
@@ -1070,19 +1070,8 @@ static const struct {
 	},
 };
 
-/**
- * Clock setup description
- * @cid:	clock id corresponding to clock subsystem
- * @pid:	parent clock id corresponding to clock subsystem
- * @rate:	clock rate
- * @prate:	parent rate
- */
-static const struct pmc_clk_setup {
-	unsigned int cid;
-	unsigned int pid;
-	unsigned long rate;
-	unsigned long prate;
-} sama7g5_clk_setup[] = {
+/* Clock setup description */
+static const struct pmc_clk_setup sama7g5_clk_setup[] = {
 	{
 		.cid = AT91_TO_CLK_ID(PMC_TYPE_CORE, ID_PLL_ETH_FRAC),
 		.rate = 625000000,
@@ -1119,7 +1108,7 @@ static int sama7g5_clk_probe(struct udevice *dev)
 	unsigned int *muxallocs[SAMA7G5_MAX_MUX_ALLOCS];
 	const char *p[10];
 	unsigned int cm[10], m[10], *tmpclkmux, *tmpmux;
-	struct clk clk, *c, *parent;
+	struct clk clk, *c;
 	bool main_osc_bypass;
 	int ret, muxallocindex = 0, clkmuxallocindex = 0, i, j;
 
@@ -1353,34 +1342,9 @@ static int sama7g5_clk_probe(struct udevice *dev)
 	}
 
 	/* Setup clocks. */
-	for (i = 0; i < ARRAY_SIZE(sama7g5_clk_setup); i++) {
-		ret = clk_get_by_id(sama7g5_clk_setup[i].cid, &c);
-		if (ret)
-			goto fail;
-
-		if (sama7g5_clk_setup[i].pid) {
-			ret = clk_get_by_id(sama7g5_clk_setup[i].pid, &parent);
-			if (ret)
-				goto fail;
-
-			ret = clk_set_parent(c, parent);
-			if (ret)
-				goto fail;
-
-			if (sama7g5_clk_setup[i].prate) {
-				ret = clk_set_rate(parent,
-					sama7g5_clk_setup[i].prate);
-				if (ret < 0)
-					goto fail;
-			}
-		}
-
-		if (sama7g5_clk_setup[i].rate) {
-			ret = clk_set_rate(c, sama7g5_clk_setup[i].rate);
-			if (ret < 0)
-				goto fail;
-		}
-	}
+	ret = at91_clk_setup(sama7g5_clk_setup, ARRAY_SIZE(sama7g5_clk_setup));
+	if (ret)
+		goto fail;
 
 	return 0;
 
-- 
2.34.1