scsi: ufs: qcom: Map devfreq OPP freq to UniPro Core Clock freq

On some platforms, the devfreq OPP freq may be different than the unipro
core clock freq. Implement ufs_qcom_opp_freq_to_clk_freq() and use it to
find the unipro core clk freq.

Fixes: c02fe9e222d1 ("scsi: ufs: qcom: Implement the freq_to_gear_speed() vop")
Signed-off-by: Can Guo <quic_cang@quicinc.com>
Co-developed-by: Ziqi Chen <quic_ziqichen@quicinc.com>
Signed-off-by: Ziqi Chen <quic_ziqichen@quicinc.com>
Link: https://lore.kernel.org/r/20250522021537.999107-3-quic_ziqichen@quicinc.com
Reported-by: Luca Weiss <luca.weiss@fairphone.com>
Closes: https://lore.kernel.org/linux-arm-msm/D9FZ9U3AEXW4.1I12FX3YQ3JPW@fairphone.com/
Tested-by: Luca Weiss <luca.weiss@fairphone.com>
Reviewed-by: Bean Huo <beanhuo@micron.com>
Tested-by: Loïc Minier <loic.minier@oss.qualcomm.com>
Signed-off-by: Martin K. Petersen <martin.petersen@oracle.com>

diff --git a/drivers/ufs/host/ufs-qcom.c b/drivers/ufs/host/ufs-qcom.c
index 654d970b6dec6826051ddaa8a9395673c54db91e..6c054727c3bcf39af0c3735de9c3a9e08e944bdb 100644 (file)
--- a/drivers/ufs/host/ufs-qcom.c
+++ b/drivers/ufs/host/ufs-qcom.c
@@ -122,7 +122,9 @@ static const struct {
 };
 
 static void ufs_qcom_get_default_testbus_cfg(struct ufs_qcom_host *host);
-static int ufs_qcom_set_core_clk_ctrl(struct ufs_hba *hba, unsigned long freq);
+static unsigned long ufs_qcom_opp_freq_to_clk_freq(struct ufs_hba *hba,
+                                                  unsigned long freq, char *name);
+static int ufs_qcom_set_core_clk_ctrl(struct ufs_hba *hba, bool is_scale_up, unsigned long freq);
 
 static struct ufs_qcom_host *rcdev_to_ufs_host(struct reset_controller_dev *rcd)
 {
@@ -656,7 +658,7 @@ static int ufs_qcom_link_startup_notify(struct ufs_hba *hba,
                        return -EINVAL;
                }
 
-               err = ufs_qcom_set_core_clk_ctrl(hba, ULONG_MAX);
+               err = ufs_qcom_set_core_clk_ctrl(hba, true, ULONG_MAX);
                if (err)
                        dev_err(hba->dev, "cfg core clk ctrl failed\n");
                /*
@@ -1414,29 +1416,46 @@ static int ufs_qcom_set_clk_40ns_cycles(struct ufs_hba *hba,
        return ufshcd_dme_set(hba, UIC_ARG_MIB(PA_VS_CORE_CLK_40NS_CYCLES), reg);
 }
 
-static int ufs_qcom_set_core_clk_ctrl(struct ufs_hba *hba, unsigned long freq)
+static int ufs_qcom_set_core_clk_ctrl(struct ufs_hba *hba, bool is_scale_up, unsigned long freq)
 {
        struct ufs_qcom_host *host = ufshcd_get_variant(hba);
        struct list_head *head = &hba->clk_list_head;
        struct ufs_clk_info *clki;
        u32 cycles_in_1us = 0;
        u32 core_clk_ctrl_reg;
+       unsigned long clk_freq;
        int err;
 
+       if (hba->use_pm_opp && freq != ULONG_MAX) {
+               clk_freq = ufs_qcom_opp_freq_to_clk_freq(hba, freq, "core_clk_unipro");
+               if (clk_freq) {
+                       cycles_in_1us = ceil(clk_freq, HZ_PER_MHZ);
+                       goto set_core_clk_ctrl;
+               }
+       }
+
        list_for_each_entry(clki, head, list) {
                if (!IS_ERR_OR_NULL(clki->clk) &&
                    !strcmp(clki->name, "core_clk_unipro")) {
-                       if (!clki->max_freq)
+                       if (!clki->max_freq) {
                                cycles_in_1us = 150; /* default for backwards compatibility */
-                       else if (freq == ULONG_MAX)
+                               break;
+                       }
+
+                       if (freq == ULONG_MAX) {
                                cycles_in_1us = ceil(clki->max_freq, HZ_PER_MHZ);
-                       else
-                               cycles_in_1us = ceil(freq, HZ_PER_MHZ);
+                               break;
+                       }
 
+                       if (is_scale_up)
+                               cycles_in_1us = ceil(clki->max_freq, HZ_PER_MHZ);
+                       else
+                               cycles_in_1us = ceil(clk_get_rate(clki->clk), HZ_PER_MHZ);
                        break;
                }
        }
 
+set_core_clk_ctrl:
        err = ufshcd_dme_get(hba,
                            UIC_ARG_MIB(DME_VS_CORE_CLK_CTRL),
                            &core_clk_ctrl_reg);
@@ -1479,7 +1498,7 @@ static int ufs_qcom_clk_scale_up_pre_change(struct ufs_hba *hba, unsigned long f
                return ret;
        }
        /* set unipro core clock attributes and clear clock divider */
-       return ufs_qcom_set_core_clk_ctrl(hba, freq);
+       return ufs_qcom_set_core_clk_ctrl(hba, true, freq);
 }
 
 static int ufs_qcom_clk_scale_up_post_change(struct ufs_hba *hba)
@@ -1511,7 +1530,7 @@ static int ufs_qcom_clk_scale_down_pre_change(struct ufs_hba *hba)
 static int ufs_qcom_clk_scale_down_post_change(struct ufs_hba *hba, unsigned long freq)
 {
        /* set unipro core clock attributes and clear clock divider */
-       return ufs_qcom_set_core_clk_ctrl(hba, freq);
+       return ufs_qcom_set_core_clk_ctrl(hba, false, freq);
 }
 
 static int ufs_qcom_clk_scale_notify(struct ufs_hba *hba, bool scale_up,
@@ -2081,11 +2100,53 @@ static int ufs_qcom_config_esi(struct ufs_hba *hba)
        return ret;
 }
 
+static unsigned long ufs_qcom_opp_freq_to_clk_freq(struct ufs_hba *hba,
+                                                  unsigned long freq, char *name)
+{
+       struct ufs_clk_info *clki;
+       struct dev_pm_opp *opp;
+       unsigned long clk_freq;
+       int idx = 0;
+       bool found = false;
+
+       opp = dev_pm_opp_find_freq_exact_indexed(hba->dev, freq, 0, true);
+       if (IS_ERR(opp)) {
+               dev_err(hba->dev, "Failed to find OPP for exact frequency %lu\n", freq);
+               return 0;
+       }
+
+       list_for_each_entry(clki, &hba->clk_list_head, list) {
+               if (!strcmp(clki->name, name)) {
+                       found = true;
+                       break;
+               }
+
+               idx++;
+       }
+
+       if (!found) {
+               dev_err(hba->dev, "Failed to find clock '%s' in clk list\n", name);
+               dev_pm_opp_put(opp);
+               return 0;
+       }
+
+       clk_freq = dev_pm_opp_get_freq_indexed(opp, idx);
+
+       dev_pm_opp_put(opp);
+
+       return clk_freq;
+}
+
 static u32 ufs_qcom_freq_to_gear_speed(struct ufs_hba *hba, unsigned long freq)
 {
        u32 gear = UFS_HS_DONT_CHANGE;
+       unsigned long unipro_freq;
+
+       if (!hba->use_pm_opp)
+               return gear;
 
-       switch (freq) {
+       unipro_freq = ufs_qcom_opp_freq_to_clk_freq(hba, freq, "core_clk_unipro");
+       switch (unipro_freq) {
        case 403000000:
                gear = UFS_HS_G5;
                break;