rk3168/rk3066b: force dclk_lcdc0/1 even div

diff --git a/arch/arm/mach-rk30/clock_data-rk3066b.c b/arch/arm/mach-rk30/clock_data-rk3066b.c
index 5f0365892201ee4871ce8d5765f803a806ca1ccc..d7b6ce57e261fc3697c9acacb896065db3af11e5 100644 (file)
--- a/arch/arm/mach-rk30/clock_data-rk3066b.c
+++ b/arch/arm/mach-rk30/clock_data-rk3066b.c
@@ -313,6 +313,19 @@ static int clksel_set_rate_even(struct clk *clk, unsigned long rate)
        return -ENOENT;
 }
 
+static u32 clk_get_evendiv(unsigned long rate_out, unsigned long rate , u32 div_max)
+{
+       u32 div;
+       unsigned long new_rate;
+       for (div = 1; div < div_max; div += 2) {
+               new_rate = rate / (div + 1);
+               if (new_rate <= rate_out) {
+                       return div + 1;
+               }
+       }
+       return div_max ? div_max : 1;
+}
+
 static u32 clk_get_freediv(unsigned long rate_out, unsigned long rate , u32 div_max)
 {
        u32 div;
@@ -325,6 +338,30 @@ static u32 clk_get_freediv(unsigned long rate_out, unsigned long rate , u32 div_
        }
        return div_max ? div_max : 1;
 }
+
+struct clk *get_evendiv_parents_div(struct clk *clk, unsigned long rate, u32 *div_out) {
+       u32 div[2] = {0, 0};
+       unsigned long new_rate[2] = {0, 0};
+       u32 i;
+
+       if(clk->rate == rate)
+               return clk->parent;
+       for(i = 0; i < 2; i++) {
+               div[i] = clk_get_evendiv(rate, clk->parents[i]->rate, clk->div_max);
+               new_rate[i] = clk->parents[i]->rate / div[i];
+               if(new_rate[i] == rate) {
+                       *div_out = div[i];
+                       return clk->parents[i];
+               }
+       }
+       if(new_rate[0] < new_rate[1])
+               i = 1;
+       else
+               i = 0;
+       *div_out = div[i];
+       return clk->parents[i];
+}
+
 struct clk *get_freediv_parents_div(struct clk *clk, unsigned long rate, u32 *div_out) {
        u32 div[2] = {0, 0};
        unsigned long new_rate[2] = {0, 0};
@@ -348,6 +385,36 @@ struct clk *get_freediv_parents_div(struct clk *clk, unsigned long rate, u32 *di
        return clk->parents[i];
 }
 
+static int clkset_rate_evendiv_autosel_parents(struct clk *clk, unsigned long rate)
+{
+       struct clk *p_clk;
+       u32 div, old_div;
+       int ret = 0;
+       if(clk->rate == rate)
+               return 0;
+       p_clk = get_evendiv_parents_div(clk, rate, &div);
+
+       if(!p_clk)
+               return -ENOENT;
+
+       CLKDATA_DBG("%s %lu,form %s\n", clk->name, rate, p_clk->name);
+       if (clk->parent != p_clk) {
+               old_div = CRU_GET_REG_BITS_VAL(cru_readl(clk->clksel_con), clk->div_shift, clk->div_mask) + 1;
+
+               if(div > old_div) {
+                       set_cru_bits_w_msk(div - 1, clk->div_mask, clk->div_shift, clk->clksel_con);
+               }
+               ret = clk_set_parent_nolock(clk, p_clk);
+               if(ret) {
+                       CLKDATA_ERR("%s can't set %lu,reparent err\n", clk->name, rate);
+                       return -ENOENT;
+               }
+       }
+       //set div
+       set_cru_bits_w_msk(div - 1, clk->div_mask, clk->div_shift, clk->clksel_con);
+       return 0;
+}
+
 static int clkset_rate_freediv_autosel_parents(struct clk *clk, unsigned long rate)
 {
        struct clk *p_clk;
@@ -1601,7 +1668,7 @@ static struct clk *dclk_lcdc0_parents[2] = {&codec_pll_clk, &general_pll_clk};
 static struct clk dclk_lcdc0 = {
        .name           = "dclk_lcdc0",
        .mode           = gate_mode,
-       .set_rate       = clkset_rate_freediv_autosel_parents,
+       .set_rate       = clkset_rate_evendiv_autosel_parents,
        .recalc         = clksel_recalc_div,
        .gate_idx       = CLK_GATE_DCLK_LCDC0_SRC,
        .clksel_con     = CRU_CLKSELS_CON(27),
@@ -1614,7 +1681,7 @@ static struct clk *dclk_lcdc1_parents[2] = {&codec_pll_clk, &general_pll_clk};
 static struct clk dclk_lcdc1 = {
        .name           = "dclk_lcdc1",
        .mode           = gate_mode,
-       .set_rate       = clkset_rate_freediv_autosel_parents,
+       .set_rate       = clkset_rate_evendiv_autosel_parents,
        .recalc         = clksel_recalc_div,
        .gate_idx       = CLK_GATE_DCLK_LCDC1_SRC,
        .clksel_con     = CRU_CLKSELS_CON(28),