From 4a812425525b37dc4ff3811b373f98f445225974 Mon Sep 17 00:00:00 2001
From: Yuhao Zhou <miskcoo@gmail.com>
Date: Mon, 8 Apr 2019 12:50:30 +0800
Subject: [PATCH] Fix kstack switch.

---
 kernel/src/arch/mipsel/boot/entry.S |  6 ++++++
 kernel/src/arch/mipsel/boot/trap.S  | 11 ++++++++---
 kernel/src/arch/mipsel/context.rs   |  8 ++++++++
 kernel/src/arch/mipsel/interrupt.rs |  5 ++++-
 4 files changed, 26 insertions(+), 4 deletions(-)

diff --git a/kernel/src/arch/mipsel/boot/entry.S b/kernel/src/arch/mipsel/boot/entry.S
index 1a2a44b..4bbdc35 100644
--- a/kernel/src/arch/mipsel/boot/entry.S
+++ b/kernel/src/arch/mipsel/boot/entry.S
@@ -3,12 +3,18 @@
 	.set noreorder
     .section .text.entry
     .globl _start
+	.extern _root_page_table_buffer
+	.extern _cur_kstack_ptr
 
 _start:
     # setup stack and gp
 	la    sp, bootstacktop
     la    gp, _gp
 
+	la    t0, _cur_kstack_ptr
+	la    t1, _root_page_table_buffer
+	sw    t1, 0(t0)
+
     # set ebase
     la    t0, trap_entry
     mfc0  t1, $15 # C0_EBASE
diff --git a/kernel/src/arch/mipsel/boot/trap.S b/kernel/src/arch/mipsel/boot/trap.S
index d232d33..bd1b02b 100644
--- a/kernel/src/arch/mipsel/boot/trap.S
+++ b/kernel/src/arch/mipsel/boot/trap.S
@@ -21,8 +21,10 @@ general_trap_vec:
 
 trap_from_user:
 	# load kstack, we can use k0 to store something
-	la     k0, kernel_stack
-	la     sp, kernel_stack_top
+#	la     k0, kernel_stack
+#	la     sp, kernel_stack_top
+	la     k0, _cur_kstack_ptr
+	lw     sp, 0(k0)
 
 trap_from_kernel:
 	/* 
@@ -161,7 +163,10 @@ kernel_stack_top:
     .align 12  #PGSHIFT
     .global _root_page_table_buffer
 _root_page_table_buffer:
-    .space 1024 * 4 # 4KB
+    .space 1024 * 64 # 64KB
     .global _root_page_table_ptr
 _root_page_table_ptr:
     .space 4 # 4bytes
+    .global _cur_kstack_ptr
+_cur_kstack_ptr:
+    .space 4 # 4bytes
diff --git a/kernel/src/arch/mipsel/context.rs b/kernel/src/arch/mipsel/context.rs
index 4dfdad9..70c0458 100644
--- a/kernel/src/arch/mipsel/context.rs
+++ b/kernel/src/arch/mipsel/context.rs
@@ -149,6 +149,10 @@ pub struct Context {
     sp: usize,
 }
 
+extern "C" {
+    fn _cur_kstack_ptr();
+}
+
 impl Context {
     /// Switch to another kernel thread.
     ///
@@ -161,6 +165,10 @@ impl Context {
             fn switch_context(src: *mut Context, dst: *mut Context);
         }
 
+        unsafe {
+            *(_cur_kstack_ptr as *mut usize) = target.sp;
+        }
+
         switch_context(self as *mut Context, target as *mut Context);
     }
 
diff --git a/kernel/src/arch/mipsel/interrupt.rs b/kernel/src/arch/mipsel/interrupt.rs
index 5a64411..ffbd5c0 100644
--- a/kernel/src/arch/mipsel/interrupt.rs
+++ b/kernel/src/arch/mipsel/interrupt.rs
@@ -68,7 +68,10 @@ pub extern fn rust_trap(tf: &mut TrapFrame) {
         E::TLBModification => page_fault(tf),
         E::TLBLoadMiss => page_fault(tf),
         E::TLBStoreMiss => page_fault(tf),
-        _ => crate::trap::error(tf),
+        _ => {
+            error!("Unhandled Exception @ CPU{}: {:?} ", 0, tf.cause.cause());
+            crate::trap::error(tf)
+        }
     }
     trace!("Interrupt end");
 }