1 files changed, 62 insertions, 2 deletions

diff --git a/drivers/clk/sunxi-ng/ccu_mp.c b/drivers/clk/sunxi-ng/ccu_mp.c
index 5d0af4051737..0357349eb767 100644
--- a/drivers/clk/sunxi-ng/ccu_mp.c
+++ b/drivers/clk/sunxi-ng/ccu_mp.c
@@ -40,6 +40,61 @@ static void ccu_mp_find_best(unsigned long parent, unsigned long rate,
 	*p = best_p;
 }
 
+static unsigned long ccu_mp_find_best_with_parent_adj(struct clk_hw *hw,
+						      unsigned long *parent,
+						      unsigned long rate,
+						      unsigned int max_m,
+						      unsigned int max_p)
+{
+	unsigned long parent_rate_saved;
+	unsigned long parent_rate, now;
+	unsigned long best_rate = 0;
+	unsigned int _m, _p, div;
+	unsigned long maxdiv;
+
+	parent_rate_saved = *parent;
+
+	/*
+	 * The maximum divider we can use without overflowing
+	 * unsigned long in rate * m * p below
+	 */
+	maxdiv = max_m * max_p;
+	maxdiv = min(ULONG_MAX / rate, maxdiv);
+
+	for (_p = 1; _p <= max_p; _p <<= 1) {
+		for (_m = 1; _m <= max_m; _m++) {
+			div = _m * _p;
+
+			if (div > maxdiv)
+				break;
+
+			if (rate * div == parent_rate_saved) {
+				/*
+				 * It's the most ideal case if the requested
+				 * rate can be divided from parent clock without
+				 * needing to change parent rate, so return the
+				 * divider immediately.
+				 */
+				*parent = parent_rate_saved;
+				return rate;
+			}
+
+			parent_rate = clk_hw_round_rate(hw, rate * div);
+			now = parent_rate / div;
+
+			if (now <= rate && now > best_rate) {
+				best_rate = now;
+				*parent = parent_rate;
+
+				if (now == rate)
+					return rate;
+			}
+		}
+	}
+
+	return best_rate;
+}
+
 static unsigned long ccu_mp_round_rate(struct ccu_mux_internal *mux,
 				       struct clk_hw *hw,
 				       unsigned long *parent_rate,
@@ -56,8 +111,13 @@ static unsigned long ccu_mp_round_rate(struct ccu_mux_internal *mux,
 	max_m = cmp->m.max ?: 1 << cmp->m.width;
 	max_p = cmp->p.max ?: 1 << ((1 << cmp->p.width) - 1);
 
-	ccu_mp_find_best(*parent_rate, rate, max_m, max_p, &m, &p);
-	rate = *parent_rate / p / m;
+	if (!(clk_hw_get_flags(hw) & CLK_SET_RATE_PARENT)) {
+		ccu_mp_find_best(*parent_rate, rate, max_m, max_p, &m, &p);
+		rate = *parent_rate / p / m;
+	} else {
+		rate = ccu_mp_find_best_with_parent_adj(hw, parent_rate, rate,
+							max_m, max_p);
+	}
 
 	if (cmp->common.features & CCU_FEATURE_FIXED_POSTDIV)
 		rate /= cmp->fixed_post_div;