1 files changed, 23 insertions, 1 deletions

diff --git a/drivers/pwm/pwm-stm32.c b/drivers/pwm/pwm-stm32.c
index ed3961b7b938..9a50acde1e61 100644
--- a/drivers/pwm/pwm-stm32.c
+++ b/drivers/pwm/pwm-stm32.c
@@ -168,7 +168,7 @@ static int stm32_pwm_capture(struct pwm_chip *chip, struct pwm_device *pwm,
         struct stm32_pwm *priv = to_stm32_pwm_dev(chip);
         unsigned long long prd, div, dty;
         unsigned long rate;
-        unsigned int psc = 0;
+        unsigned int psc = 0, scale;
         u32 raw_prd, raw_dty;
         int ret = 0;
 
@@ -219,6 +219,28 @@ static int stm32_pwm_capture(struct pwm_chip *chip, struct pwm_device *pwm,
         if (ret)
                 goto stop;
 
+        /*
+         * Got a capture. Try to improve accuracy at high rates:
+         * - decrease counter clock prescaler, scale up to max rate.
+         */
+        if (raw_prd) {
+                u32 max_arr = priv->max_arr - 0x1000; /* arbitrary margin */
+
+                scale = max_arr / min(max_arr, raw_prd);
+        } else {
+                scale = priv->max_arr; /* bellow resolution, use max scale */
+        }
+
+        if (psc && scale > 1) {
+                /* 2nd measure with new scale */
+                psc /= scale;
+                regmap_write(priv->regmap, TIM_PSC, psc);
+                ret = stm32_pwm_raw_capture(priv, pwm, tmo_ms, &raw_prd,
+                                            &raw_dty);
+                if (ret)
+                        goto stop;
+        }
+
         prd = (unsigned long long)raw_prd * (psc + 1) * NSEC_PER_SEC;
         result->period = DIV_ROUND_UP_ULL(prd, rate);
         dty = (unsigned long long)raw_dty * (psc + 1) * NSEC_PER_SEC;