svn commit: r304796 - head/sys/arm/allwinner/clk

[Jared McNeill]

Author: jmcneill
Date: Thu Aug 25 10:27:22 2016
New Revision: 304796
URL: https://svnweb.freebsd.org/changeset/base/304796

Log:
  Switch parent clock when setting frequency if a new parent is a better
  candidate for the target rate.
  
  Reviewed by:	andrew, manu

Modified:
  head/sys/arm/allwinner/clk/aw_modclk.c

Modified: head/sys/arm/allwinner/clk/aw_modclk.c
==============================================================================
--- head/sys/arm/allwinner/clk/aw_modclk.c	Thu Aug 25 10:24:14 2016	(r304795)
+++ head/sys/arm/allwinner/clk/aw_modclk.c	Thu Aug 25 10:27:22 2016	(r304796)
@@ -160,28 +160,47 @@ aw_modclk_set_freq(struct clknode *clk, 
     int flags, int *stop)
 {
 	struct aw_modclk_sc *sc;
-	uint32_t val, m, n, best_m, best_n;
+	uint32_t val, m, n, src, best_m, best_n, best_src;
 	uint64_t cur_freq;
 	int64_t best_diff, cur_diff;
+	int error;
 
 	sc = clknode_get_softc(clk);
 	best_n = best_m = 0;
 	best_diff = (int64_t)*fout; 
+	best_src = 0;
 
-	for (n = 0; n <= CLK_RATIO_N_MAX; n++)
-		for (m = 0; m <= CLK_RATIO_M_MAX; m++) {
-			cur_freq = fin / (1 << n) / (m + 1);
-			cur_diff = (int64_t)*fout - cur_freq;
-			if (cur_diff >= 0 && cur_diff < best_diff) {
-				best_diff = cur_diff;
-				best_m = m;
-				best_n = n;
+	for (src = 0; src < CLK_SRC_SEL_MAX; src++) {
+		error = clknode_set_parent_by_idx(clk, src);
+		if (error != 0)
+			continue;
+		error = clknode_get_freq(clknode_get_parent(clk), &fin);
+		if (error != 0)
+			continue;
+
+		for (n = 0; n <= CLK_RATIO_N_MAX; n++)
+			for (m = 0; m <= CLK_RATIO_M_MAX; m++) {
+				cur_freq = fin / (1 << n) / (m + 1);
+				cur_diff = (int64_t)*fout - cur_freq;
+				if (cur_diff >= 0 && cur_diff < best_diff) {
+					best_src = src;
+					best_diff = cur_diff;
+					best_m = m;
+					best_n = n;
+				}
 			}
-		}
+	}
 
 	if (best_diff == (int64_t)*fout)
 		return (ERANGE);
 
+	error = clknode_set_parent_by_idx(clk, best_src);
+	if (error != 0)
+		return (error);
+	error = clknode_get_freq(clknode_get_parent(clk), &fin);
+	if (error != 0)
+		return (error);
+
 	DEVICE_LOCK(sc);
 	MODCLK_READ(sc, &val);
 	val &= ~(CLK_RATIO_N | CLK_RATIO_M);