diff --git a/Cargo.lock b/Cargo.lock
index 8ac1beb..313a4d4 100644
--- a/Cargo.lock
+++ b/Cargo.lock
@@ -22,9 +22,21 @@ checksum = "bef38d45163c2f1dde094a7dfd33ccf595c92905c8f8f4fdc18d06fb1037718a"
 
 [[package]]
 name = "bitflags"
-version = "2.4.1"
+version = "2.4.2"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "327762f6e5a765692301e5bb513e0d9fef63be86bbc14528052b1cd3e6f03e07"
+checksum = "ed570934406eb16438a4e976b1b4500774099c13b8cb96eec99f620f05090ddf"
+
+[[package]]
+name = "critical-section"
+version = "1.1.2"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "7059fff8937831a9ae6f0fe4d658ffabf58f2ca96aa9dec1c889f936f705f216"
+
+[[package]]
+name = "embedded-hal"
+version = "1.0.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "361a90feb7004eca4019fb28352a9465666b24f840f5c3cddf0ff13920590b89"
 
 [[package]]
 name = "lazy_static"
@@ -60,6 +72,16 @@ dependencies = [
  "bitflags 1.3.2",
 ]
 
+[[package]]
+name = "riscv"
+version = "0.11.1"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "2f5c1b8bf41ea746266cdee443d1d1e9125c86ce1447e1a2615abd34330d33a9"
+dependencies = [
+ "critical-section",
+ "embedded-hal",
+]
+
 [[package]]
 name = "rustversion"
 version = "1.0.14"
@@ -68,21 +90,18 @@ checksum = "7ffc183a10b4478d04cbbbfc96d0873219d962dd5accaff2ffbd4ceb7df837f4"
 
 [[package]]
 name = "sbi-rt"
-version = "0.0.2"
+version = "0.0.3"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "8c113c53291db8ac141e01f43224ed488b8d6001ab66737b82e04695a43a42b7"
+checksum = "7fbaa69be1eedc61c426e6d489b2260482e928b465360576900d52d496a58bd0"
 dependencies = [
  "sbi-spec",
 ]
 
 [[package]]
 name = "sbi-spec"
-version = "0.0.4"
+version = "0.0.7"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "6d4027cf9bb591a9fd0fc0e283be6165c5abe96cb73e9f0e24738c227f425377"
-dependencies = [
- "static_assertions",
-]
+checksum = "e6e36312fb5ddc10d08ecdc65187402baba4ac34585cb9d1b78522ae2358d890"
 
 [[package]]
 name = "scopeguard"
@@ -115,9 +134,10 @@ checksum = "a2eb9349b6444b326872e140eb1cf5e7c522154d69e7a0ffb0fb81c06b37543f"
 name = "tiny_os"
 version = "0.1.0"
 dependencies = [
- "bitflags 2.4.1",
+ "bitflags 2.4.2",
  "lazy_static",
  "log",
+ "riscv",
  "sbi-rt",
  "spin 0.9.8",
  "static_assertions",
diff --git a/Cargo.toml b/Cargo.toml
index 7250624..2ecc078 100644
--- a/Cargo.toml
+++ b/Cargo.toml
@@ -10,8 +10,6 @@ default = ["arch_riscv64", "board_virt", "log_color", "frame_freelist"]
 
 arch_riscv64 = []
 arch_riscv32 = []
-arch_arm = []
-arch_x86 = []
 
 board_default = []
 board_virt = []
@@ -32,7 +30,8 @@ panic = "abort"
 bitflags = "2.4.1"
 lazy_static = { version = "1.4.0", features = ["spin_no_std"] }
 log = "0.4"
-sbi-rt = { version = "0.0.2", features = ["legacy"] }
+riscv = { version = "0.11.1", features = ["s-mode"] }
+sbi-rt = { version = "0.0.3", features = ["legacy"] }
 spin = "0.9.8"
 static_assertions = "1.1.0"
 uart_16550 = "0.3"
diff --git a/src/arch/riscv/asm/trap32.S b/src/arch/riscv/asm/trap32.S
index 1605f81..4bd94ef 100644
--- a/src/arch/riscv/asm/trap32.S
+++ b/src/arch/riscv/asm/trap32.S
@@ -1,123 +1,11 @@
-.section .trampoline_section
+.equ XLENB, 4
 
-.global trampoline
-trampoline:
+.macro LD_SP r1, r2
+	lw	\r1, \r2*XLENB(sp)
+.endm
 
-.align 4
-
-.global __user2kernel
-__user2kernel:
-	# load trapframe
-	csrw	sscratch, a0	# save a0, will be saved later
-	li	a0, {TRAPFRAME}	# TRAPFRAME will be mapped to the same virtual address
-
-	# save GPRs
-	sw	ra,    0(a0)
-	sw	sp,    4(a0)
-	sw	gp,    8(a0)
-	sw	tp,   12(a0)
-	sw	t0,   16(a0)
-	sw	t1,   20(a0)
-	sw	t2,   24(a0)
-	sw	s0,   28(a0)
-	sw	s1,   32(a0)
-	#sw	a0,   36(a0)
-	sw	a1,   40(a0)
-	sw	a2,   44(a0)
-	sw	a3,   48(a0)
-	sw	a4,   52(a0)
-	sw	a5,   56(a0)
-	sw	a6,   60(a0)
-	sw	a7,   64(a0)
-	sw	s2,   68(a0)
-	sw	s3,   72(a0)
-	sw	s4,   76(a0)
-	sw	s5,   80(a0)
-	sw	s6,   84(a0)
-	sw	s7,   88(a0)
-	sw	s8,   92(a0)
-	sw	s9,   96(a0)
-	sw	s10, 100(a0)
-	sw	s11, 104(a0)
-	sw	t3,  108(a0)
-	sw	t4,  112(a0)
-	sw	t5,  116(a0)
-	sw	t6,  120(a0)
-
-	# save a0
-	csrr	t0, sscratch
-	sw	t0, 36(a0)
-
-	# save user epc / sstatus
-	csrr	t0, sepc
-	csrr	t1, sstatus
-	sw	t0, 124(a0)
-	sw	t1, 128(a0)
-
-	# restore kernel env
-	lw	sp, 132(a0)	# kernel sp
-	lw	tp, 136(a0)	# kernel tp
-	lw	t0, 140(a0)	# trap handler
-	lw	t1, 144(a0)	# page table
-
-	# load kernel page table
-	sfence.vma	zero, zero
-	csrw		satp, t1
-	sfence.vma	zero, zero
-
-	# jump to trap handler
-	jr	t0
-
-.global __kernel2user
-__kernel2user:
-	# restore user page table (a0)
-	sfence.vma	zero, zero
-	csrw		satp, a0
-	sfence.vma	zero, zero
-
-	li	a0, {TRAPFRAME}
-
-	# restore user epc / sstatus
-	lw	t0, 124(a0)
-	lw	t1, 128(a0)
-	csrw	sepc, t0
-	csrw	sstatus, t1
-
-	# restore GPRs
-	lw	ra,    0(a0)
-	lw	sp,    4(a0)
-	lw	gp,    8(a0)
-	lw	tp,   12(a0)
-	lw	t0,   16(a0)
-	lw	t1,   20(a0)
-	lw	t2,   24(a0)
-	lw	s0,   28(a0)
-	lw	s1,   32(a0)
-	#lw	a0,   36(a0)
-	lw	a1,   40(a0)
-	lw	a2,   44(a0)
-	lw	a3,   48(a0)
-	lw	a4,   52(a0)
-	lw	a5,   56(a0)
-	lw	a6,   60(a0)
-	lw	a7,   64(a0)
-	lw	s2,   68(a0)
-	lw	s3,   72(a0)
-	lw	s4,   76(a0)
-	lw	s5,   80(a0)
-	lw	s6,   84(a0)
-	lw	s7,   88(a0)
-	lw	s8,   92(a0)
-	lw	s9,   96(a0)
-	lw	s10, 100(a0)
-	lw	s11, 104(a0)
-	lw	t3,  108(a0)
-	lw	t4,  112(a0)
-	lw	t5,  116(a0)
-	lw	t6,  120(a0)
-	lw	a0,   36(a0)
-
-	# go back to usermode
-	sret
+.macro SD_SP r1, r2
+	sw	\r1, \r2*XLENB(sp)
+.endm
 
 # vim:set ts=8 noexpandtab:
diff --git a/src/arch/riscv/asm/trap64.S b/src/arch/riscv/asm/trap64.S
index ab368b0..c504f9d 100644
--- a/src/arch/riscv/asm/trap64.S
+++ b/src/arch/riscv/asm/trap64.S
@@ -1,123 +1,11 @@
-.section .trampoline_section
+.equ XLENB, 8
 
-.global trampoline
-trampoline:
+.macro LD_SP r1, r2
+	ld	\r1, \r2*XLENB(sp)
+.endm
 
-.align 4
-
-.global __user2kernel
-__user2kernel:
-	# load trapframe
-	csrw	sscratch, a0	# save a0, will be saved later
-	li	a0, {TRAPFRAME}	# TRAPFRAME will be mapped to the same virtual address
-
-	# save GPRs
-	sd	ra,    0(a0)
-	sd	sp,    8(a0)
-	sd	gp,   16(a0)
-	sd	tp,   24(a0)
-	sd	t0,   32(a0)
-	sd	t1,   40(a0)
-	sd	t2,   48(a0)
-	sd	s0,   56(a0)
-	sd	s1,   64(a0)
-	#sd	a0,   72(a0)
-	sd	a1,   80(a0)
-	sd	a2,   88(a0)
-	sd	a3,   96(a0)
-	sd	a4,  104(a0)
-	sd	a5,  112(a0)
-	sd	a6,  120(a0)
-	sd	a7,  128(a0)
-	sd	s2,  136(a0)
-	sd	s3,  144(a0)
-	sd	s4,  152(a0)
-	sd	s5,  160(a0)
-	sd	s6,  168(a0)
-	sd	s7,  176(a0)
-	sd	s8,  184(a0)
-	sd	s9,  192(a0)
-	sd	s10, 200(a0)
-	sd	s11, 208(a0)
-	sd	t3,  216(a0)
-	sd	t4,  224(a0)
-	sd	t5,  232(a0)
-	sd	t6,  240(a0)
-
-	# save a0
-	csrr	t0, sscratch
-	sd	t0, 72(a0)
-
-	# save user epc / sstatus
-	csrr	t0, sepc
-	csrr	t1, sstatus
-	sd	t0, 248(a0)
-	sd	t1, 256(a0)
-
-	# restore kernel env
-	ld	sp, 264(a0)	# kernel sp
-	ld	tp, 272(a0)	# kernel tp
-	ld	t0, 280(a0)	# trap handler
-	ld	t1, 288(a0)	# page table
-
-	# load kernel page table
-	sfence.vma	zero, zero
-	csrw		satp, t1
-	sfence.vma	zero, zero
-
-	# jump to trap handler
-	jr	t0
-
-.global __kernel2user
-__kernel2user:
-	# restore user page table (a0)
-	sfence.vma	zero, zero
-	csrw		satp, a0
-	sfence.vma	zero, zero
-
-	li	a0, {TRAPFRAME}
-
-	# restore user epc / sstatus
-	ld	t0, 248(a0)
-	ld	t1, 256(a0)
-	csrw	sepc, t0
-	csrw	sstatus, t1
-
-	# restore GPRs
-	ld	ra,    0(a0)
-	ld	sp,    8(a0)
-	ld	gp,   16(a0)
-	ld	tp,   24(a0)
-	ld	t0,   32(a0)
-	ld	t1,   40(a0)
-	ld	t2,   48(a0)
-	ld	s0,   56(a0)
-	ld	s1,   64(a0)
-	#ld	a0,   72(a0)
-	ld	a1,   80(a0)
-	ld	a2,   88(a0)
-	ld	a3,   96(a0)
-	ld	a4,  104(a0)
-	ld	a5,  112(a0)
-	ld	a6,  120(a0)
-	ld	a7,  128(a0)
-	ld	s2,  136(a0)
-	ld	s3,  144(a0)
-	ld	s4,  152(a0)
-	ld	s5,  160(a0)
-	ld	s6,  168(a0)
-	ld	s7,  176(a0)
-	ld	s8,  184(a0)
-	ld	s9,  192(a0)
-	ld	s10, 200(a0)
-	ld	s11, 208(a0)
-	ld	t3,  216(a0)
-	ld	t4,  224(a0)
-	ld	t5,  232(a0)
-	ld	t6,  240(a0)
-	ld	a0,   72(a0)
-
-	# go back to usermode
-	sret
+.macro SD_SP r1, r2
+	sd	\r1, \r2*XLENB(sp)
+.endm
 
 # vim:set ts=8 noexpandtab:
diff --git a/src/arch/riscv/asm/trap_common.S b/src/arch/riscv/asm/trap_common.S
new file mode 100644
index 0000000..b303fd3
--- /dev/null
+++ b/src/arch/riscv/asm/trap_common.S
@@ -0,0 +1,169 @@
+.section .text.trap
+
+.extern trap_handler
+
+.align 4
+.global trap_entry
+trap_entry:
+	# from user: switch to saved kernel context
+	# from kernel: sscratch = 0
+	csrrw	sp, sscratch, sp
+	bnez	sp, __trap_from_user
+
+__trap_from_kernel:
+	# Trap from kernelspace, no existing context, reserve space on kernel stack
+	csrr	sp, sscratch
+	addi	sp, sp, -34*XLENB
+
+__trap_from_user:
+	# Trap from userspace, use user context
+
+	# save GPRs, except zero, sp
+	SD_SP	ra,   0
+	#SD_SP	sp,   1
+	SD_SP	gp,   2
+	SD_SP	tp,   3
+	SD_SP	t0,   4
+	SD_SP	t1,   5
+	SD_SP	t2,   6
+	SD_SP	s0,   7
+	SD_SP	s1,   8
+	SD_SP	a0,   9
+	SD_SP	a1,  10
+	SD_SP	a2,  11
+	SD_SP	a3,  12
+	SD_SP	a4,  13
+	SD_SP	a5,  14
+	SD_SP	a6,  15
+	SD_SP	a7,  16
+	SD_SP	s2,  17
+	SD_SP	s3,  18
+	SD_SP	s4,  19
+	SD_SP	s5,  20
+	SD_SP	s6,  21
+	SD_SP	s7,  22
+	SD_SP	s8,  23
+	SD_SP	s9,  24
+	SD_SP	s10, 25
+	SD_SP	s11, 26
+	SD_SP	t3,  27
+	SD_SP	t4,  28
+	SD_SP	t5,  29
+	SD_SP	t6,  30
+
+	# save sp, sepc, sstatus
+	csrrw	t0, sscratch, zero # make sure sscratch is zero in kernel
+	csrr	t1, sepc
+	csrr	t2, sstatus
+	SD_SP	t0, 1
+	SD_SP	t1, 31
+	SD_SP	t2, 32
+
+	# find out whether we are from kernel (sstatus.SPP)
+	andi	t2, t2, 1 << 8
+	beqz	t2, __trap_from_user_next
+
+__trap_from_kernel_next:
+	# goto trap_handler
+	mv	a0, sp
+	la	ra, __trap_return
+	j	trap_handler
+
+__trap_from_user_next:
+	# we come from switch_to_user, which saved callee-saved registers
+	# restore kernel sp
+	LD_SP	sp, 33
+	# restore callee-saved registers
+	LD_SP	s0,   0
+	LD_SP	s1,   1
+	LD_SP	s2,   2
+	LD_SP	s3,   3
+	LD_SP	s4,   4
+	LD_SP	s5,   5
+	LD_SP	s6,   6
+	LD_SP	s7,   7
+	LD_SP	s8,   8
+	LD_SP	s9,   9
+	LD_SP	s10, 10
+	LD_SP	s11, 11
+	LD_SP	ra,  12
+	LD_SP	tp,  13 # not callee-saved
+	addi	sp,  sp, 14*XLENB
+	# return from switch_to_user
+	ret
+
+
+.global switch_to_user
+switch_to_user:
+	# save kernel callee-saved registers (into current kernel stack)
+	addi	sp, sp, -14*XLENB
+	SD_SP	s0,   0
+	SD_SP	s1,   1
+	SD_SP	s2,   2
+	SD_SP	s3,   3
+	SD_SP	s4,   4
+	SD_SP	s5,   5
+	SD_SP	s6,   6
+	SD_SP	s7,   7
+	SD_SP	s8,   8
+	SD_SP	s9,   9
+	SD_SP	s10, 10
+	SD_SP	s11, 11
+	SD_SP	ra,  12
+	SD_SP	tp,  13 # not callee-saved
+
+	# switch to user context
+	mv	t0, sp
+	mv	sp, a0
+	# save kernel sp into current user context
+	SD_SP	t0, 33
+	# save current user context
+	csrw	sscratch, sp
+
+__trap_return:
+	# restore sepc, sstatus
+	LD_SP	t0, 31
+	LD_SP	t1, 32
+	csrw	sepc, t0
+	csrw	sstatus, t1
+
+	# restore GPRs, except zero, sp
+	LD_SP	ra,   0
+	#LD_SP	sp,   1
+	LD_SP	gp,   2
+	LD_SP	tp,   3
+	LD_SP	t0,   4
+	LD_SP	t1,   5
+	LD_SP	t2,   6
+	LD_SP	s0,   7
+	LD_SP	s1,   8
+	LD_SP	a0,   9
+	LD_SP	a1,  10
+	LD_SP	a2,  11
+	LD_SP	a3,  12
+	LD_SP	a4,  13
+	LD_SP	a5,  14
+	LD_SP	a6,  15
+	LD_SP	a7,  16
+	LD_SP	s2,  17
+	LD_SP	s3,  18
+	LD_SP	s4,  19
+	LD_SP	s5,  20
+	LD_SP	s6,  21
+	LD_SP	s7,  22
+	LD_SP	s8,  23
+	LD_SP	s9,  24
+	LD_SP	s10, 25
+	LD_SP	s11, 26
+	LD_SP	t3,  27
+	LD_SP	t4,  28
+	LD_SP	t5,  29
+	LD_SP	t6,  30
+
+	# restore user sp
+	LD_SP	sp, 1
+
+	# go back from trap
+	sret
+
+# vim:set ts=8 noexpandtab:
diff --git a/src/arch/riscv/board/virt/mod.rs b/src/arch/riscv/board/virt/mod.rs
index 38e391f..2ee1438 100644
--- a/src/arch/riscv/board/virt/mod.rs
+++ b/src/arch/riscv/board/virt/mod.rs
@@ -1,6 +1,8 @@
 pub const UART0_BASE: usize = 0x1000_0000;
 pub const UART0_LSR: usize = 0x1000_0005;
 pub const TEST_DEVICE: *mut u32 = 0x10_0000 as *mut u32;
+pub const TIMER_TICKS: u64 = 100_000; // 100ms
 
 pub mod printer;
 pub mod reader;
+pub mod timer;
diff --git a/src/arch/riscv/board/virt/timer.rs b/src/arch/riscv/board/virt/timer.rs
new file mode 100644
index 0000000..e28570b
--- /dev/null
+++ b/src/arch/riscv/board/virt/timer.rs
@@ -0,0 +1,26 @@
+use log::info;
+
+use crate::plat::timer::{Timer, TimerOps};
+
+impl TimerOps for Timer {
+    fn init() {
+        // make sure Timer extension is enabled
+        assert!(
+            sbi_rt::probe_extension(sbi_rt::Timer).is_available(),
+            "[SBI] Timer extension not found"
+        );
+
+        // enable timer interrupt and set next tick
+        unsafe { riscv::register::sie::set_stimer() }
+        Self::tick();
+        info!("[Timer] begin to tick");
+    }
+
+    fn read_cycle() -> u64 {
+        riscv::register::time::read64()
+    }
+
+    fn tick() {
+        sbi_rt::set_timer(Self::read_cycle() + super::TIMER_TICKS);
+    }
+}
diff --git a/src/arch/riscv/linker.ld b/src/arch/riscv/linker.ld
index be1c14e..96ecd7c 100644
--- a/src/arch/riscv/linker.ld
+++ b/src/arch/riscv/linker.ld
@@ -15,15 +15,6 @@ SECTIONS {
         __text_start = .;
         *(.text.entry)
         *(.text .text.*)
-
-        . = ALIGN(PAGE_SIZE);
-        __trampoline_start = .;
-        *(.trampoline_section)
-        . = ALIGN(PAGE_SIZE);
-        __trampoline_end = .;
-
-        ASSERT(__trampoline_end - __trampoline_start == PAGE_SIZE, "trampoline does not fit into one page");
-
         __text_end = .;
     } > DRAM
 
diff --git a/src/arch/riscv/lowlevel.rs b/src/arch/riscv/lowlevel.rs
index e8bb359..3edb7dc 100644
--- a/src/arch/riscv/lowlevel.rs
+++ b/src/arch/riscv/lowlevel.rs
@@ -56,4 +56,14 @@ impl LowLevel for Hardware {
         error!("[riscv/lowlevel] system_reset failed, shutdown instead");
         Self::shutdown(true);
     }
+
+    #[inline]
+    fn enable_interrupt() {
+        unsafe { riscv::register::sstatus::set_sie() }
+    }
+
+    #[inline]
+    fn disable_interrupt() {
+        unsafe { riscv::register::sstatus::clear_sie() }
+    }
 }
diff --git a/src/arch/riscv/trap.rs b/src/arch/riscv/trap.rs
index f47c206..646a326 100644
--- a/src/arch/riscv/trap.rs
+++ b/src/arch/riscv/trap.rs
@@ -1,44 +1,103 @@
-use lazy_static::lazy_static;
+use log::trace;
+use riscv::register::scause::{Interrupt as I, Trap as T};
+
+use crate::plat::timer::{Timer, TimerOps};
+use crate::plat::trap::{Trap, TrapContextOps, TrapOps};
 
 #[cfg(feature = "arch_riscv64")]
-core::arch::global_asm!(
-    include_str!("./asm/trap64.S"),
-    TRAPFRAME = const super::layout::TRAPFRAME
-);
+core::arch::global_asm!(include_str!("./asm/trap64.S"));
 
 #[cfg(feature = "arch_riscv32")]
-core::arch::global_asm!(
-    include_str!("./asm/trap32.S"),
-    TRAPFRAME = const super::layout::TRAPFRAME
-);
+core::arch::global_asm!(include_str!("./asm/trap32.S"));
+
+core::arch::global_asm!(include_str!("./asm/trap_common.S"));
 
 extern "C" {
-    pub fn __user2kernel();
-    pub fn __kernel2user(ctx: *mut TrapContext);
+    fn trap_entry();
+    fn switch_to_user(regs: &mut TrapContext);
 }
 
-// TODO: remove this when we have a real context switch
-lazy_static! {
-    pub static ref __DUMMY: usize = unsafe {
-        __user2kernel();
-        __kernel2user(core::ptr::null_mut());
-        42
-    };
+#[no_mangle]
+pub extern "C" fn trap_handler(tf: &mut TrapContext) {
+    let scause = riscv::register::scause::read();
+    trace!("[Interrupt] scause: {:?}", scause.cause());
+    match scause.cause() {
+        T::Interrupt(I::SupervisorTimer) => {
+            // TODO: refactor this
+            trace!("[Interrupt] Tick: {}", riscv::register::time::read64());
+            Timer::tick();
+        },
+        _ => {
+            panic!(
+                "[Interrupt] Unhandled Trap, scause: {:?}, bits: {:#x}",
+                scause.cause(),
+                scause.bits()
+            );
+        },
+    }
+    trace!("[Interrupt] exiting...");
 }
 
+impl TrapOps for Trap {
+    fn init() {
+        // anyway, clear sscratch
+        unsafe { core::arch::asm!("csrw sscratch, zero") }
+        // set trap_entry
+        unsafe { core::arch::asm!("csrw stvec, {}", in(reg) trap_entry as usize) }
+    }
+}
+
+impl TrapContextOps for TrapContext {
+    fn restore(&mut self) {
+        unsafe { switch_to_user(self) }
+    }
+}
+
+#[derive(Debug)]
+#[repr(C)]
+pub struct GeneralRegs {
+    pub ra:  usize,
+    pub sp:  usize,
+    pub gp:  usize,
+    pub tp:  usize,
+    pub t0:  usize,
+    pub t1:  usize,
+    pub t2:  usize,
+    pub s0:  usize,
+    pub s1:  usize,
+    pub a0:  usize,
+    pub a1:  usize,
+    pub a2:  usize,
+    pub a3:  usize,
+    pub a4:  usize,
+    pub a5:  usize,
+    pub a6:  usize,
+    pub a7:  usize,
+    pub s2:  usize,
+    pub s3:  usize,
+    pub s4:  usize,
+    pub s5:  usize,
+    pub s6:  usize,
+    pub s7:  usize,
+    pub s8:  usize,
+    pub s9:  usize,
+    pub s10: usize,
+    pub s11: usize,
+    pub t3:  usize,
+    pub t4:  usize,
+    pub t5:  usize,
+    pub t6:  usize,
+}
+
+#[derive(Debug)]
 #[repr(C)]
 pub struct TrapContext {
-    pub gprs:         [usize; 31],
-    // GPRs exclude zero
-    pub user_sepc:    usize,
-    // 248/124 user program counter
-    pub user_sstatus: usize,
-    // 256/128 user status register
-    pub kernel_sp:    usize,
-    // 264/132 kernel stack pointer
-    pub kernel_tp:    usize,
-    // 272/136 kernel tp
-    pub kernel_trap:  usize,
-    // 280/140 kernel trap handler
-    pub kernel_satp:  usize, // 288/144 kernel page table
+    // [0:30] GPRs exclude zero
+    pub gprs:      GeneralRegs,
+    // [31] program counter
+    pub sepc:      usize,
+    // [32] status register
+    pub sstatus:   usize,
+    // [33] kernel sp
+    pub kernel_sp: usize,
 }
diff --git a/src/entry.rs b/src/entry.rs
index 1953135..7f95099 100644
--- a/src/entry.rs
+++ b/src/entry.rs
@@ -1,6 +1,9 @@
 use log::{debug, error, info};
 
+use crate::plat::io::{RawConsole, Reader};
 use crate::plat::lowlevel::{Hardware, LowLevel};
+use crate::plat::timer::{Timer, TimerOps};
+use crate::plat::trap::{Trap, TrapOps};
 
 #[no_mangle]
 pub extern "C" fn rust_main(hart_id: usize, device_tree_addr: usize) -> ! {
@@ -15,8 +18,29 @@ pub extern "C" fn rust_main(hart_id: usize, device_tree_addr: usize) -> ! {
     info!("hart_id = {}", hart_id);
     debug!("device_tree_addr = {:#x}", device_tree_addr);
 
+    if hart_id != 0 {
+        error!("No SMP support now, halting...");
+        Hardware::halt();
+    }
+
     // TODO: setup and start scheduler
 
+    // --- some dummy tests ---
+
+    Trap::init();
+    Timer::init();
+    Hardware::enable_interrupt();
+
+    loop {
+        let ch = RawConsole::get_char();
+        debug!("Key: {}", ch);
+        if ch == 'q' {
+            break;
+        }
+    }
+
+    // --- end ---
+
     error!("[rust_main] Should not reach here! Maybe scheduler is not working?");
     Hardware::shutdown(true);
 }
diff --git a/src/plat/lowlevel.rs b/src/plat/lowlevel.rs
index 37c233e..49e17f8 100644
--- a/src/plat/lowlevel.rs
+++ b/src/plat/lowlevel.rs
@@ -1,7 +1,10 @@
 pub struct Hardware;
 
 pub trait LowLevel {
-    fn halt() {}
+    fn halt() {
+        #[allow(clippy::empty_loop)]
+        loop {}
+    }
 
     fn shutdown(_failure: bool) -> ! {
         Self::halt();
@@ -14,5 +17,7 @@ pub trait LowLevel {
         panic!("Reset is not implemented for this architecture");
     }
 
-    fn disable_interrupt() {}
+    fn enable_interrupt();
+
+    fn disable_interrupt();
 }
diff --git a/src/plat/mod.rs b/src/plat/mod.rs
index cb5817f..4c151ea 100644
--- a/src/plat/mod.rs
+++ b/src/plat/mod.rs
@@ -1,2 +1,4 @@
 pub mod io;
 pub mod lowlevel;
+pub mod timer;
+pub mod trap;
diff --git a/src/plat/timer.rs b/src/plat/timer.rs
new file mode 100644
index 0000000..b5166bc
--- /dev/null
+++ b/src/plat/timer.rs
@@ -0,0 +1,7 @@
+pub struct Timer;
+
+pub trait TimerOps {
+    fn init();
+    fn read_cycle() -> u64;
+    fn tick();
+}
diff --git a/src/plat/trap.rs b/src/plat/trap.rs
new file mode 100644
index 0000000..2772a8f
--- /dev/null
+++ b/src/plat/trap.rs
@@ -0,0 +1,9 @@
+pub struct Trap;
+
+pub trait TrapOps {
+    fn init();
+}
+
+pub trait TrapContextOps {
+    fn restore(&mut self);
+}