clk: at91: pmc: export clock setup to pmc

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@microchip.com>

3 files changed, 64 insertions, 42 deletions

diff --git a/drivers/clk/at91/pmc.c b/drivers/clk/at91/pmc.c
index 2708925..87d2069 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 17793b8..ff46452 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 d1ec3c8..8bd9c14 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;