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: I8eddef534724cd11aaad4a867d217f5df31a5302
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>
Reviewed-on: http://gerrit.lud.stericsson.com/gerrit/32139
Tested-by: Jonas ABERG <jonas.aberg@stericsson.com>

4 files changed, 176 insertions, 70 deletions

diff --git a/drivers/mfd/db8500-prcmu.c b/drivers/mfd/db8500-prcmu.c
index 36e07605db4..ec49a9581fd 100644
--- a/drivers/mfd/db8500-prcmu.c
+++ b/drivers/mfd/db8500-prcmu.c
@@ -439,7 +439,7 @@ enum {
 static DEFINE_SPINLOCK(clk_mgt_lock);
 
 #define CLK_MGT_ENTRY(_name, _branch, _clk38div)[PRCMU_##_name] = \
-	{ (PRCM_##_name##_MGT), 0 , _branch, _clk38div}
+	{ (PRCM_##_name##_MGT_OFF), 0 , _branch, _clk38div}
 struct clk_mgt clk_mgt[PRCMU_NUM_REG_CLOCKS] = {
 	CLK_MGT_ENTRY(SGACLK, PLL_DIV, false),
 	CLK_MGT_ENTRY(UARTCLK, PLL_FIX, true),
@@ -1527,11 +1527,12 @@ int db8500_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);
 
@@ -1565,40 +1566,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)
@@ -1610,15 +1612,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(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, 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 db8500_prcmu_config_esram0_deep_sleep(u8 state)
 {
 	if ((state > ESRAM0_DEEP_SLEEP_STATE_RET) ||
@@ -1780,41 +1905,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(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, 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.
diff --git a/include/linux/mfd/db5500-prcmu.h b/include/linux/mfd/db5500-prcmu.h
index f3f3115ae03..846486a0b76 100644
--- a/include/linux/mfd/db5500-prcmu.h
+++ b/include/linux/mfd/db5500-prcmu.h
@@ -77,6 +77,16 @@ static inline int db5500_prcmu_config_esram0_deep_sleep(u8 state)
 
 static inline void db5500_prcmu_enable_wakeups(u32 wakeups) {}
 
+static inline long db5500_prcmu_round_clock_rate(u8 clock, unsigned long rate)
+{
+	return 0;
+}
+
+static inline int db5500_prcmu_set_clock_rate(u8 clock, unsigned long rate)
+{
+	return 0;
+}
+
 static inline int prcmu_resetout(u8 resoutn, u8 state)
 {
 	return 0;
diff --git a/include/linux/mfd/db8500-prcmu.h b/include/linux/mfd/db8500-prcmu.h
index 60d27f7bfc1..c48d80dec00 100644
--- a/include/linux/mfd/db8500-prcmu.h
+++ b/include/linux/mfd/db8500-prcmu.h
@@ -514,7 +514,7 @@ void prcmu_configure_auto_pm(struct prcmu_auto_pm_config *sleep,
 bool prcmu_is_auto_pm_enabled(void);
 
 int prcmu_config_clkout(u8 clkout, u8 source, u8 div);
-int prcmu_set_clock_divider(u8 clock, u8 divider);
+
 int prcmu_config_hotdog(u8 threshold);
 int prcmu_config_hotmon(u8 low, u8 high);
 int prcmu_start_temp_sense(u16 cycles32k);
@@ -613,7 +613,6 @@ static inline int prcmu_set_hwacc(u16 hw_acc_dev, u8 state)
 {
 	return 0;
 }
-
 static inline void prcmu_configure_auto_pm(struct prcmu_auto_pm_config *sleep,
 	struct prcmu_auto_pm_config *idle)
 {
@@ -629,11 +628,6 @@ static inline int prcmu_config_clkout(u8 clkout, u8 source, u8 div)
 	return 0;
 }
 
-static inline int prcmu_set_clock_divider(u8 clock, u8 divider)
-{
-	return 0;
-}
-
 static inline int prcmu_config_hotdog(u8 threshold)
 {
 	return 0;
diff --git a/include/linux/mfd/dbx500-prcmu.h b/include/linux/mfd/dbx500-prcmu.h
index a0be8cba9fd..98367ad7d2b 100644
--- a/include/linux/mfd/dbx500-prcmu.h
+++ b/include/linux/mfd/dbx500-prcmu.h
@@ -291,6 +291,8 @@ static inline 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);
@@ -410,6 +412,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;