--- zzzz-none-000/linux-4.4.271/drivers/clk/clk.c 2021-06-03 06:22:09.000000000 +0000 +++ maple-fsgw-759/linux-4.4.271/drivers/clk/clk.c 2023-12-20 10:37:06.000000000 +0000 @@ -51,9 +51,12 @@ struct clk_core **parents; u8 num_parents; u8 new_parent_index; + u8 safe_parent_index; unsigned long rate; unsigned long req_rate; + unsigned long old_rate; unsigned long new_rate; + struct clk_core *safe_parent; struct clk_core *new_parent; struct clk_core *new_child; unsigned long flags; @@ -575,6 +578,9 @@ if (WARN_ON(core->prepare_count == 0)) return; + if (WARN_ON(core->prepare_count == 1 && core->flags & CLK_IS_CRITICAL)) + return; + if (--core->prepare_count > 0) return; @@ -680,11 +686,16 @@ if (WARN_ON(core->enable_count == 0)) return; + if (WARN_ON(core->enable_count == 1 && core->flags & CLK_IS_CRITICAL)) + return; + if (--core->enable_count > 0) return; trace_clk_disable(core); + pr_debug("%s: %s\n", __func__, core->name); + if (core->ops->disable) core->ops->disable(core->hw); @@ -738,6 +749,8 @@ trace_clk_enable(core); + pr_debug("%s: %s\n", __func__, core->name); + if (core->ops->enable) ret = core->ops->enable(core->hw); @@ -1203,6 +1216,9 @@ trace_clk_set_parent(core, parent); + pr_debug("%s: clock %s reparented from %s to %s\n", + __func__, core->name, core->parent->name, parent->name); + /* change clock input source */ if (parent && core->ops->set_parent) ret = core->ops->set_parent(core->hw, p_index); @@ -1271,7 +1287,8 @@ static void clk_calc_subtree(struct clk_core *core, unsigned long new_rate, struct clk_core *new_parent, u8 p_index) { - struct clk_core *child; + struct clk_core *child, *parent; + struct clk_hw *parent_hw; core->new_rate = new_rate; core->new_parent = new_parent; @@ -1281,6 +1298,19 @@ if (new_parent && new_parent != core->parent) new_parent->new_child = core; + if (core->ops->get_safe_parent) { + parent_hw = core->ops->get_safe_parent(core->hw); + if (parent_hw) { + parent = parent_hw->core; + p_index = clk_fetch_parent_index(core, parent); + core->safe_parent_index = p_index; + core->safe_parent = parent; + } + } else { + core->safe_parent = NULL; + } + + hlist_for_each_entry(child, &core->children, child_node) { child->new_rate = clk_recalc(child, new_rate); clk_calc_subtree(child, child->new_rate, NULL, 0); @@ -1393,14 +1423,43 @@ unsigned long event) { struct clk_core *child, *tmp_clk, *fail_clk = NULL; + struct clk_core *old_parent; int ret = NOTIFY_DONE; - if (core->rate == core->new_rate) + if (core->rate == core->new_rate && event != POST_RATE_CHANGE) return NULL; + switch (event) { + case PRE_RATE_CHANGE: + if (core->safe_parent) + core->ops->set_parent(core->hw, core->safe_parent_index); + core->old_rate = core->rate; + break; + case POST_RATE_CHANGE: + if (core->safe_parent) { + old_parent = __clk_set_parent_before(core, + core->new_parent); + if (core->ops->set_rate_and_parent) { + core->ops->set_rate_and_parent(core->hw, + core->new_rate, + core->new_parent ? + core->new_parent->rate : 0, + core->new_parent_index); + } else if (core->ops->set_parent) { + core->ops->set_parent(core->hw, + core->new_parent_index); + } + __clk_set_parent_after(core, core->new_parent, + old_parent); + } + break; + } + if (core->notifier_count) { - ret = __clk_notify(core, event, core->rate, core->new_rate); - if (ret & NOTIFY_STOP_MASK) + if (event != POST_RATE_CHANGE || core->old_rate != core->rate) + ret = __clk_notify(core, event, core->old_rate, + core->new_rate); + if (ret & NOTIFY_STOP_MASK && event != POST_RATE_CHANGE) fail_clk = core; } @@ -1443,7 +1502,8 @@ else if (core->parent) best_parent_rate = core->parent->rate; - if (core->new_parent && core->new_parent != core->parent) { + if (core->new_parent && core->new_parent != core->parent && + !core->safe_parent) { old_parent = __clk_set_parent_before(core, core->new_parent); trace_clk_set_parent(core, core->new_parent); @@ -1462,6 +1522,9 @@ trace_clk_set_rate(core, core->new_rate); + pr_debug("%s: frequency for clock %s changed from %lu to %lu\n", + __func__, core->name, old_rate, core->new_rate); + if (!skip_set_rate && core->ops->set_rate) core->ops->set_rate(core->hw, core->new_rate, best_parent_rate); @@ -1469,9 +1532,6 @@ core->rate = clk_recalc(core, best_parent_rate); - if (core->notifier_count && old_rate != core->rate) - __clk_notify(core, POST_RATE_CHANGE, old_rate, core->rate); - if (core->flags & CLK_RECALC_NEW_RATES) (void)clk_calc_new_rates(core, core->new_rate); @@ -1527,6 +1587,8 @@ core->req_rate = req_rate; + clk_propagate_rate_change(top, POST_RATE_CHANGE); + return ret; } @@ -2301,7 +2363,7 @@ struct clk_core *orphan; struct hlist_node *tmp2; struct clk_core *core; - unsigned long rate; + unsigned long rate, enable_flags; if (!clk_user) return -EINVAL; @@ -2309,6 +2371,7 @@ core = clk_user->core; clk_prepare_lock(); + enable_flags = clk_enable_lock(); /* check to see if a clock with this name is already registered */ if (clk_core_lookup(core->name)) { @@ -2469,8 +2532,14 @@ if (core->ops->init) core->ops->init(core->hw); + if (core->flags & CLK_IS_CRITICAL) { + clk_core_prepare(core); + clk_core_enable(core); + } + kref_init(&core->ref); out: + clk_enable_unlock(enable_flags); clk_prepare_unlock(); if (!ret) @@ -2821,7 +2890,7 @@ */ int clk_notifier_register(struct clk *clk, struct notifier_block *nb) { - struct clk_notifier *cn; + struct clk_notifier *tmp, *cn = NULL; int ret = -ENOMEM; if (!clk || !nb) @@ -2830,12 +2899,14 @@ clk_prepare_lock(); /* search the list of notifiers for this clk */ - list_for_each_entry(cn, &clk_notifier_list, node) - if (cn->clk == clk) + list_for_each_entry(tmp, &clk_notifier_list, node) + if (tmp->clk == clk) { + cn = tmp; break; + } /* if clk wasn't in the notifier list, allocate new clk_notifier */ - if (cn->clk != clk) { + if (!cn) { cn = kzalloc(sizeof(struct clk_notifier), GFP_KERNEL); if (!cn) goto out;