1 files changed, 71 insertions, 0 deletions

diff --git a/src/arch/armv7/bootblock_simple.c b/src/arch/armv7/bootblock_simple.c
index 0132b87bad..7012e17807 100644
--- a/src/arch/armv7/bootblock_simple.c
+++ b/src/arch/armv7/bootblock_simple.c
@@ -20,6 +20,7 @@
  */
 
 #include <bootblock_common.h>
+#include <arch/cache.h>
 #include <arch/hlt.h>
 #include <arch/stages.h>
 #include <cbfs.h>
@@ -27,6 +28,58 @@
 
 #include "stages.c"
 
+static void armv7_invalidate_caches(void)
+{
+	uint32_t clidr;
+	int level;
+
+	/* Invalidate branch predictor */
+	bpiall();
+
+	/* Iterate thru each cache identified in CLIDR and invalidate */
+	clidr = read_clidr();
+	for (level = 0; level < 7; level++) {
+		unsigned int ctype = (clidr >> (level * 3)) & 0x7;
+		uint32_t csselr;
+
+		switch(ctype) {
+		case 0x0:
+			/* no cache */
+			break;
+		case 0x1:
+			/* icache only */
+			csselr = (level << 1) | 1;
+			write_csselr(csselr);
+			icache_invalidate_all();
+			break;
+		case 0x2:
+		case 0x4:
+			/* dcache only or unified cache */
+			dcache_invalidate_all();
+			break;
+		case 0x3:
+			/* separate icache and dcache */
+			csselr = (level << 1) | 1;
+			write_csselr(csselr);
+			icache_invalidate_all();
+
+			csselr = level < 1;
+			write_csselr(csselr);
+			dcache_invalidate_all();
+			break;
+		default:
+			/* reserved */
+			break;
+		}
+	}
+
+	/* Invalidate TLB */
+	/* FIXME: ARMv7 Architecture Ref. Manual claims that the distinction
+	 * instruction vs. data TLBs is deprecated in ARMv7. But that doesn't
+	 * really seem true for Cortex-A15? */
+	tlb_invalidate_all();
+}
+
 static int boot_cpu(void)
 {
 	/*
@@ -41,6 +94,24 @@ void main(void)
 {
 	const char *stage_name = "fallback/romstage";
 	void *entry;
+	uint32_t sctlr;
+
+	/* Globally disable MMU, caches, and branch prediction (these should
+	 * be disabled by default on reset) */
+	sctlr = read_sctlr();
+	sctlr &= ~(SCTLR_M | SCTLR_C | SCTLR_Z | SCTLR_I);
+	write_sctlr(sctlr);
+
+	armv7_invalidate_caches();
+
+	/*
+	 * Re-enable caches and branch prediction. MMU will be set up later.
+	 * Note: If booting from USB, we need to disable branch prediction
+	 * before copying from USB into RAM (FIXME: why?)
+	 */
+	sctlr = read_sctlr();
+	sctlr |= SCTLR_C | SCTLR_Z | SCTLR_I;
+	write_sctlr(sctlr);
 
 	if (boot_cpu()) {
 		bootblock_cpu_init();