2 files changed, 70 insertions, 0 deletions

diff --git a/sys/dev/extres/regulator/regulator.c b/sys/dev/extres/regulator/regulator.c
index 53fe55b2bd0ec..82e8d066818a7 100644
--- a/sys/dev/extres/regulator/regulator.c
+++ b/sys/dev/extres/regulator/regulator.c
@@ -72,6 +72,7 @@ static int regnode_method_status(struct regnode *regnode, int *status);
 static int regnode_method_set_voltage(struct regnode *regnode, int min_uvolt,
     int max_uvolt, int *udelay);
 static int regnode_method_get_voltage(struct regnode *regnode, int *uvolt);
+static void regulator_constraint(void *dummy);
 static void regulator_shutdown(void *dummy);
 
 /*
@@ -154,9 +155,27 @@ SX_SYSINIT(regulator_topology, &regnode_topo_lock, "Regulator topology lock");
 #define REGNODE_XLOCK(_sc)	sx_xlock(&((_sc)->lock))
 #define REGNODE_UNLOCK(_sc)	sx_unlock(&((_sc)->lock))
 
+SYSINIT(regulator_constraint, SI_SUB_LAST, SI_ORDER_ANY, regulator_constraint,
+    NULL);
 SYSINIT(regulator_shutdown, SI_SUB_LAST, SI_ORDER_ANY, regulator_shutdown,
     NULL);
 
+static void
+regulator_constraint(void *dummy)
+{
+	struct regnode *entry;
+	int rv;
+
+	REG_TOPO_SLOCK();
+	TAILQ_FOREACH(entry, &regnode_list, reglist_link) {
+		rv = regnode_set_constraint(entry);
+		if (rv != 0 && bootverbose)
+			printf("regulator: setting constraint on %s failed (%d)\n",
+			    entry->name, rv);
+	}
+	REG_TOPO_UNLOCK();
+}
+
 /*
  * Disable unused regulator
  * We run this function at SI_SUB_LAST which mean that every driver that needs
@@ -780,6 +799,56 @@ regnode_set_voltage_checked(struct regnode *regnode, struct regulator *reg,
 	return (rv);
 }
 
+int
+regnode_set_constraint(struct regnode *regnode)
+{
+	int status, rv, uvolt;
+
+	if (regnode->std_param.boot_on != true &&
+	    regnode->std_param.always_on != true)
+		return (0);
+
+	rv = regnode_status(regnode, &status);
+	if (rv != 0) {
+		if (bootverbose)
+			printf("Cannot get regulator status for %s\n",
+			    regnode_get_name(regnode));
+		return (rv);
+	}
+
+	if (status == REGULATOR_STATUS_ENABLED)
+		return (0);
+
+	rv = regnode_get_voltage(regnode, &uvolt);
+	if (rv != 0) {
+		if (bootverbose)
+			printf("Cannot get regulator voltage for %s\n",
+			    regnode_get_name(regnode));
+		return (rv);
+	}
+
+	if (uvolt < regnode->std_param.min_uvolt ||
+	  uvolt > regnode->std_param.max_uvolt) {
+		if (bootverbose)
+			printf("Regulator %s current voltage %d is not in the"
+			    " acceptable range : %d<->%d\n",
+			    regnode_get_name(regnode),
+			    uvolt, regnode->std_param.min_uvolt,
+			    regnode->std_param.max_uvolt);
+		return (ERANGE);
+	}
+
+	rv = regnode_enable(regnode);
+	if (rv != 0) {
+		if (bootverbose)
+			printf("Cannot enable regulator %s\n",
+			    regnode_get_name(regnode));
+		return (rv);
+	}
+
+	return (0);
+}
+
 #ifdef FDT
 phandle_t
 regnode_get_ofw_node(struct regnode *regnode)
diff --git a/sys/dev/extres/regulator/regulator.h b/sys/dev/extres/regulator/regulator.h
index 7ef6524df35ac..b0bbb8a54b6ee 100644
--- a/sys/dev/extres/regulator/regulator.h
+++ b/sys/dev/extres/regulator/regulator.h
@@ -116,6 +116,7 @@ int regnode_stop(struct regnode *regnode, int depth);
 int regnode_status(struct regnode *regnode, int *status);
 int regnode_get_voltage(struct regnode *regnode, int *uvolt);
 int regnode_set_voltage(struct regnode *regnode, int min_uvolt, int max_uvolt);
+int regnode_set_constraint(struct regnode *regnode);
 #ifdef FDT
 phandle_t regnode_get_ofw_node(struct regnode *regnode);
 #endif