clk: bcm2835: add a round up ability to the clock divisor

Make bcm2835_clock_choose_div to optionally round up the chosen MASH divisor
so that the resulting average rate will not be higher than the requested one.

Signed-off-by: Remi Pommarel <repk@triplefau.lt>
Reviewed-by: Eric Anholt <eric@anholt.net>
Signed-off-by: Michael Turquette <mturquette@baylibre.com>

diff --git a/drivers/clk/bcm/clk-bcm2835.c b/drivers/clk/bcm/clk-bcm2835.c
index 39bf5820297e472d95e88ddc28c868a0c80b5f6c..9e881eef3c36f1f7835fe2369efff72334a58192 100644 (file)
--- a/drivers/clk/bcm/clk-bcm2835.c
+++ b/drivers/clk/bcm/clk-bcm2835.c
@@ -1148,22 +1148,24 @@ static int bcm2835_clock_is_on(struct clk_hw *hw)
 
 static u32 bcm2835_clock_choose_div(struct clk_hw *hw,
                                    unsigned long rate,
-                                   unsigned long parent_rate)
+                                   unsigned long parent_rate,
+                                   bool round_up)
 {
        struct bcm2835_clock *clock = bcm2835_clock_from_hw(hw);
        const struct bcm2835_clock_data *data = clock->data;
-       u32 unused_frac_mask = GENMASK(CM_DIV_FRAC_BITS - data->frac_bits, 0);
+       u32 unused_frac_mask =
+               GENMASK(CM_DIV_FRAC_BITS - data->frac_bits, 0) >> 1;
        u64 temp = (u64)parent_rate << CM_DIV_FRAC_BITS;
+       u64 rem;
        u32 div;
 
-       do_div(temp, rate);
+       rem = do_div(temp, rate);
        div = temp;
 
-       /* Round and mask off the unused bits */
-       if (unused_frac_mask != 0) {
-               div += unused_frac_mask >> 1;
-               div &= ~unused_frac_mask;
-       }
+       /* Round up and mask off the unused bits */
+       if (round_up && ((div & unused_frac_mask) != 0 || rem != 0))
+               div += unused_frac_mask + 1;
+       div &= ~unused_frac_mask;
 
        /* Clamp to the limits. */
        div = max(div, unused_frac_mask + 1);
@@ -1202,7 +1204,7 @@ static long bcm2835_clock_round_rate(struct clk_hw *hw,
                                     unsigned long *parent_rate)
 {
        struct bcm2835_clock *clock = bcm2835_clock_from_hw(hw);
-       u32 div = bcm2835_clock_choose_div(hw, rate, *parent_rate);
+       u32 div = bcm2835_clock_choose_div(hw, rate, *parent_rate, false);
 
        return bcm2835_clock_rate_from_divisor(clock, *parent_rate, div);
 }
@@ -1271,7 +1273,7 @@ static int bcm2835_clock_set_rate(struct clk_hw *hw,
        struct bcm2835_clock *clock = bcm2835_clock_from_hw(hw);
        struct bcm2835_cprman *cprman = clock->cprman;
        const struct bcm2835_clock_data *data = clock->data;
-       u32 div = bcm2835_clock_choose_div(hw, rate, parent_rate);
+       u32 div = bcm2835_clock_choose_div(hw, rate, parent_rate, false);
 
        cprman_write(cprman, data->div_reg, div);