scsi: ufs: Clean up ufshcd_scale_clks() and clock scaling error out path

This change introduces a func ufshcd_set_clk_freq() to explicitly set clock
frequency so that it can be used in reset_and_restore path and in
ufshcd_scale_clks(). This change also cleans up the clock scaling error out
path.

[mkp: commit desc]

Link: https://lore.kernel.org/r/1585214742-5466-2-git-send-email-cang@codeaurora.org
Fixes: a3cd5ec55f6c ("scsi: ufs: add load based scaling of UFS gear")
Reviewed-by: Bean Huo <beanhuo@micron.com>
Acked-by: Avri Altman <Avri.Altman@wdc.com>
Signed-off-by: Subhash Jadavani <subhashj@codeaurora.org>
Signed-off-by: Can Guo <cang@codeaurora.org>
Signed-off-by: Martin K. Petersen <martin.petersen@oracle.com>

diff --git a/drivers/scsi/ufs/ufshcd.c b/drivers/scsi/ufs/ufshcd.c
index 5f20922101cf9dda33152d7094ef24590cc96761..79cbd35675cc41e586e06a4df72c7e3b5205a3cb 100644 (file)
--- a/drivers/scsi/ufs/ufshcd.c
+++ b/drivers/scsi/ufs/ufshcd.c
@@ -868,28 +868,29 @@ static bool ufshcd_is_unipro_pa_params_tuning_req(struct ufs_hba *hba)
                return false;
 }
 
-static int ufshcd_scale_clks(struct ufs_hba *hba, bool scale_up)
+/**
+ * ufshcd_set_clk_freq - set UFS controller clock frequencies
+ * @hba: per adapter instance
+ * @scale_up: If True, set max possible frequency othewise set low frequency
+ *
+ * Returns 0 if successful
+ * Returns < 0 for any other errors
+ */
+static int ufshcd_set_clk_freq(struct ufs_hba *hba, bool scale_up)
 {
        int ret = 0;
        struct ufs_clk_info *clki;
        struct list_head *head = &hba->clk_list_head;
-       ktime_t start = ktime_get();
-       bool clk_state_changed = false;
 
        if (list_empty(head))
                goto out;
 
-       ret = ufshcd_vops_clk_scale_notify(hba, scale_up, PRE_CHANGE);
-       if (ret)
-               return ret;
-
        list_for_each_entry(clki, head, list) {
                if (!IS_ERR_OR_NULL(clki->clk)) {
                        if (scale_up && clki->max_freq) {
                                if (clki->curr_freq == clki->max_freq)
                                        continue;
 
-                               clk_state_changed = true;
                                ret = clk_set_rate(clki->clk, clki->max_freq);
                                if (ret) {
                                        dev_err(hba->dev, "%s: %s clk set rate(%dHz) failed, %d\n",
@@ -908,7 +909,6 @@ static int ufshcd_scale_clks(struct ufs_hba *hba, bool scale_up)
                                if (clki->curr_freq == clki->min_freq)
                                        continue;
 
-                               clk_state_changed = true;
                                ret = clk_set_rate(clki->clk, clki->min_freq);
                                if (ret) {
                                        dev_err(hba->dev, "%s: %s clk set rate(%dHz) failed, %d\n",
@@ -927,11 +927,37 @@ static int ufshcd_scale_clks(struct ufs_hba *hba, bool scale_up)
                                clki->name, clk_get_rate(clki->clk));
        }
 
+out:
+       return ret;
+}
+
+/**
+ * ufshcd_scale_clks - scale up or scale down UFS controller clocks
+ * @hba: per adapter instance
+ * @scale_up: True if scaling up and false if scaling down
+ *
+ * Returns 0 if successful
+ * Returns < 0 for any other errors
+ */
+static int ufshcd_scale_clks(struct ufs_hba *hba, bool scale_up)
+{
+       int ret = 0;
+       ktime_t start = ktime_get();
+
+       ret = ufshcd_vops_clk_scale_notify(hba, scale_up, PRE_CHANGE);
+       if (ret)
+               goto out;
+
+       ret = ufshcd_set_clk_freq(hba, scale_up);
+       if (ret)
+               goto out;
+
        ret = ufshcd_vops_clk_scale_notify(hba, scale_up, POST_CHANGE);
+       if (ret)
+               ufshcd_set_clk_freq(hba, !scale_up);
 
 out:
-       if (clk_state_changed)
-               trace_ufshcd_profile_clk_scaling(dev_name(hba->dev),
+       trace_ufshcd_profile_clk_scaling(dev_name(hba->dev),
                        (scale_up ? "up" : "down"),
                        ktime_to_us(ktime_sub(ktime_get(), start)), ret);
        return ret;
@@ -1119,35 +1145,32 @@ static int ufshcd_devfreq_scale(struct ufs_hba *hba, bool scale_up)
 
        ret = ufshcd_clock_scaling_prepare(hba);
        if (ret)
-               return ret;
+               goto out;
 
        /* scale down the gear before scaling down clocks */
        if (!scale_up) {
                ret = ufshcd_scale_gear(hba, false);
                if (ret)
-                       goto out;
+                       goto out_unprepare;
        }
 
        ret = ufshcd_scale_clks(hba, scale_up);
        if (ret) {
                if (!scale_up)
                        ufshcd_scale_gear(hba, true);
-               goto out;
+               goto out_unprepare;
        }
 
        /* scale up the gear after scaling up clocks */
        if (scale_up) {
                ret = ufshcd_scale_gear(hba, true);
-               if (ret) {
+               if (ret)
                        ufshcd_scale_clks(hba, false);
-                       goto out;
-               }
        }
 
-       ret = ufshcd_vops_clk_scale_notify(hba, scale_up, POST_CHANGE);
-
-out:
+out_unprepare:
        ufshcd_clock_scaling_unprepare(hba);
+out:
        ufshcd_release(hba);
        return ret;
 }
@@ -6292,7 +6315,7 @@ static int ufshcd_host_reset_and_restore(struct ufs_hba *hba)
        spin_unlock_irqrestore(hba->host->host_lock, flags);
 
        /* scale up clocks to max frequency before full reinitialization */
-       ufshcd_scale_clks(hba, true);
+       ufshcd_set_clk_freq(hba, true);
 
        err = ufshcd_hba_enable(hba);
        if (err)