From 782b02884126cc258056e8bf581a410a934f7372 Mon Sep 17 00:00:00 2001
From: Fabio Estevam <fabio.estevam@freescale.com>
Date: Mon, 15 Oct 2012 05:37:16 +0000
Subject: [PATCH] mx5: lowlevel_init.S: Fix PLL settings for mx53

Currently PLL2 is not explicitely configured for mx53 and it runs at 333MHz.

Since PLL2 is the parent clock for DDR2, IPU, VPU, we should set it at 400MHz
instead.

Without doing so, it is not possible to use a 2.6.35 FSL kernel and display HDMI
at 1080p because the IPU clock cannot reach the requested frequency.

Set PLL2 to 400MHz, so that 1080p can be played and the DDR2 can run at its
maximum frequency.

Also, setup the other PLL's as done in FSL U-boot and re-arrange the code a little
bit to allow easier comparison with the original clock setup from FSL U-boot.

Signed-off-by: Fabio Estevam <fabio.estevam@freescale.com>
---
 arch/arm/cpu/armv7/mx5/lowlevel_init.S   | 96 +++++++++++++++++++++-----------
 arch/arm/include/asm/arch-mx5/imx-regs.h |  4 ++
 2 files changed, 67 insertions(+), 33 deletions(-)

diff --git a/arch/arm/cpu/armv7/mx5/lowlevel_init.S b/arch/arm/cpu/armv7/mx5/lowlevel_init.S
index d0bab45..d62093b 100644
--- a/arch/arm/cpu/armv7/mx5/lowlevel_init.S
+++ b/arch/arm/cpu/armv7/mx5/lowlevel_init.S
@@ -296,20 +296,66 @@ setup_pll_func:
 
 	setup_pll PLL1_BASE_ADDR, 800
 
-	setup_pll PLL3_BASE_ADDR, 216
+        setup_pll PLL3_BASE_ADDR, 400
+
+        /* Switch peripheral to PLL3 */
+        ldr r0, =CCM_BASE_ADDR
+        ldr r1, =0x00015154
+        str r1, [r0, #CLKCTL_CBCMR]
+        ldr r1, =0x02888945
+        orr r1, r1, #(1 << 16)
+        str r1, [r0, #CLKCTL_CBCDR]
+        /* make sure change is effective */
+1:      ldr r1, [r0, #CLKCTL_CDHIPR]
+        cmp r1, #0x0
+        bne 1b
+
+        setup_pll PLL2_BASE_ADDR, 400
+
+	/* Switch peripheral to PLL2 */
+	ldr r0, =CCM_BASE_ADDR
+	ldr r1, =0x00808145
+	orr r1, r1, #(2 << 10)
+	orr r1, r1, #(0 << 16)
+	orr r1, r1, #(1 << 19)
+	str r1, [r0, #CLKCTL_CBCDR]
+
+	ldr r1, =0x00016154
+	str r1, [r0, #CLKCTL_CBCMR]
+
+	/*change uart clk parent to pll2*/
+	ldr r1, [r0, #CLKCTL_CSCMR1]
+	and r1, r1, #0xfcffffff
+	orr r1, r1, #0x01000000
+	str r1, [r0, #CLKCTL_CSCMR1]
+
+	/* make sure change is effective */
+1:      ldr r1, [r0, #CLKCTL_CDHIPR]
+	cmp r1, #0x0
+	bne 1b
+
+        setup_pll PLL3_BASE_ADDR, 216
+
+	setup_pll PLL4_BASE_ADDR, 455
 
 	/* Set the platform clock dividers */
 	ldr r0, =ARM_BASE_ADDR
-	ldr r1, =0x00000725
+	ldr r1, =0x00000124
 	str r1, [r0, #0x14]
 
 	ldr r0, =CCM_BASE_ADDR
-
 	mov r1, #0
 	str r1, [r0, #CLKCTL_CACRR]
 
-	/* Switch ARM back to PLL 1 */
-	str r4, [r0, #CLKCTL_CCSR]
+	/* Switch ARM back to PLL 1. */
+	mov r1, #0x0
+	str r1, [r0, #CLKCTL_CCSR]
+
+	/* make uart div=6 */
+	ldr r1, [r0, #CLKCTL_CSCDR1]
+	and r1, r1, #0xffffffc0
+	orr r1, r1, #0x0a
+	str r1, [r0, #CLKCTL_CSCDR1]
 
 	/* Restore the default values in the Gate registers */
 	ldr r1, =0xFFFFFFFF
@@ -322,36 +368,14 @@ setup_pll_func:
 	str r1, [r0, #CLKCTL_CCGR6]
 	str r1, [r0, #CLKCTL_CCGR7]
 
-	/* Switch peripheral to PLL2 */
-	ldr r0, =CCM_BASE_ADDR
-	ldr r1, =0x00808145
-	orr r1, r1, #2 << 10
-	orr r1, r1, #1 << 19
-	str r1, [r0, #CLKCTL_CBCDR]
+        mov r1, #0x00000
+        str r1, [r0, #CLKCTL_CCDR]
 
-	ldr r1, =0x00016154
-	str r1, [r0, #CLKCTL_CBCMR]
-	/* Change uart clk parent to pll2*/
-	ldr r1, [r0, #CLKCTL_CSCMR1]
-	and r1, r1, #0xfcffffff
-	orr r1, r1, #0x01000000
-	str r1, [r0, #CLKCTL_CSCMR1]
-	ldr r1, [r0, #CLKCTL_CSCDR1]
-	and r1, r1, #0xffffffc0
-	orr r1, r1, #0x0a
-	str r1, [r0, #CLKCTL_CSCDR1]
-
-	/* make sure divider effective */
-1:	ldr r1, [r0, #CLKCTL_CDHIPR]
-	cmp r1, #0x0
-	bne 1b
+        /* for cko - for ARM div by 8 */
+        mov r1, #0x000A0000
+        add r1, r1, #0x00000F0
+        str r1, [r0, #CLKCTL_CCOSR]
 
-	str r4, [r0, #CLKCTL_CCDR]
-
-	/* for cko - for ARM div by 8 */
-	mov r1, #0x000A0000
-	add r1, r1, #0x00000F0
-	str r1, [r0, #CLKCTL_CCOSR]
 #endif	/* CONFIG_MX53 */
 .endm
 
@@ -405,3 +429,9 @@ W_DP_665:		.word DP_OP_665
 W_DP_216:		.word DP_OP_216
 			.word DP_MFD_216
 			.word DP_MFN_216
+W_DP_400:               .word DP_OP_400
+			.word DP_MFD_400
+			.word DP_MFN_400
+W_DP_455:               .word DP_OP_455
+			.word DP_MFD_455
+			.word DP_MFN_455
diff --git a/arch/arm/include/asm/arch-mx5/imx-regs.h b/arch/arm/include/asm/arch-mx5/imx-regs.h
index 46017f4..1d060fd 100644
--- a/arch/arm/include/asm/arch-mx5/imx-regs.h
+++ b/arch/arm/include/asm/arch-mx5/imx-regs.h
@@ -308,6 +308,10 @@
 #define DP_MFD_400	(3 - 1)
 #define DP_MFN_400	1
 
+#define DP_OP_455	((9 << 4) + ((2 - 1)  << 0))
+#define DP_MFD_455	(48 - 1)
+#define DP_MFN_455	23
+
 #define DP_OP_216	((6 << 4) + ((3 - 1)  << 0))
 #define DP_MFD_216	(4 - 1)
 #define DP_MFN_216	3
-- 
2.7.4