diff --git a/lib/csu/riscv/crt1.c b/lib/csu/riscv/crt1.c
index 0c3fbe656add..0905e84e7b58 100644
--- a/lib/csu/riscv/crt1.c
+++ b/lib/csu/riscv/crt1.c
@@ -1,7 +1,7 @@
 /* LINTLIBRARY */
 /*-
  * Copyright 1996-1998 John D. Polstra.
- * Copyright (c) 2015 Ruslan Bukin <br@bsdpad.com>
+ * Copyright (c) 2015-2017 Ruslan Bukin <br@bsdpad.com>
  * All rights reserved.
  *
  * Portions of this software were developed by SRI International and the
@@ -64,7 +64,10 @@ __asm("	.text			\n"
 "	slli	t0, a0, 3	\n" /* mult by arg size */
 "	add	a2, a1, t0	\n" /* env is after argv */
 "	addi	a2, a2, 8	\n" /* argv is null terminated */
-"	lla	gp, _gp		\n" /* load global pointer */
+"	.option push		\n"
+"	.option norelax		\n"
+"	lla	gp, __global_pointer$\n"
+"	.option pop		\n"
 "	call	__start");
 
 void
diff --git a/lib/csu/riscv/crti.S b/lib/csu/riscv/crti.S
index df6027eee8b6..4aad1486c5c6 100644
--- a/lib/csu/riscv/crti.S
+++ b/lib/csu/riscv/crti.S
@@ -1,5 +1,5 @@
 /*-
- * Copyright (c) 2015 Ruslan Bukin <br@bsdpad.com>
+ * Copyright (c) 2015-2017 Ruslan Bukin <br@bsdpad.com>
  * All rights reserved.
  *
  * Portions of this software were developed by SRI International and the
@@ -35,12 +35,13 @@
 #include <machine/asm.h>
 __FBSDID("$FreeBSD$");
 
-# this puts _gp into .dynsym, so symlook_obj can now find that (see reloc.c)
-	.weak	_gp
-_gp:
+# this puts __global_pointer$ into .dynsym, so symlook_obj can now find that
+# (see reloc.c)
+	.weak	__global_pointer$
+__global_pointer$:
 
 	.section .init,"ax",@progbits
-	.align	2
+	.align	0
 	.globl	_init
 	.type	_init,@function
 _init:
@@ -48,7 +49,7 @@ _init:
 	sd	ra, 0(sp)
 
 	.section .fini,"ax",@progbits
-	.align	2
+	.align	0
 	.globl	_fini
 	.type	_fini,@function
 _fini:
diff --git a/lib/libc/riscv/gen/fabs.S b/lib/libc/riscv/gen/fabs.S
index 3fc791a452f4..7cd303336cf7 100644
--- a/lib/libc/riscv/gen/fabs.S
+++ b/lib/libc/riscv/gen/fabs.S
@@ -1,5 +1,5 @@
 /*-
- * Copyright (c) 2015 Ruslan Bukin <br@bsdpad.com>
+ * Copyright (c) 2015-2017 Ruslan Bukin <br@bsdpad.com>
  * All rights reserved.
  *
  * Portions of this software were developed by SRI International and the
@@ -35,7 +35,9 @@
 #include <machine/asm.h>
 __FBSDID("$FreeBSD$");
 
+#ifndef SOFTFLOAT
 ENTRY(fabs)
 	fabs.d	fa0, fa0
 	ret
 END(fabs)
+#endif
diff --git a/libexec/rtld-elf/riscv/reloc.c b/libexec/rtld-elf/riscv/reloc.c
index a0fedf1c2416..303d1a4737b0 100644
--- a/libexec/rtld-elf/riscv/reloc.c
+++ b/libexec/rtld-elf/riscv/reloc.c
@@ -1,5 +1,5 @@
 /*-
- * Copyright (c) 2015 Ruslan Bukin <br@bsdpad.com>
+ * Copyright (c) 2015-2017 Ruslan Bukin <br@bsdpad.com>
  * All rights reserved.
  *
  * This software was developed by SRI International and the University of
@@ -62,7 +62,7 @@ set_gp(Obj_Entry *obj)
 
 	__asm __volatile("mv    %0, gp" : "=r"(old));
 
-	symlook_init(&req, "_gp");
+	symlook_init(&req, "__global_pointer$");
 	req.ventry = NULL;
 	req.flags = SYMLOOK_EARLY;
 	res = symlook_obj(&req, obj);
diff --git a/share/mk/bsd.cpu.mk b/share/mk/bsd.cpu.mk
index 0daca60a0a7e..80a19bc622e5 100644
--- a/share/mk/bsd.cpu.mk
+++ b/share/mk/bsd.cpu.mk
@@ -364,8 +364,11 @@ CFLAGS += -mcpu=8540 -Wa,-me500 -mspe=yes -mabi=spe -mfloat-gprs=double
 
 .if ${MACHINE_CPUARCH} == "riscv"
 .if ${TARGET_ARCH:Mriscv*sf}
-CFLAGS += -mno-float
-ACFLAGS += -mno-float
+CFLAGS += -march=rv64imac -mabi=lp64
+ACFLAGS += -march=rv64imac -mabi=lp64
+.else
+CFLAGS += -march=rv64imafdc -mabi=lp64
+ACFLAGS += -march=rv64imafdc -mabi=lp64
 .endif
 .endif
 
diff --git a/share/mk/bsd.stand.mk b/share/mk/bsd.stand.mk
index 838a3ca7c45f..bb20d98d1f96 100644
--- a/share/mk/bsd.stand.mk
+++ b/share/mk/bsd.stand.mk
@@ -7,7 +7,7 @@
 CFLAGS+= -ffreestanding -Wformat
 CFLAGS+= ${CFLAGS_NO_SIMD} -D_STANDALONE
 .if ${MACHINE_CPUARCH} == "riscv"
-CFLAGS+=	-mno-float
+CFLAGS+=	-march=rv64imac -mabi=lp64
 .elif ${MACHINE_CPUARCH} != "aarch64"
 CFLAGS+=	-msoft-float
 .endif
diff --git a/sys/boot/fdt/dts/riscv/lowrisc.dts b/sys/boot/fdt/dts/riscv/lowrisc.dts
deleted file mode 100644
index ac47eb743a6f..000000000000
--- a/sys/boot/fdt/dts/riscv/lowrisc.dts
+++ /dev/null
@@ -1,108 +0,0 @@
-/*-
- * Copyright (c) 2016 Ruslan Bukin <br@bsdpad.com>
- * All rights reserved.
- *
- * Portions of this software were developed by SRI International and the
- * University of Cambridge Computer Laboratory under DARPA/AFRL contract
- * FA8750-10-C-0237 ("CTSRD"), as part of the DARPA CRASH research programme.
- *
- * Portions of this software were developed by the University of Cambridge
- * Computer Laboratory as part of the CTSRD Project, with support from the
- * UK Higher Education Innovation Fund (HEIF).
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in the
- *    documentation and/or other materials provided with the distribution.
- *
- * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
- * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
- * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
- * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
- * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
- * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
- * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
- * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
- * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
- * SUCH DAMAGE.
- *
- * $FreeBSD$
- */
-
-/dts-v1/;
-
-/ {
-	model = "UC Berkeley Spike Simulator RV64I";
-	compatible = "riscv,rv64i";
-	#address-cells = <1>;
-	#size-cells = <1>;
-	#interrupt-cells = <1>;
-
-	cpus {
-		#address-cells = <1>;
-		#size-cells = <0>;
-
-		cpu@0 {
-			device_type = "cpu";
-			compatible = "riscv,rv64i";
-			reg = <0x40002000>;
-		};
-	};
-
-	aliases {
-		serial0 = &serial0;
-	};
-
-	memory {
-		device_type = "memory";
-		reg = <0x0 0x8000000>; /* 128MB at 0x0 */
-	};
-
-	soc {
-		#address-cells = <1>;
-		#size-cells = <1>;
-		#interrupt-cells = <1>;
-
-		compatible = "simple-bus";
-		ranges;
-
-		pic0: pic@0 {
-			compatible = "riscv,pic";
-			interrupt-controller;
-		};
-
-		serial0: serial@80001000 {
-			compatible = "ns16550";
-			reg = <0x80001000 0x1000>;
-			reg-shift = <2>;
-			interrupts = < 4 >;
-			interrupt-parent = <&pic0>;
-			current-speed = <115200>;
-			clock-frequency = < 1000000 >;
-			status = "okay";
-		};
-
-		spi0: spi@80010000 {
-			compatible = "xlnx,xps-spi-3.2";
-			reg = <0x80010000 0x1000>;
-		};
-
-		timer0: timer@0 {
-			compatible = "riscv,timer";
-			interrupts = < 1 >;
-			interrupt-parent = < &pic0 >;
-			clock-frequency = < 1000000 >;
-		};
-	};
-
-	chosen {
-		bootargs = "-v";
-		stdin = "serial0";
-		stdout = "serial0";
-	};
-};
diff --git a/sys/boot/fdt/dts/riscv/qemu.dts b/sys/boot/fdt/dts/riscv/qemu.dts
deleted file mode 100644
index df6fe0086ae5..000000000000
--- a/sys/boot/fdt/dts/riscv/qemu.dts
+++ /dev/null
@@ -1,105 +0,0 @@
-/*-
- * Copyright (c) 2016 Ruslan Bukin <br@bsdpad.com>
- * All rights reserved.
- *
- * Portions of this software were developed by SRI International and the
- * University of Cambridge Computer Laboratory under DARPA/AFRL contract
- * FA8750-10-C-0237 ("CTSRD"), as part of the DARPA CRASH research programme.
- *
- * Portions of this software were developed by the University of Cambridge
- * Computer Laboratory as part of the CTSRD Project, with support from the
- * UK Higher Education Innovation Fund (HEIF).
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in the
- *    documentation and/or other materials provided with the distribution.
- *
- * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
- * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
- * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
- * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
- * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
- * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
- * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
- * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
- * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
- * SUCH DAMAGE.
- *
- * $FreeBSD$
- */
-
-/dts-v1/;
-
-/ {
-	model = "QEMU RV64";
-	compatible = "riscv,rv64";
-	#address-cells = <1>;
-	#size-cells = <1>;
-	#interrupt-cells = <1>;
-
-	cpus {
-		#address-cells = <1>;
-		#size-cells = <0>;
-
-		cpu@0 {
-			device_type = "cpu";
-			compatible = "riscv,rv64";
-			reg = <0x0>;
-		};
-	};
-
-	aliases {
-		console0 = &console0;
-	};
-
-	memory {
-		/*
-		 * This is not used currently.
-		 * We take information from sbi_query_memory.
-		 */
-		device_type = "memory";
-		reg = <0x80000000 0x40000000>; /* 1GB at 0x80000000 */
-	};
-
-	soc {
-		#address-cells = <1>;
-		#size-cells = <1>;
-		#interrupt-cells = <1>;
-
-		compatible = "simple-bus";
-		ranges;
-
-		pic0: pic@0 {
-			compatible = "riscv,pic";
-			interrupt-controller;
-		};
-
-		timer0: timer@0 {
-			compatible = "riscv,timer";
-			reg = < 0x40000000 0x0008 >,	/* rtc */
-			      < 0x40000008 0x1000 >;	/* timecmp */
-			interrupts = < 5 >;
-			interrupt-parent = < &pic0 >;
-			clock-frequency = < 400000000 >;
-		};
-
-		console0: console@0 {
-			compatible = "riscv,console";
-			status = "okay";
-			interrupts = < 1 >;
-			interrupt-parent = < &pic0 >;
-		};
-	};
-
-	chosen {
-		bootargs = "-v";
-		stdin = "console0";
-		stdout = "console0";
-	};
-};
diff --git a/sys/boot/fdt/dts/riscv/rocket.dts b/sys/boot/fdt/dts/riscv/rocket.dts
deleted file mode 100644
index 691f41209cf0..000000000000
--- a/sys/boot/fdt/dts/riscv/rocket.dts
+++ /dev/null
@@ -1,105 +0,0 @@
-/*-
- * Copyright (c) 2015-2016 Ruslan Bukin <br@bsdpad.com>
- * All rights reserved.
- *
- * Portions of this software were developed by SRI International and the
- * University of Cambridge Computer Laboratory under DARPA/AFRL contract
- * FA8750-10-C-0237 ("CTSRD"), as part of the DARPA CRASH research programme.
- *
- * Portions of this software were developed by the University of Cambridge
- * Computer Laboratory as part of the CTSRD Project, with support from the
- * UK Higher Education Innovation Fund (HEIF).
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in the
- *    documentation and/or other materials provided with the distribution.
- *
- * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
- * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
- * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
- * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
- * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
- * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
- * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
- * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
- * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
- * SUCH DAMAGE.
- *
- * $FreeBSD$
- */
-
-/dts-v1/;
-
-/ {
-	model = "RocketChip RV64";
-	compatible = "riscv,rv64";
-	#address-cells = <1>;
-	#size-cells = <1>;
-	#interrupt-cells = <1>;
-
-	cpus {
-		#address-cells = <1>;
-		#size-cells = <0>;
-
-		cpu@0 {
-			device_type = "cpu";
-			compatible = "riscv,rv64";
-			reg = <0x0>;
-		};
-	};
-
-	aliases {
-		console0 = &console0;
-	};
-
-	memory {
-		/*
-		 * This is not used currently.
-		 * We take information from sbi_query_memory.
-		 */
-		device_type = "memory";
-		reg = <0x80000000 0x10000000>; /* 256MB at 0x80000000 */
-	};
-
-	soc {
-		#address-cells = <1>;
-		#size-cells = <1>;
-		#interrupt-cells = <1>;
-
-		compatible = "simple-bus";
-		ranges;
-
-		pic0: pic@0 {
-			compatible = "riscv,pic";
-			interrupt-controller;
-		};
-
-		timer0: timer@0 {
-			compatible = "riscv,timer";
-			reg = < 0x4400bff8 0x0008 >,	/* rtc */
-			      < 0x44004000 0x1000 >;	/* timecmp */
-			interrupts = < 5 >;
-			interrupt-parent = < &pic0 >;
-			clock-frequency = < 1000000 >;
-		};
-
-		console0: console@0 {
-			compatible = "riscv,console";
-			status = "okay";
-			interrupts = < 1 >;
-			interrupt-parent = < &pic0 >;
-		};
-	};
-
-	chosen {
-		bootargs = "-v";
-		stdin = "console0";
-		stdout = "console0";
-	};
-};
diff --git a/sys/boot/fdt/dts/riscv/spike.dts b/sys/boot/fdt/dts/riscv/spike.dts
deleted file mode 100644
index 30ce7f5e13dc..000000000000
--- a/sys/boot/fdt/dts/riscv/spike.dts
+++ /dev/null
@@ -1,111 +0,0 @@
-/*-
- * Copyright (c) 2015-2016 Ruslan Bukin <br@bsdpad.com>
- * All rights reserved.
- *
- * Portions of this software were developed by SRI International and the
- * University of Cambridge Computer Laboratory under DARPA/AFRL contract
- * FA8750-10-C-0237 ("CTSRD"), as part of the DARPA CRASH research programme.
- *
- * Portions of this software were developed by the University of Cambridge
- * Computer Laboratory as part of the CTSRD Project, with support from the
- * UK Higher Education Innovation Fund (HEIF).
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in the
- *    documentation and/or other materials provided with the distribution.
- *
- * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
- * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
- * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
- * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
- * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
- * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
- * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
- * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
- * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
- * SUCH DAMAGE.
- *
- * $FreeBSD$
- */
-
-/dts-v1/;
-
-/ {
-	model = "UC Berkeley Spike Simulator RV64";
-	compatible = "riscv,rv64";
-	#address-cells = <1>;
-	#size-cells = <1>;
-	#interrupt-cells = <1>;
-
-	cpus {
-		#address-cells = <1>;
-		#size-cells = <0>;
-
-		cpu@0 {
-			device_type = "cpu";
-			compatible = "riscv,rv64";
-			reg = <0x0>;
-		};
-
-		cpu@1 {
-			device_type = "cpu";
-			compatible = "riscv,rv64";
-			reg = <0x0>;
-		};
-	};
-
-	aliases {
-		console0 = &console0;
-	};
-
-	memory {
-		/*
-		 * This is not used currently.
-		 * We take information from sbi_query_memory.
-		 */
-		device_type = "memory";
-		reg = <0x80000000 0x40000000>; /* 1GB at 0x80000000 */
-	};
-
-	soc {
-		#address-cells = <1>;
-		#size-cells = <1>;
-		#interrupt-cells = <1>;
-
-		compatible = "simple-bus";
-		ranges;
-
-		pic0: pic@0 {
-			compatible = "riscv,pic";
-			interrupt-controller;
-		};
-
-		timer0: timer@0 {
-			compatible = "riscv,timer";
-			reg = < 0x40000000 0x0008 >,	/* rtc */
-			      < 0x40000008 0x1000 >;	/* timecmp */
-			interrupts = < 5 >;
-			interrupt-parent = < &pic0 >;
-			clock-frequency = < 1000000 >;
-		};
-
-		console0: console@0 {
-			compatible = "riscv,console";
-			status = "okay";
-			interrupts = < 1 >;
-			interrupt-parent = < &pic0 >;
-		};
-	};
-
-	chosen {
-		bootargs = "-v";
-		stdin = "console0";
-		stdout = "console0";
-	};
-};
diff --git a/sys/conf/Makefile.riscv b/sys/conf/Makefile.riscv
index 133ac964372e..1285acc886a2 100644
--- a/sys/conf/Makefile.riscv
+++ b/sys/conf/Makefile.riscv
@@ -28,6 +28,11 @@ S=	../../..
 
 INCLUDES+= -I$S/contrib/libfdt
 
+SYSTEM_LD= @${LD} -N -m ${LD_EMULATION} -Bdynamic -T ${LDSCRIPT} ${_LDFLAGS} \
+	--no-warn-mismatch --warn-common --export-dynamic \
+	--dynamic-linker /red/herring \
+	-o ${.TARGET} -X ${SYSTEM_OBJS} vers.o
+
 .if !empty(DDB_ENABLED)
 CFLAGS += -fno-omit-frame-pointer -fno-optimize-sibling-calls
 .endif
diff --git a/sys/conf/files.riscv b/sys/conf/files.riscv
index 2a27bb1f18ad..aa035a6a7019 100644
--- a/sys/conf/files.riscv
+++ b/sys/conf/files.riscv
@@ -46,7 +46,6 @@ riscv/riscv/nexus.c		standard
 riscv/riscv/ofw_machdep.c	optional	fdt
 riscv/riscv/pmap.c		standard
 riscv/riscv/riscv_console.c	optional	rcons
-riscv/riscv/sbi.S		standard
 riscv/riscv/stack_machdep.c	optional	ddb | stack
 riscv/riscv/support.S		standard
 riscv/riscv/swtch.S		standard
diff --git a/sys/conf/kern.mk b/sys/conf/kern.mk
index 0f4c62a5c507..fa444e196000 100644
--- a/sys/conf/kern.mk
+++ b/sys/conf/kern.mk
@@ -121,7 +121,7 @@ INLINE_LIMIT?=	8000
 .endif
 
 .if ${MACHINE_CPUARCH} == "riscv"
-CFLAGS.gcc+=	-mcmodel=medany
+CFLAGS.gcc+=	-mcmodel=medany -march=rv64imafdc -mabi=lp64
 INLINE_LIMIT?=	8000
 .endif
 
diff --git a/sys/riscv/conf/GENERIC b/sys/riscv/conf/GENERIC
index a9c3c6fbdad0..23b487411598 100644
--- a/sys/riscv/conf/GENERIC
+++ b/sys/riscv/conf/GENERIC
@@ -75,6 +75,9 @@ options 	RACCT_DEFAULT_TO_DISABLED # Set kern.racct.enable=0 by default
 options 	RCTL			# Resource limits
 options 	SMP
 
+# RISC-V SBI console
+device		rcons
+
 # Uncomment for memory disk
 # options 	MD_ROOT
 # options 	MD_ROOT_SIZE=32768	# 32MB ram disk
diff --git a/sys/riscv/conf/LOWRISC b/sys/riscv/conf/LOWRISC
deleted file mode 100644
index f4a9c69baec7..000000000000
--- a/sys/riscv/conf/LOWRISC
+++ /dev/null
@@ -1,43 +0,0 @@
-#
-# Kernel configuration file for lowRISC.
-#
-# For more information on this file, please read the config(5) manual page,
-# and/or the handbook section on Kernel Configuration Files:
-#
-#    http://www.FreeBSD.org/doc/en_US.ISO8859-1/books/handbook/kernelconfig-config.html
-#
-# The handbook is also available locally in /usr/share/doc/handbook
-# if you've installed the doc distribution, otherwise always see the
-# FreeBSD World Wide Web server (http://www.FreeBSD.org/) for the
-# latest information.
-#
-# An exhaustive list of options and more detailed explanations of the
-# device lines is also present in the ../../conf/NOTES and NOTES files.
-# If you are in doubt as to the purpose or necessity of a line, check first
-# in NOTES.
-#
-# $FreeBSD$
-
-include		GENERIC
-ident		LOWRISC
-
-hints		"LOWRISC.hints"
-
-options		ROOTDEVNAME=\"ufs:mmcsd0s2\"
-
-# MMC/SD
-device		mmc
-device		mmcsd
-# device	mmc_spi
-
-# SPI
-device		spibus
-device		xilinx_spi
-
-# Serial (COM) ports
-device		uart		# Generic UART driver
-device		uart_ns8250	# ns8250-type UART driver
-
-# RISCVTODO: This needs to be done via loader (when it's available).
-options 	FDT_DTB_STATIC
-makeoptions	FDT_DTS_FILE=lowrisc.dts
diff --git a/sys/riscv/conf/LOWRISC.hints b/sys/riscv/conf/LOWRISC.hints
deleted file mode 100644
index f8ed5ff7691a..000000000000
--- a/sys/riscv/conf/LOWRISC.hints
+++ /dev/null
@@ -1,5 +0,0 @@
-# $FreeBSD$
-
-# MMC device
-hint.mmc_spi.0.at="spibus0"
-hint.mmc_spi.0.cs=0
diff --git a/sys/riscv/conf/QEMU b/sys/riscv/conf/QEMU
deleted file mode 100644
index 7411aefc6b1d..000000000000
--- a/sys/riscv/conf/QEMU
+++ /dev/null
@@ -1,29 +0,0 @@
-#
-# Kernel configuration file for QEMU emulator.
-#
-# For more information on this file, please read the config(5) manual page,
-# and/or the handbook section on Kernel Configuration Files:
-#
-#    http://www.FreeBSD.org/doc/en_US.ISO8859-1/books/handbook/kernelconfig-config.html
-#
-# The handbook is also available locally in /usr/share/doc/handbook
-# if you've installed the doc distribution, otherwise always see the
-# FreeBSD World Wide Web server (http://www.FreeBSD.org/) for the
-# latest information.
-#
-# An exhaustive list of options and more detailed explanations of the
-# device lines is also present in the ../../conf/NOTES and NOTES files.
-# If you are in doubt as to the purpose or necessity of a line, check first
-# in NOTES.
-#
-# $FreeBSD$
-
-include		GENERIC
-ident		QEMU
-
-device		rcons
-options 	ROOTDEVNAME=\"ufs:/dev/md0\"
-
-# RISCVTODO: This needs to be done via loader (when it's available).
-options 	FDT_DTB_STATIC
-makeoptions	FDT_DTS_FILE=qemu.dts
diff --git a/sys/riscv/conf/ROCKET b/sys/riscv/conf/ROCKET
deleted file mode 100644
index 1b2b1c08b8dd..000000000000
--- a/sys/riscv/conf/ROCKET
+++ /dev/null
@@ -1,29 +0,0 @@
-#
-# Kernel configuration file for Rocket Core.
-#
-# For more information on this file, please read the config(5) manual page,
-# and/or the handbook section on Kernel Configuration Files:
-#
-#    http://www.FreeBSD.org/doc/en_US.ISO8859-1/books/handbook/kernelconfig-config.html
-#
-# The handbook is also available locally in /usr/share/doc/handbook
-# if you've installed the doc distribution, otherwise always see the
-# FreeBSD World Wide Web server (http://www.FreeBSD.org/) for the
-# latest information.
-#
-# An exhaustive list of options and more detailed explanations of the
-# device lines is also present in the ../../conf/NOTES and NOTES files.
-# If you are in doubt as to the purpose or necessity of a line, check first
-# in NOTES.
-#
-# $FreeBSD$
-
-include		GENERIC
-ident		ROCKET
-
-device		rcons
-options 	ROOTDEVNAME=\"ufs:/dev/md0\"
-
-# RISCVTODO: This needs to be done via loader (when it's available).
-options 	FDT_DTB_STATIC
-makeoptions	FDT_DTS_FILE=rocket.dts
diff --git a/sys/riscv/conf/SPIKE b/sys/riscv/conf/SPIKE
deleted file mode 100644
index cc36e6f97b00..000000000000
--- a/sys/riscv/conf/SPIKE
+++ /dev/null
@@ -1,29 +0,0 @@
-#
-# Kernel configuration file for UCB Spike simulator.
-#
-# For more information on this file, please read the config(5) manual page,
-# and/or the handbook section on Kernel Configuration Files:
-#
-#    http://www.FreeBSD.org/doc/en_US.ISO8859-1/books/handbook/kernelconfig-config.html
-#
-# The handbook is also available locally in /usr/share/doc/handbook
-# if you've installed the doc distribution, otherwise always see the
-# FreeBSD World Wide Web server (http://www.FreeBSD.org/) for the
-# latest information.
-#
-# An exhaustive list of options and more detailed explanations of the
-# device lines is also present in the ../../conf/NOTES and NOTES files.
-# If you are in doubt as to the purpose or necessity of a line, check first
-# in NOTES.
-#
-# $FreeBSD$
-
-include		GENERIC
-ident		SPIKE
-
-device		rcons
-options 	ROOTDEVNAME=\"ufs:/dev/md0\"
-
-# RISCVTODO: This needs to be done via loader (when it's available).
-options 	FDT_DTB_STATIC
-makeoptions	FDT_DTS_FILE=spike.dts
diff --git a/sys/riscv/include/machdep.h b/sys/riscv/include/machdep.h
index 033f7d62af08..1cd34c22d64f 100644
--- a/sys/riscv/include/machdep.h
+++ b/sys/riscv/include/machdep.h
@@ -1,5 +1,5 @@
 /*-
- * Copyright (c) 2015 Ruslan Bukin <br@bsdpad.com>
+ * Copyright (c) 2015-2017 Ruslan Bukin <br@bsdpad.com>
  * All rights reserved.
  *
  * Portions of this software were developed by SRI International and the
@@ -39,7 +39,10 @@
 
 struct riscv_bootparams {
 	vm_offset_t	kern_l1pt;	/* Kernel L1 base */
+	vm_offset_t	kern_phys;	/* Kernel base (physical) addr */
 	vm_offset_t	kern_stack;
+	vm_offset_t	dtbp_virt;	/* Device tree blob virtual addr */
+	vm_offset_t	dtbp_phys;	/* Device tree blob physical addr */
 };
 
 extern vm_paddr_t physmap[];
diff --git a/sys/riscv/include/riscvreg.h b/sys/riscv/include/riscvreg.h
index 77f35bfa64b0..4558a4de4acc 100644
--- a/sys/riscv/include/riscvreg.h
+++ b/sys/riscv/include/riscvreg.h
@@ -1,5 +1,5 @@
 /*-
- * Copyright (c) 2015-2016 Ruslan Bukin <br@bsdpad.com>
+ * Copyright (c) 2015-2017 Ruslan Bukin <br@bsdpad.com>
  * All rights reserved.
  *
  * Portions of this software were developed by SRI International and the
@@ -51,6 +51,9 @@
 #define	EXCP_SUPERVISOR_ECALL		9
 #define	EXCP_HYPERVISOR_ECALL		10
 #define	EXCP_MACHINE_ECALL		11
+#define	EXCP_INST_PAGE_FAULT		12
+#define	EXCP_LOAD_PAGE_FAULT		13
+#define	EXCP_STORE_PAGE_FAULT		15
 #define	EXCP_INTR			(1ul << 63)
 
 #define	SSTATUS_UIE			(1 << 0)
@@ -68,7 +71,7 @@
 #define	SSTATUS_FS_MASK			(0x3 << SSTATUS_FS_SHIFT)
 #define	SSTATUS_XS_SHIFT		15
 #define	SSTATUS_XS_MASK			(0x3 << SSTATUS_XS_SHIFT)
-#define	SSTATUS_PUM			(1 << 18)
+#define	SSTATUS_SUM			(1 << 18)
 #define	SSTATUS32_SD			(1 << 63)
 #define	SSTATUS64_SD			(1 << 31)
 
@@ -141,6 +144,15 @@
 #define	SIP_SSIP	(1 << 1)
 #define	SIP_STIP	(1 << 5)
 
+#define	SATP_PPN_S	0
+#define	SATP_PPN_M	(0xfffffffffff << SATP_PPN_S)
+#define	SATP_ASID_S	44
+#define	SATP_ASID_M	(0xffff << SATP_ASID_S)
+#define	SATP_MODE_S	60
+#define	SATP_MODE_M	(0xf << SATP_MODE_S)
+#define	SATP_MODE_SV39	(8ULL << SATP_MODE_S)
+#define	SATP_MODE_SV48	(9ULL << SATP_MODE_S)
+
 #if 0
 /* lowRISC TODO */
 #define	NCSRS		4096
diff --git a/sys/riscv/include/sbi.h b/sys/riscv/include/sbi.h
index 76690ab84b08..b2505aa7f6c6 100644
--- a/sys/riscv/include/sbi.h
+++ b/sys/riscv/include/sbi.h
@@ -1,5 +1,5 @@
 /*-
- * Copyright (c) 2016 Ruslan Bukin <br@bsdpad.com>
+ * Copyright (c) 2016-2017 Ruslan Bukin <br@bsdpad.com>
  * All rights reserved.
  *
  * Portions of this software were developed by SRI International and the
@@ -37,29 +37,97 @@
 #ifndef _MACHINE_SBI_H_
 #define	_MACHINE_SBI_H_
 
-typedef struct {
-	uint64_t base;
-	uint64_t size;
-	uint64_t node_id;
-} memory_block_info;
+#define	SBI_SET_TIMER			0
+#define	SBI_CONSOLE_PUTCHAR		1
+#define	SBI_CONSOLE_GETCHAR		2
+#define	SBI_CLEAR_IPI			3
+#define	SBI_SEND_IPI			4
+#define	SBI_REMOTE_FENCE_I		5
+#define	SBI_REMOTE_SFENCE_VMA		6
+#define	SBI_REMOTE_SFENCE_VMA_ASID	7
+#define	SBI_SHUTDOWN			8
 
-uint64_t sbi_query_memory(uint64_t id, memory_block_info *p);
-uint64_t sbi_hart_id(void);
-uint64_t sbi_num_harts(void);
-uint64_t sbi_timebase(void);
-void sbi_set_timer(uint64_t stime_value);
-void sbi_send_ipi(uint64_t hart_id);
-uint64_t sbi_clear_ipi(void);
-void sbi_shutdown(void);
+static __inline uint64_t
+sbi_call(uint64_t arg7, uint64_t arg0, uint64_t arg1, uint64_t arg2)
+{
 
-void sbi_console_putchar(unsigned char ch);
-int sbi_console_getchar(void);
+	register uintptr_t a0 __asm ("a0") = (uintptr_t)(arg0);
+	register uintptr_t a1 __asm ("a1") = (uintptr_t)(arg1);
+	register uintptr_t a2 __asm ("a2") = (uintptr_t)(arg2);
+	register uintptr_t a7 __asm ("a7") = (uintptr_t)(arg7);
+	__asm __volatile(			\
+		"ecall"				\
+		:"+r"(a0)			\
+		:"r"(a1), "r"(a2), "r"(a7)	\
+		:"memory");
 
-void sbi_remote_sfence_vm(uint64_t hart_mask_ptr, uint64_t asid);
-void sbi_remote_sfence_vm_range(uint64_t hart_mask_ptr, uint64_t asid, uint64_t start, uint64_t size);
-void sbi_remote_fence_i(uint64_t hart_mask_ptr);
+	return (a0);
+}
 
-uint64_t sbi_mask_interrupt(uint64_t which);
-uint64_t sbi_unmask_interrupt(uint64_t which);
+static __inline void
+sbi_console_putchar(int ch)
+{
+
+	sbi_call(SBI_CONSOLE_PUTCHAR, ch, 0, 0);
+}
+
+static __inline int
+sbi_console_getchar(void)
+{
+
+	return (sbi_call(SBI_CONSOLE_GETCHAR, 0, 0, 0));
+}
+
+static __inline void
+sbi_set_timer(uint64_t val)
+{
+
+	sbi_call(SBI_SET_TIMER, val, 0, 0);
+}
+
+static __inline void
+sbi_shutdown(void)
+{
+
+	sbi_call(SBI_SHUTDOWN, 0, 0, 0);
+}
+
+static __inline void
+sbi_clear_ipi(void)
+{
+
+	sbi_call(SBI_CLEAR_IPI, 0, 0, 0);
+}
+
+static __inline void
+sbi_send_ipi(const unsigned long *hart_mask)
+{
+
+	sbi_call(SBI_SEND_IPI, (uint64_t)hart_mask, 0, 0);
+}
+
+static __inline void
+sbi_remote_fence_i(const unsigned long *hart_mask)
+{
+
+	sbi_call(SBI_REMOTE_FENCE_I, (uint64_t)hart_mask, 0, 0);
+}
+
+static __inline void
+sbi_remote_sfence_vma(const unsigned long *hart_mask,
+    unsigned long start, unsigned long size)
+{
+
+	sbi_call(SBI_REMOTE_SFENCE_VMA, (uint64_t)hart_mask, 0, 0);
+}
+
+static __inline void
+sbi_remote_sfence_vma_asid(const unsigned long *hart_mask,
+    unsigned long start, unsigned long size,
+    unsigned long asid)
+{
+
+	sbi_call(SBI_REMOTE_SFENCE_VMA_ASID, (uint64_t)hart_mask, 0, 0);
+}
 
 #endif /* !_MACHINE_SBI_H_ */
diff --git a/sys/riscv/riscv/cpufunc_asm.S b/sys/riscv/riscv/cpufunc_asm.S
index 1d44c22f27fa..0328383a5ca4 100644
--- a/sys/riscv/riscv/cpufunc_asm.S
+++ b/sys/riscv/riscv/cpufunc_asm.S
@@ -1,5 +1,5 @@
 /*-
- * Copyright (c) 2015-2016 Ruslan Bukin <br@bsdpad.com>
+ * Copyright (c) 2015-2017 Ruslan Bukin <br@bsdpad.com>
  * All rights reserved.
  *
  * Portions of this software were developed by SRI International and the
@@ -51,12 +51,12 @@ END(riscv_nullop)
  */
 
 ENTRY(riscv_tlb_flushID)
-	sfence.vm
+	sfence.vma
 	ret
 END(riscv_tlb_flushID)
 
 ENTRY(riscv_tlb_flushID_SE)
-	sfence.vm
+	sfence.vma
 	ret
 END(riscv_tlb_flushID_SE)
 
@@ -64,7 +64,7 @@ END(riscv_tlb_flushID_SE)
  * void riscv_dcache_wb_range(vm_offset_t, vm_size_t)
  */
 ENTRY(riscv_dcache_wb_range)
-	sfence.vm
+	sfence.vma
 	ret
 END(riscv_dcache_wb_range)
 
@@ -72,7 +72,7 @@ END(riscv_dcache_wb_range)
  * void riscv_dcache_wbinv_range(vm_offset_t, vm_size_t)
  */
 ENTRY(riscv_dcache_wbinv_range)
-	sfence.vm
+	sfence.vma
 	ret
 END(riscv_dcache_wbinv_range)
 
@@ -80,7 +80,7 @@ END(riscv_dcache_wbinv_range)
  * void riscv_dcache_inv_range(vm_offset_t, vm_size_t)
  */
 ENTRY(riscv_dcache_inv_range)
-	sfence.vm
+	sfence.vma
 	ret
 END(riscv_dcache_inv_range)
 
@@ -89,7 +89,7 @@ END(riscv_dcache_inv_range)
  */
 ENTRY(riscv_idcache_wbinv_range)
 	fence.i
-	sfence.vm
+	sfence.vma
 	ret
 END(riscv_idcache_wbinv_range)
 
diff --git a/sys/riscv/riscv/exception.S b/sys/riscv/riscv/exception.S
index d5efd732ca65..b103b4d24a31 100644
--- a/sys/riscv/riscv/exception.S
+++ b/sys/riscv/riscv/exception.S
@@ -1,5 +1,5 @@
 /*-
- * Copyright (c) 2015-2016 Ruslan Bukin <br@bsdpad.com>
+ * Copyright (c) 2015-2017 Ruslan Bukin <br@bsdpad.com>
  * All rights reserved.
  *
  * Portions of this software were developed by SRI International and the
@@ -116,8 +116,11 @@ __FBSDID("$FreeBSD$");
 .macro load_registers el
 	ld	t0, (TF_SSTATUS)(sp)
 .if \el == 0
-	/* Ensure user interrupts will be enabled on eret. */
-	li	t1, SSTATUS_SPIE
+	/*
+	 * Ensure user interrupts will be enabled on eret
+	 * and supervisor mode can access userspace on trap.
+	 */
+	li	t1, (SSTATUS_SPIE | SSTATUS_SUM)
 	or	t0, t0, t1
 .else
 	/*
diff --git a/sys/riscv/riscv/intr_machdep.c b/sys/riscv/riscv/intr_machdep.c
index b9da74fc73a2..caaefcfc5b3d 100644
--- a/sys/riscv/riscv/intr_machdep.c
+++ b/sys/riscv/riscv/intr_machdep.c
@@ -1,5 +1,5 @@
 /*-
- * Copyright (c) 2015-2016 Ruslan Bukin <br@bsdpad.com>
+ * Copyright (c) 2015-2017 Ruslan Bukin <br@bsdpad.com>
  * All rights reserved.
  *
  * Portions of this software were developed by SRI International and the
@@ -265,11 +265,14 @@ riscv_unmask_ipi(void)
 static void
 ipi_send(struct pcpu *pc, int ipi)
 {
+	uintptr_t mask;
 
 	CTR3(KTR_SMP, "%s: cpu=%d, ipi=%x", __func__, pc->pc_cpuid, ipi);
 
 	atomic_set_32(&pc->pc_pending_ipis, ipi);
-	sbi_send_ipi(pc->pc_cpuid);
+	mask = (1 << (pc->pc_cpuid));
+
+	sbi_send_ipi(&mask);
 
 	CTR1(KTR_SMP, "%s: sent", __func__);
 }
@@ -302,16 +305,20 @@ void
 ipi_selected(cpuset_t cpus, u_int ipi)
 {
 	struct pcpu *pc;
+	uintptr_t mask;
 
 	CTR1(KTR_SMP, "ipi_selected: ipi: %x", ipi);
 
+	mask = 0;
 	STAILQ_FOREACH(pc, &cpuhead, pc_allcpu) {
 		if (CPU_ISSET(pc->pc_cpuid, &cpus)) {
 			CTR3(KTR_SMP, "%s: pc: %p, ipi: %x\n", __func__, pc,
 			    ipi);
-			ipi_send(pc, ipi);
+			atomic_set_32(&pc->pc_pending_ipis, ipi);
+			mask |= (1 << (pc->pc_cpuid));
 		}
 	}
+	sbi_send_ipi(&mask);
 }
 
 #endif
diff --git a/sys/riscv/riscv/locore.S b/sys/riscv/riscv/locore.S
index 5edbf7887885..20649929d321 100644
--- a/sys/riscv/riscv/locore.S
+++ b/sys/riscv/riscv/locore.S
@@ -1,5 +1,5 @@
 /*-
- * Copyright (c) 2015-2016 Ruslan Bukin <br@bsdpad.com>
+ * Copyright (c) 2015-2017 Ruslan Bukin <br@bsdpad.com>
  * All rights reserved.
  *
  * Portions of this software were developed by SRI International and the
@@ -53,110 +53,28 @@
 	.text
 	.globl _start
 _start:
-	/* Setup supervisor trap vector */
-	la	t0, cpu_exception_handler
-	csrw	stvec, t0
+	/* Get the physical address kernel loaded to */
+	la	t0, virt_map
+	ld	t1, 0(t0)
+	sub	t1, t1, t0
+	li	t2, KERNBASE
+	sub	s9, t2, t1	/* s9 = physmem base */
+	mv	s10, a0		/* s10 = hart id */
+	mv	s11, a1		/* s11 = dtbp */
 
-	/* Ensure sscratch is zero */
-	li	t0, 0
-	csrw	sscratch, t0
-
-	/* Load physical memory information */
-	li	a0, 0
-	la	a1, memory_info
-	call	sbi_query_memory
-
-	/* Store base to s6 */
-	la	s6, memory_info
-	ld	s6, 0(s6)	/* s6 = physmem base */
+	li	t0, SSTATUS_SUM
+	csrs	sstatus, t0
 
 	/* Direct secondary cores to mpentry */
-	call	sbi_hart_id
-	bnez	a0, mpentry
+	bnez	s10, mpentry
 
 	/*
 	 * Page tables
 	 */
 
-	/* Create an L1 page for early devmap */
-	la	s1, pagetable_l1
-	la	s2, pagetable_l2_devmap	/* Link to next level PN */
-	li	t0, KERNBASE
-	sub	s2, s2, t0
-	add	s2, s2, s6
-	srli	s2, s2, PAGE_SHIFT
-
-	li	a5, (VM_MAX_KERNEL_ADDRESS - L2_SIZE)
-	srli	a5, a5, L1_SHIFT	/* >> L1_SHIFT */
-	andi	a5, a5, 0x1ff		/* & 0x1ff */
-	li	t4, PTE_V
-	slli	t5, s2, PTE_PPN0_S	/* (s2 << PTE_PPN0_S) */
-	or	t6, t4, t5
-
-	/* Store single level1 PTE entry to position */
-	li	a6, PTE_SIZE
-	mulw	a5, a5, a6
-	add	t0, s1, a5
-	sd	t6, (t0)
-
-	/* Create an L1 page for SBI */
-	la	s1, pagetable_l1
-	la	s2, pagetable_l2_sbi	/* Link to next level PN */
-	li	t0, KERNBASE
-	sub	s2, s2, t0
-	add	s2, s2, s6
-	srli	s2, s2, PAGE_SHIFT
-	li	a5, 511
-	li	t4, PTE_V
-	slli	t5, s2, PTE_PPN0_S	/* (s2 << PTE_PPN0_S) */
-	or	t6, t4, t5
-
-	/* Store SBI L1 PTE entry to position */
-	li	a6, PTE_SIZE
-	mulw	a5, a5, a6
-	add	t0, s1, a5
-	sd	t6, (t0)
-
-	/* Create an L2 page for SBI */
-	la	s1, pagetable_l2_sbi
-	la	s2, pagetable_l3_sbi	/* Link to next level PN */
-	li	t0, KERNBASE
-	sub	s2, s2, t0
-	add	s2, s2, s6
-	srli	s2, s2, PAGE_SHIFT
-	li	a5, 511
-	li	t4, PTE_V
-	slli	t5, s2, PTE_PPN0_S	/* (s2 << PTE_PPN0_S) */
-	or	t6, t4, t5
-
-	/* Store SBI L2 PTE entry to position */
-	li	a6, PTE_SIZE
-	mulw	a5, a5, a6
-	add	t0, s1, a5
-	sd	t6, (t0)
-
-	/* Create an L3 page for SBI */
-	la	s1, pagetable_l3_sbi
-	li	s2, 0x8000b000
-	srli	s2, s2, PAGE_SHIFT
-	li	a5, 511
-	li	t4, PTE_V | PTE_RX | PTE_W
-	slli	t5, s2, PTE_PPN0_S	/* (s2 << PTE_PPN0_S) */
-	or	t6, t4, t5
-
-	/* Store SBI L3 PTE entry to position */
-	li	a6, PTE_SIZE
-	mulw	a5, a5, a6
-	add	t0, s1, a5
-	sd	t6, (t0)
-	/* END SBI page creation */
-
 	/* Add L1 entry for kernel */
 	la	s1, pagetable_l1
 	la	s2, pagetable_l2	/* Link to next level PN */
-	li	t0, KERNBASE
-	sub	s2, s2, t0
-	add	s2, s2, s6
 	srli	s2, s2, PAGE_SHIFT
 
 	li	a5, KERNBASE
@@ -174,12 +92,12 @@ _start:
 
 	/* Level 2 superpages (512 x 2MiB) */
 	la	s1, pagetable_l2
-	srli	t4, s6, 21		/* Div physmem base by 2 MiB */
+	srli	t4, s9, 21		/* Div physmem base by 2 MiB */
 	li	t2, 512			/* Build 512 entries */
 	add	t3, t4, t2
 	li	t5, 0
 2:
-	li	t0, (PTE_V | PTE_RWX)
+	li	t0, (PTE_V | PTE_RWX | PTE_D)
 	slli	t2, t4, PTE_PPN1_S	/* << PTE_PPN1_S */
 	or	t5, t0, t2
 	sd	t5, (s1)		/* Store PTE entry to position */
@@ -188,13 +106,65 @@ _start:
 	addi	t4, t4, 1
 	bltu	t4, t3, 2b
 
+	/* Create an L1 page for early devmap */
+	la	s1, pagetable_l1
+	la	s2, pagetable_l2_devmap	/* Link to next level PN */
+	srli	s2, s2, PAGE_SHIFT
+
+	li	a5, (VM_MAX_KERNEL_ADDRESS - L2_SIZE)
+	srli	a5, a5, L1_SHIFT	/* >> L1_SHIFT */
+	andi	a5, a5, 0x1ff		/* & 0x1ff */
+	li	t4, PTE_V
+	slli	t5, s2, PTE_PPN0_S	/* (s2 << PTE_PPN0_S) */
+	or	t6, t4, t5
+
+	/* Store single level1 PTE entry to position */
+	li	a6, PTE_SIZE
+	mulw	a5, a5, a6
+	add	t0, s1, a5
+	sd	t6, (t0)
+
+	/* Create an L2 page superpage for DTB */
+	la	s1, pagetable_l2_devmap
+	mv	s2, s11
+	srli	s2, s2, PAGE_SHIFT
+
+	li	t0, (PTE_V | PTE_RWX | PTE_D)
+	slli	t2, s2, PTE_PPN0_S	/* << PTE_PPN0_S */
+	or	t0, t0, t2
+
+	/* Store PTE entry to position */
+	li	a6, PTE_SIZE
+	li	a5, 510
+	mulw	a5, a5, a6
+	add	t1, s1, a5
+	sd	t0, (t1)
+
+	/* Page tables END */
+
+	/* Setup supervisor trap vector */
+	la	t0, va
+	sub	t0, t0, s9
+	li	t1, KERNBASE
+	add	t0, t0, t1
+	csrw	stvec, t0
+
 	/* Set page tables base register */
 	la	s2, pagetable_l1
-	li	t0, KERNBASE
-	sub	s2, s2, t0
-	add	s2, s2, s6
 	srli	s2, s2, PAGE_SHIFT
+	li	t0, SATP_MODE_SV39
+	or	s2, s2, t0
+	sfence.vma
 	csrw	sptbr, s2
+va:
+
+	/* Setup supervisor trap vector */
+	la	t0, cpu_exception_handler
+	csrw	stvec, t0
+
+	/* Ensure sscratch is zero */
+	li	t0, 0
+	csrw	sscratch, t0
 
 	/* Initialize stack pointer */
 	la	s3, initstack_end
@@ -210,13 +180,18 @@ _start:
 	bltu	a0, s1, 1b
 
 	/* Fill riscv_bootparams */
-	addi	sp, sp, -16
+	addi	sp, sp, -40
 
 	la	t0, pagetable_l1
 	sd	t0, 0(sp) /* kern_l1pt */
+	sd	s9, 8(sp) /* kern_phys */
 
 	la	t0, initstack_end
-	sd	t0, 8(sp) /* kern_stack */
+	sd	t0, 16(sp) /* kern_stack */
+
+	li	t0, (VM_MAX_KERNEL_ADDRESS - 2 * L2_SIZE)
+	sd	t0, 24(sp) /* dtbp_virt */
+	sd	s11, 32(sp) /* dtbp_phys */
 
 	mv	a0, sp
 	call	_C_LABEL(initriscv)	/* Off we go */
@@ -258,14 +233,16 @@ pagetable_l2:
 	.space	PAGE_SIZE
 pagetable_l2_devmap:
 	.space	PAGE_SIZE
-pagetable_l2_sbi:
-	.space	PAGE_SIZE
-pagetable_l3_sbi:
-	.space	PAGE_SIZE
 
-	.globl memory_info
-memory_info:
-	.space	(24)
+        .align 3
+virt_map:
+        .quad   virt_map
+
+	/* Not in use, but required for linking. */
+	.align 3
+	.globl __global_pointer$
+__global_pointer$:
+	.space	8
 
 	.globl init_pt_va
 init_pt_va:
@@ -284,6 +261,37 @@ END(mpentry)
  * Called by a core when it is being brought online.
  */
 ENTRY(mpentry)
+	/* Setup stack pointer */
+	la	t0, secondary_stacks
+	li	t1, (PAGE_SIZE * KSTACK_PAGES)
+	mulw	t1, t1, s10
+	add	t0, t0, t1
+	sub	t0, t0, s9
+	li	t1, KERNBASE
+	add	sp, t0, t1
+
+	/* Setup supervisor trap vector */
+	la	t0, mpva
+	sub	t0, t0, s9
+	li	t1, KERNBASE
+	add	t0, t0, t1
+	csrw	stvec, t0
+
+	/* Set page tables base register */
+	la	s2, pagetable_l1
+	srli	s2, s2, PAGE_SHIFT
+	li	t0, SATP_MODE_SV39
+	or	s2, s2, t0
+	sfence.vma
+	csrw	sptbr, s2
+mpva:
+	/* Setup supervisor trap vector */
+	la	t0, cpu_exception_handler
+	csrw	stvec, t0
+
+	/* Ensure sscratch is zero */
+	li	t0, 0
+	csrw	sscratch, t0
 	/*
 	 * Calculate the offset to __riscv_boot_ap
 	 * for current core, cpuid in a0.
@@ -299,20 +307,6 @@ ENTRY(mpentry)
 	lw	t1, 0(t0)
 	beqz	t1, 1b
 
-	/* Set page tables base register */
-	la	s2, pagetable_l1
-	li	t0, KERNBASE
-	sub	s2, s2, t0
-	add	s2, s2, s6
-	srli	s2, s2, PAGE_SHIFT
-	csrw	sptbr, s2
-
-	/* Setup stack pointer */
-	la	t0, secondary_stacks
-	li	t1, (PAGE_SIZE * KSTACK_PAGES)
-	mulw	t1, t1, a0
-	add	sp, t0, t1
-
 	call	init_secondary
 END(mpentry)
 #endif
diff --git a/sys/riscv/riscv/machdep.c b/sys/riscv/riscv/machdep.c
index 14ddbc989329..f093e02b3f3e 100644
--- a/sys/riscv/riscv/machdep.c
+++ b/sys/riscv/riscv/machdep.c
@@ -1,6 +1,6 @@
 /*-
  * Copyright (c) 2014 Andrew Turner
- * Copyright (c) 2015-2016 Ruslan Bukin <br@bsdpad.com>
+ * Copyright (c) 2015-2017 Ruslan Bukin <br@bsdpad.com>
  * All rights reserved.
  *
  * Portions of this software were developed by SRI International and the
@@ -105,6 +105,8 @@ int cold = 1;
 long realmem = 0;
 long Maxmem = 0;
 
+#define	DTB_SIZE_MAX	(1024 * 1024)
+
 #define	PHYSMAP_SIZE	(2 * (VM_PHYSSEG_MAX - 1))
 vm_paddr_t physmap[PHYSMAP_SIZE];
 u_int physmap_idx;
@@ -117,7 +119,6 @@ int64_t idcache_line_size;	/* The minimum cache line size */
 
 extern int *end;
 extern int *initstack_end;
-extern memory_block_info memory_info;
 
 struct pcpu *pcpup;
 
@@ -315,6 +316,9 @@ CTASSERT(sizeof(((struct trapframe *)0)->tf_s) ==
 CTASSERT(sizeof(((struct trapframe *)0)->tf_t) ==
     sizeof((struct reg *)0)->t);
 
+/* Support for FDT configurations only. */
+CTASSERT(FDT);
+
 int
 get_mcontext(struct thread *td, mcontext_t *mcp, int clear_ret)
 {
@@ -714,16 +718,13 @@ add_physmap_entry(uint64_t base, uint64_t length, vm_paddr_t *physmap,
 
 #ifdef FDT
 static void
-try_load_dtb(caddr_t kmdp)
+try_load_dtb(caddr_t kmdp, vm_offset_t dtbp)
 {
-	vm_offset_t dtbp;
 
 #if defined(FDT_DTB_STATIC)
 	dtbp = (vm_offset_t)&fdt_static_dtb;
-#else
-	/* TODO */
-	dtbp = (vm_offset_t)NULL;
 #endif
+
 	if (dtbp == (vm_offset_t)NULL) {
 		printf("ERROR loading DTB\n");
 		return;
@@ -803,9 +804,14 @@ fake_preload_metadata(struct riscv_bootparams *rvbp __unused)
 void
 initriscv(struct riscv_bootparams *rvbp)
 {
+	struct mem_region mem_regions[FDT_MEM_REGIONS];
+	vm_offset_t rstart, rend;
+	vm_offset_t s, e;
+	int mem_regions_sz;
 	vm_offset_t lastaddr;
 	vm_size_t kernlen;
 	caddr_t kmdp;
+	int i;
 
 	/* Set the module data location */
 	lastaddr = fake_preload_metadata(rvbp);
@@ -821,26 +827,34 @@ initriscv(struct riscv_bootparams *rvbp)
 	kern_envp = NULL;
 
 #ifdef FDT
-	try_load_dtb(kmdp);
+	try_load_dtb(kmdp, rvbp->dtbp_virt);
 #endif
 
 	/* Load the physical memory ranges */
 	physmap_idx = 0;
 
-#if 0
-	struct mem_region mem_regions[FDT_MEM_REGIONS];
-	int mem_regions_sz;
-	int i;
+#ifdef FDT
 	/* Grab physical memory regions information from device tree. */
 	if (fdt_get_mem_regions(mem_regions, &mem_regions_sz, NULL) != 0)
 		panic("Cannot get physical memory regions");
-	for (i = 0; i < mem_regions_sz; i++)
-		add_physmap_entry(mem_regions[i].mr_start,
-		    mem_regions[i].mr_size, physmap, &physmap_idx);
-#endif
 
-	add_physmap_entry(memory_info.base, memory_info.size,
-	    physmap, &physmap_idx);
+	s = rvbp->dtbp_phys;
+	e = s + DTB_SIZE_MAX;
+
+	for (i = 0; i < mem_regions_sz; i++) {
+		rstart = mem_regions[i].mr_start;
+		rend = (mem_regions[i].mr_start + mem_regions[i].mr_size);
+
+		if ((rstart < s) && (rend > e)) {
+			/* Exclude DTB region. */
+			add_physmap_entry(rstart, (s - rstart), physmap, &physmap_idx);
+			add_physmap_entry(e, (rend - e), physmap, &physmap_idx);
+		} else {
+			add_physmap_entry(mem_regions[i].mr_start,
+			    mem_regions[i].mr_size, physmap, &physmap_idx);
+		}
+	}
+#endif
 
 	/* Set the pcpu data, this is needed by pmap_bootstrap */
 	pcpup = &__pcpu[0];
@@ -858,7 +872,7 @@ initriscv(struct riscv_bootparams *rvbp)
 
 	/* Bootstrap enough of pmap to enter the kernel proper */
 	kernlen = (lastaddr - KERNBASE);
-	pmap_bootstrap(rvbp->kern_l1pt, memory_info.base, kernlen);
+	pmap_bootstrap(rvbp->kern_l1pt, mem_regions[0].mr_start, kernlen);
 
 	cninit();
 
@@ -866,7 +880,7 @@ initriscv(struct riscv_bootparams *rvbp)
 
 	/* set page table base register for thread0 */
 	thread0.td_pcb->pcb_l1addr = \
-	    (rvbp->kern_l1pt - KERNBASE + memory_info.base);
+	    (rvbp->kern_l1pt - KERNBASE + rvbp->kern_phys);
 
 	msgbufinit(msgbufp, msgbufsize);
 	mutex_init();
diff --git a/sys/riscv/riscv/nexus.c b/sys/riscv/riscv/nexus.c
index 75e16b881f16..61c1c2f51be2 100644
--- a/sys/riscv/riscv/nexus.c
+++ b/sys/riscv/riscv/nexus.c
@@ -156,6 +156,8 @@ nexus_attach(device_t dev)
 	if (rman_init(&irq_rman) || rman_manage_region(&irq_rman, 0, ~0))
 		panic("nexus_attach irq_rman");
 
+	nexus_add_child(dev, 8, "timer", 0);
+	nexus_add_child(dev, 9, "rcons", 0);
 	nexus_add_child(dev, 10, "ofwbus", 0);
 
 	bus_generic_probe(dev);
diff --git a/sys/riscv/riscv/pmap.c b/sys/riscv/riscv/pmap.c
index af8806d8eead..8011ced6b662 100644
--- a/sys/riscv/riscv/pmap.c
+++ b/sys/riscv/riscv/pmap.c
@@ -13,7 +13,7 @@
  * All rights reserved.
  * Copyright (c) 2014 The FreeBSD Foundation
  * All rights reserved.
- * Copyright (c) 2015-2016 Ruslan Bukin <br@bsdpad.com>
+ * Copyright (c) 2015-2017 Ruslan Bukin <br@bsdpad.com>
  * All rights reserved.
  *
  * This code is derived from software contributed to Berkeley by
@@ -596,8 +596,10 @@ pmap_bootstrap(vm_offset_t l1pt, vm_paddr_t kernstart, vm_size_t kernlen)
 			min_pa = physmap[i];
 		if (physmap[i + 1] > max_pa)
 			max_pa = physmap[i + 1];
-		break;
 	}
+	printf("physmap_idx %lx\n", physmap_idx);
+	printf("min_pa %lx\n", min_pa);
+	printf("max_pa %lx\n", max_pa);
 
 	/* Create a direct map region early so we can use it for pa -> va */
 	pmap_bootstrap_dmap(l1pt, min_pa, max_pa);
@@ -771,7 +773,7 @@ pmap_invalidate_page(pmap_t pmap, vm_offset_t va)
 	/* TODO */
 
 	sched_pin();
-	__asm __volatile("sfence.vm");
+	__asm __volatile("sfence.vma %0" :: "r" (va) : "memory");
 	sched_unpin();
 }
 
@@ -782,7 +784,7 @@ pmap_invalidate_range(pmap_t pmap, vm_offset_t sva, vm_offset_t eva)
 	/* TODO */
 
 	sched_pin();
-	__asm __volatile("sfence.vm");
+	__asm __volatile("sfence.vma");
 	sched_unpin();
 }
 
@@ -793,7 +795,7 @@ pmap_invalidate_all(pmap_t pmap)
 	/* TODO */
 
 	sched_pin();
-	__asm __volatile("sfence.vm");
+	__asm __volatile("sfence.vma");
 	sched_unpin();
 }
 
@@ -3181,12 +3183,15 @@ void
 pmap_activate(struct thread *td)
 {
 	pmap_t pmap;
+	uint64_t reg;
 
 	critical_enter();
 	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 >> PAGE_SHIFT));
+	reg = SATP_MODE_SV39;
+	reg |= (td->td_pcb->pcb_l1addr >> PAGE_SHIFT);
+	__asm __volatile("csrw sptbr, %0" :: "r"(reg));
 
 	pmap_invalidate_all(pmap);
 	critical_exit();
diff --git a/sys/riscv/riscv/riscv_console.c b/sys/riscv/riscv/riscv_console.c
index d59ed59c18ae..df6aba155e11 100644
--- a/sys/riscv/riscv/riscv_console.c
+++ b/sys/riscv/riscv/riscv_console.c
@@ -1,5 +1,5 @@
 /*-
- * Copyright (c) 2015-2016 Ruslan Bukin <br@bsdpad.com>
+ * Copyright (c) 2015-2017 Ruslan Bukin <br@bsdpad.com>
  * All rights reserved.
  *
  * Portions of this software were developed by SRI International and the
@@ -68,11 +68,6 @@ __FBSDID("$FreeBSD$");
 #include <machine/vmparam.h>
 #include <machine/sbi.h>
 
-static struct resource_spec rcons_spec[] = {
-	{ SYS_RES_IRQ,		0,	RF_ACTIVE | RF_SHAREABLE},
-	{ -1, 0 }
-};
-
 /* bus softc */
 struct rcons_softc {
 	struct resource		*res[1];
@@ -110,17 +105,6 @@ static cn_ungrab_t	riscv_cnungrab;
 CONSOLE_DRIVER(riscv);
 
 #define	MAX_BURST_LEN		1
-#define	QUEUE_SIZE		256
-
-struct queue_entry {
-	uint64_t data;
-	uint64_t used;
-	struct queue_entry *next;
-};
-
-struct queue_entry cnqueue[QUEUE_SIZE];
-struct queue_entry *entry_last;
-struct queue_entry *entry_served;
 
 static void
 riscv_putc(int c)
@@ -195,21 +179,8 @@ riscv_cnprobe(struct consdev *cp)
 static void
 riscv_cninit(struct consdev *cp)
 {
-	int i;
 
 	strcpy(cp->cn_name, "rcons");
-
-	for (i = 0; i < QUEUE_SIZE; i++) {
-		if (i == (QUEUE_SIZE - 1))
-			cnqueue[i].next = &cnqueue[0];
-		else
-			cnqueue[i].next = &cnqueue[i+1];
-		cnqueue[i].data = 0;
-		cnqueue[i].used = 0;
-	}
-
-	entry_last = &cnqueue[0];
-	entry_served = &cnqueue[0];
 }
 
 static void
@@ -233,7 +204,6 @@ riscv_cnungrab(struct consdev *cp)
 static int
 riscv_cngetc(struct consdev *cp)
 {
-	uint8_t data;
 	int ch;
 
 #if defined(KDB)
@@ -245,26 +215,17 @@ riscv_cngetc(struct consdev *cp)
 	if (kdb_active) {
 		ch = sbi_console_getchar();
 		while (ch) {
-			entry_last->data = ch;
-			entry_last->used = 1;
-			entry_last = entry_last->next;
-
 			ch = sbi_console_getchar();
 		}
 	}
 #endif
 
-	if (entry_served->used == 1) {
-		data = entry_served->data;
-		entry_served->used = 0;
-		entry_served = entry_served->next;
-		ch = (data & 0xff);
-		if (ch > 0 && ch < 0xff) {
+	ch = sbi_console_getchar();
+	if (ch > 0 && ch < 0xff) {
 #if defined(KDB)
-			kdb_alt_break(ch, &alt_break_state);
+		kdb_alt_break(ch, &alt_break_state);
 #endif
-			return (ch);
-		}
+		return (ch);
 	}
 
 	return (-1);
@@ -279,34 +240,12 @@ riscv_cnputc(struct consdev *cp, int c)
 
 /* Bus interface */
 
-static int
-rcons_intr(void *arg)
-{
-	int c;
-
-	c = sbi_console_getchar();
-	if (c > 0 && c < 0xff) {
-		entry_last->data = c;
-		entry_last->used = 1;
-		entry_last = entry_last->next;
-	}
-
-	csr_clear(sip, SIP_SSIP);
-
-	return (FILTER_HANDLED);
-}
-
 static int
 rcons_probe(device_t dev)
 {
 
-	if (!ofw_bus_status_okay(dev))
-		return (ENXIO);
-
-	if (!ofw_bus_is_compatible(dev, "riscv,console"))
-		return (ENXIO);
-
 	device_set_desc(dev, "RISC-V console");
+
 	return (BUS_PROBE_DEFAULT);
 }
 
@@ -314,30 +253,17 @@ static int
 rcons_attach(device_t dev)
 {
 	struct rcons_softc *sc;
-	int error;
+
+	if (device_get_unit(dev) != 0)
+		return (ENXIO);
 
 	sc = device_get_softc(dev);
 	sc->dev = dev;
 
-	if (bus_alloc_resources(dev, rcons_spec, sc->res)) {
-		device_printf(dev, "could not allocate resources\n");
-		return (ENXIO);
-	}
-
-	/* Setup IRQs handler */
-	error = bus_setup_intr(dev, sc->res[0], INTR_TYPE_CLK,
-	    rcons_intr, NULL, sc, &sc->ihl[0]);
-	if (error) {
-		device_printf(dev, "Unable to alloc int resource.\n");
-		return (ENXIO);
-	}
-
 	csr_set(sie, SIE_SSIE);
 
 	bus_generic_attach(sc->dev);
 
-	sbi_console_getchar();
-
 	return (0);
 }
 
@@ -356,4 +282,4 @@ static driver_t rcons_driver = {
 
 static devclass_t rcons_devclass;
 
-DRIVER_MODULE(rcons, simplebus, rcons_driver, rcons_devclass, 0, 0);
+DRIVER_MODULE(rcons, nexus, rcons_driver, rcons_devclass, 0, 0);
diff --git a/sys/riscv/riscv/sbi.S b/sys/riscv/riscv/sbi.S
deleted file mode 100644
index b5b2916e0f4f..000000000000
--- a/sys/riscv/riscv/sbi.S
+++ /dev/null
@@ -1,52 +0,0 @@
-/*-
- * Copyright (c) 2016 Ruslan Bukin <br@bsdpad.com>
- * All rights reserved.
- *
- * Portions of this software were developed by SRI International and the
- * University of Cambridge Computer Laboratory under DARPA/AFRL contract
- * FA8750-10-C-0237 ("CTSRD"), as part of the DARPA CRASH research programme.
- *
- * Portions of this software were developed by the University of Cambridge
- * Computer Laboratory as part of the CTSRD Project, with support from the
- * UK Higher Education Innovation Fund (HEIF).
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in the
- *    documentation and/or other materials provided with the distribution.
- *
- * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
- * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
- * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
- * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
- * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
- * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
- * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
- * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
- * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
- * SUCH DAMAGE.
- */
-
-#include <machine/asm.h>
-__FBSDID("$FreeBSD$");
-
-.globl sbi_hart_id; sbi_hart_id = -2048
-.globl sbi_num_harts; sbi_num_harts = -2032
-.globl sbi_query_memory; sbi_query_memory = -2016
-.globl sbi_console_putchar; sbi_console_putchar = -2000
-.globl sbi_console_getchar; sbi_console_getchar = -1984
-.globl sbi_send_ipi; sbi_send_ipi = -1952
-.globl sbi_clear_ipi; sbi_clear_ipi = -1936
-.globl sbi_timebase; sbi_timebase = -1920
-.globl sbi_shutdown; sbi_shutdown = -1904
-.globl sbi_set_timer; sbi_set_timer = -1888
-.globl sbi_mask_interrupt; sbi_mask_interrupt = -1872
-.globl sbi_unmask_interrupt; sbi_unmask_interrupt = -1856
-.globl sbi_remote_sfence_vm; sbi_remote_sfence_vm = -1840
-.globl sbi_remote_sfence_vm_range; sbi_remote_sfence_vm_range = -1824
-.globl sbi_remote_fence_i; sbi_remote_fence_i = -1808
diff --git a/sys/riscv/riscv/swtch.S b/sys/riscv/riscv/swtch.S
index 5cc1edd69a02..8753fddfb728 100644
--- a/sys/riscv/riscv/swtch.S
+++ b/sys/riscv/riscv/swtch.S
@@ -1,5 +1,5 @@
 /*-
- * Copyright (c) 2015-2016 Ruslan Bukin <br@bsdpad.com>
+ * Copyright (c) 2015-2017 Ruslan Bukin <br@bsdpad.com>
  * All rights reserved.
  *
  * Portions of this software were developed by SRI International and the
@@ -163,16 +163,18 @@ ENTRY(cpu_throw)
 	ld	x13, TD_PCB(a1)
 	sd	x13, PC_CURPCB(gp)
 
-	sfence.vm
+	sfence.vma
 
 	/* Switch to the new pmap */
 	ld	t0, PCB_L1ADDR(x13)
 	srli	t0, t0, PAGE_SHIFT
+	li	t1, SATP_MODE_SV39
+	or	t0, t0, t1
 	csrw	sptbr, t0
 
 	/* TODO: Invalidate the TLB */
 
-	sfence.vm
+	sfence.vma
 
 	/* Load registers */
 	ld	ra, (PCB_RA)(x13)
@@ -279,16 +281,18 @@ ENTRY(cpu_switch)
 	 * to a user process.
 	 */
 
-	sfence.vm
+	sfence.vma
 
 	/* Switch to the new pmap */
 	ld	t0, PCB_L1ADDR(x13)
 	srli	t0, t0, PAGE_SHIFT
+	li	t1, SATP_MODE_SV39
+	or	t0, t0, t1
 	csrw	sptbr, t0
 
 	/* TODO: Invalidate the TLB */
 
-	sfence.vm
+	sfence.vma
 
 	/* Release the old thread */
 	sd	a2, TD_LOCK(a0)
diff --git a/sys/riscv/riscv/timer.c b/sys/riscv/riscv/timer.c
index 7876bbd5a8e9..0308ca50e6c8 100644
--- a/sys/riscv/riscv/timer.c
+++ b/sys/riscv/riscv/timer.c
@@ -1,5 +1,5 @@
 /*-
- * Copyright (c) 2015-2016 Ruslan Bukin <br@bsdpad.com>
+ * Copyright (c) 2015-2017 Ruslan Bukin <br@bsdpad.com>
  * All rights reserved.
  *
  * Portions of this software were developed by SRI International and the
@@ -61,66 +61,56 @@ __FBSDID("$FreeBSD$");
 #include <machine/trap.h>
 #include <machine/sbi.h>
 
-#include <dev/fdt/fdt_common.h>
-#include <dev/ofw/openfirm.h>
-#include <dev/ofw/ofw_bus.h>
-#include <dev/ofw/ofw_bus_subr.h>
-
-#define	DEFAULT_FREQ	1000000
+#define	DEFAULT_FREQ	10000000
 
 #define	TIMER_COUNTS		0x00
 #define	TIMER_MTIMECMP(cpu)	(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[3];
-	bus_space_tag_t		bst;
-	bus_space_handle_t	bsh;
-	bus_space_tag_t		bst_timecmp;
-	bus_space_handle_t	bsh_timecmp;
+struct riscv_timer_softc {
 	void			*ih;
 	uint32_t		clkfreq;
 	struct eventtimer	et;
+	int			intr_rid;
+	struct resource		*intr_res;
 };
 
-static struct riscv_tmr_softc *riscv_tmr_sc = NULL;
+static struct riscv_timer_softc *riscv_timer_sc = NULL;
 
-static struct resource_spec timer_spec[] = {
-	{ SYS_RES_MEMORY,	0,	RF_ACTIVE },
-	{ SYS_RES_MEMORY,	1,	RF_ACTIVE },
-	{ SYS_RES_IRQ,		0,	RF_ACTIVE },
-	{ -1, 0 }
-};
+static timecounter_get_t riscv_timer_get_timecount;
 
-static timecounter_get_t riscv_tmr_get_timecount;
-
-static struct timecounter riscv_tmr_timecount = {
+static struct timecounter riscv_timer_timecount = {
 	.tc_name           = "RISC-V Timecounter",
-	.tc_get_timecount  = riscv_tmr_get_timecount,
+	.tc_get_timecount  = riscv_timer_get_timecount,
 	.tc_poll_pps       = NULL,
 	.tc_counter_mask   = ~0u,
 	.tc_frequency      = 0,
 	.tc_quality        = 1000,
 };
 
+static inline uint64_t
+get_cycles(void)
+{
+	uint64_t cycles;
+
+	__asm __volatile("rdtime %0" : "=r" (cycles));
+
+	return (cycles);
+}
+
 static long
-get_counts(struct riscv_tmr_softc *sc)
+get_counts(struct riscv_timer_softc *sc)
 {
 	uint64_t counts;
 
-	counts = READ8(sc, TIMER_COUNTS);
+	counts = get_cycles();
 
 	return (counts);
 }
 
 static unsigned
-riscv_tmr_get_timecount(struct timecounter *tc)
+riscv_timer_get_timecount(struct timecounter *tc)
 {
-	struct riscv_tmr_softc *sc;
+	struct riscv_timer_softc *sc;
 
 	sc = tc->tc_priv;
 
@@ -128,22 +118,14 @@ riscv_tmr_get_timecount(struct timecounter *tc)
 }
 
 static int
-riscv_tmr_start(struct eventtimer *et, sbintime_t first, sbintime_t period)
+riscv_timer_start(struct eventtimer *et, sbintime_t first, sbintime_t period)
 {
-	struct riscv_tmr_softc *sc;
 	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);
-		bus_space_write_8(sc->bst_timecmp, sc->bsh_timecmp,
-		    TIMER_MTIMECMP(cpu), counts);
+		sbi_set_timer(get_cycles() + counts);
 		csr_set(sie, SIE_STIE);
-		sbi_set_timer(counts);
 
 		return (0);
 	}
@@ -153,11 +135,8 @@ riscv_tmr_start(struct eventtimer *et, sbintime_t first, sbintime_t period)
 }
 
 static int
-riscv_tmr_stop(struct eventtimer *et)
+riscv_timer_stop(struct eventtimer *et)
 {
-	struct riscv_tmr_softc *sc;
-
-	sc = (struct riscv_tmr_softc *)et->et_priv;
 
 	/* TODO */
 
@@ -165,11 +144,11 @@ riscv_tmr_stop(struct eventtimer *et)
 }
 
 static int
-riscv_tmr_intr(void *arg)
+riscv_timer_intr(void *arg)
 {
-	struct riscv_tmr_softc *sc;
+	struct riscv_timer_softc *sc;
 
-	sc = (struct riscv_tmr_softc *)arg;
+	sc = (struct riscv_timer_softc *)arg;
 
 	csr_clear(sip, SIP_STIP);
 
@@ -180,74 +159,55 @@ riscv_tmr_intr(void *arg)
 }
 
 static int
-riscv_tmr_fdt_probe(device_t dev)
+riscv_timer_probe(device_t dev)
 {
 
-	if (!ofw_bus_status_okay(dev))
-		return (ENXIO);
+	device_set_desc(dev, "RISC-V Timer");
 
-	if (ofw_bus_is_compatible(dev, "riscv,timer")) {
-		device_set_desc(dev, "RISC-V Timer");
-		return (BUS_PROBE_DEFAULT);
-	}
-
-	return (ENXIO);
+	return (BUS_PROBE_DEFAULT);
 }
 
 static int
-riscv_tmr_attach(device_t dev)
+riscv_timer_attach(device_t dev)
 {
-	struct riscv_tmr_softc *sc;
-	phandle_t node;
-	pcell_t clock;
+	struct riscv_timer_softc *sc;
 	int error;
 
 	sc = device_get_softc(dev);
-	if (riscv_tmr_sc)
+	if (riscv_timer_sc)
 		return (ENXIO);
 
-	/* Get the base clock frequency */
-	node = ofw_bus_get_node(dev);
-	if (node > 0) {
-		error = OF_getprop(node, "clock-frequency", &clock,
-		    sizeof(clock));
-		if (error > 0) {
-			sc->clkfreq = fdt32_to_cpu(clock);
-		}
-	}
-
-	if (sc->clkfreq == 0)
-		sc->clkfreq = DEFAULT_FREQ;
+	if (device_get_unit(dev) != 0)
+		return ENXIO;
 
+	sc->clkfreq = DEFAULT_FREQ;
 	if (sc->clkfreq == 0) {
 		device_printf(dev, "No clock frequency specified\n");
 		return (ENXIO);
 	}
 
-	if (bus_alloc_resources(dev, timer_spec, sc->res)) {
-		device_printf(dev, "could not allocate resources\n");
+	riscv_timer_sc = sc;
+
+	sc->intr_rid = 0;
+	sc->intr_res = bus_alloc_resource(dev,
+	    SYS_RES_IRQ, &sc->intr_rid, IRQ_TIMER_SUPERVISOR,
+	    IRQ_TIMER_SUPERVISOR, 1, RF_ACTIVE);
+	if (sc->intr_res == NULL) {
+		device_printf(dev, "failed to allocate irq\n");
 		return (ENXIO);
 	}
 
-	/* Memory interface */
-	sc->bst = rman_get_bustag(sc->res[0]);
-	sc->bsh = rman_get_bushandle(sc->res[0]);
-	sc->bst_timecmp = rman_get_bustag(sc->res[1]);
-	sc->bsh_timecmp = rman_get_bushandle(sc->res[1]);
-
-	riscv_tmr_sc = sc;
-
 	/* Setup IRQs handler */
-	error = bus_setup_intr(dev, sc->res[2], INTR_TYPE_CLK,
-	    riscv_tmr_intr, NULL, sc, &sc->ih);
+	error = bus_setup_intr(dev, sc->intr_res, INTR_TYPE_CLK,
+	    riscv_timer_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);
+	riscv_timer_timecount.tc_frequency = sc->clkfreq;
+	riscv_timer_timecount.tc_priv = sc;
+	tc_init(&riscv_timer_timecount);
 
 	sc->et.et_name = "RISC-V Eventtimer";
 	sc->et.et_flags = ET_FLAGS_ONESHOT | ET_FLAGS_PERCPU;
@@ -256,31 +216,29 @@ riscv_tmr_attach(device_t dev)
 	sc->et.et_frequency = sc->clkfreq;
 	sc->et.et_min_period = (0x00000002LLU << 32) / sc->et.et_frequency;
 	sc->et.et_max_period = (0xfffffffeLLU << 32) / sc->et.et_frequency;
-	sc->et.et_start = riscv_tmr_start;
-	sc->et.et_stop = riscv_tmr_stop;
+	sc->et.et_start = riscv_timer_start;
+	sc->et.et_stop = riscv_timer_stop;
 	sc->et.et_priv = sc;
 	et_register(&sc->et);
 
 	return (0);
 }
 
-static device_method_t riscv_tmr_fdt_methods[] = {
-	DEVMETHOD(device_probe,		riscv_tmr_fdt_probe),
-	DEVMETHOD(device_attach,	riscv_tmr_attach),
+static device_method_t riscv_timer_methods[] = {
+	DEVMETHOD(device_probe,		riscv_timer_probe),
+	DEVMETHOD(device_attach,	riscv_timer_attach),
 	{ 0, 0 }
 };
 
-static driver_t riscv_tmr_fdt_driver = {
+static driver_t riscv_timer_driver = {
 	"timer",
-	riscv_tmr_fdt_methods,
-	sizeof(struct riscv_tmr_softc),
+	riscv_timer_methods,
+	sizeof(struct riscv_timer_softc),
 };
 
-static devclass_t riscv_tmr_fdt_devclass;
+static devclass_t riscv_timer_devclass;
 
-EARLY_DRIVER_MODULE(timer, simplebus, riscv_tmr_fdt_driver, riscv_tmr_fdt_devclass,
-    0, 0, BUS_PASS_TIMER + BUS_PASS_ORDER_MIDDLE);
-EARLY_DRIVER_MODULE(timer, ofwbus, riscv_tmr_fdt_driver, riscv_tmr_fdt_devclass,
+EARLY_DRIVER_MODULE(timer, nexus, riscv_timer_driver, riscv_timer_devclass,
     0, 0, BUS_PASS_TIMER + BUS_PASS_ORDER_MIDDLE);
 
 void
@@ -293,7 +251,7 @@ DELAY(int usec)
 	 * Check the timers are setup, if not just
 	 * use a for loop for the meantime
 	 */
-	if (riscv_tmr_sc == NULL) {
+	if (riscv_timer_sc == NULL) {
 		for (; usec > 0; usec--)
 			for (counts = 200; counts > 0; counts--)
 				/*
@@ -305,7 +263,7 @@ DELAY(int usec)
 	}
 
 	/* Get the number of times to count */
-	counts_per_usec = ((riscv_tmr_timecount.tc_frequency / 1000000) + 1);
+	counts_per_usec = ((riscv_timer_timecount.tc_frequency / 1000000) + 1);
 
 	/*
 	 * Clamp the timeout at a maximum value (about 32 seconds with
@@ -318,10 +276,10 @@ DELAY(int usec)
 	else
 		counts = usec * counts_per_usec;
 
-	first = get_counts(riscv_tmr_sc);
+	first = get_counts(riscv_timer_sc);
 
 	while (counts > 0) {
-		last = get_counts(riscv_tmr_sc);
+		last = get_counts(riscv_timer_sc);
 		counts -= (int64_t)(last - first);
 		first = last;
 	}
diff --git a/sys/riscv/riscv/trap.c b/sys/riscv/riscv/trap.c
index 5af0c014e3dc..002e10e82fe0 100644
--- a/sys/riscv/riscv/trap.c
+++ b/sys/riscv/riscv/trap.c
@@ -1,5 +1,5 @@
 /*-
- * Copyright (c) 2015-2016 Ruslan Bukin <br@bsdpad.com>
+ * Copyright (c) 2015-2017 Ruslan Bukin <br@bsdpad.com>
  * All rights reserved.
  *
  * Portions of this software were developed by SRI International and the
@@ -212,7 +212,8 @@ data_abort(struct trapframe *frame, int lower)
 
 	va = trunc_page(sbadaddr);
 
-	if (frame->tf_scause == EXCP_FAULT_STORE) {
+	if ((frame->tf_scause == EXCP_FAULT_STORE) ||
+	    (frame->tf_scause == EXCP_STORE_PAGE_FAULT)) {
 		ftype = (VM_PROT_READ | VM_PROT_WRITE);
 	} else {
 		ftype = (VM_PROT_READ);
@@ -296,6 +297,8 @@ do_trap_supervisor(struct trapframe *frame)
 	case EXCP_FAULT_LOAD:
 	case EXCP_FAULT_STORE:
 	case EXCP_FAULT_FETCH:
+	case EXCP_STORE_PAGE_FAULT:
+	case EXCP_LOAD_PAGE_FAULT:
 		data_abort(frame, 0);
 		break;
 	case EXCP_BREAKPOINT:
@@ -354,6 +357,9 @@ do_trap_user(struct trapframe *frame)
 	case EXCP_FAULT_LOAD:
 	case EXCP_FAULT_STORE:
 	case EXCP_FAULT_FETCH:
+	case EXCP_STORE_PAGE_FAULT:
+	case EXCP_LOAD_PAGE_FAULT:
+	case EXCP_INST_PAGE_FAULT:
 		data_abort(frame, 1);
 		break;
 	case EXCP_USER_ECALL:
@@ -381,7 +387,7 @@ do_trap_user(struct trapframe *frame)
 		break;
 	default:
 		dump_regs(frame);
-		panic("Unknown userland exception %x badaddr %lx\n",
+		panic("Unknown userland exception %x, badaddr %lx\n",
 			exception, frame->tf_sbadaddr);
 	}
 }
diff --git a/sys/riscv/riscv/vm_machdep.c b/sys/riscv/riscv/vm_machdep.c
index c5c62fe24db2..73bb5aae7b6c 100644
--- a/sys/riscv/riscv/vm_machdep.c
+++ b/sys/riscv/riscv/vm_machdep.c
@@ -1,5 +1,5 @@
 /*-
- * Copyright (c) 2015-2016 Ruslan Bukin <br@bsdpad.com>
+ * Copyright (c) 2015-2017 Ruslan Bukin <br@bsdpad.com>
  * All rights reserved.
  *
  * Portions of this software were developed by SRI International and the
@@ -88,6 +88,7 @@ cpu_fork(struct thread *td1, struct proc *p2, struct thread *td2, int flags)
 	tf->tf_a[0] = 0;
 	tf->tf_a[1] = 0;
 	tf->tf_sstatus |= (SSTATUS_SPIE); /* Enable interrupts. */
+	tf->tf_sstatus |= (SSTATUS_SUM); /* Supervisor can access userspace. */
 	tf->tf_sstatus &= ~(SSTATUS_SPP); /* User mode. */
 
 	td2->td_frame = tf;
@@ -179,7 +180,9 @@ void
 cpu_set_upcall(struct thread *td, void (*entry)(void *), void *arg,
 	stack_t *stack)
 {
-	struct trapframe *tf = td->td_frame;
+	struct trapframe *tf;
+
+	tf = td->td_frame;
 
 	tf->tf_sp = STACKALIGN((uintptr_t)stack->ss_sp + stack->ss_size);
 	tf->tf_sepc = (register_t)entry;