[PATCH v17 07/12] soc: mediatek: Add subsys clock control for bus protection

[Weiyi Lu <weiyi.lu@xxxxxxxxxxxx>]

For the bus protection operations, some subsys clocks need to be enabled
before releasing the protection, and vice versa.
But those subsys clocks could only be controlled once its corresponding
power domain is turned on first.
In this patch, we add the subsys clock control into its relevant steps.

Signed-off-by: Weiyi Lu <weiyi.lu@xxxxxxxxxxxx>
---
 drivers/soc/mediatek/mtk-scpsys.c | 39 +++++++++++++++++++++++++++++++++++++--
 1 file changed, 37 insertions(+), 2 deletions(-)

diff --git a/drivers/soc/mediatek/mtk-scpsys.c b/drivers/soc/mediatek/mtk-scpsys.c
index 502b66f..ec62143 100644
--- a/drivers/soc/mediatek/mtk-scpsys.c
+++ b/drivers/soc/mediatek/mtk-scpsys.c
@@ -7,6 +7,7 @@
 #include <linux/io.h>
 #include <linux/iopoll.h>
 #include <linux/mfd/syscon.h>
+#include <linux/of_clk.h>
 #include <linux/of_device.h>
 #include <linux/platform_device.h>
 #include <linux/pm_domain.h>
@@ -97,6 +98,7 @@
 #define MT8173_TOP_AXI_PROT_EN_MFG_SNOOP_OUT	BIT(23)
 
 #define MAX_CLKS	3
+#define MAX_SUBSYS_CLKS 10
 
 #define MAX_STEPS	5
 
@@ -165,6 +167,7 @@ struct scp_domain {
 	struct generic_pm_domain genpd;
 	struct scp *scp;
 	struct clk *clk[MAX_CLKS];
+	struct clk *subsys_clk[MAX_SUBSYS_CLKS];
 	const struct scp_domain_data *data;
 	struct regulator *supply;
 };
@@ -427,16 +430,22 @@ static int scpsys_power_on(struct generic_pm_domain *genpd)
 	val |= PWR_RST_B_BIT;
 	writel(val, ctl_addr);
 
-	ret = scpsys_sram_enable(scpd, ctl_addr);
+	ret = scpsys_clk_enable(scpd->subsys_clk, MAX_SUBSYS_CLKS);
 	if (ret < 0)
 		goto err_pwr_ack;
 
+	ret = scpsys_sram_enable(scpd, ctl_addr);
+	if (ret < 0)
+		goto err_sram;
+
 	ret = scpsys_bus_protect_disable(scpd);
 	if (ret < 0)
-		goto err_pwr_ack;
+		goto err_sram;
 
 	return 0;
 
+err_sram:
+	scpsys_clk_disable(scpd->subsys_clk, MAX_SUBSYS_CLKS);
 err_pwr_ack:
 	scpsys_clk_disable(scpd->clk, MAX_CLKS);
 err_clk:
@@ -463,6 +472,8 @@ static int scpsys_power_off(struct generic_pm_domain *genpd)
 	if (ret < 0)
 		goto out;
 
+	scpsys_clk_disable(scpd->subsys_clk, MAX_SUBSYS_CLKS);
+
 	/* subsys power off */
 	val = readl(ctl_addr);
 	val |= PWR_ISO_BIT;
@@ -500,6 +511,24 @@ static int scpsys_power_off(struct generic_pm_domain *genpd)
 	return ret;
 }
 
+static int init_subsys_clks(struct device_node *np,
+		struct clk **clk)
+{
+	int sub_clk_cnt = of_clk_get_parent_count(np);
+	int i;
+
+	BUG_ON(sub_clk_cnt > MAX_SUBSYS_CLKS);
+
+	for (i = 0; i < sub_clk_cnt; i++) {
+		clk[i] = of_clk_get(np, i);
+
+		if (IS_ERR(clk[i]))
+			return PTR_ERR(clk[i]);
+	}
+
+	return 0;
+}
+
 static int init_basic_clks(struct platform_device *pdev, struct clk **clk,
 			const char * const *name)
 {
@@ -533,11 +562,17 @@ static int scpsys_get_domain(struct platform_device *pdev, struct scp *scp,
 	struct device_node *sub;
 	u32 parent_id, child_id;
 	int ret;
+	struct scp_domain *scpd;
 
 	ret = scpsys_get_domain_id(node, &parent_id);
 	if (ret)
 		return ret;
 
+	scpd = &scp->domains[parent_id];
+	ret = init_subsys_clks(node, scpd->subsys_clk);
+	if (ret)
+		return ret;
+
 	for_each_child_of_node(node, sub) {
 		ret = scpsys_get_domain_id(sub, &child_id);
 		if (ret)
-- 
1.8.1.1.dirty