Index: sys/arch/sparc64/include/cpu.h
===================================================================
RCS file: /cvsroot/src/sys/arch/sparc64/include/cpu.h,v
retrieving revision 1.112
diff -u -r1.112 cpu.h
--- sys/arch/sparc64/include/cpu.h	4 Sep 2014 18:48:29 -0000	1.112
+++ sys/arch/sparc64/include/cpu.h	22 Sep 2014 19:07:42 -0000
@@ -223,6 +223,7 @@
 	vaddr_t cb_ekdata;
 
 	paddr_t	cb_cpuinfo;
+	int cb_cputyp;
 };
 
 extern struct cpu_bootargs *cpu_args;
Index: sys/arch/sparc64/include/hypervisor.h
===================================================================
RCS file: /cvsroot/src/sys/arch/sparc64/include/hypervisor.h,v
retrieving revision 1.4
diff -u -r1.4 hypervisor.h
--- sys/arch/sparc64/include/hypervisor.h	8 Jun 2014 17:33:24 -0000	1.4
+++ sys/arch/sparc64/include/hypervisor.h	22 Sep 2014 19:07:42 -0000
@@ -25,6 +25,12 @@
  */
 
 /*
+ * FAST_TRAP function numbers
+ */
+
+#define FT_MMU_MAP_PERM_ADDR 0x25
+
+/*
  * API versioning
  */
 
Index: sys/arch/sparc64/sparc64/cpu.c
===================================================================
RCS file: /cvsroot/src/sys/arch/sparc64/sparc64/cpu.c,v
retrieving revision 1.117
diff -u -r1.117 cpu.c
--- sys/arch/sparc64/sparc64/cpu.c	1 Sep 2014 19:01:55 -0000	1.117
+++ sys/arch/sparc64/sparc64/cpu.c	22 Sep 2014 19:07:42 -0000
@@ -551,13 +551,18 @@
 		cpu_pmap_prepare(ci, false);
 		cpu_args->cb_node = ci->ci_node;
 		cpu_args->cb_cpuinfo = ci->ci_paddr;
+		cpu_args->cb_cputyp = cputyp;
 		membar_Sync();
 
 		/* Disable interrupts and start another CPU. */
 		pstate = getpstate();
 		setpstate(PSTATE_KERN);
 
-		prom_startcpu(ci->ci_node, (void *)cpu_spinup_trampoline, 0);
+		int rc = prom_startcpu_by_cpuid(ci->ci_cpuid,
+		    (void *)cpu_spinup_trampoline, 0);
+		if (rc == -1)
+			prom_startcpu(ci->ci_node,
+			    (void *)cpu_spinup_trampoline, 0);
 
 		for (i = 0; i < 2000; i++) {
 			membar_Sync();
Index: sys/arch/sparc64/sparc64/genassym.cf
===================================================================
RCS file: /cvsroot/src/sys/arch/sparc64/sparc64/genassym.cf,v
retrieving revision 1.74
diff -u -r1.74 genassym.cf
--- sys/arch/sparc64/sparc64/genassym.cf	4 Sep 2014 18:48:29 -0000	1.74
+++ sys/arch/sparc64/sparc64/genassym.cf	22 Sep 2014 19:07:42 -0000
@@ -182,6 +182,7 @@
 define	CBA_KDATAP	offsetof(struct cpu_bootargs, cb_kdatap)
 define	CBA_EKDATA	offsetof(struct cpu_bootargs, cb_ekdata)
 define	CBA_CPUINFO	offsetof(struct cpu_bootargs, cb_cpuinfo)
+define	CBA_CPUTYP	offsetof(struct cpu_bootargs, cb_cputyp)
 
 # FPU state
 define	FS_REGS		offsetof(struct fpstate64, fs_regs)
Index: sys/arch/sparc64/sparc64/locore.s
===================================================================
RCS file: /cvsroot/src/sys/arch/sparc64/sparc64/locore.s,v
retrieving revision 1.372
diff -u -r1.372 locore.s
--- sys/arch/sparc64/sparc64/locore.s	4 Sep 2014 18:48:29 -0000	1.372
+++ sys/arch/sparc64/sparc64/locore.s	22 Sep 2014 19:07:43 -0000
@@ -88,6 +88,9 @@
 #include <machine/intr.h>
 #include <machine/asm.h>
 #include <machine/locore.h>
+#ifdef SUN4V
+#include <machine/hypervisor.h>
+#endif	
 #include <sys/syscall.h>
 
 #define BLOCK_SIZE SPARC64_BLOCK_SIZE
@@ -120,7 +123,16 @@
 3:
 	.endm
 
-
+	.macro	SET_MMU_CONTEXTID_SUN4U ctxid,ctx
+	stxa	\ctxid, [\ctx] ASI_DMMU;
+	.endm
+	
+#ifdef SUN4V
+	.macro	SET_MMU_CONTEXTID_SUN4V ctxid,ctx
+	stxa	\ctxid, [\ctx] ASI_MMU;
+	.endm
+#endif	
+		
 	.macro	SET_MMU_CONTEXTID ctxid,ctx,scratch
 #ifdef SUN4V
 	sethi	%hi(cputyp), \scratch
@@ -129,15 +141,14 @@
 	bne,pt	%icc, 2f
 	 nop
 	/* sun4v */
-	stxa	\ctxid, [\ctx] ASI_MMU;
+	SET_MMU_CONTEXTID_SUN4V \ctxid,\ctx
 	ba	3f
 	 nop
 2:		
 #endif	
 	/* sun4u */
-	stxa	\ctxid, [\ctx] ASI_DMMU;
+	SET_MMU_CONTEXTID_SUN4U \ctxid,\ctx
 3:
-	
 	.endm
 
 #ifdef SUN4V
@@ -4412,7 +4423,7 @@
 
 ENTRY_NOPROFILE(cpu_initialize)	/* for cosmetic reasons - nicer backtrace */
 
-	/* Cache the cputyp in %l6 for later user below */
+	/* Cache the cputyp in %l6 for later use below */
 	sethi	%hi(cputyp), %l6
 	ld	[%l6 + %lo(cputyp)], %l6
 
@@ -4440,7 +4451,6 @@
 	 */
 	mov	%l1, %l7			! save cpu_info pointer
 	ldx	[%l1 + CI_PADDR], %l1		! Load the interrupt stack's PA
-
 #ifdef SUN4V
 	cmp	%l6, CPU_SUN4V
 	bne,pt	%icc, 3f
@@ -4464,7 +4474,7 @@
 	andn	%l1, %l4, %l1			! Mask the phys page number
 
 	or	%l2, %l1, %l1			! Now take care of the high bits
-	or	%l1, SUN4U_TTE_DATABITS, %l2		! And low bits:	L=1|CP=1|CV=?|E=0|P=1|W=1|G=0
+	or	%l1, SUN4U_TTE_DATABITS, %l2	! And low bits:	L=1|CP=1|CV=?|E=0|P=1|W=1|G=0
 
 	!!
 	!!  Now, map in the interrupt stack as context==0
@@ -4650,10 +4660,61 @@
 	wrpr	%g0, PSTATE_KERN, %pstate
 	flushw
 
+	/* Cache the cputyp in %l6 for later use below */
+	sethi	%hi(cputyp), %l6
+	ld	[%l6 + %lo(cputyp)], %l6
+	
 	/*
 	 * Get pointer to our cpu_info struct
 	 */
 	ldx	[%g2 + CBA_CPUINFO], %l1	! Load the interrupt stack's PA
+	
+
+#ifdef SUN4V
+	cmp	%l6, CPU_SUN4V
+	bne,pt	%icc, 3f
+	 nop
+	
+	/* sun4v */
+	
+	sethi	%hi(0x80000000), %l2		! V=1|NFO=0|SW=0
+	sllx	%l2, 32, %l2			! Shift it into place
+	mov	-1, %l3				! Create a nice mask
+	sllx	%l3, 56, %l4			! Mask off high 8 bits
+	or	%l4, 0x1fff, %l4		! Mask off low 13 bits
+	andn	%l1, %l4, %l1			! Mask the phys page number into RA
+	or	%l2, %l1, %l1			! Now take care of the 8 high bits V|NFO|SW
+	or	%l1, 0x0141, %l2		! And low 13 bits IE=0|E=0|CP=0|CV=0|P=1|
+						!		  X=0|W=1|SW=00|SZ=0001
+			
+	/*
+	 *  Now, map in the interrupt stack & cpu_info as context==0
+	 */
+	set	INTSTACK, %o0			! vaddr
+	clr	%o1				! reserved
+	mov	%l2, %o2			! tte
+	mov	MAP_DTLB, %o3			! flags
+	mov	FT_MMU_MAP_PERM_ADDR, %o5	! hv fast trap function
+	ta	ST_FAST_TRAP
+	cmp	%o0, 0
+	be,pt	%icc, 5f
+	 nop
+	sir					! crash if mapping fails
+5:
+
+	/*
+	 * Set 0 as primary context XXX
+	 */
+	mov	CTX_PRIMARY, %o0
+	SET_MMU_CONTEXTID_SUN4V %g0, %o0
+
+	ba	4f		
+	 nop
+3:
+#endif
+	
+	/* sun4u */
+	
 	sethi	%hi(0xa0000000), %l2		! V=1|SZ=01|NFO=0|IE=0
 	sllx	%l2, 32, %l2			! Shift it into place
 	mov	-1, %l3				! Create a nice mask
@@ -4661,7 +4722,7 @@
 	or	%l4, 0xfff, %l4			! We can just load this in 12 (of 13) bits
 	andn	%l1, %l4, %l1			! Mask the phys page number
 	or	%l2, %l1, %l1			! Now take care of the high bits
-	or	%l1, SUN4U_TTE_DATABITS, %l2		! And low bits:	L=1|CP=1|CV=?|E=0|P=1|W=1|G=0
+	or	%l1, SUN4U_TTE_DATABITS, %l2	! And low bits:	L=1|CP=1|CV=?|E=0|P=1|W=1|G=0
 
 	/*
 	 *  Now, map in the interrupt stack & cpu_info as context==0
@@ -4675,9 +4736,11 @@
 	 * Set 0 as primary context XXX
 	 */
 	mov	CTX_PRIMARY, %o0
-	stxa	%g0, [%o0] ASI_DMMU
-	membar	#Sync
+	SET_MMU_CONTEXTID_SUN4U %g0, %o0
 
+4:	
+	membar	#Sync
+	
 	/*
 	 * Temporarily use the interrupt stack
 	 */
@@ -4689,9 +4752,39 @@
 	set	1, %fp
 	clr	%i7
 
+#ifdef SUN4V
+	cmp	%l6, CPU_SUN4V
+	bne,pt	%icc, 2f
+	 nop
+	
+	/* sun4v */
+
 	/*
 	 * install our TSB pointers
 	 */
+
+	set	CPUINFO_VA, %o0
+	LDPTR	[%o0 + CI_TSB_DESC], %o0
+	call	_C_LABEL(pmap_setup_tsb_sun4v)
+	 nop
+	
+	/* set trap table */
+
+	set	_C_LABEL(trapbase_sun4v), %l1
+	GET_MMFSA %o1
+	call	_C_LABEL(prom_set_trap_table_sun4v)
+	 mov	%l1, %o0
+	! Now we should be running 100% from our handlers	
+	ba	3f		
+	 nop
+2:
+#endif
+	/* sun4u */
+	
+	/*
+	 * install our TSB pointers
+	 */
+
 	sethi	%hi(CPUINFO_VA+CI_TSB_DMMU), %l0
 	sethi	%hi(CPUINFO_VA+CI_TSB_IMMU), %l1
 	sethi	%hi(_C_LABEL(tsbsize)), %l2
@@ -4717,9 +4810,11 @@
 1:
 
 	/* set trap table */
+	
 	set	_C_LABEL(trapbase), %l1
 	call	_C_LABEL(prom_set_trap_table_sun4u)
 	 mov	%l1, %o0
+3:	
 	wrpr	%l1, 0, %tba			! Make sure the PROM didn't
 						! foul up.
 	/*
Index: sys/arch/sparc64/sparc64/mp_subr.S
===================================================================
RCS file: /cvsroot/src/sys/arch/sparc64/sparc64/mp_subr.S,v
retrieving revision 1.6
diff -u -r1.6 mp_subr.S
--- sys/arch/sparc64/sparc64/mp_subr.S	8 Jun 2014 17:33:24 -0000	1.6
+++ sys/arch/sparc64/sparc64/mp_subr.S	22 Sep 2014 19:07:43 -0000
@@ -214,28 +214,48 @@
 	.text
 	.align 32
 1:	rd	%pc, %l0
-	LDULNG	[%l0 + (4f-1b)], %l1
-	add	%l0, (6f-1b), %l2
+	LDULNG	[%l0 + (4f-1b)], %l1	! Load tlb slot count
+	LDULNG	[%l0 + (7f-1b)], %g2	! Load cpu_args address.
+	add	%l0, (6f-1b), %l2	! tlb slots
+	ld	[%g2 + CBA_CPUTYP], %g3 ! Load cputype
 	clr	%l3
 2:	cmp	%l3, %l1
 	be	CCCR, 3f
 	 nop
 	ldx	[%l2 + TTE_VPN], %l4
 	ldx	[%l2 + TTE_DATA], %l5
+#ifdef SUN4V	
+	cmp	%g3, CPU_SUN4V
+	bne,pt	%icc, 8f
+	 nop
+	! sun4v
+	mov	%l4, %o0			! vaddr
+	clr	%o1				! reserved
+	mov	%l5, %o2			! tte
+	mov	MAP_DTLB|MAP_ITLB, %o3		! flags
+	mov	FT_MMU_MAP_PERM_ADDR, %o5	! hv fast trap function
+	ta	ST_FAST_TRAP
+	cmp	%o0, 0
+	be,pt	%icc, 9f
+	 nop
+	sir					! crash if mapping fails
+8:
+#endif	
+	! sun4u
 	wr	%g0, ASI_DMMU, %asi
 	stxa	%l4, [%g0 + TLB_TAG_ACCESS] %asi
 	stxa	%l5, [%g0] ASI_DMMU_DATA_IN
 	wr	%g0, ASI_IMMU, %asi
 	stxa	%l4, [%g0 + TLB_TAG_ACCESS] %asi
 	stxa	%l5, [%g0] ASI_IMMU_DATA_IN
+9:		
 	membar	#Sync
 	flush	%l4
 	add	%l2, PTE_SIZE, %l2
 	add	%l3, 1, %l3
 	ba	%xcc, 2b
 	 nop
-3:	LDULNG	[%l0 + (5f-1b)], %l1
-	LDULNG	[%l0 + (7f-1b)], %g2	! Load cpu_info address.
+3:	LDULNG	[%l0 + (5f-1b)], %l1	! Load function
 	jmpl	%l1, %g0
 	 nop