1 files changed, 25 insertions, 18 deletions

diff --git a/drivers/pwm/pwm-imx.c b/drivers/pwm/pwm-imx.c
index c944f15f574c..d557279446ed 100644
--- a/drivers/pwm/pwm-imx.c
+++ b/drivers/pwm/pwm-imx.c
@@ -137,18 +137,36 @@ static void imx_pwm_sw_reset(struct pwm_chip *chip)
                 dev_warn(dev, "software reset timeout\n");
 }
 
+static void imx_pwm_wait_fifo_slot(struct pwm_chip *chip,
+                                   struct pwm_device *pwm)
+{
+        struct imx_chip *imx = to_imx_chip(chip);
+        struct device *dev = chip->dev;
+        unsigned int period_ms;
+        int fifoav;
+        u32 sr;
+
+        sr = readl(imx->mmio_base + MX3_PWMSR);
+        fifoav = sr & MX3_PWMSR_FIFOAV_MASK;
+        if (fifoav == MX3_PWMSR_FIFOAV_4WORDS) {
+                period_ms = DIV_ROUND_UP(pwm_get_period(pwm),
+                                         NSEC_PER_MSEC);
+                msleep(period_ms);
+
+                sr = readl(imx->mmio_base + MX3_PWMSR);
+                if (fifoav == (sr & MX3_PWMSR_FIFOAV_MASK))
+                        dev_warn(dev, "there is no free FIFO slot\n");
+        }
+}
 
 static int imx_pwm_config_v2(struct pwm_chip *chip,
                 struct pwm_device *pwm, int duty_ns, int period_ns)
 {
         struct imx_chip *imx = to_imx_chip(chip);
-        struct device *dev = chip->dev;
         unsigned long long c;
         unsigned long period_cycles, duty_cycles, prescale;
-        unsigned int period_ms;
         bool enable = pwm_is_enabled(pwm);
-        int fifoav;
-        u32 cr, sr;
+        u32 cr;
 
         /*
          * i.MX PWMv2 has a 4-word sample FIFO.
@@ -157,21 +175,10 @@ static int imx_pwm_config_v2(struct pwm_chip *chip,
          * wait for a full PWM cycle to get a relinquished FIFO slot
          * when the controller is enabled and the FIFO is fully loaded.
          */
-        if (enable) {
-                sr = readl(imx->mmio_base + MX3_PWMSR);
-                fifoav = sr & MX3_PWMSR_FIFOAV_MASK;
-                if (fifoav == MX3_PWMSR_FIFOAV_4WORDS) {
-                        period_ms = DIV_ROUND_UP(pwm_get_period(pwm),
-                                                 NSEC_PER_MSEC);
-                        msleep(period_ms);
-
-                        sr = readl(imx->mmio_base + MX3_PWMSR);
-                        if (fifoav == (sr & MX3_PWMSR_FIFOAV_MASK))
-                                dev_warn(dev, "there is no free FIFO slot\n");
-                }
-        } else {
+        if (enable)
+                imx_pwm_wait_fifo_slot(chip, pwm);
+        else
                 imx_pwm_sw_reset(chip);
-        }
 
         c = clk_get_rate(imx->clk_per);
         c = c * period_ns;