ENGR00180919 [MX6]Update clock tree if BUS freq is changed

As DDR freq change is by modifying CCM register directly,
we need to update the clock tree as well, or the clock
tree will be broken. Also, we need to make sure the clock
rate counting is right.

Signed-off-by: Anson Huang <b20788@freescale.com>

5 files changed, 98 insertions, 2 deletions

diff --git a/arch/arm/mach-mx6/bus_freq.c b/arch/arm/mach-mx6/bus_freq.c
index c7f8fef30923..b573f09c8920 100644
--- a/arch/arm/mach-mx6/bus_freq.c
+++ b/arch/arm/mach-mx6/bus_freq.c
@@ -91,12 +91,25 @@ struct timeval end_time;
 static int cpu_op_nr;
 static struct cpu_op *cpu_op_tbl;
 static struct clk *pll2_400;
+static struct clk *pll2_200;
 static struct clk *cpu_clk;
 static unsigned int org_ldo;
 static struct clk *pll3;
+static struct clk *pll2;
+static struct clk *periph_clk;
+static struct clk *osc;
 
 static struct delayed_work low_bus_freq_handler;
+extern void update_usecount(struct clk *clk, bool flag);
 
+static inline void update_periph_clk_parent(struct clk *new_parent)
+{
+	update_usecount(periph_clk->parent, false);
+
+	periph_clk->parent = new_parent;
+
+	update_usecount(periph_clk->parent, true);
+}
 static void reduce_bus_freq_handler(struct work_struct *work)
 {
 	unsigned long reg;
@@ -128,10 +141,15 @@ static void reduce_bus_freq_handler(struct work_struct *work)
 		/* Need to ensure that PLL2_PFD_400M is kept ON. */
 		clk_enable(pll2_400);
 		update_ddr_freq(50000000);
+		/* Make sure periph clk's parent also got updated */
+		update_periph_clk_parent(pll2_200);
+
 		audio_bus_freq_mode = 1;
 		low_bus_freq_mode = 0;
 	} else {
 		update_ddr_freq(24000000);
+		/* Make sure periph clk's parent also got updated */
+		update_periph_clk_parent(osc);
 		if (audio_bus_freq_mode)
 			clk_disable(pll2_400);
 		low_bus_freq_mode = 1;
@@ -230,6 +248,7 @@ int set_high_bus_freq(int high_bus_freq)
 	}
 	clk_enable(pll3);
 
+
 	/* Enable the PU LDO */
 	if (cpu_is_mx6q() && low_bus_freq_mode) {
 		__raw_writel(org_ldo, ANADIG_REG_CORE);
@@ -257,6 +276,8 @@ int set_high_bus_freq(int high_bus_freq)
 
 	if (high_bus_freq) {
 		update_ddr_freq(ddr_normal_rate);
+		/* Make sure periph clk's parent also got updated */
+		update_periph_clk_parent(pll2);
 		if (med_bus_freq_mode)
 			clk_disable(pll2_400);
 		high_bus_freq_mode = 1;
@@ -264,6 +285,8 @@ int set_high_bus_freq(int high_bus_freq)
 	} else {
 		clk_enable(pll2_400);
 		update_ddr_freq(ddr_med_rate);
+		/* Make sure periph clk's parent also got updated */
+		update_periph_clk_parent(pll2_400);
 		high_bus_freq_mode = 0;
 		med_bus_freq_mode = 1;
 	}
@@ -372,6 +395,20 @@ static int __devinit busfreq_probe(struct platform_device *pdev)
 		return PTR_ERR(pll2_400);
 	}
 
+	pll2_200 = clk_get(NULL, "pll2_200M");
+	if (IS_ERR(pll2_400)) {
+		printk(KERN_DEBUG "%s: failed to get pll2_200M\n",
+		       __func__);
+		return PTR_ERR(pll2_200);
+	}
+
+	pll2 = clk_get(NULL, "pll2");
+	if (IS_ERR(pll2_400)) {
+		printk(KERN_DEBUG "%s: failed to get pll2\n",
+		       __func__);
+		return PTR_ERR(pll2);
+	}
+
 	cpu_clk = clk_get(NULL, "cpu_clk");
 	if (IS_ERR(cpu_clk)) {
 		printk(KERN_DEBUG "%s: failed to get cpu_clk\n",
@@ -383,7 +420,21 @@ static int __devinit busfreq_probe(struct platform_device *pdev)
 	if (IS_ERR(pll3)) {
 		printk(KERN_DEBUG "%s: failed to get pll3\n",
 		       __func__);
-		return PTR_ERR(cpu_clk);
+		return PTR_ERR(pll3);
+	}
+
+	periph_clk = clk_get(NULL, "periph_clk");
+	if (IS_ERR(periph_clk)) {
+		printk(KERN_DEBUG "%s: failed to get periph\n",
+		       __func__);
+		return PTR_ERR(periph_clk);
+	}
+
+	osc = clk_get(NULL, "osc");
+	if (IS_ERR(osc)) {
+		printk(KERN_DEBUG "%s: failed to get osc\n",
+		       __func__);
+		return PTR_ERR(osc);
 	}
 
 	err = sysfs_create_file(&busfreq_dev->kobj, &dev_attr_enable.attr);
diff --git a/arch/arm/mach-mx6/clock.c b/arch/arm/mach-mx6/clock.c
index 2f58ca3b882f..975f22cc2263 100644
--- a/arch/arm/mach-mx6/clock.c
+++ b/arch/arm/mach-mx6/clock.c
@@ -499,6 +499,9 @@ static void _clk_pll_disable(struct clk *clk)
 	unsigned int reg;
 	void __iomem *pllbase;
 
+	if ((arm_needs_pll2_400) && (clk == &pll2_528_bus_main_clk))
+		return;
+
 	pllbase = _get_pll_base(clk);
 
 	reg = __raw_readl(pllbase);
@@ -520,6 +523,10 @@ static unsigned long  _clk_pll1_main_get_rate(struct clk *clk)
 	unsigned int div;
 	unsigned long val;
 
+	/* If PLL1 is bypassed, its rate will be from OSC directly */
+	if (__raw_readl(PLL1_SYS_BASE_ADDR) & ANADIG_PLL_SYS_BYPASS_MASK)
+		return clk_get_rate(clk->parent);
+
 	div = __raw_readl(PLL1_SYS_BASE_ADDR) & ANADIG_PLL_SYS_DIV_SELECT_MASK;
 	val = (clk_get_rate(clk->parent) * div) / 2;
 	return val;
@@ -5288,7 +5295,6 @@ int __init mx6_clocks_init(unsigned long ckil, unsigned long osc,
 	/* keep correct count. */
 	clk_enable(&cpu_clk);
 	clk_enable(&periph_clk);
-
 	/* Disable un-necessary PFDs & PLLs */
 	if (pll2_pfd_400M.usecount == 0 && cpu_is_mx6q())
 		pll2_pfd_400M.disable(&pll2_pfd_400M);
diff --git a/arch/arm/mach-mx6/crm_regs.h b/arch/arm/mach-mx6/crm_regs.h
index 188dbaebd57f..52fd75db9eb0 100644
--- a/arch/arm/mach-mx6/crm_regs.h
+++ b/arch/arm/mach-mx6/crm_regs.h
@@ -76,6 +76,8 @@
 /* PLL1_SYS defines */
 #define ANADIG_PLL_SYS_DIV_SELECT_MASK		(0x7F)
 #define ANADIG_PLL_SYS_DIV_SELECT_OFFSET	(0)
+#define ANADIG_PLL_SYS_BYPASS_MASK		(0x10000)
+#define ANADIG_PLL_SYS_BYPASS_OFFSET	(16)
 
 /* PLL2_528 defines */
 #define ANADIG_PLL_528_DIV_SELECT		(1)
diff --git a/arch/arm/mach-mx6/mx6_ddr_freq.S b/arch/arm/mach-mx6/mx6_ddr_freq.S
index fbaa44df8cdc..770b9cf1ba72 100644
--- a/arch/arm/mach-mx6/mx6_ddr_freq.S
+++ b/arch/arm/mach-mx6/mx6_ddr_freq.S
@@ -77,6 +77,9 @@ set_ahb_podf_before_switch:
 	ldr     r2, =0x381D00
 	bic     r0, r0, r2
 	orr     r0, r0, #0xD00
+	/* Make sure AXI clock divider is 1 */
+	bic     r0, r0, #0x70000
+	orr     r0, r0, #0x10000
 	str     r0, [r6, #0x14]
 
 wait_div_update528_1:
@@ -182,6 +185,9 @@ periph_clk_switch6:
 	ldr     r2, =0x381D00
 	bic     r0, r0, r2
 	orr     r0, r0, #0x900
+	/* Make sure AXI clock divider is 1 */
+	bic     r0, r0, #0x70000
+	orr     r0, r0, #0x10000
 	str     r0, [r6, #0x14]
 
 wait_div_update400_2:
diff --git a/arch/arm/plat-mxc/clock.c b/arch/arm/plat-mxc/clock.c
index 43d33376b52d..a55b3fbfb94a 100755
--- a/arch/arm/plat-mxc/clock.c
+++ b/arch/arm/plat-mxc/clock.c
@@ -209,6 +209,37 @@ int clk_get_usecount(struct clk *clk)
 
 EXPORT_SYMBOL(clk_get_usecount);
 
+/*!
+ * @brief Function to update the usage count for the requested clock.
+ *
+ * This function returns none.
+ *
+ * @param clk 	clk we want to update.
+ * @param flag 	Increase or decrease usecount.
+ *
+ * @return Returns none.
+ */
+void update_usecount(struct clk *clk, bool flag)
+{
+	if (!flag) {
+		if (clk_get_usecount(clk) > 1) {
+			mutex_lock(&clocks_mutex);
+			clk->usecount--;
+			mutex_unlock(&clocks_mutex);
+		} else
+			clk_disable(clk);
+	} else {
+		if (clk_get_usecount(clk) < 1)
+			clk_enable(clk);
+		else {
+			mutex_lock(&clocks_mutex);
+			clk->usecount++;
+			mutex_unlock(&clocks_mutex);
+		}
+	}
+}
+EXPORT_SYMBOL(update_usecount);
+
 /* Retrieve the *current* clock rate. If the clock itself
  * does not provide a special calculation routine, ask
  * its parent and so on, until one is able to return