Index: sys/arch/sparc64/include/pmap.h
===================================================================
RCS file: /cvsroot/src/sys/arch/sparc64/include/pmap.h,v
retrieving revision 1.56
diff -u -r1.56 pmap.h
--- sys/arch/sparc64/include/pmap.h	11 Sep 2013 18:27:44 -0000	1.56
+++ sys/arch/sparc64/include/pmap.h	27 Dec 2013 19:12:47 -0000
@@ -212,6 +212,12 @@
 void		pmap_copy_page_phys(paddr_t, paddr_t);
 void		pmap_zero_page_phys(paddr_t);
 
+#ifdef SUN4V
+/* sun4v specific */
+void		pmap_setup_intstack_sun4v(paddr_t);
+void		pmap_setup_tsb_sun4v(void);
+#endif
+
 /* Installed physical memory, as discovered during bootstrap. */
 extern int phys_installed_size;
 extern struct mem_region *phys_installed;
Index: sys/arch/sparc64/sparc64/genassym.cf
===================================================================
RCS file: /cvsroot/src/sys/arch/sparc64/sparc64/genassym.cf,v
retrieving revision 1.66
diff -u -r1.66 genassym.cf
--- sys/arch/sparc64/sparc64/genassym.cf	20 Jul 2011 12:06:00 -0000	1.66
+++ sys/arch/sparc64/sparc64/genassym.cf	27 Dec 2013 19:12:47 -0000
@@ -150,6 +150,7 @@
 define	CI_MTX_COUNT	offsetof(struct cpu_info, ci_mtx_count)
 define	CI_SPINUP	offsetof(struct cpu_info, ci_spinup)
 define	CI_PADDR	offsetof(struct cpu_info, ci_paddr)
+define	CI_CPUID	offsetof(struct cpu_info, ci_cpuid)
 define	CI_WANT_AST	offsetof(struct cpu_info, ci_want_ast)
 define	CI_WANT_RESCHED	offsetof(struct cpu_info, ci_want_resched)
 define	CI_EINTSTACK	offsetof(struct cpu_info, ci_eintstack)
Index: sys/arch/sparc64/sparc64/locore.s
===================================================================
RCS file: /cvsroot/src/sys/arch/sparc64/sparc64/locore.s,v
retrieving revision 1.350
diff -u -r1.350 locore.s
--- sys/arch/sparc64/sparc64/locore.s	6 Dec 2013 21:11:06 -0000	1.350
+++ sys/arch/sparc64/sparc64/locore.s	27 Dec 2013 19:12:48 -0000
@@ -4041,32 +4041,41 @@
 
 
 ENTRY_NOPROFILE(cpu_initialize)	/* for cosmetic reasons - nicer backtrace */
+
+	/* Cache the cputyp in %l6 for later user below */
+	sethi	%hi(_C_LABEL(cputyp)), %l6
+	ld	[%l6 + %lo(_C_LABEL(cputyp))], %l6
+
 	/*
 	 * Step 5: is no more.
 	 */
 	
 	/*
-	 * Step 6: hunt through cpus list and find the one that
-	 * matches our UPAID.
+	 * Step 6: hunt through cpus list and find the one that matches our cpuid
 	 */
+
+	call	_C_LABEL(cpu_myid)	! Retrieve cpuid in %o0
+	 mov	%g0, %o0
+	
 	sethi	%hi(_C_LABEL(cpus)), %l1
-	ldxa	[%g0] ASI_MID_REG, %l2
 	LDPTR	[%l1 + %lo(_C_LABEL(cpus))], %l1
-	srax	%l2, 17, %l2			! Isolate UPAID from CPU reg
-	and	%l2, 0x1f, %l2
 0:
-	ld	[%l1 + CI_UPAID], %l3		! Load UPAID
-	cmp	%l3, %l2			! Does it match?
+	ld	[%l1 + CI_CPUID], %l3		! Load CPUID
+	cmp	%l3, %o0			! Does it match?
 	bne,a,pt	%icc, 0b		! no
 	 LDPTR	[%l1 + CI_NEXT], %l1		! Load next cpu_info pointer
 
-
 	/*
 	 * Get pointer to our cpu_info struct
 	 */
 	mov	%l1, %l7			! save cpu_info pointer
 	ldx	[%l1 + CI_PADDR], %l1		! Load the interrupt stack's PA
 
+	cmp	%l6, CPU_SUN4V
+	be,pn	%icc, 3f
+	 nop
+
+	/* sun4u */	
 	sethi	%hi(0xa0000000), %l2		! V=1|SZ=01|NFO=0|IE=0
 	sllx	%l2, 32, %l2			! Shift it into place
 
@@ -4087,7 +4096,14 @@
 	stxa	%l0, [%l5] ASI_DMMU		! Make DMMU point to it
 	stxa	%l2, [%g0] ASI_DMMU_DATA_IN	! Store it
 	membar	#Sync
-
+	
+	ba	4f
+	 nop
+3:
+	/* sun4v */
+	call	_C_LABEL(pmap_setup_intstack_sun4v)	! Call nice C function for mapping INTSTACK
+	 mov	%l1, %o0
+4:		
 	!! Setup kernel stack (we rely on curlwp on this cpu
 	!! being lwp0 here and it's uarea is mapped special
 	!! and already accessible here)
@@ -4125,6 +4141,12 @@
 	/*
 	 * install our TSB pointers
 	 */
+
+	cmp	%l6, CPU_SUN4V
+	be,pn	%icc, 5f
+	 nop
+
+	/* sun4u */	
 	sethi	%hi(_C_LABEL(tsbsize)), %l2
 	sethi	%hi(0x1fff), %l3
 	sethi	%hi(TSB), %l4
@@ -4145,7 +4167,14 @@
 	set	1f, %l1
 	flush	%l1
 1:
+	ba	6f
+	 nop
+
+5:	/* sun4v */
+	call	_C_LABEL(pmap_setup_tsb_sun4v)
+	 nop
 
+6:		
 	/* set trap table */
 	set	_C_LABEL(trapbase), %l1
 	call	_C_LABEL(prom_set_trap_table)	! Now we should be running 100% from our handlers
Index: sys/arch/sparc64/sparc64/pmap.c
===================================================================
RCS file: /cvsroot/src/sys/arch/sparc64/sparc64/pmap.c,v
retrieving revision 1.282
diff -u -r1.282 pmap.c
--- sys/arch/sparc64/sparc64/pmap.c	16 Dec 2013 20:17:35 -0000	1.282
+++ sys/arch/sparc64/sparc64/pmap.c	27 Dec 2013 19:12:49 -0000
@@ -3780,3 +3780,53 @@
 	return EINVAL;
 }
 #endif
+
+#ifdef SUN4V
+void
+pmap_setup_intstack_sun4v(paddr_t pa)
+{
+	int64_t hv_rc;
+	int64_t data;
+	data = SUN4V_TSB_DATA(
+	    0 /* global */,
+	    PGSZ_64K,
+	    pa,
+	    1 /* priv */,
+	    1 /* Write */,
+	    1 /* Cacheable */,
+	    FORCE_ALIAS /* ALIAS -- Disable D$ */,
+	    1 /* valid */,
+	    0 /* IE */);
+	hv_rc = hv_mmu_map_perm_addr(INTSTACK, data, MAP_DTLB);
+	if ( hv_rc != H_EOK ) {
+		panic("hv_mmu_map_perm_addr() failed - rc = %ld\n", hv_rc);
+	}
+	else {
+		memset((void *)INTSTACK, 0, 64 * KB);
+	}
+}
+
+void
+pmap_setup_tsb_sun4v(void)
+{
+	int err;
+	extern struct tsb_desc *tsb_desc;
+	extern paddr_t pmap_kextract(vaddr_t va);
+	paddr_t tsb_desc_p;
+	tsb_desc_p = pmap_kextract((vaddr_t)tsb_desc);
+	if ( !tsb_desc_p ) {
+		panic("pmap_setup_tsb_sun4v() pmap_kextract() failed");
+	}
+	err = hv_mmu_tsb_ctx0(1, tsb_desc_p);
+	if (err != H_EOK) {
+		prom_printf("hv_mmu_tsb_ctx0() err: %d\n", err);
+		panic("pmap_setup_tsb_sun4v() hv_mmu_tsb_ctx0() failed");
+	}
+	err = hv_mmu_tsb_ctxnon0(1, tsb_desc_p);
+	if (err != H_EOK) {
+		prom_printf("hv_mmu_tsb_ctxnon0() err: %d\n", err);
+		panic("pmap_setup_tsb_sun4v() hv_mmu_tsb_ctxnon0() failed");
+	}
+}
+
+#endif