diff --git a/drivers/clk/qcom/clk-rcg2.c b/drivers/clk/qcom/clk-rcg2.c index ed782714bd61..a77b9712417b 100644 --- a/drivers/clk/qcom/clk-rcg2.c +++ b/drivers/clk/qcom/clk-rcg2.c @@ -69,6 +69,16 @@ static struct freq_tbl cxo_f = { .n = 0, }; +static void update_src_map(struct clk_hw *hw) +{ + struct clk_rcg2 *rcg = to_clk_rcg2(hw); + int i, num_parents = clk_hw_get_num_parents(hw); + + for (i = 0; i < num_parents; i++) + if (!rcg->parent_map[i].cfg) + cxo_f.src = rcg->parent_map[i].src; +} + static int clk_rcg2_is_enabled(struct clk_hw *hw) { struct clk_rcg2 *rcg = to_clk_rcg2(hw); @@ -246,17 +256,20 @@ clk_rcg2_recalc_rate(struct clk_hw *hw, unsigned long parent_rate) { struct clk_rcg2 *rcg = to_clk_rcg2(hw); const struct freq_tbl *f_curr; - u32 cfg, hid_div, m = 0, n = 0, mode = 0, mask; + u32 cfg, src, hid_div, m = 0, n = 0, mode = 0, mask; + + regmap_read(rcg->clkr.regmap, RCG_CFG_OFFSET(rcg), &cfg); + src = cfg; + src &= CFG_SRC_SEL_MASK; + src >>= CFG_SRC_SEL_SHIFT; if (rcg->enable_safe_config && (!clk_hw_is_prepared(hw) - || !clk_hw_is_enabled(hw))) { + || !clk_hw_is_enabled(hw)) && !src) { if (!rcg->current_freq) rcg->current_freq = cxo_f.freq; return rcg->current_freq; } - regmap_read(rcg->clkr.regmap, RCG_CFG_OFFSET(rcg), &cfg); - if (rcg->mnd_width) { mask = BIT(rcg->mnd_width) - 1; regmap_read(rcg->clkr.regmap, RCG_M_OFFSET(rcg), &m); @@ -269,7 +282,7 @@ clk_rcg2_recalc_rate(struct clk_hw *hw, unsigned long parent_rate) mode >>= CFG_MODE_SHIFT; } - if (rcg->enable_safe_config) { + if (rcg->enable_safe_config && !src) { f_curr = qcom_find_freq(rcg->freq_tbl, rcg->current_freq); if (!f_curr) return -EINVAL; @@ -443,6 +456,12 @@ static int __clk_rcg2_set_rate(struct clk_hw *hw, unsigned long rate, if (!f) return -EINVAL; + /* + * Set the correct source value for CXO as per + * as per defined parent map. + */ + update_src_map(hw); + /* * Return if the RCG is currently disabled. This configuration update * will happen as part of the RCG enable sequence. @@ -603,6 +622,12 @@ static int clk_rcg2_enable(struct clk_hw *hw) unsigned long rate; const struct freq_tbl *f; + /* + * Set the correct source value for CXO as per + * as per defined parent map. + */ + update_src_map(hw); + if (rcg->flags & FORCE_ENABLE_RCG) { clk_rcg2_set_force_enable(hw); return 0; @@ -644,6 +669,12 @@ static void clk_rcg2_disable(struct clk_hw *hw) { struct clk_rcg2 *rcg = to_clk_rcg2(hw); + /* + * Set the correct source value for CXO as per + * as per defined parent map. + */ + update_src_map(hw); + if (rcg->flags & FORCE_ENABLE_RCG) { clk_rcg2_clear_force_enable(hw); return;