--- zzzz-none-000/linux-4.4.60/drivers/clk/clk.c	2017-04-08 07:53:53.000000000 +0000
+++ jet-2400-727/linux-4.4.60/drivers/clk/clk.c	2021-03-17 14:36:41.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;
 }
 
@@ -2298,7 +2360,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;
@@ -2306,6 +2368,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)) {
@@ -2466,8 +2529,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)