arm: zynq: Add boot up frequency OPP

We cannot know the CPU frequency the system boots up with at
compile time.
So, we uncoditionally add this frequency to the OPP table at
runtime.
Since this might result in duplicates in the frequency table,
other points are added only when it is not present in the OPP
list already.

Signed-off-by: Soren Brinkmann <soren.brinkmann@xilinx.com>
Acked-by: Michal Simek <michal.simek@xilinx.com>

diff --git a/arch/arm/mach-zynq/common.c b/arch/arm/mach-zynq/common.c
index 305b320474d3683aa3e63a419e5ce574e1e4abc2..729ce51623dccc8783a2626d21afcad61482719b 100644 (file)
--- a/arch/arm/mach-zynq/common.c
+++ b/arch/arm/mach-zynq/common.c
@@ -130,33 +130,44 @@ static void __init xilinx_opp_init(void)
        /* We need some conditionals to enable the max frequencies for the right
         * parts only. */
        /* -3E(?) max f = 1GHz */
+       freq = clk_get_rate(cpuclk);
+       ret |= opp_add(dev, freq, 0);
        freq = clk_round_rate(cpuclk, 1000000000);
-       if (abs(1000000000 - freq) < 50000000)
+       if (abs(1000000000 - freq) < 50000000 &&
+                       IS_ERR(opp_find_freq_exact(dev, freq, 1)))
                ret |= opp_add(dev, freq, 0);
        /* -3 parts max f = 800 MHz */
        freq = clk_round_rate(cpuclk, 800000000);
-       if (abs(800000000 - freq) < 10000000)
+       if (abs(800000000 - freq) < 10000000 &&
+                       IS_ERR(opp_find_freq_exact(dev, freq, 1)))
                ret |= opp_add(dev, freq, 0);
        freq = clk_round_rate(cpuclk, 666666667);
-       if (abs(666666667 - freq) < 10000000)
+       if (abs(666666667 - freq) < 10000000 &&
+                       IS_ERR(opp_find_freq_exact(dev, freq, 1)))
                ret |= opp_add(dev, freq, 0);
        freq = clk_round_rate(cpuclk, 555555556);
-       if (abs(555555556 - freq) < 10000000)
+       if (abs(555555556 - freq) < 10000000 &&
+                       IS_ERR(opp_find_freq_exact(dev, freq, 1)))
                ret |= opp_add(dev, freq, 0);
        freq = clk_round_rate(cpuclk, 444444444);
-       if (abs(444444444 - freq) < 10000000)
+       if (abs(444444444 - freq) < 10000000 &&
+                       IS_ERR(opp_find_freq_exact(dev, freq, 1)))
                ret |= opp_add(dev, freq, 0);
        freq = clk_round_rate(cpuclk, 333333333);
-       if (abs(333333333 - freq) < 10000000)
+       if (abs(333333333 - freq) < 10000000 &&
+                       IS_ERR(opp_find_freq_exact(dev, freq, 1)))
                ret |= opp_add(dev, freq, 0);
        freq = clk_round_rate(cpuclk, 222222222);
-       if (abs(222222222 - freq) < 10000000)
+       if (abs(222222222 - freq) < 10000000 &&
+                       IS_ERR(opp_find_freq_exact(dev, freq, 1)))
                ret |= opp_add(dev, freq, 0);
        freq = clk_round_rate(cpuclk, 111111111);
-       if (abs(111111111 - freq) < 10000000)
+       if (abs(111111111 - freq) < 10000000 &&
+                       IS_ERR(opp_find_freq_exact(dev, freq, 1)))
                ret |= opp_add(dev, freq, 0);
        freq = clk_round_rate(cpuclk, 50000000);
-       if (abs(50000000 - freq) < 5000000)
+       if (abs(50000000 - freq) < 5000000 &&
+                       IS_ERR(opp_find_freq_exact(dev, freq, 1)))
                ret |= opp_add(dev, freq, 0);
 
        if (ret)