[PATCH v4 1/2] regulator: qcom-rpmh-regulator: fix regulator mode mismatch

[Federico Amedeo Izzo via B4 Relay]

From: Federico Amedeo Izzo <federico at izzo.pro>

Initial regulator mode was read from dts but never applied.
This caused a mismatch between saved mode and actual regulator mode.

Apply the current mode from priv->mode during enable() and move
rpmh_regulator_vrm_set_mode function before rpmh_regulator_set_enable_state().

Signed-off-by: Federico Amedeo Izzo <federico at izzo.pro>
---
 drivers/power/regulator/qcom-rpmh-regulator.c | 106 ++++++++++++++------------
 1 file changed, 56 insertions(+), 50 deletions(-)

diff --git a/drivers/power/regulator/qcom-rpmh-regulator.c b/drivers/power/regulator/qcom-rpmh-regulator.c
index 4d65aae1690..f789b5b6f86 100644
--- a/drivers/power/regulator/qcom-rpmh-regulator.c
+++ b/drivers/power/regulator/qcom-rpmh-regulator.c
@@ -295,6 +295,56 @@ static int rpmh_regulator_vrm_get_value(struct udevice *rdev)
 	return vreg->uv;
 }
 
+static int rpmh_regulator_vrm_set_mode_bypass(struct rpmh_vreg *vreg,
+					      unsigned int mode, bool bypassed)
+{
+	struct tcs_cmd cmd = {
+		.addr = vreg->addr + RPMH_REGULATOR_REG_VRM_MODE,
+	};
+	struct dm_regulator_mode *pmic_mode;
+	int i;
+
+	if (mode > REGULATOR_MODE_HPM)
+		return -EINVAL;
+
+	for (i = 0; i < vreg->hw_data->n_modes; i++) {
+		pmic_mode = &vreg->hw_data->pmic_mode_map[i];
+		if (pmic_mode->id == mode)
+			break;
+	}
+	if (pmic_mode->id != mode) {
+		printf("Invalid mode %d\n", mode);
+		return -EINVAL;
+	}
+
+	if (bypassed)
+		// XXX: should have a version check for PMIC4 but we don't have any yet
+		// and we don't use bypass mode
+		cmd.data = PMIC5_BOB_MODE_PASS;
+	else
+		cmd.data = pmic_mode->register_value;
+
+	return rpmh_regulator_send_request(vreg, &cmd, true);
+}
+
+static int rpmh_regulator_vrm_set_mode(struct udevice *rdev,
+				       int mode)
+{
+	struct rpmh_vreg *vreg = dev_get_priv(rdev);
+	int ret;
+
+	debug("%s: set_mode %d (current %d)\n", rdev->name, mode, vreg->mode);
+
+	if (mode == vreg->mode)
+		return 0;
+
+	ret = rpmh_regulator_vrm_set_mode_bypass(vreg, mode, vreg->bypassed);
+	if (!ret)
+		vreg->mode = mode;
+
+	return ret;
+}
+
 static int rpmh_regulator_is_enabled(struct udevice *rdev)
 {
 	struct rpmh_vreg *vreg = dev_get_priv(rdev);
@@ -331,6 +381,12 @@ static int rpmh_regulator_set_enable_state(struct udevice *rdev,
 	debug("%s: set_enable %d (current %d)\n", rdev->name, enable,
 	      vreg->enabled);
 
+	if (vreg->mode != -EINVAL) {
+		ret = rpmh_regulator_vrm_set_mode_bypass(vreg, vreg->mode, vreg->bypassed);
+		if (ret < 0)
+			return ret;
+	}
+
 	if (vreg->enabled == -EINVAL &&
 	    vreg->uv != -ENOTRECOVERABLE) {
 		ret = _rpmh_regulator_vrm_set_value(rdev,
@@ -346,56 +402,6 @@ static int rpmh_regulator_set_enable_state(struct udevice *rdev,
 	return ret;
 }
 
-static int rpmh_regulator_vrm_set_mode_bypass(struct rpmh_vreg *vreg,
-					      unsigned int mode, bool bypassed)
-{
-	struct tcs_cmd cmd = {
-		.addr = vreg->addr + RPMH_REGULATOR_REG_VRM_MODE,
-	};
-	struct dm_regulator_mode *pmic_mode;
-	int i;
-
-	if (mode > REGULATOR_MODE_HPM)
-		return -EINVAL;
-
-	for (i = 0; i < vreg->hw_data->n_modes; i++) {
-		pmic_mode = &vreg->hw_data->pmic_mode_map[i];
-		if (pmic_mode->id == mode)
-			break;
-	}
-	if (pmic_mode->id != mode) {
-		printf("Invalid mode %d\n", mode);
-		return -EINVAL;
-	}
-
-	if (bypassed)
-		// XXX: should have a version check for PMIC4 but we don't have any yet
-		// and we don't use bypass mode
-		cmd.data = PMIC5_BOB_MODE_PASS;
-	else
-		cmd.data = pmic_mode->register_value;
-
-	return rpmh_regulator_send_request(vreg, &cmd, true);
-}
-
-static int rpmh_regulator_vrm_set_mode(struct udevice *rdev,
-				       int mode)
-{
-	struct rpmh_vreg *vreg = dev_get_priv(rdev);
-	int ret;
-
-	debug("%s: set_mode %d (current %d)\n", rdev->name, mode, vreg->mode);
-
-	if (mode == vreg->mode)
-		return 0;
-
-	ret = rpmh_regulator_vrm_set_mode_bypass(vreg, mode, vreg->bypassed);
-	if (!ret)
-		vreg->mode = mode;
-
-	return ret;
-}
-
 static int rpmh_regulator_vrm_get_pmic_mode(struct rpmh_vreg *vreg, int *pmic_mode)
 {
 	struct tcs_cmd cmd = {

-- 
2.54.0