[PATCH 5.1 15/30] cpufreq: armada-37xx: fix frequency calculation for opp

[Greg Kroah-Hartman]

From: Gregory CLEMENT <gregory.clem...@bootlin.com>

commit 8db82563451f976597ab7b282ec655e4390a4088 upstream.

The frequency calculation was based on the current(max) frequency of the
CPU. However for low frequency, the value used was already the parent
frequency divided by a factor of 2.

Instead of using this frequency, this fix directly get the frequency from
the parent clock.

Fixes: 92ce45fb875d ("cpufreq: Add DVFS support for Armada 37xx")
Cc: <sta...@vger.kernel.org>
Reported-by: Christian Neubert <christian.neubert...@gmail.com>
Signed-off-by: Gregory CLEMENT <gregory.clem...@bootlin.com>
Signed-off-by: Viresh Kumar <viresh.ku...@linaro.org>
Signed-off-by: Greg Kroah-Hartman <gre...@linuxfoundation.org>

---
 drivers/cpufreq/armada-37xx-cpufreq.c |   22 +++++++++++++++++++---
 1 file changed, 19 insertions(+), 3 deletions(-)

--- a/drivers/cpufreq/armada-37xx-cpufreq.c
+++ b/drivers/cpufreq/armada-37xx-cpufreq.c
@@ -359,11 +359,11 @@ static int __init armada37xx_cpufreq_dri
        struct armada_37xx_dvfs *dvfs;
        struct platform_device *pdev;
        unsigned long freq;
-       unsigned int cur_frequency;
+       unsigned int cur_frequency, base_frequency;
        struct regmap *nb_pm_base, *avs_base;
        struct device *cpu_dev;
        int load_lvl, ret;
-       struct clk *clk;
+       struct clk *clk, *parent;
 
        nb_pm_base =
                syscon_regmap_lookup_by_compatible("marvell,armada-3700-nb-pm");
@@ -399,6 +399,22 @@ static int __init armada37xx_cpufreq_dri
                return PTR_ERR(clk);
        }
 
+       parent = clk_get_parent(clk);
+       if (IS_ERR(parent)) {
+               dev_err(cpu_dev, "Cannot get parent clock for CPU0\n");
+               clk_put(clk);
+               return PTR_ERR(parent);
+       }
+
+       /* Get parent CPU frequency */
+       base_frequency =  clk_get_rate(parent);
+
+       if (!base_frequency) {
+               dev_err(cpu_dev, "Failed to get parent clock rate for CPU\n");
+               clk_put(clk);
+               return -EINVAL;
+       }
+
        /* Get nominal (current) CPU frequency */
        cur_frequency = clk_get_rate(clk);
        if (!cur_frequency) {
@@ -431,7 +447,7 @@ static int __init armada37xx_cpufreq_dri
        for (load_lvl = ARMADA_37XX_DVFS_LOAD_0; load_lvl < LOAD_LEVEL_NR;
             load_lvl++) {
                unsigned long u_volt = avs_map[dvfs->avs[load_lvl]] * 1000;
-               freq = cur_frequency / dvfs->divider[load_lvl];
+               freq = base_frequency / dvfs->divider[load_lvl];
                ret = dev_pm_opp_add(cpu_dev, freq, u_volt);
                if (ret)
                        goto remove_opp;