[PATCH v1 4/4] pwm: tegra: properly calculate pulse and frequency divider fields

[Svyatoslav Ryhel]

From: Ion Agorria <ion at agorria.com>

The pulse width field requires better precision during calculation. Add a
proper frequency divider calculation based on the PWM clock instead of
hardcoding it to 1.

Signed-off-by: Ion Agorria <ion at agorria.com>
Signed-off-by: Svyatoslav Ryhel <clamor95 at gmail.com>
---
 drivers/pwm/tegra_pwm.c | 61 ++++++++++++++++++++++++++++++++++++-----
 1 file changed, 54 insertions(+), 7 deletions(-)

diff --git a/drivers/pwm/tegra_pwm.c b/drivers/pwm/tegra_pwm.c
index 6ddaf626dc4..2601ed6c5a5 100644
--- a/drivers/pwm/tegra_pwm.c
+++ b/drivers/pwm/tegra_pwm.c
@@ -4,14 +4,22 @@
  */
 
 #include <dm.h>
+#include <div64.h>
 #include <log.h>
 #include <pwm.h>
 #include <asm/io.h>
 #include <asm/arch/clock.h>
 #include <asm/arch/pwm.h>
+#include <linux/time.h>
+
+#define PWM_PDIV_WIDTH		8
+#define PWM_PDIV_MAX		BIT(PWM_PDIV_WIDTH)
+#define PWM_FDIV_WIDTH		13
 
 struct tegra_pwm_priv {
 	struct pwm_ctlr *regs;
+	u64 clk_rate;
+	u32 min_period_ns;
 	u8 polarity;
 };
 
@@ -32,23 +40,56 @@ static int tegra_pwm_set_config(struct udevice *dev, uint channel,
 {
 	struct tegra_pwm_priv *priv = dev_get_priv(dev);
 	struct pwm_ctlr *regs = priv->regs;
-	uint pulse_width;
-	u32 reg;
+	u64 pulse_width;
+	u32 rate, reg;
 
 	if (channel >= 4)
 		return -EINVAL;
 	debug("%s: Configure '%s' channel %u\n", __func__, dev->name, channel);
 
-	pulse_width = duty_ns * 256 / period_ns;
+	if (period_ns < priv->min_period_ns) {
+		debug("%s: Channel %u period too low, period_ns %u minimum %u\n",
+		      __func__, channel, period_ns, priv->min_period_ns);
+		return -EINVAL;
+	}
+
+	/*
+	 * Convert from duty_ns / period_ns to a fixed number of duty ticks
+	 * per (1 << PWM_PDIV_WIDTH) cycles and make sure to round to the
+	 * nearest integer during division.
+	 */
+	pulse_width = duty_ns * PWM_PDIV_MAX;
+	pulse_width = DIV_ROUND_CLOSEST_ULL(pulse_width, period_ns);
 
 	if (priv->polarity & BIT(channel))
-		pulse_width = 256 - pulse_width;
+		pulse_width = PWM_PDIV_MAX - pulse_width;
+
+	if (pulse_width > PWM_PDIV_MAX) {
+		debug("%s: Channel %u pulse_width too high %llu\n",
+		      __func__, channel, pulse_width);
+		return -EINVAL;
+	}
+
+	/*
+	 * Since the actual PWM divider is the register's frequency divider
+	 * field plus 1, we need to decrement to get the correct value to
+	 * write to the register.
+	 */
+	rate = (priv->clk_rate * period_ns) / ((u64)NSEC_PER_SEC << PWM_PDIV_WIDTH) - 1;
+	if (rate < 0) {
+		debug("%s: Channel %u rate is not positive\n", __func__, channel);
+		return -EINVAL;
+	}
+
+	if (rate >> PWM_FDIV_WIDTH) {
+		debug("%s: Channel %u rate too high %u\n", __func__, channel, rate);
+		return -EINVAL;
+	}
 
 	reg = pulse_width << PWM_WIDTH_SHIFT;
-	reg |= 1 << PWM_DIVIDER_SHIFT;
+	reg |= rate << PWM_DIVIDER_SHIFT;
 	reg |= PWM_ENABLE_MASK;
 	writel(reg, &regs[channel].control);
-	debug("%s: pulse_width=%u\n", __func__, pulse_width);
 
 	return 0;
 }
@@ -78,9 +119,15 @@ static int tegra_pwm_of_to_plat(struct udevice *dev)
 
 static int tegra_pwm_probe(struct udevice *dev)
 {
+	struct tegra_pwm_priv *priv = dev_get_priv(dev);
 	const u32 pwm_max_freq = dev_get_driver_data(dev);
 
-	clock_start_periph_pll(PERIPH_ID_PWM, CLOCK_ID_PERIPH, pwm_max_freq);
+	priv->clk_rate = clock_start_periph_pll(PERIPH_ID_PWM, CLOCK_ID_PERIPH,
+						pwm_max_freq);
+	priv->min_period_ns = (NSEC_PER_SEC / (pwm_max_freq >> PWM_PDIV_WIDTH)) + 1;
+
+	debug("%s: clk_rate = %llu min_period_ns = %u\n", __func__,
+	      priv->clk_rate, priv->min_period_ns);
 
 	return 0;
 }
-- 
2.51.0