add sun4v support to the sparc64 boot loader

Approved by: rwatson (mentor)
Reviewed by: jmg
Tested by: kris, dwhite, and jmg

5 files changed, 1109 insertions, 10 deletions

diff --git a/sys/boot/Makefile b/sys/boot/Makefile
index bbb0ef8..bf76176 100644
--- a/sys/boot/Makefile
+++ b/sys/boot/Makefile
@@ -18,6 +18,6 @@ SUBDIR+=		ofw
 .endif
 
 # Pick the machine-dependent subdir based on the target architecture.
-SUBDIR+=		${MACHINE:S/amd64/i386/}
+SUBDIR+=		${MACHINE:S/amd64/i386/:S/sun4v/sparc64/}
 
 .include <bsd.subdir.mk>
diff --git a/sys/boot/ofw/libofw/Makefile b/sys/boot/ofw/libofw/Makefile
index 8a56a77..493a1cb 100644
--- a/sys/boot/ofw/libofw/Makefile
+++ b/sys/boot/ofw/libofw/Makefile
@@ -5,7 +5,7 @@ INTERNALLIB=
 
 SRCS=	devicename.c elf_freebsd.c ofw_console.c ofw_copy.c ofw_disk.c \
 	ofw_memory.c ofw_module.c ofw_net.c ofw_reboot.c \
-	ofw_time.c openfirm.c
+	ofw_time.c openfirm.c openfirm_mmu.c
 
 CFLAGS+=	-I${.CURDIR}/../../../../lib/libstand/
 
diff --git a/sys/boot/ofw/libofw/openfirm.c b/sys/boot/ofw/libofw/openfirm.c
index cfb9d1f..9972f8f 100644
--- a/sys/boot/ofw/libofw/openfirm.c
+++ b/sys/boot/ofw/libofw/openfirm.c
@@ -87,6 +87,20 @@ OF_init(int (*openfirm)(void *))
 		panic("failed to get mmu ihandle");
 }
 
+phandle_t
+OF_chosennode(void)
+{
+        static phandle_t        chosen;
+
+        if (chosen)
+                return (chosen);
+
+        if ((chosen = OF_finddevice("/chosen")) == -1)
+                OF_exit();
+
+        return (chosen);
+}
+
 /*
  * Generic functions
  */
diff --git a/sys/boot/sparc64/loader/hcall.S b/sys/boot/sparc64/loader/hcall.S
new file mode 100644
index 0000000..0377085
--- /dev/null
+++ b/sys/boot/sparc64/loader/hcall.S
@@ -0,0 +1,986 @@
+/* 
+ * $FreeBSD$ 
+ */
+	
+/*
+ * Hypervisor calls
+ */
+#define LOCORE
+#define _ASM
+		
+#include <machine/asm.h>
+#include <machine/asi.h>
+#include <machine/hypervisor_api.h>
+#include <machine/pstate.h>
+	/*
+	 * %o0 - character
+	 */
+	ENTRY(hv_cnputchar)
+	mov	CONS_WRITE, %o5
+	ta	FAST_TRAP
+	tst	%o0
+	retl
+	movnz	%xcc, -1, %o0
+	SET_SIZE(hv_cnputchar)
+
+	/*
+	 * %o0 pointer to character buffer
+	 * return values:
+	 * 0 success
+	 * hv_errno failure
+	 */
+	ENTRY(hv_cngetchar)
+	mov	%o0, %o2
+	mov	CONS_READ, %o5
+	ta	FAST_TRAP
+	brnz,a	%o0, 1f		! failure, just return error
+	mov	1, %o0
+
+	cmp	%o1, H_BREAK
+	be	1f
+	mov	%o1, %o0
+
+	cmp	%o1, H_HUP
+	be	1f
+	mov	%o1, %o0
+
+	stb	%o1, [%o2]	! success, save character and return 0
+	mov	0, %o0
+1:
+	retl
+	nop
+	SET_SIZE(hv_cngetchar)
+
+	ENTRY(hv_tod_get)
+	mov	%o0, %o4
+	mov	TOD_GET, %o5
+	ta	FAST_TRAP
+	retl
+	  stx	%o1, [%o4] 
+	SET_SIZE(hv_tod_get)
+
+	ENTRY(hv_tod_set)
+	mov	TOD_SET, %o5
+	ta	FAST_TRAP
+	retl
+	nop
+	SET_SIZE(hv_tod_set)
+
+	/*
+	 * Map permanent address
+	 * arg0 vaddr (%o0)
+	 * arg1 context (%o1)
+	 * arg2 tte (%o2)
+	 * arg3 flags (%o3)  0x1=d 0x2=i
+	 */
+	ENTRY(hv_mmu_map_perm_addr)
+	mov	MAP_PERM_ADDR, %o5
+	ta	FAST_TRAP
+	retl
+	nop
+	SET_SIZE(hv_mmu_map_perm_addr)
+
+	/*
+	 * Unmap permanent address
+	 * arg0 vaddr (%o0)
+	 * arg1 context (%o1)
+	 * arg2 flags (%o2)  0x1=d 0x2=i
+	 */
+	ENTRY(hv_mmu_unmap_perm_addr)
+	mov	UNMAP_PERM_ADDR, %o5
+	ta	FAST_TRAP
+	retl
+	nop
+	SET_SIZE(hv_mmu_unmap_perm_addr)
+
+	/*
+	 * Set TSB for context 0
+	 * arg0 ntsb_descriptor (%o0)
+	 * arg1 desc_ra (%o1)
+	 */
+	ENTRY(hv_set_ctx0)
+	mov	MMU_TSB_CTX0, %o5
+	ta	FAST_TRAP
+	retl
+	nop
+	SET_SIZE(hv_set_ctx0)
+
+	/*
+	 * Set TSB for context non0
+	 * arg0 ntsb_descriptor (%o0)
+	 * arg1 desc_ra (%o1)
+	 */
+	ENTRY(hv_set_ctxnon0)
+	mov	MMU_TSB_CTXNON0, %o5
+	ta	FAST_TRAP
+	retl
+	nop
+	SET_SIZE(hv_set_ctxnon0)
+
+#ifdef SET_MMU_STATS
+	/*
+	 * Returns old stat area on success
+	 */
+	ENTRY(hv_mmu_set_stat_area)
+	mov	MMU_STAT_AREA, %o5
+	ta	FAST_TRAP
+	retl
+	nop
+	SET_SIZE(hv_mmu_set_stat_area)
+#endif /* SET_MMU_STATS */
+
+	/*
+	 * CPU Q Configure
+	 * arg0 queue (%o0)
+	 * arg1 Base address RA (%o1)
+	 * arg2 Size (%o2)
+	 */
+	ENTRY(hv_cpu_qconf)
+	mov	CPU_QCONF, %o5
+	ta	FAST_TRAP
+	retl
+	nop
+	SET_SIZE(hv_cpu_qconf)
+
+	/*
+	 * arg0 - devhandle
+	 * arg1 - pci_device
+	 * arg2 - pci_config_offset
+	 * arg3 - pci_config_size
+	 *
+	 * ret0 - status
+	 * ret1 - error_flag
+	 * ret2 - pci_cfg_data
+	 */
+	ENTRY(hvio_config_get)
+	mov	HVIO_CONFIG_GET, %o5
+	ta	FAST_TRAP
+	brnz	%o0, 1f
+	movrnz	%o1, -1, %o2
+	brz,a	%o1, 1f
+	stuw	%o2, [%o4]
+1:	retl
+	nop
+	SET_SIZE(hvio_config_get)
+
+	/*
+	 * arg0 - devhandle
+	 * arg1 - pci_device
+	 * arg2 - pci_config_offset
+	 * arg3 - pci_config_size
+	 * arg4 - pci_cfg_data
+	 *
+	 * ret0 - status
+	 * ret1 - error_flag
+	 */
+	ENTRY(hvio_config_put)
+	mov	HVIO_CONFIG_PUT, %o5
+	ta	FAST_TRAP
+	retl
+	nop
+	SET_SIZE(hvio_config_put)
+
+	/*
+	 * arg0 - devhandle
+	 * arg1 - devino
+	 *
+	 * ret0 - status
+	 * ret1 - sysino
+	 */
+	ENTRY(hvio_intr_devino_to_sysino)
+	mov	HVIO_INTR_DEVINO2SYSINO, %o5
+	ta	FAST_TRAP
+	brz,a	%o0, 1f
+	stx	%o1, [%o2]
+1:	retl
+	nop
+	SET_SIZE(hvio_intr_devino_to_sysino)
+
+	/*
+	 * arg0 - sysino
+	 *
+	 * ret0 - status
+	 * ret1 - intr_valid_state
+	 */
+	ENTRY(hvio_intr_getvalid)
+	mov	%o1, %o2
+	mov	HVIO_INTR_GETVALID, %o5
+	ta	FAST_TRAP
+	brz,a	%o0, 1f
+	stuw	%o1, [%o2]
+1:	retl
+	nop
+	SET_SIZE(hvio_intr_getvalid)
+
+	/*
+	 * arg0 - sysino
+	 * arg1 - intr_valid_state
+	 *
+	 * ret0 - status
+	 */
+	ENTRY(hvio_intr_setvalid)
+	mov	HVIO_INTR_SETVALID, %o5
+	ta	FAST_TRAP
+	retl
+	nop
+	SET_SIZE(hvio_intr_setvalid)
+
+	/*
+	 * arg0 - sysino
+	 *
+	 * ret0 - status
+	 * ret1 - intr_state
+	 */
+	ENTRY(hvio_intr_getstate)
+	mov	%o1, %o2
+	mov	HVIO_INTR_GETSTATE, %o5
+	ta	FAST_TRAP
+	brz,a	%o0, 1f
+	stuw	%o1, [%o2]
+1:	retl
+	nop
+	SET_SIZE(hvio_intr_getstate)
+
+	/*
+	 * arg0 - sysino
+	 * arg1 - intr_state
+	 *
+	 * ret0 - status
+	 */
+	ENTRY(hvio_intr_setstate)
+	mov	HVIO_INTR_SETSTATE, %o5
+	ta	FAST_TRAP
+	retl
+	nop
+	SET_SIZE(hvio_intr_setstate)
+
+	/*
+	 * arg0 - sysino
+	 *
+	 * ret0 - status
+	 * ret1 - cpu_id
+	 */
+	ENTRY(hvio_intr_gettarget)
+	mov	%o1, %o2
+	mov	HVIO_INTR_GETTARGET, %o5
+	ta	FAST_TRAP
+	brz,a	%o0, 1f
+	stuw	%o1, [%o2]
+1:	retl
+	nop
+	SET_SIZE(hvio_intr_gettarget)
+
+	/*
+	 * arg0 - sysino
+	 * arg1 - cpu_id
+	 *
+	 * ret0 - status
+	 */
+	ENTRY(hvio_intr_settarget)
+	mov	HVIO_INTR_SETTARGET, %o5
+	ta	FAST_TRAP
+	retl
+	nop
+	SET_SIZE(hvio_intr_settarget)
+
+	/*
+	 * arg0 - devhandle
+	 * arg1 - tsbid
+	 * arg2 - pages
+	 * arg3 - io_attributes
+	 * arg4 - io_page_list_p
+	 *
+	 * ret1 - pages_mapped
+	 */
+	ENTRY(hvio_iommu_map)
+	save	%sp, -SA(MINFRAME64), %sp
+	mov	%i0, %o0
+	mov	%i1, %o1
+	mov	%i2, %o2
+	mov	%i3, %o3
+	mov	%i4, %o4
+	mov	HVIO_IOMMU_MAP, %o5
+	ta	FAST_TRAP
+	brnz	%o0, 1f
+	mov	%o0, %i0
+	stuw	%o1, [%i5]
+1:
+	ret
+	restore
+	SET_SIZE(hvio_iommu_map)
+
+	/*
+	 * arg0 - devhandle
+	 * arg1 - tsbid
+	 * arg2 - pages
+	 *
+	 * ret1 - pages_demapped
+	 */
+	ENTRY(hvio_iommu_demap)
+	mov	HVIO_IOMMU_DEMAP, %o5
+	ta	FAST_TRAP
+	brz,a	%o0, 1f
+	stuw	%o1, [%o3]
+1:	retl
+	nop
+	SET_SIZE(hvio_iommu_demap)
+
+	/*
+	 * arg0 - devhandle
+	 * arg1 - tsbid
+	 *
+	 *
+	 * ret0 - status
+	 * ret1 - io_attributes
+	 * ret2 - r_addr
+	 */
+	ENTRY(hvio_iommu_getmap)
+	mov	%o2, %o4
+	mov	HVIO_IOMMU_GETMAP, %o5
+	ta	FAST_TRAP
+	brnz	%o0, 1f
+	nop
+	stx	%o2, [%o3]
+	st	%o1, [%o4]
+1:
+	retl
+	nop
+	SET_SIZE(hvio_iommu_getmap)
+
+	/*
+	 * arg0 - devhandle
+	 * arg1 - r_addr
+	 * arg2 - io_attributes
+	 *
+	 *
+	 * ret0 - status
+	 * ret1 - io_addr
+	 */
+	ENTRY(hvio_iommu_getbypass)
+	mov	HVIO_IOMMU_GETBYPASS, %o5
+	ta	FAST_TRAP
+	brz,a	%o0, 1f
+	stx	%o1, [%o3]
+1:	retl
+	nop
+	SET_SIZE(hvio_iommu_getbypass)
+
+	/*
+	 * arg0 - devhandle
+	 * arg1 - r_addr
+	 * arg2 - size
+	 *
+	 * ret1 - error_flag
+	 * ret2 - data
+	 */
+	ENTRY(hvio_peek)
+	mov	HVIO_PEEK, %o5
+	ta	FAST_TRAP
+	brnz	%o0, 1f
+	nop
+	stx	%o2, [%o4]
+	st	%o1, [%o3]
+1:
+	retl
+	nop
+	SET_SIZE(hvio_peek)
+
+	/*
+	 * arg0 - devhandle
+	 * arg1 - r_addr
+	 * arg2 - sizes
+	 * arg3 - data
+	 * arg4 - r_addr2
+	 *
+	 * ret1 - error_flag
+	 */
+	ENTRY(hvio_poke)
+	save	%sp, -SA(MINFRAME64), %sp
+	mov	%i0, %o0
+	mov	%i1, %o1
+	mov	%i2, %o2
+	mov	%i3, %o3
+	mov	%i4, %o4
+	mov	HVIO_POKE, %o5
+	ta	FAST_TRAP
+	brnz	%o0, 1f
+	mov	%o0, %i0
+	stuw	%o1, [%i5]
+1:
+	ret
+	restore
+	SET_SIZE(hvio_poke)
+
+	/*
+	 * arg0 - devhandle
+	 * arg1 - r_addr
+	 * arg2 - num_bytes
+	 * arg3 - io_sync_direction
+	 *
+	 * ret0 - status
+	 * ret1 - bytes_synched
+	 */
+	ENTRY(hvio_dma_sync)
+	mov	HVIO_DMA_SYNC, %o5
+	ta	FAST_TRAP
+	brz,a	%o0, 1f
+	stx	%o1, [%o4]
+1:	retl
+	nop
+	SET_SIZE(hvio_dma_sync)
+
+	/*
+	 * arg0 - devhandle
+	 * arg1 - msiq_id
+	 * arg2 - r_addr
+	 * arg3 - nentries
+	 *
+	 * ret0 - status
+	 */
+	ENTRY(hvio_msiq_conf)
+	mov	HVIO_MSIQ_CONF, %o5
+	ta	FAST_TRAP
+	retl
+	nop
+	SET_SIZE(hvio_msiq_conf)
+
+	/*
+	 * arg0 - devhandle
+	 * arg1 - msiq_id
+	 *
+	 * ret0 - status
+	 * ret1 - r_addr
+	 * ret1 - nentries
+	 */
+	ENTRY(hvio_msiq_info)
+	mov     %o2, %o4
+	mov     HVIO_MSIQ_INFO, %o5
+	ta      FAST_TRAP
+	brnz    %o0, 1f
+	nop
+	stx     %o1, [%o4]
+	stuw    %o2, [%o3]
+1:      retl
+	nop
+	SET_SIZE(hvio_msiq_info)
+
+	/*
+	 * arg0 - devhandle
+	 * arg1 - msiq_id
+	 *
+	 * ret0 - status
+	 * ret1 - msiq_valid_state
+	 */
+	ENTRY(hvio_msiq_getvalid)
+	mov	HVIO_MSIQ_GETVALID, %o5
+	ta	FAST_TRAP
+	brz,a	%o0, 1f
+	stuw	%o1, [%o2]
+1:	retl
+	nop
+	SET_SIZE(hvio_msiq_getvalid)
+
+	/*
+	 * arg0 - devhandle
+	 * arg1 - msiq_id
+	 * arg2 - msiq_valid_state
+	 *
+	 * ret0 - status
+	 */
+	ENTRY(hvio_msiq_setvalid)
+	mov	HVIO_MSIQ_SETVALID, %o5
+	ta	FAST_TRAP
+	retl
+	nop
+	SET_SIZE(hvio_msiq_setvalid)
+
+	/*
+	 * arg0 - devhandle
+	 * arg1 - msiq_id
+	 *
+	 * ret0 - status
+	 * ret1 - msiq_state
+	 */
+	ENTRY(hvio_msiq_getstate)
+	mov	HVIO_MSIQ_GETSTATE, %o5
+	ta	FAST_TRAP
+	brz,a	%o0, 1f
+	stuw	%o1, [%o2]
+1:	retl
+	nop
+	SET_SIZE(hvio_msiq_getstate)
+
+	/*
+	 * arg0 - devhandle
+	 * arg1 - msiq_id
+	 * arg2 - msiq_state
+	 *
+	 * ret0 - status
+	 */
+	ENTRY(hvio_msiq_setstate)
+	mov	HVIO_MSIQ_SETSTATE, %o5
+	ta	FAST_TRAP
+	retl
+	nop
+	SET_SIZE(hvio_msiq_setstate)
+
+	/*
+	 * arg0 - devhandle
+	 * arg1 - msiq_id
+	 *
+	 * ret0 - status
+	 * ret1 - msiq_head
+	 */
+	ENTRY(hvio_msiq_gethead)
+	mov	HVIO_MSIQ_GETHEAD, %o5
+	ta	FAST_TRAP
+	brz,a	%o0, 1f
+	stx	%o1, [%o2]
+1:	retl
+	nop
+	SET_SIZE(hvio_msiq_gethead)
+
+	/*
+	 * arg0 - devhandle
+	 * arg1 - msiq_id
+	 * arg2 - msiq_head
+	 *
+	 * ret0 - status
+	 */
+	ENTRY(hvio_msiq_sethead)
+	mov	HVIO_MSIQ_SETHEAD, %o5
+	ta	FAST_TRAP
+	retl
+	nop
+	SET_SIZE(hvio_msiq_sethead)
+
+	/*
+	 * arg0 - devhandle
+	 * arg1 - msiq_id
+	 *
+	 * ret0 - status
+	 * ret1 - msiq_tail
+	 */
+	ENTRY(hvio_msiq_gettail)
+	mov	HVIO_MSIQ_GETTAIL, %o5
+	ta	FAST_TRAP
+	brz,a	%o0, 1f
+	stx	%o1, [%o2]
+1:	retl
+	nop
+	SET_SIZE(hvio_msiq_gettail)
+
+	/*
+	 * arg0 - devhandle
+	 * arg1 - msi_num
+	 *
+	 * ret0 - status
+	 * ret1 - msiq_id
+	 */
+	ENTRY(hvio_msi_getmsiq)
+	mov	HVIO_MSI_GETMSIQ, %o5
+	ta	FAST_TRAP
+	brz,a	%o0, 1f
+	stuw	%o1, [%o2]
+1:	retl
+	nop
+	SET_SIZE(hvio_msi_getmsiq)
+
+	/*
+	 * arg0 - devhandle
+	 * arg1 - msi_num
+	 * arg2 - msiq_id
+	 * arg2 - msitype
+	 *
+	 * ret0 - status
+	 */
+	ENTRY(hvio_msi_setmsiq)
+	mov	HVIO_MSI_SETMSIQ, %o5
+	ta	FAST_TRAP
+	retl
+	nop
+	SET_SIZE(hvio_msi_setmsiq)
+
+	/*
+	 * arg0 - devhandle
+	 * arg1 - msi_num
+	 *
+	 * ret0 - status
+	 * ret1 - msi_valid_state
+	 */
+	ENTRY(hvio_msi_getvalid)
+	mov	HVIO_MSI_GETVALID, %o5
+	ta	FAST_TRAP
+	brz,a	%o0, 1f
+	stuw	%o1, [%o2]
+1:	retl
+	nop
+	SET_SIZE(hvio_msi_getvalid)
+
+	/*
+	 * arg0 - devhandle
+	 * arg1 - msi_num
+	 * arg2 - msi_valid_state
+	 *
+	 * ret0 - status
+	 */
+	ENTRY(hvio_msi_setvalid)
+	mov	HVIO_MSI_SETVALID, %o5
+	ta	FAST_TRAP
+	retl
+	nop
+	SET_SIZE(hvio_msi_setvalid)
+
+	/*
+	 * arg0 - devhandle
+	 * arg1 - msi_num
+	 *
+	 * ret0 - status
+	 * ret1 - msi_state
+	 */
+	ENTRY(hvio_msi_getstate)
+	mov	HVIO_MSI_GETSTATE, %o5
+	ta	FAST_TRAP
+	brz,a	%o0, 1f
+	stuw	%o1, [%o2]
+1:	retl
+	nop
+	SET_SIZE(hvio_msi_getstate)
+
+	/*
+	 * arg0 - devhandle
+	 * arg1 - msi_num
+	 * arg2 - msi_state
+	 *
+	 * ret0 - status
+	 */
+	ENTRY(hvio_msi_setstate)
+	mov	HVIO_MSI_SETSTATE, %o5
+	ta	FAST_TRAP
+	retl
+	nop
+	SET_SIZE(hvio_msi_setstate)
+
+	/*
+	 * arg0 - devhandle
+	 * arg1 - msg_type
+	 *
+	 * ret0 - status
+	 * ret1 - msiq_id
+	 */
+	ENTRY(hvio_msg_getmsiq)
+	mov	HVIO_MSG_GETMSIQ, %o5
+	ta	FAST_TRAP
+	brz,a	%o0, 1f
+	stuw	%o1, [%o2]
+1:	retl
+	nop
+	SET_SIZE(hvio_msg_getmsiq)
+
+	/*
+	 * arg0 - devhandle
+	 * arg1 - msg_type
+	 * arg2 - msiq_id
+	 *
+	 * ret0 - status
+	 */
+	ENTRY(hvio_msg_setmsiq)
+	mov	HVIO_MSG_SETMSIQ, %o5
+	ta	FAST_TRAP
+	retl
+	nop
+	SET_SIZE(hvio_msg_setmsiq)
+
+	/*
+	 * arg0 - devhandle
+	 * arg1 - msg_type
+	 *
+	 * ret0 - status
+	 * ret1 - msg_valid_state
+	 */
+	ENTRY(hvio_msg_getvalid)
+	mov	HVIO_MSG_GETVALID, %o5
+	ta	FAST_TRAP
+	brz,a	%o0, 1f
+	stuw	%o1, [%o2]
+1:	retl
+	nop
+	SET_SIZE(hvio_msg_getvalid)
+
+	/*
+	 * arg0 - devhandle
+	 * arg1 - msg_type
+	 * arg2 - msg_valid_state
+	 *
+	 * ret0 - status
+	 */
+	ENTRY(hvio_msg_setvalid)
+	mov	HVIO_MSG_SETVALID, %o5
+	ta	FAST_TRAP
+	retl
+	nop
+	SET_SIZE(hvio_msg_setvalid)
+
+	/*
+	 * hv_cpu_yield(void)
+	 */
+	ENTRY(hv_cpu_yield)
+	mov	HV_CPU_YIELD, %o5
+	ta	FAST_TRAP
+	retl
+	nop
+	SET_SIZE(hv_cpu_yield)
+
+	/*
+	 * hv_service_recv(uint64_t s_id, uint64_t buf_pa,
+	 *     uint64_t size, uint64_t *recv_bytes);
+	 */
+	ENTRY(hv_service_recv)
+	save	%sp, -SA(MINFRAME), %sp
+	mov	%i0, %o0
+	mov	%i1, %o1
+	mov	%i2, %o2
+	mov	%i3, %o3
+	mov	SVC_RECV, %o5
+	ta	FAST_TRAP
+	brnz	%o0, 1f
+	mov	%o0, %i0
+	stx	%o1, [%i3]
+1:
+	ret
+	restore
+	SET_SIZE(hv_service_recv)
+
+	/*
+	 * hv_service_send(uint64_t s_id, uint64_t buf_pa,
+	 *     uint64_t size, uint64_t *recv_bytes);
+	 */
+	ENTRY(hv_service_send)
+	save	%sp, -SA(MINFRAME), %sp
+	mov	%i0, %o0
+	mov	%i1, %o1
+	mov	%i2, %o2
+	mov	%i3, %o3
+	mov	SVC_SEND, %o5
+	ta	FAST_TRAP
+	brnz	%o0, 1f
+	mov	%o0, %i0
+	stx	%o1, [%i3]
+1:
+	ret
+	restore	
+	SET_SIZE(hv_service_send)
+	
+	/*
+	 * hv_service_getstatus(uint64_t s_id, uint64_t *vreg);
+	 */
+	ENTRY(hv_service_getstatus)
+	mov	%o1, %o4			! save datap
+	mov	SVC_GETSTATUS, %o5
+	ta	FAST_TRAP
+	brz,a	%o0, 1f
+	stx	%o1, [%o4]
+1:
+	retl
+	nop
+	SET_SIZE(hv_service_getstatus)
+	
+	/*
+	 * hv_service_setstatus(uint64_t s_id, uint64_t bits);
+	 */
+	ENTRY(hv_service_setstatus)
+	mov	SVC_SETSTATUS, %o5
+	ta	FAST_TRAP
+	retl
+	nop
+	SET_SIZE(hv_service_setstatus)
+
+	/*
+	 * hv_service_clrstatus(uint64_t s_id, uint64_t bits);
+	 */
+	ENTRY(hv_service_clrstatus)
+	mov	SVC_CLRSTATUS, %o5
+	ta	FAST_TRAP
+	retl
+	nop
+	SET_SIZE(hv_service_clrstatus)
+
+	/*
+	 * int hv_cpu_state(uint64_t cpuid, uint64_t *cpu_state);
+	 */
+	ENTRY(hv_cpu_state)
+	mov	%o1, %o4			! save datap
+	mov	HV_CPU_STATE, %o5
+	ta	FAST_TRAP
+	brz,a	%o0, 1f
+	stx	%o1, [%o4]
+1:
+	retl
+	nop
+	SET_SIZE(hv_cpu_state)
+
+	/*
+	 * HV state dump zone Configure
+	 * arg0 real adrs of dump buffer (%o0)
+	 * arg1 size of dump buffer (%o1)
+	 * ret0 status (%o0)
+	 * ret1 size of buffer on success and min size on EINVAL (%o1)
+	 * hv_dump_buf_update(uint64_t paddr, uint64_t size, uint64_t *ret_size)
+	 */
+	ENTRY(hv_dump_buf_update)
+	mov	DUMP_BUF_UPDATE, %o5
+	ta	FAST_TRAP
+	retl
+	stx	%o1, [%o2]
+	SET_SIZE(hv_dump_buf_update)
+
+
+	/*
+	 * For memory scrub
+	 * int hv_mem_scrub(uint64_t real_addr, uint64_t length,
+	 * 	uint64_t *scrubbed_len);
+	 * Retun %o0 -- status
+	 *       %o1 -- bytes scrubbed
+	 */
+	ENTRY(hv_mem_scrub)
+	mov	%o2, %o4
+	mov	HV_MEM_SCRUB, %o5
+	ta	FAST_TRAP
+	retl
+	stx	%o1, [%o4]
+	SET_SIZE(hv_mem_scrub)
+
+	/*
+	 * Flush ecache 
+	 * int hv_mem_sync(uint64_t real_addr, uint64_t length,
+	 * 	uint64_t *flushed_len);
+	 * Retun %o0 -- status
+	 *       %o1 -- bytes flushed
+	 */
+	ENTRY(hv_mem_sync)
+	mov	%o2, %o4
+	mov	HV_MEM_SYNC, %o5
+	ta	FAST_TRAP
+	retl
+	stx	%o1, [%o4]
+	SET_SIZE(hv_mem_sync)
+
+	/*
+	 * TTRACE_BUF_CONF Configure
+	 * arg0 RA base of buffer (%o0)
+	 * arg1 buf size in no. of entries (%o1)
+	 * ret0 status (%o0)
+	 * ret1 minimum size in no. of entries on failure,
+	 * actual size in no. of entries on success (%o1)
+	 */
+	ENTRY(hv_ttrace_buf_conf)
+	mov	TTRACE_BUF_CONF, %o5
+	ta	FAST_TRAP
+	retl
+	stx	%o1, [%o2]
+	SET_SIZE(hv_ttrace_buf_conf)
+
+	 /*
+	 * TTRACE_BUF_INFO
+	 * ret0 status (%o0)
+	 * ret1 RA base of buffer (%o1)
+	 * ret2 size in no. of entries (%o2)
+	 */
+	ENTRY(hv_ttrace_buf_info)
+	mov	%o0, %o3
+	mov	%o1, %o4
+	mov	TTRACE_BUF_INFO, %o5
+	ta	FAST_TRAP
+	stx	%o1, [%o3]
+	retl
+	stx	%o2, [%o4]
+	SET_SIZE(hv_ttrace_buf_info)
+
+	/*
+	 * TTRACE_ENABLE
+	 * arg0 enable/ disable (%o0)
+	 * ret0 status (%o0)
+	 * ret1 previous enable state (%o1)
+	 */
+	ENTRY(hv_ttrace_enable)
+	mov	%o1, %o2
+	mov	TTRACE_ENABLE, %o5
+	ta	FAST_TRAP
+	retl
+	stx	%o1, [%o2]
+	SET_SIZE(hv_ttrace_enable)
+
+	/*
+	 * TTRACE_FREEZE
+	 * arg0 enable/ freeze (%o0)
+	 * ret0 status (%o0)
+	 * ret1 previous freeze state (%o1)
+	*/
+	ENTRY(hv_ttrace_freeze)
+	mov	%o1, %o2
+	mov	TTRACE_FREEZE, %o5
+	ta	FAST_TRAP
+	retl
+	stx	%o1, [%o2]
+	SET_SIZE(hv_ttrace_freeze)
+
+	/*
+	 * MACH_DESC
+	 * arg0 buffer real address
+	 * arg1 pointer to uint64_t for size of buffer
+	 * ret0 status
+	 * ret1 return required size of buffer / returned data size
+	 */
+	ENTRY(hv_mach_desc)
+	mov     %o1, %o4                ! save datap
+	ldx     [%o1], %o1
+	mov     HV_MACH_DESC, %o5
+	ta      FAST_TRAP
+	retl
+	stx   %o1, [%o4]
+	SET_SIZE(hv_mach_desc)
+
+	/*
+	 * hv_ncs_request(int cmd, uint64_t realaddr, size_t sz)
+	 */
+	ENTRY(hv_ncs_request)
+	mov	HV_NCS_REQUEST, %o5
+	ta	FAST_TRAP
+	retl
+	nop
+	SET_SIZE(hv_ncs_request)
+
+	/*
+	 * hv_ra2pa(uint64_t ra)
+	 *
+	 * MACH_DESC
+	 * arg0 Real address to convert
+	 * ret0 Returned physical address or -1 on error
+	 */
+	ENTRY(hv_ra2pa)
+	mov	HV_RA2PA, %o5
+	ta	FAST_TRAP
+	cmp	%o0, 0
+	move	%xcc, %o1, %o0
+	movne	%xcc, -1, %o0
+	retl
+	nop
+	SET_SIZE(hv_ra2pa)
+
+	/*
+	 * hv_hpriv(void *func, uint64_t arg1, uint64_t arg2, uint64_t arg3)
+	 *
+	 * MACH_DESC
+	 * arg0 OS function to call
+	 * arg1 First arg to OS function
+	 * arg2 Second arg to OS function
+	 * arg3 Third arg to OS function
+	 * ret0 Returned value from function
+	 */
+	
+	ENTRY(hv_hpriv)
+	mov	HV_HPRIV, %o5
+	ta	FAST_TRAP
+	retl
+	nop
+	SET_SIZE(hv_hpriv)
diff --git a/sys/boot/sparc64/loader/main.c b/sys/boot/sparc64/loader/main.c
index c93e610..4d7031d 100644
--- a/sys/boot/sparc64/loader/main.c
+++ b/sys/boot/sparc64/loader/main.c
@@ -9,7 +9,6 @@
 
 #include <sys/cdefs.h>
 __FBSDID("$FreeBSD$");
-
 /*
  * FreeBSD/sparc64 kernel loader - machine dependent part
  *
@@ -25,7 +24,9 @@ __FBSDID("$FreeBSD$");
 #include <sys/param.h>
 #include <sys/queue.h>
 #include <sys/linker.h>
+#include <sys/types.h>
 
+#include <vm/vm.h>
 #include <machine/asi.h>
 #include <machine/atomic.h>
 #include <machine/cpufunc.h>
@@ -34,6 +35,7 @@ __FBSDID("$FreeBSD$");
 #include <machine/metadata.h>
 #include <machine/tte.h>
 #include <machine/upa.h>
+#include <machine/hypervisor_api.h>
 
 #include "bootstrap.h"
 #include "libofw.h"
@@ -50,6 +52,12 @@ struct memory_slice {
 	vm_offset_t size;
 };
 
+struct mmu_ops {
+	void (*tlb_init)(void);
+	int (*mmu_mapin)(vm_offset_t va, vm_size_t len);
+} *mmu_ops;
+
+
 typedef void kernel_entry_t(vm_offset_t mdp, u_long o1, u_long o2, u_long o3,
 			    void *openfirmware);
 
@@ -60,7 +68,33 @@ extern vm_offset_t dtlb_va_to_pa(vm_offset_t);
 extern vm_offset_t md_load(char *, vm_offset_t *);
 static int __elfN(exec)(struct preloaded_file *);
 static int sparc64_autoload(void);
-static int mmu_mapin(vm_offset_t, vm_size_t);
+static int mmu_mapin_sun4u(vm_offset_t, vm_size_t);
+static int mmu_mapin_sun4v(vm_offset_t, vm_size_t);
+static void tlb_init_sun4u(void);
+static void tlb_init_sun4v(void);
+
+struct mmu_ops mmu_ops_sun4u = 	{ tlb_init_sun4u, mmu_mapin_sun4u };
+struct mmu_ops mmu_ops_sun4v = 	{ tlb_init_sun4v, mmu_mapin_sun4v };
+
+extern char bootprog_name[], bootprog_rev[], bootprog_date[], bootprog_maker[];
+
+/* sun4u */
+struct tlb_entry *dtlb_store;
+struct tlb_entry *itlb_store;
+int dtlb_slot;
+int itlb_slot;
+int dtlb_slot_max;
+int itlb_slot_max;
+
+/* sun4v */
+struct tlb_entry *tlb_store;
+/* 
+ * no direct TLB access on sun4v
+ * we somewhat arbitrarily declare enough 
+ * slots to cover a 4GB AS with 4MB pages
+ */
+#define SUN4V_TLB_SLOT_MAX (1 << 10)
+
 
 extern char bootprog_name[], bootprog_rev[], bootprog_date[], bootprog_maker[];
 
@@ -204,14 +238,14 @@ sparc64_autoload(void)
 static ssize_t
 sparc64_readin(const int fd, vm_offset_t va, const size_t len)
 {
-	mmu_mapin(va, len);
+	mmu_ops->mmu_mapin(va, len);
 	return read(fd, (void *)va, len);
 }
 
 static ssize_t
 sparc64_copyin(const void *src, vm_offset_t dest, size_t len)
 {
-	mmu_mapin(dest, len);
+	mmu_ops->mmu_mapin(dest, len);
 	memcpy((void *)dest, src, len);
 	return len;
 }
@@ -252,7 +286,7 @@ __elfN(exec)(struct preloaded_file *fp)
 }
 
 static int
-mmu_mapin(vm_offset_t va, vm_size_t len)
+mmu_mapin_sun4u(vm_offset_t va, vm_size_t len)
 {
 	vm_offset_t pa, mva;
 	u_long data;
@@ -307,6 +341,53 @@ mmu_mapin(vm_offset_t va, vm_size_t len)
 	}
 	if (pa != (vm_offset_t)-1)
 		OF_release_phys(pa, PAGE_SIZE_4M);
+
+	return 0;
+}
+
+static int
+mmu_mapin_sun4v(vm_offset_t va, vm_size_t len)
+{
+
+	vm_offset_t pa, mva;
+	u_long data;
+	int ret;
+
+	if (va + len > curkva)
+		curkva = va + len;
+
+	pa = (vm_offset_t)-1;
+	len += va & PAGE_MASK_4M;
+	va &= ~PAGE_MASK_4M;
+	while (len) {
+		if ((va >> 22) > SUN4V_TLB_SLOT_MAX)
+			panic("trying to map more than 4GB");
+		if (tlb_store[va >> 22].te_pa == -1) {
+			/* Allocate a physical page, claim the virtual area */
+			if (pa == (vm_offset_t)-1) {
+				pa = (vm_offset_t)OF_alloc_phys(PAGE_SIZE_4M,
+				    PAGE_SIZE_4M);
+				if (pa == (vm_offset_t)-1)
+					panic("out of memory");
+				mva = (vm_offset_t)OF_claim_virt(va,
+				    PAGE_SIZE_4M, 0);
+				if (mva != va) {
+					panic("can't claim virtual page "
+					    "(wanted %#lx, got %#lx)",
+					    va, mva);
+				}
+			}
+
+			tlb_store[va >> 22].te_pa = pa;
+			if ((ret = OF_map_phys(-1, PAGE_SIZE_4M, va, pa)) != 0)
+				printf("OF_map_phys failed: %d\n", ret);
+			pa = (vm_offset_t)-1;
+		}
+		len -= len > PAGE_SIZE_4M ? PAGE_SIZE_4M : len;
+		va += PAGE_SIZE_4M;
+	}
+	if (pa != (vm_offset_t)-1)
+		OF_release_phys(pa, PAGE_SIZE_4M);
 	return 0;
 }
 
@@ -324,7 +405,7 @@ init_heap(void)
 }
 
 static void
-tlb_init(void)
+tlb_init_sun4u(void)
 {
 	phandle_t child;
 	phandle_t root;
@@ -361,11 +442,20 @@ tlb_init(void)
 		panic("init_tlb: malloc");
 }
 
+static void
+tlb_init_sun4v(void)
+{
+	tlb_store = malloc(SUN4V_TLB_SLOT_MAX * sizeof(*tlb_store));
+	memset(tlb_store, 0xFF, SUN4V_TLB_SLOT_MAX * sizeof(*tlb_store));
+}
+
 int
 main(int (*openfirm)(void *))
 {
 	char bootpath[64];
+	char compatible[32];
 	struct devsw **dp;
+	phandle_t rooth;
 	phandle_t chosenh;
 
 	/*
@@ -381,13 +471,22 @@ main(int (*openfirm)(void *))
 
 	init_heap();
 	setheap((void *)heapva, (void *)(heapva + HEAPSZ));
-
 	/*
 	 * Probe for a console.
 	 */
 	cons_probe();
 
-	tlb_init();
+	rooth = OF_peer(0);
+	OF_getprop(rooth, "compatible", compatible, sizeof(compatible));
+	if (!strcmp(compatible, "sun4v")) {
+		printf("\nBooting with sun4v support.\n");
+		mmu_ops = &mmu_ops_sun4v;
+	} else {
+		printf("\nBooting with sun4u support.\n");
+		mmu_ops = &mmu_ops_sun4u;
+	}
+
+	mmu_ops->tlb_init();
 
 	bcache_init(32, 512);