ENGR00120520: Enable MMU for mx51 and mx35

MMU enable code is missed in mx51 and mx35 u-boot.
So add these codes.

Signed-off-by: Terry Lv <r65388@freescale.com>

5 files changed, 185 insertions, 30 deletions

diff --git a/cpu/arm1136/cpu.c b/cpu/arm1136/cpu.c
index a4afd89..71f039c 100644
--- a/cpu/arm1136/cpu.c
+++ b/cpu/arm1136/cpu.c
@@ -8,7 +8,7 @@
  * (C) Copyright 2002
  * Gary Jennejohn, DENX Software Engineering, <garyj@denx.de>
  *
- * (C) Copyright 2008-2009 Freescale Semiconductor, Inc.
+ * (C) Copyright 2008-2010 Freescale Semiconductor, Inc.
  *
  * See file CREDITS for list of people who contributed to this
  * project.
@@ -36,8 +36,65 @@
 #include <common.h>
 #include <command.h>
 #include <asm/system.h>
+#include <asm/cache-cp15.h>
+#include <asm/mmu.h>
 
-static void cache_flush(void);
+#define dcache_invalidate_all_l1()	\
+{	\
+	int i = 0;	\
+	/* Clean and Invalidate Entire Data Cache */       \
+	asm volatile ("mcr p15, 0, %0, c7, c14, 0;"	\
+			:	\
+			: "r"(i)	\
+			: "memory");	\
+	asm volatile ("mcr p15, 0, %0, c8, c7, 0;"	\
+			:	\
+			: "r"(i)	\
+			: "memory"); /* Invalidate i+d-TLBs */	\
+}
+
+#define dcache_disable_l1()	\
+{	\
+	int i = 0;	\
+	asm volatile ("mcr p15, 0, %0, c7, c6, 0;"	\
+			:	\
+			: "r"(i)); /* clear data cache */	\
+	asm volatile ("mrc p15, 0, %0, c1, c0, 0;"	\
+			: "=r"(i));	\
+	i &= (~0x0004);	/* disable DCache */	\
+			/* but not MMU and alignment faults */     \
+	asm volatile ("mcr p15, 0, %0, c1, c0, 0;"	\
+			:	\
+			: "r"(i));	\
+}
+
+#define icache_invalidate_all_l1()        \
+{	\
+	/* this macro can discard dirty cache lines (N/A for ICache) */	\
+	int i = 0;	\
+	asm volatile ("mcr p15, 0, %0, c7, c5, 0;"	\
+			:	\
+			: "r"(i)); /* flush ICache */	\
+	asm volatile ("mcr p15, 0, %0, c8, c5, 0;"	\
+			:	\
+			: "r"(i)); /* flush ITLB only */	\
+	asm volatile ("mcr p15, 0, %0, c7, c5, 4;"	\
+			:	\
+			: "r"(i)); /* flush prefetch buffer */	\
+	asm (	\
+	"nop;" /* next few instructions may be via cache */	\
+	"nop;"	\
+	"nop;"	\
+	"nop;"	\
+	"nop;"	\
+	"nop;");	\
+}
+
+#define cache_flush()	\
+{	\
+	dcache_invalidate_all_l1();	\
+	icache_invalidate_all_l1();	\
+}
 
 int cleanup_before_linux (void)
 {
@@ -59,12 +116,16 @@ int cleanup_before_linux (void)
 		lcd_panel_disable();
 	}
 #endif
+	/* flush I/D-cache */
+	cache_flush();
 
 	/* turn off I/D-cache */
 	icache_disable();
 	dcache_disable();
-	/* flush I/D-cache */
-	cache_flush();
+
+	/* MMU Off */
+	MMU_OFF();
+
 /*Workaround to enable L2CC during kernel decompressing*/
 #ifdef fixup_before_linux
 	fixup_before_linux;
@@ -72,10 +133,3 @@ int cleanup_before_linux (void)
 	return 0;
 }
 
-static void cache_flush(void)
-{
-	unsigned long i = 0;
-
-	asm ("mcr p15, 0, %0, c7, c7, 0": :"r" (i));  /* invalidate both caches and flush btb */
-	asm ("mcr p15, 0, %0, c7, c10, 4": :"r" (i)); /* mem barrier to sync things */
-}
diff --git a/cpu/arm1136/mx35/generic.c b/cpu/arm1136/mx35/generic.c
index 7d1bbca..05feda4 100644
--- a/cpu/arm1136/mx35/generic.c
+++ b/cpu/arm1136/mx35/generic.c
@@ -27,6 +27,7 @@
 #include <asm/io.h>
 #include <asm/errno.h>
 #include <asm/arch/mx35.h>
+#include <asm/cache-cp15.h>
 #include "crm_regs.h"
 
 #define CLK_CODE(arm, ahb, sel) (((arm) << 16) + ((ahb) << 8) + (sel))
@@ -116,7 +117,7 @@ static u32 __decode_pll(u32 reg, u32 infreq)
 
 static u32 __get_mcu_main_clk(void)
 {
-	u32 arm_div, fi, fd;
+	u32 arm_div = 0, fi = 0, fd = 0;
 	arm_div = __get_arm_div(__REG(CCM_BASE_ADDR + CLKCTL_PDR0), &fi, &fd);
 	fi *=
 	    __decode_pll(__REG(MCU_PLL),
@@ -357,6 +358,11 @@ int print_cpuinfo(void)
 }
 #endif
 
+#if defined(CONFIG_MXC_FEC)
+extern int mxc_fec_initialize(bd_t *bis);
+extern void mxc_fec_set_mac_from_env(char *mac_addr);
+#endif
+
 /*
  * Initializes on-chip ethernet controllers.
  * to override, implement board_eth_init()
@@ -364,15 +370,16 @@ int print_cpuinfo(void)
 int cpu_eth_init(bd_t *bis)
 {
 	int rc = -ENODEV;
-	char *env = NULL;
 
 #if defined(CONFIG_MXC_FEC)
+	char *env = NULL;
+
 	rc = mxc_fec_initialize(bis);
-#endif
 
 	env = getenv("fec_addr");
 	if (env)
 		mxc_fec_set_mac_from_env(env);
+#endif
 
 	return rc;
 }
diff --git a/cpu/arm926ejs/mx25/generic.c b/cpu/arm926ejs/mx25/generic.c
index a47d796..5155f9a 100644
--- a/cpu/arm926ejs/mx25/generic.c
+++ b/cpu/arm926ejs/mx25/generic.c
@@ -112,6 +112,13 @@ int print_cpuinfo(void)
 	mx25_dump_clocks();
 	return 0;
 }
+#endif
+
+#if defined(CONFIG_MXC_FEC)
+extern int mxc_fec_initialize(bd_t *bis);
+extern void mxc_fec_set_mac_from_env(char *mac_addr);
+#endif
+
 /*
  * Initializes on-chip ethernet controllers.
  * to override, implement board_eth_init()
@@ -119,16 +126,17 @@ int print_cpuinfo(void)
 int cpu_eth_init(bd_t *bis)
 {
 	int rc = -ENODEV;
-	char *env = NULL;
 
 #if defined(CONFIG_MXC_FEC)
+	char *env = NULL;
+
 	rc = mxc_fec_initialize(bis);
-#endif
 
 	env = getenv("fec_addr");
 	if (env)
 		mxc_fec_set_mac_from_env(env);
+#endif
 
 	return rc;
 }
-#endif
+
diff --git a/cpu/arm_cortexa8/cpu.c b/cpu/arm_cortexa8/cpu.c
index df96804..e4bd7cf 100644
--- a/cpu/arm_cortexa8/cpu.c
+++ b/cpu/arm_cortexa8/cpu.c
@@ -35,6 +35,8 @@
 #include <command.h>
 #include <asm/system.h>
 #include <asm/cache.h>
+#include <asm/cache-cp15.h>
+#include <asm/mmu.h>
 
 #ifndef CONFIG_L2_OFF
 #ifndef CONFIG_MXC
@@ -42,7 +44,88 @@
 #endif
 #endif
 
-static void cache_flush(void);
+#define cache_flush(void)	\
+{	\
+	asm volatile (	\
+		"stmfd sp!, {r0-r5, r7, r9-r11};"	\
+		"mrc        p15, 1, r0, c0, c0, 1;" /*@ read clidr*/	\
+		/* @ extract loc from clidr */       \
+		"ands       r3, r0, #0x7000000;"	\
+		/* @ left align loc bit field*/      \
+		"mov        r3, r3, lsr #23;"	\
+		/* @ if loc is 0, then no need to clean*/    \
+		"beq        555f;" /* finished;" */	\
+		/* @ start clean at cache level 0*/  \
+		"mov        r10, #0;"	\
+		"111:" /*"loop1: */	\
+		/* @ work out 3x current cache level */	\
+		"add        r2, r10, r10, lsr #1;"	\
+		/* @ extract cache type bits from clidr */    \
+		"mov        r1, r0, lsr r2;"	\
+		/* @ mask of the bits for current cache only */	\
+		"and        r1, r1, #7;"	\
+		/* @ see what cache we have at this level*/  \
+		"cmp        r1, #2;"	\
+		/* @ skip if no cache, or just i-cache*/	\
+		"blt        444f;" /* skip;" */	\
+		/* @ select current cache level in cssr*/   \
+		"mcr        p15, 2, r10, c0, c0, 0;"	\
+		/* @ isb to sych the new cssr&csidr */	\
+		"mcr        p15, 0, r10, c7, c5, 4;"	\
+		/* @ read the new csidr */    \
+		"mrc        p15, 1, r1, c0, c0, 0;"	\
+		/* @ extract the length of the cache lines */ \
+		"and        r2, r1, #7;"	\
+		/* @ add 4 (line length offset) */   \
+		"add        r2, r2, #4;"	\
+		"ldr        r4, =0x3ff;"	\
+		/* @ find maximum number on the way size*/   \
+		"ands       r4, r4, r1, lsr #3;"	\
+		/*"clz  r5, r4;" @ find bit position of way size increment*/ \
+		".word 0xE16F5F14;"	\
+		"ldr        r7, =0x7fff;"	\
+		/* @ extract max number of the index size*/  \
+		"ands       r7, r7, r1, lsr #13;"	\
+		"222:" /* loop2:"  */	\
+		/* @ create working copy of max way size*/   \
+		"mov        r9, r4;"	\
+		"333:" /* loop3:"  */	\
+		/* @ factor way and cache number into r11*/  \
+		"orr        r11, r10, r9, lsl r5;"	\
+		/* @ factor index number into r11*/  \
+		"orr        r11, r11, r7, lsl r2;"	\
+		/* @ clean & invalidate by set/way */	\
+		"mcr        p15, 0, r11, c7, c14, 2;"	\
+		/* @ decrement the way */	\
+		"subs       r9, r9, #1;"	\
+		"bge        333b;" /* loop3;" */	\
+		/* @ decrement the index */	\
+		"subs       r7, r7, #1;"	\
+		"bge        222b;" /* loop2;" */	\
+		"444:" /* skip: */	\
+		/*@ increment cache number */	\
+		"add        r10, r10, #2;"	\
+		"cmp        r3, r10;" 	\
+		"bgt        111b;" /* loop1; */	\
+		"555:" /* "finished:" */	\
+		/* @ swith back to cache level 0 */	\
+		"mov        r10, #0;"	\
+		/* @ select current cache level in cssr */	\
+		"mcr        p15, 2, r10, c0, c0, 0;"	\
+		/* @ isb to sych the new cssr&csidr */	\
+		"mcr        p15, 0, r10, c7, c5, 4;" 	\
+		"ldmfd 	    sp!, {r0-r5, r7, r9-r11};"	\
+		"666:" /* iflush:" */	\
+		"mov        r0, #0x0;"	\
+		/* @ invalidate I+BTB */	\
+		"mcr        p15, 0, r0, c7, c5, 0;" 	\
+		/* @ drain WB */	\
+		"mcr        p15, 0, r0, c7, c10, 4;"	\
+		:	\
+		:	\
+		: "r0" /* Clobber list */	\
+	);	\
+}
 
 int cleanup_before_linux(void)
 {
@@ -56,13 +139,14 @@ int cleanup_before_linux(void)
 	 */
 	disable_interrupts();
 
+	/* flush cache */
+	cache_flush();
+
 	/* turn off I/D-cache */
 	icache_disable();
+	/* invalidate D-cache */
 	dcache_disable();
 
-	/* invalidate I-cache */
-	cache_flush();
-
 #ifndef CONFIG_L2_OFF
 	/* turn off L2 cache */
 	l2_cache_disable();
@@ -73,6 +157,9 @@ int cleanup_before_linux(void)
 	/* mem barrier to sync up things */
 	asm("mcr p15, 0, %0, c7, c10, 4": :"r"(i));
 
+	/* turn off MMU */
+	MMU_OFF();
+
 #ifndef CONFIG_L2_OFF
 	l2_cache_enable();
 #endif
@@ -80,7 +167,3 @@ int cleanup_before_linux(void)
 	return 0;
 }
 
-static void cache_flush(void)
-{
-	asm ("mcr p15, 0, %0, c7, c5, 0": :"r" (0));
-}
diff --git a/cpu/arm_cortexa8/mx51/generic.c b/cpu/arm_cortexa8/mx51/generic.c
index ee35dd9..0762c03 100644
--- a/cpu/arm_cortexa8/mx51/generic.c
+++ b/cpu/arm_cortexa8/mx51/generic.c
@@ -26,6 +26,7 @@
 #include <common.h>
 #include <asm/arch/mx51.h>
 #include <asm/errno.h>
+#include <asm/cache-cp15.h>
 #include "crm_regs.h"
 
 enum pll_clocks {
@@ -245,22 +246,24 @@ int print_cpuinfo(void)
  * Initializes on-chip ethernet controllers.
  * to override, implement board_eth_init()
  */
- #if defined(CONFIG_MXC_FEC)
- extern int mxc_fec_initialize(bd_t *bis);
- #endif
+#if defined(CONFIG_MXC_FEC)
+extern int mxc_fec_initialize(bd_t *bis);
+extern void mxc_fec_set_mac_from_env(char *mac_addr);
+#endif
 
 int cpu_eth_init(bd_t *bis)
 {
 	int rc = -ENODEV;
-	char *env = NULL;
 
 #if defined(CONFIG_MXC_FEC)
+	char *env = NULL;
+
 	rc = mxc_fec_initialize(bis);
-#endif
 
 	env = getenv("fec_addr");
 	if (env)
 		mxc_fec_set_mac_from_env(env);
+#endif
 
 	return rc;
 }