diff --git a/Rakefile b/Rakefile
index a105f77..9b158fe 100644
--- a/Rakefile
+++ b/Rakefile
@@ -144,11 +144,13 @@ namespace :cross do
     cwd = source_directory.dirname + 'build-binutils'
     cwd.mkpath
     options.rootfs.mkpath
+    options.sysroot.mkpath
 
     env = ENV.slice 'CC', 'CXX'
     configure_options = [
       "--prefix=#{options.rootfs.realpath}",
       "--target=#{options.target}",
+      "--with-sysroot=#{options.sysroot.realpath}",
       '--disable-nls',
       '--enable-gprofng=no',
       '--disable-werror',
@@ -167,7 +169,6 @@ namespace :cross do
     cwd = source_directory.dirname + 'build-gcc'
     cwd.mkpath
     options.rootfs.mkpath
-    options.sysroot.mkpath
 
     sh 'contrib/download_prerequisites', chdir: source_directory.to_path
     configure_options = options.configuration('-with-') + [
@@ -354,10 +355,10 @@ rule '.elna.o' => ->(match) {
 } do |t|
   options = BuildTarget.new relative_from_tmp(t.name).first
   compiler = options.rootfs + "bin/#{options.target}-gcc"
-  flags = ['-O2', '-g3', '-fno-stack-protector'] + options.configuration('m')
   sources = t.prerequisites.select { |prerequisite| File.extname(prerequisite) == '.elna' }
 
-  system compiler.to_s, *flags, '-c', '-o', t.name, *sources, exception: true
+  system compiler.to_s, '-O2', '-g3', '-fno-stack-protector', *options.configuration('m'),
+    '-c', '-o', t.name, *sources, exception: true
 end
 
 rule 'kernel.elf' => ->(match) {
@@ -373,17 +374,17 @@ rule 'kernel.elf' => ->(match) {
   objects << "arch/#{instruction_set}/kernel.ld"
 } do |t|
   options = BuildTarget.new relative_from_tmp(t.name).first
-  compiler = options.rootfs + "bin/#{options.target}-gcc"
-  flags = options.configuration('m')
+  linker = options.rootfs + options.target + 'bin/ld'
   objects, linker_script = t.prerequisites.partition { |prerequisite| File.extname(prerequisite) == '.o' }
 
-  system compiler.to_s, '-nostdlib', '-T', *linker_script, '-o', t.name, *objects, exception: true
+  system linker.to_s, '-nostdlib', '-T', *linker_script, '-o', t.name, *objects, exception: true
 end
 
 task default: 'build/riscv32/kernel.elf'
 task :default do |t|
   QEMU = 'qemu-system-riscv32'
 
-  system QEMU, '-machine', 'virt', '-bios', 'default', '-nographic', '-serial', 'mon:stdio', '--no-reboot',
-    '-kernel', t.prerequisites.last, exception: true
+  system QEMU, '-machine', 'virt', '-nographic', '-serial', 'mon:stdio', '--no-reboot',
+    '-bios', '../opensbi-1.7-rv-bin/share/opensbi/ilp32/generic/firmware/fw_jump.bin',
+    '-device', "loader,file=#{t.prerequisites.last}", exception: true
 end
diff --git a/arch/riscv/kernel.ld b/arch/riscv/kernel.ld
index a416e6a..474e87a 100644
--- a/arch/riscv/kernel.ld
+++ b/arch/riscv/kernel.ld
@@ -1,23 +1,20 @@
 OUTPUT_ARCH("riscv")
 OUTPUT_FORMAT("elf32-littleriscv")
-ENTRY(_entry)
+ENTRY(_start)
 
 PHDRS {
-	boot PT_LOAD FLAGS(5);
 	text PT_LOAD FLAGS(5);
+	data PT_LOAD FLAGS(6);
 }
 
 SECTIONS {
-	.text.boot 0x80200000 : {
+	_kernel_base = 0x80400000;
+	_kernel_virtual = 0xc0000000;
+	_kernel_offset = _kernel_virtual - _kernel_base;
+
+	.text _kernel_virtual : AT(ADDR(.text) - _kernel_offset) {
 		KEEP(*(.text.boot));
-		. = ALIGN(CONSTANT(MAXPAGESIZE));
-		ASSERT(. - ADDR(.text.boot) == 0x1000, "error: stub larger than one page");
-	} :boot
-
-	_kernel_offset = 0x3fdff000;
-
-	.text 0xc0000000 : AT(ADDR(.text) - _kernel_offset) {
-		PROVIDE(__executable_start = . - _kernel_offset);
+		ASSERT(. - ADDR(.text) <= 0x1000, "error: stub larger than one page");
 		*(.text .text.*);
 	} :text
 
@@ -33,10 +30,10 @@ SECTIONS {
 
 	.data : AT(ADDR(.data) - _kernel_offset) ALIGN(16) {
 		*(.data .data.*);
-	}
+	} :data
 
 	.bss (NOLOAD) : {
-		PROVIDE(__bss_start = . - _kernel_offset);
+		PROVIDE(__bss_start = .);
 		*(.bss .bss.* .sbss .sbss.*);
 	}
 
diff --git a/arch/riscv/kernel.s b/arch/riscv/kernel.s
index a188c8e..e004a17 100644
--- a/arch/riscv/kernel.s
+++ b/arch/riscv/kernel.s
@@ -1,10 +1,253 @@
-.global _entry
-
 .section .rodata
 
 panic_message: .asciz "\nPanic"
 .equ PANIC_MESSAGE_SIZE, . - panic_message
 
+.section .text.boot
+
+.type _start, @function
+.global _start
+_start:
+	mv s1, a1 # Save the device tree pointer.
+	auipc s2, 0 # Save the physical start of the text section.
+
+	# Set the stack pointer.
+	lui sp, %hi(end)
+	addi sp, sp, %lo(end)
+	li t0, 64 * 1024
+	add sp, sp, t0
+
+	mv a0, s2
+	mv a1, sp
+	jal imem_initialize
+	jal imem_mirror
+
+	la a0, __bss_start
+	la a1, end
+	call bzero
+
+	lui t0, %hi(trap_routine)
+	addi t0, t0, %lo(trap_routine)
+	csrw stvec, t0
+
+	mv a0, sp
+	mv a1, s1
+
+	lui t0, %hi(kernel_main)
+	jr t0, %lo(kernel_main)
+
+# Copies the identity mappings to the higher half.
+.type imem_mirror, @function
+imem_mirror:
+	csrr t0, satp
+	slli t0, t0, 12
+
+	li t3, 0x1000
+	add t1, t0, t3 # Original page table.
+	add t2, t1, t3 # Copied page table.
+
+	mv t3, t2
+
+.Limem_mirror_copy:
+	lw t4, (t1)
+	sw t4, (t3)
+
+	addi t3, t3, 4
+	addi t1, t1, 4
+	blt t1, t2, .Limem_mirror_copy
+
+	# Persist the copied page table in the root page table.
+	lui t4, %hi(_start)
+	srli t4, t4, 20 # VPN[1] * PTESIZE.
+	add t4, t0, t4 # a + VPN[1] * PTESIZE.
+	srli t3, t2, 2
+	ori t3, t3, 0b1
+	sw t3, (t4)
+
+	ret
+
+# Activates paging and creates root page table.
+# Allocates enough pages for the kernel and its stack.
+#
+# Parameters:
+# a0 - Physical start of the text section.
+# a1 - Page aligned address following the kernel (kernel end).
+.type imem_initialize, @function
+imem_initialize:
+	# Prologue.
+	addi sp, sp, -32
+	sw ra, 28(sp)
+	sw s0, 24(sp)
+	addi s0, sp, 32
+
+	sw a1, 20(sp)
+	sw s1, 16(sp)
+	sw s2, 12(sp)
+	sw s3, 8(sp)
+
+	li s1, 4096
+	add s1, a1, s1
+	# Set boundaries between segments.
+	lui s2, %hi(etext)
+	lui s3, %hi(_data)
+
+	# Set the root page since it is used by mapping functions,
+	# but don't activate paging before the segments are property mapped.
+	srli t0, a1, 12
+	csrw satp, t0
+
+	# Executable section.
+	# a0 is coming from the parameter.
+	addi a1, s2, %lo(etext)
+	mv a2, s1
+	li a3, 1
+	li a4, 0b1010
+	jal imem_map_range
+
+	# Read only section.
+	addi a0, s2, %lo(etext)
+	addi a1, s3, %lo(_data)
+	mv a2, s1
+	li a3, 1
+	li a4, 0b0010
+	jal imem_map_range
+
+	# Data section.
+	addi a0, s3, %lo(_data)
+	lw a1, 20(sp)
+	li t0, 4096 * 3
+	add a1, a1, t0
+	mv a2, s1
+	li a3, 1
+	li a4, 0b0110
+	jal imem_map_range
+
+	# Enable paging.
+	csrr t0, satp
+	li t1, 0x80000000
+	or t0, t0, t1
+
+	sfence.vma
+	csrw satp, t0
+	sfence.vma
+
+	lw s3, 8(sp)
+	lw s2, 12(sp)
+	lw s1, 16(sp)
+
+	# Epilogue.
+	lw ra, 28(sp)
+	lw s0, 24(sp)
+	addi sp, sp, 32
+	ret
+
+# Maps multiple pages of physical memory to the identical virtual memory.
+#
+# Parameters:
+# a0 - Start address.
+# a1 - End address.
+# a2 - Start of the free memory that can be used to create leaf page tables.
+# a3 - Page table level. If a3 is 0 a superpage is allocated, otherwise a normal page.
+# a4 - Flags.
+.type imem_map_range, @function
+imem_map_range:
+	# Prologue.
+	addi sp, sp, -32
+	sw ra, 28(sp)
+	sw s0, 24(sp)
+	addi s0, sp, 32
+
+	sw s1, 20(sp)
+	sw a2, 16(sp)
+	sw a3, 12(sp)
+	sw a4, 8(sp)
+
+	mv s1, a1
+
+.Limem_map_range_loop:
+	bge a0, s1, .Limem_map_range_end
+
+	mv a1, a0
+	lw a2, 16(sp)
+	lw a3, 12(sp)
+	lw a4, 8(sp)
+	jal imem_map_at
+
+	li t0, 4096
+	add a0, a0, t0
+
+	j .Limem_map_range_loop
+
+.Limem_map_range_end:
+	lw s1, 20(sp)
+
+	# Epilogue.
+	lw ra, 28(sp)
+	lw s0, 24(sp)
+	addi sp, sp, 32
+	ret
+
+# Maps a page of physical memory to the identical virtual memory.
+#
+# Parameters:
+# a0 - Corresponding virtual address.
+# a1 - Physical page address.
+# a2 - Free memory page that can be used to create a leaf page table.
+# a3 - Page table level. If a3 is 0 a superpage is allocated, otherwise a normal page.
+# a4 - Flags.
+#
+# Returns virtual page address in a0.
+.type imem_map_at, @function
+imem_map_at:
+	csrr t0, satp
+	slli t0, t0, 12 # Root table address.
+
+	li t4, 0xfffffc00
+	li t5, 0xffc # 10 bit * PTESIZE mask.
+	li t1, 20
+
+.Limem_map_at_loop:
+	# Multiply VPN[i] by 4 and add to the start address of the page table.
+	# This gives the physical address of the page table entry.
+	srl t2, a0, t1
+	and t2, t2, t5 # VPN[i] * PTESIZE.
+	add t2, t0, t2 # a + VPN[i] * PTESIZE.
+
+	beqz a3, .Limem_map_at_leaf
+
+	lw t3, (t2)
+	andi t6, t3, 0b1
+
+	beqz t6, .Limem_map_at_create
+
+	and t0, t3, t4
+	slli t0, t0, 2
+
+	j .Limem_map_at_next
+
+.Limem_map_at_create:
+	# Take a chunk of free memory and use it as the next page table.
+	# The beginning addres off the chunk is the base address (a) for the next level.
+	mv t0, a2
+	srli t3, t0, 2
+	ori t3, t3, 0b1
+	sw t3, (t2)
+
+.Limem_map_at_next:
+	# Calculate the next page table address for the next level.
+	addi t1, t1, -10
+	addi a3, a3, -1
+
+	j .Limem_map_at_loop
+
+.Limem_map_at_leaf:
+	srli t3, a1, 2 # Physical page number mapped to 12 bits of the page table entry.
+	ori a4, a4, 0b1
+	or t3, t3, a4 # Execute, write, read and valid.
+	sw t3, (t2)
+
+	ret
+
 .section .text
 
 .type kernel_main, @function
@@ -18,9 +261,6 @@ kernel_main:
 	sw a0, 20(sp)
 	sw a1, 16(sp)
 
-	la t0, kernel_entry
-	csrw stvec, t0
-
 	# Map flat device tree to the virtual memory.
 	lw a0, 16(sp)
 	li t0, 0xffc00000
@@ -45,8 +285,8 @@ kernel_main:
 	ret
 
 .balign 4
-.type kernel_entry, @function
-kernel_entry:
+.type trap_routine, @function
+trap_routine:
 	csrw sscratch, sp
 	addi sp, sp, -4 * 31
 
@@ -184,217 +424,3 @@ panic:
 	lw s0, 24(sp)
 	addi sp, sp, 32
 	ret
-
-.section .text.boot
-_entry:
-	# Set the stack pointer.
-	la t0, end
-	li t1, 64 * 1024
-	add sp, t0, t1
-
-	mv s1, a1
-
-	mv a0, sp
-	call imem_initialize
-
-	li s2, 0xc0000000
-	li s3, 0x80201000
-	li s4, 0x1000
-
-.rept 12
-	mv a0, s2
-	mv a1, s3
-	li t0, 8192
-	add a2, sp, t0
-	li a3, 1
-	li a4, 0b1110
-	call imem_map_at
-
-	add s2, s2, s4
-	add s3, s3, s4
-.endr
-
-	la a0, __bss_start
-	la a1, end
-	call bzero
-
-	mv a0, sp
-	mv a1, s1
-	tail kernel_main
-
-# Activates paging and creates root page table.
-# Allocates enough pages for the kernel and its stack.
-#
-# Parameters:
-# a0 - Page aligned address following the kernel (kernel end).
-.type imem_initialize, @function
-imem_initialize:
-	# Prologue.
-	addi sp, sp, -32
-	sw ra, 28(sp)
-	sw s0, 24(sp)
-	addi s0, sp, 32
-
-	sw a0, 20(sp)
-
-	srli t0, a0, 12
-	csrw satp, t0
-
-	la a0, .text.boot
-	mv a1, a0
-	lw a2, 20(sp)
-	li t0, 4096
-	add a2, a2, t0
-	li a3, 1
-	li a4, 0b1010
-	call imem_map_at
-
-	la a0, __executable_start
-	la a1, etext
-	lw a2, 20(sp)
-	li t0, 4096
-	add a2, a2, t0
-	li a3, 1
-	li a4, 0b1010
-	call imem_map_range
-
-	la a0, etext
-	la a1, _data
-	lw a2, 20(sp)
-	li t0, 4096
-	add a2, a2, t0
-	li a3, 1
-	li a4, 0b0010
-	call imem_map_range
-
-	la a0, _data
-	lw a1, 20(sp)
-	li t0, 4096
-	add a1, a1, t0
-	add a1, a1, t0
-	add a1, a1, t0
-	add a2, a1, t0
-	li a3, 1
-	li a4, 0b0110
-	call imem_map_range
-
-	csrr t0, satp
-	li t1, 0x80000000
-	or t0, t0, t1
-
-	sfence.vma
-	csrw satp, t0
-	sfence.vma
-
-	# Epilogue.
-	lw ra, 28(sp)
-	lw s0, 24(sp)
-	addi sp, sp, 32
-	ret
-
-# Maps multiple pages of physical memory to the identical virtual memory.
-#
-# Parameters:
-# a0 - Start address.
-# a1 - End address.
-# a2 - Start of the free memory that can be used to create leaf page tables.
-# a3 - Page table level. If a3 is 0 a superpage is allocated, otherwise a normal page.
-# a4 - Flags.
-.type imem_map_range, @function
-imem_map_range:
-	# Prologue.
-	addi sp, sp, -32
-	sw ra, 28(sp)
-	sw s0, 24(sp)
-	addi s0, sp, 32
-
-	sw s1, 20(sp)
-	sw a2, 16(sp)
-	sw a3, 12(sp)
-	sw a4, 8(sp)
-
-	mv s1, a1
-
-.Limem_map_range_loop:
-	bge a0, s1, .Limem_map_range_end
-
-	mv a1, a0
-	lw a2, 16(sp)
-	lw a3, 12(sp)
-	lw a4, 8(sp)
-	call imem_map_at
-
-	li t0, 4096
-	add a0, a0, t0
-
-	j .Limem_map_range_loop
-
-.Limem_map_range_end:
-	lw s1, 20(sp)
-
-	# Epilogue.
-	lw ra, 28(sp)
-	lw s0, 24(sp)
-	addi sp, sp, 32
-	ret
-
-# Maps a page of physical memory to the identical virtual memory.
-#
-# Parameters:
-# a0 - Physical page address.
-# a1 - Corresponding virtual address.
-# a2 - Free memory page that can be used to create a leaf page table.
-# a3 - Page table level. If a3 is 0 a superpage is allocated, otherwise a normal page.
-# a4 - Flags.
-#
-# Returns virtual page address in a0.
-.type imem_map_at, @function
-imem_map_at:
-	csrr t0, satp
-	slli t0, t0, 12 # Root table address.
-
-	li t4, 0xfffffc00
-	li t5, 0xffc # 10 bit * PTESIZE mask.
-	li t1, 20
-
-.Limem_map_at_loop:
-	# Multiply VPN[i] by 4 and add to the start address of the page table.
-	# This gives the physical address of the page table entry.
-	srl t2, a0, t1
-	and t2, t2, t5 # VPN[i] * PTESIZE.
-	add t2, t0, t2 # a + VPN[i] * PTESIZE.
-
-	beqz a3, .Limem_map_at_leaf
-
-	lw t3, (t2)
-	andi t6, t3, 0b1
-
-	beqz t6, .Limem_map_at_create
-
-	and t0, t3, t4
-	slli t0, t0, 2
-
-	j .Limem_map_at_next
-
-.Limem_map_at_create:
-	# Take a chunk of free memory and use it as the next page table.
-	# The beginning addres off the chunk is the base address (a) for the next level.
-	mv t0, a2
-	srli t3, t0, 2
-	ori t3, t3, 0b1
-	sw t3, (t2)
-
-.Limem_map_at_next:
-	# Calculate the next page table address for the next level.
-	addi t1, t1, -10
-	addi a3, a3, -1
-
-	j .Limem_map_at_loop
-
-.Limem_map_at_leaf:
-	srli t3, a1, 2 # Physical page number mapped to 12 bits of the page table entry.
-	ori a4, a4, 0b1
-	or t3, t3, a4 # Execute, write, read and valid.
-	sw t3, (t2)
-
-	ret