[PATCH V10 5/7] soc: qcom: rpmpd: Add support for get/set performance state

[Viresh Kumar]

From: Rajendra Nayak <rnayak@xxxxxxxxxxxxxx>

THIS IS TEST CODE, SHOULDN'T BE MERGED.

With genpd now expecting powerdomain drivers supporting performance
state to support get/set performance state callbacks, add support for it
in the rpmpd driver.

Not-signed-off-by: Rajendra Nayak <rnayak@xxxxxxxxxxxxxx>
Not-signed-off-by: Viresh Kumar <viresh.kumar@xxxxxxxxxx>
---
 drivers/soc/qcom/rpmpd.c | 100 +++++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 100 insertions(+)

diff --git a/drivers/soc/qcom/rpmpd.c b/drivers/soc/qcom/rpmpd.c
index d34d9c363815..4e4f5cda9ce2 100644
--- a/drivers/soc/qcom/rpmpd.c
+++ b/drivers/soc/qcom/rpmpd.c
@@ -101,6 +101,20 @@ struct rpmpd_desc {
 	size_t num_pds;
 };
 
+enum rpmpd_levels {
+	NONE,
+	LOWER,          /* SVS2 */
+	LOW,            /* SVS */
+	NOMINAL,        /* NOMINAL */
+	HIGH,           /* Turbo */
+	MAX_LEVEL,
+};
+
+struct rpmpd_freq_map {
+	struct rpmpd *pd;
+	unsigned long freq[MAX_LEVEL];
+};
+
 static DEFINE_MUTEX(rpmpd_lock);
 
 /* msm8996 RPM powerdomains */
@@ -126,6 +140,47 @@ static const struct rpmpd_desc msm8996_desc = {
 	.num_pds = ARRAY_SIZE(msm8996_rpmpds),
 };
 
+enum msm8996_devices {
+	SDHCI,
+	UFS,
+	PCIE,
+	USB3,
+};
+
+static struct rpmpd_freq_map msm8996_rpmpd_freq_map[] = {
+	[SDHCI] = {
+		.pd = &msm8996_vddcx,
+		.freq[LOWER] = 19200000,
+		.freq[LOW] = 200000000,
+		.freq[NOMINAL] = 400000000,
+	},
+	[UFS] = {
+		.pd = &msm8996_vddcx,
+		.freq[LOWER] = 19200000,
+		.freq[LOW] = 100000000,
+		.freq[NOMINAL] = 200000000,
+		.freq[HIGH] = 240000000,
+	},
+	[PCIE] = {
+		.pd = &msm8996_vddcx,
+		.freq[LOWER] = 1011000,
+	},
+	[USB3] = {
+		.pd = &msm8996_vddcx,
+		.freq[LOWER] = 60000000,
+		.freq[LOW] = 120000000,
+		.freq[NOMINAL] = 150000000,
+	},
+};
+
+static const struct of_device_id rpmpd_performance_table[] = {
+	{ .compatible = "qcom,sdhci-msm-v4", .data = (void *)SDHCI },
+	{ .compatible = "qcom,ufshc", .data = (void *)UFS },
+	{ .compatible = "qcom,pcie-msm8996", .data = (void *)PCIE },
+	{ .compatible = "qcom,dwc3", .data = (void *)USB3 },
+	{ }
+};
+
 static const struct of_device_id rpmpd_match_table[] = {
 	{ .compatible = "qcom,rpmpd-msm8996", .data = &msm8996_desc },
 	{ }
@@ -230,6 +285,49 @@ static int rpmpd_power_off(struct generic_pm_domain *domain)
 	return ret;
 }
 
+static int rpmpd_set_performance(struct generic_pm_domain *domain,
+			     unsigned int state)
+{
+	int ret = 0;
+	struct rpmpd *pd = domain_to_rpmpd(domain);
+
+	mutex_lock(&rpmpd_lock);
+
+	pd->corner = state;
+
+	if (!pd->enabled && (pd->key != KEY_FLOOR_CORNER))
+		goto out;
+
+	ret = rpmpd_aggregate_corner(pd);
+
+out:
+	mutex_unlock(&rpmpd_lock);
+
+	return ret;
+}
+
+
+static int rpmpd_get_performance(struct device *dev, unsigned long rate)
+{
+	int i;
+	unsigned long index;
+	const struct of_device_id *id;
+
+	if (!rate)
+		return 0;
+
+	id = of_match_device(rpmpd_performance_table, dev);
+	if (!id)
+		return -EINVAL;
+
+	index = (unsigned long)id->data;
+	for (i = 0; i < MAX_LEVEL; i++)
+		if (msm8996_rpmpd_freq_map[index].freq[i] >= rate)
+			return i;
+
+	return MAX_LEVEL;
+}
+
 static int rpmpd_probe(struct platform_device *pdev)
 {
 	int i;
@@ -267,6 +365,8 @@ static int rpmpd_probe(struct platform_device *pdev)
 		rpmpds[i]->rpm = rpm;
 		rpmpds[i]->pd.power_off = rpmpd_power_off;
 		rpmpds[i]->pd.power_on = rpmpd_power_on;
+		rpmpds[i]->pd.genpd_set_performance_state = rpmpd_set_performance;
+		rpmpds[i]->pd.dev_get_performance_state = rpmpd_get_performance;
 		pm_genpd_init(&rpmpds[i]->pd, NULL, true);
 
 		data->domains[i] = &rpmpds[i]->pd;
-- 
2.7.4