Index: boot/arch/arm32/include/cp15.h
===================================================================
--- boot/arch/arm32/include/cp15.h	(revision 8abcf4e7f08fa2a990c4bde86e31ad7865a13354)
+++ boot/arch/arm32/include/cp15.h	(revision 8abcf4e7f08fa2a990c4bde86e31ad7865a13354)
@@ -0,0 +1,1 @@
+../../../../kernel/arch/arm32/include/arch/cp15.h
Index: boot/arch/arm32/src/main.c
===================================================================
--- boot/arch/arm32/src/main.c	(revision 296a80e2d4d51e45c959768550ffb9137741db4b)
+++ boot/arch/arm32/src/main.c	(revision 8abcf4e7f08fa2a990c4bde86e31ad7865a13354)
@@ -53,20 +53,4 @@
 extern void *bdata_end;
 
-
-static inline void invalidate_icache(void)
-{
-	/* ICIALLU Invalidate entire ICache */
-	asm volatile ("mov r0, #0\n" "mcr p15, 0, r0, c7, c5, 0\n" ::: "r0" );
-}
-
-static inline void invalidate_dcache(void *address, size_t size)
-{
-	const uintptr_t addr = (uintptr_t)address;
-	for (uintptr_t a = addr; a < addr + size; a += 4) {
-		/* DCIMVAC - invalidate by address to the point of coherence */
-		asm volatile ("mcr p15, 0, %[a], c7, c6, 1\n" :: [a]"r"(a) : );
-	}
-}
-
 static inline void clean_dcache_poc(void *address, size_t size)
 {
@@ -82,11 +66,4 @@
 void bootstrap(void)
 {
-	/* Make sure we run in memory code when caches are enabled,
-	 * make sure we read memory data too. This part is ARMv7 specific as
-	 * ARMv7 no longer invalidates caches on restart.
-	 * See chapter B2.2.2 of ARM Architecture Reference Manual p. B2-1263*/
-	invalidate_icache();
-	invalidate_dcache(&bdata_start, &bdata_end - &bdata_start);
-
 	/* Enable MMU and caches */
 	mmu_start();
@@ -105,6 +82,4 @@
 		    components[i].start, components[i].name, components[i].inflated,
 		    components[i].size);
-		/* Make sure there is no cache garbage in read locations */
-		invalidate_dcache(components[i].start, components[i].size);
 	}
 	
Index: boot/arch/arm32/src/mm.c
===================================================================
--- boot/arch/arm32/src/mm.c	(revision 296a80e2d4d51e45c959768550ffb9137741db4b)
+++ boot/arch/arm32/src/mm.c	(revision 8abcf4e7f08fa2a990c4bde86e31ad7865a13354)
@@ -37,4 +37,50 @@
 #include <arch/asm.h>
 #include <arch/mm.h>
+#include <arch/cp15.h>
+
+#ifdef PROCESSOR_ARCH_armv7_a
+static unsigned log2(unsigned val)
+{
+	unsigned log = 0;
+	while (val >> log++);
+	return log - 2;
+}
+
+static void dcache_invalidate_level(unsigned level)
+{
+	CSSELR_write(level << 1);
+	const uint32_t ccsidr = CCSIDR_read();
+	const unsigned sets = CCSIDR_SETS(ccsidr);
+	const unsigned ways = CCSIDR_WAYS(ccsidr);
+	const unsigned line_log = CCSIDR_LINESIZE_LOG(ccsidr);
+	const unsigned set_shift = line_log;
+	const unsigned way_shift = 32 - log2(ways);
+
+	for (unsigned k = 0; k < ways; ++k)
+		for (unsigned j = 0; j < sets; ++j) {
+			const uint32_t val = (level << 1) |
+			    (j << set_shift) | (k << way_shift);
+			DCISW_write(val);
+		}
+}
+
+/** invalidate all dcaches -- armv7 */
+static void cache_invalidate(void)
+{
+	const uint32_t cinfo = CLIDR_read();
+	for (unsigned i = 0; i < 7; ++i) {
+		switch (CLIDR_CACHE(i, cinfo))
+		{
+		case CLIDR_DCACHE_ONLY:
+		case CLIDR_SEP_CACHE:
+		case CLIDR_UNI_CACHE:
+			dcache_invalidate_level(i);
+		}
+	}
+	asm volatile ( "dsb\n" );
+	ICIALLU_write(0);
+	asm volatile ( "isb\n" );
+}
+#endif
 
 /** Disable the MMU */
@@ -156,4 +202,11 @@
 void mmu_start() {
 	disable_paging();
+#ifdef PROCESSOR_ARCH_armv7_a
+	/* Make sure we run in memory code when caches are enabled,
+	 * make sure we read memory data too. This part is ARMv7 specific as
+	 * ARMv7 no longer invalidates caches on restart.
+	 * See chapter B2.2.2 of ARM Architecture Reference Manual p. B2-1263*/
+	cache_invalidate();
+#endif
 	init_boot_pt();
 	enable_paging();