x86: implement setjmp/longjmp/initjmp

For use with bthreads, implement CONFIG_HAS_ARCH_SJLJ.
Code is taken from U-Boot, itself based on the Linux ARCH=um.

Signed-off-by: Ahmad Fatoum <ahmad@a3f.at>
Signed-off-by: Sascha Hauer <s.hauer@pengutronix.de>

3 files changed, 123 insertions, 0 deletions

diff --git a/arch/x86/lib/Makefile b/arch/x86/lib/Makefile
index 05e43f0f2b..6a8fa7c0ff 100644
--- a/arch/x86/lib/Makefile
+++ b/arch/x86/lib/Makefile
@@ -4,5 +4,8 @@ obj-y += memory.o
 obj-y += gdt.o
 endif
 
+obj-$(CONFIG_X86_32) += setjmp_32.o
+obj-$(CONFIG_X86_64) += setjmp_64.o
+
 # needed, when running via a 16 bit BIOS
 obj-$(CONFIG_CMD_LINUX16) += linux_start.o
diff --git a/arch/x86/lib/setjmp_32.S b/arch/x86/lib/setjmp_32.S
new file mode 100644
index 0000000000..38dcb68c1b
--- /dev/null
+++ b/arch/x86/lib/setjmp_32.S
@@ -0,0 +1,60 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Written by H. Peter Anvin <hpa@zytor.com>
+ * Brought in from Linux v4.4 and modified for U-Boot
+ * From Linux arch/um/sys-i386/setjmp.S
+ */
+
+#define _REGPARM
+
+#include <linux/linkage.h>
+
+.text
+.align 8
+
+/*
+ * The jmp_buf is assumed to contain the following, in order:
+ *	%ebx
+ *	%esp
+ *	%ebp
+ *	%esi
+ *	%edi
+ *	<return address>
+ */
+
+ENTRY(setjmp)
+
+	movl %eax, %edx
+	popl %ecx		/* Return address, and adjust the stack */
+	xorl %eax, %eax		/* Return value */
+	movl %ebx, (%edx)
+	movl %esp, 4(%edx)	/* Post-return %esp! */
+	pushl %ecx		/* Make the call/return stack happy */
+	movl %ebp, 8(%edx)
+	movl %esi, 12(%edx)
+	movl %edi, 16(%edx)
+	movl %ecx, 20(%edx)	/* Return address */
+	ret
+
+ENDPROC(setjmp)
+
+ENTRY(longjmp)
+
+	xchgl %eax, %edx
+	movl (%edx), %ebx
+	movl 4(%edx), %esp
+	movl 8(%edx), %ebp
+	movl 12(%edx), %esi
+	movl 16(%edx), %edi
+	jmp *20(%edx)
+
+ENDPROC(longjmp)
+
+ENTRY(initjmp)
+
+	movl %edx, 20(%eax)	/* Return address */
+	movl %ecx, 4(%eax)	/* Post-return %esp! */
+	xorl %eax, %eax		/* Return value */
+	ret
+
+ENDPROC(initjmp)
diff --git a/arch/x86/lib/setjmp_64.S b/arch/x86/lib/setjmp_64.S
new file mode 100644
index 0000000000..28ea576cd2
--- /dev/null
+++ b/arch/x86/lib/setjmp_64.S
@@ -0,0 +1,60 @@
+/* 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)
+
+.align 8
+
+ENTRY(initjmp)
+
+	movq	%rsi, (%rdi)	/* Return address */
+	movq	%rdx, 8(%rdi)	/* Stack top */
+	xorq	%rax, %rax
+	ret
+
+ENDPROC(initjmp)