- mrc p15, 0, r10, c0, c0, 1 @ read cache type register
- tst r10, #1 << 24 @ Harvard cache?
- mov r10, #0
- mcrne p15, 0, r10, c7, c14, 0 @ clean+invalidate D cache
- mcrne p15, 0, r10, c7, c5, 0 @ invalidate I cache
- mcreq p15, 0, r10, c7, c15, 0 @ clean+invalidate cache
- mcr p15, 0, r10, c7, c10, 4 @ drain write buffer
- mcr p15, 0, r10, c8, c7, 0 @ invalidate I + D TLBs
- mcr p15, 0, r10, c2, c0, 2 @ TTB control register
- mcr p15, 0, r4, c2, c0, 0 @ load TTB0
+#ifdef CONFIG_SMP
+ /* Set up the SCU on core 0 only */
+ mrc p15, 0, r0, c0, c0, 5 @ CPU core number
+ ands r0, r0, #15
+ moveq r0, #0x10000000 @ SCU_BASE
+ orreq r0, r0, #0x00100000
+ ldreq r5, [r0, #SCU_CTRL]
+ orreq r5, r5, #1
+ streq r5, [r0, #SCU_CTRL]
+
+#ifndef CONFIG_CPU_DCACHE_DISABLE
+ mrc p15, 0, r0, c1, c0, 1 @ Enable SMP/nAMP mode
+ orr r0, r0, #0x20
+ mcr p15, 0, r0, c1, c0, 1
+#endif
+#endif
+
+ mov r0, #0
+ mcr p15, 0, r0, c7, c14, 0 @ clean+invalidate D cache
+ mcr p15, 0, r0, c7, c5, 0 @ invalidate I cache
+ mcr p15, 0, r0, c7, c15, 0 @ clean+invalidate cache
+ mcr p15, 0, r0, c7, c10, 4 @ drain write buffer
+ mcr p15, 0, r0, c8, c7, 0 @ invalidate I + D TLBs
+ mcr p15, 0, r0, c2, c0, 2 @ TTB control register
+#ifdef CONFIG_SMP
+ orr r4, r4, #TTB_RGN_WBWA|TTB_S @ mark PTWs shared, outer cacheable
+#endif