[PATCH 2/5] pwm: jz4740: Fix pin level of disabled TCU2 channels, part 2

[Paul Cercueil <paul@crapouillou.net>]

After commit a020f22a4ff5 ("pwm: jz4740: Make PWM start with the active part"),
the trick to set duty > period to properly shut down TCU2 channels did
not work anymore, because of the polarity inversion.

Address this issue by restoring the proper polarity before disabling the
channels.

Fixes: a020f22a4ff5 ("pwm: jz4740: Make PWM start with the active part")
Signed-off-by: Paul Cercueil <paul@crapouillou.net>
Cc: stable@vger.kernel.org
---
 drivers/pwm/pwm-jz4740.c | 62 ++++++++++++++++++++++++++--------------
 1 file changed, 40 insertions(+), 22 deletions(-)

diff --git a/drivers/pwm/pwm-jz4740.c b/drivers/pwm/pwm-jz4740.c
index 228eb104bf1e..65462a0052af 100644
--- a/drivers/pwm/pwm-jz4740.c
+++ b/drivers/pwm/pwm-jz4740.c
@@ -97,6 +97,19 @@ static int jz4740_pwm_enable(struct pwm_chip *chip, struct pwm_device *pwm)
 	return 0;
 }
 
+static void jz4740_pwm_set_polarity(struct jz4740_pwm_chip *jz,
+				    unsigned int hwpwm,
+				    enum pwm_polarity polarity)
+{
+	unsigned int value = 0;
+
+	if (polarity == PWM_POLARITY_INVERSED)
+		value = TCU_TCSR_PWM_INITL_HIGH;
+
+	regmap_update_bits(jz->map, TCU_REG_TCSRc(hwpwm),
+			   TCU_TCSR_PWM_INITL_HIGH, value);
+}
+
 static void jz4740_pwm_disable(struct pwm_chip *chip, struct pwm_device *pwm)
 {
 	struct jz4740_pwm_chip *jz = to_jz4740(chip);
@@ -130,6 +143,7 @@ static int jz4740_pwm_apply(struct pwm_chip *chip, struct pwm_device *pwm,
 	unsigned long long tmp = 0xffffull * NSEC_PER_SEC;
 	struct clk *clk = pwm_get_chip_data(pwm);
 	unsigned long period, duty;
+	enum pwm_polarity polarity;
 	long rate;
 	int err;
 
@@ -169,6 +183,9 @@ static int jz4740_pwm_apply(struct pwm_chip *chip, struct pwm_device *pwm,
 	if (duty >= period)
 		duty = period - 1;
 
+	/* Restore regular polarity before disabling the channel. */
+	jz4740_pwm_set_polarity(jz4740, pwm->hwpwm, state->polarity);
+
 	jz4740_pwm_disable(chip, pwm);
 
 	err = clk_set_rate(clk, rate);
@@ -190,29 +207,30 @@ static int jz4740_pwm_apply(struct pwm_chip *chip, struct pwm_device *pwm,
 	regmap_update_bits(jz4740->map, TCU_REG_TCSRc(pwm->hwpwm),
 			   TCU_TCSR_PWM_SD, TCU_TCSR_PWM_SD);
 
-	/*
-	 * Set polarity.
-	 *
-	 * The PWM starts in inactive state until the internal timer reaches the
-	 * duty value, then becomes active until the timer reaches the period
-	 * value. In theory, we should then use (period - duty) as the real duty
-	 * value, as a high duty value would otherwise result in the PWM pin
-	 * being inactive most of the time.
-	 *
-	 * Here, we don't do that, and instead invert the polarity of the PWM
-	 * when it is active. This trick makes the PWM start with its active
-	 * state instead of its inactive state.
-	 */
-	if ((state->polarity == PWM_POLARITY_NORMAL) ^ state->enabled)
-		regmap_update_bits(jz4740->map, TCU_REG_TCSRc(pwm->hwpwm),
-				   TCU_TCSR_PWM_INITL_HIGH, 0);
-	else
-		regmap_update_bits(jz4740->map, TCU_REG_TCSRc(pwm->hwpwm),
-				   TCU_TCSR_PWM_INITL_HIGH,
-				   TCU_TCSR_PWM_INITL_HIGH);
-
-	if (state->enabled)
+	if (state->enabled) {
+		/*
+		 * Set polarity.
+		 *
+		 * The PWM starts in inactive state until the internal timer
+		 * reaches the duty value, then becomes active until the timer
+		 * reaches the period value. In theory, we should then use
+		 * (period - duty) as the real duty value, as a high duty value
+		 * would otherwise result in the PWM pin being inactive most of
+		 * the time.
+		 *
+		 * Here, we don't do that, and instead invert the polarity of
+		 * the PWM when it is active. This trick makes the PWM start
+		 * with its active state instead of its inactive state.
+		 */
+		if (state->polarity == PWM_POLARITY_NORMAL)
+			polarity = PWM_POLARITY_INVERSED;
+		else
+			polarity = PWM_POLARITY_NORMAL;
+
+		jz4740_pwm_set_polarity(jz4740, pwm->hwpwm, polarity);
+
 		jz4740_pwm_enable(chip, pwm);
+	}
 
 	return 0;
 }
-- 
2.35.1