From af6d533cb597eafa5e0f86ea11a40d4f5ea6afba Mon Sep 17 00:00:00 2001
From: WangRunji <wangrunji0408@163.com>
Date: Tue, 25 Dec 2018 00:52:53 +0800
Subject: [PATCH] fix trap return on M-Mode

---
 kernel/src/arch/riscv32/boot/trap.asm | 5 +++--
 kernel/src/arch/riscv32/mod.rs        | 8 ++++++++
 2 files changed, 11 insertions(+), 2 deletions(-)

diff --git a/kernel/src/arch/riscv32/boot/trap.asm b/kernel/src/arch/riscv32/boot/trap.asm
index d32216c..35f2e44 100644
--- a/kernel/src/arch/riscv32/boot/trap.asm
+++ b/kernel/src/arch/riscv32/boot/trap.asm
@@ -8,6 +8,7 @@
 #   XLENB
 #   LOAD
 #   STORE
+#   TEST_BACK_TO_KERNEL
 
 .macro SAVE_ALL
     # If coming from userspace, preserve the user stack pointer and load
@@ -72,8 +73,8 @@ _save_context:
 .macro RESTORE_ALL
     LOAD s1, 32             # s1 = sstatus
     LOAD s2, 33             # s2 = sepc
-    andi s0, s1, 1 << 8
-    bnez s0, _restore_context   # back to S-mode? (sstatus.SPP = 1)
+    TEST_BACK_TO_KERNEL
+    bnez s0, _restore_context   # s0 = back to kernel?
 _save_kernel_sp:
     addi s0, sp, 36*XLENB
     csrw (xscratch), s0         # sscratch = kernel-sp
diff --git a/kernel/src/arch/riscv32/mod.rs b/kernel/src/arch/riscv32/mod.rs
index ac9b816..7dc2d33 100644
--- a/kernel/src/arch/riscv32/mod.rs
+++ b/kernel/src/arch/riscv32/mod.rs
@@ -52,6 +52,10 @@ global_asm!("
     .equ xcause,    0x342
     .equ xtval,     0x343
     .macro XRET\n mret\n .endm
+    .macro TEST_BACK_TO_KERNEL  // s0 == back to kernel?
+        li   s3, 3 << 11
+        and  s0, s1, s3         // mstatus.MPP = 3
+    .endm
 ");
 #[cfg(not(feature = "m_mode"))]
 global_asm!("
@@ -61,6 +65,10 @@ global_asm!("
     .equ xcause,    0x142
     .equ xtval,     0x143
     .macro XRET\n sret\n .endm
+    .macro TEST_BACK_TO_KERNEL
+        andi s0, s1, 1 << 8     // sstatus.SPP = 1
+    .endm
+
 ");
 
 #[cfg(target_pointer_width = "32")]