[PATCH 09/18] cpufreq: qcom-nvmem: Add support for voltage scaling

[Dmitry Baryshkov <dmitry.baryshkov@linaro.org>]

If requested by the platform, scale voltages according to data specified
in the OPP tables.

Signed-off-by: Dmitry Baryshkov <dmitry.baryshkov@linaro.org>
---
 drivers/cpufreq/qcom-cpufreq-nvmem.c | 115 ++++++++++++++++++++++++++-
 1 file changed, 114 insertions(+), 1 deletion(-)

diff --git a/drivers/cpufreq/qcom-cpufreq-nvmem.c b/drivers/cpufreq/qcom-cpufreq-nvmem.c
index fee9736f7326..18d6e6ed1bd0 100644
--- a/drivers/cpufreq/qcom-cpufreq-nvmem.c
+++ b/drivers/cpufreq/qcom-cpufreq-nvmem.c
@@ -26,6 +26,7 @@
 #include <linux/platform_device.h>
 #include <linux/pm_domain.h>
 #include <linux/pm_opp.h>
+#include <linux/regulator/consumer.h>
 #include <linux/slab.h>
 #include <linux/soc/qcom/smem.h>
 
@@ -39,6 +40,7 @@ struct qcom_cpufreq_match_data {
 			   char **pvs_name,
 			   struct qcom_cpufreq_drv *drv);
 	const char **genpd_names;
+	const char * const *regulator_names;
 };
 
 struct qcom_cpufreq_drv {
@@ -218,6 +220,110 @@ static const struct qcom_cpufreq_match_data match_data_qcs404 = {
 	.genpd_names = qcs404_genpd_names,
 };
 
+#define NUM_SUPPLIES 2
+static int qcom_cpufreq_config_regulators(struct device *dev,
+					  struct dev_pm_opp *old_opp,
+					  struct dev_pm_opp *new_opp,
+					  struct regulator **regulators,
+					  unsigned int count)
+{
+	struct dev_pm_opp_supply supplies[NUM_SUPPLIES];
+	unsigned long old_freq, freq;
+	unsigned int i;
+	int ret;
+
+	if (WARN_ON_ONCE(count != NUM_SUPPLIES))
+		return -EINVAL;
+
+	ret = dev_pm_opp_get_supplies(new_opp, supplies);
+	if (WARN_ON(ret))
+		return ret;
+
+	old_freq = dev_pm_opp_get_freq(old_opp);
+	freq = dev_pm_opp_get_freq(new_opp);
+
+	WARN_ON(!old_freq || !freq);
+	if (freq > old_freq) {
+		for (i = 0; i < count; i++) {
+			struct regulator *reg = regulators[i];
+			struct dev_pm_opp_supply *supply = &supplies[i];
+
+			dev_dbg(dev, "%s: voltages (mV): %lu %lu %lu\n", __func__,
+				supply->u_volt_min, supply->u_volt, supply->u_volt_max);
+
+			ret = regulator_set_voltage_triplet(reg,
+							    supply->u_volt_min,
+							    supply->u_volt,
+							    supply->u_volt_max);
+			if (ret) {
+				dev_err(dev, "%s: failed to set voltage (%lu %lu %lu mV): %d\n",
+					__func__, supply->u_volt_min, supply->u_volt,
+					supply->u_volt_max, ret);
+				goto restore_backwards;
+			}
+		}
+	} else {
+		for (i = count; i > 0; i--) {
+			struct regulator *reg = regulators[i - 1];
+			struct dev_pm_opp_supply *supply = &supplies[i - 1];
+
+			dev_dbg(dev, "%s: voltages (mV): %lu %lu %lu\n", __func__,
+				supply->u_volt_min, supply->u_volt, supply->u_volt_max);
+
+			ret = regulator_set_voltage_triplet(reg,
+							    supply->u_volt_min,
+							    supply->u_volt,
+							    supply->u_volt_max);
+			if (ret) {
+				dev_err(dev, "%s: failed to set voltage (%lu %lu %lu mV): %d\n",
+					__func__, supply->u_volt_min, supply->u_volt,
+					supply->u_volt_max, ret);
+				goto restore_forward;
+			}
+		}
+	}
+
+	return 0;
+
+restore_backwards:
+
+	dev_pm_opp_get_supplies(old_opp, supplies);
+
+	for (; i > 0; i--) {
+		struct regulator *reg = regulators[i - 1];
+		struct dev_pm_opp_supply *supply = &supplies[i - 1];
+
+		dev_dbg(dev, "%s: voltages (mV): %lu %lu %lu\n", __func__,
+			supply->u_volt_min, supply->u_volt, supply->u_volt_max);
+
+		regulator_set_voltage_triplet(reg,
+					      supply->u_volt_min,
+					      supply->u_volt,
+					      supply->u_volt_max);
+	}
+
+	return ret;
+
+restore_forward:
+
+	dev_pm_opp_get_supplies(old_opp, supplies);
+
+	for ( ; i < count; i++) {
+		struct regulator *reg = regulators[i];
+		struct dev_pm_opp_supply *supply = &supplies[i];
+
+		dev_dbg(dev, "%s: voltages (mV): %lu %lu %lu\n", __func__,
+			supply->u_volt_min, supply->u_volt, supply->u_volt_max);
+
+		regulator_set_voltage_triplet(reg,
+					      supply->u_volt_min,
+					      supply->u_volt,
+					      supply->u_volt_max);
+	}
+
+	return ret;
+}
+
 static int qcom_cpufreq_probe(struct platform_device *pdev)
 {
 	struct qcom_cpufreq_drv *drv;
@@ -305,7 +411,14 @@ static int qcom_cpufreq_probe(struct platform_device *pdev)
 			config.virt_devs = NULL;
 		}
 
-		if (config.supported_hw || config.genpd_names) {
+		if (drv->data->regulator_names) {
+			config.config_regulators = qcom_cpufreq_config_regulators;
+			config.regulator_names = drv->data->regulator_names;
+		}
+
+		if (config.supported_hw ||
+		    config.genpd_names ||
+		    config.regulator_names) {
 			drv->opp_tokens[cpu] = dev_pm_opp_set_config(cpu_dev, &config);
 			if (drv->opp_tokens[cpu] < 0) {
 				ret = drv->opp_tokens[cpu];
-- 
2.39.2