diff --git a/Rakefile b/Rakefile
old mode 100755
new mode 100644
diff --git a/arch/riscv/kernel.ld b/arch/riscv/kernel.ld
index 502dd14..7f34b36 100644
--- a/arch/riscv/kernel.ld
+++ b/arch/riscv/kernel.ld
@@ -8,30 +8,26 @@ PHDRS {
 }
 
 SECTIONS {
-    .text 0x80200000 : {
-		__kernel_base = .;
-        KEEP(*(.text.boot));
-        *(.text .text.*);
-    } :text
+	.text 0x80200000 : {
+		KEEP(*(.text.boot));
+		*(.text .text.*);
+	} :text
+	PROVIDE(estart = .);
 
-    .rodata : ALIGN(16) {
-        *(.rodata .rodata.*);
-    }
+	.rodata : ALIGN(16) {
+		*(.rodata .rodata.*);
+	}
 
-    .data : ALIGN(16) {
-        *(.data .data.*);
-    } :data
+	.data : ALIGN(16) {
+		*(.data .data.*);
+	} :data
 
-    .bss : ALIGN(16) {
-        __bss = .;
-        *(.bss .bss.* .sbss .sbss.*);
-        __bss_end = .;
-    }
-
-    . = ALIGN(4);
-    . += 128 * 1024; /* 128KB */
-    __stack_top = .;
+	.bss : ALIGN(16) {
+		__bss = .;
+		*(.bss .bss.* .sbss .sbss.*);
+		__bss_end = .;
+	}
 
 	. = ALIGN(CONSTANT(MAXPAGESIZE));
-	__free_ram = .;
+	PROVIDE(end = .);
 }
diff --git a/arch/riscv/kernel.s b/arch/riscv/kernel.s
index 08fc6ef..8ee06d7 100644
--- a/arch/riscv/kernel.s
+++ b/arch/riscv/kernel.s
@@ -19,6 +19,105 @@ bzero:
 .Lbzero_end:
 	ret
 
+# Maps a page of physical memory to the identical virtual memory.
+#
+# Parameters:
+# a0 - Physical page address.
+# a1 - Free memory page that can be used to create a leaf page table.
+# a2 - Page table level. If a2 is 0 a superpage is allocated, otherwise a normal page.
+#
+# 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 t1, 0xffc # 10 bit * PTESIZE mask.
+	srli t4, a0, 2 # Physical page number.
+
+	# Multiply VPN[1] by 4 and add to the start address of the page table.
+	# This gives the physical address of the page table entry.
+	srli t2, a0, 20
+	and t2, t2, t1 # VPN[1] * PTESIZE.
+	add t2, t0, t2 # a + VPN[1] * PTESIZE.
+
+	beqz a2, .Limem_map_at_super
+
+	srli t3, a1, 2
+	ori t3, t3, 0b1
+	sw t3, (t2)
+
+	srli t2, a0, 10
+	and t2, t2, t1 # VPN[0] * PTESIZE.
+	add t2, a1, t2 # a + VPN[0] * PTESIZE.
+
+	j .Limem_map_at_leaf
+
+.Limem_map_at_super:
+	# Calculate PPN[1] (10 most significant bits of the physical address
+	# and map it to 12 bits of the table page entry.
+	li t1, 0xfff00000
+	and t4, t4, t1
+
+.Limem_map_at_leaf:
+	ori t4, t4, 0b1111 # Execute, write, read and valid.
+	sw t4, (t2)
+	ret
+
+# Activates paging and creates root page table.
+# Allocates enough pages for the kernel and its stack.
+#
+# Parameters:
+#
+# a0 - Start (base) address of the kernel.
+# 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
+
+	li t0, 64 * 1024
+	add t0, a1, t0
+	sw t0, 20(sp)
+	sw a1, 16(sp)
+
+	srli t0, a1, 12
+	csrw satp, t0
+
+.Limem_initialize_loop:
+	lw t0, 20(sp)
+	lw a1, 16(sp)
+
+	bge a0, t0, .Limem_initialize_activate
+
+	li t0, 4096
+	add a1, a1, t0
+	li a2, 1
+	call imem_map_at
+
+	li t0, 4096
+	add a0, a0, t0
+
+	j .Limem_initialize_loop
+
+.Limem_initialize_activate:
+	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
+
 .type kernel_main, @function
 kernel_main:
 	# Prologue.
@@ -27,42 +126,21 @@ kernel_main:
 	sw s0, 24(sp)
 	addi s0, sp, 32
 
+	sw a0, 20(sp)
+	sw a1, 16(sp)
+
 	la t0, kernel_entry
 	csrw stvec, t0
 
-	mv a0, a1
+	# Map flat device tree to the virtual memory.
+	lw a0, 16(sp)
+	lw a1, 20(sp)
+	li a2, 0
+	call imem_map_at
+
+	lw a0, 16(sp)
 	call device_tree
 
-	# Prepare the kernel root memory table.
-	la a0, __kernel_base
-	srli a0, a0, 22
-	andi a0, a0, 0x3ff # VPN[1].
-
-	# Multiply VPN[1] by 4 and add to the start address of the page table.
-	# This gives the physical address of the page table entry.
-	la t0, __free_ram
-	slli a0, a0, 2
-	add a0, t0, a0
-
-	# Calculate PPN[1] (10 most significant bits of the physical address
-	# and map it to 12 bits of the table page entry.
-	la a1, __kernel_base
-	srli a1, a1, 2
-	li t0, 0xff300000
-	and a1, a1, t0
-	ori a1, a1, 0b1111
-
-	sw a1, (a0)	# Save the page entry.
-
-	la a0, __free_ram
-	srli a0, a0, 12
-	li t0, 0x80000000
-	or a0, a0, t0
-
-	sfence.vma
-	csrw satp, a0
-	sfence.vma
-
 	# Do nothing in a loop.
 .Lkernel_main:
 	j .Lkernel_main
@@ -216,12 +294,22 @@ panic:
 
 .section .text.boot
 _entry:
-	la t0, __stack_top
-	mv sp, t0 # Set the stack pointer
+	# Set the stack pointer.
+	la t0, end
+	li t1, 64 * 1024
+	add sp, t0, t1
 
-	lw t0, (__free_ram)
+	mv s1, a1
+
+	la a0, .text.boot
+	mv a1, sp
+	call imem_initialize
+
+	lw t0, (end)
 	la t1, next_paddr
 	sw t0, (t1)
 
 	call bzero
+	mv a0, sp
+	mv a1, s1
 	j kernel_main