clk: zynqmp: Add support for clock with CLK_DIVIDER_POWER_OF_TWO flag

Existing clock divider functions is not checking for
base of divider. So, if any clock divider is power of 2
then clock rate calculation will be wrong.

Add support to calculate divider value for the clocks
with CLK_DIVIDER_POWER_OF_TWO flag.

Signed-off-by: Tejas Patel <tejas.patel@xilinx.com>
Signed-off-by: Radhey Shyam Pandey <radhey.shyam.pandey@xilinx.com>
Signed-off-by: Rajan Vaja <rajan.vaja@xilinx.com>
Link: https://lkml.kernel.org/r/1575527759-26452-7-git-send-email-rajan.vaja@xilinx.com
Signed-off-by: Stephen Boyd <sboyd@kernel.org>

diff --git a/drivers/clk/zynqmp/divider.c b/drivers/clk/zynqmp/divider.c
index c6af0c4..4be2cc7 100644 (file)
--- a/drivers/clk/zynqmp/divider.c
+++ b/drivers/clk/zynqmp/divider.c
@@ -2,7 +2,7 @@
 /*
  * Zynq UltraScale+ MPSoC Divider support
  *
- *  Copyright (C) 2016-2018 Xilinx
+ *  Copyright (C) 2016-2019 Xilinx
  *
  * Adjustable divider clock implementation
  */
@@ -45,9 +45,26 @@ struct zynqmp_clk_divider {
 };
 
 static inline int zynqmp_divider_get_val(unsigned long parent_rate,
-                                        unsigned long rate)
+                                        unsigned long rate, u16 flags)
 {
-       return DIV_ROUND_CLOSEST(parent_rate, rate);
+       int up, down;
+       unsigned long up_rate, down_rate;
+
+       if (flags & CLK_DIVIDER_POWER_OF_TWO) {
+               up = DIV_ROUND_UP_ULL((u64)parent_rate, rate);
+               down = DIV_ROUND_DOWN_ULL((u64)parent_rate, rate);
+
+               up = __roundup_pow_of_two(up);
+               down = __rounddown_pow_of_two(down);
+
+               up_rate = DIV_ROUND_UP_ULL((u64)parent_rate, up);
+               down_rate = DIV_ROUND_UP_ULL((u64)parent_rate, down);
+
+               return (rate - up_rate) <= (down_rate - rate) ? up : down;
+
+       } else {
+               return DIV_ROUND_CLOSEST(parent_rate, rate);
+       }
 }
 
 /**
@@ -79,6 +96,9 @@ static unsigned long zynqmp_clk_divider_recalc_rate(struct clk_hw *hw,
        else
                value = div >> 16;
 
+       if (divider->flags & CLK_DIVIDER_POWER_OF_TWO)
+               value = 1 << value;
+
        if (!value) {
                WARN(!(divider->flags & CLK_DIVIDER_ALLOW_ZERO),
                     "%s: Zero divisor and CLK_DIVIDER_ALLOW_ZERO not set\n",
@@ -157,10 +177,13 @@ static long zynqmp_clk_divider_round_rate(struct clk_hw *hw,
                else
                        bestdiv  = bestdiv >> 16;
 
+               if (divider->flags & CLK_DIVIDER_POWER_OF_TWO)
+                       bestdiv = 1 << bestdiv;
+
                return DIV_ROUND_UP_ULL((u64)*prate, bestdiv);
        }
 
-       bestdiv = zynqmp_divider_get_val(*prate, rate);
+       bestdiv = zynqmp_divider_get_val(*prate, rate, divider->flags);
 
        /*
         * In case of two divisors, compute best divider values and return
@@ -198,7 +221,7 @@ static int zynqmp_clk_divider_set_rate(struct clk_hw *hw, unsigned long rate,
        int ret;
        const struct zynqmp_eemi_ops *eemi_ops = zynqmp_pm_get_eemi_ops();
 
-       value = zynqmp_divider_get_val(parent_rate, rate);
+       value = zynqmp_divider_get_val(parent_rate, rate, divider->flags);
        if (div_type == TYPE_DIV1) {
                div = value & 0xFFFF;
                div |= 0xffff << 16;
@@ -207,6 +230,9 @@ static int zynqmp_clk_divider_set_rate(struct clk_hw *hw, unsigned long rate,
                div |= value << 16;
        }
 
+       if (divider->flags & CLK_DIVIDER_POWER_OF_TWO)
+               div = __ffs(div);
+
        ret = eemi_ops->clock_setdivider(clk_id, div);
 
        if (ret)