fix trap return on M-Mode

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

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")]