diff --git a/drivers/clk/qcom/clk-rcg.h b/drivers/clk/qcom/clk-rcg.h index 1b56484edfc0..d3eb87a12fb5 100644 --- a/drivers/clk/qcom/clk-rcg.h +++ b/drivers/clk/qcom/clk-rcg.h @@ -141,6 +141,7 @@ extern const struct clk_ops clk_dyn_rcg_ops; * @enable_safe_config: When set, the RCG is parked at CXO when it's disabled * @clkr: regmap clock handle * @cfg_off: defines the cfg register offset from the CMD_RCGR + CFG_REG + * @flags: additional flag parameters for the RCG */ struct clk_rcg2 { u32 cmd_rcgr; @@ -153,6 +154,8 @@ struct clk_rcg2 { bool enable_safe_config; struct clk_regmap clkr; u8 cfg_off; + u8 flags; +#define FORCE_ENABLE_RCG BIT(0) }; #define to_clk_rcg2(_hw) container_of(to_clk_regmap(_hw), struct clk_rcg2, clkr) diff --git a/drivers/clk/qcom/clk-rcg2.c b/drivers/clk/qcom/clk-rcg2.c index 29b32f18d07d..7530ab056aa6 100644 --- a/drivers/clk/qcom/clk-rcg2.c +++ b/drivers/clk/qcom/clk-rcg2.c @@ -175,6 +175,47 @@ static int clk_rcg2_clear_force_enable(struct clk_hw *hw) CMD_ROOT_EN, 0); } +static int prepare_enable_rcg_srcs(struct clk *curr, struct clk *new) +{ + int rc = 0; + + rc = clk_prepare(curr); + if (rc) + return rc; + + rc = clk_prepare(new); + if (rc) + goto err_new_src_prepare; + + rc = clk_enable(curr); + if (rc) + goto err_curr_src_enable; + + rc = clk_enable(new); + if (rc) + goto err_new_src_enable; + + return rc; + +err_new_src_enable: + clk_disable(curr); +err_curr_src_enable: + clk_unprepare(new); +err_new_src_prepare: + clk_unprepare(curr); + + return rc; +} + +static void disable_unprepare_rcg_srcs(struct clk *curr, struct clk *new) +{ + clk_disable(new); + clk_disable(curr); + + clk_unprepare(new); + clk_unprepare(curr); +} + /* * Calculate m/n:d rate * @@ -358,8 +399,9 @@ static int __clk_rcg2_set_rate(struct clk_hw *hw, unsigned long rate, enum freq_policy policy) { struct clk_rcg2 *rcg = to_clk_rcg2(hw); - const struct freq_tbl *f; - int ret; + const struct freq_tbl *f, *f_curr; + int ret, curr_src_index, new_src_index; + struct clk_hw *curr_src = NULL, *new_src = NULL; switch (policy) { case FLOOR: @@ -384,10 +426,44 @@ static int __clk_rcg2_set_rate(struct clk_hw *hw, unsigned long rate, return 0; } + if (rcg->flags & FORCE_ENABLE_RCG) { + rcg->current_freq = clk_get_rate(hw->clk); + if (rcg->current_freq == cxo_f.freq) + curr_src_index = 0; + else { + f_curr = qcom_find_freq(rcg->freq_tbl, + rcg->current_freq); + if (!f_curr) + return -EINVAL; + + curr_src_index = qcom_find_src_index(hw, + rcg->parent_map, f_curr->src); + } + + new_src_index = qcom_find_src_index(hw, rcg->parent_map, + f->src); + + curr_src = clk_hw_get_parent_by_index(hw, curr_src_index); + if (!curr_src) + return -EINVAL; + new_src = clk_hw_get_parent_by_index(hw, new_src_index); + if (!new_src) + return -EINVAL; + + /* The RCG could currently be disabled. Enable its parents. */ + ret = prepare_enable_rcg_srcs(curr_src->clk, new_src->clk); + clk_rcg2_set_force_enable(hw); + } + ret = clk_rcg2_configure(rcg, f); if (ret) return ret; + if (rcg->flags & FORCE_ENABLE_RCG) { + clk_rcg2_clear_force_enable(hw); + disable_unprepare_rcg_srcs(curr_src->clk, new_src->clk); + } + /* Update current frequency with the requested frequency. */ rcg->current_freq = rate; return ret; @@ -500,6 +576,11 @@ static int clk_rcg2_enable(struct clk_hw *hw) unsigned long rate; const struct freq_tbl *f; + if (rcg->flags & FORCE_ENABLE_RCG) { + clk_rcg2_set_force_enable(hw); + return 0; + } + if (!rcg->enable_safe_config) return 0; @@ -536,6 +617,11 @@ static void clk_rcg2_disable(struct clk_hw *hw) { struct clk_rcg2 *rcg = to_clk_rcg2(hw); + if (rcg->flags & FORCE_ENABLE_RCG) { + clk_rcg2_clear_force_enable(hw); + return; + } + if (!rcg->enable_safe_config) return; /*