1 files changed, 144 insertions, 124 deletions

diff --git a/drivers/clk/ingenic/cgu.c b/drivers/clk/ingenic/cgu.c
index 6e963031cd87..861c50d6cb24 100644
--- a/drivers/clk/ingenic/cgu.c
+++ b/drivers/clk/ingenic/cgu.c
@@ -12,15 +12,24 @@
 #include <linux/clkdev.h>
 #include <linux/delay.h>
 #include <linux/io.h>
+#include <linux/iopoll.h>
 #include <linux/math64.h>
 #include <linux/of.h>
 #include <linux/of_address.h>
 #include <linux/slab.h>
 #include <linux/spinlock.h>
+#include <linux/time.h>
+
 #include "cgu.h"
 
 #define MHZ (1000 * 1000)
 
+static inline const struct ingenic_cgu_clk_info *
+to_clk_info(struct ingenic_clk *clk)
+{
+	return &clk->cgu->clock_info[clk->idx];
+}
+
 /**
  * ingenic_cgu_gate_get() - get the value of clock gate register bit
  * @cgu: reference to the CGU whose registers should be read
@@ -71,21 +80,17 @@ static unsigned long
 ingenic_pll_recalc_rate(struct clk_hw *hw, unsigned long parent_rate)
 {
 	struct ingenic_clk *ingenic_clk = to_ingenic_clk(hw);
+	const struct ingenic_cgu_clk_info *clk_info = to_clk_info(ingenic_clk);
 	struct ingenic_cgu *cgu = ingenic_clk->cgu;
-	const struct ingenic_cgu_clk_info *clk_info;
 	const struct ingenic_cgu_pll_info *pll_info;
 	unsigned m, n, od_enc, od;
 	bool bypass;
-	unsigned long flags;
 	u32 ctl;
 
-	clk_info = &cgu->clock_info[ingenic_clk->idx];
 	BUG_ON(clk_info->type != CGU_CLK_PLL);
 	pll_info = &clk_info->pll;
 
-	spin_lock_irqsave(&cgu->lock, flags);
 	ctl = readl(cgu->base + pll_info->reg);
-	spin_unlock_irqrestore(&cgu->lock, flags);
 
 	m = (ctl >> pll_info->m_shift) & GENMASK(pll_info->m_bits - 1, 0);
 	m += pll_info->m_offset;
@@ -93,11 +98,15 @@ ingenic_pll_recalc_rate(struct clk_hw *hw, unsigned long parent_rate)
 	n += pll_info->n_offset;
 	od_enc = ctl >> pll_info->od_shift;
 	od_enc &= GENMASK(pll_info->od_bits - 1, 0);
-	bypass = !pll_info->no_bypass_bit &&
-		 !!(ctl & BIT(pll_info->bypass_bit));
 
-	if (bypass)
-		return parent_rate;
+	if (pll_info->bypass_bit >= 0) {
+		ctl = readl(cgu->base + pll_info->bypass_reg);
+
+		bypass = !!(ctl & BIT(pll_info->bypass_bit));
+
+		if (bypass)
+			return parent_rate;
+	}
 
 	for (od = 0; od < pll_info->od_max; od++) {
 		if (pll_info->od_encoding[od] == od_enc)
@@ -106,31 +115,46 @@ ingenic_pll_recalc_rate(struct clk_hw *hw, unsigned long parent_rate)
 	BUG_ON(od == pll_info->od_max);
 	od++;
 
-	return div_u64((u64)parent_rate * m, n * od);
+	return div_u64((u64)parent_rate * m * pll_info->rate_multiplier,
+		n * od);
 }
 
-static unsigned long
-ingenic_pll_calc(const struct ingenic_cgu_clk_info *clk_info,
-		 unsigned long rate, unsigned long parent_rate,
-		 unsigned *pm, unsigned *pn, unsigned *pod)
+static void
+ingenic_pll_calc_m_n_od(const struct ingenic_cgu_pll_info *pll_info,
+			unsigned long rate, unsigned long parent_rate,
+			unsigned int *pm, unsigned int *pn, unsigned int *pod)
 {
-	const struct ingenic_cgu_pll_info *pll_info;
-	unsigned m, n, od;
-
-	pll_info = &clk_info->pll;
-	od = 1;
+	unsigned int m, n, od = 1;
 
 	/*
 	 * The frequency after the input divider must be between 10 and 50 MHz.
 	 * The highest divider yields the best resolution.
 	 */
 	n = parent_rate / (10 * MHZ);
-	n = min_t(unsigned, n, 1 << clk_info->pll.n_bits);
-	n = max_t(unsigned, n, pll_info->n_offset);
+	n = min_t(unsigned int, n, 1 << pll_info->n_bits);
+	n = max_t(unsigned int, n, pll_info->n_offset);
 
 	m = (rate / MHZ) * od * n / (parent_rate / MHZ);
-	m = min_t(unsigned, m, 1 << clk_info->pll.m_bits);
-	m = max_t(unsigned, m, pll_info->m_offset);
+	m = min_t(unsigned int, m, 1 << pll_info->m_bits);
+	m = max_t(unsigned int, m, pll_info->m_offset);
+
+	*pm = m;
+	*pn = n;
+	*pod = od;
+}
+
+static unsigned long
+ingenic_pll_calc(const struct ingenic_cgu_clk_info *clk_info,
+		 unsigned long rate, unsigned long parent_rate,
+		 unsigned int *pm, unsigned int *pn, unsigned int *pod)
+{
+	const struct ingenic_cgu_pll_info *pll_info = &clk_info->pll;
+	unsigned int m, n, od;
+
+	if (pll_info->calc_m_n_od)
+		(*pll_info->calc_m_n_od)(pll_info, rate, parent_rate, &m, &n, &od);
+	else
+		ingenic_pll_calc_m_n_od(pll_info, rate, parent_rate, &m, &n, &od);
 
 	if (pm)
 		*pm = m;
@@ -139,19 +163,8 @@ ingenic_pll_calc(const struct ingenic_cgu_clk_info *clk_info,
 	if (pod)
 		*pod = od;
 
-	return div_u64((u64)parent_rate * m, n * od);
-}
-
-static inline const struct ingenic_cgu_clk_info *to_clk_info(
-		struct ingenic_clk *ingenic_clk)
-{
-	struct ingenic_cgu *cgu = ingenic_clk->cgu;
-	const struct ingenic_cgu_clk_info *clk_info;
-
-	clk_info = &cgu->clock_info[ingenic_clk->idx];
-	BUG_ON(clk_info->type != CGU_CLK_PLL);
-
-	return clk_info;
+	return div_u64((u64)parent_rate * m * pll_info->rate_multiplier,
+		n * od);
 }
 
 static long
@@ -164,6 +177,16 @@ ingenic_pll_round_rate(struct clk_hw *hw, unsigned long req_rate,
 	return ingenic_pll_calc(clk_info, req_rate, *prate, NULL, NULL, NULL);
 }
 
+static inline int ingenic_pll_check_stable(struct ingenic_cgu *cgu,
+					   const struct ingenic_cgu_pll_info *pll_info)
+{
+	u32 ctl;
+
+	return readl_poll_timeout(cgu->base + pll_info->reg, ctl,
+				  ctl & BIT(pll_info->stable_bit),
+				  0, 100 * USEC_PER_MSEC);
+}
+
 static int
 ingenic_pll_set_rate(struct clk_hw *hw, unsigned long req_rate,
 		     unsigned long parent_rate)
@@ -174,6 +197,7 @@ ingenic_pll_set_rate(struct clk_hw *hw, unsigned long req_rate,
 	const struct ingenic_cgu_pll_info *pll_info = &clk_info->pll;
 	unsigned long rate, flags;
 	unsigned int m, n, od;
+	int ret = 0;
 	u32 ctl;
 
 	rate = ingenic_pll_calc(clk_info, req_rate, parent_rate,
@@ -195,9 +219,14 @@ ingenic_pll_set_rate(struct clk_hw *hw, unsigned long req_rate,
 	ctl |= pll_info->od_encoding[od - 1] << pll_info->od_shift;
 
 	writel(ctl, cgu->base + pll_info->reg);
+
+	/* If the PLL is enabled, verify that it's stable */
+	if (ctl & BIT(pll_info->enable_bit))
+		ret = ingenic_pll_check_stable(cgu, pll_info);
+
 	spin_unlock_irqrestore(&cgu->lock, flags);
 
-	return 0;
+	return ret;
 }
 
 static int ingenic_pll_enable(struct clk_hw *hw)
@@ -206,33 +235,29 @@ static int ingenic_pll_enable(struct clk_hw *hw)
 	struct ingenic_cgu *cgu = ingenic_clk->cgu;
 	const struct ingenic_cgu_clk_info *clk_info = to_clk_info(ingenic_clk);
 	const struct ingenic_cgu_pll_info *pll_info = &clk_info->pll;
-	const unsigned int timeout = 100;
 	unsigned long flags;
-	unsigned int i;
+	int ret;
 	u32 ctl;
 
 	spin_lock_irqsave(&cgu->lock, flags);
+	if (pll_info->bypass_bit >= 0) {
+		ctl = readl(cgu->base + pll_info->bypass_reg);
+
+		ctl &= ~BIT(pll_info->bypass_bit);
+
+		writel(ctl, cgu->base + pll_info->bypass_reg);
+	}
+
 	ctl = readl(cgu->base + pll_info->reg);
 
-	ctl &= ~BIT(pll_info->bypass_bit);
 	ctl |= BIT(pll_info->enable_bit);
 
 	writel(ctl, cgu->base + pll_info->reg);
 
-	/* wait for the PLL to stabilise */
-	for (i = 0; i < timeout; i++) {
-		ctl = readl(cgu->base + pll_info->reg);
-		if (ctl & BIT(pll_info->stable_bit))
-			break;
-		mdelay(1);
-	}
-
+	ret = ingenic_pll_check_stable(cgu, pll_info);
 	spin_unlock_irqrestore(&cgu->lock, flags);
 
-	if (i == timeout)
-		return -EBUSY;
-
-	return 0;
+	return ret;
 }
 
 static void ingenic_pll_disable(struct clk_hw *hw)
@@ -259,12 +284,9 @@ static int ingenic_pll_is_enabled(struct clk_hw *hw)
 	struct ingenic_cgu *cgu = ingenic_clk->cgu;
 	const struct ingenic_cgu_clk_info *clk_info = to_clk_info(ingenic_clk);
 	const struct ingenic_cgu_pll_info *pll_info = &clk_info->pll;
-	unsigned long flags;
 	u32 ctl;
 
-	spin_lock_irqsave(&cgu->lock, flags);
 	ctl = readl(cgu->base + pll_info->reg);
-	spin_unlock_irqrestore(&cgu->lock, flags);
 
 	return !!(ctl & BIT(pll_info->enable_bit));
 }
@@ -286,13 +308,11 @@ static const struct clk_ops ingenic_pll_ops = {
 static u8 ingenic_clk_get_parent(struct clk_hw *hw)
 {
 	struct ingenic_clk *ingenic_clk = to_ingenic_clk(hw);
+	const struct ingenic_cgu_clk_info *clk_info = to_clk_info(ingenic_clk);
 	struct ingenic_cgu *cgu = ingenic_clk->cgu;
-	const struct ingenic_cgu_clk_info *clk_info;
 	u32 reg;
 	u8 i, hw_idx, idx = 0;
 
-	clk_info = &cgu->clock_info[ingenic_clk->idx];
-
 	if (clk_info->type & CGU_CLK_MUX) {
 		reg = readl(cgu->base + clk_info->mux.reg);
 		hw_idx = (reg >> clk_info->mux.shift) &
@@ -314,14 +334,12 @@ static u8 ingenic_clk_get_parent(struct clk_hw *hw)
 static int ingenic_clk_set_parent(struct clk_hw *hw, u8 idx)
 {
 	struct ingenic_clk *ingenic_clk = to_ingenic_clk(hw);
+	const struct ingenic_cgu_clk_info *clk_info = to_clk_info(ingenic_clk);
 	struct ingenic_cgu *cgu = ingenic_clk->cgu;
-	const struct ingenic_cgu_clk_info *clk_info;
 	unsigned long flags;
 	u8 curr_idx, hw_idx, num_poss;
 	u32 reg, mask;
 
-	clk_info = &cgu->clock_info[ingenic_clk->idx];
-
 	if (clk_info->type & CGU_CLK_MUX) {
 		/*
 		 * Convert the parent index to the hardware index by adding
@@ -364,24 +382,27 @@ static unsigned long
 ingenic_clk_recalc_rate(struct clk_hw *hw, unsigned long parent_rate)
 {
 	struct ingenic_clk *ingenic_clk = to_ingenic_clk(hw);
+	const struct ingenic_cgu_clk_info *clk_info = to_clk_info(ingenic_clk);
 	struct ingenic_cgu *cgu = ingenic_clk->cgu;
-	const struct ingenic_cgu_clk_info *clk_info;
 	unsigned long rate = parent_rate;
 	u32 div_reg, div;
-
-	clk_info = &cgu->clock_info[ingenic_clk->idx];
+	u8 parent;
 
 	if (clk_info->type & CGU_CLK_DIV) {
-		div_reg = readl(cgu->base + clk_info->div.reg);
-		div = (div_reg >> clk_info->div.shift) &
-		      GENMASK(clk_info->div.bits - 1, 0);
+		parent = ingenic_clk_get_parent(hw);
 
-		if (clk_info->div.div_table)
-			div = clk_info->div.div_table[div];
-		else
-			div = (div + 1) * clk_info->div.div;
+		if (!(clk_info->div.bypass_mask & BIT(parent))) {
+			div_reg = readl(cgu->base + clk_info->div.reg);
+			div = (div_reg >> clk_info->div.shift) &
+			      GENMASK(clk_info->div.bits - 1, 0);
+
+			if (clk_info->div.div_table)
+				div = clk_info->div.div_table[div];
+			else
+				div = (div + 1) * clk_info->div.div;
 
-		rate /= div;
+			rate /= div;
+		}
 	} else if (clk_info->type & CGU_CLK_FIXDIV) {
 		rate /= clk_info->fixdiv.div;
 	}
@@ -393,22 +414,34 @@ static unsigned int
 ingenic_clk_calc_hw_div(const struct ingenic_cgu_clk_info *clk_info,
 			unsigned int div)
 {
-	unsigned int i;
+	unsigned int i, best_i = 0, best = (unsigned int)-1;
 
 	for (i = 0; i < (1 << clk_info->div.bits)
 				&& clk_info->div.div_table[i]; i++) {
-		if (clk_info->div.div_table[i] >= div)
-			return i;
+		if (clk_info->div.div_table[i] >= div &&
+		    clk_info->div.div_table[i] < best) {
+			best = clk_info->div.div_table[i];
+			best_i = i;
+
+			if (div == best)
+				break;
+		}
 	}
 
-	return i - 1;
+	return best_i;
 }
 
 static unsigned
-ingenic_clk_calc_div(const struct ingenic_cgu_clk_info *clk_info,
+ingenic_clk_calc_div(struct clk_hw *hw,
+		     const struct ingenic_cgu_clk_info *clk_info,
 		     unsigned long parent_rate, unsigned long req_rate)
 {
 	unsigned int div, hw_div;
+	u8 parent;
+
+	parent = ingenic_clk_get_parent(hw);
+	if (clk_info->div.bypass_mask & BIT(parent))
+		return 1;
 
 	/* calculate the divide */
 	div = DIV_ROUND_UP(parent_rate, req_rate);
@@ -420,15 +453,15 @@ ingenic_clk_calc_div(const struct ingenic_cgu_clk_info *clk_info,
 	}
 
 	/* Impose hardware constraints */
-	div = min_t(unsigned, div, 1 << clk_info->div.bits);
-	div = max_t(unsigned, div, 1);
+	div = clamp_t(unsigned int, div, clk_info->div.div,
+		      clk_info->div.div << clk_info->div.bits);
 
 	/*
 	 * If the divider value itself must be divided before being written to
 	 * the divider register, we must ensure we don't have any bits set that
 	 * would be lost as a result of doing so.
 	 */
-	div /= clk_info->div.div;
+	div = DIV_ROUND_UP(div, clk_info->div.div);
 	div *= clk_info->div.div;
 
 	return div;
@@ -439,37 +472,43 @@ ingenic_clk_round_rate(struct clk_hw *hw, unsigned long req_rate,
 		       unsigned long *parent_rate)
 {
 	struct ingenic_clk *ingenic_clk = to_ingenic_clk(hw);
-	struct ingenic_cgu *cgu = ingenic_clk->cgu;
-	const struct ingenic_cgu_clk_info *clk_info;
+	const struct ingenic_cgu_clk_info *clk_info = to_clk_info(ingenic_clk);
 	unsigned int div = 1;
 
-	clk_info = &cgu->clock_info[ingenic_clk->idx];
-
 	if (clk_info->type & CGU_CLK_DIV)
-		div = ingenic_clk_calc_div(clk_info, *parent_rate, req_rate);
+		div = ingenic_clk_calc_div(hw, clk_info, *parent_rate, req_rate);
 	else if (clk_info->type & CGU_CLK_FIXDIV)
 		div = clk_info->fixdiv.div;
+	else if (clk_hw_can_set_rate_parent(hw))
+		*parent_rate = req_rate;
 
 	return DIV_ROUND_UP(*parent_rate, div);
 }
 
+static inline int ingenic_clk_check_stable(struct ingenic_cgu *cgu,
+					   const struct ingenic_cgu_clk_info *clk_info)
+{
+	u32 reg;
+
+	return readl_poll_timeout(cgu->base + clk_info->div.reg, reg,
+				  !(reg & BIT(clk_info->div.busy_bit)),
+				  0, 100 * USEC_PER_MSEC);
+}
+
 static int
 ingenic_clk_set_rate(struct clk_hw *hw, unsigned long req_rate,
 		     unsigned long parent_rate)
 {
 	struct ingenic_clk *ingenic_clk = to_ingenic_clk(hw);
+	const struct ingenic_cgu_clk_info *clk_info = to_clk_info(ingenic_clk);
 	struct ingenic_cgu *cgu = ingenic_clk->cgu;
-	const struct ingenic_cgu_clk_info *clk_info;
-	const unsigned timeout = 100;
 	unsigned long rate, flags;
-	unsigned int hw_div, div, i;
+	unsigned int hw_div, div;
 	u32 reg, mask;
 	int ret = 0;
 
-	clk_info = &cgu->clock_info[ingenic_clk->idx];
-
 	if (clk_info->type & CGU_CLK_DIV) {
-		div = ingenic_clk_calc_div(clk_info, parent_rate, req_rate);
+		div = ingenic_clk_calc_div(hw, clk_info, parent_rate, req_rate);
 		rate = DIV_ROUND_UP(parent_rate, div);
 
 		if (rate != req_rate)
@@ -500,16 +539,8 @@ ingenic_clk_set_rate(struct clk_hw *hw, unsigned long req_rate,
 		writel(reg, cgu->base + clk_info->div.reg);
 
 		/* wait for the change to take effect */
-		if (clk_info->div.busy_bit != -1) {
-			for (i = 0; i < timeout; i++) {
-				reg = readl(cgu->base + clk_info->div.reg);
-				if (!(reg & BIT(clk_info->div.busy_bit)))
-					break;
-				mdelay(1);
-			}
-			if (i == timeout)
-				ret = -EBUSY;
-		}
+		if (clk_info->div.busy_bit != -1)
+			ret = ingenic_clk_check_stable(cgu, clk_info);
 
 		spin_unlock_irqrestore(&cgu->lock, flags);
 		return ret;
@@ -521,12 +552,10 @@ ingenic_clk_set_rate(struct clk_hw *hw, unsigned long req_rate,
 static int ingenic_clk_enable(struct clk_hw *hw)
 {
 	struct ingenic_clk *ingenic_clk = to_ingenic_clk(hw);
+	const struct ingenic_cgu_clk_info *clk_info = to_clk_info(ingenic_clk);
 	struct ingenic_cgu *cgu = ingenic_clk->cgu;
-	const struct ingenic_cgu_clk_info *clk_info;
 	unsigned long flags;
 
-	clk_info = &cgu->clock_info[ingenic_clk->idx];
-
 	if (clk_info->type & CGU_CLK_GATE) {
 		/* ungate the clock */
 		spin_lock_irqsave(&cgu->lock, flags);
@@ -543,12 +572,10 @@ static int ingenic_clk_enable(struct clk_hw *hw)
 static void ingenic_clk_disable(struct clk_hw *hw)
 {
 	struct ingenic_clk *ingenic_clk = to_ingenic_clk(hw);
+	const struct ingenic_cgu_clk_info *clk_info = to_clk_info(ingenic_clk);
 	struct ingenic_cgu *cgu = ingenic_clk->cgu;
-	const struct ingenic_cgu_clk_info *clk_info;
 	unsigned long flags;
 
-	clk_info = &cgu->clock_info[ingenic_clk->idx];
-
 	if (clk_info->type & CGU_CLK_GATE) {
 		/* gate the clock */
 		spin_lock_irqsave(&cgu->lock, flags);
@@ -560,18 +587,12 @@ static void ingenic_clk_disable(struct clk_hw *hw)
 static int ingenic_clk_is_enabled(struct clk_hw *hw)
 {
 	struct ingenic_clk *ingenic_clk = to_ingenic_clk(hw);
+	const struct ingenic_cgu_clk_info *clk_info = to_clk_info(ingenic_clk);
 	struct ingenic_cgu *cgu = ingenic_clk->cgu;
-	const struct ingenic_cgu_clk_info *clk_info;
-	unsigned long flags;
 	int enabled = 1;
 
-	clk_info = &cgu->clock_info[ingenic_clk->idx];
-
-	if (clk_info->type & CGU_CLK_GATE) {
-		spin_lock_irqsave(&cgu->lock, flags);
+	if (clk_info->type & CGU_CLK_GATE)
 		enabled = !ingenic_cgu_gate_get(cgu, &clk_info->gate);
-		spin_unlock_irqrestore(&cgu->lock, flags);
-	}
 
 	return enabled;
 }
@@ -639,11 +660,18 @@ static int ingenic_register_clock(struct ingenic_cgu *cgu, unsigned idx)
 	ingenic_clk->idx = idx;
 
 	clk_init.name = clk_info->name;
-	clk_init.flags = 0;
+	clk_init.flags = clk_info->flags;
 	clk_init.parent_names = parent_names;
 
 	caps = clk_info->type;
 
+	if (caps & CGU_CLK_DIV) {
+		caps &= ~CGU_CLK_DIV;
+	} else if (!(caps & CGU_CLK_CUSTOM)) {
+		/* pass rate changes to the parent clock */
+		clk_init.flags |= CLK_SET_RATE_PARENT;
+	}
+
 	if (caps & (CGU_CLK_MUX | CGU_CLK_CUSTOM)) {
 		clk_init.num_parents = 0;
 
@@ -683,7 +711,6 @@ static int ingenic_register_clock(struct ingenic_cgu *cgu, unsigned idx)
 		}
 	} else if (caps & CGU_CLK_PLL) {
 		clk_init.ops = &ingenic_pll_ops;
-		clk_init.flags |= CLK_SET_RATE_GATE;
 
 		caps &= ~CGU_CLK_PLL;
 
@@ -706,13 +733,6 @@ static int ingenic_register_clock(struct ingenic_cgu *cgu, unsigned idx)
 		caps &= ~(CGU_CLK_MUX | CGU_CLK_MUX_GLITCHFREE);
 	}
 
-	if (caps & CGU_CLK_DIV) {
-		caps &= ~CGU_CLK_DIV;
-	} else {
-		/* pass rate changes to the parent clock */
-		clk_init.flags |= CLK_SET_RATE_PARENT;
-	}
-
 	if (caps) {
 		pr_err("%s: unknown clock type 0x%x\n", __func__, caps);
 		goto out;