Add SMP support for MTI Malta 34kf CPU.

Sponsored by:	DARPA, AFRL
Sponsored by:	HEIF5

2 files changed, 66 insertions, 10 deletions

diff --git a/sys/mips/malta/asm_malta.S b/sys/mips/malta/asm_malta.S
index c79529c12b2b..5b0397708454 100644
--- a/sys/mips/malta/asm_malta.S
+++ b/sys/mips/malta/asm_malta.S
@@ -37,6 +37,7 @@
 #include <machine/asm.h>
 
 #define	VPECONF0_MVP	(1 << 1)
+#define	VPECONF0_VPA	(1 << 0)
 
 	.set noreorder
 
@@ -54,16 +55,16 @@ LEAF(platform_processor_id)
 	.set pop
 END(platform_processor_id)
 
-LEAF(enable_mvp)
+LEAF(malta_cpu_configure)
 	.set push
 	.set mips32r2
 	.set noat
-	li	t2, (VPECONF0_MVP)
+	li	t2, (VPECONF0_MVP | VPECONF0_VPA)
 	move	$1, t2
 	jr	ra
 	.word	0x41810000 | (1 << 11) | 2	# mttc0 t2, $1, 2
 	.set pop
-END(enable_mvp)
+END(malta_cpu_configure)
 
 /*
  * Called on APs to wait until they are told to launch.
diff --git a/sys/mips/malta/malta_mp.c b/sys/mips/malta/malta_mp.c
index 605f6024e989..7683d92d704d 100644
--- a/sys/mips/malta/malta_mp.c
+++ b/sys/mips/malta/malta_mp.c
@@ -49,6 +49,9 @@ __FBSDID("$FreeBSD$");
 #include <machine/smp.h>
 
 #define	MALTA_MAXCPU	2
+#define	VPECONF0_VPA	(1 << 0)
+#define	MVPCONTROL_VPC	(1 << 1)
+#define	TCSTATUS_A	(1 << 13)
 
 unsigned malta_ap_boot = ~0;
 
@@ -62,6 +65,19 @@ unsigned malta_ap_boot = ~0;
 #define	C_IRQ5		(1 << 15)
 
 static inline void
+evpe(void)
+{
+	__asm __volatile(
+	"	.set push			\n"
+	"	.set noreorder			\n"
+	"	.set noat			\n"
+	"	.set mips32r2			\n"
+	"	.word	0x41600021	# evpe	\n"
+	"	ehb				\n"
+	"	.set pop			\n");
+}
+
+static inline void
 ehb(void)
 {
 	__asm __volatile(
@@ -118,25 +134,30 @@ ehb(void)
 	__retval;							\
 })
 
-void
-platform_ipi_send(int cpuid)
+static void
+set_thread_context(int cpuid)
 {
 	uint32_t reg;
 
-	/*
-	 * Set thread context.
-	 * Note this is not global, so we don't need lock.
-	 */
 	reg = read_c0_register32(1, 1);
 	reg &= ~(0xff);
 	reg |= cpuid;
 	write_c0_register32(1, 1, reg);
 
 	ehb();
+}
+
+void
+platform_ipi_send(int cpuid)
+{
+	uint32_t reg;
+
+	set_thread_context(cpuid);
 
 	/* Set cause */
 	reg = mftc0(13, 0);
-	mttc0(13, 0, (reg | C_SW1));
+	reg |= (C_SW1);
+	mttc0(13, 0, reg);
 }
 
 void
@@ -204,8 +225,42 @@ platform_smp_topo(void)
 int
 platform_start_ap(int cpuid)
 {
+	uint32_t reg;
 	int timeout;
 
+	/* Enter into configuration */
+	reg = read_c0_register32(0, 1);
+	reg |= (MVPCONTROL_VPC);
+	write_c0_register32(0, 1, reg);
+
+	set_thread_context(cpuid);
+
+	/*
+	 * Hint: how to set entry point.
+	 * reg = 0x80000000;
+	 * mttc0(2, 3, reg);
+	 */
+
+	/* Enable thread */
+	reg = mftc0(2, 1);
+	reg |= (TCSTATUS_A);
+	mttc0(2, 1, reg);
+
+	/* Unhalt CPU core */
+	mttc0(2, 4, 0);
+
+	/* Activate VPE */
+	reg = mftc0(1, 2);
+	reg |= (VPECONF0_VPA);
+	mttc0(1, 2, reg);
+
+	/* Out of configuration */
+	reg = read_c0_register32(0, 1);
+	reg &= ~(MVPCONTROL_VPC);
+	write_c0_register32(0, 1, reg);
+
+	evpe();
+
 	if (atomic_cmpset_32(&malta_ap_boot, ~0, cpuid) == 0)
 		return (-1);