[PATCH RFC v4 08/13] regulator: move monitor handling into own function

[Benjamin Bara <bbara93@gmail.com>]

From: Benjamin Bara <benjamin.bara@skidata.com>

Extract the current monitor handling into an own function and create
helper for initialization, disabling and re-enabling of monitors.
For reenabling the monitors, the current state and mode is considered to
avoid entering an invalid state on regulators with enabled workarounds.

Additionally, monitors of disabled regulators are not disabled before
changing state. The mon_disable_reg_disabled property is still respected
in this case, because turning off the monitor happens when the regulator
is still enabled.

Differ between initialization and normal "workaround handling" when an
EOPNOTSUPP is returned.

Signed-off-by: Benjamin Bara <benjamin.bara@skidata.com>
---
 drivers/regulator/core.c | 234 +++++++++++++++++++++++++++++++++++------------
 1 file changed, 178 insertions(+), 56 deletions(-)

diff --git a/drivers/regulator/core.c b/drivers/regulator/core.c
index 9bddab17450e..873e53633698 100644
--- a/drivers/regulator/core.c
+++ b/drivers/regulator/core.c
@@ -1426,7 +1426,7 @@ static int notif_set_limit(struct regulator_dev *rdev,
 
 static int handle_notify_limits(struct regulator_dev *rdev,
 			int (*set)(struct regulator_dev *, int, int, bool),
-			struct notification_limit *limits)
+			const struct notification_limit *limits)
 {
 	int ret = 0;
 
@@ -1451,6 +1451,180 @@ static int handle_notify_limits(struct regulator_dev *rdev,
 
 	return ret;
 }
+
+static const struct notification_limit disable_limits = {
+	.prot = REGULATOR_NOTIF_LIMIT_DISABLE,
+	.err = REGULATOR_NOTIF_LIMIT_DISABLE,
+	.warn = REGULATOR_NOTIF_LIMIT_DISABLE,
+};
+
+static int monitors_set_state(struct regulator_dev *rdev,  bool enable,
+			      unsigned int mons)
+{
+	const struct regulation_constraints *reg_c = rdev->constraints;
+	const struct regulator_ops *ops = rdev->desc->ops;
+	int tmp, ret = 0;
+
+	rdev_dbg(rdev, "%s: en: %d, mons: %x\n", __func__, enable, mons);
+
+	/* only set the state if monitoring is activated in the device-tree. */
+	if ((mons & REGULATOR_MONITOR_OVER_VOLTAGE) && reg_c->over_voltage_detection) {
+		tmp = handle_notify_limits(rdev, ops->set_over_voltage_protection,
+					   enable ? &reg_c->over_voltage_limits
+					   : &disable_limits);
+		if (tmp) {
+			if (tmp != -EOPNOTSUPP) {
+				rdev_err(rdev, "failed to set over voltage limits %pe\n",
+					 ERR_PTR(tmp));
+				return tmp;
+			}
+			rdev_warn(rdev,
+				  "IC does not support requested over voltage limits\n");
+			ret = tmp;
+		}
+	}
+	if ((mons & REGULATOR_MONITOR_UNDER_VOLTAGE) && reg_c->under_voltage_detection) {
+		tmp = handle_notify_limits(rdev, ops->set_under_voltage_protection,
+					   enable ? &reg_c->under_voltage_limits
+					   : &disable_limits);
+		if (tmp) {
+			if (tmp != -EOPNOTSUPP) {
+				rdev_err(rdev, "failed to set under voltage limits %pe\n",
+					 ERR_PTR(tmp));
+				return ret;
+			}
+			rdev_warn(rdev,
+				  "IC does not support requested under voltage limits\n");
+			ret = tmp;
+		}
+	}
+	if ((mons & REGULATOR_MONITOR_OVER_CURRENT) && reg_c->over_current_detection) {
+		tmp = handle_notify_limits(rdev, ops->set_over_current_protection,
+					   enable ? &reg_c->over_curr_limits
+					   : &disable_limits);
+		if (ret) {
+			if (tmp != -EOPNOTSUPP) {
+				rdev_err(rdev, "failed to set over current limits: %pe\n",
+					 ERR_PTR(tmp));
+				return tmp;
+			}
+			rdev_warn(rdev,
+				  "IC does not support requested over-current limits\n");
+			ret = tmp;
+		}
+	}
+	if ((mons & REGULATOR_MONITOR_OVER_TEMPERATURE) && reg_c->over_temp_detection) {
+		tmp = handle_notify_limits(rdev, ops->set_thermal_protection,
+					   enable ? &reg_c->temp_limits
+					   : &disable_limits);
+		if (tmp) {
+			if (tmp != -EOPNOTSUPP) {
+				rdev_err(rdev, "failed to set temperature limits %pe\n",
+					 ERR_PTR(tmp));
+				return tmp;
+			}
+			rdev_warn(rdev,
+				  "IC does not support requested temperature limits\n");
+			ret = tmp;
+		}
+	}
+
+	return ret;
+}
+
+/**
+ * monitors_disable - disables given monitors if the regulator is enabled
+ * @rdev: regulator source
+ * @mons: monitors to enable
+ */
+static int monitors_disable(struct regulator_dev *rdev, unsigned int mons)
+{
+	int reg_enabled;
+
+	if (!mons)
+		return 0;
+
+	reg_enabled = _regulator_is_enabled(rdev);
+	if (reg_enabled <= 0)
+		return reg_enabled;
+
+	return monitors_set_state(rdev, false, mons);
+}
+
+/**
+ * monitors_enable - enables given monitors
+ * @rdev: regulator source
+ * @mons: monitors to enable
+ *
+ * Enables monitors based on their workaround properties and the current state
+ * or mode.
+ */
+static int monitors_enable(struct regulator_dev *rdev, unsigned int mons)
+{
+	const struct regulator_desc *desc = rdev->desc;
+	const struct regulator_ops *ops = desc->ops;
+
+	/* don't enable monitors if regulator is in unsupported mode. */
+	if (desc->mon_unsupported_reg_modes &&
+	    (desc->mon_unsupported_reg_modes & ops->get_mode(rdev)))
+		return 0;
+
+	/* don't enable monitor on disabled regulator with workaround active. */
+	if (mons & desc->mon_disable_reg_disabled) {
+		int reg_enabled = _regulator_is_enabled(rdev);
+
+		if (reg_enabled < 0)
+			return reg_enabled;
+		if (!reg_enabled)
+			mons &= ~desc->mon_disable_reg_disabled;
+	}
+
+	return monitors_set_state(rdev, true, mons);
+}
+
+static int monitors_init(struct regulator_dev *rdev)
+{
+	unsigned int mons = REGULATOR_MONITOR_NONE;
+	int reg_enabled = _regulator_is_enabled(rdev);
+	int ret;
+
+	/*
+	 * Ensure that monitors of disabled regulators with respective
+	 * workaround active are disabled during initialization.
+	 */
+	if (reg_enabled < 0)
+		return reg_enabled;
+	if (!reg_enabled && rdev->desc->mon_disable_reg_disabled) {
+		mons = rdev->desc->mon_disable_reg_disabled;
+		ret = monitors_set_state(rdev, false, mons);
+	}
+
+	/* Ignore EOPNOTSUPP at initialization, but not during workarounds. */
+	ret = monitors_enable(rdev, ~mons);
+	if (ret && ret != -EOPNOTSUPP)
+		return ret;
+
+	return 0;
+}
+
+static int monitors_reenable(struct regulator_dev *rdev, unsigned int mons)
+{
+	int reg_enabled;
+
+	if (!mons)
+		return 0;
+
+	/*
+	 * Monitors of disabled regulators are not turned off, therefore skip
+	 * turning on.
+	 */
+	reg_enabled = _regulator_is_enabled(rdev);
+	if (reg_enabled <= 0)
+		return reg_enabled;
+
+	return monitors_enable(rdev, mons);
+}
+
 /**
  * set_machine_constraints - sets regulator constraints
  * @rdev: regulator source
@@ -1605,61 +1779,9 @@ static int set_machine_constraints(struct regulator_dev *rdev)
 		}
 	}
 
-	if (rdev->constraints->over_current_detection)
-		ret = handle_notify_limits(rdev,
-					   ops->set_over_current_protection,
-					   &rdev->constraints->over_curr_limits);
-	if (ret) {
-		if (ret != -EOPNOTSUPP) {
-			rdev_err(rdev, "failed to set over current limits: %pe\n",
-				 ERR_PTR(ret));
-			return ret;
-		}
-		rdev_warn(rdev,
-			  "IC does not support requested over-current limits\n");
-	}
-
-	if (rdev->constraints->over_voltage_detection)
-		ret = handle_notify_limits(rdev,
-					   ops->set_over_voltage_protection,
-					   &rdev->constraints->over_voltage_limits);
-	if (ret) {
-		if (ret != -EOPNOTSUPP) {
-			rdev_err(rdev, "failed to set over voltage limits %pe\n",
-				 ERR_PTR(ret));
-			return ret;
-		}
-		rdev_warn(rdev,
-			  "IC does not support requested over voltage limits\n");
-	}
-
-	if (rdev->constraints->under_voltage_detection)
-		ret = handle_notify_limits(rdev,
-					   ops->set_under_voltage_protection,
-					   &rdev->constraints->under_voltage_limits);
-	if (ret) {
-		if (ret != -EOPNOTSUPP) {
-			rdev_err(rdev, "failed to set under voltage limits %pe\n",
-				 ERR_PTR(ret));
-			return ret;
-		}
-		rdev_warn(rdev,
-			  "IC does not support requested under voltage limits\n");
-	}
-
-	if (rdev->constraints->over_temp_detection)
-		ret = handle_notify_limits(rdev,
-					   ops->set_thermal_protection,
-					   &rdev->constraints->temp_limits);
-	if (ret) {
-		if (ret != -EOPNOTSUPP) {
-			rdev_err(rdev, "failed to set temperature limits %pe\n",
-				 ERR_PTR(ret));
-			return ret;
-		}
-		rdev_warn(rdev,
-			  "IC does not support requested temperature limits\n");
-	}
+	ret = monitors_init(rdev);
+	if (ret)
+		return ret;
 
 	if (rdev->constraints->active_discharge && ops->set_active_discharge) {
 		bool ad_state = (rdev->constraints->active_discharge ==

-- 
2.34.1