x86: Add 64-bit setjmp/longjmp implementation

Add setjmp/longjmp functions for x86_64.

Signed-off-by: Ivan Gorinov <ivan.gorinov@intel.com>
Reviewed-by: Bin Meng <bmeng.cn@gmail.com>

3 files changed, 66 insertions, 19 deletions

diff --git a/arch/x86/cpu/x86_64/setjmp.S b/arch/x86/cpu/x86_64/setjmp.S
new file mode 100644
index 0000000000..97b812854c
--- /dev/null
+++ b/arch/x86/cpu/x86_64/setjmp.S
@@ -0,0 +1,49 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright (C) 2018 Intel Corporation
+ *
+ * See arch/x86/include/asm/setjmp.h for jmp_buf format
+ */
+
+#include <linux/linkage.h>
+
+.text
+.align 8
+
+ENTRY(setjmp)
+
+	pop	%rcx
+	movq	%rcx, (%rdi)	/* Return address */
+	movq	%rsp, 8(%rdi)
+	movq	%rbp, 16(%rdi)
+	movq	%rbx, 24(%rdi)
+	movq	%r12, 32(%rdi)
+	movq	%r13, 40(%rdi)
+	movq	%r14, 48(%rdi)
+	movq	%r15, 56(%rdi)
+	xorq	%rax, %rax	/* Direct invocation returns 0 */
+	jmpq	*%rcx
+
+ENDPROC(setjmp)
+
+.align 8
+
+ENTRY(longjmp)
+
+	movq	(%rdi), %rcx	/* Return address */
+	movq	8(%rdi), %rsp
+	movq	16(%rdi), %rbp
+	movq	24(%rdi), %rbx
+	movq	32(%rdi), %r12
+	movq	40(%rdi), %r13
+	movq	48(%rdi), %r14
+	movq	56(%rdi), %r15
+
+	movq	%rsi, %rax	/* Value to be returned by setjmp() */
+	testq	%rax, %rax	/* cannot be 0 in this case */
+	jnz	1f
+	incq	%rax		/* Return 1 instead */
+1:
+	jmpq	*%rcx
+
+ENDPROC(longjmp)
diff --git a/arch/x86/cpu/x86_64/setjmp.c b/arch/x86/cpu/x86_64/setjmp.c
deleted file mode 100644
index 5d4a74a571..0000000000
--- a/arch/x86/cpu/x86_64/setjmp.c
+++ /dev/null
@@ -1,19 +0,0 @@
-// SPDX-License-Identifier: GPL-2.0+
-/*
- * Copyright (c) 2016 Google, Inc
- */
-
-#include <common.h>
-#include <asm/setjmp.h>
-
-int setjmp(struct jmp_buf_data *jmp_buf)
-{
-	printf("WARNING: setjmp() is not supported\n");
-
-	return 0;
-}
-
-void longjmp(struct jmp_buf_data *jmp_buf, int val)
-{
-	printf("WARNING: longjmp() is not supported\n");
-}
diff --git a/arch/x86/include/asm/setjmp.h b/arch/x86/include/asm/setjmp.h
index f25975fe1d..49c36c1cc8 100644
--- a/arch/x86/include/asm/setjmp.h
+++ b/arch/x86/include/asm/setjmp.h
@@ -8,6 +8,21 @@
 #ifndef __setjmp_h
 #define __setjmp_h
 
+#ifdef CONFIG_X86_64
+
+struct jmp_buf_data {
+	unsigned long __rip;
+	unsigned long __rsp;
+	unsigned long __rbp;
+	unsigned long __rbx;
+	unsigned long __r12;
+	unsigned long __r13;
+	unsigned long __r14;
+	unsigned long __r15;
+};
+
+#else
+
 struct jmp_buf_data {
 	unsigned int __ebx;
 	unsigned int __esp;
@@ -17,6 +32,8 @@ struct jmp_buf_data {
 	unsigned int __eip;
 };
 
+#endif
+
 int setjmp(struct jmp_buf_data *jmp_buf);
 void longjmp(struct jmp_buf_data *jmp_buf, int val);