diff --git a/.cargo/config.toml b/.cargo/config.toml
index f0716781..61d5f46c 100644
--- a/.cargo/config.toml
+++ b/.cargo/config.toml
@@ -3,7 +3,7 @@ runner = """
 qemu-system-riscv64
 -cpu rv64,smstateen=true
 -machine virt
--bios none
+-bios default
 -nographic
 -m 2G
 -initrd vmlinux_debug
@@ -22,7 +22,6 @@ qemu-system-riscv64
 
 rustflags = [
   "-C", "link-arg=-Tmemory.x",
-  "-C", "link-arg=-Tlink.x",
   "-C", "target-feature=+h",
 ]
 
diff --git a/Cargo.toml b/Cargo.toml
index a40e9819..26caf523 100644
--- a/Cargo.toml
+++ b/Cargo.toml
@@ -22,10 +22,9 @@ embedded_host_dtb = []
 elf = { version = "0.7.2", default-features = false }
 fdt = "0.1.5"
 linked_list_allocator = "0.10.5"
-raki = "1.2.0"
+raki = "1.3.1"
 riscv = "0.11.1"
-riscv-rt = { git = "https://github.com/Alignof/riscv", branch = "fix/link_error_on_latest_rust" }
 rustsbi = { version = "0.4.0", features = ["machine"] }
 sbi-rt = "0.0.3"
-sbi-spec = { version = "0.0.7", features = [ "legacy" ] }
+sbi-spec = { version = "0.0.8", features = [ "legacy" ] }
 spin = "0.9.8"
diff --git a/guest.dtb b/guest.dtb
index 67ca935e..d377c0b3 100644
Binary files a/guest.dtb and b/guest.dtb differ
diff --git a/guest_image/build_dtb.sh b/guest_image/build_dtb.sh
index 27740639..da2eda05 100755
--- a/guest_image/build_dtb.sh
+++ b/guest_image/build_dtb.sh
@@ -10,7 +10,7 @@ function help() {
 function create_host() {
     $qemu_path -S -gdb tcp::10000 \
         -machine virt \
-        -bios none  \
+        -bios default \
         -m 256M \
         -initrd ../vmlinux_debug \
         -drive file=../rootfs.ext2,format=raw,id=hd0,if=none \
@@ -28,7 +28,7 @@ function create_host() {
 function create_guest() {
     $qemu_path -S -gdb tcp::10000 \
         -machine virt \
-        -bios none  \
+        -bios default \
         -m 256M \
         -initrd ../vmlinux_debug \
         -drive file=../rootfs.ext2,format=raw,id=hd0,if=none \
diff --git a/guest_image/guest.dts b/guest_image/guest.dts
index ffe6b007..fe50a0fb 100644
--- a/guest_image/guest.dts
+++ b/guest_image/guest.dts
@@ -90,10 +90,10 @@
 
 	chosen {
 		bootargs = "root=/dev/vda rw console=ttyS0";
-		linux,initrd-end = <0x00 0x9812b230>;
-		linux,initrd-start = <0x00 0x88000000>;
+		linux,initrd-end = <0x00 0x9832b230>;
+		linux,initrd-start = <0x00 0x88200000>;
 		stdout-path = "/soc/serial@10000000";
-		rng-seed = <0x2b05a034 0xe739bfb7 0x512d2025 0xf0ec97e8 0x69399e86 0x1f2de5a9 0xcacea4aa 0xafb54655>;
+		rng-seed = <0x9775b34d 0xe39158f2 0x3e9d10b1 0x73692dfd 0x13c15814 0xa4ad203f 0x8b0d62f7 0x6bf8a79d>;
 	};
 
 	soc {
diff --git a/guest_image/host.dts b/guest_image/host.dts
index 9c840f52..ec884b89 100644
--- a/guest_image/host.dts
+++ b/guest_image/host.dts
@@ -90,10 +90,10 @@
 
 	chosen {
 		bootargs = "root=/dev/vda rw console=ttyS0";
-		linux,initrd-end = <0x00 0x9812b230>;
-		linux,initrd-start = <0x00 0x88000000>;
+		linux,initrd-end = <0x00 0x9832b230>;
+		linux,initrd-start = <0x00 0x88200000>;
 		stdout-path = "/soc/serial@10000000";
-		rng-seed = <0x43b57d4e 0xc47d1e53 0xf72be401 0x8782f496 0x6b3aef66 0xccfa8bb3 0x710c4d0d 0x40c7b7c6>;
+		rng-seed = <0xb7ff67db 0xb4fa9154 0xf9f2f06c 0xed096f3b 0x953e577a 0x32c67d9 0x8272b7a5 0xb93e1980>;
 	};
 
 	soc {
diff --git a/host.dtb b/host.dtb
index f8f70461..ac9c8ec7 100644
Binary files a/host.dtb and b/host.dtb differ
diff --git a/memory.x b/memory.x
index caaa17b6..de564ea8 100644
--- a/memory.x
+++ b/memory.x
@@ -1,16 +1,16 @@
 MEMORY
 {
-  FLASH (rx) : ORIGIN = 0x80000000, LENGTH = 2M
-  MACHINE_RAM (rw) : ORIGIN = 0x80200000, LENGTH = 6M
+  FLASH (rx) : ORIGIN = 0x80200000, LENGTH = 2M
+  BOOT_RAM (rw) : ORIGIN = 0x80400000, LENGTH = 6M
   RAM (rwx) : ORIGIN = 0xc1000000, LENGTH = 528M
   L2_LIM (rw) : ORIGIN = 0xe2000000, LENGTH = 8M
 }
 
 /*
- * FLASH (TEXT), 0x8000_0000..0x8020_0000
- * MACHINE_RAM , 0x8020_0000..0x8080_0000
- * RAM (DATA, BSS, HEAP), 0x8100_0000..0xa200_0000
- * L2_LIM (STACK), 0xa200_0000..0xa300_0000
+ * FLASH (TEXT), 0x8020_0000..0x8040_0000
+ * BOOT_RAM , 0x8040_0000..0x80a0_0000
+ * RAM (DATA, BSS, HEAP), 0xc100_0000..0xe200_0000
+ * L2_LIM (STACK), 0xe200_0000..0xa300_0000
  */
 
 REGION_ALIAS("REGION_TEXT", FLASH);
@@ -22,16 +22,22 @@ REGION_ALIAS("REGION_STACK", L2_LIM);
 
 _stack_start = ORIGIN(L2_LIM) + LENGTH(L2_LIM);
 _hv_heap_size = 0x20000000;
-_m_stack_size = 0x200000;
+_b_stack_size = 0x200000;
 
 /* defined section in hikami */
 SECTIONS
 {
-    .machine_stack : ALIGN(4K) {
-        _bottom_m_stack = .;
-        . += _m_stack_size;
-        _top_m_stack = .;
-    } > MACHINE_RAM
+    .text : {
+        *(.text.entry)
+        . = ALIGN(4K);
+        *(.text .text.*)
+    } > REGION_TEXT
+
+    .boot_stack : ALIGN(4K) {
+        _bottom_b_stack = .;
+        . += _b_stack_size;
+        _top_b_stack = .;
+    } > BOOT_RAM
 
     .host_dtb : ALIGN(4K)
     {
diff --git a/rust-toolchain.toml b/rust-toolchain.toml
index b445b062..11f7a7a7 100644
--- a/rust-toolchain.toml
+++ b/rust-toolchain.toml
@@ -1,4 +1,4 @@
 [toolchain]
-channel = "1.82.0"
+channel = "nightly"
 components = [ "rustfmt", "clippy" ]
 targets = [ "riscv64imac-unknown-none-elf" ]
diff --git a/src/device/iommu.rs b/src/device/iommu.rs
index 24257204..3b1cbc09 100644
--- a/src/device/iommu.rs
+++ b/src/device/iommu.rs
@@ -57,16 +57,16 @@ impl PciDevice for IoMmu {
             .next()
             .unwrap();
         assert_eq!(pci_reg.address.len(), 12); // 4 bytes * 3
-        let pci_first_reg = u32::from(pci_reg.address[0]) << 24
-            | u32::from(pci_reg.address[1]) << 16
-            | u32::from(pci_reg.address[2]) << 8
+        let pci_first_reg = (u32::from(pci_reg.address[0]) << 24)
+            | (u32::from(pci_reg.address[1]) << 16)
+            | (u32::from(pci_reg.address[2]) << 8)
             | u32::from(pci_reg.address[3]);
 
         // https://www.kernel.org/doc/Documentation/devicetree/bindings/pci/pci.txt
         Some(IoMmu {
-            bus: pci_first_reg >> 16 & 0b1111_1111, // 8 bit
-            device: pci_first_reg >> 11 & 0b1_1111, // 5 bit
-            function: pci_first_reg >> 8 & 0b111,   // 3 bit
+            bus: (pci_first_reg >> 16) & 0b1111_1111, // 8 bit
+            device: (pci_first_reg >> 11) & 0b1_1111, // 5 bit
+            function: (pci_first_reg >> 8) & 0b111,   // 3 bit
         })
     }
 
diff --git a/src/device/iommu/register_map.rs b/src/device/iommu/register_map.rs
index f13d65b0..37fb7486 100644
--- a/src/device/iommu/register_map.rs
+++ b/src/device/iommu/register_map.rs
@@ -49,7 +49,7 @@ impl Capabilities {
     /// Return (major version, minor version)
     pub fn version(&self) -> (u8, u8) {
         let version_reg = self.0;
-        ((version_reg >> 4 & 0xf) as u8, (version_reg & 0xf) as u8)
+        (((version_reg >> 4) & 0xf) as u8, (version_reg & 0xf) as u8)
     }
 
     /// Is base format?
@@ -65,7 +65,7 @@ impl Capabilities {
         /// Field `Sv39x4` of `capabilities` register.
         const FIELD_CAPABILITIES_SV39X4: usize = 17;
 
-        self.0 >> FIELD_CAPABILITIES_SV39X4 & 0x1 == 1
+        (self.0 >> FIELD_CAPABILITIES_SV39X4) & 0x1 == 1
     }
 }
 
@@ -78,7 +78,7 @@ impl Cqb {
         assert!(queue_addr % 4096 == 0);
 
         // CQB.PPN = B, CQB.LOG2SZ-1 = k - 1
-        self.0 = (queue_addr.0 as u64 >> 12) << 10 | u64::from(size.ilog2() - 1);
+        self.0 = ((queue_addr.0 as u64 >> 12) << 10) | u64::from(size.ilog2() - 1);
     }
 }
 
@@ -118,7 +118,7 @@ impl Fqb {
         assert!(queue_addr % 4096 == 0);
 
         // FQB.PPN = B, FQB.LOG2SZ-1 = k - 1
-        self.0 = (queue_addr.0 as u64 >> 12) << 10 | u64::from(size.ilog2() - 1);
+        self.0 = ((queue_addr.0 as u64 >> 12) << 10) | u64::from(size.ilog2() - 1);
     }
 }
 
@@ -144,7 +144,7 @@ impl FqCsr {
         /// Field `fqon` of `fqcsr` register. (16 bit)
         const FIELD_FQCSR_FQON: usize = 0x10;
         let fqcsr = self.0;
-        fqcsr >> FIELD_FQCSR_FQON & 0x1 == 1
+        (fqcsr >> FIELD_FQCSR_FQON) & 0x1 == 1
     }
 }
 
@@ -157,7 +157,7 @@ impl Pqb {
         assert!(queue_addr % 4096 == 0);
 
         // PQB.PPN = B, PQB.LOG2SZ-1 = k - 1
-        self.0 = (queue_addr.0 as u64 >> 12) << 10 | u64::from(size.ilog2() - 1);
+        self.0 = ((queue_addr.0 as u64 >> 12) << 10) | u64::from(size.ilog2() - 1);
     }
 }
 
@@ -184,7 +184,7 @@ impl PqCsr {
         const FIELD_PQCSR_PQON: usize = 0x10;
 
         let pqcsr = self.0;
-        pqcsr >> FIELD_PQCSR_PQON & 0x1 == 1
+        (pqcsr >> FIELD_PQCSR_PQON) & 0x1 == 1
     }
 }
 
@@ -211,6 +211,6 @@ impl Ddtp {
         /// Field `ppn` of `ddtp` register. (16 bit)
         const FIELD_DDTP_PPN: usize = 10;
 
-        self.0 = (ddt_addr.0 as u64 >> 12) << FIELD_DDTP_PPN | mode as u64;
+        self.0 = ((ddt_addr.0 as u64 >> 12) << FIELD_DDTP_PPN) | mode as u64;
     }
 }
diff --git a/src/device/pci.rs b/src/device/pci.rs
index 9b61cb87..d61f199d 100644
--- a/src/device/pci.rs
+++ b/src/device/pci.rs
@@ -50,9 +50,9 @@ impl Pci {
         reg: ConfigSpaceRegister,
     ) -> u32 {
         let config_data_reg_addr = self.base_addr.0 as u32
-            | (bus_num & 0b1111_1111) << 20
-            | (device_num & 0b1_1111) << 15
-            | (function_num & 0b111) << 12
+            | ((bus_num & 0b1111_1111) << 20)
+            | ((device_num & 0b1_1111) << 15)
+            | ((function_num & 0b111) << 12)
             | reg as u32;
 
         match reg {
@@ -80,9 +80,9 @@ impl Pci {
         data: u32,
     ) {
         let config_data_reg_addr = self.base_addr.0 as u32
-            | (bus_num & 0b1111_1111) << 20
-            | (device_num & 0b1_1111) << 15
-            | (function_num & 0b111) << 12
+            | ((bus_num & 0b1111_1111) << 20)
+            | ((device_num & 0b1_1111) << 15)
+            | ((function_num & 0b111) << 12)
             | reg as u32;
         match reg {
             ConfigSpaceRegister::VendorId
@@ -133,9 +133,9 @@ impl MmioDevice for Pci {
 
         let get_u32 = |range: &[u8], four_bytes_index: usize| {
             let index = four_bytes_index * 4;
-            u32::from(range[index]) << 24
-                | u32::from(range[index + 1]) << 16
-                | u32::from(range[index + 2]) << 8
+            (u32::from(range[index]) << 24)
+                | (u32::from(range[index + 1]) << 16)
+                | (u32::from(range[index + 2]) << 8)
                 | u32::from(range[index + 3])
         };
         let mut memory_maps = Vec::new();
@@ -145,8 +145,8 @@ impl MmioDevice for Pci {
             // ignore I/O space map
             // https://elinux.org/Device_Tree_Usage#PCI_Address_Translation
             if (bus_address >> 24) & 0b11 != 0b01 {
-                let address = (get_u32(range, 3) as usize) << 32 | get_u32(range, 4) as usize;
-                let size = (get_u32(range, 5) as usize) << 32 | get_u32(range, 6) as usize;
+                let address = ((get_u32(range, 3) as usize) << 32) | get_u32(range, 4) as usize;
+                let size = ((get_u32(range, 5) as usize) << 32) | get_u32(range, 6) as usize;
 
                 memory_maps.push(MemoryMap::new(
                     GuestPhysicalAddress(address)..GuestPhysicalAddress(address) + size,
diff --git a/src/device/uart.rs b/src/device/uart.rs
index ef666fcf..310ed431 100644
--- a/src/device/uart.rs
+++ b/src/device/uart.rs
@@ -124,7 +124,7 @@ impl rustsbi::Console for Uart {
         };
         for c in byte_data {
             unsafe {
-                while (uart_lsr_ptr.read_volatile() >> 5 & 0x1) == 1 {}
+                while ((uart_lsr_ptr.read_volatile() >> 5) & 0x1) == 1 {}
                 uart_ptr.write_volatile(u32::from(*c));
             }
         }
@@ -159,7 +159,7 @@ impl rustsbi::Console for Uart {
         let uart_ptr = self.base_addr.raw() as *mut u32;
         let uart_lsr_ptr = self.lsr_addr().raw() as *mut u32;
         unsafe {
-            while (uart_lsr_ptr.read_volatile() >> 5 & 0x1) == 1 {}
+            while ((uart_lsr_ptr.read_volatile() >> 5) & 0x1) == 1 {}
             uart_ptr.write_volatile(u32::from(byte));
         }
         SbiRet::success(0)
diff --git a/src/emulate_extension.rs b/src/emulate_extension.rs
index 290f7bfb..be0bba5f 100644
--- a/src/emulate_extension.rs
+++ b/src/emulate_extension.rs
@@ -3,7 +3,7 @@
 pub mod zicfiss;
 
 use crate::h_extension::csrs::vstvec;
-use crate::trap::hypervisor_supervisor::hstrap_exit;
+use crate::trap::hstrap_exit;
 use crate::HYPERVISOR_DATA;
 
 use core::arch::asm;
@@ -74,10 +74,10 @@ pub fn pseudo_vs_exception(exception_num: usize, trap_value: usize) -> ! {
         let spp = sstatus::read().spp();
         let vsstatus: usize;
         asm!("csrr {status}, vsstatus", status = out(reg) vsstatus);
-        let sie = vsstatus >> 1 & 0x1;
+        let sie = (vsstatus >> 1) & 0x1;
         asm!(
             "csrw vsstatus, {status}",
-            status = in(reg) (vsstatus & !(1 << 8)) | (spp as usize) << 8
+            status = in(reg) (vsstatus & !(1 << 8)) | ((spp as usize) << 8)
         );
         // disable interrupt
         asm!(
@@ -85,7 +85,7 @@ pub fn pseudo_vs_exception(exception_num: usize, trap_value: usize) -> ! {
             "csrci vsstatus, 0b10",
             status = in(reg) sie << 5,
         );
-        context.set_sstatus(context.sstatus() | 1 << 8);
+        context.set_sstatus(context.sstatus() | (1 << 8));
 
         context.set_sepc(vstvec::read().bits());
 
diff --git a/src/emulate_extension/zicfiss.rs b/src/emulate_extension/zicfiss.rs
index 9a4eae85..50c87676 100644
--- a/src/emulate_extension/zicfiss.rs
+++ b/src/emulate_extension/zicfiss.rs
@@ -84,7 +84,7 @@ impl Zicfiss {
     ///
     /// Chack corresponding `SSE` bit of xenvcfg.
     fn is_ss_enable(&self, sstatus: usize) -> bool {
-        let spp = sstatus >> 8 & 0x1;
+        let spp = (sstatus >> 8) & 0x1;
         if spp == 0 {
             self.senv_sse
         } else {
@@ -219,12 +219,12 @@ impl EmulateExtension for Zicfiss {
                     | ZicsrOpcode::CSRRWI
                     | ZicsrOpcode::CSRRSI,
                 ) => {
-                    if write_to_csr_value >> 3 & 0x1 == 1 {
+                    if (write_to_csr_value >> 3) & 0x1 == 1 {
                         self.senv_sse = true;
                     }
                 }
                 OpcodeKind::Zicsr(ZicsrOpcode::CSRRC | ZicsrOpcode::CSRRCI) => {
-                    if write_to_csr_value >> 3 & 0x1 == 1 {
+                    if (write_to_csr_value >> 3) & 0x1 == 1 {
                         self.senv_sse = false;
                     }
                 }
diff --git a/src/guest.rs b/src/guest.rs
index 8fe91b6f..2756f527 100644
--- a/src/guest.rs
+++ b/src/guest.rs
@@ -209,7 +209,7 @@ impl Guest {
     }
 
     /// Allocate guest memory space from heap and create corresponding page table.
-    pub fn filling_memory_region(&self, region: Range<GuestPhysicalAddress>) {
+    pub fn allocate_memory_region(&self, region: Range<GuestPhysicalAddress>) {
         use PteFlag::{Accessed, Dirty, Exec, Read, User, Valid, Write};
 
         let all_pte_flags_are_set = &[Dirty, Accessed, Exec, Write, Read, User, Valid];
diff --git a/src/h_extension/csrs.rs b/src/h_extension/csrs.rs
index b16d2359..b782860e 100644
--- a/src/h_extension/csrs.rs
+++ b/src/h_extension/csrs.rs
@@ -440,7 +440,7 @@ pub mod hgatp {
 
     /// Set Hgatp fields.
     pub fn set(mode: Mode, vmid: usize, ppn: usize) {
-        write((0xF & (mode as usize)) << 60 | (0x3FFF & vmid) << 44 | 0x0FFF_FFFF_FFFF & ppn);
+        write(((0xF & (mode as usize)) << 60) | ((0x3FFF & vmid) << 44) | 0x0FFF_FFFF_FFFF & ppn);
     }
 
     impl_bits!(Hgatp);
diff --git a/src/hypervisor_init.rs b/src/hypervisor_init.rs
index 215ef030..c26e0439 100644
--- a/src/hypervisor_init.rs
+++ b/src/hypervisor_init.rs
@@ -13,8 +13,10 @@ use crate::memmap::{
     constant::guest_memory, page_table::sv39x4::ROOT_PAGE_TABLE, GuestPhysicalAddress,
     HostPhysicalAddress,
 };
-use crate::trap::hypervisor_supervisor::hstrap_vector;
+use crate::trap::hstrap_vector;
+use crate::ALLOCATOR;
 use crate::{HypervisorData, GUEST_DTB, HYPERVISOR_DATA};
+use crate::{_hv_heap_size, _start_heap};
 
 use core::arch::asm;
 
@@ -30,6 +32,14 @@ pub extern "C" fn hstart(hart_id: usize, dtb_addr: usize) -> ! {
     // dtb_addr test and hint for register usage.
     assert_ne!(dtb_addr, 0);
 
+    unsafe {
+        // Initialize global allocator
+        ALLOCATOR.lock().init(
+            core::ptr::addr_of_mut!(_start_heap),
+            core::ptr::addr_of!(_hv_heap_size) as usize,
+        );
+    }
+
     // clear all hs-mode to vs-mode interrupts.
     hvip::clear(VsInterruptKind::External);
     hvip::clear(VsInterruptKind::Timer);
@@ -128,8 +138,8 @@ fn vsmode_setup(hart_id: usize, dtb_addr: HostPhysicalAddress) -> ! {
     let (guest_entry_point, elf_end_addr) =
         new_guest.load_guest_elf(&guest_elf, initrd.paddr().raw() as *mut u8);
 
-    // filling remain memory region
-    new_guest.filling_memory_region(
+    // allocate all remain memory region
+    new_guest.allocate_memory_region(
         elf_end_addr..guest_memory_begin + guest_memory::DRAM_SIZE_PER_GUEST,
     );
 
diff --git a/src/machine_init.rs b/src/machine_init.rs
deleted file mode 100644
index 1e802950..00000000
--- a/src/machine_init.rs
+++ /dev/null
@@ -1,160 +0,0 @@
-//! M-mode level initialization.
-
-use crate::hypervisor_init;
-use crate::memmap::constant::STACK_SIZE_PER_HART;
-use crate::trap::machine::mtrap_vector;
-use crate::{sbi::Sbi, SBI};
-use core::arch::asm;
-use riscv::asm::sfence_vma_all;
-use riscv::register::{
-    mcounteren, medeleg, mepc, mideleg, mie, mscratch, mstatus, mtvec, pmpaddr0, pmpaddr1,
-    pmpaddr2, pmpcfg0, satp, Permission, Range,
-};
-
-/// Machine start function
-pub 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();
-        asm!("csrs medeleg, {vsmode_ecall}", vsmode_ecall = in(reg) 1 << 10, options(nomem)); // deleg env call from VS-mode
-        asm!("csrs medeleg, {load_guest_page_fault}", load_guest_page_fault = in(reg) 1 << 21, options(nomem)); // deleg load guest page fault
-        asm!("csrs medeleg, {virtual_instruction}", virtual_instruction = in(reg) 1 << 22, options(nomem)); // deleg virtual instruction
-        asm!("csrs medeleg, {store_amo_guest_page_fault}", store_amo_guest_page_fault = in(reg) 1 << 23, options(nomem)); // deleg store/amo guest page fault
-        medeleg::clear_supervisor_env_call();
-
-        // 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);
-
-        // switch to S-mode when mret executed.
-        mstatus::set_mpp(mstatus::MPP::Supervisor);
-
-        // set M-mode stack pointer
-        mscratch::write(
-            core::ptr::addr_of!(crate::_top_m_stack) as usize + STACK_SIZE_PER_HART * hart_id,
-        );
-
-        // Enable all custom state
-        asm!("csrs mstateen0, {all_set}", all_set = in(reg) u64::MAX);
-
-        // pmp settings
-        pmpcfg0::set_pmp(0, Range::OFF, Permission::NONE, false);
-        pmpaddr0::write(0);
-        // 0x0 - 0x8000_0000 = RW
-        pmpcfg0::set_pmp(1, Range::TOR, Permission::RW, false);
-        pmpaddr1::write(0x8000_0000 >> 2);
-        // 0x8000_0000 - 0xffff_ffff = RWX
-        pmpcfg0::set_pmp(2, Range::TOR, Permission::RWX, false);
-        pmpaddr2::write(0xffff_ffff_ffff_ffff);
-
-        // no address translation
-        satp::set(satp::Mode::Bare, 0, 0);
-
-        // enable Sstc and Zicboz extention
-        asm!("csrs menvcfg, {sstc_cbze}", sstc_cbze = in(reg) (1u64 << 63) | (1u64 << 7) | (1u64 << 6), options(nomem)); // deleg env call from VS-mode
-
-        // set `hstart` to jump after mret
-        mepc::write(hypervisor_init::hstart 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();
-    }
-
-    SBI.lock().get_or_init(|| {
-        // parse device tree
-        let device_tree = unsafe {
-            match fdt::Fdt::from_ptr(dtb_addr as *const u8) {
-                Ok(fdt) => fdt,
-                Err(e) => panic!("{}", e),
-            }
-        };
-
-        Sbi::new(device_tree)
-    });
-
-    enter_hypervisor_mode(hart_id, dtb_addr);
-}
-
-/// Enter hypervisor. (just exec mret)
-///
-/// Jump to hstart via mret.
-#[inline(never)]
-#[no_mangle]
-extern "C" fn enter_hypervisor_mode(hart_id: usize, dtb_addr: usize) -> ! {
-    unsafe {
-        // set stack pointer
-        asm!(
-            "
-            mv t0, {hart_id}
-            mv t1, {dtb_addr}
-            mv sp, {machine_sp}
-            ",
-            hart_id = in(reg) hart_id,
-            dtb_addr = in(reg) dtb_addr,
-            machine_sp = in(reg) core::ptr::addr_of!(crate::_top_m_stack) as usize + STACK_SIZE_PER_HART * hart_id
-        );
-        // enter HS-mode.
-        asm!(
-            "
-            mv a0, t0
-            mv a1, t1
-            mret
-            ",
-            options(noreturn)
-        );
-    }
-}
diff --git a/src/main.rs b/src/main.rs
index c92c991f..5594a8f5 100644
--- a/src/main.rs
+++ b/src/main.rs
@@ -1,6 +1,8 @@
 #![doc = include_str!("../README.md")]
 #![no_main]
 #![no_std]
+// TODO: remove nightly when `naked_functions` become stable.
+#![feature(naked_functions)]
 
 extern crate alloc;
 mod device;
@@ -8,28 +10,24 @@ mod emulate_extension;
 mod guest;
 mod h_extension;
 mod hypervisor_init;
-mod machine_init;
 mod memmap;
-mod sbi;
 mod trap;
 
 use alloc::boxed::Box;
 use alloc::vec::Vec;
-use core::arch::asm;
+use core::arch::naked_asm;
 use core::cell::OnceCell;
 use core::panic::PanicInfo;
 
 use fdt::Fdt;
 use linked_list_allocator::LockedHeap;
-use riscv_rt::entry;
 use spin::Mutex;
 
 use crate::device::Devices;
 use crate::guest::Guest;
-use crate::machine_init::mstart;
+use crate::hypervisor_init::hstart;
 use crate::memmap::constant::{DRAM_BASE, MAX_HART_NUM, STACK_SIZE_PER_HART};
 use crate::memmap::HostPhysicalAddress;
-use crate::sbi::Sbi;
 
 #[global_allocator]
 /// Global allocator.
@@ -40,7 +38,7 @@ static ALLOCATOR: LockedHeap = LockedHeap::empty();
 static mut HYPERVISOR_DATA: Mutex<OnceCell<HypervisorData>> = Mutex::new(OnceCell::new());
 
 /// Singleton for SBI handler.
-static SBI: Mutex<OnceCell<Sbi>> = Mutex::new(OnceCell::new());
+//static SBI: Mutex<OnceCell<Sbi>> = Mutex::new(OnceCell::new());
 
 /// Device tree blob that is passed to hypervisor
 #[cfg(feature = "embedded_host_dtb")]
@@ -58,8 +56,8 @@ extern "C" {
     static mut _start_heap: u8;
     /// heap size (defined in `memory.x`)
     static _hv_heap_size: u8;
-    /// machine stack top (defined in `memory.x`)
-    static _top_m_stack: u8;
+    /// boot stack top (defined in `memory.x`)
+    static _top_b_stack: u8;
 }
 
 /// Panic handler
@@ -147,42 +145,33 @@ impl HypervisorData {
     }
 }
 
-/// 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) -> ! {
-    unsafe {
-        // Initialize global allocator
-        ALLOCATOR.lock().init(
-            core::ptr::addr_of_mut!(_start_heap),
-            core::ptr::addr_of!(_hv_heap_size) as usize,
-        );
-    }
-
+/// Entry function of the hypervisor.
+///
+/// - set stack pointer
+/// - init stvec
+/// - jump to hstart
+#[link_section = ".text.entry"]
+#[no_mangle]
+#[naked]
+extern "C" fn _start() -> ! {
     unsafe {
         // set stack pointer
-        asm!(
+        naked_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}
+            li t0, {stack_size_per_hart}
+            mul t1, a0, t0
+            la sp, {stack_top}
+            sub sp, sp, t1
+
+            li t2, {DRAM_BASE}
+            csrw stvec, t2
+
+            call {hstart}
             ",
-            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) core::ptr::addr_of!(_top_m_stack) as usize,
-            DRAM_BASE = in(reg) DRAM_BASE,
-            mstart = sym mstart,
-        );
+            stack_top = sym _top_b_stack,
+            stack_size_per_hart = const STACK_SIZE_PER_HART,
+            DRAM_BASE = const DRAM_BASE,
+            hstart = sym hstart,
+        )
     }
-
-    unreachable!();
 }
diff --git a/src/memmap/constant.rs b/src/memmap/constant.rs
index 13c46262..492a4fd8 100644
--- a/src/memmap/constant.rs
+++ b/src/memmap/constant.rs
@@ -21,27 +21,13 @@ pub const DRAM_BASE: usize = 0x8000_0000;
 /// Stack size for each HART.
 pub const STACK_SIZE_PER_HART: usize = 0x1_0000;
 
-pub mod device {
-    //! Device memory map
-    //! TODO?: parse device tree in `machine_init.rs`
-
-    use crate::memmap::HostPhysicalAddress;
-
-    /// CLINT address
-    /// For trap `SupervisorSoftware` interrupt
-    pub const CLINT_ADDR: HostPhysicalAddress = HostPhysicalAddress(0x200_0000);
-
-    /// mtimecmp CSRs address
-    pub const MTIMECMP_ADDR: HostPhysicalAddress = HostPhysicalAddress(0x200_4000);
-}
-
 pub mod guest_memory {
     //! Guest memory region on Guest Physical Address
 
     use crate::memmap::GuestPhysicalAddress;
 
     /// Dram base address
-    pub const DRAM_BASE: GuestPhysicalAddress = GuestPhysicalAddress(0x8000_0000);
+    pub const DRAM_BASE: GuestPhysicalAddress = GuestPhysicalAddress(super::DRAM_BASE);
     /// Dram memory space per HART.
     pub const DRAM_SIZE_PER_GUEST: usize = 256 * 1024 * 1024; // 256 MB = 0x1000_0000
     /// Guest DTB space size
diff --git a/src/memmap/page_table.rs b/src/memmap/page_table.rs
index a82b612d..de1ce493 100644
--- a/src/memmap/page_table.rs
+++ b/src/memmap/page_table.rs
@@ -78,14 +78,14 @@ pub struct PageTableEntry(u64);
 impl PageTableEntry {
     /// Constructor for `PageTableEntry`.
     fn new(ppn: u64, flags: u8) -> Self {
-        Self(ppn << 10 | u64::from(flags))
+        Self((ppn << 10) | u64::from(flags))
     }
 
     /// Is leaf page table entry
     fn is_leaf(self) -> bool {
-        let pte_r = self.0 >> 1 & 0x1;
-        let pte_w = self.0 >> 2 & 0x1;
-        let pte_x = self.0 >> 3 & 0x1;
+        let pte_r = (self.0 >> 1) & 0x1;
+        let pte_w = (self.0 >> 2) & 0x1;
+        let pte_x = (self.0 >> 3) & 0x1;
 
         // For Zicfilp (TODO: remove it)
         pte_r == 1 || pte_x == 1 || (pte_r == 0 && pte_w == 1 && pte_x == 0)
diff --git a/src/memmap/page_table/sv39.rs b/src/memmap/page_table/sv39.rs
index 804281b7..3379e3d0 100644
--- a/src/memmap/page_table/sv39.rs
+++ b/src/memmap/page_table/sv39.rs
@@ -74,7 +74,10 @@ pub fn trans_addr(gva: GuestVirtualAddress) -> GuestPhysicalAddress {
                         "Address translation failed: pte.ppn[1] != 0"
                     );
                     return GuestPhysicalAddress(
-                        pte.ppn(2) << 30 | gva.vpn(1) << 21 | gva.vpn(0) << 12 | gva.page_offset(),
+                        (pte.ppn(2) << 30)
+                            | (gva.vpn(1) << 21)
+                            | (gva.vpn(0) << 12)
+                            | gva.page_offset(),
                     );
                 }
                 PageTableLevel::Lv2MB => {
@@ -83,12 +86,18 @@ pub fn trans_addr(gva: GuestVirtualAddress) -> GuestPhysicalAddress {
                         "Address translation failed: pte.ppn[0] != 0"
                     );
                     return GuestPhysicalAddress(
-                        pte.ppn(2) << 30 | pte.ppn(1) << 21 | gva.vpn(0) << 12 | gva.page_offset(),
+                        (pte.ppn(2) << 30)
+                            | (pte.ppn(1) << 21)
+                            | (gva.vpn(0) << 12)
+                            | gva.page_offset(),
                     );
                 }
                 PageTableLevel::Lv4KB => {
                     return GuestPhysicalAddress(
-                        pte.ppn(2) << 30 | pte.ppn(1) << 21 | pte.ppn(0) << 12 | gva.page_offset(),
+                        (pte.ppn(2) << 30)
+                            | (pte.ppn(1) << 21)
+                            | (pte.ppn(0) << 12)
+                            | gva.page_offset(),
                     );
                 }
             }
diff --git a/src/memmap/page_table/sv39x4.rs b/src/memmap/page_table/sv39x4.rs
index 1164a08d..0ce18f70 100644
--- a/src/memmap/page_table/sv39x4.rs
+++ b/src/memmap/page_table/sv39x4.rs
@@ -185,7 +185,10 @@ pub fn trans_addr(gpa: GuestPhysicalAddress) -> HostPhysicalAddress {
                         "Address translation failed: pte.ppn[1] != 0"
                     );
                     return HostPhysicalAddress(
-                        pte.ppn(2) << 30 | gpa.vpn(1) << 21 | gpa.vpn(0) << 12 | gpa.page_offset(),
+                        (pte.ppn(2) << 30)
+                            | (gpa.vpn(1) << 21)
+                            | (gpa.vpn(0) << 12)
+                            | gpa.page_offset(),
                     );
                 }
                 PageTableLevel::Lv2MB => {
@@ -194,12 +197,18 @@ pub fn trans_addr(gpa: GuestPhysicalAddress) -> HostPhysicalAddress {
                         "Address translation failed: pte.ppn[0] != 0"
                     );
                     return HostPhysicalAddress(
-                        pte.ppn(2) << 30 | pte.ppn(1) << 21 | gpa.vpn(0) << 12 | gpa.page_offset(),
+                        (pte.ppn(2) << 30)
+                            | (pte.ppn(1) << 21)
+                            | (gpa.vpn(0) << 12)
+                            | gpa.page_offset(),
                     );
                 }
                 PageTableLevel::Lv4KB => {
                     return HostPhysicalAddress(
-                        pte.ppn(2) << 30 | pte.ppn(1) << 21 | pte.ppn(0) << 12 | gpa.page_offset(),
+                        (pte.ppn(2) << 30)
+                            | (pte.ppn(1) << 21)
+                            | (pte.ppn(0) << 12)
+                            | gpa.page_offset(),
                     )
                 }
             }
diff --git a/src/memmap/page_table/sv57.rs b/src/memmap/page_table/sv57.rs
index d4a4c56f..553038f0 100644
--- a/src/memmap/page_table/sv57.rs
+++ b/src/memmap/page_table/sv57.rs
@@ -87,11 +87,11 @@ pub fn trans_addr(gva: GuestVirtualAddress) -> Result<GuestPhysicalAddress, ()>
                         "Address translation failed: pte.ppn[0] != 0"
                     );
                     return Ok(GuestPhysicalAddress(
-                        pte.ppn(4) << 48
-                            | gva.vpn(3) << 39
-                            | gva.vpn(2) << 30
-                            | gva.vpn(1) << 21
-                            | gva.vpn(0) << 12
+                        (pte.ppn(4) << 48)
+                            | (gva.vpn(3) << 39)
+                            | (gva.vpn(2) << 30)
+                            | (gva.vpn(1) << 21)
+                            | (gva.vpn(0) << 12)
                             | gva.page_offset(),
                     ));
                 }
@@ -109,11 +109,11 @@ pub fn trans_addr(gva: GuestVirtualAddress) -> Result<GuestPhysicalAddress, ()>
                         "Address translation failed: pte.ppn[0] != 0"
                     );
                     return Ok(GuestPhysicalAddress(
-                        pte.ppn(4) << 48
-                            | pte.ppn(3) << 39
-                            | gva.vpn(2) << 30
-                            | gva.vpn(1) << 21
-                            | gva.vpn(0) << 12
+                        (pte.ppn(4) << 48)
+                            | (pte.ppn(3) << 39)
+                            | (gva.vpn(2) << 30)
+                            | (gva.vpn(1) << 21)
+                            | (gva.vpn(0) << 12)
                             | gva.page_offset(),
                     ));
                 }
@@ -127,11 +127,11 @@ pub fn trans_addr(gva: GuestVirtualAddress) -> Result<GuestPhysicalAddress, ()>
                         "Address translation failed: pte.ppn[0] != 0"
                     );
                     return Ok(GuestPhysicalAddress(
-                        pte.ppn(4) << 48
-                            | pte.ppn(3) << 39
-                            | pte.ppn(2) << 30
-                            | gva.vpn(1) << 21
-                            | gva.vpn(0) << 12
+                        (pte.ppn(4) << 48)
+                            | (pte.ppn(3) << 39)
+                            | (pte.ppn(2) << 30)
+                            | (gva.vpn(1) << 21)
+                            | (gva.vpn(0) << 12)
                             | gva.page_offset(),
                     ));
                 }
@@ -141,21 +141,21 @@ pub fn trans_addr(gva: GuestVirtualAddress) -> Result<GuestPhysicalAddress, ()>
                         "Address translation failed: pte.ppn[0] != 0"
                     );
                     return Ok(GuestPhysicalAddress(
-                        pte.ppn(4) << 48
-                            | pte.ppn(3) << 39
-                            | pte.ppn(2) << 30
-                            | pte.ppn(1) << 21
-                            | gva.vpn(0) << 12
+                        (pte.ppn(4) << 48)
+                            | (pte.ppn(3) << 39)
+                            | (pte.ppn(2) << 30)
+                            | (pte.ppn(1) << 21)
+                            | (gva.vpn(0) << 12)
                             | gva.page_offset(),
                     ));
                 }
                 PageTableLevel::Lv4KB => {
                     return Ok(GuestPhysicalAddress(
-                        pte.ppn(4) << 48
-                            | pte.ppn(3) << 39
-                            | pte.ppn(2) << 30
-                            | pte.ppn(1) << 21
-                            | pte.ppn(0) << 12
+                        (pte.ppn(4) << 48)
+                            | (pte.ppn(3) << 39)
+                            | (pte.ppn(2) << 30)
+                            | (pte.ppn(1) << 21)
+                            | (pte.ppn(0) << 12)
                             | gva.page_offset(),
                     ));
                 }
diff --git a/src/sbi.rs b/src/sbi.rs
deleted file mode 100644
index fe2ddabb..00000000
--- a/src/sbi.rs
+++ /dev/null
@@ -1,38 +0,0 @@
-//! SBI Implementation.  
-//! Ref: [https://github.com/rustsbi/rustsbi-qemu](https://github.com/rustsbi/rustsbi-qemu)  
-//! Document: [https://docs.rs/rustsbi/0.4.0-alpha.1/rustsbi/derive.RustSBI.html](https://docs.rs/rustsbi/0.4.0-alpha.1/rustsbi/derive.RustSBI.html)  
-
-mod rfence;
-
-use crate::device::{clint, uart, MmioDevice};
-use fdt::Fdt;
-use rustsbi::RustSBI;
-
-/// Device reference for `RustSBI`.
-#[derive(RustSBI)]
-#[allow(clippy::doc_markdown)]
-pub struct Sbi {
-    /// Core Local INTerrupt
-    #[rustsbi(ipi, timer)]
-    pub clint: clint::Clint,
-
-    /// Universal Asynchronous Receiver Transmitter
-    /// For debug console.
-    #[rustsbi(console)]
-    pub uart: uart::Uart,
-
-    /// Remote fence
-    #[rustsbi(fence)]
-    pub rfence: rfence::RemoteFence,
-}
-
-impl Sbi {
-    /// Constructor for `Sbi`
-    pub fn new(device_tree: Fdt) -> Self {
-        Sbi {
-            uart: uart::Uart::new(&device_tree, "/soc/serial"),
-            clint: clint::Clint::new(&device_tree, "/soc/clint"),
-            rfence: rfence::RemoteFence,
-        }
-    }
-}
diff --git a/src/sbi/rfence.rs b/src/sbi/rfence.rs
deleted file mode 100644
index 9f6b2c6b..00000000
--- a/src/sbi/rfence.rs
+++ /dev/null
@@ -1,59 +0,0 @@
-//! Remote fence implementation for `RustSBI`.
-
-use core::arch::asm;
-use rustsbi::{HartMask, SbiRet};
-
-use crate::memmap::constant::MAX_HART_NUM;
-use crate::memmap::page_table::constants::PAGE_SIZE;
-
-/// Remote fence implementation.
-/// ref: [https://docs.rs/rustsbi/0.4.0-alpha.3/rustsbi/trait.Fence.html](https://docs.rs/rustsbi/0.4.0-alpha.3/rustsbi/trait.Fence.html)
-pub struct RemoteFence;
-
-impl rustsbi::Fence for RemoteFence {
-    // Required methods
-    fn remote_fence_i(&self, hart_mask: HartMask) -> SbiRet {
-        // current hart must be 0.
-        for hart_id in 1..MAX_HART_NUM {
-            debug_assert!(!hart_mask.has_bit(hart_id));
-        }
-
-        if hart_mask.has_bit(0) {
-            unsafe { asm!("fence.i") }
-        }
-
-        SbiRet::success(0)
-    }
-
-    fn remote_sfence_vma(&self, hart_mask: HartMask, start_addr: usize, size: usize) -> SbiRet {
-        // current hart must be 0.
-        for hart_id in 1..MAX_HART_NUM {
-            debug_assert!(!hart_mask.has_bit(hart_id));
-        }
-
-        for addr in (start_addr..start_addr + size).step_by(PAGE_SIZE) {
-            unsafe { asm!("sfence.vma {vaddr}, x0", vaddr = in(reg) addr) }
-        }
-
-        SbiRet::success(0)
-    }
-
-    fn remote_sfence_vma_asid(
-        &self,
-        hart_mask: HartMask,
-        start_addr: usize,
-        size: usize,
-        asid: usize,
-    ) -> SbiRet {
-        // current hart must be 0.
-        for hart_id in 1..MAX_HART_NUM {
-            debug_assert!(!hart_mask.has_bit(hart_id));
-        }
-
-        for addr in (start_addr..start_addr + size).step_by(PAGE_SIZE) {
-            unsafe { asm!("sfence.vma {vaddr}, {asid}", vaddr = in(reg) addr, asid = in(reg) asid) }
-        }
-
-        SbiRet::success(0)
-    }
-}
diff --git a/src/trap.S b/src/trap.S
deleted file mode 100644
index f0a8eee2..00000000
--- a/src/trap.S
+++ /dev/null
@@ -1,158 +0,0 @@
-.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
index 1bbe49c1..f9599692 100644
--- a/src/trap.rs
+++ b/src/trap.rs
@@ -1,4 +1,159 @@
-//! Trap exception / interrupt.
+//! Trap VS-mode exception / interrupt.
 
-pub mod hypervisor_supervisor;
-pub mod machine;
+mod exception;
+mod interrupt;
+
+use crate::guest::context::ContextData;
+use exception::trap_exception;
+use interrupt::trap_interrupt;
+
+use crate::HYPERVISOR_DATA;
+use core::arch::asm;
+use riscv::register::scause::{self, Trap};
+
+/// Switch to original mode stack and save contexts.
+#[inline(always)]
+#[allow(clippy::inline_always)]
+pub unsafe fn hstrap_exit() -> ! {
+    // aquire hypervisor data
+    let hypervisor_data = unsafe { HYPERVISOR_DATA.lock() };
+    let stack_top = hypervisor_data.get().unwrap().guest().stack_top();
+    // release HYPERVISOR_DATA lock
+    drop(hypervisor_data);
+
+    asm!(
+        ".align 4
+        fence.i
+
+        // set to stack top
+        mv sp, {stack_top}  
+        addi sp, sp, -{HS_CONTEXT_SIZE}
+
+        // restore sstatus 
+        ld t0, 32*8(sp)
+        csrw sstatus, t0
+
+        // restore pc
+        ld t1, 33*8(sp)
+        csrw sepc, t1
+
+        // restore registers
+        ld ra, 1*8(sp)
+        ld gp, 3*8(sp)
+        ld tp, 4*8(sp)
+        ld t0, 5*8(sp)
+        ld t1, 6*8(sp)
+        ld t2, 7*8(sp)
+        ld s0, 8*8(sp)
+        ld s1, 9*8(sp)
+        ld a0, 10*8(sp)
+        ld a1, 11*8(sp)
+        ld a2, 12*8(sp)
+        ld a3, 13*8(sp)
+        ld a4, 14*8(sp)
+        ld a5, 15*8(sp)
+        ld a6, 16*8(sp)
+        ld a7, 17*8(sp)
+        ld s2, 18*8(sp)
+        ld s3, 19*8(sp)
+        ld s4, 20*8(sp)
+        ld s5, 21*8(sp)
+        ld s6, 22*8(sp)
+        ld s7, 23*8(sp)
+        ld s8, 24*8(sp)
+        ld s9, 25*8(sp)
+        ld s10, 26*8(sp)
+        ld s11, 27*8(sp)
+        ld t3, 28*8(sp)
+        ld t4, 29*8(sp)
+        ld t5, 30*8(sp)
+        ld t6, 31*8(sp)
+
+        // swap HS-mode sp for original mode sp.
+        addi sp, sp, {HS_CONTEXT_SIZE}
+        csrrw sp, sscratch, sp
+
+        sret
+        ",
+        HS_CONTEXT_SIZE = const size_of::<ContextData>(),
+        stack_top = in(reg) stack_top.raw(),
+        options(noreturn)
+    );
+}
+
+/// Trap vector for HS-mode.
+/// Switch to hypervisor stack and save contexts.
+///
+/// ## `fn_align`
+/// function alignment (feature `fn_align`).  
+/// See: [https://github.com/rust-lang/rust/issues/82232](https://github.com/rust-lang/rust/issues/82232).
+/// ```no_run
+/// #[repr(align(4))]
+/// pub unsafe extern "C" fn hstrap_vector() -> ! { }
+/// ```
+#[no_mangle]
+#[inline(never)]
+pub unsafe extern "C" fn hstrap_vector() -> ! {
+    unsafe {
+        asm!(
+            ".align 4
+            fence.i
+
+            // swap original mode sp for HS-mode sp 
+            csrrw sp, sscratch, sp
+            addi sp, sp, -{HS_CONTEXT_SIZE}
+
+            // save registers
+            sd ra, 1*8(sp)
+            sd gp, 3*8(sp)
+            sd tp, 4*8(sp)
+            sd t0, 5*8(sp)
+            sd t1, 6*8(sp)
+            sd t2, 7*8(sp)
+            sd s0, 8*8(sp)
+            sd s1, 9*8(sp)
+            sd a0, 10*8(sp)
+            sd a1, 11*8(sp)
+            sd a2, 12*8(sp)
+            sd a3, 13*8(sp)
+            sd a4, 14*8(sp)
+            sd a5, 15*8(sp)
+            sd a6, 16*8(sp)
+            sd a7, 17*8(sp)
+            sd s2, 18*8(sp)
+            sd s3, 19*8(sp)
+            sd s4, 20*8(sp)
+            sd s5, 21*8(sp)
+            sd s6, 22*8(sp)
+            sd s7, 23*8(sp)
+            sd s8, 24*8(sp)
+            sd s9, 25*8(sp)
+            sd s10, 26*8(sp)
+            sd s11, 27*8(sp)
+            sd t3, 28*8(sp)
+            sd t4, 29*8(sp)
+            sd t5, 30*8(sp)
+            sd t6, 31*8(sp)
+
+            // save sstatus
+            csrr t0, sstatus
+            sd t0, 32*8(sp)
+
+            // save pc
+            csrr t1, sepc
+            sd t1, 33*8(sp)
+            ",
+            HS_CONTEXT_SIZE = const size_of::<ContextData>(),
+        );
+    }
+
+    hstrap_vector2();
+}
+
+/// Separated from `hsrap_vector` by stack pointer circumstance.
+pub unsafe extern "C" fn hstrap_vector2() -> ! {
+    match scause::read().cause() {
+        Trap::Interrupt(interrupt_cause) => trap_interrupt(interrupt_cause),
+        Trap::Exception(exception_cause) => trap_exception(exception_cause),
+    }
+}
diff --git a/src/trap/hypervisor_supervisor/exception.rs b/src/trap/exception.rs
similarity index 93%
rename from src/trap/hypervisor_supervisor/exception.rs
rename to src/trap/exception.rs
index 3c7a5a45..73c0b351 100644
--- a/src/trap/hypervisor_supervisor/exception.rs
+++ b/src/trap/exception.rs
@@ -8,13 +8,14 @@ use super::hstrap_exit;
 use crate::guest;
 use crate::h_extension::{csrs::vstvec, HvException};
 use crate::HYPERVISOR_DATA;
+use sbi_handler::sbi_call;
 
 use core::arch::asm;
 use riscv::register::{
     scause::{self, Exception},
     stval,
 };
-use sbi_handler::{sbi_base_handler, sbi_fwft_handler, sbi_rfnc_handler};
+use sbi_handler::{sbi_base_handler, sbi_fwft_handler, sbi_pmu_handler, sbi_rfnc_handler};
 
 /// Delegate exception to supervisor mode from VS-mode.
 #[no_mangle]
@@ -54,12 +55,10 @@ fn sbi_vs_mode_handler(context: &mut guest::context::Context) {
 
     let sbiret = match ext_id {
         sbi_spec::base::EID_BASE => sbi_base_handler(func_id),
+        sbi_spec::pmu::EID_PMU => sbi_pmu_handler(func_id, arguments),
         sbi_spec::rfnc::EID_RFNC => sbi_rfnc_handler(func_id, arguments),
         EID_FWFT => sbi_fwft_handler(func_id, arguments),
-        _ => panic!(
-            "Unsupported SBI call, eid: {:#x}, fid: {:#x}",
-            ext_id, func_id
-        ),
+        _ => sbi_call(ext_id, func_id, arguments),
     };
 
     context.set_xreg(10, sbiret.error as u64);
diff --git a/src/trap/hypervisor_supervisor/exception/instruction_handler.rs b/src/trap/exception/instruction_handler.rs
similarity index 96%
rename from src/trap/hypervisor_supervisor/exception/instruction_handler.rs
rename to src/trap/exception/instruction_handler.rs
index 37716ad0..4d908ba4 100644
--- a/src/trap/hypervisor_supervisor/exception/instruction_handler.rs
+++ b/src/trap/exception/instruction_handler.rs
@@ -35,7 +35,8 @@ pub fn illegal_instruction() {
             }
         },
         _ => unimplemented!(
-            "unsupported illegal instruction: {:#?}, at {:#x}",
+            "unsupported illegal instruction({:#x}): {:#?}, at {:#x}",
+            fault_inst_value,
             fault_inst,
             sepc::read()
         ),
diff --git a/src/trap/hypervisor_supervisor/exception/page_fault_handler.rs b/src/trap/exception/page_fault_handler.rs
similarity index 87%
rename from src/trap/hypervisor_supervisor/exception/page_fault_handler.rs
rename to src/trap/exception/page_fault_handler.rs
index a416888e..97bd094b 100644
--- a/src/trap/hypervisor_supervisor/exception/page_fault_handler.rs
+++ b/src/trap/exception/page_fault_handler.rs
@@ -38,7 +38,10 @@ pub fn load_guest_page_fault() {
             DeviceEmulateError::InvalidAddress
             | DeviceEmulateError::InvalidContextId
             | DeviceEmulateError::ReservedRegister,
-        ) => hs_forward_exception(),
+        ) => {
+            drop(hypervisor_data);
+            hs_forward_exception();
+        }
     }
 }
 
@@ -54,7 +57,11 @@ pub fn store_guest_page_fault() {
 
     let mut hypervisor_data = unsafe { HYPERVISOR_DATA.lock() };
     let mut context = hypervisor_data.get().unwrap().guest().context;
-    let store_value = context.xreg(fault_inst.rs2.expect("rs2 is not found"));
+    //let store_value = context.xreg(fault_inst.rs2.expect("rs2 is not found"));
+    let store_value = context.xreg(match fault_inst.rs2 {
+        Some(x) => x,
+        None => panic!("rs2 is not found: {fault_inst:#?}"),
+    });
 
     if let Ok(()) = hypervisor_data
         .get_mut()
@@ -70,5 +77,6 @@ pub fn store_guest_page_fault() {
         }
     }
 
+    drop(hypervisor_data);
     hs_forward_exception();
 }
diff --git a/src/trap/hypervisor_supervisor/exception/sbi_handler.rs b/src/trap/exception/sbi_handler.rs
similarity index 62%
rename from src/trap/hypervisor_supervisor/exception/sbi_handler.rs
rename to src/trap/exception/sbi_handler.rs
index 6d517b06..503948a7 100644
--- a/src/trap/hypervisor_supervisor/exception/sbi_handler.rs
+++ b/src/trap/exception/sbi_handler.rs
@@ -2,6 +2,27 @@
 //! See [https://github.com/riscv-non-isa/riscv-sbi-doc/releases/download/v2.0/riscv-sbi.pdf](https://github.com/riscv-non-isa/riscv-sbi-doc/releases/download/v2.0/riscv-sbi.pdf)
 
 use sbi_rt::SbiRet;
+use sbi_rt::{ConfigFlags, StartFlags, StopFlags};
+
+/// SBI re-ecall
+///
+/// For now, pass all arguments regardless of the actual number of arguments.
+pub fn sbi_call(ext_id: usize, func_id: usize, args: &[u64; 5]) -> SbiRet {
+    let (error, value);
+    unsafe {
+        core::arch::asm!(
+            "ecall",
+            in("a7") ext_id,
+            in("a6") func_id,
+            inlateout("a0") args[0] => error,
+            inlateout("a1") args[1] => value,
+            in("a2") args[2],
+            in("a3") args[3],
+            in("a4") args[4],
+        );
+    }
+    SbiRet { error, value }
+}
 
 /// SBI ecall handler for Base Extension (EID: #0x10)
 ///
@@ -16,7 +37,7 @@ pub fn sbi_base_handler(func_id: usize) -> SbiRet {
     let result_value = match func_id {
         GET_SBI_SPEC_VERSION => {
             let spec = sbi_rt::get_spec_version();
-            spec.major() << 24 | spec.minor()
+            (spec.major() << 24) | spec.minor()
         }
         GET_SBI_IMPL_ID => sbi_rt::get_sbi_impl_id(),
         GET_SBI_IMPL_VERSION => sbi_rt::get_sbi_impl_version(),
@@ -33,6 +54,71 @@ pub fn sbi_base_handler(func_id: usize) -> SbiRet {
     }
 }
 
+/// Type of flag for SBI PMU extension.
+struct PmuFlag(u64);
+impl PmuFlag {
+    /// Create `PmuFlag` from a register value.
+    pub fn new(val: u64) -> Self {
+        PmuFlag(0b1111_1111 & val)
+    }
+}
+impl ConfigFlags for PmuFlag {
+    #[allow(clippy::cast_possible_truncation)]
+    fn raw(&self) -> usize {
+        self.0 as usize
+    }
+}
+impl StartFlags for PmuFlag {
+    #[allow(clippy::cast_possible_truncation)]
+    fn raw(&self) -> usize {
+        self.0 as usize
+    }
+}
+impl StopFlags for PmuFlag {
+    #[allow(clippy::cast_possible_truncation)]
+    fn raw(&self) -> usize {
+        self.0 as usize
+    }
+}
+
+/// SBI ecall handler for PMU Extension (EID: #0x504D55)
+#[allow(clippy::cast_possible_truncation)]
+pub fn sbi_pmu_handler(func_id: usize, args: &[u64; 5]) -> SbiRet {
+    use sbi_spec::pmu::{
+        COUNTER_CONFIG_MATCHING, COUNTER_FW_READ, COUNTER_FW_READ_HI, COUNTER_GET_INFO,
+        COUNTER_START, COUNTER_STOP, EID_PMU, NUM_COUNTERS, SNAPSHOT_SET_SHMEM,
+    };
+    match func_id {
+        NUM_COUNTERS => SbiRet {
+            error: 0,
+            value: sbi_rt::pmu_num_counters(),
+        },
+        COUNTER_GET_INFO => sbi_rt::pmu_counter_get_info(args[0] as usize),
+        COUNTER_CONFIG_MATCHING => sbi_rt::pmu_counter_config_matching(
+            args[0] as usize,
+            args[1] as usize,
+            PmuFlag::new(args[2]),
+            args[3] as usize,
+            args[4],
+        ),
+        COUNTER_START => sbi_rt::pmu_counter_start(
+            args[0] as usize,
+            args[1] as usize,
+            PmuFlag::new(args[2]),
+            args[3],
+        ),
+        COUNTER_STOP => {
+            sbi_rt::pmu_counter_stop(args[0] as usize, args[1] as usize, PmuFlag::new(args[2]))
+        }
+        COUNTER_FW_READ => sbi_rt::pmu_counter_fw_read(args[0] as usize),
+        COUNTER_FW_READ_HI => sbi_rt::pmu_counter_fw_read_hi(args[0] as usize),
+        // `sbi_rt::pmu_snapshot_set_shmem` is unimplemented.
+        // thus it is called by ecall instruction directly.
+        SNAPSHOT_SET_SHMEM => sbi_call(EID_PMU, SNAPSHOT_SET_SHMEM, args),
+        _ => panic!("unsupported fid: {}", func_id),
+    }
+}
+
 /// SBI ecall handler for RFENCE Extension (EID: #0x52464E43)
 #[allow(clippy::module_name_repetitions, clippy::cast_possible_truncation)]
 pub fn sbi_rfnc_handler(func_id: usize, args: &[u64; 5]) -> SbiRet {
diff --git a/src/trap/hypervisor_supervisor.rs b/src/trap/hypervisor_supervisor.rs
deleted file mode 100644
index f9599692..00000000
--- a/src/trap/hypervisor_supervisor.rs
+++ /dev/null
@@ -1,159 +0,0 @@
-//! Trap VS-mode exception / interrupt.
-
-mod exception;
-mod interrupt;
-
-use crate::guest::context::ContextData;
-use exception::trap_exception;
-use interrupt::trap_interrupt;
-
-use crate::HYPERVISOR_DATA;
-use core::arch::asm;
-use riscv::register::scause::{self, Trap};
-
-/// Switch to original mode stack and save contexts.
-#[inline(always)]
-#[allow(clippy::inline_always)]
-pub unsafe fn hstrap_exit() -> ! {
-    // aquire hypervisor data
-    let hypervisor_data = unsafe { HYPERVISOR_DATA.lock() };
-    let stack_top = hypervisor_data.get().unwrap().guest().stack_top();
-    // release HYPERVISOR_DATA lock
-    drop(hypervisor_data);
-
-    asm!(
-        ".align 4
-        fence.i
-
-        // set to stack top
-        mv sp, {stack_top}  
-        addi sp, sp, -{HS_CONTEXT_SIZE}
-
-        // restore sstatus 
-        ld t0, 32*8(sp)
-        csrw sstatus, t0
-
-        // restore pc
-        ld t1, 33*8(sp)
-        csrw sepc, t1
-
-        // restore registers
-        ld ra, 1*8(sp)
-        ld gp, 3*8(sp)
-        ld tp, 4*8(sp)
-        ld t0, 5*8(sp)
-        ld t1, 6*8(sp)
-        ld t2, 7*8(sp)
-        ld s0, 8*8(sp)
-        ld s1, 9*8(sp)
-        ld a0, 10*8(sp)
-        ld a1, 11*8(sp)
-        ld a2, 12*8(sp)
-        ld a3, 13*8(sp)
-        ld a4, 14*8(sp)
-        ld a5, 15*8(sp)
-        ld a6, 16*8(sp)
-        ld a7, 17*8(sp)
-        ld s2, 18*8(sp)
-        ld s3, 19*8(sp)
-        ld s4, 20*8(sp)
-        ld s5, 21*8(sp)
-        ld s6, 22*8(sp)
-        ld s7, 23*8(sp)
-        ld s8, 24*8(sp)
-        ld s9, 25*8(sp)
-        ld s10, 26*8(sp)
-        ld s11, 27*8(sp)
-        ld t3, 28*8(sp)
-        ld t4, 29*8(sp)
-        ld t5, 30*8(sp)
-        ld t6, 31*8(sp)
-
-        // swap HS-mode sp for original mode sp.
-        addi sp, sp, {HS_CONTEXT_SIZE}
-        csrrw sp, sscratch, sp
-
-        sret
-        ",
-        HS_CONTEXT_SIZE = const size_of::<ContextData>(),
-        stack_top = in(reg) stack_top.raw(),
-        options(noreturn)
-    );
-}
-
-/// Trap vector for HS-mode.
-/// Switch to hypervisor stack and save contexts.
-///
-/// ## `fn_align`
-/// function alignment (feature `fn_align`).  
-/// See: [https://github.com/rust-lang/rust/issues/82232](https://github.com/rust-lang/rust/issues/82232).
-/// ```no_run
-/// #[repr(align(4))]
-/// pub unsafe extern "C" fn hstrap_vector() -> ! { }
-/// ```
-#[no_mangle]
-#[inline(never)]
-pub unsafe extern "C" fn hstrap_vector() -> ! {
-    unsafe {
-        asm!(
-            ".align 4
-            fence.i
-
-            // swap original mode sp for HS-mode sp 
-            csrrw sp, sscratch, sp
-            addi sp, sp, -{HS_CONTEXT_SIZE}
-
-            // save registers
-            sd ra, 1*8(sp)
-            sd gp, 3*8(sp)
-            sd tp, 4*8(sp)
-            sd t0, 5*8(sp)
-            sd t1, 6*8(sp)
-            sd t2, 7*8(sp)
-            sd s0, 8*8(sp)
-            sd s1, 9*8(sp)
-            sd a0, 10*8(sp)
-            sd a1, 11*8(sp)
-            sd a2, 12*8(sp)
-            sd a3, 13*8(sp)
-            sd a4, 14*8(sp)
-            sd a5, 15*8(sp)
-            sd a6, 16*8(sp)
-            sd a7, 17*8(sp)
-            sd s2, 18*8(sp)
-            sd s3, 19*8(sp)
-            sd s4, 20*8(sp)
-            sd s5, 21*8(sp)
-            sd s6, 22*8(sp)
-            sd s7, 23*8(sp)
-            sd s8, 24*8(sp)
-            sd s9, 25*8(sp)
-            sd s10, 26*8(sp)
-            sd s11, 27*8(sp)
-            sd t3, 28*8(sp)
-            sd t4, 29*8(sp)
-            sd t5, 30*8(sp)
-            sd t6, 31*8(sp)
-
-            // save sstatus
-            csrr t0, sstatus
-            sd t0, 32*8(sp)
-
-            // save pc
-            csrr t1, sepc
-            sd t1, 33*8(sp)
-            ",
-            HS_CONTEXT_SIZE = const size_of::<ContextData>(),
-        );
-    }
-
-    hstrap_vector2();
-}
-
-/// Separated from `hsrap_vector` by stack pointer circumstance.
-pub unsafe extern "C" fn hstrap_vector2() -> ! {
-    match scause::read().cause() {
-        Trap::Interrupt(interrupt_cause) => trap_interrupt(interrupt_cause),
-        Trap::Exception(exception_cause) => trap_exception(exception_cause),
-    }
-}
diff --git a/src/trap/hypervisor_supervisor/interrupt.rs b/src/trap/interrupt.rs
similarity index 100%
rename from src/trap/hypervisor_supervisor/interrupt.rs
rename to src/trap/interrupt.rs
diff --git a/src/trap/machine.rs b/src/trap/machine.rs
deleted file mode 100644
index 72c974ad..00000000
--- a/src/trap/machine.rs
+++ /dev/null
@@ -1,186 +0,0 @@
-//! Trap machine exception / interrupt.
-
-mod exception;
-mod interrupt;
-
-use exception::trap_exception;
-use interrupt::trap_interrupt;
-
-use core::arch::asm;
-use riscv::register::mcause::{self, Trap};
-
-/// Size of context data in M-mode.
-const MACHINE_CONTEXT_SIZE: usize = 256;
-
-/// Epilogue of Machine trap vector
-#[inline(always)]
-#[allow(clippy::inline_always)]
-unsafe fn mtrap_exit() -> ! {
-    asm!(
-        "
-        // set to stack top
-    1:
-        auipc sp, %pcrel_hi(_top_m_stack)
-        addi sp, sp, %pcrel_lo(1b)
-        addi sp, sp, -{M_CONTEXT_SIZE}
-        
-        ld ra, 1*8(sp)
-        ld gp, 3*8(sp)
-        ld tp, 4*8(sp)
-        ld t0, 5*8(sp)
-        ld t1, 6*8(sp)
-        ld t2, 7*8(sp)
-        ld s0, 8*8(sp)
-        ld s1, 9*8(sp)
-        ld a0, 10*8(sp)
-        ld a1, 11*8(sp)
-        ld a2, 12*8(sp)
-        ld a3, 13*8(sp)
-        ld a4, 14*8(sp)
-        ld a5, 15*8(sp)
-        ld a6, 16*8(sp)
-        ld a7, 17*8(sp)
-        ld s2, 18*8(sp)
-        ld s3, 19*8(sp)
-        ld s4, 20*8(sp)
-        ld s5, 21*8(sp)
-        ld s6, 22*8(sp)
-        ld s7, 23*8(sp)
-        ld s8, 24*8(sp)
-        ld s9, 25*8(sp)
-        ld s10, 26*8(sp)
-        ld s11, 27*8(sp)
-        ld t3, 28*8(sp)
-        ld t4, 29*8(sp)
-        ld t5, 30*8(sp)
-        ld t6, 31*8(sp)
-
-        // revert stack pointer top
-        addi sp, sp, {M_CONTEXT_SIZE}
-
-        // swap current sp for stored original mode sp
-        csrrw sp, mscratch, sp
-
-        mret
-        ",
-        M_CONTEXT_SIZE = const MACHINE_CONTEXT_SIZE,
-        options(noreturn),
-    );
-}
-
-/// Epilogue of Machine trap vector with SBI return value.
-///
-/// - error: Error code (store to `a0`).
-/// - value: Return value (store to `a1`).
-#[inline(always)]
-#[allow(clippy::inline_always)]
-unsafe fn mtrap_exit_sbi(error: usize, value: usize) -> ! {
-    asm!("
-        // set to stack top
-    1:
-        auipc sp, %pcrel_hi(_top_m_stack)
-        addi sp, sp, %pcrel_lo(1b)
-        addi sp, sp, -{M_CONTEXT_SIZE}
-
-        ld ra, 1*8(sp)
-        ld gp, 3*8(sp)
-        ld tp, 4*8(sp)
-        ld t0, 5*8(sp)
-        ld t1, 6*8(sp)
-        ld t2, 7*8(sp)
-        ld s0, 8*8(sp)
-        ld s1, 9*8(sp)
-        mv a0, {error}
-        mv a1, {value}
-        ld a2, 12*8(sp)
-        ld a3, 13*8(sp)
-        ld a4, 14*8(sp)
-        ld a5, 15*8(sp)
-        ld a6, 16*8(sp)
-        ld a7, 17*8(sp)
-        ld s2, 18*8(sp)
-        ld s3, 19*8(sp)
-        ld s4, 20*8(sp)
-        ld s5, 21*8(sp)
-        ld s6, 22*8(sp)
-        ld s7, 23*8(sp)
-        ld s8, 24*8(sp)
-        ld s9, 25*8(sp)
-        ld s10, 26*8(sp)
-        ld s11, 27*8(sp)
-        ld t3, 28*8(sp)
-        ld t4, 29*8(sp)
-        ld t5, 30*8(sp)
-        ld t6, 31*8(sp)
-
-        // revert stack pointer to top (0x80800000)
-        addi sp, sp, {M_CONTEXT_SIZE}
-
-        // swap current sp for stored original mode sp
-        csrrw sp, mscratch, sp
-
-        mret
-        ",
-        M_CONTEXT_SIZE = const MACHINE_CONTEXT_SIZE,
-        error = in(reg) error,
-        value = in(reg) value,
-        options(noreturn),
-    );
-}
-
-/// Trap vector for M-mode.
-#[no_mangle]
-#[inline(never)]
-pub unsafe extern "C" fn mtrap_vector() -> ! {
-    asm!(
-        ".align 4
-        fence.i
-        // swap original mode sp for machine mode sp
-        csrrw sp, mscratch, sp
-        addi sp, sp, -{M_CONTEXT_SIZE}
-
-        sd ra, 1*8(sp)
-        sd gp, 3*8(sp)
-        sd tp, 4*8(sp)
-        sd t0, 5*8(sp)
-        sd t1, 6*8(sp)
-        sd t2, 7*8(sp)
-        sd s0, 8*8(sp)
-        sd s1, 9*8(sp)
-        sd a0, 10*8(sp)
-        sd a1, 11*8(sp)
-        sd a2, 12*8(sp)
-        sd a3, 13*8(sp)
-        sd a4, 14*8(sp)
-        sd a5, 15*8(sp)
-        sd a6, 16*8(sp)
-        sd a7, 17*8(sp)
-        sd s2, 18*8(sp)
-        sd s3, 19*8(sp)
-        sd s4, 20*8(sp)
-        sd s5, 21*8(sp)
-        sd s6, 22*8(sp)
-        sd s7, 23*8(sp)
-        sd s8, 24*8(sp)
-        sd s9, 25*8(sp)
-        sd s10, 26*8(sp)
-        sd s11, 27*8(sp)
-        sd t3, 28*8(sp)
-        sd t4, 29*8(sp)
-        sd t5, 30*8(sp)
-        sd t6, 31*8(sp)
-        ",
-        M_CONTEXT_SIZE = const MACHINE_CONTEXT_SIZE,
-    );
-
-    mtrap_vector2();
-}
-
-/// Separated from `mtrap_vector` by stack pointer circumstance.
-#[no_mangle]
-pub unsafe extern "C" fn mtrap_vector2() -> ! {
-    match mcause::read().cause() {
-        Trap::Interrupt(interrupt_cause) => trap_interrupt(interrupt_cause),
-        Trap::Exception(exception_cause) => trap_exception(exception_cause),
-    }
-}
diff --git a/src/trap/machine/exception.rs b/src/trap/machine/exception.rs
deleted file mode 100644
index e72dcf6f..00000000
--- a/src/trap/machine/exception.rs
+++ /dev/null
@@ -1,109 +0,0 @@
-//! Trap machine exception.
-
-use super::{mtrap_exit, mtrap_exit_sbi};
-use crate::device::MmioDevice;
-use crate::print;
-use crate::{HYPERVISOR_DATA, SBI};
-use riscv::register::{
-    mcause::{self, Exception},
-    mepc, mstatus, mtval, scause, sepc, stval, stvec,
-};
-use rustsbi::{RustSBI, Timer};
-use sbi_spec::legacy;
-
-/// Trap SBI Ecall
-///
-/// Handling SBI ecall is delegated to `Sbi` struct.
-#[allow(clippy::cast_possible_truncation)]
-pub unsafe fn trap_envcall(a0: usize, a1: usize, a2: usize, a6: usize, a7: usize) -> ! {
-    let sbi_cell = SBI.lock();
-    let sbi_data = sbi_cell.get().unwrap();
-    let ret_val = sbi_data.handle_ecall(a7, a6, [a0, a1, a2, 0, 0, 0]);
-
-    mepc::write(mepc::read() + 4);
-
-    if ret_val.error == 0 {
-        drop(sbi_cell);
-        mtrap_exit_sbi(ret_val.error, ret_val.value)
-    } else {
-        match a7 {
-            // Set Timer (EID #0x00)
-            legacy::LEGACY_SET_TIMER => {
-                sbi_data.clint.set_timer(a0 as u64);
-                drop(sbi_cell);
-                mtrap_exit_sbi(0, 0)
-            }
-            // Console Putchar (EID #0x01)
-            legacy::LEGACY_CONSOLE_PUTCHAR => {
-                print!("{}", a0 as u8 as char);
-                drop(sbi_cell);
-                mtrap_exit_sbi(0, 0)
-            }
-            // Console Getchar (EID #0x02)
-            legacy::LEGACY_CONSOLE_GETCHAR => {
-                let uart_addr = sbi_data.uart.paddr().raw() as *mut u32;
-                let uart_lsr_addr = sbi_data.uart.lsr_addr().raw() as *mut u32;
-
-                while uart_lsr_addr.read_volatile() & 0x1 == 0 {}
-                let c = uart_addr.read_volatile() as u8;
-                drop(sbi_cell);
-                mtrap_exit_sbi(0, c.into())
-            }
-            _ => panic!(
-                "SBI call failed: error:{}, eid:{a7}, fid:{a6}",
-                ret_val.error
-            ),
-        }
-    }
-}
-
-/// Delegate exception to supervisor or user mode from machine mode.
-#[no_mangle]
-#[allow(clippy::module_name_repetitions)]
-pub 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);
-    }
-}
-
-/// Trap handler for exception
-#[allow(
-    clippy::cast_possible_wrap,
-    clippy::cast_possible_truncation,
-    clippy::module_name_repetitions
-)]
-pub unsafe fn trap_exception(exception_cause: Exception) -> ! {
-    match exception_cause {
-        Exception::MachineEnvCall | Exception::SupervisorEnvCall | Exception::UserEnvCall => {
-            let context = unsafe { HYPERVISOR_DATA.lock().get().unwrap().guest().context };
-            let a0 = context.xreg(10) as usize;
-            let a1 = context.xreg(11) as usize;
-            let a2 = context.xreg(12) as usize;
-            let a6 = context.xreg(16) as usize;
-            let a7 = context.xreg(17) as usize;
-            trap_envcall(a0, a1, a2, a6, a7);
-        }
-        _ => {
-            forward_exception();
-            mtrap_exit();
-        }
-    }
-}
diff --git a/src/trap/machine/interrupt.rs b/src/trap/machine/interrupt.rs
deleted file mode 100644
index a304c9ad..00000000
--- a/src/trap/machine/interrupt.rs
+++ /dev/null
@@ -1,27 +0,0 @@
-//! Trap machine interrupt.
-
-use super::mtrap_exit;
-use crate::memmap::constant::device::{CLINT_ADDR, MTIMECMP_ADDR};
-use riscv::register::mcause::Interrupt;
-use riscv::register::{mhartid, mip};
-
-/// Trap handler for Interrupt
-#[allow(clippy::module_name_repetitions)]
-pub unsafe fn trap_interrupt(interrupt_cause: Interrupt) -> ! {
-    match interrupt_cause {
-        Interrupt::MachineSoft => {
-            mip::set_ssoft();
-            let interrupt_addr = (CLINT_ADDR.raw() + mhartid::read() * 4) as *mut u64;
-            interrupt_addr.write_volatile(0);
-        }
-        Interrupt::MachineTimer => {
-            mip::set_stimer();
-            let mtimecmp_addr = (MTIMECMP_ADDR.raw() + mhartid::read() * 8) as *mut u64;
-            mtimecmp_addr.write_volatile(u64::MAX);
-        }
-        Interrupt::MachineExternal => riscv::asm::wfi(), // wait for interrupt
-        _ => panic!("unknown interrupt type"),
-    }
-
-    mtrap_exit();
-}