From b2cd72d8f3c4a3710fdbaf36802de3d6e74a4066 Mon Sep 17 00:00:00 2001
From: Alex Smith <alex@alex-smith.me.uk>
Date: Sat, 14 Jul 2012 09:23:48 +0100
Subject: [PATCH] Implemented arch_debug_call_with_fault_handler for x86_64.

---
 src/system/kernel/arch/x86/64/arch.S    | 47 +++++++++++++++++++++++++
 src/system/kernel/arch/x86/64/stubs.cpp | 10 ------
 2 files changed, 47 insertions(+), 10 deletions(-)

diff --git a/src/system/kernel/arch/x86/64/arch.S b/src/system/kernel/arch/x86/64/arch.S
index 58b4bcb994..7ec20cb39a 100644
--- a/src/system/kernel/arch/x86/64/arch.S
+++ b/src/system/kernel/arch/x86/64/arch.S
@@ -260,3 +260,50 @@ FUNCTION(arch_cpu_user_strlcpy):
 	movq	$-1, %rax
 	ret
 FUNCTION_END(arch_cpu_user_strlcpy)
+
+
+/*!	\fn void arch_debug_call_with_fault_handler(cpu_ent* cpu,
+		jmp_buf jumpBuffer, void (*function)(void*), void* parameter)
+
+	Called by debug_call_with_fault_handler() to do the dirty work of setting
+	the fault handler and calling the function. If the function causes a page
+	fault, the arch_debug_call_with_fault_handler() calls longjmp() with the
+	given \a jumpBuffer. Otherwise it returns normally.
+
+	debug_call_with_fault_handler() has already saved the CPU's fault_handler
+	and fault_handler_stack_pointer and will reset them later, so
+	arch_debug_call_with_fault_handler() doesn't need to care about it.
+
+	\param cpu The \c cpu_ent for the current CPU.
+	\param jumpBuffer Buffer to be used for longjmp().
+	\param function The function to be called.
+	\param parameter The parameter to be passed to the function to be called.
+*/
+FUNCTION(arch_debug_call_with_fault_handler):
+	push	%rbp
+	movq	%rsp, %rbp
+
+	// Preserve the jump buffer address for the fault return.
+	push	%rsi
+
+	// Set fault handler address, and fault handler stack pointer address. We
+	// don't need to save the previous values, since that's done by the caller.
+	movq	$.L_debug_call_fault_handler, CPU_ENT_fault_handler(%rdi)
+	movq	%rbp, CPU_ENT_fault_handler_stack_pointer(%rdi)
+
+	// Call the function.
+	movq	%rcx, %rdi
+	call	*%rdx
+
+	// Regular return.
+	movq	%rbp, %rsp
+	pop		%rbp
+	ret
+
+.L_debug_call_fault_handler:
+	// Fault -- return via longjmp(jumpBuffer, 1)
+	movq	%rbp, %rsp
+	movq	-8(%rsp), %rdi
+	movq	$1, %rsi
+	call	longjmp
+FUNCTION_END(arch_debug_call_with_fault_handler)
diff --git a/src/system/kernel/arch/x86/64/stubs.cpp b/src/system/kernel/arch/x86/64/stubs.cpp
index b8efb2ff25..292572cc25 100644
--- a/src/system/kernel/arch/x86/64/stubs.cpp
+++ b/src/system/kernel/arch/x86/64/stubs.cpp
@@ -51,16 +51,6 @@ arch_commpage_init_post_cpus(void)
 }
 
 
-void
-arch_debug_call_with_fault_handler(cpu_ent* cpu, jmp_buf jumpBuffer,
-	void (*function)(void*), void* parameter)
-{
-	// To be implemented in asm, not here.
-
-	function(parameter);
-}
-
-
 // The software breakpoint instruction (int3).
 const uint8 kX86SoftwareBreakpoint[1] = { 0xcc };