Call idcache_inv_all from the AP core entry code before turning on th…

…e MMU.

Also, enable instruction and branch caches, which should be safe now that
they're properly initialized/invalidated first.

sys/arm/arm/cpufunc_asm_armv7.S

@@ -319,6 +319,10 @@ ENTRY(armv7_auxctrl)
 	RET
 END(armv7_auxctrl)
 
+/*
+ * Invalidate all I+D+branch cache.  Used by startup code, which counts
+ * on the fact that only r0-r3,ip are modified and no stack space is used.
+ */
 ENTRY(armv7_idcache_inv_all)
 	mov     r0, #0
 	mcr     p15, 2, r0, c0, c0, 0   @ set cache level to L1

sys/arm/arm/locore.S

@@ -353,24 +353,24 @@ ASENTRY_NP(mpentry)
 	orr	r7, r7, #(I32_bit|F32_bit)
 	msr	cpsr_c, r7
 
-
-	adr     r7, Ltag
-	bic     r7, r7, #0xf0000000
-	orr     r7, r7, #PHYSADDR
-
-	/* Disable MMU for a while */
+	/* Disable MMU.  It should be disabled already, but make sure. */
 	mrc	p15, 0, r2, c1, c0, 0
 	bic	r2, r2, #(CPU_CONTROL_MMU_ENABLE | CPU_CONTROL_DC_ENABLE |\
 	    CPU_CONTROL_WBUF_ENABLE)
 	bic	r2, r2, #(CPU_CONTROL_IC_ENABLE)
 	bic	r2, r2, #(CPU_CONTROL_BPRD_ENABLE)
 	mcr	p15, 0, r2, c1, c0, 0
-
 	nop
 	nop
 	nop
+	CPWAIT(r0)
+
+#if defined(ARM_MMU_V6)
+	bl	armv6_idcache_inv_all	/* Modifies r0 only */
+#elif defined(ARM_MMU_V7)
+	bl	armv7_idcache_inv_all	/* Modifies r0-r3, ip */
+#endif
 
-Ltag:
 	ldr	r0, Lstartup_pagetable_secondary
 	bic	r0, r0, #0xf0000000
 	orr	r0, r0, #PHYSADDR
@@ -389,7 +389,10 @@ Ltag:
 	mrc	p15, 0, r0, c1, c0, 0
 	orr	r0, r0, #CPU_CONTROL_V6_EXTPAGE
 	orr	r0, r0, #CPU_CONTROL_AF_ENABLE
-	orr	r0, r0, #(CPU_CONTROL_MMU_ENABLE | CPU_CONTROL_DC_ENABLE)
+	orr	r0, r0, #(CPU_CONTROL_MMU_ENABLE | CPU_CONTROL_DC_ENABLE |\
+	    CPU_CONTROL_WBUF_ENABLE)
+	orr	r0, r0, #(CPU_CONTROL_IC_ENABLE)
+	orr	r0, r0, #(CPU_CONTROL_BPRD_ENABLE)
 	mcr	p15, 0, r0, c1, c0, 0
 	nop
 	nop