[PATCH 3/3] scsi: ufs: core: Report the current clock frequency to devfreq

[Bean Huo <beanhuo@iokpp.de>]

From: Bean Huo <beanhuo@micron.com>

When a driver does not provide a ->get_cur_freq() callback, the cur_freq
sysfs attribute shows devfreq->previous_freq, which only tracks the
scaling that the governor itself did.

The UFS controller is also scaled outside the governor. The clearest
example is writing 0 to clkscale_enable: ufshcd_clkscale_enable_store()
sets the clocks to max_freq through ufshcd_devfreq_scale() and suspends
the governor, so devfreq_set_target() is never called. After that,
cur_freq keeps showing the last frequency the governor chose instead of
the one the controller runs at, and it does so as long as clock scaling
stays disabled.

Add ufshcd_devfreq_get_cur_freq(). It reports clk_scaling.target_freq
when OPPs are used and the first clock's curr_freq otherwise, the same
values that ufshcd_devfreq_get_dev_status() reports. Both are 0 until
the controller is scaled for the first time, so return an error in that
case and let devfreq use the frequency it last set.

Signed-off-by: Bean Huo <beanhuo@micron.com>
---
 drivers/ufs/core/ufshcd.c | 32 ++++++++++++++++++++++++++++++++
 1 file changed, 32 insertions(+)

diff --git a/drivers/ufs/core/ufshcd.c b/drivers/ufs/core/ufshcd.c
index 2ba244cf40ac..45187a7e4f4f 100644
--- a/drivers/ufs/core/ufshcd.c
+++ b/drivers/ufs/core/ufshcd.c
@@ -1716,6 +1716,37 @@ static int ufshcd_devfreq_get_dev_status(struct device *dev,
 	return 0;
 }
 
+static int ufshcd_devfreq_get_cur_freq(struct device *dev, unsigned long *freq)
+{
+	struct ufs_hba *hba = dev_get_drvdata(dev);
+	unsigned long cur_freq;
+
+	if (!ufshcd_is_clkscaling_supported(hba))
+		return -EINVAL;
+
+	if (hba->use_pm_opp) {
+		cur_freq = hba->clk_scaling.target_freq;
+	} else {
+		struct ufs_clk_info *clki;
+
+		clki = list_first_entry(&hba->clk_list_head,
+					struct ufs_clk_info, list);
+		cur_freq = clki->curr_freq;
+	}
+
+	/*
+	 * target_freq stays 0 until something scales the controller for the
+	 * first time. Report nothing rather than 0 so devfreq falls back to
+	 * the frequency it last set.
+	 */
+	if (!cur_freq)
+		return -EINVAL;
+
+	*freq = cur_freq;
+
+	return 0;
+}
+
 static int ufshcd_devfreq_init(struct ufs_hba *hba)
 {
 	struct list_head *clk_list = &hba->clk_list_head;
@@ -9612,6 +9643,7 @@ static struct ufs_hba_variant_params ufs_hba_vps = {
 	.devfreq_profile.polling_ms	= 100,
 	.devfreq_profile.target		= ufshcd_devfreq_target,
 	.devfreq_profile.get_dev_status	= ufshcd_devfreq_get_dev_status,
+	.devfreq_profile.get_cur_freq	= ufshcd_devfreq_get_cur_freq,
 	.ondemand_data.upthreshold	= 70,
 	.ondemand_data.downdifferential	= 5,
 };
-- 
2.34.1