MFC 265694, 265705, 265784:

  Move the mptramp code which is specific to the Marvell ArmadaXP SoC out of
  the common locore.S file and into the mv/armadaxp directory.

  Consolidate all the AP core startup stuff under a single #ifdef SMP block

  Call idcache_inv_all from the AP core entry code before turning on the MMU.
  Also, enable instruction and branch caches, which should be safe now that
  they're properly initialized/invalidated first.

4 files changed, 78 insertions, 70 deletions

diff --git a/sys/arm/arm/cpufunc_asm_armv7.S b/sys/arm/arm/cpufunc_asm_armv7.S
index 45f4593..71e9ca5 100644
--- a/sys/arm/arm/cpufunc_asm_armv7.S
+++ b/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
diff --git a/sys/arm/arm/locore.S b/sys/arm/arm/locore.S
index 90eeeaf..3a6c3c5 100644
--- a/sys/arm/arm/locore.S
+++ b/sys/arm/arm/locore.S
@@ -308,11 +308,6 @@ Lreal_start:
 Lend:
 	.word	_edata
 
-#ifdef SMP
-Lstartup_pagetable_secondary:
-	.word	temp_pagetable
-#endif
-
 .Lstart:
 	.word	_edata
 	.word	_ebss
@@ -320,10 +315,6 @@ Lstartup_pagetable_secondary:
 
 .Lvirt_done:
 	.word	virt_done
-#if defined(SMP)
-.Lmpvirt_done:
-	.word	mpvirt_done
-#endif
 
 .Lmainreturned:
 	.asciz	"main() returned"
@@ -349,104 +340,59 @@ pagetable:
 	.word	_C_LABEL(cpufuncs)
 
 #if defined(SMP)
-Lsramaddr:
-	.word	0xffff0080
-
-#if 0
-#define	AP_DEBUG(tmp)			\
-	mrc	p15, 0, r1, c0, c0, 5;	\
-	ldr	r0, Lsramaddr;		\
-	add	r0, r1, lsl #2;		\
-	mov	r1, tmp;		\
-	str	r1, [r0], #0x0000;
-#else
-#define AP_DEBUG(tmp)
-#endif
-
 
-ASENTRY_NP(mptramp)
-	mov	r0, #0
-	mcr	p15, 0, r0, c7, c7, 0
-
-	AP_DEBUG(#1)
-
-	mrs	r3, cpsr
-	bic	r3, r3, #(PSR_MODE)
-	orr	r3, r3, #(PSR_SVC32_MODE)
-        msr	cpsr_fsxc, r3
-
-	mrc	p15, 0, r0, c0, c0, 5
-	and	r0, #0x0f		/* Get CPU ID */
-
-	/* Read boot address for CPU */
-	mov	r1, #0x100
-	mul	r2, r0, r1
-	ldr	r1, Lpmureg
-	add	r0, r2, r1
-	ldr	r1, [r0], #0x00
-
-	mov pc, r1
-
-Lpmureg:
-        .word   0xd0022124
-END(mptramp)
+.Lmpvirt_done:
+	.word	mpvirt_done
+Lstartup_pagetable_secondary:
+	.word	temp_pagetable
 
 ASENTRY_NP(mpentry)
 
-	AP_DEBUG(#2)
-
 	/* Make sure interrupts are disabled. */
 	mrs	r7, cpsr
 	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)
 
-	AP_DEBUG(#3)
+#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
 	ldr	r0, [r0]
-#if defined(SMP)
 	orr 	r0, r0, #2		/* Set TTB shared memory flag */
-#endif
 	mcr	p15, 0, r0, c2, c0, 0	/* Set TTB */
 	mcr	p15, 0, r0, c8, c7, 0	/* Flush TLB */
 
-#if defined(CPU_ARM1136) || defined(CPU_ARM1176) || defined(CPU_MV_PJ4B) || defined(CPU_CORTEXA) || defined(CPU_KRAIT)
 	mov	r0, #0
 	mcr	p15, 0, r0, c13, c0, 1	/* Set ASID to 0 */
-#endif
-
-	AP_DEBUG(#4)
 
 	/* Set the Domain Access register.  Very important! */
 	mov	r0, #((DOMAIN_CLIENT << (PMAP_DOMAIN_KERNEL*2)) | DOMAIN_CLIENT)
 	mcr	p15, 0, r0, c3, c0, 0
 	/* Enable MMU */
 	mrc	p15, 0, r0, c1, c0, 0
-#if defined(CPU_ARM1136) || defined(CPU_ARM1176) || defined(CPU_MV_PJ4B) || defined(CPU_CORTEXA) || defined(CPU_KRAIT)
 	orr	r0, r0, #CPU_CONTROL_V6_EXTPAGE
 	orr	r0, r0, #CPU_CONTROL_AF_ENABLE
-#endif
-	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
@@ -473,7 +419,7 @@ mpvirt_done:
 	/* NOTREACHED */
 
 .Lmpreturned:
-	.asciz	"main() returned"
+	.asciz	"init_secondary() returned"
 	.align	0
 END(mpentry)
 #endif
diff --git a/sys/arm/mv/armadaxp/files.armadaxp b/sys/arm/mv/armadaxp/files.armadaxp
index bd0316f..2ca0836 100644
--- a/sys/arm/mv/armadaxp/files.armadaxp
+++ b/sys/arm/mv/armadaxp/files.armadaxp
@@ -4,3 +4,5 @@ arm/mv/armadaxp/armadaxp.c	standard
 arm/mv/mpic.c			standard
 arm/mv/rtc.c			standard
 arm/mv/armadaxp/armadaxp_mp.c	optional smp
+arm/mv/armadaxp/mptramp.S	optional smp
+
diff --git a/sys/arm/mv/armadaxp/mptramp.S b/sys/arm/mv/armadaxp/mptramp.S
new file mode 100644
index 0000000..4d657e4
--- /dev/null
+++ b/sys/arm/mv/armadaxp/mptramp.S
@@ -0,0 +1,56 @@
+/*-
+ * Copyright 2011 Semihalf
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
+ * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
+ * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+ * SUCH DAMAGE.
+ */
+
+#include <machine/asm.h>
+#include <machine/armreg.h>
+
+__FBSDID("$FreeBSD$");
+
+ASENTRY_NP(mptramp)
+	mov	r0, #0
+	mcr	p15, 0, r0, c7, c7, 0
+
+	mrs	r3, cpsr
+	bic	r3, r3, #(PSR_MODE)
+	orr	r3, r3, #(PSR_SVC32_MODE)
+        msr	cpsr_fsxc, r3
+
+	mrc	p15, 0, r0, c0, c0, 5
+	and	r0, #0x0f		/* Get CPU ID */
+
+	/* Read boot address for CPU */
+	mov	r1, #0x100
+	mul	r2, r0, r1
+	ldr	r1, Lpmureg
+	add	r0, r2, r1
+	ldr	r1, [r0], #0x00
+
+	mov pc, r1
+
+Lpmureg:
+        .word   0xd0022124
+END(mptramp)
+