Index: sys/arch/arm/cortex/a9_mpsubr.S
===================================================================
RCS file: /cvsroot/src/sys/arch/arm/cortex/a9_mpsubr.S,v
retrieving revision 1.44
diff -u -p -r1.44 a9_mpsubr.S
--- sys/arch/arm/cortex/a9_mpsubr.S	25 Nov 2015 08:39:45 -0000	1.44
+++ sys/arch/arm/cortex/a9_mpsubr.S	16 Dec 2015 19:58:34 -0000
@@ -347,9 +347,40 @@ xputc:
 cortex_init:
 	mov	r10, lr				// save lr
 
-	cpsid	if, #PSR_SVC32_MODE		// SVC32 with no interrupts
-        mov	r0, #0
-        msr	spsr_sxc, r0			// set SPSR[23:8] to known value
+	/* Leave HYP mode and move into supervisor mode with IRQs/FIQs disabled. */
+	mrs	r0, cpsr
+	and	r0, r0, #(PSR_MODE)	/* Mode is in the low 5 bits of CPSR */
+	teq	r0, #(PSR_HYP32_MODE)	/* Hyp Mode? */
+	bne	1f
+
+#if  0
+	mov	r7, #0
+	mcr	p15, 4, r7, c1, c1, 0	@ HCR
+	mcr	p15, 4, r7, c1, c1, 2	@ HCPTR
+	mcr	p15, 4, r7, c1, c1, 3	@ HSTR
+
+	mcr	p15, 4, r7, c1, c0, 0	@ HSCTLR
+
+	mrc	p15, 4, r7, c1, c1, 1	@ HDCR
+	and	r7, #0x1f		@ Preserve HPMN
+	mcr	p15, 4, r7, c1, c1, 1	@ HDCR
+#endif
+
+	/* Ensure that IRQ, and FIQ will be disabled after eret */
+	mrs	r0, cpsr
+	bic	r0, r0, #(PSR_MODE)
+	orr	r0, r0, #(PSR_SVC32_MODE)
+	orr	r0, r0, #(I32_bit | F32_bit)
+	msr	spsr_cxsf, r0
+	/* Exit hypervisor mode */
+	adr	lr, 1f
+	msr	elr_hyp, lr
+	eret
+1:
+
+	/* Can this be done above??? */
+	mov	r0, #0
+	msr	spsr_sxc, r0			// set SPSR[23:8] to known value
 
 #if 0
 	mrc	p14, 0, r0, c0, c0, 0		// MIDR read
Index: sys/arch/evbarm/exynos/exynos_start.S
===================================================================
RCS file: /cvsroot/src/sys/arch/evbarm/exynos/exynos_start.S,v
retrieving revision 1.2
diff -u -p -r1.2 exynos_start.S
--- sys/arch/evbarm/exynos/exynos_start.S	14 Dec 2015 05:13:01 -0000	1.2
+++ sys/arch/evbarm/exynos/exynos_start.S	16 Dec 2015 19:58:40 -0000
@@ -87,24 +87,6 @@ _C_LABEL(exynos_start):
 	setend	be			/* force big endian */
 #endif
 
-	/* Leave HYP (hypervisor or monitor) mode and move into supervisor mode
-	 * with IRQs/FIQs disabled. */
-	mrs	r0, cpsr
-	and	r0, r0, #(PSR_MODE)	/* Mode is in the low 5 bits of CPSR */
-	teq	r0, #(PSR_HYP32_MODE)	/* Hyp Mode? */
-	bne	1f
-	/* Ensure that IRQ, and FIQ will be disabled after eret */
-	mrs	r0, cpsr
-	bic	r0, r0, #(PSR_MODE)
-	orr	r0, r0, #(PSR_SVC32_MODE)
-	orr	r0, r0, #(I32_bit | F32_bit)
-	msr	spsr_cxsf, r0
-	/* Exit hypervisor mode */
-	adr	lr, 1f
-	msr	elr_hyp, lr
-	eret
-1:
-
 	/*
 	 * Save any arguments passed to us.  If .start is not at
 	 * 0x80000000 but .text is, we can't directly use the address that
@@ -119,6 +101,11 @@ _C_LABEL(exynos_start):
 	stmia	r4, {r0-r3}			// Save the arguments
 
 	/*
+	 * Setup the CPU. (sctlr, actlr, etc)
+	 */
+	bl	cortex_init
+
+	/*
 	 * For easy and early SoC / PoP dependency, retrieve the IDs
 	 */
 	movw	r6, #:lower16:EXYNOS_CORE_PBASE
@@ -149,11 +136,6 @@ _C_LABEL(exynos_start):
 	add	r2, r2, #EXYNOS_CORE_PBASE
 	mcr	p15, 0, r2, c13, c0, 3		// TPIDRURO set (uart address)
 
-	/*
-	 * Turn on the SMP bit
-	 */
-	bl	cortex_init
-
 	XPUTC(#'C')
 
 	/*
Index: sys/arch/evbarm/rpi/rpi2_start.S
===================================================================
RCS file: /cvsroot/src/sys/arch/evbarm/rpi/rpi2_start.S,v
retrieving revision 1.2
diff -u -p -r1.2 rpi2_start.S
--- sys/arch/evbarm/rpi/rpi2_start.S	18 Apr 2015 11:03:31 -0000	1.2
+++ sys/arch/evbarm/rpi/rpi2_start.S	16 Dec 2015 19:58:40 -0000
@@ -78,11 +78,6 @@ _C_LABEL(rpi_start):
 #ifdef __ARMEB__
 	setend	be			/* force big endian */
 #endif
-	mov	r9, #0
-
-	/* Move into supervisor mode and disable IRQs/FIQs. */
-	cpsid	if, #PSR_SVC32_MODE
-
 	/*
 	 * Save any arguments passed to us.
 	 */
@@ -101,7 +96,7 @@ _C_LABEL(rpi_start):
 	stmia	r4, {r0-r3}		// Save the arguments
 
 	/*
-	 * Turn on the SMP bit
+	 * Setup the CPU
 	 */
 	bl	cortex_init