clk: bcm2835: Fix setting of PLL divider clock rates

[firefly-linux-kernel-4.4.55.git] / drivers / clk / bcm / clk-bcm2835.c

diff --git a/drivers/clk/bcm/clk-bcm2835.c b/drivers/clk/bcm/clk-bcm2835.c
index 39bf5820297e472d95e88ddc28c868a0c80b5f6c..4f9830c1b121f76c60a95203eb4ea906999b97f3 100644 (file)
--- a/drivers/clk/bcm/clk-bcm2835.c
+++ b/drivers/clk/bcm/clk-bcm2835.c
@@ -1097,13 +1097,15 @@ static int bcm2835_pll_divider_set_rate(struct clk_hw *hw,
        struct bcm2835_pll_divider *divider = bcm2835_pll_divider_from_hw(hw);
        struct bcm2835_cprman *cprman = divider->cprman;
        const struct bcm2835_pll_divider_data *data = divider->data;
-       u32 cm;
-       int ret;
+       u32 cm, div, max_div = 1 << A2W_PLL_DIV_BITS;
 
-       ret = clk_divider_ops.set_rate(hw, rate, parent_rate);
-       if (ret)
-               return ret;
+       div = DIV_ROUND_UP_ULL(parent_rate, rate);
+
+       div = min(div, max_div);
+       if (div == max_div)
+               div = 0;
 
+       cprman_write(cprman, data->a2w_reg, div);
        cm = cprman_read(cprman, data->cm_reg);
        cprman_write(cprman, data->cm_reg, cm | data->load_mask);
        cprman_write(cprman, data->cm_reg, cm & ~data->load_mask);