arm: ux500: implement clk_set_rate for some multimedia clocks

This patch implements clk_set_rate (and clk_round_rate)
for lcdclk, tvclk, and hdmiclk, as well as some
infrastructure needed by this and coming changes.

ST Ericsson ID: 343004, 359227
ST Ericsson FOSS-OUT ID: trivial
ST Ericsson Linux next: -

Change-Id: I4caabc881d3785771827319fe062d62d6ada7938
Signed-off-by: Mattias Nilsson <mattias.i.nilsson@stericsson.com>
Reviewed-on: http://gerrit.lud.stericsson.com/gerrit/30913
Reviewed-by: Jonas ABERG <jonas.aberg@stericsson.com>

7 files changed, 244 insertions, 76 deletions

diff --git a/arch/arm/mach-ux500/clock-db8500.c b/arch/arm/mach-ux500/clock-db8500.c
index f26958689b4..0130172f048 100644
--- a/arch/arm/mach-ux500/clock-db8500.c
+++ b/arch/arm/mach-ux500/clock-db8500.c
@@ -509,11 +509,11 @@ static DEF_PRCMU_CLK(per3clk, PRCMU_PER3CLK, 133330000);
 static DEF_PRCMU_CLK(per5clk, PRCMU_PER5CLK, 133330000);
 static DEF_PRCMU_CLK(per6clk, PRCMU_PER6CLK, 133330000);
 static DEF_PRCMU_CLK(per7clk, PRCMU_PER7CLK, 100000000);
-static DEF_PRCMU_CLK(lcdclk, PRCMU_LCDCLK, 48000000);
+static DEF_PRCMU_SCALABLE_CLK(lcdclk, PRCMU_LCDCLK);
 static DEF_PRCMU_OPP100_CLK(bmlclk, PRCMU_BMLCLK, 200000000);
 static DEF_PRCMU_CLK(hsitxclk, PRCMU_HSITXCLK, 100000000);
 static DEF_PRCMU_CLK(hsirxclk, PRCMU_HSIRXCLK, 200000000);
-static DEF_PRCMU_CLK(hdmiclk, PRCMU_HDMICLK, 76800000);
+static DEF_PRCMU_SCALABLE_CLK(hdmiclk, PRCMU_HDMICLK);
 static DEF_PRCMU_CLK(apeatclk, PRCMU_APEATCLK, 160000000);
 static DEF_PRCMU_CLK(apetraceclk, PRCMU_APETRACECLK, 160000000);
 static DEF_PRCMU_CLK(mcdeclk, PRCMU_MCDECLK, 160000000);
@@ -521,7 +521,7 @@ static DEF_PRCMU_OPP100_CLK(ipi2cclk, PRCMU_IPI2CCLK, 24000000);
 static DEF_PRCMU_CLK(dsialtclk, PRCMU_DSIALTCLK, 384000000);
 static DEF_PRCMU_CLK(dmaclk, PRCMU_DMACLK, 200000000);
 static DEF_PRCMU_CLK(b2r2clk, PRCMU_B2R2CLK, 200000000);
-static DEF_PRCMU_CLK(tvclk, PRCMU_TVCLK, 76800000);
+static DEF_PRCMU_SCALABLE_CLK(tvclk, PRCMU_TVCLK);
 /* TODO: For SSPCLK, the spec says 24MHz, while the old driver says 48MHz. */
 static DEF_PRCMU_CLK(sspclk, PRCMU_SSPCLK, 24000000);
 static DEF_PRCMU_CLK(rngclk, PRCMU_RNGCLK, 19200000);
diff --git a/arch/arm/mach-ux500/clock.c b/arch/arm/mach-ux500/clock.c
index ce167af6def..95e6a8e705d 100644
--- a/arch/arm/mach-ux500/clock.c
+++ b/arch/arm/mach-ux500/clock.c
@@ -134,7 +134,7 @@ unsigned long __clk_get_rate(struct clk *clk, void *current_lock)
 	return rate;
 }
 
-static unsigned long __clk_round_rate(struct clk *clk, unsigned long rate)
+static long __clk_round_rate(struct clk *clk, unsigned long rate)
 {
 	if ((clk->ops != NULL) && (clk->ops->round_rate != NULL))
 		return clk->ops->round_rate(clk, rate);
@@ -181,6 +181,7 @@ EXPORT_SYMBOL(clk_get_rate);
 
 long clk_round_rate(struct clk *clk, unsigned long rate)
 {
+	long rounded_rate;
 	unsigned long flags;
 
 	if (clk == NULL)
@@ -188,14 +189,31 @@ long clk_round_rate(struct clk *clk, unsigned long rate)
 
 	__clk_lock(clk, NO_LOCK, &flags);
 
-	rate = __clk_round_rate(clk, rate);
+	rounded_rate = __clk_round_rate(clk, rate);
 
 	__clk_unlock(clk, NO_LOCK, flags);
 
-	return rate;
+	return rounded_rate;
 }
 EXPORT_SYMBOL(clk_round_rate);
 
+long clk_round_rate_rec(struct clk *clk, unsigned long rate)
+{
+	long rounded_rate;
+	unsigned long flags;
+
+	if ((clk == NULL) || (clk->parent == NULL))
+		return -EINVAL;
+
+	__clk_lock(clk->parent, clk->mutex, &flags);
+
+	rounded_rate = __clk_round_rate(clk->parent, rate);
+
+	__clk_unlock(clk->parent, clk->mutex, flags);
+
+	return rounded_rate;
+}
+
 int clk_set_rate(struct clk *clk, unsigned long rate)
 {
 	int err;
@@ -214,6 +232,23 @@ int clk_set_rate(struct clk *clk, unsigned long rate)
 }
 EXPORT_SYMBOL(clk_set_rate);
 
+int clk_set_rate_rec(struct clk *clk, unsigned long rate)
+{
+	int err;
+	unsigned long flags;
+
+	if ((clk == NULL) || (clk->parent == NULL))
+		return -EINVAL;
+
+	__clk_lock(clk->parent, clk->mutex, &flags);
+
+	err = __clk_set_rate(clk->parent, rate);
+
+	__clk_unlock(clk->parent, clk->mutex, flags);
+
+	return err;
+}
+
 int clk_set_parent(struct clk *clk, struct clk *parent)
 {
 	int err = 0;
@@ -308,12 +343,30 @@ static unsigned long prcmu_clk_get_rate(struct clk *clk)
 	return prcmu_clock_rate(clk->cg_sel);
 }
 
+static long prcmu_clk_round_rate(struct clk *clk, unsigned long rate)
+{
+	return prcmu_round_clock_rate(clk->cg_sel, rate);
+}
+
+static int prcmu_clk_set_rate(struct clk *clk, unsigned long rate)
+{
+	return prcmu_set_clock_rate(clk->cg_sel, rate);
+}
+
 struct clkops prcmu_clk_ops = {
 	.enable = prcmu_clk_enable,
 	.disable = prcmu_clk_disable,
 	.get_rate = prcmu_clk_get_rate,
 };
 
+struct clkops prcmu_scalable_clk_ops = {
+	.enable = prcmu_clk_enable,
+	.disable = prcmu_clk_disable,
+	.get_rate = prcmu_clk_get_rate,
+	.round_rate = prcmu_clk_round_rate,
+	.set_rate = prcmu_clk_set_rate,
+};
+
 struct clkops prcmu_opp100_clk_ops = {
 	.enable = prcmu_opp100_clk_enable,
 	.disable = prcmu_opp100_clk_disable,
diff --git a/arch/arm/mach-ux500/clock.h b/arch/arm/mach-ux500/clock.h
index b5ed8a095ec..c4a66540c0e 100644
--- a/arch/arm/mach-ux500/clock.h
+++ b/arch/arm/mach-ux500/clock.h
@@ -66,11 +66,12 @@ struct clkops {
 	void (*disable)(struct clk *);
 	unsigned long (*get_rate)(struct clk *);
 	int (*set_rate)(struct clk *, unsigned long);
-	unsigned long (*round_rate)(struct clk *, unsigned long);
+	long (*round_rate)(struct clk *, unsigned long);
 	int (*set_parent)(struct clk *, struct clk *);
 };
 
 extern struct clkops prcmu_clk_ops;
+extern struct clkops prcmu_scalable_clk_ops;
 extern struct clkops prcmu_opp100_clk_ops;
 extern struct mutex clk_opp100_mutex;
 extern struct clkops prcc_pclk_ops;
@@ -86,6 +87,13 @@ extern struct clkops sga_clk_ops;
 		.rate = _rate, \
 	}
 
+#define DEF_PRCMU_SCALABLE_CLK(_name, _cg_sel) \
+	struct clk _name = { \
+		.name = #_name, \
+		.ops = &prcmu_scalable_clk_ops, \
+		.cg_sel = _cg_sel, \
+	}
+
 /* Use this for clocks that are only defined at OPP 100%. */
 #define DEF_PRCMU_OPP100_CLK(_name, _cg_sel, _rate) \
 	struct clk _name = { \
@@ -129,6 +137,8 @@ void clks_register(struct clk_lookup *clks, size_t num);
 int __clk_enable(struct clk *clk, void *current_lock);
 void __clk_disable(struct clk *clk, void *current_lock);
 unsigned long __clk_get_rate(struct clk *clk, void *current_lock);
+long clk_round_rate_rec(struct clk *clk, unsigned long rate);
+int clk_set_rate_rec(struct clk *clk, unsigned long rate);
 
 #ifdef CONFIG_DEBUG_FS
 int dbx500_clk_debug_init(struct clk **clks, int num);
diff --git a/arch/arm/mach-ux500/include/mach/prcmu-db5500.h b/arch/arm/mach-ux500/include/mach/prcmu-db5500.h
index 6b0efb088d9..2a9bcf4d1d3 100644
--- a/arch/arm/mach-ux500/include/mach/prcmu-db5500.h
+++ b/arch/arm/mach-ux500/include/mach/prcmu-db5500.h
@@ -20,6 +20,16 @@ static inline unsigned long prcmu_clock_rate(u8 clock)
 	return 0;
 }
 
+static inline long prcmu_round_clock_rate(u8 clock, unsigned long rate)
+{
+	return 0;
+}
+
+static inline int prcmu_set_clock_rate(u8 clock, unsigned long rate)
+{
+	return 0;
+}
+
 int prcmu_resetout(u8 resoutn, u8 state);
 
 unsigned int prcmu_get_ddr_freq(void);
diff --git a/arch/arm/mach-ux500/include/mach/prcmu-db8500.h b/arch/arm/mach-ux500/include/mach/prcmu-db8500.h
index 572d68b7213..b30fb134038 100644
--- a/arch/arm/mach-ux500/include/mach/prcmu-db8500.h
+++ b/arch/arm/mach-ux500/include/mach/prcmu-db8500.h
@@ -163,8 +163,6 @@ int prcmu_request_ape_opp_100_voltage(bool enable);
 
 int prcmu_release_usb_wakeup_state(void);
 
-int prcmu_set_clock_divider(u8 clock, u8 divider);
-
 void prcmu_configure_auto_pm(struct prcmu_auto_pm_config *sleep,
 	struct prcmu_auto_pm_config *idle);
 bool prcmu_is_auto_pm_enabled(void);
@@ -209,11 +207,6 @@ static inline int prcmu_release_usb_wakeup_state(void)
 	return 0;
 }
 
-static inline int prcmu_set_clock_divider(u8 clock, u8 divider)
-{
-	return 0;
-}
-
 static inline void prcmu_configure_auto_pm(struct prcmu_auto_pm_config *sleep,
 	struct prcmu_auto_pm_config *idle)
 {
diff --git a/arch/arm/mach-ux500/include/mach/prcmu.h b/arch/arm/mach-ux500/include/mach/prcmu.h
index 3b4c52cfdb0..8c714a5e93a 100644
--- a/arch/arm/mach-ux500/include/mach/prcmu.h
+++ b/arch/arm/mach-ux500/include/mach/prcmu.h
@@ -274,6 +274,8 @@ int prcmu_config_clkout(u8 clkout, u8 source, u8 div);
 
 int prcmu_request_clock(u8 clock, bool enable);
 unsigned long prcmu_clock_rate(u8 clock);
+long prcmu_round_clock_rate(u8 clock, unsigned long rate);
+int prcmu_set_clock_rate(u8 clock, unsigned long rate);
 
 int prcmu_set_ape_opp(u8 opp);
 int prcmu_get_ape_opp(void);
@@ -334,6 +336,16 @@ static inline int prcmu_request_clock(u8 clock, bool enable)
 	return 0;
 }
 
+static inline long prcmu_round_clock_rate(u8 clock, unsigned long rate)
+{
+	return 0;
+}
+
+static inline int prcmu_set_clock_rate(u8 clock, unsigned long rate)
+{
+	return 0;
+}
+
 static inline unsigned long prcmu_clock_rate(u8 clock)
 {
 	return 0;
diff --git a/arch/arm/mach-ux500/prcmu-db8500.c b/arch/arm/mach-ux500/prcmu-db8500.c
index c12ced614f1..9744018c7fd 100644
--- a/arch/arm/mach-ux500/prcmu-db8500.c
+++ b/arch/arm/mach-ux500/prcmu-db8500.c
@@ -1551,11 +1551,12 @@ int prcmu_request_clock(u8 clock, bool enable)
 }
 
 static unsigned long pll_rate(unsigned int reg_offset, unsigned long src_rate,
-	u32 div, int branch)
+	int branch)
 {
 	u64 rate;
 	u32 val;
 	u32 d;
+	u32 div = 1;
 
 	val = readl(_PRCMU_BASE + reg_offset);
 
@@ -1589,40 +1590,41 @@ static unsigned long pll_rate(unsigned int reg_offset, unsigned long src_rate,
 static unsigned long clock_rate(u8 clock)
 {
 	u32 val;
-	u32 div;
-	unsigned long src_rate = ROOT_CLOCK_RATE;
+	u32 pllsw;
+	unsigned long rate = ROOT_CLOCK_RATE;
 
 	val = readl(_PRCMU_BASE + clk_mgt[clock].offset);
 
 	if (val & PRCM_CLK_MGT_CLK38) {
 		if (clk_mgt[clock].clk38div && (val & PRCM_CLK_MGT_CLK38DIV))
-			src_rate /= 2;
-		return src_rate;
+			rate /= 2;
+		return rate;
 	}
 
+	val |= clk_mgt[clock].pllsw;
+	pllsw = (val & PRCM_CLK_MGT_CLKPLLSW_MASK);
+
+	if (pllsw == PRCM_CLK_MGT_CLKPLLSW_SOC0)
+		rate = pll_rate(PRCM_PLLSOC0_FREQ, rate, clk_mgt[clock].branch);
+	else if (pllsw == PRCM_CLK_MGT_CLKPLLSW_SOC1)
+		rate = pll_rate(PRCM_PLLSOC1_FREQ, rate, clk_mgt[clock].branch);
+	else if (pllsw == PRCM_CLK_MGT_CLKPLLSW_DDR)
+		rate = pll_rate(PRCM_PLLDDR_FREQ, rate, clk_mgt[clock].branch);
+	else
+		return 0;
+
 	if ((clock == PRCMU_SGACLK) &&
 		(val & PRCM_SGACLK_MGT_SGACLKDIV_BY_2_5_EN)) {
-		src_rate *= 10;
-		div = 25;
-	} else {
-		div = (val & PRCM_CLK_MGT_CLKPLLDIV_MASK);
-		if (!div)
-			return 0;
-	}
+		u64 r = (rate * 10);
 
-	switch ((val | clk_mgt[clock].pllsw) & PRCM_CLK_MGT_CLKPLLSW_MASK) {
-	case PRCM_CLK_MGT_CLKPLLSW_SOC0:
-		return pll_rate(PRCM_PLLSOC0_FREQ, src_rate, div,
-			clk_mgt[clock].branch);
-	case PRCM_CLK_MGT_CLKPLLSW_SOC1:
-		return pll_rate(PRCM_PLLSOC1_FREQ, src_rate, div,
-			clk_mgt[clock].branch);
-	case PRCM_CLK_MGT_CLKPLLSW_DDR:
-		return pll_rate(PRCM_PLLDDR_FREQ, src_rate, div,
-			clk_mgt[clock].branch);
-	default:
-		return 0;
+		(void)do_div(r, 25);
+		return (unsigned long)r;
 	}
+	val &= PRCM_CLK_MGT_CLKPLLDIV_MASK;
+	if (val)
+		return rate / val;
+	else
+		return 0;
 }
 
 unsigned long prcmu_clock_rate(u8 clock)
@@ -1634,15 +1636,138 @@ unsigned long prcmu_clock_rate(u8 clock)
 	else if (clock == PRCMU_SYSCLK)
 		return ROOT_CLOCK_RATE;
 	else if (clock == PRCMU_PLLSOC0)
-		return pll_rate(PRCM_PLLSOC0_FREQ, ROOT_CLOCK_RATE, 1, PLL_RAW);
+		return pll_rate(PRCM_PLLSOC0_FREQ, ROOT_CLOCK_RATE, PLL_RAW);
 	else if (clock == PRCMU_PLLSOC1)
-		return pll_rate(PRCM_PLLSOC1_FREQ, ROOT_CLOCK_RATE, 1, PLL_RAW);
+		return pll_rate(PRCM_PLLSOC1_FREQ, ROOT_CLOCK_RATE, PLL_RAW);
 	else if (clock == PRCMU_PLLDDR)
-		return pll_rate(PRCM_PLLDDR_FREQ, ROOT_CLOCK_RATE, 1, PLL_RAW);
+		return pll_rate(PRCM_PLLDDR_FREQ, ROOT_CLOCK_RATE, PLL_RAW);
 	else
 		return 0;
 }
 
+static unsigned long clock_source_rate(u32 clk_mgt_val, int branch)
+{
+	if (clk_mgt_val & PRCM_CLK_MGT_CLK38)
+		return ROOT_CLOCK_RATE;
+	clk_mgt_val &= PRCM_CLK_MGT_CLKPLLSW_MASK;
+	if (clk_mgt_val == PRCM_CLK_MGT_CLKPLLSW_SOC0)
+		return pll_rate(PRCM_PLLSOC0_FREQ, ROOT_CLOCK_RATE, branch);
+	else if (clk_mgt_val == PRCM_CLK_MGT_CLKPLLSW_SOC1)
+		return pll_rate(PRCM_PLLSOC1_FREQ, ROOT_CLOCK_RATE, branch);
+	else if (clk_mgt_val == PRCM_CLK_MGT_CLKPLLSW_DDR)
+		return pll_rate(PRCM_PLLDDR_FREQ, ROOT_CLOCK_RATE, branch);
+	else
+		return 0;
+}
+
+static u32 clock_divider(unsigned long src_rate, unsigned long rate)
+{
+	u32 div;
+
+	div = (src_rate / rate);
+	if (div == 0)
+		return 1;
+	if (rate < (src_rate / div))
+		div++;
+	if (div > 31)
+		div = 31;
+	return div;
+}
+
+static long round_clock_rate(u8 clock, unsigned long rate)
+{
+	u32 val;
+	u32 div;
+	unsigned long src_rate;
+	long rounded_rate;
+
+	val = readl(_PRCMU_BASE + clk_mgt[clock].offset);
+	src_rate = clock_source_rate((val | clk_mgt[clock].pllsw),
+		clk_mgt[clock].branch);
+	div = clock_divider(src_rate, rate);
+	if (val & PRCM_CLK_MGT_CLK38) {
+		if (clk_mgt[clock].clk38div) {
+			if (div > 2)
+				div = 2;
+		} else {
+			div = 1;
+		}
+	} else if ((clock == PRCMU_SGACLK) && (div == 3)) {
+		u64 r = (src_rate * 10);
+
+		(void)do_div(r, 25);
+		if (r <= rate)
+			return (unsigned long)r;
+	}
+	rounded_rate = (src_rate / div);
+
+	return rounded_rate;
+}
+
+long prcmu_round_clock_rate(u8 clock, unsigned long rate)
+{
+	if (clock < PRCMU_NUM_REG_CLOCKS)
+		return round_clock_rate(clock, rate);
+	else
+		return (long)prcmu_clock_rate(clock);
+}
+
+static void set_clock_rate(u8 clock, unsigned long rate)
+{
+	u32 val;
+	u32 div;
+	unsigned long src_rate;
+	unsigned long flags;
+
+	spin_lock_irqsave(&clk_mgt_lock, flags);
+
+	/* Grab the HW semaphore. */
+	while ((readl(_PRCMU_BASE + PRCM_SEM) & PRCM_SEM_PRCM_SEM) != 0)
+		cpu_relax();
+
+	val = readl(_PRCMU_BASE + clk_mgt[clock].offset);
+	src_rate = clock_source_rate((val | clk_mgt[clock].pllsw),
+		clk_mgt[clock].branch);
+	div = clock_divider(src_rate, rate);
+	if (val & PRCM_CLK_MGT_CLK38) {
+		if (clk_mgt[clock].clk38div) {
+			if (div > 1)
+				val |= PRCM_CLK_MGT_CLK38DIV;
+			else
+				val &= ~PRCM_CLK_MGT_CLK38DIV;
+		}
+	} else if (clock == PRCMU_SGACLK) {
+		val &= ~(PRCM_CLK_MGT_CLKPLLDIV_MASK |
+			PRCM_SGACLK_MGT_SGACLKDIV_BY_2_5_EN);
+		if (div == 3) {
+			u64 r = (src_rate * 10);
+
+			(void)do_div(r, 25);
+			if (r <= rate) {
+				val |= PRCM_SGACLK_MGT_SGACLKDIV_BY_2_5_EN;
+				div = 0;
+			}
+		}
+		val |= div;
+	} else {
+		val &= ~PRCM_CLK_MGT_CLKPLLDIV_MASK;
+		val |= div;
+	}
+	writel(val, (_PRCMU_BASE + clk_mgt[clock].offset));
+
+	/* Release the HW semaphore. */
+	writel(0, (_PRCMU_BASE + PRCM_SEM));
+
+	spin_unlock_irqrestore(&clk_mgt_lock, flags);
+}
+
+int prcmu_set_clock_rate(u8 clock, unsigned long rate)
+{
+	if (clock < PRCMU_NUM_REG_CLOCKS)
+		set_clock_rate(clock, rate);
+	return 0;
+}
+
 int prcmu_config_esram0_deep_sleep(u8 state)
 {
 	if ((state > ESRAM0_DEEP_SLEEP_STATE_RET) ||
@@ -1804,41 +1929,6 @@ int prcmu_load_a9wdog(u8 id, u32 timeout)
 }
 
 /**
- * prcmu_set_clock_divider() - Configure the clock divider.
- * @clock:	The clock for which the request is made.
- * @divider:	The clock divider. (< 32)
- *
- * This function should only be used by the clock implementation.
- * Do not use it from any other place!
- */
-int prcmu_set_clock_divider(u8 clock, u8 divider)
-{
-	u32 val;
-	unsigned long flags;
-
-	if ((clock >= PRCMU_NUM_REG_CLOCKS) || (divider < 1) || (31 < divider))
-		return -EINVAL;
-
-	spin_lock_irqsave(&clk_mgt_lock, flags);
-
-	/* Grab the HW semaphore. */
-	while ((readl(_PRCMU_BASE + PRCM_SEM) & PRCM_SEM_PRCM_SEM) != 0)
-		cpu_relax();
-
-	val = readl(_PRCMU_BASE + clk_mgt[clock].offset);
-	val &= ~(PRCM_CLK_MGT_CLKPLLDIV_MASK);
-	val |= (u32)divider;
-	writel(val, (_PRCMU_BASE + clk_mgt[clock].offset));
-
-	/* Release the HW semaphore. */
-	writel(0, (_PRCMU_BASE + PRCM_SEM));
-
-	spin_unlock_irqrestore(&clk_mgt_lock, flags);
-
-	return 0;
-}
-
-/**
  * prcmu_abb_read() - Read register value(s) from the ABB.
  * @slave:	The I2C slave address.
  * @reg:	The (start) register address.