pwm: imx: Provide atomic PWM support for i.MX PWMv2

This commit provides apply() callback implementation for i.MX's PWMv2.
Suggested-by: NStefan Agner <stefan@agner.ch>
Suggested-by: NBoris Brezillon <boris.brezillon@free-electrons.com>
Signed-off-by: NLukasz Majewski <l.majewski@majess.pl>
Reviewed-by: NBoris Brezillon <boris.brezillon@free-electrons.com>
Signed-off-by: NThierry Reding <thierry.reding@gmail.com>

drivers/pwm/pwm-imx.c

@@ -248,6 +248,72 @@ static int imx_pwm_config(struct pwm_chip *chip,
 	return ret;
 }
 
+static int imx_pwm_apply_v2(struct pwm_chip *chip, struct pwm_device *pwm,
+			    struct pwm_state *state)
+{
+	unsigned long period_cycles, duty_cycles, prescale;
+	struct imx_chip *imx = to_imx_chip(chip);
+	struct pwm_state cstate;
+	unsigned long long c;
+	int ret;
+
+	pwm_get_state(pwm, &cstate);
+
+	if (state->enabled) {
+		c = clk_get_rate(imx->clk_per);
+		c *= state->period;
+
+		do_div(c, 1000000000);
+		period_cycles = c;
+
+		prescale = period_cycles / 0x10000 + 1;
+
+		period_cycles /= prescale;
+		c = (unsigned long long)period_cycles * state->duty_cycle;
+		do_div(c, state->period);
+		duty_cycles = c;
+
+		/*
+		 * according to imx pwm RM, the real period value should be
+		 * PERIOD value in PWMPR plus 2.
+		 */
+		if (period_cycles > 2)
+			period_cycles -= 2;
+		else
+			period_cycles = 0;
+
+		/*
+		 * Wait for a free FIFO slot if the PWM is already enabled, and
+		 * flush the FIFO if the PWM was disabled and is about to be
+		 * enabled.
+		 */
+		if (cstate.enabled) {
+			imx_pwm_wait_fifo_slot(chip, pwm);
+		} else {
+			ret = clk_prepare_enable(imx->clk_per);
+			if (ret)
+				return ret;
+
+			imx_pwm_sw_reset(chip);
+		}
+
+		writel(duty_cycles, imx->mmio_base + MX3_PWMSAR);
+		writel(period_cycles, imx->mmio_base + MX3_PWMPR);
+
+		writel(MX3_PWMCR_PRESCALER(prescale) |
+		       MX3_PWMCR_DOZEEN | MX3_PWMCR_WAITEN |
+		       MX3_PWMCR_DBGEN | MX3_PWMCR_CLKSRC_IPG_HIGH |
+		       MX3_PWMCR_EN,
+		       imx->mmio_base + MX3_PWMCR);
+	} else if (cstate.enabled) {
+		writel(0, imx->mmio_base + MX3_PWMCR);
+
+		clk_disable_unprepare(imx->clk_per);
+	}
+
+	return 0;
+}
+
 static int imx_pwm_enable(struct pwm_chip *chip, struct pwm_device *pwm)
 {
 	struct imx_chip *imx = to_imx_chip(chip);
@@ -279,6 +345,7 @@ static const struct pwm_ops imx_pwm_ops_v1 = {
 };
 
 static const struct pwm_ops imx_pwm_ops_v2 = {
+	.apply = imx_pwm_apply_v2,
 	.enable = imx_pwm_enable,
 	.disable = imx_pwm_disable,
 	.config = imx_pwm_config,