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@xxxxxxxxxxxx> --- drivers/soc/mediatek/mtk-svs.c | 44 +++++++++++++++++----------------- 1 file changed, 22 insertions(+), 22 deletions(-) diff --git a/drivers/soc/mediatek/mtk-svs.c b/drivers/soc/mediatek/mtk-svs.c index 567536e32a46..f4382def9736 100644 --- a/drivers/soc/mediatek/mtk-svs.c +++ b/drivers/soc/mediatek/mtk-svs.c @@ -651,6 +651,26 @@ static int svs_adjust_pm_opp_volts(struct svs_bank *svsb) 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; + + /* This spinlock 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); +} + static int svs_dump_debug_show(struct seq_file *m, void *p) { struct svs_platform *svsp = (struct svs_platform *)m->private; @@ -726,7 +746,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; @@ -742,16 +761,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); @@ -1568,7 +1579,6 @@ 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; @@ -1580,17 +1590,7 @@ static int svs_suspend(struct device *dev) 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); -- 2.18.0