[PATCH 4/7] PM / OPP: use OPP node clock to set CPU frequency

[Dong Aisheng]

If OPP node has a valid clock, we use this corresponding OPP clock to
set the device clock frequency instead of the default opp_table->clk which
is shared by all OPPs.

Cc: Viresh Kumar <vire...@kernel.org>
Cc: Nishanth Menon <n...@ti.com>
Cc: Stephen Boyd <sb...@codeaurora.org>
Cc: "Rafael J. Wysocki" <r...@rjwysocki.net>
Signed-off-by: Dong Aisheng <aisheng.d...@nxp.com>
---
 drivers/base/power/opp/core.c | 10 +++++++++-
 drivers/base/power/opp/of.c   |  8 ++++++++
 drivers/base/power/opp/opp.h  |  3 +++
 3 files changed, 20 insertions(+), 1 deletion(-)

diff --git a/drivers/base/power/opp/core.c b/drivers/base/power/opp/core.c
index 2579401..d4656cc 100644
--- a/drivers/base/power/opp/core.c
+++ b/drivers/base/power/opp/core.c
@@ -600,7 +600,7 @@ int dev_pm_opp_set_rate(struct device *dev, unsigned long 
target_freq)
        struct opp_table *opp_table;
        unsigned long freq, old_freq;
        struct dev_pm_opp *old_opp, *opp;
-       struct clk *clk;
+       struct clk *clk, *old_clk = NULL;
        int ret, size;
 
        if (unlikely(!target_freq)) {
@@ -651,6 +651,11 @@ int dev_pm_opp_set_rate(struct device *dev, unsigned long 
target_freq)
                goto put_old_opp;
        }
 
+       if (!IS_ERR_OR_NULL(opp->clk)) {
+               old_clk = opp_table->cur_clk;
+               opp_table->cur_clk = opp->clk;
+       }
+
        dev_dbg(dev, "%s: switching OPP: %lu Hz --> %lu Hz\n", __func__,
                old_freq, freq);
 
@@ -683,6 +688,9 @@ int dev_pm_opp_set_rate(struct device *dev, unsigned long 
target_freq)
                ret = opp_table->set_opp(data);
        }
 
+       if (ret && !IS_ERR_OR_NULL(opp->clk))
+               opp_table->cur_clk = old_clk;
+
        dev_pm_opp_put(opp);
 put_old_opp:
        if (!IS_ERR(old_opp))
diff --git a/drivers/base/power/opp/of.c b/drivers/base/power/opp/of.c
index 57eec1c..64909e7 100644
--- a/drivers/base/power/opp/of.c
+++ b/drivers/base/power/opp/of.c
@@ -13,6 +13,7 @@
 
 #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
 
+#include <linux/clk.h>
 #include <linux/cpu.h>
 #include <linux/errno.h>
 #include <linux/device.h>
@@ -314,6 +315,13 @@ static int _opp_add_static_v2(struct opp_table *opp_table, 
struct device *dev,
        new_opp->dynamic = false;
        new_opp->available = true;
 
+       new_opp->clk = of_clk_get(np, 0);
+       if (IS_ERR(new_opp->clk)) {
+               ret = PTR_ERR(new_opp->clk);
+               if (ret == -EPROBE_DEFER)
+                       return -EPROBE_DEFER;
+       }
+
        if (!of_property_read_u32(np, "clock-latency-ns", &val))
                new_opp->clock_latency_ns = val;
 
diff --git a/drivers/base/power/opp/opp.h b/drivers/base/power/opp/opp.h
index 30a637c..dbe8ec6 100644
--- a/drivers/base/power/opp/opp.h
+++ b/drivers/base/power/opp/opp.h
@@ -60,6 +60,7 @@ extern struct list_head opp_tables;
  * @suspend:   true if suspend OPP
  * @rate:      Frequency in hertz
  * @supplies:  Power supplies voltage/current values
+ * @clk:       Clock used for this OPP
  * @clock_latency_ns: Latency (in nanoseconds) of switching to this OPP's
  *             frequency from any other OPP's frequency.
  * @opp_table: points back to the opp_table struct this opp belongs to
@@ -80,6 +81,8 @@ struct dev_pm_opp {
 
        struct dev_pm_opp_supply *supplies;
 
+       struct clk *clk;
+
        unsigned long clock_latency_ns;
 
        struct opp_table *opp_table;
-- 
2.7.4