[PATCH 3/9] clk: pistachio: Implement PLL rate adjustment

[Ezequiel Garcia <ezequiel.garcia@xxxxxxxxxx>]

This commit implements small rate changes to the fractional PLL.
This is done using the PLL frac parameter. The .set_rate function
first finds the parameters associated to the closest nominal rate.

Then the new rate is set, using parameters from the table entry,
except for the frac parameter, which is calculated from the rate
using the fractional PLL rate formula.

Using .round_rate, the driver guarantees that only rates near
a table nominal rate is applied. To this extent, add two parameters
fout_min and fout_max, which allows to define the allowed rate
adjustment.

Signed-off-by: Ezequiel Garcia <ezequiel.garcia@xxxxxxxxxx>
---
 drivers/clk/pistachio/clk-pll.c | 48 +++++++++++++++++++++++++++++++----------
 drivers/clk/pistachio/clk.h     |  2 ++
 2 files changed, 39 insertions(+), 11 deletions(-)

diff --git a/drivers/clk/pistachio/clk-pll.c b/drivers/clk/pistachio/clk-pll.c
index f12d520..cf000bb 100644
--- a/drivers/clk/pistachio/clk-pll.c
+++ b/drivers/clk/pistachio/clk-pll.c
@@ -90,29 +90,50 @@ static struct pistachio_pll_rate_table *
 pll_get_params(struct pistachio_clk_pll *pll, unsigned long fref,
 	       unsigned long fout)
 {
-	unsigned int i;
+	unsigned int i, best;
+	unsigned long err, best_err = ~0;
 
 	for (i = 0; i < pll->nr_rates; i++) {
-		if (pll->rates[i].fref == fref && pll->rates[i].fout == fout)
-			return &pll->rates[i];
+		err = abs(pll->rates[i].fout - fout);
+		if (pll->rates[i].fref == fref && err < best_err) {
+			best = i;
+			best_err = err;
+		}
 	}
 
-	return NULL;
+	return &pll->rates[best];
 }
 
 static long pll_round_rate(struct clk_hw *hw, unsigned long rate,
 			   unsigned long *parent_rate)
 {
 	struct pistachio_clk_pll *pll = to_pistachio_pll(hw);
-	unsigned int i;
+	unsigned int i, best;
+	unsigned long err, best_err = ~0;
 
 	for (i = 0; i < pll->nr_rates; i++) {
-		if (i > 0 && pll->rates[i].fref == *parent_rate &&
-		    pll->rates[i].fout <= rate)
-			return pll->rates[i - 1].fout;
+		err = abs(pll->rates[i].fout - rate);
+		if (pll->rates[i].fref == *parent_rate && err < best_err) {
+			best = i;
+			best_err = err;
+		}
 	}
 
-	return pll->rates[0].fout;
+	/* Make sure fout_{min,max} parameters have sane values */
+	if (!pll->rates[best].fout_min)
+		pll->rates[best].fout_min = pll->rates[best].fout;
+	if (!pll->rates[best].fout_max)
+		pll->rates[best].fout_max = pll->rates[best].fout;
+
+	/*
+	 * If the chosen rate is within the maximum allowed PLL adjustment
+	 * then we accept it.
+	 * Otherwise, just return the closest nominal table rate.
+	 */
+	if (rate <= pll->rates[best].fout_max &&
+	    rate >= pll->rates[best].fout_min)
+		return rate;
+	return pll->rates[best].fout;
 }
 
 static int pll_gf40lp_frac_enable(struct clk_hw *hw)
@@ -158,12 +179,17 @@ static int pll_gf40lp_frac_set_rate(struct clk_hw *hw, unsigned long rate,
 	struct pistachio_clk_pll *pll = to_pistachio_pll(hw);
 	struct pistachio_pll_rate_table *params;
 	int enabled = pll_gf40lp_frac_is_enabled(hw);
-	u32 val;
+	u32 val, frac;
 
 	params = pll_get_params(pll, parent_rate, rate);
 	if (!params)
 		return -EINVAL;
 
+	/* Calculate the frac parameter */
+	frac = rate * params->refdiv * params->postdiv1 * params->postdiv2;
+	frac -= (params->fbdiv * parent_rate);
+	frac = do_div_round_closest((u64)frac << 24, parent_rate);
+
 	val = pll_readl(pll, PLL_CTRL1);
 	val &= ~((PLL_CTRL1_REFDIV_MASK << PLL_CTRL1_REFDIV_SHIFT) |
 		 (PLL_CTRL1_FBDIV_MASK << PLL_CTRL1_FBDIV_SHIFT));
@@ -177,7 +203,7 @@ static int pll_gf40lp_frac_set_rate(struct clk_hw *hw, unsigned long rate,
 		  PLL_FRAC_CTRL2_POSTDIV1_SHIFT) |
 		 (PLL_FRAC_CTRL2_POSTDIV2_MASK <<
 		  PLL_FRAC_CTRL2_POSTDIV2_SHIFT));
-	val |= (params->frac << PLL_FRAC_CTRL2_FRAC_SHIFT) |
+	val |= (frac << PLL_FRAC_CTRL2_FRAC_SHIFT) |
 		(params->postdiv1 << PLL_FRAC_CTRL2_POSTDIV1_SHIFT) |
 		(params->postdiv2 << PLL_FRAC_CTRL2_POSTDIV2_SHIFT);
 	pll_writel(pll, val, PLL_CTRL2);
diff --git a/drivers/clk/pistachio/clk.h b/drivers/clk/pistachio/clk.h
index 52fabbc..ea48d15 100644
--- a/drivers/clk/pistachio/clk.h
+++ b/drivers/clk/pistachio/clk.h
@@ -97,6 +97,8 @@ struct pistachio_fixed_factor {
 struct pistachio_pll_rate_table {
 	unsigned long fref;
 	unsigned long fout;
+	unsigned long fout_min;
+	unsigned long fout_max;
 	unsigned int refdiv;
 	unsigned int fbdiv;
 	unsigned int postdiv1;
-- 
2.3.3