From 2f5dcfef1db42f3b073ae657f8a94925abecd768 Mon Sep 17 00:00:00 2001
From: Andrew Thoelke <andrew.thoelke@arm.com>
Date: Mon, 28 Apr 2014 12:06:18 +0100
Subject: [PATCH] Replace disable_mmu with assembler version

disable_mmu() cannot work as a C function as there is no control
over data accesses generated by the compiler between disabling and
cleaning the data cache. This results in reading stale data from
main memory.

As assembler version is provided for EL3, and a variant that also
disables the instruction cache which is now used by the BL1
exception handling function.

Fixes ARM-software/tf-issues#147

Change-Id: I0cf394d2579a125a23c2f2989c2e92ace6ddb1a6
---
 bl1/aarch64/bl1_exceptions.S       | 12 ++----------
 include/lib/aarch64/arch_helpers.h |  3 +++
 lib/aarch64/misc_helpers.S         | 27 +++++++++++++++++++++++++++
 plat/fvp/aarch64/plat_common.c     | 23 -----------------------
 plat/fvp/platform.h                |  1 -
 5 files changed, 32 insertions(+), 34 deletions(-)

diff --git a/bl1/aarch64/bl1_exceptions.S b/bl1/aarch64/bl1_exceptions.S
index 7f930d837..71fd4cd75 100644
--- a/bl1/aarch64/bl1_exceptions.S
+++ b/bl1/aarch64/bl1_exceptions.S
@@ -212,18 +212,10 @@ func process_exception
 	/* ---------------------------------------------
 	 * If BL31 is to be executed in EL3 as well
 	 * then turn off the MMU so that it can perform
-	 * its own setup. TODO: Assuming flat mapped
-	 * translations here. Also all should go into a
-	 * separate MMU teardown function
+	 * its own setup.
 	 * ---------------------------------------------
 	 */
-	mov	x1, #(SCTLR_M_BIT | SCTLR_C_BIT | SCTLR_I_BIT)
-	bl	read_sctlr_el3
-	bic	x0, x0, x1
-	bl	write_sctlr_el3
-	isb
-	mov	x0, #DCCISW
-	bl	dcsw_op_all
+	bl	disable_mmu_icache_el3
 	bl	tlbialle3
 skip_mmu_teardown:
 	ldp     x6, x7, [sp, #0x30]
diff --git a/include/lib/aarch64/arch_helpers.h b/include/lib/aarch64/arch_helpers.h
index 565b1b488..08fb7f788 100644
--- a/include/lib/aarch64/arch_helpers.h
+++ b/include/lib/aarch64/arch_helpers.h
@@ -78,6 +78,9 @@ extern void inv_dcache_range(unsigned long, unsigned long);
 extern void dcsw_op_louis(unsigned int);
 extern void dcsw_op_all(unsigned int);
 
+extern void disable_mmu_el3(void);
+extern void disable_mmu_icache_el3(void);
+
 /*******************************************************************************
  * Misc. accessor prototypes
  ******************************************************************************/
diff --git a/lib/aarch64/misc_helpers.S b/lib/aarch64/misc_helpers.S
index c33ade28d..bccc936ad 100644
--- a/lib/aarch64/misc_helpers.S
+++ b/lib/aarch64/misc_helpers.S
@@ -79,6 +79,9 @@
 	.globl	zeromem16
 	.globl	memcpy16
 
+	.globl	disable_mmu_el3
+	.globl	disable_mmu_icache_el3
+
 
 func get_afflvl_shift
 	cmp	x0, #3
@@ -332,3 +335,27 @@ m_loop1:
 	subs	x2, x2, #1
 	b.ne	m_loop1
 m_end:	ret
+
+/* ---------------------------------------------------------------------------
+ * Disable the MMU at EL3
+ * This is implemented in assembler to ensure that the data cache is cleaned
+ * and invalidated after the MMU is disabled without any intervening cacheable
+ * data accesses
+ * ---------------------------------------------------------------------------
+ */
+
+func disable_mmu_el3
+	mov	x1, #(SCTLR_M_BIT | SCTLR_C_BIT)
+do_disable_mmu:
+	mrs	x0, sctlr_el3
+	bic	x0, x0, x1
+	msr	sctlr_el3, x0
+	isb				// ensure MMU is off
+	mov	x0, #DCCISW		// DCache clean and invalidate
+	b	dcsw_op_all
+
+
+func disable_mmu_icache_el3
+	mov	x1, #(SCTLR_M_BIT | SCTLR_C_BIT | SCTLR_I_BIT)
+	b	do_disable_mmu
+
diff --git a/plat/fvp/aarch64/plat_common.c b/plat/fvp/aarch64/plat_common.c
index e2f234372..edeb6e0b4 100644
--- a/plat/fvp/aarch64/plat_common.c
+++ b/plat/fvp/aarch64/plat_common.c
@@ -118,29 +118,6 @@ void enable_mmu()
 	return;
 }
 
-void disable_mmu(void)
-{
-	unsigned long sctlr;
-	unsigned long current_el = read_current_el();
-
-	if (GET_EL(current_el) == MODE_EL3) {
-		sctlr = read_sctlr_el3();
-		sctlr = sctlr & ~(SCTLR_M_BIT | SCTLR_C_BIT);
-		write_sctlr_el3(sctlr);
-	} else {
-		sctlr = read_sctlr_el1();
-		sctlr = sctlr & ~(SCTLR_M_BIT | SCTLR_C_BIT);
-		write_sctlr_el1(sctlr);
-	}
-	/* ensure the MMU disable takes effect immediately */
-	isb();
-
-	/* Flush the caches */
-	dcsw_op_all(DCCISW);
-
-	return;
-}
-
 /*
  * Table of regions to map using the MMU.
  * This doesn't include TZRAM as the 'mem_layout' argument passed to to
diff --git a/plat/fvp/platform.h b/plat/fvp/platform.h
index 1f4e4324c..988fe236a 100644
--- a/plat/fvp/platform.h
+++ b/plat/fvp/platform.h
@@ -371,7 +371,6 @@ extern void bl2_plat_arch_setup(void);
 extern void bl31_plat_arch_setup(void);
 extern int platform_setup_pm(const struct plat_pm_ops **);
 extern unsigned int platform_get_core_pos(unsigned long mpidr);
-extern void disable_mmu(void);
 extern void enable_mmu(void);
 extern void configure_mmu(struct meminfo *,
 			  unsigned long,