From 9723d7c1a3ee886ae1f6ccbcaff6c79c503857ba Mon Sep 17 00:00:00 2001
From: WangRunji <wangrunji0408@163.com>
Date: Sun, 13 May 2018 01:36:16 +0800
Subject: [PATCH] Add Invalid Opcode handler. Set rsp when going to user.

---
 src/arch/x86_64/idt/mod.rs            |  1 +
 src/arch/x86_64/interrupt/handler.rs  |  6 ++++++
 src/arch/x86_64/interrupt/template.rs | 14 ++++++++------
 3 files changed, 15 insertions(+), 6 deletions(-)

diff --git a/src/arch/x86_64/idt/mod.rs b/src/arch/x86_64/idt/mod.rs
index 1e1cff0..548e614 100644
--- a/src/arch/x86_64/idt/mod.rs
+++ b/src/arch/x86_64/idt/mod.rs
@@ -14,6 +14,7 @@ pub fn init() {
         idt[T_BRKPT].set_handler_fn(breakpoint);
         idt[T_PGFLT].set_handler_fn(page_fault);
         idt[T_GPFLT].set_handler_fn(general_protection_fault);
+        idt[T_ILLOP].set_handler_fn(invalid_opcode);
         idt[T_IRQ0 + IRQ_COM1].set_handler_fn(com1);
         idt[T_IRQ0 + IRQ_COM2].set_handler_fn(com2);
         idt[T_IRQ0 + IRQ_KBD].set_handler_fn(keyboard);
diff --git a/src/arch/x86_64/interrupt/handler.rs b/src/arch/x86_64/interrupt/handler.rs
index 6810c1f..bbab556 100644
--- a/src/arch/x86_64/interrupt/handler.rs
+++ b/src/arch/x86_64/interrupt/handler.rs
@@ -28,6 +28,12 @@ interrupt_error_p!(general_protection_fault, stack, {
     loop {}
 });
 
+interrupt_stack_p!(invalid_opcode, stack, {
+    println!("\nEXCEPTION: Invalid Opcode");
+    stack.dump();
+    loop {}
+});
+
 #[cfg(feature = "use_apic")]
 use arch::driver::apic::ack;
 #[cfg(not(feature = "use_apic"))]
diff --git a/src/arch/x86_64/interrupt/template.rs b/src/arch/x86_64/interrupt/template.rs
index 64451d7..8067527 100644
--- a/src/arch/x86_64/interrupt/template.rs
+++ b/src/arch/x86_64/interrupt/template.rs
@@ -336,16 +336,18 @@ macro_rules! interrupt_switch {
             let mut rsp: usize;
             asm!("" : "={rsp}"(rsp) : : : "intel", "volatile");
 
-            // Map kernel
-//            $crate::arch::x86_64::pti::map();
-
             // Call inner rust function
             inner(&mut rsp);
 
-            asm!("" : : "{rsp}"(rsp) : : "intel", "volatile");
+            // Set return rsp if to user
+            use arch::gdt;
+            use core::mem::size_of;
+            let tf = &mut *(rsp as *mut TrapFrame);
+            if tf.iret.cs == gdt::UCODE_SELECTOR.0 as usize {
+                gdt::set_ring0_rsp(rsp + size_of::<TrapFrame>());
+            }
 
-            // Unmap kernel
-//            $crate::arch::x86_64::pti::unmap();
+            asm!("" : : "{rsp}"(rsp) : : "intel", "volatile");
 
             // Pop scratch registers and return
             fs_pop!();