diff --git a/.cargo/config b/.cargo/config
new file mode 100644
index 0000000..096e848
--- /dev/null
+++ b/.cargo/config
@@ -0,0 +1,11 @@
+[target.riscv64imac-unknown-none-elf]
+runner = "qemu-system-riscv64 -S -gdb tcp::10000 -d int -machine virt -bios none -nographic -m 2G -initrd fedora-vmlinux -kernel"
+#runner = "qemu-system-riscv64 -machine virt -bios none -nographic -m 2G -initrd fedora-vmlinux -kernel"
+
+rustflags = [
+  "-C", "link-arg=-Tmemory.x",
+  "-C", "link-arg=-Tlink.x",
+]
+
+[build]
+target = "riscv64imac-unknown-none-elf"
diff --git a/.github/workflows/rust.yml b/.github/workflows/rust.yml
new file mode 100644
index 0000000..f734ffd
--- /dev/null
+++ b/.github/workflows/rust.yml
@@ -0,0 +1,45 @@
+name: Rust
+
+on:
+  push:
+    branches:
+      - master
+  pull_request:
+
+env:
+  CARGO_INCREMENTAL: 0
+
+# Copyright (c) 2021 sksat
+# https://github.com/sksat/action-clippy
+jobs:
+  lint:
+    runs-on: ubuntu-latest
+    steps:
+      - uses: actions/checkout@v4
+        with:
+          submodules: recursive
+
+      - name: install clippy
+        uses: dtolnay/rust-toolchain@stable
+        with:
+          toolchain: stable # Rustup toolchain specifier e.g. stable, nightly, 1.42.0, nightly-2022-01-01. 
+          target: riscv64imac-unknown-none-elf # Comma-separated string of additional targets to install e.g. wasm32-unknown-unknown
+          components: clippy, rustfmt
+
+      - uses: sksat/action-clippy@v0.7.0
+        if: github.event_name == 'push'
+        with:
+          reporter: github-check
+
+      - uses: sksat/action-clippy@v0.7.0
+        if: github.event_name == 'pull_request'
+        with:
+          reporter: github-pr-review
+
+      - name: format check 
+        run: cargo fmt --all -- --check
+      
+      # no crate for test in no_std
+      #- name: unit test
+      #  run: cargo test
+
diff --git a/.gitignore b/.gitignore
index 6985cf1..5d0210b 100644
--- a/.gitignore
+++ b/.gitignore
@@ -12,3 +12,12 @@ Cargo.lock
 
 # MSVC Windows builds of rustc generate these, which store debugging information
 *.pdb
+
+
+# Added by cargo
+/target
+
+# Added by me
+.gdb_history
+fedora-vmlinux
+stage4-disk.img
diff --git a/.gitmodules b/.gitmodules
new file mode 100644
index 0000000..20dd4cd
--- /dev/null
+++ b/.gitmodules
@@ -0,0 +1,3 @@
+[submodule "wild-screen-alloc"]
+	path = wild-screen-alloc
+	url = git@github.com:Alignof/wild-screen-alloc.git
diff --git a/Cargo.toml b/Cargo.toml
new file mode 100644
index 0000000..f52fbad
--- /dev/null
+++ b/Cargo.toml
@@ -0,0 +1,21 @@
+[package]
+name = "hikami"
+version = "0.1.0"
+edition = "2021"
+
+[lints.clippy]
+pedantic = "warn"
+
+# See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html
+
+[[bin]]
+name = "hikami"
+path = "src/machine_init.rs"
+
+[dependencies]
+elf = { version = "0.7.2", default-features = false }
+fdt = "0.1.5"
+panic-halt = "0.2.0"
+riscv = "0.10.1"
+riscv-rt = "0.11.0"
+wild_screen_alloc = { version = "0.1.1",  path = "wild-screen-alloc" }
diff --git a/build.rs b/build.rs
new file mode 100644
index 0000000..143e014
--- /dev/null
+++ b/build.rs
@@ -0,0 +1,14 @@
+use std::env;
+use std::fs;
+use std::path::PathBuf;
+
+fn main() {
+    let out_dir = PathBuf::from(env::var("OUT_DIR").unwrap());
+
+    // Put the linker script somewhere the linker can find it.
+    fs::write(out_dir.join("memory.x"), include_bytes!("memory.x")).unwrap();
+    println!("cargo:rustc-link-search={}", out_dir.display());
+    println!("cargo:rerun-if-changed=memory.x");
+
+    println!("cargo:rerun-if-changed=build.rs");
+}
diff --git a/linker.ld b/linker.ld
new file mode 100644
index 0000000..3b115b1
--- /dev/null
+++ b/linker.ld
@@ -0,0 +1,38 @@
+OUTPUT_ARCH("riscv")
+
+ENTRY(_start)
+EXTERN(abort);
+
+SECTIONS
+{
+    . = 0x80000000;
+
+    .reset : 
+    {
+        KEEP(*(.reset.boot));
+    }
+
+    /* text: Program code section */
+    .text :
+    {
+        *(.text. text.*);
+    }
+
+    /* rodata: Read-only data */
+    .rodata :
+    {
+        *(.rdata .rodata. .rodata.*);
+    }
+
+    /* data: Writable data */
+    .data :
+    {
+        *(.data .data.*);
+    }
+
+    _bss_start = .;
+    .bss :
+    {
+        *(.bss .bss.*);
+    }
+}
diff --git a/memory.x b/memory.x
new file mode 100644
index 0000000..dbb8d7f
--- /dev/null
+++ b/memory.x
@@ -0,0 +1,21 @@
+MEMORY
+{
+  L2_LIM : ORIGIN = 0x807f0000, LENGTH = 64K
+  RAM : ORIGIN = 0x80200000, LENGTH = 6M
+  FLASH : ORIGIN = 0x80000000, LENGTH = 2M
+}
+
+/*
+ * L2_LIC (STACK),  0x807f_0000..0x8080_0000
+ * RAM (DATA, BSS, HEAP),  0x8020_0000..0x8080_0000
+ * FLASH (TEXT),  0x8000_0000..0x8020_0000
+*/
+
+REGION_ALIAS("REGION_TEXT", FLASH);
+REGION_ALIAS("REGION_RODATA", FLASH);
+REGION_ALIAS("REGION_DATA", RAM);
+REGION_ALIAS("REGION_BSS", RAM);
+REGION_ALIAS("REGION_HEAP", RAM);
+REGION_ALIAS("REGION_STACK", L2_LIM);
+
+_stack_start = ORIGIN(L2_LIM) + LENGTH(L2_LIM);
diff --git a/src/machine_init.rs b/src/machine_init.rs
new file mode 100644
index 0000000..dbc9239
--- /dev/null
+++ b/src/machine_init.rs
@@ -0,0 +1,183 @@
+#![no_main]
+#![no_std]
+
+extern crate alloc;
+mod memmap;
+mod supervisor_init;
+mod trap;
+mod util;
+
+use crate::memmap::constant::{DRAM_BASE, HEAP_BASE, HEAP_SIZE, STACK_BASE, STACK_SIZE_PER_HART};
+use crate::trap::machine::mtrap_vector;
+use core::arch::asm;
+use core::panic::PanicInfo;
+use riscv::asm::sfence_vma_all;
+use riscv::register::{
+    mcause, mcounteren, medeleg, mepc, mideleg, mie, mscratch, mstatus, mtval, mtvec, pmpaddr0,
+    pmpcfg0, satp, scause, sepc, stval, stvec,
+};
+use riscv_rt::entry;
+use wild_screen_alloc::WildScreenAlloc;
+
+/// Panic handler
+#[panic_handler]
+pub fn panic(info: &PanicInfo) -> ! {
+    println!("{}", info);
+    loop {
+        unsafe {
+            asm!("nop");
+        }
+    }
+}
+
+#[global_allocator]
+static mut ALLOCATOR: WildScreenAlloc = WildScreenAlloc::empty();
+
+/// Entry function. `__risc_v_rt__main` is alias of `__init` function in machine_init.rs.
+/// * set stack pointer
+/// * init mtvec and stvec
+/// * jump to mstart
+#[entry]
+fn _start(hart_id: usize, dtb_addr: usize) -> ! {
+    // Initialize global allocator
+    unsafe {
+        ALLOCATOR.init(HEAP_BASE, HEAP_SIZE);
+    }
+
+    unsafe {
+        // set stack pointer
+        asm!(
+            "
+            mv a0, {hart_id}
+            mv a1, {dtb_addr}
+            mv t1, {stack_size_per_hart}
+            mul t0, a0, t1
+            mv sp, {stack_base}
+            add sp, sp, t0
+            csrw mtvec, {DRAM_BASE}
+            csrw stvec, {DRAM_BASE}
+            j {mstart}
+            ",
+            hart_id = in(reg) hart_id,
+            dtb_addr = in(reg) dtb_addr,
+            stack_size_per_hart = in(reg) STACK_SIZE_PER_HART,
+            stack_base = in(reg) STACK_BASE,
+            DRAM_BASE = in(reg) DRAM_BASE,
+            mstart = sym mstart,
+        );
+    }
+
+    unreachable!();
+}
+
+/// Machine start function
+fn mstart(hart_id: usize, dtb_addr: usize) {
+    unsafe {
+        // mideleg = 0x0222
+        mideleg::set_sext();
+        mideleg::set_ssoft();
+        mideleg::set_stimer();
+        // medeleg = 0xb1ff
+        medeleg::set_instruction_misaligned();
+        medeleg::set_instruction_fault();
+        medeleg::set_illegal_instruction();
+        medeleg::set_breakpoint();
+        medeleg::set_load_misaligned();
+        medeleg::set_load_fault();
+        medeleg::set_store_misaligned();
+        medeleg::set_store_fault();
+        medeleg::set_user_env_call();
+        medeleg::set_instruction_page_fault();
+        medeleg::set_load_page_fault();
+        medeleg::set_store_page_fault();
+        // mie = 0x088
+        mie::set_msoft();
+        mie::set_mtimer();
+
+        // mcounteren = 0xffff_ffff
+        mcounteren::set_cy();
+        mcounteren::set_tm();
+        mcounteren::set_ir();
+        mcounteren::set_hpm(3);
+        mcounteren::set_hpm(4);
+        mcounteren::set_hpm(5);
+        mcounteren::set_hpm(6);
+        mcounteren::set_hpm(7);
+        mcounteren::set_hpm(8);
+        mcounteren::set_hpm(9);
+        mcounteren::set_hpm(10);
+        mcounteren::set_hpm(11);
+        mcounteren::set_hpm(12);
+        mcounteren::set_hpm(13);
+        mcounteren::set_hpm(14);
+        mcounteren::set_hpm(15);
+        mcounteren::set_hpm(16);
+        mcounteren::set_hpm(17);
+        mcounteren::set_hpm(18);
+        mcounteren::set_hpm(19);
+        mcounteren::set_hpm(20);
+        mcounteren::set_hpm(21);
+        mcounteren::set_hpm(22);
+        mcounteren::set_hpm(23);
+        mcounteren::set_hpm(24);
+        mcounteren::set_hpm(25);
+        mcounteren::set_hpm(26);
+        mcounteren::set_hpm(27);
+        mcounteren::set_hpm(28);
+        mcounteren::set_hpm(29);
+        mcounteren::set_hpm(30);
+        mcounteren::set_hpm(31);
+        mstatus::set_mpp(mstatus::MPP::Supervisor);
+        mscratch::write(STACK_BASE + STACK_SIZE_PER_HART * hart_id);
+        pmpaddr0::write(0xffff_ffff_ffff_ffff);
+        pmpcfg0::write(pmpcfg0::read().bits | 0x1f);
+        satp::set(satp::Mode::Bare, 0, 0);
+
+        mepc::write(supervisor_init::sstart as *const fn() as usize);
+
+        // set trap_vector in trap.S to mtvec
+        mtvec::write(
+            mtrap_vector as *const fn() as usize,
+            mtvec::TrapMode::Direct,
+        );
+
+        sfence_vma_all();
+    }
+
+    enter_supervisor_mode(hart_id, dtb_addr);
+}
+
+/// Delegate exception to supervisor mode
+#[no_mangle]
+extern "C" fn forward_exception() {
+    unsafe {
+        sepc::write(mepc::read());
+        scause::write(mcause::read().bits());
+        stval::write(mtval::read());
+        mepc::write(stvec::read().bits() & !0x3);
+
+        if mstatus::read().sie() {
+            mstatus::set_spie();
+        } else {
+            // clear?
+        }
+
+        if mstatus::read().mpp() == mstatus::MPP::Supervisor {
+            mstatus::set_spp(mstatus::SPP::Supervisor);
+        } else {
+            mstatus::set_spp(mstatus::SPP::User);
+        }
+
+        mstatus::clear_sie();
+        mstatus::set_mpp(mstatus::MPP::Supervisor);
+    }
+}
+
+/// Enter supervisor. (just exec mret)
+/// Jump to sstart via mret.
+#[inline(never)]
+fn enter_supervisor_mode(_hart_id: usize, _dtb_addr: usize) {
+    unsafe {
+        asm!("mret");
+    }
+}
diff --git a/src/memmap.rs b/src/memmap.rs
new file mode 100644
index 0000000..2342868
--- /dev/null
+++ b/src/memmap.rs
@@ -0,0 +1,92 @@
+//! See `memmap/constant` module for specefic memmory map.
+
+pub mod constant;
+pub mod device;
+pub mod page_table;
+
+use crate::memmap::page_table::PteFlag;
+use alloc::vec::Vec;
+use core::ops::Range;
+use device::{initrd, plic, uart, virtio, Device};
+use fdt::Fdt;
+
+#[derive(Clone)]
+pub struct MemoryMap {
+    virt: Range<usize>,
+    phys: Range<usize>,
+    flags: u8,
+}
+
+impl MemoryMap {
+    pub fn new(virt: Range<usize>, phys: Range<usize>, flags: &[PteFlag]) -> Self {
+        Self {
+            virt,
+            phys,
+            flags: flags.iter().fold(0, |pte_f, f| (pte_f | *f as u8)),
+        }
+    }
+}
+
+/// Memmap has memory region data of each devices.  
+/// Each devices **must** be implemented Device trait.
+#[allow(clippy::module_name_repetitions)]
+pub struct DeviceMemmap {
+    pub uart: uart::Uart,
+    pub virtio: Vec<virtio::VirtIO>,
+    pub initrd: initrd::Initrd,
+    pub plic: plic::Plic,
+    pub plic_context: usize,
+}
+
+impl DeviceMemmap {
+    /// Create Memmap from device tree blob.
+    pub fn new(device_tree: Fdt) -> Self {
+        DeviceMemmap {
+            uart: uart::Uart::new(&device_tree, "/soc/serial"),
+            virtio: virtio::VirtIO::new_all(&device_tree, "/soc/virtio_mmio"),
+            initrd: initrd::Initrd::new(&device_tree, "/chosen"),
+            plic: plic::Plic::new(&device_tree, "/soc/plic"),
+            plic_context: device_tree
+                .find_node("/cpus/cpu")
+                .unwrap()
+                .children()
+                .next() // interrupt-controller
+                .unwrap()
+                .property("phandle")
+                .unwrap()
+                .value[0] as usize,
+            /*
+            plic_context: device_tree
+                .find_node("/cpus/cpu/interrupt-controller")
+                .unwrap()
+                .property("phandle")
+                .unwrap()
+                .value[0] as usize,
+            */
+        }
+    }
+
+    pub fn device_mapping(&self, page_table_start: usize) {
+        let memory_map = self.create_device_map();
+        page_table::generate_page_table(page_table_start, &memory_map, false);
+    }
+
+    fn create_device_map(&self) -> Vec<MemoryMap> {
+        let mut device_mapping: Vec<MemoryMap> = self
+            .virtio
+            .iter()
+            .flat_map(|virt| [virt.memmap(), virt.identity_memmap()])
+            .collect();
+
+        device_mapping.extend_from_slice(&[
+            self.uart.memmap(),
+            self.uart.identity_memmap(),
+            self.initrd.memmap(),
+            self.initrd.identity_memmap(),
+            self.plic.memmap(),
+            self.plic.identity_memmap(),
+        ]);
+
+        device_mapping
+    }
+}
diff --git a/src/memmap/constant.rs b/src/memmap/constant.rs
new file mode 100644
index 0000000..148cf0d
--- /dev/null
+++ b/src/memmap/constant.rs
@@ -0,0 +1,54 @@
+//! Constant for memory map.
+//!
+//! | start       | end         | region    |
+//! |-------------|-------------|-----------|
+//! | 0x0200_0000 | 0x0210_0000 | QEMU CLINT |
+//! | 0x0c00_0000 | 0x0c60_0000 | QEMU PLIC |
+//! | 0x1000_0000 | 0x1000_0100 | QEMU UART |
+//! | 0x1000_1000 | 0x1000_8000 | QEMU VirtIO |
+//! | 0x8000_0000 | 0x8020_0000 | text data of hikami |
+//! | 0x8020_0000 | 0x8030_0000 | hypervisor static |
+//! | 0x8030_0000 | 0x8030_2000 | hypervisor page table |
+//! | 0x8030_4000 | 0x8030_xxxx | hypervisor device tree blob |
+//! | 0x8040_0000 |     ...     | hypervisor heap |
+//! |     ...     | 0x8080_0000 | hypervisor stack |
+//! | 0x9000_0000 | 0x9000_2000 | hypervisor page table |
+//! | 0x9000_2000 | 0x9000_4000 | hypervisor device tree |
+//! | 0x9100_0000 |     ...     | hypervisor heap |
+//! |     ...     | 0x9300_0000 | hypervisor stack |
+//! | 0x9300_0000 |     ...     | text data of hikami |
+
+/// Uart addr
+pub const UART_ADDR: usize = 0x1000_0000;
+
+/// Base address of dram.
+pub const DRAM_BASE: usize = 0x8000_0000;
+/// Memory region on dram that be allocated each HARTs.
+pub const DRAM_SIZE_PAR_HART: usize = 0x1000_0000;
+/// Base address of page table.
+pub const PAGE_TABLE_BASE: usize = 0x8030_0000;
+/// Size of memory areathat a page can point to.
+pub const PAGE_SIZE: usize = 4096;
+/// Page table offset for each HART.
+pub const PAGE_TABLE_OFFSET_PER_HART: usize = 1024;
+/// Base address of heap.
+pub const HEAP_BASE: usize = 0x8040_0000;
+/// Heap size.
+pub const HEAP_SIZE: usize = 0x40_0000;
+/// Base address of stack.
+pub const STACK_BASE: usize = 0x8080_0000; // modify trap vector immediate when change it.
+/// Stack size for each HART.
+pub const STACK_SIZE_PER_HART: usize = 0x1_0000;
+/// Offset for converting physical address on dram to virtual address.
+pub const PA2VA_DRAM_OFFSET: usize = 0xffff_ffff_4000_0000;
+/// Offset for converting physical device address to virtual address.
+pub const PA2VA_DEVICE_OFFSET: usize = 0xffff_fffc_0000_0000;
+
+/// loading device tree offset of guest space
+pub const GUEST_DEVICE_TREE_OFFSET: usize = 0x2000;
+/// Heap offset of guest space
+pub const GUEST_HEAP_OFFSET: usize = 0x100_0000;
+/// Stack offset of guest space
+pub const GUEST_STACK_OFFSET: usize = 0x300_0000;
+/// Guest Text secion offset
+pub const GUEST_TEXT_OFFSET: usize = 0x300_0000;
diff --git a/src/memmap/device.rs b/src/memmap/device.rs
new file mode 100644
index 0000000..eb9bebe
--- /dev/null
+++ b/src/memmap/device.rs
@@ -0,0 +1,28 @@
+//! A module about device on memory map.  
+//! This module holds each devices implementation.
+
+pub mod initrd;
+pub mod plic;
+pub mod uart;
+pub mod virtio;
+
+use super::MemoryMap;
+use fdt::Fdt;
+
+/// A struct that implement Device trait **must** has `base_addr` and size member.
+pub trait Device {
+    /// Create self instance.
+    /// * `device_tree` - struct Fdt
+    /// * `node_path` - node path in fdt
+    fn new(device_tree: &Fdt, node_path: &str) -> Self;
+    /// Return size of memory region.
+    fn size(&self) -> usize;
+    /// Return address of physical memory
+    fn paddr(&self) -> usize;
+    /// Return address of virtual memory
+    fn vaddr(&self) -> usize;
+    /// Return memory map between virtual to physical
+    fn memmap(&self) -> MemoryMap;
+    /// Return memory map between physical to physical
+    fn identity_memmap(&self) -> MemoryMap;
+}
diff --git a/src/memmap/device/initrd.rs b/src/memmap/device/initrd.rs
new file mode 100644
index 0000000..d769990
--- /dev/null
+++ b/src/memmap/device/initrd.rs
@@ -0,0 +1,64 @@
+use super::Device;
+use crate::memmap::page_table::PteFlag;
+use crate::memmap::{constant, MemoryMap};
+use fdt::Fdt;
+
+const DEVICE_FLAGS: [PteFlag; 5] = [
+    PteFlag::Dirty,
+    PteFlag::Accessed,
+    PteFlag::Write,
+    PteFlag::Read,
+    PteFlag::Valid,
+];
+
+/// A scheme for loading a temporary root file system into memory,
+/// to be used as part of the Linux startup process.
+pub struct Initrd {
+    base_addr: usize,
+    size: usize,
+}
+
+impl Device for Initrd {
+    fn new(device_tree: &Fdt, node_path: &str) -> Self {
+        let start_prop = "linux,initrd-start";
+        let end_prop = "linux,initrd-end";
+        let node = device_tree.find_node(node_path).unwrap();
+        let start = node.property(start_prop).unwrap().value;
+        let start = u32::from_be_bytes(start.try_into().unwrap()) as usize;
+        let end = node.property(end_prop).unwrap().value;
+        let end = u32::from_be_bytes(end.try_into().unwrap()) as usize;
+
+        Initrd {
+            base_addr: start,
+            size: end - start,
+        }
+    }
+
+    fn size(&self) -> usize {
+        self.size
+    }
+
+    fn paddr(&self) -> usize {
+        self.base_addr
+    }
+
+    fn vaddr(&self) -> usize {
+        self.base_addr + constant::PA2VA_DEVICE_OFFSET
+    }
+
+    fn memmap(&self) -> MemoryMap {
+        MemoryMap::new(
+            self.vaddr()..self.vaddr() + self.size(),
+            self.paddr()..self.paddr() + self.size(),
+            &DEVICE_FLAGS,
+        )
+    }
+
+    fn identity_memmap(&self) -> MemoryMap {
+        MemoryMap::new(
+            self.paddr()..self.paddr() + self.size(),
+            self.paddr()..self.paddr() + self.size(),
+            &DEVICE_FLAGS,
+        )
+    }
+}
diff --git a/src/memmap/device/plic.rs b/src/memmap/device/plic.rs
new file mode 100644
index 0000000..8e9b1f6
--- /dev/null
+++ b/src/memmap/device/plic.rs
@@ -0,0 +1,70 @@
+use super::Device;
+use crate::memmap::page_table::PteFlag;
+use crate::memmap::{constant, MemoryMap};
+use fdt::Fdt;
+
+const DEVICE_FLAGS: [PteFlag; 5] = [
+    PteFlag::Dirty,
+    PteFlag::Accessed,
+    PteFlag::Write,
+    PteFlag::Read,
+    PteFlag::Valid,
+];
+
+pub const ENABLE_BASE: usize = 0x2000;
+pub const ENABLE_PER_HART: usize = 0x80;
+pub const CONTEXT_BASE: usize = 0x20_0000;
+pub const CONTEXT_PER_HART: usize = 0x1000;
+pub const CONTEXT_CLAIM: usize = 0x4;
+
+/// PLIC: Platform-Level Interrupt Controller  
+/// Interrupt controller for global interrupts.
+pub struct Plic {
+    base_addr: usize,
+    size: usize,
+}
+
+impl Device for Plic {
+    fn new(device_tree: &Fdt, node_path: &str) -> Self {
+        let region = device_tree
+            .find_node(node_path)
+            .unwrap()
+            .reg()
+            .unwrap()
+            .next()
+            .unwrap();
+
+        Plic {
+            base_addr: region.starting_address as usize,
+            size: region.size.unwrap(),
+        }
+    }
+
+    fn size(&self) -> usize {
+        self.size
+    }
+
+    fn paddr(&self) -> usize {
+        self.base_addr
+    }
+
+    fn vaddr(&self) -> usize {
+        self.base_addr + constant::PA2VA_DEVICE_OFFSET
+    }
+
+    fn memmap(&self) -> MemoryMap {
+        MemoryMap::new(
+            self.vaddr()..self.vaddr() + self.size(),
+            self.paddr()..self.paddr() + self.size(),
+            &DEVICE_FLAGS,
+        )
+    }
+
+    fn identity_memmap(&self) -> MemoryMap {
+        MemoryMap::new(
+            self.paddr()..self.paddr() + self.size(),
+            self.paddr()..self.paddr() + self.size(),
+            &DEVICE_FLAGS,
+        )
+    }
+}
diff --git a/src/memmap/device/uart.rs b/src/memmap/device/uart.rs
new file mode 100644
index 0000000..2ad9871
--- /dev/null
+++ b/src/memmap/device/uart.rs
@@ -0,0 +1,63 @@
+use super::Device;
+use crate::memmap::page_table::PteFlag;
+use crate::memmap::{constant, MemoryMap};
+use fdt::Fdt;
+
+const DEVICE_FLAGS: [PteFlag; 5] = [
+    PteFlag::Dirty,
+    PteFlag::Accessed,
+    PteFlag::Write,
+    PteFlag::Read,
+    PteFlag::Valid,
+];
+
+/// UART: Universal asynchronous receiver-transmitter
+pub struct Uart {
+    base_addr: usize,
+    size: usize,
+}
+
+impl Device for Uart {
+    fn new(device_tree: &Fdt, node_path: &str) -> Self {
+        let region = device_tree
+            .find_node(node_path)
+            .unwrap()
+            .reg()
+            .unwrap()
+            .next()
+            .unwrap();
+
+        Uart {
+            base_addr: region.starting_address as usize,
+            size: region.size.unwrap(),
+        }
+    }
+
+    fn size(&self) -> usize {
+        self.size
+    }
+
+    fn paddr(&self) -> usize {
+        self.base_addr
+    }
+
+    fn vaddr(&self) -> usize {
+        self.base_addr + constant::PA2VA_DEVICE_OFFSET
+    }
+
+    fn memmap(&self) -> MemoryMap {
+        MemoryMap::new(
+            self.vaddr()..self.vaddr() + self.size(),
+            self.paddr()..self.paddr() + self.size(),
+            &DEVICE_FLAGS,
+        )
+    }
+
+    fn identity_memmap(&self) -> MemoryMap {
+        MemoryMap::new(
+            self.paddr()..self.paddr() + self.size(),
+            self.paddr()..self.paddr() + self.size(),
+            &DEVICE_FLAGS,
+        )
+    }
+}
diff --git a/src/memmap/device/virtio.rs b/src/memmap/device/virtio.rs
new file mode 100644
index 0000000..8867485
--- /dev/null
+++ b/src/memmap/device/virtio.rs
@@ -0,0 +1,85 @@
+use super::Device;
+use crate::memmap::page_table::PteFlag;
+use crate::memmap::{constant, MemoryMap};
+use alloc::vec::Vec;
+use fdt::Fdt;
+
+const DEVICE_FLAGS: [PteFlag; 5] = [
+    PteFlag::Dirty,
+    PteFlag::Accessed,
+    PteFlag::Write,
+    PteFlag::Read,
+    PteFlag::Valid,
+];
+
+/// A virtualization standard for network and disk device drivers.
+/// Since more than one may be found, we will temporarily use the first one.
+pub struct VirtIO {
+    base_addr: usize,
+    size: usize,
+    irq: u8,
+}
+
+impl VirtIO {
+    /// Create each Virt IO data when device has multiple IOs.
+    pub fn new_all(device_tree: &Fdt, node_path: &str) -> Vec<Self> {
+        device_tree
+            .find_all_nodes(node_path)
+            .map(|node| {
+                let region = node.reg().unwrap().next().unwrap();
+                let irq = node.property("interrupts").unwrap().value[0];
+                VirtIO {
+                    base_addr: region.starting_address as usize,
+                    size: region.size.unwrap(),
+                    irq,
+                }
+            })
+            .collect()
+    }
+
+    pub fn irq(&self) -> u8 {
+        self.irq
+    }
+}
+
+impl Device for VirtIO {
+    fn new(device_tree: &Fdt, node_path: &str) -> Self {
+        let node = device_tree.find_all_nodes(node_path).next().unwrap();
+        let region = node.reg().unwrap().next().unwrap();
+        let irq = node.property("interrupts").unwrap().value[0];
+
+        VirtIO {
+            base_addr: region.starting_address as usize,
+            size: region.size.unwrap(),
+            irq,
+        }
+    }
+
+    fn size(&self) -> usize {
+        self.size
+    }
+
+    fn paddr(&self) -> usize {
+        self.base_addr
+    }
+
+    fn vaddr(&self) -> usize {
+        self.base_addr + constant::PA2VA_DEVICE_OFFSET
+    }
+
+    fn memmap(&self) -> MemoryMap {
+        MemoryMap::new(
+            self.vaddr()..self.vaddr() + self.size(),
+            self.paddr()..self.paddr() + self.size(),
+            &DEVICE_FLAGS,
+        )
+    }
+
+    fn identity_memmap(&self) -> MemoryMap {
+        MemoryMap::new(
+            self.paddr()..self.paddr() + self.size(),
+            self.paddr()..self.paddr() + self.size(),
+            &DEVICE_FLAGS,
+        )
+    }
+}
diff --git a/src/memmap/page_table.rs b/src/memmap/page_table.rs
new file mode 100644
index 0000000..ffd8260
--- /dev/null
+++ b/src/memmap/page_table.rs
@@ -0,0 +1,125 @@
+use alloc::boxed::Box;
+use core::slice::from_raw_parts_mut;
+
+use super::constant::PAGE_SIZE;
+use super::MemoryMap;
+
+/// Each flags for page tables.
+#[allow(dead_code)]
+#[derive(Copy, Clone)]
+pub enum PteFlag {
+    /// PTE is valid.
+    Valid = 0b0000_0001,
+    /// PTE is readable.
+    Read = 0b0000_0010,
+    /// PTE is writable.
+    Write = 0b0000_0100,
+    /// PTE is executable.
+    Exec = 0b0000_1000,
+    /// The page may only accessed by U-mode software.
+    User = 0b0001_0000,
+    /// Global mapping.
+    Global = 0b0010_0000,
+    /// This page has been read, written or fetched.
+    Accessed = 0b0100_0000,
+    /// This page has been written.
+    Dirty = 0b1000_0000,
+}
+
+/// Page table entry
+#[derive(Copy, Clone)]
+struct PageTableEntry(u64);
+
+impl PageTableEntry {
+    fn new(ppn: u64, flags: u8) -> Self {
+        Self(ppn << 10 | u64::from(flags))
+    }
+
+    fn already_created(self) -> bool {
+        self.0 & PteFlag::Valid as u64 == 1
+    }
+
+    fn pte(self) -> u64 {
+        self.0 >> 10
+    }
+}
+
+/// Generate third-level page table.
+#[allow(clippy::module_name_repetitions)]
+pub fn generate_page_table(root_table_start_addr: usize, memmaps: &[MemoryMap], initialize: bool) {
+    use crate::{print, println};
+
+    const PAGE_TABLE_SIZE: usize = 512;
+
+    let first_lv_page_table: &mut [PageTableEntry] = unsafe {
+        from_raw_parts_mut(
+            root_table_start_addr as *mut PageTableEntry,
+            PAGE_TABLE_SIZE,
+        )
+    };
+
+    // zero filling page table
+    if initialize {
+        first_lv_page_table.fill(PageTableEntry(0));
+    }
+
+    println!(
+        "=========gen page table: {:x}====================",
+        root_table_start_addr
+    );
+    for memmap in memmaps {
+        println!("{:x?} -> {:x?}", memmap.virt, memmap.phys);
+
+        assert!(memmap.virt.len() == memmap.phys.len());
+        assert!(memmap.virt.start % PAGE_SIZE == 0);
+        assert!(memmap.phys.start % PAGE_SIZE == 0);
+
+        for offset in (0..memmap.virt.len()).step_by(PAGE_SIZE) {
+            let v_start = memmap.virt.start + offset;
+            let p_start = memmap.phys.start + offset;
+
+            // first level
+            let vpn2 = (v_start >> 30) & 0x1ff;
+            if !first_lv_page_table[vpn2].already_created() {
+                let second_pt = Box::new([0u64; PAGE_TABLE_SIZE]);
+                let second_pt_paddr = Box::into_raw(second_pt);
+
+                first_lv_page_table[vpn2] = PageTableEntry::new(
+                    second_pt_paddr as u64 / PAGE_SIZE as u64,
+                    PteFlag::Valid as u8,
+                );
+            }
+
+            // second level
+            let vpn1 = (v_start >> 21) & 0x1ff;
+            let second_table_start_addr = first_lv_page_table[vpn2].pte() * PAGE_SIZE as u64;
+            let second_lv_page_table: &mut [PageTableEntry] = unsafe {
+                from_raw_parts_mut(
+                    second_table_start_addr as *mut PageTableEntry,
+                    PAGE_TABLE_SIZE,
+                )
+            };
+            if !second_lv_page_table[vpn1].already_created() {
+                let third_pt = Box::new([0u64; PAGE_TABLE_SIZE]);
+                let third_pt_paddr = Box::into_raw(third_pt);
+
+                second_lv_page_table[vpn1] = PageTableEntry::new(
+                    third_pt_paddr as u64 / PAGE_SIZE as u64,
+                    PteFlag::Valid as u8,
+                );
+            }
+
+            // third level
+            let vpn0 = (v_start >> 12) & 0x1ff;
+            let third_table_start_addr = second_lv_page_table[vpn1].pte() * PAGE_SIZE as u64;
+            let third_lv_page_table: &mut [PageTableEntry] = unsafe {
+                from_raw_parts_mut(
+                    third_table_start_addr as *mut PageTableEntry,
+                    PAGE_TABLE_SIZE,
+                )
+            };
+            third_lv_page_table[vpn0] =
+                PageTableEntry::new((p_start / PAGE_SIZE).try_into().unwrap(), memmap.flags);
+        }
+    }
+}
diff --git a/src/supervisor_init.rs b/src/supervisor_init.rs
new file mode 100644
index 0000000..67885c8
--- /dev/null
+++ b/src/supervisor_init.rs
@@ -0,0 +1,376 @@
+use crate::memmap::constant::{
+    DRAM_BASE, DRAM_SIZE_PAR_HART, GUEST_DEVICE_TREE_OFFSET, GUEST_HEAP_OFFSET, GUEST_STACK_OFFSET,
+    GUEST_TEXT_OFFSET, PA2VA_DRAM_OFFSET, PAGE_TABLE_BASE, PAGE_TABLE_OFFSET_PER_HART, STACK_BASE,
+};
+use crate::memmap::device::plic::{
+    CONTEXT_BASE, CONTEXT_CLAIM, CONTEXT_PER_HART, ENABLE_BASE, ENABLE_PER_HART,
+};
+use crate::memmap::device::Device;
+use crate::memmap::{page_table, page_table::PteFlag, DeviceMemmap, MemoryMap};
+use crate::trap::supervisor::strap_vector;
+use core::arch::asm;
+use elf::endian::AnyEndian;
+use elf::ElfBytes;
+use riscv::register::{satp, sepc, sie, sstatus, stvec};
+
+/// Supervisor start function
+/// * Init page tables
+/// * Init trap vector
+/// * Init stack pointer
+#[inline(never)]
+pub extern "C" fn sstart(hart_id: usize, dtb_addr: usize) {
+    use PteFlag::{Accessed, Dirty, Exec, Read, Valid, Write};
+
+    // init page tables
+    let page_table_start = PAGE_TABLE_BASE + hart_id * PAGE_TABLE_OFFSET_PER_HART;
+    let memory_map: [MemoryMap; 7] = [
+        // (virtual_memory_range, physical_memory_range, flags),
+        // uart
+        MemoryMap::new(
+            0x1000_0000..0x1000_0100,
+            0x1000_0000..0x1000_0100,
+            &[Dirty, Accessed, Write, Read, Valid],
+        ),
+        // Device tree
+        MemoryMap::new(
+            0xbfe0_0000..0xc000_0000,
+            0xbfe0_0000..0xc000_0000,
+            &[Dirty, Accessed, Write, Read, Valid],
+        ),
+        // TEXT (physical map)
+        MemoryMap::new(
+            0x8000_0000..0x8020_0000,
+            0x8000_0000..0x8020_0000,
+            &[Dirty, Accessed, Exec, Read, Valid],
+        ),
+        // RAM
+        MemoryMap::new(
+            0x8020_0000..0x8080_0000,
+            0x8020_0000..0x8080_0000,
+            &[Dirty, Accessed, Write, Read, Valid],
+        ),
+        // hypervisor RAM
+        MemoryMap::new(
+            0x9000_0000..0x9000_4000,
+            0x9000_0000..0x9000_4000,
+            &[Dirty, Accessed, Write, Read, Valid],
+        ),
+        // TEXT
+        MemoryMap::new(
+            0xffff_ffff_c000_0000..0xffff_ffff_c020_0000,
+            0x8000_0000..0x8020_0000,
+            &[Dirty, Accessed, Exec, Read, Valid],
+        ),
+        // RAM
+        MemoryMap::new(
+            0xffff_ffff_c020_0000..0xffff_ffff_c080_0000,
+            0x8020_0000..0x8080_0000,
+            &[Dirty, Accessed, Write, Read, Valid],
+        ),
+    ];
+    page_table::generate_page_table(page_table_start, &memory_map, true);
+
+    unsafe {
+        // init trap vector
+        stvec::write(
+            // stvec address must be 4byte aligned.
+            trampoline as *const fn() as usize & !0b11,
+            stvec::TrapMode::Direct,
+        );
+
+        // init stack pointer
+        let stack_pointer = STACK_BASE + PA2VA_DRAM_OFFSET;
+        let satp_config = (0b1000 << 60) | (page_table_start >> 12);
+        asm!(
+            "
+            mv a0, {hart_id}
+            mv a1, {dtb_addr}
+            mv sp, {stack_pointer}
+            csrw satp, {satp_config}
+            sfence.vma
+            j {trampoline}
+            ",
+            hart_id = in(reg) hart_id,
+            dtb_addr = in(reg) dtb_addr,
+            stack_pointer = in(reg) stack_pointer,
+            satp_config = in(reg) satp_config,
+            trampoline = sym trampoline
+        );
+    }
+
+    unreachable!()
+}
+
+/// Jump to `smode_setup`
+#[inline(never)]
+extern "C" fn trampoline(hart_id: usize, dtb_addr: usize) {
+    smode_setup(hart_id, dtb_addr);
+}
+
+/// Setup for S-mode
+/// * parse device tree
+/// * Init plic priorities
+/// * Set trap vector
+/// * Set ppn via setp
+/// * Set stack pointer
+/// * Jump to `enter_user_mode` via asm j instruction
+#[allow(clippy::too_many_lines)]
+extern "C" fn smode_setup(hart_id: usize, dtb_addr: usize) {
+    use PteFlag::{Accessed, Dirty, Exec, Read, User, Valid, Write};
+    unsafe {
+        sstatus::clear_sie();
+        stvec::write(
+            panic_handler as *const fn() as usize + PA2VA_DRAM_OFFSET,
+            stvec::TrapMode::Direct,
+        );
+    }
+
+    // parse device tree
+    let device_tree = unsafe {
+        match fdt::Fdt::from_ptr(dtb_addr as *const u8) {
+            Ok(fdt) => fdt,
+            Err(e) => panic!("{}", e),
+        }
+    };
+    let mmap = DeviceMemmap::new(device_tree);
+
+    // set page table for each devices
+    let page_table_start = PAGE_TABLE_BASE + hart_id * PAGE_TABLE_OFFSET_PER_HART;
+    mmap.device_mapping(page_table_start);
+
+    // set plic priorities
+    for plic_num in 1..127 {
+        unsafe {
+            *((mmap.plic.vaddr() + plic_num * 4) as *mut u32) = 1;
+        }
+    }
+
+    let mut irq_mask = 0;
+    for vio in mmap.virtio.iter().take(4) {
+        irq_mask |= 1 << vio.irq();
+    }
+
+    // set plic
+    unsafe {
+        ((mmap.plic.paddr() + CONTEXT_BASE + CONTEXT_PER_HART * mmap.plic_context) as *mut u32)
+            .write_volatile(0);
+        ((mmap.plic.paddr() + ENABLE_BASE + ENABLE_PER_HART * mmap.plic_context) as *mut u32)
+            .write_volatile(irq_mask);
+        ((mmap.plic.paddr() + ENABLE_BASE + ENABLE_PER_HART * mmap.plic_context + CONTEXT_CLAIM)
+            as *mut u32)
+            .write_volatile(0);
+    }
+
+    let guest_id = hart_id + 1;
+    let guest_base_addr = DRAM_BASE + guest_id * DRAM_SIZE_PAR_HART;
+    unsafe {
+        // boot page tables
+        let page_table_start = guest_base_addr;
+        let memory_map: [MemoryMap; 7] = [
+            // (virtual_memory_range, physical_memory_range, flags),
+            // Device tree
+            MemoryMap::new(
+                0xbfe0_0000..0xc000_0000,
+                0xbfe0_0000..0xc000_0000,
+                &[Dirty, Accessed, Write, Read, Valid],
+            ),
+            // TEXT (physical map)
+            MemoryMap::new(
+                0x8000_0000..0x8020_0000,
+                0x8000_0000..0x8020_0000,
+                &[Dirty, Accessed, Exec, Read, Valid],
+            ),
+            // RAM
+            MemoryMap::new(
+                0x8020_0000..0x8080_0000,
+                0x8020_0000..0x8080_0000,
+                &[Dirty, Accessed, Write, Read, Valid],
+            ),
+            // RAM
+            MemoryMap::new(
+                0x9000_0000..0x9040_0000,
+                0x9000_0000..0x9040_0000,
+                &[Dirty, Accessed, Write, Read, Valid],
+            ),
+            // Privious stack area
+            MemoryMap::new(
+                0xffff_ffff_c040_0000..0xffff_ffff_c080_0000,
+                0x8040_0000..0x8080_0000,
+                &[Dirty, Accessed, Write, Read, Valid],
+            ),
+            // RAM
+            MemoryMap::new(
+                0xffff_ffff_d000_0000..0xffff_ffff_d300_0000,
+                0x9000_0000..0x9300_0000,
+                &[Dirty, Accessed, User, Write, Read, Valid],
+            ),
+            // TEXT
+            MemoryMap::new(
+                0xffff_ffff_d300_0000..0xffff_ffff_d600_0000,
+                0x9300_0000..0x9600_0000,
+                &[Dirty, Accessed, Exec, Write, Read, Valid],
+            ),
+        ];
+        page_table::generate_page_table(page_table_start, &memory_map, true);
+        mmap.device_mapping(page_table_start);
+
+        // allow access to user page to supervisor priv
+        sstatus::set_sum();
+        // satp = Sv39 | 0x9000_0000 >> 12
+        satp::set(satp::Mode::Sv39, 0, page_table_start >> 12);
+
+        // copy dtb to guest space
+        let guest_dtb_addr = guest_base_addr + GUEST_DEVICE_TREE_OFFSET + PA2VA_DRAM_OFFSET;
+        core::ptr::copy(
+            dtb_addr as *const u8,
+            guest_dtb_addr as *mut u8,
+            device_tree.total_size(),
+        );
+
+        // copy initrd to guest space
+        core::ptr::copy(
+            mmap.initrd.vaddr() as *const u8,
+            (guest_base_addr + GUEST_HEAP_OFFSET + PA2VA_DRAM_OFFSET) as *mut u8,
+            mmap.initrd.size(),
+        );
+
+        // set sie = 0x222
+        sie::set_ssoft();
+        sie::set_stimer();
+        sie::set_sext();
+
+        let stack_pointer = guest_base_addr + GUEST_STACK_OFFSET + PA2VA_DRAM_OFFSET;
+        asm!(
+            "
+            mv a0, {hart_id}
+            mv a1, {dtb_addr}
+            mv a2, {guest_base_addr}
+            mv a3, {guest_id}
+            mv a4, {guest_initrd_size}
+            mv sp, {stack_pointer_in_umode}
+            j {enter_user_mode}
+            ",
+            hart_id = in(reg) hart_id,
+            dtb_addr = in(reg) guest_dtb_addr,
+            guest_base_addr = in(reg) guest_base_addr,
+            guest_id = in(reg) guest_id,
+            guest_initrd_size = in(reg) mmap.initrd.size(),
+            stack_pointer_in_umode = in(reg) stack_pointer ,
+            enter_user_mode = sym enter_user_mode
+        );
+    }
+}
+
+/// Load elf to guest memory.
+///
+/// It only load `PT_LOAD` type segments.
+/// Entry address is determined by ... .
+///
+/// # Arguments
+/// * `guest_elf` - Elf loading guest space.
+/// * `guest_base_addr` - Base address of loading memory space.
+fn load_elf(guest_elf: &ElfBytes<AnyEndian>, elf_addr: *mut u8, guest_base_addr: usize) -> usize {
+    for prog_header in guest_elf
+        .segments()
+        .expect("failed to get segments from elf")
+        .iter()
+    {
+        const PT_LOAD: u32 = 1;
+        if prog_header.p_type == PT_LOAD && prog_header.p_filesz > 0 {
+            unsafe {
+                core::ptr::copy(
+                    elf_addr.wrapping_add(usize::try_from(prog_header.p_offset).unwrap()),
+                    (guest_base_addr + usize::try_from(prog_header.p_paddr).unwrap()) as *mut u8,
+                    usize::try_from(prog_header.p_filesz).unwrap(),
+                );
+            }
+        }
+    }
+
+    guest_base_addr
+}
+
+/// Prepare to enter U-mode and jump to linux kernel
+fn enter_user_mode(
+    _hart_id: usize,
+    dtb_addr: usize,
+    guest_base_addr: usize,
+    _guest_id: usize,
+    guest_initrd_size: usize,
+) {
+    unsafe {
+        // set sie = 0x222
+        sie::set_ssoft();
+        sie::set_stimer();
+        sie::set_sext();
+
+        // sstatus.SUM = 1, sstatus.SPP = 0
+        sstatus::set_sum();
+        sstatus::set_spp(sstatus::SPP::Supervisor);
+
+        // copy initrd to guest text space(0x9000_0000-) and set initrd entry point to sepc
+        let elf_addr = (guest_base_addr + GUEST_HEAP_OFFSET + PA2VA_DRAM_OFFSET) as *mut u8;
+        let guest_elf = ElfBytes::<AnyEndian>::minimal_parse(core::slice::from_raw_parts(
+            elf_addr,
+            guest_initrd_size,
+        ))
+        .unwrap();
+        let entry_point = load_elf(
+            &guest_elf,
+            elf_addr,
+            guest_base_addr + GUEST_TEXT_OFFSET + PA2VA_DRAM_OFFSET,
+        );
+        sepc::write(entry_point);
+
+        // stvec = trap_vector
+        stvec::write(
+            strap_vector as *const fn() as usize,
+            stvec::TrapMode::Direct,
+        );
+
+        asm!(
+            "
+            mv a1, {dtb_addr}
+
+            li ra, 0
+            li sp, 0
+            li gp, 0
+            li tp, 0
+            li t0, 0
+            li t1, 0
+            li t2, 0
+            li s0, 0
+            li s1, 0
+            li a0, 0
+            li a2, 0
+            li a3, 0
+            li a4, 0
+            li a5, 0
+            li a6, 0
+            li a7, 0
+            li s2, 0
+            li s3, 0
+            li s4, 0
+            li s5, 0
+            li s6, 0
+            li s7, 0
+            li s8, 0
+            li s9, 0
+            li s10, 0
+            li s11, 0
+            li t3, 0
+            li t4, 0
+            li t5, 0
+            li t6, 0
+            sret
+            ",
+            dtb_addr = in(reg) dtb_addr
+        );
+    }
+    unreachable!();
+}
+
+/// Panic handler for S-mode
+fn panic_handler() {
+    panic!("trap from panic macro")
+}
diff --git a/src/trap.S b/src/trap.S
new file mode 100644
index 0000000..f0a8eee
--- /dev/null
+++ b/src/trap.S
@@ -0,0 +1,158 @@
+.equ SYSCALL_NUM_ECALL, 9
+.globl trap_vector
+
+.align 4
+trap_vector:
+    addi sp, sp, -128
+    sd ra, 0(sp)
+    sd t0, 8(sp)
+    sd t1, 16(sp)
+    sd t2, 24(sp)
+    sd t3, 32(sp)
+    sd t4, 40(sp)
+    sd t5, 48(sp)
+    sd t6, 56(sp)
+    sd a0, 64(sp)
+    sd a1, 72(sp)
+    sd a2, 80(sp)
+    sd a3, 88(sp)
+    sd a4, 96(sp)
+    sd a5, 104(sp)
+    sd a6, 112(sp)
+    sd a7, 120(sp)
+
+    csrr t0, mcause
+    bgez t0, exception
+    j interrupt
+
+interrupt:
+    li t1, 0x8000000000000003
+    beq t0, t1, machine_software_interrupt
+    
+    li t1, 0x8000000000000007
+    beq t0, t1, machine_timer_interrupt
+    
+    li t1, 0x800000000000000b
+    beq t0, t1, machine_external_interrupt
+
+unknown_interrupt:
+    j unknown_interrupt // loop
+
+machine_software_interrupt:
+    csrsi mip, 0x2 // mip.ssip = 1
+
+    csrr t0, mhartid
+    slli t0, t0, 2
+    li t1, 0x2000000
+    add t1, t1, t0
+    sw zero, 0(t1) // mtimecmp[hart_id] = 0
+
+    j return
+
+machine_timer_interrupt:
+    li t0, 0x20
+    csrs mip, t0 // mip.stip = 1
+
+    csrr t0, mhartid
+    slli t0, t0, 3
+    li t1, 0x2004000 // mtimecmp
+    add t1, t1, t0
+    li t0, 0xffffffffffff
+    sd t0, 0(t1) // mtimecmp[hart_id] = 0
+
+    j return
+
+machine_external_interrupt:
+    j machine_external_interrupt // loop
+
+exception:
+    li t1, SYSCALL_NUM_ECALL
+    beq t0, t1, ecall_exception
+
+    // other exception
+    call forward_exception
+
+    j return
+
+ecall_exception:
+	csrr t0, mepc
+	addi t0, t0, 4
+	csrw mepc, t0
+
+	beqz a7, sbi_set_timer
+
+	li t1, 3
+	beq a7, t1, sbi_clear_ipi
+
+	li t1, 4
+	beq a7, t1, sbi_send_ipi
+
+	li t1, 8
+	beq a7, t1, sbi_shutdown
+
+	j unknown_exception
+
+unknown_exception:
+    j unknown_exception
+
+sbi_set_timer:
+	csrr t0, mhartid
+	slli t0, t0, 3
+	li t1, 0x2004000
+	add t1, t0, t1
+	sd a0, 0(t1)  // mtimecmp[hartid] = a0
+
+	li t0, 0x20
+	csrc mip, t0 // mip.stip = 0
+
+	li a0, 0
+	j return_with_value
+
+sbi_clear_ipi:
+	csrci mip, 0x2
+	li a0, 0
+	j return_with_value
+
+sbi_send_ipi:
+	li t2, 1 << 17 // t2 = MPRV
+	csrrs t1, mstatus, t2
+	ld t0, 0(a0)
+	csrw mstatus, t1
+
+	li t2, (0x02000000 - 4)
+1:	addi t2, t2, 4
+	andi t1, t0, 0x1
+	srli t0, t0, 1
+	beqz t1, 1b
+	sw t1, 0(t2)
+	bnez t0, 1b
+
+	li a0, 0
+	j return_with_value
+
+sbi_shutdown:
+	j sbi_shutdown
+
+
+return:
+	ld a0, 64(sp)
+return_with_value:
+	ld ra, 0(sp)
+	ld t0, 8(sp)
+	ld t1, 16(sp)
+	ld t2, 24(sp)
+	ld t3, 32(sp)
+	ld t4, 40(sp)
+	ld t5, 48(sp)
+	ld t6, 56(sp)
+	ld a1, 72(sp)
+	ld a2, 80(sp)
+	ld a3, 88(sp)
+	ld a4, 96(sp)
+	ld a5, 104(sp)
+	ld a6, 112(sp)
+	ld a7, 120(sp)
+	addi sp, sp, 128
+	csrrw sp, mscratch, sp
+	mret
+
diff --git a/src/trap.rs b/src/trap.rs
new file mode 100644
index 0000000..dbc71a6
--- /dev/null
+++ b/src/trap.rs
@@ -0,0 +1,152 @@
+pub mod machine;
+pub mod supervisor;
+
+use core::arch::asm;
+use riscv::register::mcause;
+use riscv::register::mcause::{Exception, Interrupt};
+use riscv::register::{mepc, mhartid, mip, mstatus, mtval, scause, sepc, stval, stvec};
+
+/// CLINT MTIMECMP address
+const MTIMECMP_ADDR: usize = 0x200_4000;
+
+/// Trap handler for exception
+#[allow(clippy::cast_possible_wrap)]
+unsafe fn trap_exception(a0: u64, a7: u64, exception_cause: Exception) {
+    let ret_with_value = |ret_value: u64| {
+        asm!("
+            ld ra, 0(sp)
+            ld t0, 8(sp)
+            ld t1, 16(sp)
+            ld t2, 24(sp)
+            ld t3, 32(sp)
+            ld t4, 40(sp)
+            ld t5, 48(sp)
+            ld t6, 56(sp)
+            mv a0, {ret_value}
+            ld a1, 72(sp)
+            ld a2, 80(sp)
+            ld a3, 88(sp)
+            ld a4, 96(sp)
+            ld a5, 104(sp)
+            ld a6, 112(sp)
+            ld a7, 120(sp)
+            ld s2, 128(sp)
+            ld s3, 136(sp)
+            ld s4, 144(sp)
+            ld s5, 152(sp)
+            ld s6, 160(sp)
+            ld s7, 168(sp)
+            ld s8, 176(sp)
+            ld s9, 184(sp)
+            ld s10, 192(sp)
+            ld s11, 200(sp)
+            ld t3, 208(sp)
+            ld t4, 216(sp)
+            ld t5, 224(sp)
+            ld t6, 232(sp)
+
+            addi sp, sp, 240
+            csrrw sp, mscratch, sp
+            mret
+            ",
+            ret_value = in(reg) ret_value,
+        );
+    };
+
+    if exception_cause == Exception::UserEnvCall {
+        // https://doxygen.coreboot.org/d6/dfc/sbi_8c_source.html
+        mepc::write(mepc::read() + 4);
+
+        // ecall_number = a7
+        let ecall_number: u64 = a7;
+        match ecall_number {
+            // sbi_set_timer
+            0 => {
+                // timer_value = a0
+                let timer_value: u64 = a0;
+
+                let mtimecmp_addr = (MTIMECMP_ADDR + mhartid::read() * 8) as *mut u64;
+                mtimecmp_addr.write_volatile(timer_value);
+
+                ret_with_value(0);
+                unreachable!();
+            }
+            // sbi_clear_ipi
+            3 => {
+                mip::clear_ssoft();
+
+                ret_with_value(0);
+                unreachable!();
+            }
+            // sbi_send_ipi
+            4 => {
+                // mask_addr = a0
+                let mask_addr: *mut u64 = a0 as *mut u64;
+                let mut mask = if mstatus::read().mprv() {
+                    mask_addr.read_volatile()
+                } else {
+                    mstatus::set_mprv();
+                    let mask = mask_addr.read_volatile();
+                    mstatus::clear_mprv();
+                    mask
+                };
+
+                let mut clint_addr: *mut u8 = 0x200_0000 as *mut u8;
+                while mask != 0 {
+                    if mask & 1 == 1 {
+                        clint_addr.write_volatile(1);
+                    }
+                    clint_addr = clint_addr.add(4);
+                    mask >>= 1;
+                }
+
+                ret_with_value(0);
+                unreachable!();
+            }
+            // sbi_shutdown
+            8 => panic!("sbi shutdown"),
+            // other
+            _ => panic!("unknown ecall number"),
+        }
+    } else {
+        sepc::write(mepc::read());
+        scause::write(mcause::read().bits());
+        stval::write(mtval::read());
+        mepc::write(stvec::read().bits() & !0x3);
+
+        if mstatus::read().sie() {
+            mstatus::set_spie();
+        } else {
+            // clear?
+        }
+
+        if mstatus::read().mpp() == mstatus::MPP::Supervisor {
+            mstatus::set_spp(mstatus::SPP::Supervisor);
+        } else {
+            mstatus::set_spp(mstatus::SPP::User);
+        }
+
+        mstatus::clear_sie();
+        mstatus::set_mpp(mstatus::MPP::Supervisor);
+    }
+}
+
+/// Trap handler for Interrupt
+unsafe fn trap_interrupt(interrupt_cause: Interrupt) {
+    const CLINT_ADDR: usize = 0x200_0000;
+
+    match interrupt_cause {
+        Interrupt::MachineSoft => {
+            mip::set_ssoft();
+            let interrupt_addr = (CLINT_ADDR + mhartid::read() * 4) as *mut u64;
+            interrupt_addr.write_volatile(0);
+        }
+        Interrupt::MachineTimer => {
+            mip::set_stimer();
+            let mtimecmp_addr = (MTIMECMP_ADDR + mhartid::read() * 8) as *mut u64;
+            mtimecmp_addr.write_volatile(u64::MAX);
+        }
+        Interrupt::MachineExternal => riscv::asm::wfi(), // wait for interrupt
+        _ => panic!("unknown interrupt type"),
+    }
+}
diff --git a/src/trap/machine.rs b/src/trap/machine.rs
new file mode 100644
index 0000000..e4715e5
--- /dev/null
+++ b/src/trap/machine.rs
@@ -0,0 +1,96 @@
+use super::{trap_exception, trap_interrupt};
+// use crate::memmap::constant::STACK_BASE;
+use core::arch::asm;
+use riscv::register::mcause;
+use riscv::register::mcause::Trap;
+
+#[no_mangle]
+pub unsafe extern "C" fn mtrap_vector() {
+    asm!(
+        ".align 4
+        csrrw sp, mscratch, sp
+        li sp, 0x80300000
+
+        addi sp, sp, -240
+        sd ra, 0(sp)
+        sd t0, 8(sp)
+        sd t1, 16(sp)
+        sd t2, 24(sp)
+        sd t3, 32(sp)
+        sd t4, 40(sp)
+        sd t5, 48(sp)
+        sd t6, 56(sp)
+        sd a0, 64(sp)
+        sd a1, 72(sp)
+        sd a2, 80(sp)
+        sd a3, 88(sp)
+        sd a4, 96(sp)
+        sd a5, 104(sp)
+        sd a6, 112(sp)
+        sd a7, 120(sp)
+        sd s2, 128(sp)
+        sd s3, 136(sp)
+        sd s4, 144(sp)
+        sd s5, 152(sp)
+        sd s6, 160(sp)
+        sd s7, 168(sp)
+        sd s8, 176(sp)
+        sd s9, 184(sp)
+        sd s10, 192(sp)
+        sd s11, 200(sp)
+        sd t3, 208(sp)
+        sd t4, 216(sp)
+        sd t5, 224(sp)
+        sd t6, 232(sp)
+        ",
+        // stack_base = const STACK_BASE
+    );
+
+    let a0: u64 = 0;
+    let a7: u64 = 0;
+    asm!("ld {a0_reg}, 64(sp)", a0_reg = in(reg) a0);
+    asm!("ld {a7_reg}, 120(sp)", a7_reg = in(reg) a7);
+    match mcause::read().cause() {
+        Trap::Interrupt(interrupt_cause) => trap_interrupt(interrupt_cause),
+        Trap::Exception(exception_cause) => trap_exception(a0, a7, exception_cause),
+    }
+
+    asm!(
+        "
+        ld ra, 0(sp)
+        ld t0, 8(sp)
+        ld t1, 16(sp)
+        ld t2, 24(sp)
+        ld t3, 32(sp)
+        ld t4, 40(sp)
+        ld t5, 48(sp)
+        ld t6, 56(sp)
+        ld a0, 64(sp)
+        ld a1, 72(sp)
+        ld a2, 80(sp)
+        ld a3, 88(sp)
+        ld a4, 96(sp)
+        ld a5, 104(sp)
+        ld a6, 112(sp)
+        ld a7, 120(sp)
+        ld s2, 128(sp)
+        ld s3, 136(sp)
+        ld s4, 144(sp)
+        ld s5, 152(sp)
+        ld s6, 160(sp)
+        ld s7, 168(sp)
+        ld s8, 176(sp)
+        ld s9, 184(sp)
+        ld s10, 192(sp)
+        ld s11, 200(sp)
+        ld t3, 208(sp)
+        ld t4, 216(sp)
+        ld t5, 224(sp)
+        ld t6, 232(sp)
+
+        addi sp, sp, 240
+        csrrw sp, mscratch, sp
+        mret
+        ",
+    );
+}
diff --git a/src/trap/supervisor.rs b/src/trap/supervisor.rs
new file mode 100644
index 0000000..9f85d6d
--- /dev/null
+++ b/src/trap/supervisor.rs
@@ -0,0 +1,96 @@
+use super::{trap_exception, trap_interrupt};
+// use crate::memmap::constant::STACK_BASE;
+use core::arch::asm;
+use riscv::register::mcause;
+use riscv::register::mcause::Trap;
+
+#[no_mangle]
+pub unsafe extern "C" fn strap_vector() {
+    asm!(
+        ".align 4
+        csrrw sp, sscratch, sp
+        li sp, 0x80300000
+
+        addi sp, sp, -240
+        sd ra, 0(sp)
+        sd t0, 8(sp)
+        sd t1, 16(sp)
+        sd t2, 24(sp)
+        sd t3, 32(sp)
+        sd t4, 40(sp)
+        sd t5, 48(sp)
+        sd t6, 56(sp)
+        sd a0, 64(sp)
+        sd a1, 72(sp)
+        sd a2, 80(sp)
+        sd a3, 88(sp)
+        sd a4, 96(sp)
+        sd a5, 104(sp)
+        sd a6, 112(sp)
+        sd a7, 120(sp)
+        sd s2, 128(sp)
+        sd s3, 136(sp)
+        sd s4, 144(sp)
+        sd s5, 152(sp)
+        sd s6, 160(sp)
+        sd s7, 168(sp)
+        sd s8, 176(sp)
+        sd s9, 184(sp)
+        sd s10, 192(sp)
+        sd s11, 200(sp)
+        sd t3, 208(sp)
+        sd t4, 216(sp)
+        sd t5, 224(sp)
+        sd t6, 232(sp)
+        ",
+        // stack_base = in(reg) STACK_BASE
+    );
+
+    let a0: u64 = 0;
+    let a7: u64 = 0;
+    asm!("ld {a0_reg}, 64(sp)", a0_reg = in(reg) a0);
+    asm!("ld {a7_reg}, 120(sp)", a7_reg = in(reg) a7);
+    match mcause::read().cause() {
+        Trap::Interrupt(interrupt_cause) => trap_interrupt(interrupt_cause),
+        Trap::Exception(exception_cause) => trap_exception(a0, a7, exception_cause),
+    }
+
+    asm!(
+        "
+        ld ra, 0(sp)
+        ld t0, 8(sp)
+        ld t1, 16(sp)
+        ld t2, 24(sp)
+        ld t3, 32(sp)
+        ld t4, 40(sp)
+        ld t5, 48(sp)
+        ld t6, 56(sp)
+        ld a0, 64(sp)
+        ld a1, 72(sp)
+        ld a2, 80(sp)
+        ld a3, 88(sp)
+        ld a4, 96(sp)
+        ld a5, 104(sp)
+        ld a6, 112(sp)
+        ld a7, 120(sp)
+        ld s2, 128(sp)
+        ld s3, 136(sp)
+        ld s4, 144(sp)
+        ld s5, 152(sp)
+        ld s6, 160(sp)
+        ld s7, 168(sp)
+        ld s8, 176(sp)
+        ld s9, 184(sp)
+        ld s10, 192(sp)
+        ld s11, 200(sp)
+        ld t3, 208(sp)
+        ld t4, 216(sp)
+        ld t5, 224(sp)
+        ld t6, 232(sp)
+
+        addi sp, sp, 240
+        csrrw sp, sscratch, sp
+        sret
+        ",
+    );
+}
diff --git a/src/util.rs b/src/util.rs
new file mode 100644
index 0000000..978ee37
--- /dev/null
+++ b/src/util.rs
@@ -0,0 +1,36 @@
+use crate::memmap::constant::UART_ADDR;
+use core::fmt::{self, Write};
+
+#[macro_export]
+macro_rules! print {
+    ($($arg:tt)*) => ($crate::util::_print(format_args!($($arg)*)));
+}
+
+#[macro_export]
+macro_rules! println {
+    ($fmt:expr) => (print!(concat!($fmt, "\n")));
+    ($fmt:expr, $($arg:tt)*) => (print!(concat!($fmt, "\n"), $($arg)*));
+}
+
+/// Print function calling from print macro
+pub fn _print(args: fmt::Arguments) {
+    let mut writer = UartWriter {};
+    writer.write_fmt(args).unwrap();
+}
+
+struct UartWriter;
+
+impl Write for UartWriter {
+    /// Write string to tty via UART.
+    #[allow(clippy::cast_possible_wrap)]
+    fn write_str(&mut self, s: &str) -> fmt::Result {
+        let uart_addr = UART_ADDR as *mut u32;
+        for c in s.bytes() {
+            unsafe {
+                while (uart_addr.read_volatile() as i32) < 0 {}
+                uart_addr.write_volatile(u32::from(c));
+            }
+        }
+        Ok(())
+    }
+}
diff --git a/wild-screen-alloc b/wild-screen-alloc
new file mode 160000
index 0000000..6abe81f
--- /dev/null
+++ b/wild-screen-alloc
@@ -0,0 +1 @@
+Subproject commit 6abe81f71d248f11530ccbf21373083b3c3fd2e7