diff --git a/sys/boot/fdt/dts/riscv/spike.dts b/sys/boot/fdt/dts/riscv/spike.dts
index dc671141ac27..27d68f034c3b 100644
--- a/sys/boot/fdt/dts/riscv/spike.dts
+++ b/sys/boot/fdt/dts/riscv/spike.dts
@@ -1,5 +1,5 @@
 /*-
- * Copyright (c) 2015 Ruslan Bukin <br@bsdpad.com>
+ * Copyright (c) 2015-2016 Ruslan Bukin <br@bsdpad.com>
  * All rights reserved.
  *
  * Portions of this software were developed by SRI International and the
@@ -37,8 +37,8 @@
 /dts-v1/;
 
 / {
-	model = "UC Berkeley Spike Simulator RV64I";
-	compatible = "riscv,rv64i";
+	model = "UC Berkeley Spike Simulator RV64";
+	compatible = "riscv,rv64";
 	#address-cells = <1>;
 	#size-cells = <1>;
 	#interrupt-cells = <1>;
@@ -49,14 +49,14 @@
 
 		cpu@0 {
 			device_type = "cpu";
-			compatible = "riscv,rv64i";
-			reg = <0x40002000>;
+			compatible = "riscv,rv64";
+			reg = <0x40001000>;
 		};
 
 		cpu@1 {
 			device_type = "cpu";
-			compatible = "riscv,rv64i";
-			reg = <0x4000a000>;
+			compatible = "riscv,rv64";
+			reg = <0x40002000>;
 		};
 	};
 
@@ -66,12 +66,12 @@
 
 	memory {
 		device_type = "memory";
-		reg = <0x0 0x40000000>; /* 1GB at 0x0 */
+		reg = <0x80000000 0x40000000>; /* 1GB at 0x80000000 */
 	};
 
 	soc {
-		#address-cells = <2>;
-		#size-cells = <2>;
+		#address-cells = <1>;
+		#size-cells = <1>;
 		#interrupt-cells = <1>;
 
 		compatible = "simple-bus";
@@ -84,14 +84,15 @@
 
 		timer0: timer@0 {
 			compatible = "riscv,timer";
-			interrupts = < 1 >;
+			reg = < 0x40000000 0x100 >;
+			interrupts = < 5 >;
 			interrupt-parent = < &pic0 >;
 			clock-frequency = < 1000000 >;
 		};
 
 		htif0: htif@0 {
 			compatible = "riscv,htif";
-			interrupts = < 0 >;
+			interrupts = < 1 >;
 			interrupt-parent = < &pic0 >;
 
 			console0: console@0 {
diff --git a/sys/cddl/contrib/opensolaris/uts/common/sys/dtrace.h b/sys/cddl/contrib/opensolaris/uts/common/sys/dtrace.h
index cd5f4d0d5eed..92d35690ad75 100644
--- a/sys/cddl/contrib/opensolaris/uts/common/sys/dtrace.h
+++ b/sys/cddl/contrib/opensolaris/uts/common/sys/dtrace.h
@@ -2498,8 +2498,8 @@ extern void dtrace_helpers_destroy(proc_t *);
 
 #elif defined(__riscv__)
 
-#define	SD_RA_SP_MASK		0x1fff07f
-#define	SD_RA_SP		0x0113023
+#define	SD_RA_SP_MASK		0x01fff07f
+#define	SD_RA_SP		0x00113023
 
 #define	DTRACE_INVOP_SD		1
 #define	DTRACE_INVOP_RET	2
diff --git a/sys/cddl/dev/dtrace/riscv/dtrace_asm.S b/sys/cddl/dev/dtrace/riscv/dtrace_asm.S
index 21baa63483d6..a2e4b5283d93 100644
--- a/sys/cddl/dev/dtrace/riscv/dtrace_asm.S
+++ b/sys/cddl/dev/dtrace/riscv/dtrace_asm.S
@@ -57,8 +57,8 @@ END(dtrace_membar_consumer)
 dtrace_icookie_t dtrace_interrupt_disable(void)
 */
 ENTRY(dtrace_interrupt_disable)
-	csrrci	a0, sstatus, SSTATUS_IE
-	andi	a0, a0, SSTATUS_IE
+	csrrci	a0, sstatus, (SSTATUS_SIE)
+	andi	a0, a0, (SSTATUS_SIE)
 	RET
 END(dtrace_interrupt_disable)
 
diff --git a/sys/cddl/dev/dtrace/riscv/dtrace_subr.c b/sys/cddl/dev/dtrace/riscv/dtrace_subr.c
index 90d15fc1e0d5..429a442aa4d9 100644
--- a/sys/cddl/dev/dtrace/riscv/dtrace_subr.c
+++ b/sys/cddl/dev/dtrace/riscv/dtrace_subr.c
@@ -203,9 +203,9 @@ dtrace_trap(struct trapframe *frame, u_int type)
 		 * All the rest will be handled in the usual way.
 		 */
 		switch (type) {
-		case EXCP_LOAD_ACCESS_FAULT:
-		case EXCP_STORE_ACCESS_FAULT:
-		case EXCP_INSTR_ACCESS_FAULT:
+		case EXCP_FAULT_LOAD:
+		case EXCP_FAULT_STORE:
+		case EXCP_FAULT_FETCH:
 			/* Flag a bad address. */
 			cpu_core[curcpu].cpuc_dtrace_flags |= CPU_DTRACE_BADADDR;
 			cpu_core[curcpu].cpuc_dtrace_illval = 0;
diff --git a/sys/conf/ldscript.riscv b/sys/conf/ldscript.riscv
index 6c7d2d8ec258..152b97eca30d 100644
--- a/sys/conf/ldscript.riscv
+++ b/sys/conf/ldscript.riscv
@@ -6,7 +6,7 @@ SEARCH_DIR(/usr/lib);
 SECTIONS
 {
   /* Read-only sections, merged into text segment: */
-  . = kernbase + 0x100;
+  . = kernbase + 0x80000000 /* KERNENTRY */;
   .text      : AT(ADDR(.text) - kernbase)
   {
     *(.text)
diff --git a/sys/riscv/conf/GENERIC b/sys/riscv/conf/GENERIC
index 13f595a43fbb..dd73c3a352ac 100644
--- a/sys/riscv/conf/GENERIC
+++ b/sys/riscv/conf/GENERIC
@@ -26,6 +26,7 @@ makeoptions	DEBUG=-g		# Build kernel with gdb(1) debug symbols
 
 # FIXME: linker error. "--relax and -r may not be used together"
 makeoptions	WITHOUT_MODULES="usb otusfw mwlfw ispfw mwlfw ralfw rtwnfw urtwnfw"
+# makeoptions	NO_MODULES
 
 options 	SCHED_ULE		# ULE scheduler
 options 	PREEMPTION		# Enable kernel thread preemption
@@ -40,7 +41,6 @@ options 	UFS_ACL			# Support for access control lists
 options 	UFS_DIRHASH		# Improve performance on big directories
 options 	UFS_GJOURNAL		# Enable gjournal-based UFS journaling
 options 	QUOTA			# Enable disk quotas for UFS
-options 	MD_ROOT			# MD is a potential root device
 options 	NFSCL			# Network Filesystem Client
 options 	NFSD			# Network Filesystem Server
 options 	NFSLOCKD		# Network Lock Manager
@@ -93,6 +93,7 @@ options 	INVARIANT_SUPPORT	# Extra sanity checks of internal structures, require
 # options 	WITNESS_SKIPSPIN	# Don't run witness on spinlocks for speed
 options 	MALLOC_DEBUG_MAXZONES=8	# Separate malloc(9) zones
 # options 	EARLY_PRINTF
+# options 	VERBOSE_SYSINIT
 
 # Pseudo devices.
 device		loop		# Network loopback
diff --git a/sys/riscv/htif/htif.c b/sys/riscv/htif/htif.c
index b07b78998591..9a42db2c427e 100644
--- a/sys/riscv/htif/htif.c
+++ b/sys/riscv/htif/htif.c
@@ -177,11 +177,6 @@ htif_enumerate(struct htif_softc *sc)
 
 		htif_command(cmd);
 
-		/* Do poll as interrupts are disabled yet */
-		while (sc->identify_done == 0) {
-			htif_handle_entry(sc);
-		}
-
 		len = strnlen(id, sizeof(id));
 		if (len <= 0)
 			break;
diff --git a/sys/riscv/htif/htif_block.c b/sys/riscv/htif/htif_block.c
index a31de1c75a11..93662d825c1e 100644
--- a/sys/riscv/htif/htif_block.c
+++ b/sys/riscv/htif/htif_block.c
@@ -202,6 +202,7 @@ htif_blk_task(void *arg)
 	uint64_t req_paddr;
 	struct bio *bp;
 	uint64_t paddr;
+	uint64_t resp;
 	uint64_t cmd;
 	int i;
 
@@ -239,7 +240,8 @@ htif_blk_task(void *arg)
 			cmd |= req_paddr;
 
 			sc->cmd_done = 0;
-			htif_command(cmd);
+			resp = htif_command(cmd);
+			htif_blk_intr(sc, resp);
 
 			/* Wait for interrupt */
 			i = 0;
diff --git a/sys/riscv/htif/htif_console.c b/sys/riscv/htif/htif_console.c
index 200ad9e94244..7eca398b799b 100644
--- a/sys/riscv/htif/htif_console.c
+++ b/sys/riscv/htif/htif_console.c
@@ -57,8 +57,6 @@ __FBSDID("$FreeBSD$");
 
 #include <ddb/ddb.h>
 
-extern uint64_t console_intr;
-
 static tsw_outwakeup_t riscvtty_outwakeup;
 
 static struct ttydevsw riscv_ttydevsw = {
@@ -102,7 +100,7 @@ struct queue_entry *entry_last;
 struct queue_entry *entry_served;
 
 static void
-htif_putc(int c)
+riscv_putc(int c)
 {
 	uint64_t cmd;
 
@@ -110,57 +108,7 @@ htif_putc(int c)
 	cmd |= (CONSOLE_DEFAULT_ID << HTIF_DEV_ID_SHIFT);
 	cmd |= c;
 
-#ifdef SPIN_IN_MACHINE_MODE
-	machine_command(ECALL_HTIF_LOWPUTC, cmd);
-#else
-	htif_command(cmd);
-#endif
-
-}
-
-static uint8_t
-htif_getc(void)
-{
-	uint64_t cmd;
-	uint8_t res;
-
-	cmd = (HTIF_CMD_READ << HTIF_CMD_SHIFT);
-	cmd |= (CONSOLE_DEFAULT_ID << HTIF_DEV_ID_SHIFT);
-
-	res = htif_command(cmd);
-
-	return (res);
-}
-
-static void
-riscv_putc(int c)
-{
-	uint64_t counter;
-	uint64_t *cc;
-	uint64_t val;
-
-	val = 0;
-	counter = 0;
-
-	cc = (uint64_t*)&console_intr;
-	*cc = 0;
-
-	htif_putc(c);
-
-#ifndef SPIN_IN_MACHINE_MODE
-	/* Wait for an interrupt */
-	__asm __volatile(
-		"li	%0, 1\n"	/* counter = 1 */
-		"slli	%0, %0, 12\n"	/* counter <<= 12 */
-	"1:"
-		"addi	%0, %0, -1\n"	/* counter -= 1 */
-		"beqz	%0, 2f\n"	/* counter == 0 ? finish */
-		"ld	%1, 0(%2)\n"	/* val = *cc */
-		"beqz	%1, 1b\n"	/* val == 0 ? repeat */
-	"2:"
-		: "=&r"(counter), "=&r"(val) : "r"(cc)
-	);
-#endif
+	machine_command(ECALL_HTIF_CMD, cmd);
 }
 
 #ifdef EARLY_PRINTF
@@ -272,14 +220,19 @@ riscv_cngetc(struct consdev *cp)
 	uint64_t entry;
 	uint64_t devid;
 #endif
+	uint64_t cmd;
 	uint8_t data;
 	int ch;
 
-	htif_getc();
+	cmd = (HTIF_CMD_READ << HTIF_CMD_SHIFT);
+	cmd |= (CONSOLE_DEFAULT_ID << HTIF_DEV_ID_SHIFT);
+
+	machine_command(ECALL_HTIF_CMD_REQ, cmd);
 
 #if defined(KDB)
 	if (kdb_active) {
-		entry = machine_command(ECALL_HTIF_GET_ENTRY, 0);
+
+		entry = machine_command(ECALL_HTIF_CMD_RESP, 0);
 		while (entry) {
 			devid = HTIF_DEV_ID(entry);
 			devcmd = HTIF_DEV_CMD(entry);
@@ -294,7 +247,7 @@ riscv_cngetc(struct consdev *cp)
 				    devid);
 			}
 
-			entry = machine_command(ECALL_HTIF_GET_ENTRY, 0);
+			entry = machine_command(ECALL_HTIF_CMD_RESP, 0);
 		}
 	}
 #endif
diff --git a/sys/riscv/include/cpu.h b/sys/riscv/include/cpu.h
index c0a55a78a275..9b488ac9ecb6 100644
--- a/sys/riscv/include/cpu.h
+++ b/sys/riscv/include/cpu.h
@@ -1,5 +1,5 @@
 /*-
- * Copyright (c) 2015 Ruslan Bukin <br@bsdpad.com>
+ * Copyright (c) 2015-2016 Ruslan Bukin <br@bsdpad.com>
  * All rights reserved.
  *
  * Portions of this software were developed by SRI International and the
@@ -67,11 +67,10 @@
 
 #define	CPU_PART_SHIFT		62
 #define	CPU_PART_MASK		(0x3ul << CPU_PART_SHIFT)
-#define	CPU_PART(mcpuid)	((mcpuid & CPU_PART_MASK) >> CPU_PART_SHIFT)
-#define	CPU_PART_RV32I		0x0
-#define	CPU_PART_RV32E		0x1
-#define	CPU_PART_RV64I		0x2
-#define	CPU_PART_RV128I		0x3
+#define	CPU_PART(misa)		((misa & CPU_PART_MASK) >> CPU_PART_SHIFT)
+#define	CPU_PART_RV32		0x1
+#define	CPU_PART_RV64		0x2
+#define	CPU_PART_RV128		0x3
 
 extern char btext[];
 extern char etext[];
diff --git a/sys/riscv/include/cpufunc.h b/sys/riscv/include/cpufunc.h
index 4ee68dcfccbd..2db73ede884f 100644
--- a/sys/riscv/include/cpufunc.h
+++ b/sys/riscv/include/cpufunc.h
@@ -1,5 +1,5 @@
 /*-
- * Copyright (c) 2015 Ruslan Bukin <br@bsdpad.com>
+ * Copyright (c) 2015-2016 Ruslan Bukin <br@bsdpad.com>
  * All rights reserved.
  *
  * Portions of this software were developed by SRI International and the
@@ -54,11 +54,11 @@ intr_disable(void)
 	uint64_t ret;
 
 	__asm __volatile(
-		"csrrci %0, sstatus, 1"
-		: "=&r" (ret)
+		"csrrci %0, sstatus, %1"
+		: "=&r" (ret) : "i" (SSTATUS_SIE)
 	);
 
-	return (ret & SSTATUS_IE);
+	return (ret & (SSTATUS_SIE));
 }
 
 static __inline void
@@ -76,7 +76,8 @@ intr_enable(void)
 {
 
 	__asm __volatile(
-		"csrsi sstatus, 1"
+		"csrsi sstatus, %0"
+		:: "i" (SSTATUS_SIE)
 	);
 }
 
diff --git a/sys/riscv/include/db_machdep.h b/sys/riscv/include/db_machdep.h
index 7dfd90299d42..21550e4d574a 100644
--- a/sys/riscv/include/db_machdep.h
+++ b/sys/riscv/include/db_machdep.h
@@ -41,7 +41,7 @@
 #include <machine/frame.h>
 #include <machine/trap.h>
 
-#define	T_BREAKPOINT	(EXCP_INSTR_BREAKPOINT)
+#define	T_BREAKPOINT	(EXCP_BREAKPOINT)
 #define	T_WATCHPOINT	(0)
 
 typedef vm_offset_t	db_addr_t;
diff --git a/sys/riscv/include/intr.h b/sys/riscv/include/intr.h
index bc8c138ad01f..85c107bf507c 100644
--- a/sys/riscv/include/intr.h
+++ b/sys/riscv/include/intr.h
@@ -57,11 +57,23 @@ void riscv_unmask_ipi(void);
 #endif
 
 enum {
-	IRQ_SOFTWARE,
-	IRQ_TIMER,
-	IRQ_HTIF,
+	IRQ_SOFTWARE_USER,
+	IRQ_SOFTWARE_SUPERVISOR,
+	IRQ_SOFTWARE_HYPERVISOR,
+	IRQ_SOFTWARE_MACHINE,
+	IRQ_TIMER_USER,
+	IRQ_TIMER_SUPERVISOR,
+	IRQ_TIMER_HYPERVISOR,
+	IRQ_TIMER_MACHINE,
+	IRQ_EXTERNAL_USER,
+	IRQ_EXTERNAL_SUPERVISOR,
+	IRQ_EXTERNAL_HYPERVISOR,
+	IRQ_EXTERNAL_MACHINE,
+#if 0
+	/* lowRISC TODO */
 	IRQ_COP,	/* lowRISC only */
 	IRQ_UART,	/* lowRISC only */
+#endif
 	NIRQS
 };
 
diff --git a/sys/riscv/include/pte.h b/sys/riscv/include/pte.h
index d15cb84379fa..1658b90dca67 100644
--- a/sys/riscv/include/pte.h
+++ b/sys/riscv/include/pte.h
@@ -1,6 +1,6 @@
 /*-
  * Copyright (c) 2014 Andrew Turner
- * Copyright (c) 2015 Ruslan Bukin <br@bsdpad.com>
+ * Copyright (c) 2015-2016 Ruslan Bukin <br@bsdpad.com>
  * All rights reserved.
  *
  * Portions of this software were developed by SRI International and the
@@ -66,29 +66,18 @@ typedef	uint64_t	pn_t;			/* page number */
 #define	Ln_ADDR_MASK	(Ln_ENTRIES - 1)
 
 /* Bits 9:7 are reserved for software */
-#define	PTE_SW_MANAGED	(1 << 8)
-#define	PTE_SW_WIRED	(1 << 7)
-#define	PTE_DIRTY	(1 << 6) /* Virtual page is written */
-#define	PTE_REF		(1 << 5) /* Virtual page is referenced */
-#define	PTE_VALID	(1 << 0) /* Virtual page is valid */
-#define	PTE_TYPE_S	1
-#define	PTE_TYPE_M	(0xf << PTE_TYPE_S)
-#define	PTE_TYPE_PTR	0
-#define	PTE_TYPE_PTR_G	1
-#define	PTE_TYPE_SROURX	2	/* Supervisor read-only, user read-execute page. */
-#define	PTE_TYPE_SRWURWX 3	/* Supervisor read-write, user read-write-execute page. */
-#define	PTE_TYPE_SURO	4	/* Supervisor and user read-only page. */
-#define	PTE_TYPE_SURW	5	/* Supervisor and user read-write page. */
-#define	PTE_TYPE_SURX	6	/* Supervisor and user read-execute page. */
-#define	PTE_TYPE_SURWX	7	/* Supervisor and User Read Write Execute */
-#define	PTE_TYPE_SRO	8	/* Supervisor read-only page. */
-#define	PTE_TYPE_SRW	9	/* Supervisor read-write page. */
-#define	PTE_TYPE_SRX	10	/* Supervisor read-execute page. */
-#define	PTE_TYPE_SRWX	11	/* Supervisor read-write-execute page. */
-#define	PTE_TYPE_SRO_G	12	/* Supervisor read-only page--global mapping. */
-#define	PTE_TYPE_SRW_G	13	/* Supervisor read-write page--global mapping. */
-#define	PTE_TYPE_SRX_G	14	/* Supervisor read-execute page--global mapping. */
-#define	PTE_TYPE_SRWX_G	15	/* Supervisor Read Write Execute Global */
+#define	PTE_SW_MANAGED	(1 << 9)
+#define	PTE_SW_WIRED	(1 << 8)
+#define	PTE_D		(1 << 7) /* Dirty */
+#define	PTE_A		(1 << 6) /* Accessed */
+#define	PTE_G		(1 << 5) /* Global */
+#define	PTE_U		(1 << 4) /* User */
+#define	PTE_X		(1 << 3) /* Execute */
+#define	PTE_W		(1 << 2) /* Write */
+#define	PTE_R		(1 << 1) /* Read */
+#define	PTE_V		(1 << 0) /* Valid */
+#define	PTE_RWX		(PTE_R | PTE_W | PTE_X)
+#define	PTE_RX		(PTE_R | PTE_X)
 
 #define	PTE_PPN0_S	10
 #define	PTE_PPN1_S	19
diff --git a/sys/riscv/include/riscvreg.h b/sys/riscv/include/riscvreg.h
index b18df1798446..8c6b4dce2ebd 100644
--- a/sys/riscv/include/riscvreg.h
+++ b/sys/riscv/include/riscvreg.h
@@ -39,88 +39,127 @@
 
 /* Machine mode requests */
 #define	ECALL_MTIMECMP		0x01
-#define	ECALL_CLEAR_PENDING	0x02
-#define	ECALL_HTIF_CMD		0x03
-#define	ECALL_HTIF_GET_ENTRY	0x04
-#define	ECALL_MCPUID_GET	0x05
-#define	ECALL_MIMPID_GET	0x06
-#define	ECALL_SEND_IPI		0x07
-#define	ECALL_CLEAR_IPI		0x08
-#define	ECALL_HTIF_LOWPUTC	0x09
-#define	ECALL_MIE_SET		0x0a
-#define	ECALL_IO_IRQ_MASK	0x0b
+#define	ECALL_HTIF_GET_ENTRY	0x02
+#define	ECALL_MCPUID_GET	0x03
+#define	ECALL_MIMPID_GET	0x04
+#define	ECALL_SEND_IPI		0x05
+#define	ECALL_CLEAR_IPI		0x06
+#define	ECALL_MIE_SET		0x07
+#define	ECALL_IO_IRQ_MASK	0x08
+#define	ECALL_HTIF_CMD		0x09
+#define	ECALL_HTIF_CMD_REQ	0x0a
+#define	ECALL_HTIF_CMD_RESP	0x0b
 
 #define	EXCP_SHIFT			0
 #define	EXCP_MASK			(0xf << EXCP_SHIFT)
-#define	EXCP_INSTR_ADDR_MISALIGNED	0
-#define	EXCP_INSTR_ACCESS_FAULT		1
-#define	EXCP_INSTR_ILLEGAL		2
-#define	EXCP_INSTR_BREAKPOINT		3
-#define	EXCP_LOAD_ADDR_MISALIGNED	4
-#define	EXCP_LOAD_ACCESS_FAULT		5
-#define	EXCP_STORE_ADDR_MISALIGNED	6
-#define	EXCP_STORE_ACCESS_FAULT		7
-#define	EXCP_UMODE_ENV_CALL		8
-#define	EXCP_SMODE_ENV_CALL		9
-#define	EXCP_HMODE_ENV_CALL		10
-#define	EXCP_MMODE_ENV_CALL		11
-#define	EXCP_INTR			(1 << 31)
+#define	EXCP_MISALIGNED_FETCH		0
+#define	EXCP_FAULT_FETCH		1
+#define	EXCP_ILLEGAL_INSTRUCTION	2
+#define	EXCP_BREAKPOINT			3
+#define	EXCP_MISALIGNED_LOAD		4
+#define	EXCP_FAULT_LOAD			5
+#define	EXCP_MISALIGNED_STORE		6
+#define	EXCP_FAULT_STORE		7
+#define	EXCP_USER_ECALL			8
+#define	EXCP_SUPERVISOR_ECALL		9
+#define	EXCP_HYPERVISOR_ECALL		10
+#define	EXCP_MACHINE_ECALL		11
+#define	EXCP_INTR			(1ul << 63)
 #define	EXCP_INTR_SOFTWARE		0
 #define	EXCP_INTR_TIMER			1
 #define	EXCP_INTR_HTIF			2
 
-#define	SSTATUS_IE			(1 << 0)
-#define	SSTATUS_PIE			(1 << 3)
-#define	SSTATUS_PS			(1 << 4)
+#define	SSTATUS_UIE			(1 << 0)
+#define	SSTATUS_SIE			(1 << 1)
+#define	SSTATUS_UPIE			(1 << 4)
+#define	SSTATUS_SPIE			(1 << 5)
+#define	SSTATUS_SPIE_SHIFT		5
+#define	SSTATUS_SPP			(1 << 8)
+#define	SSTATUS_SPP_SHIFT		8
+#define	SSTATUS_FS_MASK			0x3
+#define	SSTATUS_FS_SHIFT		13
+#define	SSTATUS_XS_MASK			0x3
+#define	SSTATUS_XS_SHIFT		15
+#define	SSTATUS_PUM			(1 << 18)
+#define	SSTATUS32_SD			(1 << 63)
+#define	SSTATUS64_SD			(1 << 31)
 
-#define	MSTATUS_MPRV		(1 << 16)
-#define	MSTATUS_PRV_SHIFT	1
-#define	MSTATUS_PRV1_SHIFT	4
-#define	MSTATUS_PRV2_SHIFT	7
-#define	MSTATUS_PRV_MASK	(0x3 << MSTATUS_PRV_SHIFT)
-#define	MSTATUS_PRV_U		0	/* user */
-#define	MSTATUS_PRV_S		1	/* supervisor */
-#define	MSTATUS_PRV_H		2	/* hypervisor */
-#define	MSTATUS_PRV_M		3	/* machine */
+#define	MSTATUS_UIE			(1 << 0)
+#define	MSTATUS_SIE			(1 << 1)
+#define	MSTATUS_HIE			(1 << 2)
+#define	MSTATUS_MIE			(1 << 3)
+#define	MSTATUS_UPIE			(1 << 4)
+#define	MSTATUS_SPIE			(1 << 5)
+#define	MSTATUS_SPIE_SHIFT		5
+#define	MSTATUS_HPIE			(1 << 6)
+#define	MSTATUS_MPIE			(1 << 7)
+#define	MSTATUS_MPIE_SHIFT		7
+#define	MSTATUS_SPP			(1 << 8)
+#define	MSTATUS_SPP_SHIFT		8
+#define	MSTATUS_HPP_MASK		0x3
+#define	MSTATUS_HPP_SHIFT		9
+#define	MSTATUS_MPP_MASK		0x3
+#define	MSTATUS_MPP_SHIFT		11
+#define	MSTATUS_FS_MASK			0x3
+#define	MSTATUS_FS_SHIFT		13
+#define	MSTATUS_XS_MASK			0x3
+#define	MSTATUS_XS_SHIFT		15
+#define	MSTATUS_MPRV			(1 << 17)
+#define	MSTATUS_PUM			(1 << 18)
+#define	MSTATUS_VM_MASK			0x1f
+#define	MSTATUS_VM_SHIFT		24
+#define	 MSTATUS_VM_MBARE		0
+#define	 MSTATUS_VM_MBB			1
+#define	 MSTATUS_VM_MBBID		2
+#define	 MSTATUS_VM_SV32		8
+#define	 MSTATUS_VM_SV39		9
+#define	 MSTATUS_VM_SV48		10
+#define	 MSTATUS_VM_SV57		11
+#define	 MSTATUS_VM_SV64		12
+#define	MSTATUS32_SD			(1 << 63)
+#define	MSTATUS64_SD			(1 << 31)
 
-#define	MSTATUS_VM_SHIFT	17
-#define	MSTATUS_VM_MASK		0x1f
-#define	MSTATUS_VM_MBARE	0
-#define	MSTATUS_VM_MBB		1
-#define	MSTATUS_VM_MBBID	2
-#define	MSTATUS_VM_SV32		8
-#define	MSTATUS_VM_SV39		9
-#define	MSTATUS_VM_SV48		10
+#define	MSTATUS_PRV_U			0	/* user */
+#define	MSTATUS_PRV_S			1	/* supervisor */
+#define	MSTATUS_PRV_H			2	/* hypervisor */
+#define	MSTATUS_PRV_M			3	/* machine */
 
+#define	MIE_USIE	(1 << 0)
 #define	MIE_SSIE	(1 << 1)
 #define	MIE_HSIE	(1 << 2)
 #define	MIE_MSIE	(1 << 3)
+#define	MIE_UTIE	(1 << 4)
 #define	MIE_STIE	(1 << 5)
 #define	MIE_HTIE	(1 << 6)
 #define	MIE_MTIE	(1 << 7)
 
+#define	MIP_USIP	(1 << 0)
 #define	MIP_SSIP	(1 << 1)
 #define	MIP_HSIP	(1 << 2)
 #define	MIP_MSIP	(1 << 3)
+#define	MIP_UTIP	(1 << 4)
 #define	MIP_STIP	(1 << 5)
 #define	MIP_HTIP	(1 << 6)
 #define	MIP_MTIP	(1 << 7)
 
-#define	SR_IE		(1 << 0)
-#define	SR_IE1		(1 << 3)
-#define	SR_IE2		(1 << 6)
-#define	SR_IE3		(1 << 9)
-
+#define	SIE_USIE	(1 << 0)
 #define	SIE_SSIE	(1 << 1)
+#define	SIE_UTIE	(1 << 4)
 #define	SIE_STIE	(1 << 5)
 
+#define	MIP_SEIP	(1 << 9)
+
 /* Note: sip register has no SIP_STIP bit in Spike simulator */
 #define	SIP_SSIP	(1 << 1)
 #define	SIP_STIP	(1 << 5)
 
+#if 0
+/* lowRISC TODO */
 #define	NCSRS		4096
 #define	CSR_IPI		0x783
 #define	CSR_IO_IRQ	0x7c0	/* lowRISC only? */
+#endif
+
 #define	XLEN		8
 #define	INSN_SIZE	4
 
diff --git a/sys/riscv/include/vmparam.h b/sys/riscv/include/vmparam.h
index a2166c80e1df..4e1cdcfd6e52 100644
--- a/sys/riscv/include/vmparam.h
+++ b/sys/riscv/include/vmparam.h
@@ -196,7 +196,7 @@
 #define	SHAREDPAGE		(VM_MAXUSER_ADDRESS - PAGE_SIZE)
 #define	USRSTACK		SHAREDPAGE
 
-#define	KERNENTRY		(0x200)
+#define	KERNENTRY		(0x80000000)
 
 /*
  * How many physical pages per kmem arena virtual page.
diff --git a/sys/riscv/riscv/exception.S b/sys/riscv/riscv/exception.S
index c00094c3e911..5b8ef7a7ae49 100644
--- a/sys/riscv/riscv/exception.S
+++ b/sys/riscv/riscv/exception.S
@@ -117,14 +117,15 @@ __FBSDID("$FreeBSD$");
 	ld	t0, (TF_SSTATUS)(sp)
 .if \el == 0
 	/* Ensure user interrupts will be enabled on eret. */
-	ori	t0, t0, SSTATUS_PIE
+	li	t1, SSTATUS_SPIE
+	or	t0, t0, t1
 .else
 	/*
 	 * Disable interrupts for supervisor mode exceptions.
 	 * For user mode exceptions we have already done this
 	 * in do_ast.
 	 */
-	li	t1, ~SSTATUS_IE
+	li	t1, ~SSTATUS_SIE
 	and	t0, t0, t1
 .endif
 	csrw	sstatus, t0
@@ -182,7 +183,7 @@ __FBSDID("$FreeBSD$");
 	/* Disable interrupts */
 	csrr	a4, sstatus
 1:
-	csrci	sstatus, SSTATUS_IE
+	csrci	sstatus, (SSTATUS_SIE)
 
 	ld	a1, PC_CURTHREAD(gp)
 	lw	a2, TD_FLAGS(a1)
@@ -192,7 +193,7 @@ __FBSDID("$FreeBSD$");
 	beqz	a2, 2f
 
 	/* Restore interrupts */
-	andi	a4, a4, SSTATUS_IE
+	andi	a4, a4, (SSTATUS_SIE)
 	csrs	sstatus, a4
 
 	/* Handle the ast */
@@ -204,12 +205,24 @@ __FBSDID("$FreeBSD$");
 2:
 .endm
 
+ENTRY(cpu_exception_handler)
+	csrrw	sp, sscratch, sp
+	beqz	sp, 1f
+	/* User mode detected */
+	csrrw	sp, sscratch, sp
+	j	cpu_exception_handler_user
+1:
+	/* Supervisor mode detected */
+	csrrw	sp, sscratch, sp
+	j	cpu_exception_handler_supervisor
+END(cpu_exception_handler)
+
 ENTRY(cpu_exception_handler_supervisor)
 	save_registers 1
 	mv	a0, sp
 	call	_C_LABEL(do_trap_supervisor)
 	load_registers 1
-	eret
+	sret
 END(cpu_exception_handler_supervisor)
 
 ENTRY(cpu_exception_handler_user)
@@ -220,7 +233,7 @@ ENTRY(cpu_exception_handler_user)
 	do_ast
 	load_registers 0
 	csrrw	sp, sscratch, sp
-	eret
+	sret
 END(cpu_exception_handler_user)
 
 /*
@@ -230,7 +243,7 @@ END(cpu_exception_handler_user)
 bad_trap:
 	j bad_trap
 
-user_trap:
+machine_trap:
 	/* Save state */
 	csrrw	sp, mscratch, sp
 	addi	sp, sp, -64
@@ -242,91 +255,84 @@ user_trap:
 	sd	t5, (8 * 5)(sp)
 	sd	a0, (8 * 7)(sp)
 
-	la	t2, _C_LABEL(cpu_exception_handler_user)
-
-	csrr    t0, mcause
-	bltz    t0, machine_interrupt
-	j	exit_mrts
-
-supervisor_trap:
-	/* Save state */
-	csrrw	sp, mscratch, sp
-	addi	sp, sp, -64
-	sd	t0, (8 * 0)(sp)
-	sd	t1, (8 * 1)(sp)
-	sd	t2, (8 * 2)(sp)
-	sd	t3, (8 * 3)(sp)
-	sd	t4, (8 * 4)(sp)
-	sd	t5, (8 * 5)(sp)
-	sd	a0, (8 * 7)(sp)
-
-	la	t2, _C_LABEL(cpu_exception_handler_supervisor)
-
+	csrr	t3, mstatus	/* Required for debug */
 	csrr	t0, mcause
 	bltz	t0, machine_interrupt
 
-	li	t1, EXCP_SMODE_ENV_CALL
+	li	t1, EXCP_SUPERVISOR_ECALL
 	beq	t0, t1, supervisor_call
-	j	exit_mrts
+4:
+	/* NOT REACHED */
+	j	4b
 
 machine_interrupt:
 	/* Type of interrupt ? */
 	csrr	t0, mcause
 	andi	t0, t0, EXCP_MASK
-	li	t1, 0
-	beq	t1, t0, software_interrupt
-	li	t1, 1
-	beq	t1, t0, timer_interrupt
-	li	t1, 2
-	beq	t1, t0, htif_interrupt
+#if 0
+	/* lowRISC TODO */
 	li	t1, 4
 	beq	t1, t0, io_interrupt	/* lowRISC only */
+#endif
+	li	t1, 1
+	beq	t1, t0, supervisor_software_interrupt
+	li	t1, 3
+	beq	t1, t0, machine_software_interrupt
+	li	t1, 5
+	beq	t1, t0, supervisor_timer_interrupt
+	li	t1, 7
+	beq	t1, t0, machine_timer_interrupt
 
-	/* not reached */
+	/* NOT REACHED */
 1:
 	j	1b
 
+#if 0
+	/* lowRISC TODO */
 io_interrupt:
 	/* Disable IO interrupts so we can go to supervisor mode */
 	csrwi	CSR_IO_IRQ, 0
 
 	/* Handle the trap in supervisor mode */
 	j	exit_mrts
+#endif
 
-software_interrupt:
+supervisor_software_interrupt:
+1:
+	/* Nothing here as we are using mideleg feature */
+	j	1b
+
+machine_software_interrupt:
+	/* Clear IPI */
+	li	t0, 0x40001000
+	csrr	t2, mhartid
+	li	t3, 0x1000
+	mul	t2, t2, t3
+	add	t0, t0, t2
+	li	t2, 0
+	sd	t2, 0(t0)
+
+	/* Clear machine software pending bit */
 	li	t0, MIP_MSIP
 	csrc	mip, t0
+
+	/* Post supervisor software interrupt */
 	li	t0, MIP_SSIP
 	csrs	mip, t0
 
-	/* If PRV1 is PRV_U (user) then serve the trap */
-	csrr	t0, mstatus
-	li	t1, (MSTATUS_PRV_M << MSTATUS_PRV1_SHIFT)
-	and	t0, t0, t1
-	beqz	t0, 1f
-
-	/*
-	 * If PRV1 is supervisor and interrupts were enabled,
-	 * then serve the trap.
-	 */
-	csrr	t0, mstatus
-	li	t1, (SR_IE1 | (MSTATUS_PRV_M << MSTATUS_PRV1_SHIFT))
-	and	t0, t0, t1
-	li	t1, (SR_IE1 | (MSTATUS_PRV_S << MSTATUS_PRV1_SHIFT))
-	beq	t0, t1, 1f
-
 	j	exit
 
+supervisor_timer_interrupt:
 1:
-	/* Handle the trap in supervisor mode */
-	j	exit_mrts
+	/* Nothing here as we are using mideleg feature */
+	j	1b
 
-timer_interrupt:
+machine_timer_interrupt:
 	/* Disable machine timer interrupts */
 	li	t0, MIE_MTIE
 	csrc	mie, t0
 
-	/* Clear machine pending */
+	/* Clear machine timer interrupt pending */
 	li	t0, MIP_MTIP
 	csrc	mip, t0
 
@@ -334,54 +340,32 @@ timer_interrupt:
 	li	t0, MIP_STIP
 	csrs	mip, t0
 
-	/* If PRV1 is PRV_U (user) then serve the trap */
-	csrr	t0, mstatus
-	li	t1, (MSTATUS_PRV_M << MSTATUS_PRV1_SHIFT)
-	and	t0, t0, t1
-	beqz	t0, 1f
-
 	/*
-	 * If PRV1 is supervisor and interrupts were enabled,
-	 * then serve the trap.
+	 * Check for HTIF interrupts.
+	 * The only interrupt expected here is key press.
 	 */
-	csrr	t0, mstatus
-	li	t1, (SR_IE1 | (MSTATUS_PRV_M << MSTATUS_PRV1_SHIFT))
-	and	t0, t0, t1
-	li	t1, (SR_IE1 | (MSTATUS_PRV_S << MSTATUS_PRV1_SHIFT))
-	beq	t0, t1, 1f
+	la	t0, htif_lock
+	li	t2, 1
+	amoswap.d t3, t2, 0(t0)
+	bnez	t3, 5f		/* Another operation in progress, give up */
 
-	j	exit
+	/* We have lock */
+	la	t1, fromhost
+	ld	t5, 0(t1)
+	beqz	t5, 4f
 
-1:
-	/* Serve a trap in supervisor mode */
-	j	exit_mrts
-
-htif_interrupt:
-1:
-	li	t5, 0
-	csrrw	t5, mfromhost, t5
-	beqz	t5, 3f
-
-	/* Console PUT intr ? */
+	/* Console GET intr ? */
 	mv	t1, t5
-	li	t0, 0x101
+	li	t0, 0x100
 	srli	t1, t1, 48
-	bne	t1, t0, 2f
-	/* Yes */
-	la	t0, console_intr
-	li	t1, 1
-	sd	t1, 0(t0)
-
-	/* Check if there is any other pending event */
+	beq	t1, t0, 2f
+1:
+	/* There is no interrupts except keypress */
 	j	1b
 
 2:
 	/* Save entry */
 	la	t0, htif_ring
-	csrr	t1, mhartid
-	li	t4, (HTIF_RING_SIZE + 16)
-	mulw	t4, t4, t1
-	add	t0, t0, t4
 	li	t4, (HTIF_RING_SIZE)
 	add	t0, t0, t4	/* t0 == htif_ring_cursor */
 
@@ -397,10 +381,18 @@ htif_interrupt:
 	li	t0, MIP_SSIP
 	csrs	mip, t0
 
-	/* Check if there is any other pending event */
-	j	1b
-
 3:
+	la	t1, fromhost
+	li	t5, 0
+	sd	t5, 0(t1)
+
+4:
+	/* Release lock */
+	la	t0, htif_lock
+	li	t2, 0
+	amoswap.d t3, t2, 0(t0)
+
+5:
 	j	exit
 
 supervisor_call:
@@ -408,12 +400,14 @@ supervisor_call:
 	addi	t1, t1, 4	/* Next instruction in t1 */
 	li	t4, ECALL_HTIF_CMD
 	beq	t5, t4, htif_cmd
+	li	t4, ECALL_HTIF_CMD_REQ
+	beq	t5, t4, htif_cmd_req
+	li	t4, ECALL_HTIF_CMD_RESP
+	beq	t5, t4, htif_cmd_resp
 	li	t4, ECALL_HTIF_GET_ENTRY
 	beq	t5, t4, htif_get_entry
 	li	t4, ECALL_MTIMECMP
 	beq	t5, t4, set_mtimecmp
-	li	t4, ECALL_CLEAR_PENDING
-	beq	t5, t4, clear_pending
 	li	t4, ECALL_MCPUID_GET
 	beq	t5, t4, mcpuid_get
 	li	t4, ECALL_MIMPID_GET
@@ -422,24 +416,28 @@ supervisor_call:
 	beq	t5, t4, send_ipi
 	li	t4, ECALL_CLEAR_IPI
 	beq	t5, t4, clear_ipi
-	li	t4, ECALL_HTIF_LOWPUTC
-	beq	t5, t4, htif_lowputc
 	li	t4, ECALL_MIE_SET
 	beq	t5, t4, mie_set
+#if 0
+	/* lowRISC TODO */
 	li	t4, ECALL_IO_IRQ_MASK
 	beq	t5, t4, io_irq_mask
+#endif
 	j	exit_next_instr
 
+#if 0
+	/* lowRISC TODO */
 io_irq_mask:
 	csrw	CSR_IO_IRQ, t6
 	j	exit_next_instr
+#endif
 
 mie_set:
 	csrs	mie, t6
 	j	exit_next_instr
 
 mcpuid_get:
-	csrr	t6, mcpuid
+	csrr	t6, misa
 	j	exit_next_instr
 
 mimpid_get:
@@ -447,10 +445,8 @@ mimpid_get:
 	j	exit_next_instr
 
 send_ipi:
-	/* CPU mmio base in t6 */
+	/* CPU ipi MMIO register in t6 */
 	mv	t0, t6
-	li	t2, (CSR_IPI * XLEN)
-	add	t0, t0, t2	/* t0 = CSR_IPI */
 	li	t2, 1
 	sd	t2, 0(t0)
 	j	exit_next_instr
@@ -458,10 +454,6 @@ send_ipi:
 clear_ipi:
 	/* Do only clear if there are no new entries in HTIF ring */
 	la	t0, htif_ring
-	csrr	t2, mhartid
-	li	t4, (HTIF_RING_SIZE + 16)
-	mulw	t4, t4, t2
-	add	t0, t0, t4
 	li	t4, (HTIF_RING_SIZE)
 	add	t0, t0, t4	/* t0  == ptr to htif_ring_cursor */
 	ld	t2, 8(t0)	/* load htif_ring_last */
@@ -478,10 +470,6 @@ clear_ipi:
 htif_get_entry:
 	/* Get a htif_ring for current core */
 	la	t0, htif_ring
-	csrr	t2, mhartid
-	li	t4, (HTIF_RING_SIZE + 16)
-	mulw	t4, t4, t2
-	add	t0, t0, t4
 	li	t4, (HTIF_RING_SIZE + 8)
 	add	t0, t0, t4	/* t0 == htif_ring_last */
 
@@ -502,41 +490,81 @@ htif_get_entry:
 	/* Exit. Result is stored in t6 */
 	j	exit_next_instr
 
-htif_cmd:
+htif_cmd_resp:
+	la	t0, htif_lock
+	li	t2, 1
 1:
-	mv	t0, t6
-	csrrw	t0, mtohost, t0
-	bnez	t0, 1b
-	j	exit_next_instr
+	amoswap.d t3, t2, 0(t0)
+	bnez	t3, 1b
 
-htif_lowputc:
-1:
-	mv	t0, t6
-	csrrw	t0, mtohost, t0
-	bnez	t0, 1b
+	/* We have lock. Read for data */
+	la	t4, fromhost
+	ld	t6, 0(t4)
+	beqz	t6, 2f
+
+	/* Clear event */
+	li	t5, 0
+	sd	t5, 0(t4)
 
 2:
-	li	t4, 0
-	csrrw	t5, mfromhost, t4
-	beqz	t5, 2b
+	/* Release lock */
+	la	t0, htif_lock
+	li	t2, 0
+	amoswap.d t3, t2, 0(t0)
 
-	/* Console PUT intr ? */
-	mv	t2, t5
-	srli	t2, t2, 48
-	li	t3, 0x0101
-	beq	t2, t3, 3f
+	j	exit_next_instr
 
-	/* Not a console PUT, so save entry */
+htif_cmd_req:
+	la	t0, htif_lock
+	li	t2, 1
+1:
+	amoswap.d t3, t2, 0(t0)
+	bnez	t3, 1b
+
+	/* We have lock. Store new request */
+	la	t4, tohost
+	sd	t6, 0(t4)
+
+	/* Release lock */
+	la	t0, htif_lock
+	li	t2, 0
+	amoswap.d t3, t2, 0(t0)
+
+	j	exit_next_instr
+
+htif_cmd:
+	la	t0, htif_lock
+	li	t2, 1
+1:
+	amoswap.d t3, t2, 0(t0)
+	bnez	t3, 1b
+
+	mv	t3, t6
+
+	/* We have lock. Store new request */
+	la	t4, tohost
+	sd	t6, 0(t4)
+2:
+	/* Poll for result */
+	la	t4, fromhost
+	ld	t6, 0(t4)
+	beqz	t6, 2b
+
+	/* Check for unexpected event */
+	srli	t0, t6, 48
+	srli	t2, t3, 48
+	beq	t2, t0, 3f
+
+	/*
+	 * We have something unexpected (e.g. keyboard keypress)
+	 * Save entry.
+	 */
 	la	t0, htif_ring
-	csrr	t2, mhartid
-	li	t4, (HTIF_RING_SIZE + 16)
-	mulw	t4, t4, t2
-	add	t0, t0, t4
 	li	t4, (HTIF_RING_SIZE)
 	add	t0, t0, t4	/* t0 == htif_ring_cursor */
 
 	ld	t2, 0(t0)	/* load ptr to cursor */
-	sd	t5, 0(t2)	/* put entry */
+	sd	t6, 0(t2)	/* put entry */
 	li	t4, 1
 	sd	t4, 8(t2)	/* mark used */
 	ld	t4, 16(t2)	/* take next */
@@ -547,27 +575,30 @@ htif_lowputc:
 	li	t0, MIP_SSIP
 	csrs	mip, t0
 
-	/* Wait for console intr again */
+	/* Clear and look for response again */
+	la	t2, fromhost
+	li	t5, 0
+	sd	t5, 0(t2)
 	j	2b
 
 3:
+	la	t2, fromhost
+	li	t5, 0
+	sd	t5, 0(t2)
+
+	/* Release lock */
+	la	t0, htif_lock
+	li	t2, 0
+	amoswap.d t3, t2, 0(t0)
+
 	j	exit_next_instr
 
 set_mtimecmp:
-	csrr	t2, stime
-	add	t6, t6, t2
-	csrw	mtimecmp, t6
-
 	/* Enable interrupts */
 	li	t0, (MIE_MTIE | MIE_STIE)
 	csrs	mie, t0
 	j	exit_next_instr
 
-clear_pending:
-	li      t0, MIP_STIP
-	csrc    mip, t0
-	j	exit_next_instr
-
 /*
  * Trap exit functions
  */
@@ -585,27 +616,7 @@ exit:
 	ld	a0, (8 * 7)(sp)
 	addi	sp, sp, 64
 	csrrw	sp, mscratch, sp
-	eret
+	mret
 
-/*
- * Redirect to supervisor
- */
 exit_mrts:
-	/* Setup exception handler */
-	li	t1, KERNBASE
-	add	t2, t2, t1
-	csrw	stvec, t2
-
-	/* Restore state */
-	ld	t0, (8 * 0)(sp)
-	ld	t1, (8 * 1)(sp)
-	ld	t2, (8 * 2)(sp)
-	ld	t3, (8 * 3)(sp)
-	ld	t4, (8 * 4)(sp)
-	ld	t5, (8 * 5)(sp)
-	ld	a0, (8 * 7)(sp)
-	addi	sp, sp, 64
-	csrrw	sp, mscratch, sp
-
-	/* Redirect to supervisor */
-	mrts
+	j	exit_mrts
diff --git a/sys/riscv/riscv/genassym.c b/sys/riscv/riscv/genassym.c
index c9e9bc1c991a..c6c8b8518fa5 100644
--- a/sys/riscv/riscv/genassym.c
+++ b/sys/riscv/riscv/genassym.c
@@ -57,6 +57,7 @@ __FBSDID("$FreeBSD$");
 #include <machine/intr.h>
 
 ASSYM(KERNBASE, KERNBASE);
+ASSYM(KERNENTRY, KERNENTRY);
 ASSYM(VM_MAXUSER_ADDRESS, VM_MAXUSER_ADDRESS);
 ASSYM(VM_MAX_KERNEL_ADDRESS, VM_MAX_KERNEL_ADDRESS);
 ASSYM(TDF_ASTPENDING, TDF_ASTPENDING);
diff --git a/sys/riscv/riscv/identcpu.c b/sys/riscv/riscv/identcpu.c
index dd1f2ba13382..8226423b2fc1 100644
--- a/sys/riscv/riscv/identcpu.c
+++ b/sys/riscv/riscv/identcpu.c
@@ -1,5 +1,5 @@
 /*-
- * Copyright (c) 2015 Ruslan Bukin <br@bsdpad.com>
+ * Copyright (c) 2015-2016 Ruslan Bukin <br@bsdpad.com>
  * All rights reserved.
  *
  * Portions of this software were developed by SRI International and the
@@ -67,28 +67,16 @@ struct cpu_parts {
 struct cpu_implementers {
 	u_int			impl_id;
 	const char		*impl_name;
-	/*
-	 * Part number is implementation defined
-	 * so each vendor will have its own set of values and names.
-	 */
-	const struct cpu_parts	*cpu_parts;
 };
-#define	CPU_IMPLEMENTER_NONE	{ 0, "Unknown Implementer", cpu_parts_none }
+#define	CPU_IMPLEMENTER_NONE	{ 0, "Unknown Implementer" }
 
 /*
- * Per-implementer table of (PartNum, CPU Name) pairs.
+ * CPU base
  */
-/* UC Berkeley */
-static const struct cpu_parts cpu_parts_ucb[] = {
-	{ CPU_PART_RV32I,	"RV32I" },
-	{ CPU_PART_RV32E,	"RV32E" },
-	{ CPU_PART_RV64I,	"RV64I" },
-	{ CPU_PART_RV128I,	"RV128I" },
-	CPU_PART_NONE,
-};
-
-/* Unknown */
-static const struct cpu_parts cpu_parts_none[] = {
+static const struct cpu_parts cpu_parts_std[] = {
+	{ CPU_PART_RV32,	"RV32" },
+	{ CPU_PART_RV64,	"RV64" },
+	{ CPU_PART_RV128,	"RV128" },
 	CPU_PART_NONE,
 };
 
@@ -96,7 +84,7 @@ static const struct cpu_parts cpu_parts_none[] = {
  * Implementers table.
  */
 const struct cpu_implementers cpu_implementers[] = {
-	{ CPU_IMPL_UCB_ROCKET,	"UC Berkeley Rocket",	cpu_parts_ucb },
+	{ CPU_IMPL_UCB_ROCKET,	"UC Berkeley Rocket" },
 	CPU_IMPLEMENTER_NONE,
 };
 
@@ -107,16 +95,15 @@ identify_cpu(void)
 	uint32_t part_id;
 	uint32_t impl_id;
 	uint64_t mimpid;
-	uint64_t mcpuid;
+	uint64_t misa;
 	u_int cpu;
 	size_t i;
 
 	cpu_partsp = NULL;
 
 	mimpid = machine_command(ECALL_MIMPID_GET, 0);
-	mcpuid = machine_command(ECALL_MCPUID_GET, 0);
+	misa = machine_command(ECALL_MCPUID_GET, 0);
 
-	/* SMPTODO: use mhartid ? */
 	cpu = PCPU_GET(cpuid);
 
 	impl_id	= CPU_IMPL(mimpid);
@@ -125,12 +112,12 @@ identify_cpu(void)
 		    cpu_implementers[i].impl_id == 0) {
 			cpu_desc[cpu].cpu_impl = impl_id;
 			cpu_desc[cpu].cpu_impl_name = cpu_implementers[i].impl_name;
-			cpu_partsp = cpu_implementers[i].cpu_parts;
+			cpu_partsp = cpu_parts_std;
 			break;
 		}
 	}
 
-	part_id = CPU_PART(mcpuid);
+	part_id = CPU_PART(misa);
 	for (i = 0; &cpu_partsp[i] != NULL; i++) {
 		if (part_id == cpu_partsp[i].part_id ||
 		    cpu_partsp[i].part_id == -1) {
diff --git a/sys/riscv/riscv/intr_machdep.c b/sys/riscv/riscv/intr_machdep.c
index ad30bf2f991f..3dd988f712b3 100644
--- a/sys/riscv/riscv/intr_machdep.c
+++ b/sys/riscv/riscv/intr_machdep.c
@@ -95,15 +95,20 @@ riscv_mask_irq(void *source)
 	irq = (uintptr_t)source;
 
 	switch (irq) {
-	case IRQ_TIMER:
+	case IRQ_TIMER_SUPERVISOR:
 		csr_clear(sie, SIE_STIE);
 		break;
-	case IRQ_SOFTWARE:
+	case IRQ_SOFTWARE_USER:
+		csr_clear(sie, SIE_USIE);
+	case IRQ_SOFTWARE_SUPERVISOR:
 		csr_clear(sie, SIE_SSIE);
 		break;
+#if 0
+	/* lowRISC TODO */
 	case IRQ_UART:
 		machine_command(ECALL_IO_IRQ_MASK, 0);
 		break;
+#endif
 	default:
 		panic("Unknown irq %d\n", irq);
 	}
@@ -117,15 +122,21 @@ riscv_unmask_irq(void *source)
 	irq = (uintptr_t)source;
 
 	switch (irq) {
-	case IRQ_TIMER:
+	case IRQ_TIMER_SUPERVISOR:
 		csr_set(sie, SIE_STIE);
 		break;
-	case IRQ_SOFTWARE:
+	case IRQ_SOFTWARE_USER:
+		csr_set(sie, SIE_USIE);
+		break;
+	case IRQ_SOFTWARE_SUPERVISOR:
 		csr_set(sie, SIE_SSIE);
 		break;
+#if 0
+	/* lowRISC TODO */
 	case IRQ_UART:
 		machine_command(ECALL_IO_IRQ_MASK, 1);
 		break;
+#endif
 	default:
 		panic("Unknown irq %d\n", irq);
 	}
@@ -209,18 +220,18 @@ riscv_cpu_intr(struct trapframe *frame)
 	active_irq = (frame->tf_scause & EXCP_MASK);
 
 	switch (active_irq) {
+#if 0
+	/* lowRISC TODO */
 	case IRQ_UART:
-	case IRQ_SOFTWARE:
-	case IRQ_TIMER:
+#endif
+	case IRQ_SOFTWARE_USER:
+	case IRQ_SOFTWARE_SUPERVISOR:
+	case IRQ_TIMER_SUPERVISOR:
 		event = intr_events[active_irq];
 		/* Update counters */
 		atomic_add_long(riscv_intr_counters[active_irq], 1);
 		PCPU_INC(cnt.v_intr);
 		break;
-	case IRQ_HTIF:
-		/* HTIF interrupts are only handled in machine mode */
-		panic("%s: HTIF interrupt", __func__);
-		break;
 	default:
 		event = NULL;
 	}
@@ -237,7 +248,7 @@ void
 riscv_setup_ipihandler(driver_filter_t *filt)
 {
 
-	riscv_setup_intr("ipi", filt, NULL, NULL, IRQ_SOFTWARE,
+	riscv_setup_intr("ipi", filt, NULL, NULL, IRQ_SOFTWARE_SUPERVISOR,
 	    INTR_TYPE_MISC, NULL);
 }
 
diff --git a/sys/riscv/riscv/locore.S b/sys/riscv/riscv/locore.S
index 9df5c2da258c..1783fb908171 100644
--- a/sys/riscv/riscv/locore.S
+++ b/sys/riscv/riscv/locore.S
@@ -49,7 +49,7 @@
 #define	HW_STACK_SIZE		(96)
 
 /*
- * Event queue for each CPU core:
+ * Event queue:
  *
  * struct htif_ring {
  *     uint64_t data;
@@ -62,12 +62,6 @@
 
 .macro build_ring
 	la	t0, htif_ring
-#ifdef SMP
-	csrr	a0, mhartid
-	li	s0, (HTIF_RING_SIZE + 16)
-	mulw	s0, a0, s0
-	add	t0, t0, s0
-#endif
 	li	t1, 0
 	sd	t1, 0(t0)	/* zero data */
 	sd	t1, 8(t0)	/* zero used */
@@ -101,31 +95,48 @@
 	.text
 
 mentry:
-	/* User mode entry point (mtvec + 0x000) */
-	.align 6
-	j	user_trap
-
-	/* Supervisor mode entry point (mtvec + 0x040) */
-	.align 6
-	j	supervisor_trap
-
-	/* Hypervisor mode entry point (mtvec + 0x080) */
-	.align 6
-	j	bad_trap
-
-	/* Machine mode entry point (mtvec + 0x0C0) */
-	.align 6
-	j	bad_trap
+	/* Vectors */
+	j	_start		/* reset */
+	j	bad_trap	/* NMI (non-maskable interrupt) */
+	j	machine_trap
 
 	/* Reset vector */
 	.text
-	.align 8
 	.globl _start
 _start:
+	/* Setup machine trap vector */
+	la	t0, machine_trap
+	csrw	mtvec, t0
+
+	/* Delegate interrupts to supervisor mode */
+	li	t0, (MIP_SSIP | MIP_STIP | MIP_SEIP)
+	csrw	mideleg, t0
+
+	/* Delegate exceptions to supervisor mode */
+	li	t0,	(1 << EXCP_MISALIGNED_FETCH)	| \
+			(1 << EXCP_FAULT_FETCH)		| \
+			(1 << EXCP_ILLEGAL_INSTRUCTION)	| \
+			(1 << EXCP_FAULT_LOAD)		| \
+			(1 << EXCP_FAULT_STORE)		| \
+			(1 << EXCP_BREAKPOINT)		| \
+			(1 << EXCP_USER_ECALL)
+	csrw	medeleg, t0
+
+	la	t0, cpu_exception_handler
+	li	t1, KERNBASE
+	add	t0, t0, t1
+	csrw	stvec, t0
+
 	/* Direct secondary cores to mpentry */
 	csrr	a0, mhartid
 	bnez	a0, mpentry
 
+	li	t1, 0
+	la	t0, tohost
+	sd	t1, 0(t0)
+	la	t0, fromhost
+	sd	t1, 0(t0)
+
 	/* Build event queue for current core */
 	build_ring
 
@@ -149,7 +160,7 @@ _start:
 	li	a5, (VM_MAX_KERNEL_ADDRESS - L2_SIZE)
 	srli	a5, a5, L1_SHIFT	/* >> L1_SHIFT */
 	andi	a5, a5, 0x1ff		/* & 0x1ff */
-	li	t4, (PTE_VALID | (PTE_TYPE_PTR << PTE_TYPE_S))
+	li	t4, PTE_V
 	slli	t5, s2, PTE_PPN0_S	/* (s2 << PTE_PPN0_S) */
 	or	t6, t4, t5
 
@@ -164,10 +175,10 @@ _start:
 	la	s2, pagetable_l2	/* Link to next level PN */
 	srli	s2, s2, PAGE_SHIFT
 
-	li	a5, KERNBASE
+	li	a5, (KERNBASE + KERNENTRY)
 	srli	a5, a5, L1_SHIFT	/* >> L1_SHIFT */
 	andi	a5, a5, 0x1ff		/* & 0x1ff */
-	li	t4, (PTE_VALID | (PTE_TYPE_PTR << PTE_TYPE_S))
+	li	t4, PTE_V
 	slli	t5, s2, PTE_PPN0_S	/* (s2 << PTE_PPN0_S) */
 	or	t6, t4, t5
 
@@ -179,11 +190,13 @@ _start:
 
 	/* Level 2 superpages (512 x 2MiB) */
 	la	s1, pagetable_l2
-	li	t3, 512			/* Build 512 entries */
-	li	t4, 0			/* Counter */
+	li	t4, KERNENTRY
+	srli	t4, t4, 21		/* Div by 2 MiB */
+	li	t2, 512			/* Build 512 entries */
+	add	t3, t4, t2
 	li	t5, 0
 2:
-	li	t0, (PTE_VALID | (PTE_TYPE_SRWX << PTE_TYPE_S))
+	li	t0, (PTE_V | PTE_RWX)
 	slli	t2, t4, PTE_PPN1_S	/* << PTE_PPN1_S */
 	or	t5, t0, t2
 	sd	t5, (s1)		/* Store PTE entry to position */
@@ -194,15 +207,14 @@ _start:
 
 	/* Set page tables base register */
 	la	s1, pagetable_l1
+	srli	s1, s1, PAGE_SHIFT
 	csrw	sptbr, s1
 
 	/* Page tables END */
 
 	/* Enter supervisor mode */
 	li	s0, ((MSTATUS_VM_SV39 << MSTATUS_VM_SHIFT) | \
-		     (MSTATUS_PRV_M << MSTATUS_PRV_SHIFT) | \
-		     (MSTATUS_PRV_S << MSTATUS_PRV1_SHIFT) | \
-		     (MSTATUS_PRV_U << MSTATUS_PRV2_SHIFT));
+		     (MSTATUS_PRV_S << MSTATUS_MPP_SHIFT));
 	csrw	mstatus, s0
 
 	/*
@@ -217,7 +229,7 @@ _start:
 	li	s11, KERNBASE
 	add	t0, t0, s11
 	csrw	mepc, t0
-	eret
+	mret
 
 .Lmmu_on:
 	/* Initialize stack pointer */
@@ -254,10 +266,12 @@ hardstack_end:
 
 	.globl htif_ring
 htif_ring:
-	.space ((HTIF_RING_SIZE + 16) * MAXCPU)
-
-	.globl console_intr
-console_intr:
+	.space (HTIF_RING_SIZE + 16)
+htif_lock:
+	.space (8)
+tohost:
+	.space (8)
+fromhost:
 	.space (8)
 
 ENTRY(sigcode)
@@ -325,18 +339,14 @@ ENTRY(mpentry)
 	lw	t1, 0(t0)
 	beqz	t1, 1b
 
-	/* Build event queue ring for this core */
-	build_ring
-
 	/* Set page tables base register */
 	la	t0, pagetable_l1
+	srli	t0, t0, PAGE_SHIFT
 	csrw	sptbr, t0
 
 	/* Configure mstatus */
 	li	s0, ((MSTATUS_VM_SV39 << MSTATUS_VM_SHIFT) | \
-		     (MSTATUS_PRV_M << MSTATUS_PRV_SHIFT) | \
-		     (MSTATUS_PRV_S << MSTATUS_PRV1_SHIFT) | \
-		     (MSTATUS_PRV_U << MSTATUS_PRV2_SHIFT));
+		     (MSTATUS_PRV_S << MSTATUS_MPP_SHIFT));
 	csrw	mstatus, s0
 
 	/* Setup stack for machine mode exceptions */
@@ -364,7 +374,7 @@ ENTRY(mpentry)
 	li	s11, KERNBASE
 	add	t0, t0, s11
 	csrw	mepc, t0
-	eret
+	mret
 
 mp_virtdone:
 	/* We are now in virtual address space */
diff --git a/sys/riscv/riscv/machdep.c b/sys/riscv/riscv/machdep.c
index fafa2f439788..fb969f1e7702 100644
--- a/sys/riscv/riscv/machdep.c
+++ b/sys/riscv/riscv/machdep.c
@@ -440,10 +440,10 @@ sys_sigreturn(struct thread *td, struct sigreturn_args *uap)
 	/*
 	 * Make sure the processor mode has not been tampered with and
 	 * interrupts have not been disabled.
+	 * Supervisor interrupts in user mode are always enabled.
 	 */
 	sstatus = uc.uc_mcontext.mc_gpregs.gp_sstatus;
-	if ((sstatus & SSTATUS_PS) != 0 ||
-	    (sstatus & SSTATUS_PIE) == 0)
+	if ((sstatus & SSTATUS_SPP) != 0)
 		return (EINVAL);
 
 	error = set_mcontext(td, &uc.uc_mcontext);
@@ -743,7 +743,8 @@ initriscv(struct riscv_bootparams *rvbp)
 	if (kmdp == NULL)
 		kmdp = preload_search_by_type("elf64 kernel");
 
-	boothowto = 0;
+	boothowto = RB_VERBOSE | RB_SINGLE;
+	boothowto = RB_VERBOSE;
 
 	kern_envp = NULL;
 
@@ -775,7 +776,7 @@ initriscv(struct riscv_bootparams *rvbp)
 
 	cache_setup();
 
-	/* Bootstrap enough of pmap  to enter the kernel proper */
+	/* Bootstrap enough of pmap to enter the kernel proper */
 	kernlen = (lastaddr - KERNBASE);
 	pmap_bootstrap(rvbp->kern_l1pt, KERNENTRY, kernlen);
 
diff --git a/sys/riscv/riscv/pmap.c b/sys/riscv/riscv/pmap.c
index a6c899b0776e..73bc5c89687d 100644
--- a/sys/riscv/riscv/pmap.c
+++ b/sys/riscv/riscv/pmap.c
@@ -311,9 +311,9 @@ pmap_l2(pmap_t pmap, vm_offset_t va)
 	l1 = pmap_l1(pmap, va);
 	if (l1 == NULL)
 		return (NULL);
-	if ((pmap_load(l1) & PTE_VALID) == 0)
+	if ((pmap_load(l1) & PTE_V) == 0)
 		return (NULL);
-	if ((pmap_load(l1) & PTE_TYPE_M) != (PTE_TYPE_PTR << PTE_TYPE_S))
+	if ((pmap_load(l1) & PTE_RX) != 0)
 		return (NULL);
 
 	return (pmap_l1_to_l2(l1, va));
@@ -339,9 +339,9 @@ pmap_l3(pmap_t pmap, vm_offset_t va)
 	l2 = pmap_l2(pmap, va);
 	if (l2 == NULL)
 		return (NULL);
-	if ((pmap_load(l2) & PTE_VALID) == 0)
+	if ((pmap_load(l2) & PTE_V) == 0)
 		return (NULL);
-	if ((pmap_load(l2) & PTE_TYPE_M) != (PTE_TYPE_PTR << PTE_TYPE_S))
+	if ((pmap_load(l2) & PTE_RX) != 0)
 		return (NULL);
 
 	return (pmap_l2_to_l3(l2, va));
@@ -352,10 +352,7 @@ static __inline int
 pmap_is_write(pt_entry_t entry)
 {
 
-	if (entry & (1 << PTE_TYPE_S))
-		return (1);
-
-	return (0);
+	return (entry & PTE_W);
 }
 
 static __inline int
@@ -370,7 +367,7 @@ static __inline int
 pmap_l3_valid(pt_entry_t l3)
 {
 
-	return (l3 & PTE_VALID);
+	return (l3 & PTE_V);
 }
 
 static __inline int
@@ -389,7 +386,7 @@ static inline int
 pmap_page_dirty(pt_entry_t pte)
 {
 
-	return (pte & PTE_DIRTY);
+	return (pte & PTE_D);
 }
 
 static __inline void
@@ -442,7 +439,7 @@ pmap_early_page_idx(vm_offset_t l1pt, vm_offset_t va, u_int *l1_slot,
 	*l1_slot = (va >> L1_SHIFT) & Ln_ADDR_MASK;
 
 	/* Check locore has used a table L1 map */
-	KASSERT((l1[*l1_slot] & PTE_TYPE_M) == (PTE_TYPE_PTR << PTE_TYPE_S),
+	KASSERT((l1[*l1_slot] & PTE_RX) == 0,
 		("Invalid bootstrap L1 table"));
 
 	/* Find the address of the L2 table */
@@ -478,6 +475,13 @@ pmap_bootstrap_dmap(vm_offset_t l1pt, vm_paddr_t kernstart)
 	pt_entry_t entry;
 	pn_t pn;
 
+	/*
+	 * Initialize DMAP starting from zero physical address.
+	 * TODO: remove this once machine-mode code splitted out.
+	 */
+	kernstart = 0;
+	printf("%s: l1pt 0x%016lx kernstart 0x%016lx\n", __func__, l1pt, kernstart);
+
 	pa = kernstart & ~L1_OFFSET;
 	va = DMAP_MIN_ADDRESS;
 	l1 = (pd_entry_t *)l1pt;
@@ -489,7 +493,7 @@ pmap_bootstrap_dmap(vm_offset_t l1pt, vm_paddr_t kernstart)
 
 		/* superpages */
 		pn = (pa / PAGE_SIZE);
-		entry = (PTE_VALID | (PTE_TYPE_SRWX << PTE_TYPE_S));
+		entry = (PTE_V | PTE_RWX);
 		entry |= (pn << PTE_PPN0_S);
 		pmap_load_store(&l1[l1_slot], entry);
 	}
@@ -521,19 +525,20 @@ pmap_bootstrap_l3(vm_offset_t l1pt, vm_offset_t va, vm_offset_t l3_start)
 
 		pa = pmap_early_vtophys(l1pt, l3pt);
 		pn = (pa / PAGE_SIZE);
-		entry = (PTE_VALID | (PTE_TYPE_PTR << PTE_TYPE_S));
+		entry = (PTE_V);
 		entry |= (pn << PTE_PPN0_S);
 		pmap_load_store(&l2[l2_slot], entry);
 		l3pt += PAGE_SIZE;
 	}
 
+
 	/* Clean the L2 page table */
 	memset((void *)l3_start, 0, l3pt - l3_start);
 	cpu_dcache_wb_range(l3_start, l3pt - l3_start);
 
 	cpu_dcache_wb_range((vm_offset_t)l2, PAGE_SIZE);
 
-	return l3pt;
+	return (l3pt);
 }
 
 /*
@@ -598,6 +603,10 @@ pmap_bootstrap(vm_offset_t l1pt, vm_paddr_t kernstart, vm_size_t kernlen)
 		if (physmap[map_slot] == physmap[map_slot + 1])
 			continue;
 
+		if (physmap[map_slot] <= pa &&
+		    physmap[map_slot + 1] > pa)
+			break;
+
 		phys_avail[avail_slot] = physmap[map_slot];
 		phys_avail[avail_slot + 1] = physmap[map_slot + 1];
 		physmem += (phys_avail[avail_slot + 1] -
@@ -627,11 +636,11 @@ pmap_bootstrap(vm_offset_t l1pt, vm_paddr_t kernstart, vm_size_t kernlen)
 
 	/* Find how many pages we have mapped */
 	for (; l2_slot < Ln_ENTRIES; l2_slot++) {
-		if ((l2[l2_slot] & PTE_VALID) == 0)
+		if ((l2[l2_slot] & PTE_V) == 0)
 			break;
 
 		/* Check locore used L2 superpages */
-		KASSERT((l2[l2_slot] & PTE_TYPE_M) != (PTE_TYPE_PTR << PTE_TYPE_S),
+		KASSERT((l2[l2_slot] & PTE_RX) != 0,
 		    ("Invalid bootstrap L2 table"));
 
 		va += L2_SIZE;
@@ -672,12 +681,14 @@ pmap_bootstrap(vm_offset_t l1pt, vm_paddr_t kernstart, vm_size_t kernlen)
 	map_slot = used_map_slot;
 	for (; avail_slot < (PHYS_AVAIL_SIZE - 2) &&
 	    map_slot < (physmap_idx * 2); map_slot += 2) {
-		if (physmap[map_slot] == physmap[map_slot + 1])
+		if (physmap[map_slot] == physmap[map_slot + 1]) {
 			continue;
+		}
 
 		/* Have we used the current range? */
-		if (physmap[map_slot + 1] <= pa)
+		if (physmap[map_slot + 1] <= pa) {
 			continue;
+		}
 
 		/* Do we need to split the entry? */
 		if (physmap[map_slot] < pa) {
@@ -797,7 +808,7 @@ pmap_extract(pmap_t pmap, vm_offset_t va)
 	l2p = pmap_l2(pmap, va);
 	if (l2p != NULL) {
 		l2 = pmap_load(l2p);
-		if ((l2 & PTE_TYPE_M) == (PTE_TYPE_PTR << PTE_TYPE_S)) {
+		if ((l2 & PTE_RX) == 0) {
 			l3p = pmap_l2_to_l3(l2p, va);
 			if (l3p != NULL) {
 				l3 = pmap_load(l3p);
@@ -861,7 +872,7 @@ pmap_kextract(vm_offset_t va)
 		l2 = pmap_l2(kernel_pmap, va);
 		if (l2 == NULL)
 			panic("pmap_kextract: No l2");
-		if ((pmap_load(l2) & PTE_TYPE_M) != (PTE_TYPE_PTR << PTE_TYPE_S)) {
+		if ((pmap_load(l2) & PTE_RX) != 0) {
 			/* superpages */
 			pa = (pmap_load(l2) >> PTE_PPN1_S) << L2_SHIFT;
 			pa |= (va & L2_OFFSET);
@@ -902,7 +913,7 @@ pmap_kenter_device(vm_offset_t sva, vm_size_t size, vm_paddr_t pa)
 		KASSERT(l3 != NULL, ("Invalid page table, va: 0x%lx", va));
 
 		pn = (pa / PAGE_SIZE);
-		entry = (PTE_VALID | (PTE_TYPE_SRWX << PTE_TYPE_S));
+		entry = (PTE_V | PTE_RWX);
 		entry |= (pn << PTE_PPN0_S);
 		pmap_load_store(l3, entry);
 
@@ -1004,7 +1015,7 @@ pmap_qenter(vm_offset_t sva, vm_page_t *ma, int count)
 		pn = (pa / PAGE_SIZE);
 		l3 = pmap_l3(kernel_pmap, va);
 
-		entry = (PTE_VALID | (PTE_TYPE_SRWX << PTE_TYPE_S));
+		entry = (PTE_V | PTE_RWX);
 		entry |= (pn << PTE_PPN0_S);
 		pmap_load_store(l3, entry);
 
@@ -1261,7 +1272,7 @@ _pmap_alloc_l3(pmap_t pmap, vm_pindex_t ptepindex, struct rwlock **lockp)
 		l1 = &pmap->pm_l1[l1index];
 
 		pn = (VM_PAGE_TO_PHYS(m) / PAGE_SIZE);
-		entry = (PTE_VALID | (PTE_TYPE_PTR << PTE_TYPE_S));
+		entry = (PTE_V);
 		entry |= (pn << PTE_PPN0_S);
 		pmap_load_store(l1, entry);
 		pmap_distribute_l1(pmap, l1index, entry);
@@ -1294,7 +1305,7 @@ _pmap_alloc_l3(pmap_t pmap, vm_pindex_t ptepindex, struct rwlock **lockp)
 		l2 = &l2[ptepindex & Ln_ADDR_MASK];
 
 		pn = (VM_PAGE_TO_PHYS(m) / PAGE_SIZE);
-		entry = (PTE_VALID | (PTE_TYPE_PTR << PTE_TYPE_S));
+		entry = (PTE_V);
 		entry |= (pn << PTE_PPN0_S);
 		pmap_load_store(l2, entry);
 
@@ -1428,7 +1439,7 @@ pmap_growkernel(vm_offset_t addr)
 			paddr = VM_PAGE_TO_PHYS(nkpg);
 
 			pn = (paddr / PAGE_SIZE);
-			entry = (PTE_VALID | (PTE_TYPE_PTR << PTE_TYPE_S));
+			entry = (PTE_V);
 			entry |= (pn << PTE_PPN0_S);
 			pmap_load_store(l1, entry);
 			pmap_distribute_l1(kernel_pmap,
@@ -1438,7 +1449,7 @@ pmap_growkernel(vm_offset_t addr)
 			continue; /* try again */
 		}
 		l2 = pmap_l1_to_l2(l1, kernel_vm_end);
-		if ((pmap_load(l2) & PTE_REF) != 0) {
+		if ((pmap_load(l2) & PTE_A) != 0) {
 			kernel_vm_end = (kernel_vm_end + L2_SIZE) & ~L2_OFFSET;
 			if (kernel_vm_end - 1 >= kernel_map->max_offset) {
 				kernel_vm_end = kernel_map->max_offset;
@@ -1452,12 +1463,13 @@ pmap_growkernel(vm_offset_t addr)
 		    VM_ALLOC_ZERO);
 		if (nkpg == NULL)
 			panic("pmap_growkernel: no memory to grow kernel");
-		if ((nkpg->flags & PG_ZERO) == 0)
+		if ((nkpg->flags & PG_ZERO) == 0) {
 			pmap_zero_page(nkpg);
+		}
 		paddr = VM_PAGE_TO_PHYS(nkpg);
 
 		pn = (paddr / PAGE_SIZE);
-		entry = (PTE_VALID | (PTE_TYPE_PTR << PTE_TYPE_S));
+		entry = (PTE_V);
 		entry |= (pn << PTE_PPN0_S);
 		pmap_load_store(l2, entry);
 
@@ -1754,7 +1766,7 @@ pmap_remove_l3(pmap_t pmap, pt_entry_t *l3, vm_offset_t va,
 		m = PHYS_TO_VM_PAGE(phys);
 		if (pmap_page_dirty(old_l3))
 			vm_page_dirty(m);
-		if (old_l3 & PTE_REF)
+		if (old_l3 & PTE_A)
 			vm_page_aflag_set(m, PGA_REFERENCED);
 		CHANGE_PV_LIST_LOCK_TO_VM_PAGE(lockp, m);
 		pmap_pvh_free(&m->md, pmap, va);
@@ -1822,7 +1834,7 @@ pmap_remove(pmap_t pmap, vm_offset_t sva, vm_offset_t eva)
 		 */
 		if (l3_pte == 0)
 			continue;
-		if ((pmap_load(l2) & PTE_TYPE_M) != (PTE_TYPE_PTR << PTE_TYPE_S))
+		if ((pmap_load(l2) & PTE_RX) != 0)
 			continue;
 
 		/*
@@ -1899,7 +1911,7 @@ pmap_remove_all(vm_page_t m)
 		KASSERT(l2 != NULL, ("pmap_remove_all: no l2 table found"));
 		tl2 = pmap_load(l2);
 
-		KASSERT((tl2 & PTE_TYPE_M) == (PTE_TYPE_PTR << PTE_TYPE_S),
+		KASSERT((tl2 & PTE_RX) == 0,
 		    ("pmap_remove_all: found a table when expecting "
 		    "a block in %p's pv list", m));
 
@@ -1912,7 +1924,7 @@ pmap_remove_all(vm_page_t m)
 		pmap_invalidate_page(pmap, pv->pv_va);
 		if (tl3 & PTE_SW_WIRED)
 			pmap->pm_stats.wired_count--;
-		if ((tl3 & PTE_REF) != 0)
+		if ((tl3 & PTE_A) != 0)
 			vm_page_aflag_set(m, PGA_REFERENCED);
 
 		/*
@@ -1969,7 +1981,7 @@ pmap_protect(pmap_t pmap, vm_offset_t sva, vm_offset_t eva, vm_prot_t prot)
 		l2 = pmap_l1_to_l2(l1, sva);
 		if (l2 == NULL)
 			continue;
-		if ((pmap_load(l2) & PTE_TYPE_M) != (PTE_TYPE_PTR << PTE_TYPE_S))
+		if ((pmap_load(l2) & PTE_RX) != 0)
 			continue;
 
 		if (va_next > eva)
@@ -1981,7 +1993,7 @@ pmap_protect(pmap_t pmap, vm_offset_t sva, vm_offset_t eva, vm_prot_t prot)
 			l3 = pmap_load(l3p);
 			if (pmap_l3_valid(l3)) {
 				entry = pmap_load(l3p);
-				entry &= ~(1 << PTE_TYPE_S);
+				entry &= ~(PTE_W);
 				pmap_load_store(l3p, entry);
 				PTE_SYNC(l3p);
 				/* XXX: Use pmap_invalidate_range */
@@ -2030,19 +2042,11 @@ pmap_enter(pmap_t pmap, vm_offset_t va, vm_page_t m, vm_prot_t prot,
 	pa = VM_PAGE_TO_PHYS(m);
 	pn = (pa / PAGE_SIZE);
 
-	new_l3 = PTE_VALID;
-
-	if ((prot & VM_PROT_WRITE) == 0) { /* Read-only */
-		if ((va >> 63) == 0) /* USER */
-			new_l3 |= (PTE_TYPE_SURX << PTE_TYPE_S);
-		else /* KERNEL */
-			new_l3 |= (PTE_TYPE_SRX << PTE_TYPE_S);
-	} else {
-		if ((va >> 63) == 0) /* USER */
-			new_l3 |= (PTE_TYPE_SURWX << PTE_TYPE_S);
-		else /* KERNEL */
-			new_l3 |= (PTE_TYPE_SRWX << PTE_TYPE_S);
-	}
+	new_l3 = PTE_V | PTE_R | PTE_X;
+	if (prot & VM_PROT_WRITE)
+		new_l3 |= PTE_W;
+	if ((va >> 63) == 0)
+		new_l3 |= PTE_U;
 
 	new_l3 |= (pn << PTE_PPN0_S);
 	if ((flags & PMAP_ENTER_WIRED) != 0)
@@ -2086,7 +2090,7 @@ pmap_enter(pmap_t pmap, vm_offset_t va, vm_page_t m, vm_prot_t prot,
 				l2_pn = (l2_pa / PAGE_SIZE);
 
 				l1 = pmap_l1(pmap, va);
-				entry = (PTE_VALID | (PTE_TYPE_PTR << PTE_TYPE_S));
+				entry = (PTE_V);
 				entry |= (l2_pn << PTE_PPN0_S);
 				pmap_load_store(l1, entry);
 				pmap_distribute_l1(pmap, pmap_l1_index(va), entry);
@@ -2107,7 +2111,7 @@ pmap_enter(pmap_t pmap, vm_offset_t va, vm_page_t m, vm_prot_t prot,
 
 			l3_pa = VM_PAGE_TO_PHYS(l3_m);
 			l3_pn = (l3_pa / PAGE_SIZE);
-			entry = (PTE_VALID | (PTE_TYPE_PTR << PTE_TYPE_S));
+			entry = (PTE_V);
 			entry |= (l3_pn << PTE_PPN0_S);
 			pmap_load_store(l2, entry);
 			PTE_SYNC(l2);
@@ -2201,7 +2205,7 @@ pmap_enter(pmap_t pmap, vm_offset_t va, vm_page_t m, vm_prot_t prot,
 				om = PHYS_TO_VM_PAGE(opa);
 				if (pmap_page_dirty(orig_l3))
 					vm_page_dirty(om);
-				if ((orig_l3 & PTE_REF) != 0)
+				if ((orig_l3 & PTE_A) != 0)
 					vm_page_aflag_set(om, PGA_REFERENCED);
 				CHANGE_PV_LIST_LOCK_TO_PHYS(&lock, opa);
 				pmap_pvh_free(&om->md, pmap, va);
@@ -2388,7 +2392,7 @@ pmap_enter_quick_locked(pmap_t pmap, vm_offset_t va, vm_page_t m,
 	pn = (pa / PAGE_SIZE);
 
 	/* RISCVTODO: check permissions */
-	entry = (PTE_VALID | (PTE_TYPE_SRWX << PTE_TYPE_S));
+	entry = (PTE_V | PTE_RWX);
 	entry |= (pn << PTE_PPN0_S);
 
 	/*
@@ -2831,12 +2835,12 @@ pmap_page_test_mappings(vm_page_t m, boolean_t accessed, boolean_t modified)
 		mask = 0;
 		value = 0;
 		if (modified) {
-			mask |= PTE_DIRTY;
-			value |= PTE_DIRTY;
+			mask |= PTE_D;
+			value |= PTE_D;
 		}
 		if (accessed) {
-			mask |= PTE_REF;
-			value |= PTE_REF;
+			mask |= PTE_A;
+			value |= PTE_A;
 		}
 
 #if 0
@@ -2968,11 +2972,11 @@ pmap_remove_write(vm_page_t m)
 		oldl3 = pmap_load(l3);
 
 		if (pmap_is_write(oldl3)) {
-			newl3 = oldl3 & ~(1 << PTE_TYPE_S);
+			newl3 = oldl3 & ~(PTE_W);
 			if (!atomic_cmpset_long(l3, oldl3, newl3))
 				goto retry;
 			/* TODO: use pmap_page_dirty(oldl3) ? */
-			if ((oldl3 & PTE_REF) != 0)
+			if ((oldl3 & PTE_A) != 0)
 				vm_page_dirty(m);
 			pmap_invalidate_page(pmap, pv->pv_va);
 		}
@@ -3045,11 +3049,11 @@ pmap_ts_referenced(vm_page_t m)
 		}
 		l2 = pmap_l2(pmap, pv->pv_va);
 
-		KASSERT((pmap_load(l2) & PTE_TYPE_M) == (PTE_TYPE_PTR << PTE_TYPE_S),
+		KASSERT((pmap_load(l2) & PTE_RX) == 0,
 		    ("pmap_ts_referenced: found an invalid l2 table"));
 
 		l3 = pmap_l2_to_l3(l2, pv->pv_va);
-		if ((pmap_load(l3) & PTE_REF) != 0) {
+		if ((pmap_load(l3) & PTE_A) != 0) {
 			if (safe_to_clear_referenced(pmap, pmap_load(l3))) {
 				/*
 				 * TODO: We don't handle the access flag
@@ -3178,7 +3182,7 @@ pmap_activate(struct thread *td)
 	pmap = vmspace_pmap(td->td_proc->p_vmspace);
 	td->td_pcb->pcb_l1addr = vtophys(pmap->pm_l1);
 
-	__asm __volatile("csrw sptbr, %0" :: "r"(td->td_pcb->pcb_l1addr));
+	__asm __volatile("csrw sptbr, %0" :: "r"(td->td_pcb->pcb_l1addr >> PAGE_SHIFT));
 
 	pmap_invalidate_all(pmap);
 	critical_exit();
diff --git a/sys/riscv/riscv/swtch.S b/sys/riscv/riscv/swtch.S
index 90461d8d0e7a..b5cced30d8f7 100644
--- a/sys/riscv/riscv/swtch.S
+++ b/sys/riscv/riscv/swtch.S
@@ -56,6 +56,7 @@ ENTRY(cpu_throw)
 
 	/* Switch to the new pmap */
 	ld	t0, PCB_L1ADDR(x13)
+	srli	t0, t0, PAGE_SHIFT
 	csrw	sptbr, t0
 
 	/* TODO: Invalidate the TLB */
@@ -79,6 +80,7 @@ ENTRY(cpu_throw)
 	ld	s9, (PCB_S + 9 * 8)(x13)
 	ld	s10, (PCB_S + 10 * 8)(x13)
 	ld	s11, (PCB_S + 11 * 8)(x13)
+
 	ret
 
 .Lcpu_throw_panic_str:
@@ -135,6 +137,7 @@ ENTRY(cpu_switch)
 
 	/* Switch to the new pmap */
 	ld	t0, PCB_L1ADDR(x13)
+	srli	t0, t0, PAGE_SHIFT
 	csrw	sptbr, t0
 
 	/* TODO: Invalidate the TLB */
@@ -187,7 +190,7 @@ ENTRY(fork_trampoline)
 	/* Restore sstatus */
 	ld	t0, (TF_SSTATUS)(sp)
 	/* Ensure interrupts disabled */
-	li	t1, ~SSTATUS_IE
+	li	t1, ~SSTATUS_SIE
 	and	t0, t0, t1
 	csrw	sstatus, t0
 
@@ -244,7 +247,7 @@ ENTRY(fork_trampoline)
 	/* Load user stack */
 	ld	sp, (TF_SP - TF_SIZE)(sp)
 
-	eret
+	sret
 END(fork_trampoline)
 
 ENTRY(savectx)
diff --git a/sys/riscv/riscv/timer.c b/sys/riscv/riscv/timer.c
index fbef1a142d44..ae8ec67b6e90 100644
--- a/sys/riscv/riscv/timer.c
+++ b/sys/riscv/riscv/timer.c
@@ -1,5 +1,5 @@
 /*-
- * Copyright (c) 2015 Ruslan Bukin <br@bsdpad.com>
+ * Copyright (c) 2015-2016 Ruslan Bukin <br@bsdpad.com>
  * All rights reserved.
  *
  * Portions of this software were developed by SRI International and the
@@ -67,9 +67,19 @@ __FBSDID("$FreeBSD$");
 
 #define	DEFAULT_FREQ	1000000
 
+#define	TIMER_COUNTS		0x00
+#define	TIMER_MTIMECMP(cpu)	(0x08 + (cpu * 8))
+
+#define	READ8(_sc, _reg)        \
+	bus_space_read_8(_sc->bst, _sc->bsh, _reg)
+#define	WRITE8(_sc, _reg, _val) \
+	bus_space_write_8(_sc->bst, _sc->bsh, _reg, _val)
+
 struct riscv_tmr_softc {
-	struct resource		*res[1];
-	void			*ihl[1];
+	struct resource		*res[2];
+	bus_space_tag_t		bst;
+	bus_space_handle_t	bsh;
+	void			*ih;
 	uint32_t		clkfreq;
 	struct eventtimer	et;
 };
@@ -77,6 +87,7 @@ struct riscv_tmr_softc {
 static struct riscv_tmr_softc *riscv_tmr_sc = NULL;
 
 static struct resource_spec timer_spec[] = {
+	{ SYS_RES_MEMORY,	0,	RF_ACTIVE },
 	{ SYS_RES_IRQ,		0,	RF_ACTIVE },
 	{ -1, 0 }
 };
@@ -93,30 +104,39 @@ static struct timecounter riscv_tmr_timecount = {
 };
 
 static long
-get_counts(void)
+get_counts(struct riscv_tmr_softc *sc)
 {
 
-	return (csr_read(stime));
+	return (READ8(sc, TIMER_COUNTS));
 }
 
 static unsigned
 riscv_tmr_get_timecount(struct timecounter *tc)
 {
+	struct riscv_tmr_softc *sc;
 
-	return (get_counts());
+	sc = tc->tc_priv;
+
+	return (get_counts(sc));
 }
 
 static int
 riscv_tmr_start(struct eventtimer *et, sbintime_t first, sbintime_t period)
 {
 	struct riscv_tmr_softc *sc;
-	int counts;
+	uint64_t counts;
+	int cpu;
 
 	sc = (struct riscv_tmr_softc *)et->et_priv;
 
 	if (first != 0) {
 		counts = ((uint32_t)et->et_frequency * first) >> 32;
+		counts += READ8(sc, TIMER_COUNTS);
+		cpu = PCPU_GET(cpuid);
+		WRITE8(sc, TIMER_MTIMECMP(cpu), counts);
+		csr_set(sie, SIE_STIE);
 		machine_command(ECALL_MTIMECMP, counts);
+
 		return (0);
 	}
 
@@ -143,13 +163,7 @@ riscv_tmr_intr(void *arg)
 
 	sc = (struct riscv_tmr_softc *)arg;
 
-	/*
-	 * Clear interrupt pending bit.
-	 * Note: SIP_STIP bit is not implemented in sip register
-	 * in Spike simulator, so use machine command to clear
-	 * interrupt pending bit in mip.
-	 */
-	machine_command(ECALL_CLEAR_PENDING, 0);
+	csr_clear(sip, SIP_STIP);
 
 	if (sc->et.et_active)
 		sc->et.et_event_cb(&sc->et, sc->et.et_arg);
@@ -207,17 +221,22 @@ riscv_tmr_attach(device_t dev)
 		return (ENXIO);
 	}
 
+	/* Memory interface */
+	sc->bst = rman_get_bustag(sc->res[0]);
+	sc->bsh = rman_get_bushandle(sc->res[0]);
+
 	riscv_tmr_sc = sc;
 
 	/* Setup IRQs handler */
-	error = bus_setup_intr(dev, sc->res[0], INTR_TYPE_CLK,
-	    riscv_tmr_intr, NULL, sc, &sc->ihl[0]);
+	error = bus_setup_intr(dev, sc->res[1], INTR_TYPE_CLK,
+	    riscv_tmr_intr, NULL, sc, &sc->ih);
 	if (error) {
 		device_printf(dev, "Unable to alloc int resource.\n");
 		return (ENXIO);
 	}
 
 	riscv_tmr_timecount.tc_frequency = sc->clkfreq;
+	riscv_tmr_timecount.tc_priv = sc;
 	tc_init(&riscv_tmr_timecount);
 
 	sc->et.et_name = "RISC-V Eventtimer";
@@ -257,8 +276,8 @@ EARLY_DRIVER_MODULE(timer, ofwbus, riscv_tmr_fdt_driver, riscv_tmr_fdt_devclass,
 void
 DELAY(int usec)
 {
-	int32_t counts, counts_per_usec;
-	uint32_t first, last;
+	int64_t counts, counts_per_usec;
+	uint64_t first, last;
 
 	/*
 	 * Check the timers are setup, if not just
@@ -289,11 +308,11 @@ DELAY(int usec)
 	else
 		counts = usec * counts_per_usec;
 
-	first = get_counts();
+	first = get_counts(riscv_tmr_sc);
 
 	while (counts > 0) {
-		last = get_counts();
-		counts -= (int32_t)(last - first);
+		last = get_counts(riscv_tmr_sc);
+		counts -= (int64_t)(last - first);
 		first = last;
 	}
 }
diff --git a/sys/riscv/riscv/trap.c b/sys/riscv/riscv/trap.c
index 22d27f41e592..97d9a2c1efdd 100644
--- a/sys/riscv/riscv/trap.c
+++ b/sys/riscv/riscv/trap.c
@@ -1,5 +1,5 @@
 /*-
- * Copyright (c) 2015 Ruslan Bukin <br@bsdpad.com>
+ * Copyright (c) 2015-2016 Ruslan Bukin <br@bsdpad.com>
  * All rights reserved.
  *
  * Portions of this software were developed by SRI International and the
@@ -211,7 +211,7 @@ data_abort(struct trapframe *frame, int lower)
 
 	va = trunc_page(sbadaddr);
 
-	if (frame->tf_scause == EXCP_STORE_ACCESS_FAULT) {
+	if (frame->tf_scause == EXCP_FAULT_STORE) {
 		ftype = (VM_PROT_READ | VM_PROT_WRITE);
 	} else {
 		ftype = (VM_PROT_READ);
@@ -269,6 +269,12 @@ void
 do_trap_supervisor(struct trapframe *frame)
 {
 	uint64_t exception;
+	uint64_t sstatus;
+
+	/* Ensure we came from supervisor mode, interrupts disabled */
+	__asm __volatile("csrr %0, sstatus" : "=&r" (sstatus));
+	KASSERT((sstatus & (SSTATUS_SPP | SSTATUS_SIE)) == SSTATUS_SPP,
+			("We must came from S mode with interrupts disabled"));
 
 	exception = (frame->tf_scause & EXCP_MASK);
 	if (frame->tf_scause & EXCP_INTR) {
@@ -286,12 +292,12 @@ do_trap_supervisor(struct trapframe *frame)
 	    curthread, frame->tf_sepc, frame);
 
 	switch(exception) {
-	case EXCP_LOAD_ACCESS_FAULT:
-	case EXCP_STORE_ACCESS_FAULT:
-	case EXCP_INSTR_ACCESS_FAULT:
+	case EXCP_FAULT_LOAD:
+	case EXCP_FAULT_STORE:
+	case EXCP_FAULT_FETCH:
 		data_abort(frame, 0);
 		break;
-	case EXCP_INSTR_BREAKPOINT:
+	case EXCP_BREAKPOINT:
 #ifdef KDTRACE_HOOKS
 		if (dtrace_invop_jump_addr != 0) {
 			dtrace_invop_jump_addr(frame);
@@ -305,7 +311,7 @@ do_trap_supervisor(struct trapframe *frame)
 		panic("No debugger in kernel.\n");
 #endif
 		break;
-	case EXCP_INSTR_ILLEGAL:
+	case EXCP_ILLEGAL_INSTRUCTION:
 		dump_regs(frame);
 		panic("Illegal instruction at 0x%016lx\n", frame->tf_sepc);
 		break;
@@ -321,10 +327,16 @@ do_trap_user(struct trapframe *frame)
 {
 	uint64_t exception;
 	struct thread *td;
+	uint64_t sstatus;
 
 	td = curthread;
 	td->td_frame = frame;
 
+	/* Ensure we came from usermode, interrupts disabled */
+	__asm __volatile("csrr %0, sstatus" : "=&r" (sstatus));
+	KASSERT((sstatus & (SSTATUS_SPP | SSTATUS_SIE)) == 0,
+			("We must came from U mode with interrupts disabled"));
+
 	exception = (frame->tf_scause & EXCP_MASK);
 	if (frame->tf_scause & EXCP_INTR) {
 		/* Interrupt */
@@ -336,20 +348,20 @@ do_trap_user(struct trapframe *frame)
 	    curthread, frame->tf_sepc, frame);
 
 	switch(exception) {
-	case EXCP_LOAD_ACCESS_FAULT:
-	case EXCP_STORE_ACCESS_FAULT:
-	case EXCP_INSTR_ACCESS_FAULT:
+	case EXCP_FAULT_LOAD:
+	case EXCP_FAULT_STORE:
+	case EXCP_FAULT_FETCH:
 		data_abort(frame, 1);
 		break;
-	case EXCP_UMODE_ENV_CALL:
+	case EXCP_USER_ECALL:
 		frame->tf_sepc += 4;	/* Next instruction */
 		svc_handler(frame);
 		break;
-	case EXCP_INSTR_ILLEGAL:
+	case EXCP_ILLEGAL_INSTRUCTION:
 		call_trapsignal(td, SIGILL, ILL_ILLTRP, (void *)frame->tf_sepc);
 		userret(td, frame);
 		break;
-	case EXCP_INSTR_BREAKPOINT:
+	case EXCP_BREAKPOINT:
 		call_trapsignal(td, SIGTRAP, TRAP_BRKPT, (void *)frame->tf_sepc);
 		userret(td, frame);
 		break;
diff --git a/sys/riscv/riscv/vm_machdep.c b/sys/riscv/riscv/vm_machdep.c
index 833482c73f65..1f6613077142 100644
--- a/sys/riscv/riscv/vm_machdep.c
+++ b/sys/riscv/riscv/vm_machdep.c
@@ -1,5 +1,5 @@
 /*-
- * Copyright (c) 2015 Ruslan Bukin <br@bsdpad.com>
+ * Copyright (c) 2015-2016 Ruslan Bukin <br@bsdpad.com>
  * All rights reserved.
  *
  * Portions of this software were developed by SRI International and the
@@ -86,7 +86,8 @@ cpu_fork(struct thread *td1, struct proc *p2, struct thread *td2, int flags)
 	/* Arguments for child */
 	tf->tf_a[0] = 0;
 	tf->tf_a[1] = 0;
-	tf->tf_sstatus = SSTATUS_PIE;
+	tf->tf_sstatus = (SSTATUS_SPIE);
+	tf->tf_sstatus |= (MSTATUS_PRV_U << MSTATUS_SPP_SHIFT);
 
 	td2->td_frame = tf;
 
@@ -98,7 +99,7 @@ cpu_fork(struct thread *td1, struct proc *p2, struct thread *td2, int flags)
 
 	/* Setup to release spin count in fork_exit(). */
 	td2->td_md.md_spinlock_count = 1;
-	td2->td_md.md_saved_sstatus_ie = 1;
+	td2->td_md.md_saved_sstatus_ie = (SSTATUS_SIE);
 }
 
 void
@@ -166,7 +167,7 @@ cpu_copy_thread(struct thread *td, struct thread *td0)
 
 	/* Setup to release spin count in fork_exit(). */
 	td->td_md.md_spinlock_count = 1;
-	td->td_md.md_saved_sstatus_ie = 1;
+	td->td_md.md_saved_sstatus_ie = (SSTATUS_SIE);
 }
 
 /*