[PATCH 2/2] arm: make disabling the cache actually work

[Uwe Kleine-König]

The code before looks to me like work in progress.  It only drained the
write buffer and skipped disabling mmu and caches by an early return
from inline assembler (which is broken in itself as it doesn't restore
frame pointer, stack pointer and other saved registers).

Signed-off-by: Uwe Kleine-König <u.kleine-koenig at pengutronix.de>
---
 arch/arm/cpu/mmu.c |   29 ++++++++++++++++++++++++++++-
 1 files changed, 28 insertions(+), 1 deletions(-)

diff --git a/arch/arm/cpu/mmu.c b/arch/arm/cpu/mmu.c
index 9e00927..95b8302 100644
--- a/arch/arm/cpu/mmu.c
+++ b/arch/arm/cpu/mmu.c
@@ -54,7 +54,7 @@ void mmu_enable(void)
 {
 	asm volatile (
 		"mrc p15, 0, r1, c1, c0, 0;"
-		"orr r1, r1, #0x0007;"  /* enable MMU + Dcache */
+		"orr r1, r1, #0x0007;"  /* enable MMU + Dcache + alignment fault checking */
 		"mcr p15, 0, r1, c1, c0, 0"
 		:
 		:
@@ -67,6 +67,30 @@ void mmu_enable(void)
  */
 void mmu_disable(void)
 {
+#if defined(CONFIG_CPU_ARM926T)
+	unsigned int tmp = 0;
+	asm volatile (
+		/* test, clean and invalidate D cache */
+		"1:	mrc	p15, 0, r15, c7, c14, 3\n"
+		"	bne	1b\n"
+		/* invalidate I cache (Rd SBZ) */
+		"	mcr	p15, 0, %1, c7, c5, 0\n"
+		/* drain write buffer (Rd SBZ) */
+		"	mcr	p15, 0, %1, c7, c10, 4\n"
+
+		/* read control register */
+		"	mrc	p15, 0, %0, c1, c0\n"
+		/* clear C and M */
+		"	bic	%0, %0, #0x0005\n"
+		/* write back control register */
+		"	mcr	p15, 0, %0, c1, c0\n"
+
+		/* invalidate TLBs (Rd SBZ) */
+		"	mcr p15, 0, %1, c8, c7, 0;"
+		: "=&r" (tmp) : "r" (tmp) : "cc" );
+
+#else
+	/* ??? */
 	asm volatile (
 		"nop; "
 		"nop; "
@@ -75,9 +99,11 @@ void mmu_disable(void)
 		"nop; "
 		"nop; "
 		/* test, clean and invalidate cache */
+		/* mrc     p15, 0, $rd, c7, c14, 3; is armv5tej specific */
 		"1:	mrc	p15, 0, r15, c7, c14, 3;" 
 		"	bne	1b;"
 		"	mov	pc, lr;"
+		/* the following code is never reached */
 		"	mov r0, #0x0;"
 		"	mcr p15, 0, r0, c7, c10, 4;" /* drain the write buffer   */
 		"	mcr p15, 0, r1, c7, c6, 0;"  /* clear data cache         */
@@ -90,6 +116,7 @@ void mmu_disable(void)
 		:
 		: "r0" /* Clobber list */
 	);
+#endif
 }
 
 /*
-- 
1.6.6