diff --git a/drivers/clk/rockchip/clk-cpu.c b/drivers/clk/rockchip/clk-cpu.c index 34123f6888b7..3293174a62c6 100644 --- a/drivers/clk/rockchip/clk-cpu.c +++ b/drivers/clk/rockchip/clk-cpu.c @@ -118,6 +118,42 @@ static void rockchip_cpuclk_set_dividers(struct rockchip_cpuclk *cpuclk, } } +static void rockchip_cpuclk_set_pre_muxs(struct rockchip_cpuclk *cpuclk, + const struct rockchip_cpuclk_rate_table *rate) +{ + int i; + + /* alternate parent is active now. set the pre_muxs */ + for (i = 0; i < ARRAY_SIZE(rate->pre_muxs); i++) { + const struct rockchip_cpuclk_clksel *clksel = &rate->pre_muxs[i]; + + if (!clksel->reg) + break; + + pr_debug("%s: setting reg 0x%x to 0x%x\n", + __func__, clksel->reg, clksel->val); + writel(clksel->val, cpuclk->reg_base + clksel->reg); + } +} + +static void rockchip_cpuclk_set_post_muxs(struct rockchip_cpuclk *cpuclk, + const struct rockchip_cpuclk_rate_table *rate) +{ + int i; + + /* alternate parent is active now. set the muxs */ + for (i = 0; i < ARRAY_SIZE(rate->post_muxs); i++) { + const struct rockchip_cpuclk_clksel *clksel = &rate->post_muxs[i]; + + if (!clksel->reg) + break; + + pr_debug("%s: setting reg 0x%x to 0x%x\n", + __func__, clksel->reg, clksel->val); + writel(clksel->val, cpuclk->reg_base + clksel->reg); + } +} + static int rockchip_cpuclk_pre_rate_change(struct rockchip_cpuclk *cpuclk, struct clk_notifier_data *ndata) { @@ -175,6 +211,8 @@ static int rockchip_cpuclk_pre_rate_change(struct rockchip_cpuclk *cpuclk, rockchip_boost_add_core_div(cpuclk->pll_hw, alt_prate); + rockchip_cpuclk_set_pre_muxs(cpuclk, rate); + /* select alternate parent */ if (reg_data->mux_core_reg) writel(HIWORD_UPDATE(reg_data->mux_core_alt, @@ -229,6 +267,8 @@ static int rockchip_cpuclk_post_rate_change(struct rockchip_cpuclk *cpuclk, reg_data->mux_core_shift), cpuclk->reg_base + reg_data->core_reg[0]); + rockchip_cpuclk_set_post_muxs(cpuclk, rate); + /* remove dividers */ for (i = 0; i < reg_data->num_cores; i++) { writel(HIWORD_UPDATE(0, reg_data->div_core_mask[i], diff --git a/drivers/clk/rockchip/clk.h b/drivers/clk/rockchip/clk.h index 6a58866266a7..73dcc088c680 100644 --- a/drivers/clk/rockchip/clk.h +++ b/drivers/clk/rockchip/clk.h @@ -447,6 +447,8 @@ struct rockchip_cpuclk_clksel { struct rockchip_cpuclk_rate_table { unsigned long prate; struct rockchip_cpuclk_clksel divs[ROCKCHIP_CPUCLK_NUM_DIVIDERS]; + struct rockchip_cpuclk_clksel pre_muxs[ROCKCHIP_CPUCLK_NUM_DIVIDERS]; + struct rockchip_cpuclk_clksel post_muxs[ROCKCHIP_CPUCLK_NUM_DIVIDERS]; }; /**