1 files changed, 96 insertions, 44 deletions

diff --git a/drivers/pwm/pwm-sti.c b/drivers/pwm/pwm-sti.c
index 76346e32f403..2d3260571b5a 100644
--- a/drivers/pwm/pwm-sti.c
+++ b/drivers/pwm/pwm-sti.c
@@ -58,6 +58,7 @@ struct sti_pwm_chip {
         struct regmap_field *pwm_int_en;
         unsigned long *pwm_periods;
         struct pwm_chip chip;
+        struct pwm_device *cur;
         void __iomem *mmio;
 };
 
@@ -99,6 +100,24 @@ static void sti_pwm_calc_periods(struct sti_pwm_chip *pc)
         }
 }
 
+/* Calculate the number of PWM devices configured with a period. */
+static unsigned int sti_pwm_count_configured(struct pwm_chip *chip)
+{
+        struct pwm_device *pwm;
+        unsigned int ncfg = 0;
+        unsigned int i;
+
+        for (i = 0; i < chip->npwm; i++) {
+                pwm = &chip->pwms[i];
+                if (test_bit(PWMF_REQUESTED, &pwm->flags)) {
+                        if (pwm_get_period(pwm))
+                                ncfg++;
+                }
+        }
+
+        return ncfg;
+}
+
 static int sti_pwm_cmp_periods(const void *key, const void *elt)
 {
         unsigned long i = *(unsigned long *)key;
@@ -124,57 +143,90 @@ static int sti_pwm_config(struct pwm_chip *chip, struct pwm_device *pwm,
 {
         struct sti_pwm_chip *pc = to_sti_pwmchip(chip);
         struct sti_pwm_compat_data *cdata = pc->cdata;
+        struct pwm_device *cur = pc->cur;
         struct device *dev = pc->dev;
-        unsigned int prescale, pwmvalx;
+        unsigned int prescale = 0, pwmvalx;
         unsigned long *found;
         int ret;
-
-        /*
-         * Search for matching period value. The corresponding index is our
-         * prescale value
+        unsigned int ncfg;
+        bool period_same = false;
+
+        ncfg = sti_pwm_count_configured(chip);
+        if (ncfg)
+                period_same = (period_ns == pwm_get_period(cur));
+
+        /* Allow configuration changes if one of the
+         * following conditions satisfy.
+         * 1. No channels have been configured.
+         * 2. Only one channel has been configured and the new request
+         *    is for the same channel.
+         * 3. Only one channel has been configured and the new request is
+         *    for a new channel and period of the new channel is same as
+         *    the current configured period.
+         * 4. More than one channels are configured and period of the new
+         *    requestis the same as the current period.
          */
-        found = bsearch(&period_ns, &pc->pwm_periods[0],
-                        cdata->max_prescale + 1, sizeof(unsigned long),
-                        sti_pwm_cmp_periods);
-        if (!found) {
-                dev_err(dev, "failed to find matching period\n");
+        if (!ncfg ||
+            ((ncfg == 1) && (pwm->hwpwm == cur->hwpwm)) ||
+            ((ncfg == 1) && (pwm->hwpwm != cur->hwpwm) && period_same) ||
+            ((ncfg > 1) && period_same)) {
+                /* Enable clock before writing to PWM registers. */
+                ret = clk_enable(pc->clk);
+                if (ret)
+                        return ret;
+
+                if (!period_same) {
+                        /*
+                         * Search for matching period value.
+                         * The corresponding index is our prescale value.
+                         */
+                        found = bsearch(&period_ns, &pc->pwm_periods[0],
+                                        cdata->max_prescale + 1,
+                                        sizeof(unsigned long),
+                                        sti_pwm_cmp_periods);
+                        if (!found) {
+                                dev_err(dev,
+                                        "failed to find matching period\n");
+                                ret = -EINVAL;
+                                goto clk_dis;
+                        }
+                        prescale = found - &pc->pwm_periods[0];
+
+                        ret =
+                        regmap_field_write(pc->prescale_low,
+                                           prescale & PWM_PRESCALE_LOW_MASK);
+                        if (ret)
+                                goto clk_dis;
+
+                        ret =
+                        regmap_field_write(pc->prescale_high,
+                                (prescale & PWM_PRESCALE_HIGH_MASK) >> 4);
+                        if (ret)
+                                goto clk_dis;
+                }
+
+                /*
+                 * When PWMVal == 0, PWM pulse = 1 local clock cycle.
+                 * When PWMVal == max_pwm_count,
+                 * PWM pulse = (max_pwm_count + 1) local cycles,
+                 * that is continuous pulse: signal never goes low.
+                 */
+                pwmvalx = cdata->max_pwm_cnt * duty_ns / period_ns;
+
+                ret = regmap_write(pc->regmap, STI_DS_REG(pwm->hwpwm), pwmvalx);
+                if (ret)
+                        goto clk_dis;
+
+                ret = regmap_field_write(pc->pwm_int_en, 0);
+
+                pc->cur = pwm;
+
+                dev_dbg(dev, "prescale:%u, period:%i, duty:%i, pwmvalx:%u\n",
+                        prescale, period_ns, duty_ns, pwmvalx);
+        } else {
                 return -EINVAL;
         }
 
-        prescale = found - &pc->pwm_periods[0];
-
-        /*
-         * When PWMVal == 0, PWM pulse = 1 local clock cycle.
-         * When PWMVal == max_pwm_count,
-         * PWM pulse = (max_pwm_count + 1) local cycles,
-         * that is continuous pulse: signal never goes low.
-         */
-        pwmvalx = cdata->max_pwm_cnt * duty_ns / period_ns;
-
-        dev_dbg(dev, "prescale:%u, period:%i, duty:%i, pwmvalx:%u\n",
-                prescale, period_ns, duty_ns, pwmvalx);
-
-        /* Enable clock before writing to PWM registers */
-        ret = clk_enable(pc->clk);
-        if (ret)
-                return ret;
-
-        ret = regmap_field_write(pc->prescale_low,
-                                 prescale & PWM_PRESCALE_LOW_MASK);
-        if (ret)
-                goto clk_dis;
-
-        ret = regmap_field_write(pc->prescale_high,
-                                 (prescale & PWM_PRESCALE_HIGH_MASK) >> 4);
-        if (ret)
-                goto clk_dis;
-
-        ret = regmap_write(pc->regmap, STI_PWMVAL(pwm->hwpwm), pwmvalx);
-        if (ret)
-                goto clk_dis;
-
-        ret = regmap_field_write(pc->pwm_int_en, 0);
-
 clk_dis:
         clk_disable(pc->clk);
         return ret;