idt: implement #CP handler

This exception handler will be executed when the CPU detects a mismatch
between the return address on the stack and the return address on the
shadow stack.

Signed-off-by: Tom Dohrmann <erbse.13@gmx.de>

kernel/src/cpu/idt/entry.S

@@ -349,7 +349,11 @@ default_entry_no_ist	name=xf		handler=panic			error_code=0	vector=19
 // Vector 20 not defined
 
 // #CP Control-Protection Exception (Vector 21)
+.if CFG_SHADOW_STACKS
+default_entry_no_ist	name=cp		handler=control_protection			error_code=1	vector=21
+.else
 default_entry_no_ist	name=cp		handler=panic			error_code=1	vector=21
+.endif
 
 // Vectors 22-27 not defined
 

kernel/src/cpu/idt/svsm.rs

@@ -18,6 +18,7 @@ use super::common::{
 use crate::address::VirtAddr;
 use crate::cpu::X86ExceptionContext;
 use crate::debug::gdbstub::svsm_gdbstub::handle_debug_exception;
+use crate::mm::GuestPtr;
 use crate::platform::SVSM_PLATFORM;
 use crate::task::{is_task_fault, terminate};
 
@@ -202,6 +203,51 @@ extern "C" fn ex_handler_page_fault(ctxt: &mut X86ExceptionContext, vector: usiz
     }
 }
 
+// Control-Protection handler
+#[no_mangle]
+extern "C" fn ex_handler_control_protection(ctxt: &mut X86ExceptionContext, _vector: usize) {
+    // From AMD64 Architecture Programmer's Manual, Volume 2, 8.4.3
+    // Control-Protection Error Code:
+    /// A RET (near) instruction encountered a return address mismatch.
+    const NEAR_RET: usize = 1;
+    /// A RET (far) or IRET instruction encountered a return address mismatch.
+    const FAR_RET_IRET: usize = 2;
+    /// An RSTORSSP instruction encountered an invalid shadow stack restore
+    /// token.
+    const RSTORSSP: usize = 4;
+    /// A SETSSBSY instruction encountered an invalid supervisor shadow stack
+    /// token.
+    const SETSSBSY: usize = 5;
+
+    let rip = ctxt.frame.rip;
+    match ctxt.error_code & 0x7fff {
+        code @ (NEAR_RET | FAR_RET_IRET) => {
+            // Read the return address on the normal stack.
+            let ret_ptr: GuestPtr<u64> = GuestPtr::new(VirtAddr::from(ctxt.frame.rsp));
+            let ret = unsafe { ret_ptr.read() }.expect("Failed to read return address");
+
+            // Read the return address on the shadow stack.
+            let prev_rssp_ptr: GuestPtr<u64> = GuestPtr::new(VirtAddr::from(ctxt.ssp));
+            let prev_rssp = unsafe { prev_rssp_ptr.read() }
+                .expect("Failed to read address of previous shadow stack pointer");
+            // The offset to the return pointer is different for RET and IRET.
+            let offset = if code == NEAR_RET { 0 } else { 8 };
+            let ret_ptr: GuestPtr<u64> = GuestPtr::new(VirtAddr::from(prev_rssp + offset));
+            let ret_on_ssp =
+                unsafe { ret_ptr.read() }.expect("Failed to read return address on shadow stack");
+
+            panic!("thread tried to return to {ret:#x}, but return address on shadow stack was {ret_on_ssp:#x}!");
+        }
+        RSTORSSP => {
+            panic!("rstorssp instruction encountered an unexpected shadow stack restore token at RIP {rip:#018x}");
+        }
+        SETSSBSY => {
+            panic!("setssbsy instruction encountered an unexpected supervisor shadow stack token at RIP {rip:#018x}");
+        }
+        code => unreachable!("unexpected code for #CP exception: {code}"),
+    }
+}
+
 // VMM Communication handler
 #[no_mangle]
 extern "C" fn ex_handler_vmm_communication(ctxt: &mut X86ExceptionContext, vector: usize) {