From 6a1d3bccc836caba3e674059b797de719d94e8aa Mon Sep 17 00:00:00 2001
From: Finley Xiao <finley.xiao@rock-chips.com>
Date: Fri, 25 Mar 2022 14:54:35 +0800
Subject: [PATCH] PM / devfreq: rockchip_dmc: Implement
 rockchip_dmcfreq_adjust_opp_table()

The dev_pm_opp_add() can't be used to add opp with multiple voltage
ranges, and there's no need to add new opp, just need adjust opp rate
according to the rate support by DDR.

Signed-off-by: Finley Xiao <finley.xiao@rock-chips.com>
Change-Id: Ic9dd29bedd22d0b7928110c6d0c6483414b48415
---
 drivers/devfreq/rockchip_dmc.c | 67 +++++++++++++++++++++++++++++++++-
 1 file changed, 66 insertions(+), 1 deletion(-)

diff --git a/drivers/devfreq/rockchip_dmc.c b/drivers/devfreq/rockchip_dmc.c
index 750b2426cbc4..1bd2f8ba1e43 100644
--- a/drivers/devfreq/rockchip_dmc.c
+++ b/drivers/devfreq/rockchip_dmc.c
@@ -1292,6 +1292,71 @@ out:
 	return ret;
 }
 
+static __maybe_unused int
+rockchip_dmcfreq_adjust_opp_table(struct rockchip_dmcfreq *dmcfreq)
+{
+	struct device *dev = dmcfreq->dev;
+	struct arm_smccc_res res;
+	struct dev_pm_opp *opp;
+	struct opp_table *opp_table;
+	unsigned long target_rate = 0, last_rate = 0;
+	int i, count = 0;
+
+	res = sip_smc_dram(SHARE_PAGE_TYPE_DDR, 0,
+			   ROCKCHIP_SIP_CONFIG_DRAM_GET_FREQ_INFO);
+	if (res.a0) {
+		dev_err(dev, "rockchip_sip_config_dram_get_freq_info error:%lx\n",
+			res.a0);
+		return -ENOMEM;
+	}
+
+	if (ddr_psci_param->freq_count == 0 || ddr_psci_param->freq_count > 6) {
+		dev_err(dev, "there is no available frequencies!\n");
+		return -EPERM;
+	}
+
+	for (i = 0; i < ddr_psci_param->freq_count; i++)
+		dmcfreq->freq_info_rate[i] = ddr_psci_param->freq_info_mhz[i] * 1000000;
+	dmcfreq->freq_count = ddr_psci_param->freq_count;
+
+	opp_table = dev_pm_opp_get_opp_table(dev);
+	if (!opp_table)
+		return -ENOMEM;
+
+	mutex_lock(&opp_table->lock);
+	list_for_each_entry(opp, &opp_table->opp_list, node) {
+		if (!opp->available)
+			continue;
+		/* Search for a rounded floor frequency */
+		target_rate = 0;
+		for (i = 0; i < dmcfreq->freq_count; i++) {
+			if (dmcfreq->freq_info_rate[i] <= opp->rate)
+				target_rate = dmcfreq->freq_info_rate[i];
+		}
+		/* If not find, disable the opp */
+		if (!target_rate) {
+			opp->available = false;
+		} else {
+			/* If the opp rate is equal to last opp rate, disable it */
+			if (target_rate == last_rate) {
+				opp->available = false;
+			} else {
+				opp->rate = target_rate;
+				last_rate = opp->rate;
+				count++;
+			}
+		}
+	}
+	mutex_unlock(&opp_table->lock);
+	dev_pm_opp_put_opp_table(opp_table);
+	if (!count) {
+		dev_err(dev, "there is no available opp\n");
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
 static __maybe_unused int px30_dmc_init(struct platform_device *pdev,
 					struct rockchip_dmcfreq *dmcfreq)
 {
@@ -1927,7 +1992,7 @@ static __maybe_unused int rk3588_dmc_init(struct platform_device *pdev,
 		return -ENOMEM;
 	}
 
-	ret = rockchip_get_freq_info(dmcfreq);
+	ret = rockchip_dmcfreq_adjust_opp_table(dmcfreq);
 	if (ret < 0) {
 		dev_err(&pdev->dev, "cannot get frequency info\n");
 		return ret;