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.

2 files changed, 58 insertions, 0 deletions

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)
+