From 145396ebace50355e19ebe26d491713bbc635bd8 Mon Sep 17 00:00:00 2001 From: chenxing Date: Tue, 5 Mar 2013 18:25:53 +0800 Subject: [PATCH] rk3168/rk3066b: force dclk_lcdc0/1 even div --- arch/arm/mach-rk30/clock_data-rk3066b.c | 71 ++++++++++++++++++++++++- 1 file changed, 69 insertions(+), 2 deletions(-) diff --git a/arch/arm/mach-rk30/clock_data-rk3066b.c b/arch/arm/mach-rk30/clock_data-rk3066b.c index 5f0365892201..d7b6ce57e261 100644 --- 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), -- 2.34.1