soc: mediatek: mtk-svs: restore default voltages when svs_init02() fail

[ Upstream commit 5e0d99bad2bc44d8a22d1846198bb3d4387c8c77 ]

If svs init02 fail, it means we cannot rely on svs bank voltages anymore.
We need to disable svs function and restore DVFS opp voltages back to the
default voltages for making sure we have enough DVFS voltages.

Fixes: 7ba1a4be26a6 ("soc: mediatek: SVS: introduce MTK SVS engine")
Fixes: 2ea21532c884 ("soc: mediatek: SVS: add mt8192 SVS GPU driver")
Signed-off-by: Roger Lu <roger.lu@mediatek.com>
Link: https://lore.kernel.org/r/20230111074528.29354-2-roger.lu@mediatek.com
Signed-off-by: Matthias Brugger <matthias.bgg@gmail.com>
Signed-off-by: Sasha Levin <sashal@kernel.org>

diff --git a/drivers/soc/mediatek/mtk-svs.c b/drivers/soc/mediatek/mtk-svs.c
index 75b2f534aa9d05f36e2c41c2518bc4eadb826999..9859e6cf6b8f97c29c2dce671022cdd8b10fb23f 100644 (file)
--- a/drivers/soc/mediatek/mtk-svs.c
+++ b/drivers/soc/mediatek/mtk-svs.c
@@ -1461,6 +1461,7 @@ static int svs_init02(struct svs_platform *svsp)
 {
        struct svs_bank *svsb;
        unsigned long flags, time_left;
+       int ret;
        u32 idx;
 
        for (idx = 0; idx < svsp->bank_max; idx++) {
@@ -1479,7 +1480,8 @@ static int svs_init02(struct svs_platform *svsp)
                                                        msecs_to_jiffies(5000));
                if (!time_left) {
                        dev_err(svsb->dev, "init02 completion timeout\n");
-                       return -EBUSY;
+                       ret = -EBUSY;
+                       goto out_of_init02;
                }
        }
 
@@ -1497,12 +1499,30 @@ static int svs_init02(struct svs_platform *svsp)
                if (svsb->type == SVSB_HIGH || svsb->type == SVSB_LOW) {
                        if (svs_sync_bank_volts_from_opp(svsb)) {
                                dev_err(svsb->dev, "sync volt fail\n");
-                               return -EPERM;
+                               ret = -EPERM;
+                               goto out_of_init02;
                        }
                }
        }
 
        return 0;
+
+out_of_init02:
+       for (idx = 0; idx < svsp->bank_max; idx++) {
+               svsb = &svsp->banks[idx];
+
+               spin_lock_irqsave(&svs_lock, flags);
+               svsp->pbank = svsb;
+               svs_switch_bank(svsp);
+               svs_writel_relaxed(svsp, SVSB_PTPEN_OFF, SVSEN);
+               svs_writel_relaxed(svsp, SVSB_INTSTS_VAL_CLEAN, INTSTS);
+               spin_unlock_irqrestore(&svs_lock, flags);
+
+               svsb->phase = SVSB_PHASE_ERROR;
+               svs_adjust_pm_opp_volts(svsb);
+       }
+
+       return ret;
 }
 
 static void svs_mon_mode(struct svs_platform *svsp)