[PATCH v3 6/7] PM: opp: parse multiple frequencies in each OPP

[Krzysztof Kozlowski <krzysztof.kozlowski@linaro.org>]

Devices might need to control several clocks when scaling the frequency
and voltage, with each clock having different frequency.  Parse the
'opp-hz' Devicetree property as an array and store all frequencies for
the custom set_opp in a new field 'rates'.

Since the PM OPP framework operates on only one clock (multiple clocks
are for custom set_opp helpers), these frequencies are actually not used
internally and still the core relies on the original field 'rate'.

All other PM OPP functions, like finding floor/ceil PM OPPs, operate
still on first rate given in the 'opp-hz' frequency.

Signed-off-by: Krzysztof Kozlowski <krzysztof.kozlowski@linaro.org>

---

Cc: Manivannan Sadhasivam <manivannan.sadhasivam@linaro.org>
---
 drivers/opp/core.c     | 20 +++++++++++++++---
 drivers/opp/of.c       | 47 ++++++++++++++++++++++++++++++++++++++++++
 drivers/opp/opp.h      |  2 ++
 include/linux/pm_opp.h |  3 +++
 4 files changed, 69 insertions(+), 3 deletions(-)

diff --git a/drivers/opp/core.c b/drivers/opp/core.c
index aac3c6d89ae0..563e962c3411 100644
--- a/drivers/opp/core.c
+++ b/drivers/opp/core.c
@@ -1033,7 +1033,9 @@ static int _set_opp_custom(const struct opp_table *opp_table,
 	data->clks = opp_table->clks;
 	data->dev = dev;
 	data->old_opp.rate = old_opp->rate;
+	data->old_opp.rates = old_opp->rates;
 	data->new_opp.rate = freq;
+	data->new_opp.rates = opp->rates;
 
 	return opp_table->set_opp(data);
 }
@@ -1307,6 +1309,11 @@ int dev_pm_opp_set_rate(struct device *dev, unsigned long target_freq)
 				__func__, freq, ret);
 			goto put_opp_table;
 		}
+		/*
+		 * opp->rates are used for scaling clocks, so be sure accurate
+		 * 'freq' is used, instead what was defined via e.g. Devicetree.
+		 */
+		opp->rates[0] = freq;
 	}
 
 	ret = _set_opp(dev, opp_table, opp, freq);
@@ -1761,23 +1768,29 @@ EXPORT_SYMBOL_GPL(dev_pm_opp_remove_all_dynamic);
 struct dev_pm_opp *_opp_allocate(struct opp_table *table)
 {
 	struct dev_pm_opp *opp;
-	int supply_count, supply_size, icc_size;
+	int rate_count, rate_size, supply_count, supply_size, icc_size;
 
 	/* Allocate space for at least one supply */
 	supply_count = table->regulator_count > 0 ? table->regulator_count : 1;
 	supply_size = sizeof(*opp->supplies) * supply_count;
+	/* Allocate space for at least one rate */
+	rate_count = table->clk_count > 0 ? table->clk_count : 1;
+	rate_size = sizeof(*opp->rates) * rate_count;
 	icc_size = sizeof(*opp->bandwidth) * table->path_count;
 
 	/* allocate new OPP node and supplies structures */
-	opp = kzalloc(sizeof(*opp) + supply_size + icc_size, GFP_KERNEL);
+	opp = kzalloc(sizeof(*opp) + rate_size + supply_size + icc_size,
+		      GFP_KERNEL);
 
 	if (!opp)
 		return NULL;
 
 	/* Put the supplies at the end of the OPP structure as an empty array */
 	opp->supplies = (struct dev_pm_opp_supply *)(opp + 1);
+	opp->rates = (unsigned long *)(opp->supplies + supply_count);
 	if (icc_size)
-		opp->bandwidth = (struct dev_pm_opp_icc_bw *)(opp->supplies + supply_count);
+		opp->bandwidth = (struct dev_pm_opp_icc_bw *)(opp->rates + rate_count);
+
 	INIT_LIST_HEAD(&opp->node);
 
 	return opp;
@@ -1957,6 +1970,7 @@ int _opp_add_v1(struct opp_table *opp_table, struct device *dev,
 
 	/* populate the opp table */
 	new_opp->rate = freq;
+	new_opp->rates[0] = freq;
 	tol = u_volt * opp_table->voltage_tolerance_v1 / 100;
 	new_opp->supplies[0].u_volt = u_volt;
 	new_opp->supplies[0].u_volt_min = u_volt - tol;
diff --git a/drivers/opp/of.c b/drivers/opp/of.c
index 30394929d700..b226d2c7a84b 100644
--- a/drivers/opp/of.c
+++ b/drivers/opp/of.c
@@ -767,6 +767,46 @@ void dev_pm_opp_of_remove_table(struct device *dev)
 }
 EXPORT_SYMBOL_GPL(dev_pm_opp_of_remove_table);
 
+static int _read_clocks(struct dev_pm_opp *opp, struct opp_table *opp_table,
+			struct device_node *np)
+{
+	int i, count, ret;
+	u64 *freq;
+
+	count = of_property_count_u64_elems(np, "opp-hz");
+	if (count < 0) {
+		pr_err("%s: Invalid %s property (%d)\n",
+			__func__, of_node_full_name(np), count);
+		return count;
+	}
+
+	if (count != opp_table->clk_count) {
+		pr_err("%s: number of rates %d does not match number of clocks %d in %pOF\n",
+		       __func__, count, opp_table->clk_count, np);
+		return -EINVAL;
+	}
+
+	freq = kmalloc_array(count, sizeof(*freq), GFP_KERNEL);
+	if (!freq)
+		return -ENOMEM;
+
+	ret = of_property_read_u64_array(np, "opp-hz", freq, count);
+	if (ret) {
+		pr_err("%s: error parsing %s: %d\n", __func__,
+		       of_node_full_name(np), ret);
+		ret = -EINVAL;
+		goto free_freq;
+	}
+
+	for (i = 0; i < count; i++)
+		opp->rates[i] = freq[i];
+
+free_freq:
+	kfree(freq);
+
+	return ret;
+}
+
 static int _read_bw(struct dev_pm_opp *new_opp, struct opp_table *table,
 		    struct device_node *np, bool peak)
 {
@@ -827,6 +867,13 @@ static int _read_opp_key(struct dev_pm_opp *new_opp, struct opp_table *table,
 	}
 	*rate_not_available = !!ret;
 
+	if (!ret) {
+		ret = _read_clocks(new_opp, table, np);
+		/* The properties were found but we failed to parse them */
+		if (ret && ret != -ENODEV)
+			return ret;
+	}
+
 	/*
 	 * Bandwidth consists of peak and average (optional) values:
 	 * opp-peak-kBps = <path1_value path2_value>;
diff --git a/drivers/opp/opp.h b/drivers/opp/opp.h
index 59aac08baf82..217d4376af9b 100644
--- a/drivers/opp/opp.h
+++ b/drivers/opp/opp.h
@@ -61,6 +61,7 @@ extern struct list_head opp_tables, lazy_opp_tables;
  * @rate:	Frequency in hertz
  * @level:	Performance level
  * @supplies:	Power supplies voltage/current values
+ * @rates:	Frequency rates for the clocks (equal to @rate for one clock)
  * @bandwidth:	Interconnect bandwidth values
  * @clock_latency_ns: Latency (in nanoseconds) of switching to this OPP's
  *		frequency from any other OPP's frequency.
@@ -85,6 +86,7 @@ struct dev_pm_opp {
 	unsigned int level;
 
 	struct dev_pm_opp_supply *supplies;
+	unsigned long *rates;
 	struct dev_pm_opp_icc_bw *bandwidth;
 
 	unsigned long clock_latency_ns;
diff --git a/include/linux/pm_opp.h b/include/linux/pm_opp.h
index 5f17f10fcc3f..5fd3895af605 100644
--- a/include/linux/pm_opp.h
+++ b/include/linux/pm_opp.h
@@ -60,12 +60,15 @@ struct dev_pm_opp_icc_bw {
 /**
  * struct dev_pm_opp_info - OPP freq/voltage/current values
  * @rate:	Target clk rate in hz
+ * @rates:	Array of clock frequencies for all clocks (equal to @rate for
+ *		one clock)
  * @supplies:	Array of voltage/current values for all power supplies
  *
  * This structure stores the freq/voltage/current values for a single OPP.
  */
 struct dev_pm_opp_info {
 	unsigned long rate;
+	unsigned long *rates;
 	struct dev_pm_opp_supply *supplies;
 };
 
-- 
2.32.0