soc: mediatek: mtk-svs: use common function to disable restore voltages

The timing of disabling SVS bank and restore default voltage is more
than one place. Therefore, add a common function to use for removing
the superfluous codes.

Signed-off-by: Roger Lu <roger.lu@mediatek.com>
Reviewed-by: AngeloGioacchino Del Regno <angelogioacchino.delregno@collabora.com>
Link: https://lore.kernel.org/r/20230202124104.16504-3-roger.lu@mediatek.com
Signed-off-by: Matthias Brugger <matthias.bgg@gmail.com>

diff --git a/drivers/soc/mediatek/mtk-svs.c b/drivers/soc/mediatek/mtk-svs.c
index c9899f5..299f580 100644 (file)
--- a/drivers/soc/mediatek/mtk-svs.c
+++ b/drivers/soc/mediatek/mtk-svs.c
@@ -624,6 +624,25 @@ unlock_mutex:
        return ret;
 }
 
+static void svs_bank_disable_and_restore_default_volts(struct svs_platform *svsp,
+                                                      struct svs_bank *svsb)
+{
+       unsigned long flags;
+
+       if (svsb->mode_support == SVSB_MODE_ALL_DISABLE)
+               return;
+
+       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);
+}
+
 #ifdef CONFIG_DEBUG_FS
 static int svs_dump_debug_show(struct seq_file *m, void *p)
 {
@@ -700,7 +719,6 @@ static ssize_t svs_enable_debug_write(struct file *filp,
 {
        struct svs_bank *svsb = file_inode(filp)->i_private;
        struct svs_platform *svsp = dev_get_drvdata(svsb->dev);
-       unsigned long flags;
        int enabled, ret;
        char *buf = NULL;
 
@@ -716,16 +734,8 @@ static ssize_t svs_enable_debug_write(struct file *filp,
                return ret;
 
        if (!enabled) {
-               spin_lock_irqsave(&svs_lock, flags);
-               svsp->pbank = svsb;
+               svs_bank_disable_and_restore_default_volts(svsp, svsb);
                svsb->mode_support = SVSB_MODE_ALL_DISABLE;
-               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);
        }
 
        kfree(buf);
@@ -1508,16 +1518,7 @@ static int svs_init02(struct svs_platform *svsp)
 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);
+               svs_bank_disable_and_restore_default_volts(svsp, svsb);
        }
 
        return ret;
@@ -1563,23 +1564,12 @@ static int svs_suspend(struct device *dev)
 {
        struct svs_platform *svsp = dev_get_drvdata(dev);
        struct svs_bank *svsb;
-       unsigned long flags;
        int ret;
        u32 idx;
 
        for (idx = 0; idx < svsp->bank_max; idx++) {
                svsb = &svsp->banks[idx];
-
-               /* This might wait for svs_isr() process */
-               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);
+               svs_bank_disable_and_restore_default_volts(svsp, svsb);
        }
 
        ret = reset_control_assert(svsp->rst);