diff --git a/kernel/src/arch/mipsel/boot/regdef.h b/kernel/src/arch/mipsel/boot/regdef.h
new file mode 100644
index 0000000..14721cb
--- /dev/null
+++ b/kernel/src/arch/mipsel/boot/regdef.h
@@ -0,0 +1,55 @@
+/*
+ * This file is subject to the terms and conditions of the GNU General Public
+ * License.  See the file "COPYING" in the main directory of this archive
+ * for more details.
+ *
+ * Copyright (C) 1985 MIPS Computer Systems, Inc.
+ * Copyright (C) 1994, 95, 99, 2003 by Ralf Baechle
+ * Copyright (C) 1990 - 1992, 1999 Silicon Graphics, Inc.
+ */
+#ifndef _ASM_REGDEF_H
+#define _ASM_REGDEF_H
+
+
+
+/*
+ * Symbolic register names for 32 bit ABI
+ */
+#define zero	$0	/* wired zero */
+#define AT	$1	/* assembler temp  - uppercase because of ".set at" */
+#define v0	$2	/* return value */
+#define v1	$3
+#define a0	$4	/* argument registers */
+#define a1	$5
+#define a2	$6
+#define a3	$7
+#define t0	$8	/* caller saved */
+#define t1	$9
+#define t2	$10
+#define t3	$11
+#define t4	$12
+#define t5	$13
+#define t6	$14
+#define t7	$15
+#define s0	$16	/* callee saved */
+#define s1	$17
+#define s2	$18
+#define s3	$19
+#define s4	$20
+#define s5	$21
+#define s6	$22
+#define s7	$23
+#define t8	$24	/* caller saved */
+#define t9	$25
+#define jp	$25	/* PIC jump register */
+#define k0	$26	/* kernel scratch */
+#define k1	$27
+#define gp	$28	/* global pointer */
+#define sp	$29	/* stack pointer */
+#define fp	$30	/* frame pointer */
+#define s8	$30	/* same like fp! */
+#define ra	$31	/* return address */
+
+
+
+#endif /* _ASM_REGDEF_H */
diff --git a/kernel/src/arch/mipsel/boot/trap.S b/kernel/src/arch/mipsel/boot/trap.S
new file mode 100644
index 0000000..4da8de4
--- /dev/null
+++ b/kernel/src/arch/mipsel/boot/trap.S
@@ -0,0 +1,150 @@
+#include "regdef.h"
+
+	.set noat
+	.set noreorder
+    .section .text
+    .globl trap_entry
+trap_entry:
+# +0x000: TLB-miss vector
+	b general_trap_vec
+
+# +0x180: general vector
+.org 0x180
+general_trap_vec:
+	move k1, sp         # save stack pointer to k1
+	mfc0 k0, $12        # read cp0.status
+	andi k0, k0, 0x10   # extract cp0.status.ksu
+	beq  k0, zero, trap_from_kernel
+	nop  # delayslot
+
+trap_from_user:
+	# TODO: load kstack, we can use k0 to store something
+	# la     k0, address_of_kstack
+	# addiu  sp, k0, size_of_kstack
+	nop
+
+trap_from_kernel:
+	/* 
+	 *  k0 is damaged
+	 *  k1 = old stack pointer
+	 *  sp = kernel stack */
+	
+	# allocate 38 words for trapframe + 4 extra words
+	addiu sp, sp, -168
+
+	# save general registers
+	sw ra, 160(sp)
+	sw fp, 156(sp)
+	sw k1, 152(sp)  # k1 = old sp
+	sw gp, 148(sp)
+	sw k1, 144(sp)  # real k1 is damaged
+	sw k0, 140(sp)	# real k0 is damaged
+	sw t9, 136(sp)
+	sw t8, 132(sp)
+	sw s7, 128(sp)
+	sw s6, 124(sp)
+	sw s5, 120(sp)
+	sw s4, 116(sp)
+	sw s3, 112(sp)
+	sw s2, 108(sp)
+	sw s1, 104(sp)
+	sw s0, 100(sp)
+	sw t7, 96(sp)
+	sw t6, 92(sp)
+	sw t5, 88(sp)
+	sw t4, 84(sp)
+	sw t3, 80(sp)
+	sw t2, 76(sp)
+	sw t1, 72(sp)
+	sw t0, 68(sp)
+	sw a3, 64(sp)
+	sw a2, 60(sp)
+	sw a1, 56(sp)
+	sw a0, 52(sp)
+	sw v1, 48(sp)
+	sw v0, 44(sp)
+	sw AT, 40(sp)
+	nop	
+
+	# save hi/lo
+	mflo t1
+	sw t1, 36(sp)
+	mfhi t0
+	sw t0, 32(sp)
+
+	# save special registers
+	mfc0 t0, $8     # cp0.vaddr
+	sw t0, 28(sp)
+
+	mfc0 t1, $14    # cp0.epc
+	sw t1, 24(sp)
+
+	mfc0 t0, $13    # cp0.cause
+	sw t0, 20(sp)
+
+	mfc0 t1, $12    # cp0.status
+	sw t1, 16(sp)
+
+	# support nested interrupt
+	la   t0, ~0x1b   # reset status.ksu, status.exl, status.ie
+	and  t1, t1, t0
+	mtc0 t1, $12     # cp0.status
+
+	# prepare to call rust_trap
+	jal rust_trap
+	addiu a0, sp, 16             /* set argument */
+
+	.globl trap_return
+trap_return:
+	# restore special registers
+	lw   t1, 16(sp)
+	ori  t1, t1, 0x2  # status.exl
+	nop
+	mtc0 t1, $12      # cp0.status
+
+	lw k0, 24(sp)
+	mtc0 k0, $14      # cp0.epc
+
+	lw t0, 32(sp)
+	mthi t0
+	lw t1, 36(sp)
+	mtlo t1
+
+	# restore general registers
+	lw AT, 40(sp)
+	lw v0, 44(sp)
+	lw v1, 48(sp)
+	lw a0, 52(sp)
+	lw a1, 56(sp)
+	lw a2, 60(sp)
+	lw a3, 64(sp)
+	lw t0, 68(sp)
+	lw t1, 72(sp)
+	lw t2, 76(sp)
+	lw t3, 80(sp)
+	lw t4, 84(sp)
+	lw t5, 88(sp)
+	lw t6, 92(sp)
+	lw t7, 96(sp)
+	lw s0, 100(sp)
+	lw s1, 104(sp)
+	lw s2, 108(sp)
+	lw s3, 112(sp)
+	lw s4, 116(sp)
+	lw s5, 120(sp)
+	lw s6, 124(sp)
+	lw s7, 128(sp)
+	lw t8, 132(sp)
+	lw t9, 136(sp)
+
+	# lw k0, 140(sp)
+	# lw k1, 144(sp)
+	lw gp, 148(sp)
+	lw fp, 156(sp)
+	lw ra, 160(sp)
+
+	lw sp, 152(sp)
+
+	eret
+	nop
+
diff --git a/kernel/src/arch/mipsel/boot/trap.asm b/kernel/src/arch/mipsel/boot/trap.asm
deleted file mode 100644
index 17ff77e..0000000
--- a/kernel/src/arch/mipsel/boot/trap.asm
+++ /dev/null
@@ -1,129 +0,0 @@
-# Constants / Macros defined in Rust code:
-#   XLENB
-#   LOAD
-#   STORE
-#   TEST_BACK_TO_KERNEL
-
-.macro SAVE_ALL
-    # If coming from userspace, preserve the user stack pointer and load
-    # the kernel stack pointer. If we came from the kernel, sscratch
-    # will contain 0, and we should continue on the current stack.
-    csrrw sp, sscratch, sp
-    bnez sp, trap_from_user
-trap_from_kernel:
-    csrr sp, sscratch
-    STORE gp, -1
-    # sscratch = previous-sp, sp = kernel-sp
-trap_from_user:
-    # provide room for trap frame
-    addi sp, sp, -37 * XLENB
-    # save x registers except x2 (sp)
-    STORE x1, 1
-    STORE x3, 3
-    STORE x4, 4
-    STORE x5, 5
-    STORE x6, 6
-    STORE x7, 7
-    STORE x8, 8
-    STORE x9, 9
-    STORE x10, 10
-    STORE x11, 11
-    STORE x12, 12
-    STORE x13, 13
-    STORE x14, 14
-    STORE x15, 15
-    STORE x16, 16
-    STORE x17, 17
-    STORE x18, 18
-    STORE x19, 19
-    STORE x20, 20
-    STORE x21, 21
-    STORE x22, 22
-    STORE x23, 23
-    STORE x24, 24
-    STORE x25, 25
-    STORE x26, 26
-    STORE x27, 27
-    STORE x28, 28
-    STORE x29, 29
-    STORE x30, 30
-    STORE x31, 31
-
-    # load hartid to gp from sp[36]
-    LOAD gp, 36
-
-    # get sp, sstatus, sepc, stval, scause
-    # set sscratch = 0
-    csrrw s0, sscratch, x0
-    csrr s1, sstatus
-    csrr s2, sepc
-    csrr s3, stval
-    csrr s4, scause
-    # store sp, sstatus, sepc, sbadvaddr, scause
-    STORE s0, 2
-    STORE s1, 32
-    STORE s2, 33
-    STORE s3, 34
-    STORE s4, 35
-.endm
-
-.macro RESTORE_ALL
-    LOAD s1, 32             # s1 = sstatus
-    LOAD s2, 33             # s2 = sepc
-    andi s0, s1, 1 << 8     # sstatus.SPP = 1
-    bnez s0, _to_kernel     # s0 = back to kernel?
-_to_user:
-    addi s0, sp, 37*XLENB
-    csrw sscratch, s0     # sscratch = kernel-sp
-    STORE gp, 36            # store hartid from gp to sp[36]
-_to_kernel:
-    # restore sstatus, sepc
-    csrw sstatus, s1
-    csrw sepc, s2
-
-    # restore x registers except x2 (sp)
-    LOAD x1, 1
-    LOAD x3, 3
-    LOAD x4, 4
-    LOAD x5, 5
-    LOAD x6, 6
-    LOAD x7, 7
-    LOAD x8, 8
-    LOAD x9, 9
-    LOAD x10, 10
-    LOAD x11, 11
-    LOAD x12, 12
-    LOAD x13, 13
-    LOAD x14, 14
-    LOAD x15, 15
-    LOAD x16, 16
-    LOAD x17, 17
-    LOAD x18, 18
-    LOAD x19, 19
-    LOAD x20, 20
-    LOAD x21, 21
-    LOAD x22, 22
-    LOAD x23, 23
-    LOAD x24, 24
-    LOAD x25, 25
-    LOAD x26, 26
-    LOAD x27, 27
-    LOAD x28, 28
-    LOAD x29, 29
-    LOAD x30, 30
-    LOAD x31, 31
-    # restore sp last
-    LOAD x2, 2
-.endm
-
-    .section .text
-    .globl trap_entry
-trap_entry:
-    SAVE_ALL
-    mv a0, sp
-    jal rust_trap
-    .globl trap_return
-trap_return:
-    RESTORE_ALL
-    # return from supervisor call
-    sret
diff --git a/kernel/src/arch/mipsel/context.rs b/kernel/src/arch/mipsel/context.rs
index a8b6e5f..dc6eea7 100644
--- a/kernel/src/arch/mipsel/context.rs
+++ b/kernel/src/arch/mipsel/context.rs
@@ -1,23 +1,27 @@
-use riscv::register::{
-    sstatus,
-    sstatus::Sstatus,
-    scause::Scause,
-};
+use mips::registers;
 
 /// Saved registers on a trap.
 #[derive(Clone)]
 #[repr(C)]
 pub struct TrapFrame {
-    /// General registers
-    pub x: [usize; 32],
-    /// Supervisor Status
-    pub sstatus: Sstatus,
-    /// Supervisor Exception Program Counter
-    pub sepc: usize,
-    /// Supervisor Trap Value
-    pub stval: usize,
-    /// Supervisor Cause
-    pub scause: Scause,
+    /// CP0 status register
+    pub status: usize;
+    /// CP0 cause register
+    pub cause: usize;
+    /// CP0 EPC register
+    pub epc: usize;
+    /// CP0 vaddr register
+    pub vaddr: usize;
+    /// HI/LO registers
+    pub hi, lo: usize;
+    /// General registers 1-7
+    pub at, v0, v1, a0, a1, a2, a3: u32;
+    /// General registers 8-15
+    pub t0, t1, t2, t3, t4, t5, t6, t7: u32;  
+    /// General registers 16-23
+    pub s0, s1, s2, s3, s4, s5, s6, s7: u32;
+    /// General registers 24-31
+    pub t8, t9, k0, k1, gp, sp, fp, ra: u32;
     /// Reserve space for hartid
     pub _hartid: usize,
 }
diff --git a/kernel/src/arch/mipsel/interrupt.rs b/kernel/src/arch/mipsel/interrupt.rs
index 9488234..f6bea8a 100644
--- a/kernel/src/arch/mipsel/interrupt.rs
+++ b/kernel/src/arch/mipsel/interrupt.rs
@@ -1,4 +1,5 @@
 use mips::interrupts::*;
+use mips::registers::*;
 use crate::drivers::DRIVERS;
 pub use self::context::*;
 use log::*;
@@ -8,6 +9,7 @@ mod context;
 
 /// Initialize interrupt
 pub fn init() {
+    // TODO
  //   extern {
  //       fn trap_entry();
  //   }
@@ -30,23 +32,23 @@ pub fn init() {
 /// Enable interrupt
 #[inline]
 pub unsafe fn enable() {
- //   sstatus::set_sie();
+    interrupts::enable();
 }
 
 /// Disable interrupt and return current interrupt status
 #[inline]
 pub unsafe fn disable_and_store() -> usize {
-  //  let e = sstatus::read().sie() as usize;
-  //  sstatus::clear_sie();
-  //  e
+    let e = cp0::status::read_u32() & 1;
+    interrupts::disable();
+    e
 }
 
 /// Enable interrupt if `flags` != 0
 #[inline]
 pub unsafe fn restore(flags: usize) {
-  //  if flags != 0 {
-  //      enable();
-  //  }
+    if flags != 0 {
+        enable();
+    }
 }
 
 /// Dispatch and handle interrupt.
@@ -54,22 +56,24 @@ pub unsafe fn restore(flags: usize) {
 /// This function is called from `trap.asm`.
 #[no_mangle]
 pub extern fn rust_trap(tf: &mut TrapFrame) {
-  //  use self::scause::{Trap, Interrupt as I, Exception as E};
-  //  trace!("Interrupt @ CPU{}: {:?} ", super::cpu::id(), tf.scause.cause());
-  //  match tf.scause.cause() {
-  //      Trap::Interrupt(I::SupervisorExternal) => external(),
-  //      Trap::Interrupt(I::SupervisorSoft) => ipi(),
-  //      Trap::Interrupt(I::SupervisorTimer) => timer(),
-  //      Trap::Exception(E::UserEnvCall) => syscall(tf),
-  //      Trap::Exception(E::LoadPageFault) => page_fault(tf),
-  //      Trap::Exception(E::StorePageFault) => page_fault(tf),
-  //      Trap::Exception(E::InstructionPageFault) => page_fault(tf),
-  //      _ => crate::trap::error(tf),
-  //  }
-  //  trace!("Interrupt end");
+    // TODO
+    use self::scause::{Trap, Interrupt as I, Exception as E};
+    // trace!("Interrupt @ CPU{}: {:?} ", super::cpu::id(), tf.scause.cause());
+    match tf.scause.cause() {
+        Trap::Interrupt(I::SupervisorExternal) => external(),
+        Trap::Interrupt(I::SupervisorSoft) => ipi(),
+        Trap::Interrupt(I::SupervisorTimer) => timer(),
+        Trap::Exception(E::UserEnvCall) => syscall(tf),
+        Trap::Exception(E::LoadPageFault) => page_fault(tf),
+        Trap::Exception(E::StorePageFault) => page_fault(tf),
+        Trap::Exception(E::InstructionPageFault) => page_fault(tf),
+        _ => crate::trap::error(tf),
+    }
+    trace!("Interrupt end");
 }
 
 fn external() {
+    // TODO
     #[cfg(feature = "board_u540")]
     unsafe { super::board::handle_external_interrupt(); }
 
@@ -83,6 +87,7 @@ fn external() {
 }
 
 fn try_process_serial() -> bool {
+    // TODO
     match super::io::getchar_option() {
         Some(ch) => {
             crate::trap::serial(ch);
@@ -93,6 +98,7 @@ fn try_process_serial() -> bool {
 }
 
 fn try_process_drivers() -> bool {
+    // TODO
     for driver in DRIVERS.read().iter() {
         if driver.try_handle_interrupt(None) == true {
             return true
@@ -102,8 +108,9 @@ fn try_process_drivers() -> bool {
 }
 
 fn ipi() {
+    // TODO
     debug!("IPI");
-    super::sbi::clear_ipi();
+//    super::sbi::clear_ipi();
 }
 
 fn timer() {
@@ -112,12 +119,14 @@ fn timer() {
 }
 
 fn syscall(tf: &mut TrapFrame) {
+    // TODO
     tf.sepc += 4;   // Must before syscall, because of fork.
     let ret = crate::syscall::syscall(tf.x[17], [tf.x[10], tf.x[11], tf.x[12], tf.x[13], tf.x[14], tf.x[15]], tf);
     tf.x[10] = ret as usize;
 }
 
 fn page_fault(tf: &mut TrapFrame) {
+    // TODO
     let addr = tf.stval;
     trace!("\nEXCEPTION: Page Fault @ {:#x}", addr);