3 files changed, 76 insertions, 0 deletions
diff --git a/src/cpu/x86/64bit/Makefile.inc b/src/cpu/x86/64bit/Makefile.inc
index e1cf743f6c..24a5a966fb 100644
--- a/src/cpu/x86/64bit/Makefile.inc
+++ b/src/cpu/x86/64bit/Makefile.inc
@@ -1,6 +1,7 @@
 ## SPDX-License-Identifier: GPL-2.0-only
 
 all_x86-y += mode_switch.S
+all_x86-y += mode_switch2.S
 
 # Add --defsym=_start=0 to suppress a linker warning.
 $(objcbfs)/pt: $(dir)/pt.S $(obj)/config.h
diff --git a/src/cpu/x86/64bit/mode_switch2.S b/src/cpu/x86/64bit/mode_switch2.S
new file mode 100644
index 0000000000..65e9d94a98
--- /dev/null
+++ b/src/cpu/x86/64bit/mode_switch2.S
@@ -0,0 +1,48 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * Calls a x86_64 function from x86_32 context.
+ * Must not be directly invoked from C code!
+ */
+
+.text
+.code32
+	.section ".text.long_mode_call_3arg", "ax", @progbits
+	.global long_mode_call_3arg
+long_mode_call_3arg:
+
+	/* Function to call is passed in EAX. */
+
+	/* Backup registers */
+	pushal
+
+	/* Backup stack pointer */
+	mov	%esp, %ebp
+
+	/* Enter long mode, preserves ebx */
+	#include <cpu/x86/64bit/entry64.inc>
+
+	/* Align stack */
+	movabs	$0xfffffffffffffff0, %rax
+	andq	%rax, %rsp
+
+	movl	28(%rbp), %ebx	/* Function to call */
+	movl	36(%rbp), %edi	/* 1st arg */
+	movl	40(%rbp), %esi	/* 2nd arg */
+	movl	44(%rbp), %edx	/* 3rd arg */
+
+	call	*%rbx
+
+	/* Store return value on stack. popal will fetch it. */
+	mov	%eax, 28(%rbp)
+	shr	$32, %rax
+	movl	%eax, 24(%rbp)
+
+	#include <cpu/x86/64bit/exit32.inc>
+
+	/* Restore stack pointer */
+	mov	%ebp, %esp
+
+	/* Restore registers */
+	popal
+
+	ret
diff --git a/src/cpu/x86/64bit/prot2long.inc b/src/cpu/x86/64bit/prot2long.inc
new file mode 100644
index 0000000000..96c44a86f5
--- /dev/null
+++ b/src/cpu/x86/64bit/prot2long.inc
@@ -0,0 +1,27 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+
+.text
+.code32
+/*
+ * Macro to wrap a x86_64 function to be called from x86_32 code.
+ * This assumes that 0-3 32bit arguments are passed to the
+ * calling function.
+ *
+ * In order to preserve ESP without setting up a stack frame
+ * pass the function to call in EAX. The macro prepends "__prot2lm_"
+ * to the wrapped function name.
+ */
+.macro prot2lm_wrapper func2call:req
+	.global __prot2lm_\func2call
+__prot2lm_\func2call :
+	/* Get function to call */
+	mov	$\func2call, %eax
+
+	/*
+	 * Jump to function instead of call.
+	 * It will return directly to caller.
+	 */
+	jmp	long_mode_call_3arg
+
+	/* Not reachable */
+.endm