[PATCH 5/6] clk: composite: pick best parent for round_rate / set_rate

[Sascha Hauer <s.hauer@pengutronix.de>]

From: Sascha Hauer <sascha@saschahauer.de>

Currently our clk composite driver uses the set_rate hook to set the rate
when available, otherwise it uses the mux to set the rate, but it never
tries to find the best combination of both. This patch fills the gap.
Doing this is necessary for example on Rockchip RK3588 where the clock
is adjustable only external to the SDHC controller. The knobs are a
6bit divider and a mux. The divider alone is not sufficient to scale a
high frequency down to the necessary 400kHz init clock while the 24MHz
clock is not high enough for faster speeds.

Walk all parents and pick the one whose rate-clock gets closest to
the requested rate, then reparent if needed before applying the
divider.

For mux-only, rate-only and clocks with CLK_SET_RATE_NO_REPARENT the
previous behaviour is preserved.

Assisted-by: Claude Opus 4.7 <noreply@anthropic.com>
Signed-off-by: Sascha Hauer <s.hauer@pengutronix.de>
---
 drivers/clk/clk-composite.c | 110 ++++++++++++++++++++++++++++++++++++++------
 1 file changed, 95 insertions(+), 15 deletions(-)

diff --git a/drivers/clk/clk-composite.c b/drivers/clk/clk-composite.c
index e8f1fa7a72..a23e828f58 100644
--- a/drivers/clk/clk-composite.c
+++ b/drivers/clk/clk-composite.c
@@ -39,16 +39,91 @@ static unsigned long clk_composite_recalc_rate(struct clk_hw *hw,
 	return parent_rate;
 }
 
+/*
+ * Walk all parents of a (rate_hw + mux_hw) composite and pick the one
+ * whose rate-clock can get closest to the requested rate. Returns the
+ * chosen parent's index in *out_idx (or -1 to mean "stay on current parent"),
+ * the parent's rate in *parent_rate, and the achievable output rate as the
+ * function value.
+ * Falls back to round_rate on the current parent when reparenting
+ * isn't possible (no mux, or CLK_SET_RATE_NO_REPARENT).
+ */
+static long clk_composite_pick_parent(struct clk_hw *hw, unsigned long rate,
+				      int *out_idx, unsigned long *parent_rate)
+{
+	struct clk_composite *composite = to_clk_composite(hw);
+	struct clk_hw *rate_hw = composite->rate_hw;
+	struct clk_hw *mux_hw = composite->mux_hw;
+	unsigned long best_rate = 0, best_prate = 0, best_diff = ULONG_MAX;
+	int best_idx = -1;
+	int i;
+
+	if (!rate_hw || !rate_hw->clk.ops->round_rate)
+		return -ENOSYS;
+
+	if (!mux_hw || (hw->clk.flags & CLK_SET_RATE_NO_REPARENT)) {
+		unsigned long prate = *parent_rate;
+		long achievable;
+
+		achievable = rate_hw->clk.ops->round_rate(rate_hw, rate, &prate);
+		if (achievable < 0)
+			return achievable;
+
+		*out_idx = -1;
+		*parent_rate = prate;
+		return achievable;
+	}
+
+	for (i = 0; i < hw->clk.num_parents; i++) {
+		struct clk_hw *p_hw = clk_hw_get_parent_by_index(hw, i);
+		unsigned long prate, diff;
+		long achievable;
+
+		if (!p_hw)
+			continue;
+
+		prate = clk_hw_get_rate(p_hw);
+		achievable = rate_hw->clk.ops->round_rate(rate_hw, rate, &prate);
+		if (achievable < 0)
+			continue;
+
+		diff = (achievable >= rate) ? achievable - rate
+					    : rate - achievable;
+
+		if (diff < best_diff) {
+			best_idx = i;
+			best_prate = prate;
+			best_rate = achievable;
+			best_diff = diff;
+			if (!diff)
+				break;
+		}
+	}
+
+	if (best_idx < 0)
+		return -EINVAL;
+
+	*out_idx = best_idx;
+	*parent_rate = best_prate;
+	return best_rate;
+}
+
 static long clk_composite_round_rate(struct clk_hw *hw, unsigned long rate,
 				  unsigned long *prate)
 {
 	struct clk_composite *composite = to_clk_composite(hw);
-	struct clk_hw *rate_hw = composite->rate_hw;
 	struct clk_hw *mux_hw = composite->mux_hw;
+	int idx;
+	long achievable;
 
-	if (rate_hw)
-		return rate_hw->clk.ops->round_rate(rate_hw, rate, prate);
+	achievable = clk_composite_pick_parent(hw, rate, &idx, prate);
+	if (achievable >= 0)
+		return achievable;
+
+	if (achievable != -ENOSYS)
+		return achievable;
 
+	/* No rate_hw — fall back to mux's round_rate if available. */
 	if (!(hw->clk.flags & CLK_SET_RATE_NO_REPARENT) &&
 	    mux_hw &&
 	    mux_hw->clk.ops->round_rate)
@@ -63,24 +138,29 @@ static int clk_composite_set_rate(struct clk_hw *hw, unsigned long rate,
 	struct clk_composite *composite = to_clk_composite(hw);
 	struct clk_hw *rate_hw = composite->rate_hw;
 	struct clk_hw *mux_hw = composite->mux_hw;
+	int idx = -1;
+	long achievable;
+
+	achievable = clk_composite_pick_parent(hw, rate, &idx, &parent_rate);
+	if (achievable >= 0) {
+		if (idx >= 0 && mux_hw && mux_hw->clk.ops->set_parent) {
+			int ret = mux_hw->clk.ops->set_parent(mux_hw, idx);
+			if (ret)
+				return ret;
+		}
+		return rate_hw->clk.ops->set_rate(rate_hw, rate, parent_rate);
+	}
+
+	if (achievable != -ENOSYS)
+		return achievable;
 
 	/*
-	 * When the rate clock is present use that to set the rate,
-	 * otherwise try the mux clock. We currently do not support
-	 * to find the best rate using a combination of both.
+	 * No rate_hw. Fall back to letting the mux clk reparent itself,
+	 * preserving the existing enable-count handoff.
 	 */
-	if (rate_hw)
-		return rate_hw->clk.ops->set_rate(rate_hw, rate, parent_rate);
-
 	if (!(hw->clk.flags & CLK_SET_RATE_NO_REPARENT) &&
 	    mux_hw &&
 	    mux_hw->clk.ops->set_rate) {
-		/*
-		 * We'll call set_rate on the mux clk which in turn results
-		 * in reparenting the mux clk. Make sure the enable count
-		 * (which is stored in the composite clk, not the mux clk)
-		 * is transferred correctly.
-		 */
 		mux_hw->clk.enable_count = hw->clk.enable_count;
 		return mux_hw->clk.ops->set_rate(mux_hw, rate, parent_rate);
 	}

-- 
2.47.3