2 files changed, 63 insertions, 38 deletions

diff --git a/drivers/soc/mediatek/mtk-pmic-wrap.c b/drivers/soc/mediatek/mtk-pmic-wrap.c
index 105597a885cb..0d9b19a78d27 100644
--- a/drivers/soc/mediatek/mtk-pmic-wrap.c
+++ b/drivers/soc/mediatek/mtk-pmic-wrap.c
@@ -60,6 +60,15 @@
 #define PWRAP_MAN_CMD_OP_OUTD		(0x9 << 8)
 #define PWRAP_MAN_CMD_OP_OUTQ		(0xa << 8)
 
+/* macro for Watch Dog Timer Source */
+#define PWRAP_WDT_SRC_EN_STAUPD_TRIG		(1 << 25)
+#define PWRAP_WDT_SRC_EN_HARB_STAUPD_DLE	(1 << 20)
+#define PWRAP_WDT_SRC_EN_HARB_STAUPD_ALE	(1 << 6)
+#define PWRAP_WDT_SRC_MASK_ALL			0xffffffff
+#define PWRAP_WDT_SRC_MASK_NO_STAUPD	~(PWRAP_WDT_SRC_EN_STAUPD_TRIG | \
+					  PWRAP_WDT_SRC_EN_HARB_STAUPD_DLE | \
+					  PWRAP_WDT_SRC_EN_HARB_STAUPD_ALE)
+
 /* macro for slave device wrapper registers */
 #define PWRAP_DEW_BASE			0xbc00
 #define PWRAP_DEW_EVENT_OUT_EN		(PWRAP_DEW_BASE + 0x0)
@@ -412,6 +421,20 @@ static bool pwrap_is_fsm_vldclr(struct pmic_wrapper *wrp)
 	return PWRAP_GET_WACS_FSM(val) == PWRAP_WACS_FSM_WFVLDCLR;
 }
 
+/*
+ * Timeout issue sometimes caused by the last read command
+ * failed because pmic wrap could not got the FSM_VLDCLR
+ * in time after finishing WACS2_CMD. It made state machine
+ * still on FSM_VLDCLR and timeout next time.
+ * Check the status of FSM and clear the vldclr to recovery the
+ * error.
+ */
+static inline void pwrap_leave_fsm_vldclr(struct pmic_wrapper *wrp)
+{
+	if (pwrap_is_fsm_vldclr(wrp))
+		pwrap_writel(wrp, 1, PWRAP_WACS2_VLDCLR);
+}
+
 static bool pwrap_is_sync_idle(struct pmic_wrapper *wrp)
 {
 	return pwrap_readl(wrp, PWRAP_WACS2_RDATA) & PWRAP_STATE_SYNC_IDLE0;
@@ -445,8 +468,10 @@ static int pwrap_write(struct pmic_wrapper *wrp, u32 adr, u32 wdata)
 	int ret;
 
 	ret = pwrap_wait_for_state(wrp, pwrap_is_fsm_idle);
-	if (ret)
+	if (ret) {
+		pwrap_leave_fsm_vldclr(wrp);
 		return ret;
+	}
 
 	pwrap_writel(wrp, (1 << 31) | ((adr >> 1) << 16) | wdata,
 			PWRAP_WACS2_CMD);
@@ -459,8 +484,10 @@ static int pwrap_read(struct pmic_wrapper *wrp, u32 adr, u32 *rdata)
 	int ret;
 
 	ret = pwrap_wait_for_state(wrp, pwrap_is_fsm_idle);
-	if (ret)
+	if (ret) {
+		pwrap_leave_fsm_vldclr(wrp);
 		return ret;
+	}
 
 	pwrap_writel(wrp, (adr >> 1) << 16, PWRAP_WACS2_CMD);
 
@@ -804,7 +831,7 @@ MODULE_DEVICE_TABLE(of, of_pwrap_match_tbl);
 
 static int pwrap_probe(struct platform_device *pdev)
 {
-	int ret, irq;
+	int ret, irq, wdt_src;
 	struct pmic_wrapper *wrp;
 	struct device_node *np = pdev->dev.of_node;
 	const struct of_device_id *of_id =
@@ -894,7 +921,13 @@ static int pwrap_probe(struct platform_device *pdev)
 
 	/* Initialize watchdog, may not be done by the bootloader */
 	pwrap_writel(wrp, 0xf, PWRAP_WDT_UNIT);
-	pwrap_writel(wrp, 0xffffffff, PWRAP_WDT_SRC_EN);
+	/*
+	 * Since STAUPD was not used on mt8173 platform,
+	 * so STAUPD of WDT_SRC which should be turned off
+	 */
+	wdt_src = pwrap_is_mt8173(wrp) ?
+			PWRAP_WDT_SRC_MASK_NO_STAUPD : PWRAP_WDT_SRC_MASK_ALL;
+	pwrap_writel(wrp, wdt_src, PWRAP_WDT_SRC_EN);
 	pwrap_writel(wrp, 0x1, PWRAP_TIMER_EN);
 	pwrap_writel(wrp, ~((1 << 31) | (1 << 1)), PWRAP_INT_EN);
 
diff --git a/drivers/soc/mediatek/mtk-scpsys.c b/drivers/soc/mediatek/mtk-scpsys.c
index 0221387e5e27..57e781c71e67 100644
--- a/drivers/soc/mediatek/mtk-scpsys.c
+++ b/drivers/soc/mediatek/mtk-scpsys.c
@@ -76,7 +76,7 @@ struct scp_domain_data {
 	bool active_wakeup;
 };
 
-static const struct scp_domain_data scp_domain_data[] __initconst = {
+static const struct scp_domain_data scp_domain_data[] = {
 	[MT8173_POWER_DOMAIN_VDEC] = {
 		.name = "vdec",
 		.sta_mask = PWR_STATUS_VDEC,
@@ -174,12 +174,7 @@ struct scp_domain {
 	struct generic_pm_domain genpd;
 	struct scp *scp;
 	struct clk *clk[MAX_CLKS];
-	u32 sta_mask;
-	void __iomem *ctl_addr;
-	u32 sram_pdn_bits;
-	u32 sram_pdn_ack_bits;
-	u32 bus_prot_mask;
-	bool active_wakeup;
+	const struct scp_domain_data *data;
 	struct regulator *supply;
 };
 
@@ -195,8 +190,9 @@ static int scpsys_domain_is_on(struct scp_domain *scpd)
 {
 	struct scp *scp = scpd->scp;
 
-	u32 status = readl(scp->base + SPM_PWR_STATUS) & scpd->sta_mask;
-	u32 status2 = readl(scp->base + SPM_PWR_STATUS_2ND) & scpd->sta_mask;
+	u32 status = readl(scp->base + SPM_PWR_STATUS) & scpd->data->sta_mask;
+	u32 status2 = readl(scp->base + SPM_PWR_STATUS_2ND) &
+				scpd->data->sta_mask;
 
 	/*
 	 * A domain is on when both status bits are set. If only one is set
@@ -217,8 +213,8 @@ static int scpsys_power_on(struct generic_pm_domain *genpd)
 	struct scp *scp = scpd->scp;
 	unsigned long timeout;
 	bool expired;
-	void __iomem *ctl_addr = scpd->ctl_addr;
-	u32 sram_pdn_ack = scpd->sram_pdn_ack_bits;
+	void __iomem *ctl_addr = scp->base + scpd->data->ctl_offs;
+	u32 sram_pdn_ack = scpd->data->sram_pdn_ack_bits;
 	u32 val;
 	int ret;
 	int i;
@@ -273,7 +269,7 @@ static int scpsys_power_on(struct generic_pm_domain *genpd)
 	val |= PWR_RST_B_BIT;
 	writel(val, ctl_addr);
 
-	val &= ~scpd->sram_pdn_bits;
+	val &= ~scpd->data->sram_pdn_bits;
 	writel(val, ctl_addr);
 
 	/* wait until SRAM_PDN_ACK all 0 */
@@ -292,9 +288,9 @@ static int scpsys_power_on(struct generic_pm_domain *genpd)
 			expired = true;
 	}
 
-	if (scpd->bus_prot_mask) {
+	if (scpd->data->bus_prot_mask) {
 		ret = mtk_infracfg_clear_bus_protection(scp->infracfg,
-				scpd->bus_prot_mask);
+				scpd->data->bus_prot_mask);
 		if (ret)
 			goto err_pwr_ack;
 	}
@@ -321,21 +317,21 @@ static int scpsys_power_off(struct generic_pm_domain *genpd)
 	struct scp *scp = scpd->scp;
 	unsigned long timeout;
 	bool expired;
-	void __iomem *ctl_addr = scpd->ctl_addr;
-	u32 pdn_ack = scpd->sram_pdn_ack_bits;
+	void __iomem *ctl_addr = scp->base + scpd->data->ctl_offs;
+	u32 pdn_ack = scpd->data->sram_pdn_ack_bits;
 	u32 val;
 	int ret;
 	int i;
 
-	if (scpd->bus_prot_mask) {
+	if (scpd->data->bus_prot_mask) {
 		ret = mtk_infracfg_set_bus_protection(scp->infracfg,
-				scpd->bus_prot_mask);
+				scpd->data->bus_prot_mask);
 		if (ret)
 			goto out;
 	}
 
 	val = readl(ctl_addr);
-	val |= scpd->sram_pdn_bits;
+	val |= scpd->data->sram_pdn_bits;
 	writel(val, ctl_addr);
 
 	/* wait until SRAM_PDN_ACK all 1 */
@@ -409,10 +405,10 @@ static bool scpsys_active_wakeup(struct device *dev)
 	genpd = pd_to_genpd(dev->pm_domain);
 	scpd = container_of(genpd, struct scp_domain, genpd);
 
-	return scpd->active_wakeup;
+	return scpd->data->active_wakeup;
 }
 
-static int __init scpsys_probe(struct platform_device *pdev)
+static int scpsys_probe(struct platform_device *pdev)
 {
 	struct genpd_onecell_data *pd_data;
 	struct resource *res;
@@ -485,12 +481,7 @@ static int __init scpsys_probe(struct platform_device *pdev)
 		pd_data->domains[i] = genpd;
 		scpd->scp = scp;
 
-		scpd->sta_mask = data->sta_mask;
-		scpd->ctl_addr = scp->base + data->ctl_offs;
-		scpd->sram_pdn_bits = data->sram_pdn_bits;
-		scpd->sram_pdn_ack_bits = data->sram_pdn_ack_bits;
-		scpd->bus_prot_mask = data->bus_prot_mask;
-		scpd->active_wakeup = data->active_wakeup;
+		scpd->data = data;
 		for (j = 0; j < MAX_CLKS && data->clk_id[j]; j++)
 			scpd->clk[j] = clk[data->clk_id[j]];
 
@@ -500,14 +491,13 @@ static int __init scpsys_probe(struct platform_device *pdev)
 		genpd->dev_ops.active_wakeup = scpsys_active_wakeup;
 
 		/*
-		 * Initially turn on all domains to make the domains usable
-		 * with !CONFIG_PM and to get the hardware in sync with the
-		 * software.  The unused domains will be switched off during
-		 * late_init time.
+		 * With CONFIG_PM disabled turn on all domains to make the
+		 * hardware usable.
 		 */
-		genpd->power_on(genpd);
+		if (!IS_ENABLED(CONFIG_PM))
+			genpd->power_on(genpd);
 
-		pm_genpd_init(genpd, NULL, false);
+		pm_genpd_init(genpd, NULL, true);
 	}
 
 	/*
@@ -542,10 +532,12 @@ static const struct of_device_id of_scpsys_match_tbl[] = {
 };
 
 static struct platform_driver scpsys_drv = {
+	.probe = scpsys_probe,
 	.driver = {
 		.name = "mtk-scpsys",
+		.suppress_bind_attrs = true,
 		.owner = THIS_MODULE,
 		.of_match_table = of_match_ptr(of_scpsys_match_tbl),
 	},
 };
-builtin_platform_driver_probe(scpsys_drv, scpsys_probe);
+builtin_platform_driver(scpsys_drv);