clk: mmp: add init callback for clk-frac

For the clk-frac, we need to make sure that the initial
clock rate is one item of the table.
If it is not, we use the first item in the table by default.

Signed-off-by: Chao Xie <chao.xie@marvell.com>
Acked-by: Haojian Zhuang <haojian.zhuang@gmail.com>
Signed-off-by: Michael Turquette <mturquette@linaro.org>

diff --git a/drivers/clk/mmp/clk-frac.c b/drivers/clk/mmp/clk-frac.c
index e29d006ab85e9d39cfbda5cdc86bf7cf6e3ee9e1..1876d2c94bd33987a7100776b778993271b76871 100644 (file)
--- a/drivers/clk/mmp/clk-frac.c
+++ b/drivers/clk/mmp/clk-frac.c
@@ -118,10 +118,50 @@ static int clk_factor_set_rate(struct clk_hw *hw, unsigned long drate,
        return 0;
 }
 
+void clk_factor_init(struct clk_hw *hw)
+{
+       struct mmp_clk_factor *factor = to_clk_factor(hw);
+       struct mmp_clk_factor_masks *masks = factor->masks;
+       u32 val, num, den;
+       int i;
+       unsigned long flags = 0;
+
+       if (factor->lock)
+               spin_lock_irqsave(factor->lock, flags);
+
+       val = readl(factor->base);
+
+       /* calculate numerator */
+       num = (val >> masks->num_shift) & masks->num_mask;
+
+       /* calculate denominator */
+       den = (val >> masks->den_shift) & masks->den_mask;
+
+       for (i = 0; i < factor->ftbl_cnt; i++)
+               if (den == factor->ftbl[i].den && num == factor->ftbl[i].num)
+                       break;
+
+       if (i >= factor->ftbl_cnt) {
+               val &= ~(masks->num_mask << masks->num_shift);
+               val |= (factor->ftbl[0].num & masks->num_mask) <<
+                       masks->num_shift;
+
+               val &= ~(masks->den_mask << masks->den_shift);
+               val |= (factor->ftbl[0].den & masks->den_mask) <<
+                       masks->den_shift;
+
+               writel(val, factor->base);
+       }
+
+       if (factor->lock)
+               spin_unlock_irqrestore(factor->lock, flags);
+}
+
 static struct clk_ops clk_factor_ops = {
        .recalc_rate = clk_factor_recalc_rate,
        .round_rate = clk_factor_round_rate,
        .set_rate = clk_factor_set_rate,
+       .init = clk_factor_init,
 };
 
 struct clk *mmp_clk_register_factor(const char *name, const char *parent_name,