1 files changed, 40 insertions, 18 deletions

diff --git a/sys/dev/cpufreq/cpufreq_dt.c b/sys/dev/cpufreq/cpufreq_dt.c
index 27f70c910e49..21f14ccea5ad 100644
--- a/sys/dev/cpufreq/cpufreq_dt.c
+++ b/sys/dev/cpufreq/cpufreq_dt.c
@@ -166,7 +166,7 @@ cpufreq_dt_set(device_t dev, const struct cf_setting *set)
 	struct cpufreq_dt_softc *sc;
 	const struct cpufreq_dt_opp *opp, *copp;
 	uint64_t freq;
-	int error = 0;
+	int uvolt, error;
 
 	sc = device_get_softc(dev);
 
@@ -174,23 +174,38 @@ cpufreq_dt_set(device_t dev, const struct cf_setting *set)
 		device_printf(dev, "Can't get current clk freq\n");
 		return (ENXIO);
 	}
+	/* Try to get current valtage by using regulator first. */
+	error = regulator_get_voltage(sc->reg, &uvolt);
+	if (error != 0) {
+		/*
+		 * Try oppoints table as backup way. However,
+		 * this is insufficient because the actual processor
+		 * frequency may not be in the table. PLL frequency
+		 * granularity can be different that granularity of
+		 * oppoint table.
+		 */
+		copp = cpufreq_dt_find_opp(sc->dev, freq);
+		if (copp == NULL) {
+			device_printf(dev,
+			    "Can't find the current freq in opp\n");
+			return (ENOENT);
+		}
+		uvolt = copp->uvolt_target;
 
-	DEBUG(sc->dev, "Current freq %ju\n", freq);
-	DEBUG(sc->dev, "Target freq %ju\n", (uint64_t)set->freq * 1000000);
-	copp = cpufreq_dt_find_opp(sc->dev, freq);
-	if (copp == NULL) {
-		device_printf(dev, "Can't find the current freq in opp\n");
-		return (ENOENT);
 	}
+
 	opp = cpufreq_dt_find_opp(sc->dev, set->freq * 1000000);
 	if (opp == NULL) {
 		device_printf(dev, "Couldn't find an opp for this freq\n");
 		return (EINVAL);
 	}
+	DEBUG(sc->dev, "Current freq %ju, uvolt: %d\n", freq, uvolt);
+	DEBUG(sc->dev, "Target freq %ju, , uvolt: %d\n",
+	    opp->freq, opp->uvolt_target);
 
-	if (copp->uvolt_target < opp->uvolt_target) {
+	if (uvolt < opp->uvolt_target) {
 		DEBUG(dev, "Changing regulator from %u to %u\n",
-		    copp->uvolt_target, opp->uvolt_target);
+		    uvolt, opp->uvolt_target);
 		error = regulator_set_voltage(sc->reg,
 		    opp->uvolt_min,
 		    opp->uvolt_max);
@@ -201,7 +216,7 @@ cpufreq_dt_set(device_t dev, const struct cf_setting *set)
 	}
 
 	DEBUG(dev, "Setting clk to %ju\n", opp->freq);
-	error = clk_set_freq(sc->clk, opp->freq, 0);
+	error = clk_set_freq(sc->clk, opp->freq, CLK_SET_ROUND_DOWN);
 	if (error != 0) {
 		DEBUG(dev, "Failed, backout\n");
 		/* Restore previous voltage (best effort) */
@@ -211,7 +226,9 @@ cpufreq_dt_set(device_t dev, const struct cf_setting *set)
 		return (ENXIO);
 	}
 
-	if (copp->uvolt_target > opp->uvolt_target) {
+	if (uvolt > opp->uvolt_target) {
+		DEBUG(dev, "Changing regulator from %u to %u\n",
+		    uvolt, opp->uvolt_target);
 		error = regulator_set_voltage(sc->reg,
 		    opp->uvolt_min,
 		    opp->uvolt_max);
@@ -219,8 +236,7 @@ cpufreq_dt_set(device_t dev, const struct cf_setting *set)
 			DEBUG(dev, "Failed to switch regulator to %d\n",
 			    opp->uvolt_target);
 			/* Restore previous CPU frequency (best effort) */
-			(void)clk_set_freq(sc->clk,
-			    copp->freq, 0);
+			(void)clk_set_freq(sc->clk, copp->freq, 0);
 			return (ENXIO);
 		}
 	}
@@ -277,7 +293,8 @@ cpufreq_dt_identify(driver_t *driver, device_t parent)
 
 	/* The cpu@0 node must have the following properties */
 	if (!OF_hasprop(node, "clocks") ||
-	    !OF_hasprop(node, "cpu-supply"))
+	    (!OF_hasprop(node, "cpu-supply") &&
+	    !OF_hasprop(node, "cpu0-supply")))
 		return;
 
 	if (!OF_hasprop(node, "operating-points") &&
@@ -299,7 +316,9 @@ cpufreq_dt_probe(device_t dev)
 	node = ofw_bus_get_node(device_get_parent(dev));
 
 	if (!OF_hasprop(node, "clocks") ||
-	    !OF_hasprop(node, "cpu-supply"))
+	    (!OF_hasprop(node, "cpu-supply") &&
+	    !OF_hasprop(node, "cpu0-supply")))
+
 		return (ENXIO);
 
 	if (!OF_hasprop(node, "operating-points") &&
@@ -439,9 +458,12 @@ cpufreq_dt_attach(device_t dev)
 
 	if (regulator_get_by_ofw_property(dev, node,
 	    "cpu-supply", &sc->reg) != 0) {
-		device_printf(dev, "no regulator for %s\n",
-		    ofw_bus_get_name(device_get_parent(dev)));
-		return (ENXIO);
+		if (regulator_get_by_ofw_property(dev, node,
+		    "cpu0-supply", &sc->reg) != 0) {
+			device_printf(dev, "no regulator for %s\n",
+			    ofw_bus_get_name(device_get_parent(dev)));
+			return (ENXIO);
+		}
 	}
 
 	if (clk_get_by_ofw_index(dev, node, 0, &sc->clk) != 0) {