[PATCH v3 11/16] x86: implement setjmp/longjmp/initjmp

[Ahmad Fatoum]

From: Ahmad Fatoum <ahmad at a3f.at>

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 at a3f.at>
---
 arch/x86/Kconfig              |  2 ++
 arch/x86/include/asm/setjmp.h | 44 +++++++++++++++++++++++++
 arch/x86/lib/Makefile         |  3 ++
 arch/x86/lib/setjmp_32.S      | 60 +++++++++++++++++++++++++++++++++++
 arch/x86/lib/setjmp_64.S      | 60 +++++++++++++++++++++++++++++++++++
 5 files changed, 169 insertions(+)
 create mode 100644 arch/x86/include/asm/setjmp.h
 create mode 100644 arch/x86/lib/setjmp_32.S
 create mode 100644 arch/x86/lib/setjmp_64.S

diff --git a/arch/x86/Kconfig b/arch/x86/Kconfig
index 0e3e5d61872a..e942c79cbd49 100644
--- a/arch/x86/Kconfig
+++ b/arch/x86/Kconfig
@@ -36,10 +36,12 @@ config 64BIT
 config X86_32
 	def_bool y
 	depends on !64BIT
+	select HAS_ARCH_SJLJ
 
 config X86_64
 	def_bool y
 	depends on 64BIT
+	select HAS_ARCH_SJLJ
 
 endmenu
 
diff --git a/arch/x86/include/asm/setjmp.h b/arch/x86/include/asm/setjmp.h
new file mode 100644
index 000000000000..88c198cfaeb9
--- /dev/null
+++ b/arch/x86/include/asm/setjmp.h
@@ -0,0 +1,44 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Written by H. Peter Anvin <hpa at zytor.com>
+ * Brought in from Linux v4.4 and modified for U-Boot
+ * From Linux arch/um/sys-i386/setjmp.S
+ */
+
+#ifndef __setjmp_h
+#define __setjmp_h
+
+#include <linux/compiler.h>
+
+struct jmp_buf_data {
+#if defined CONFIG_X86_64
+#define __sjlj_attr
+	unsigned long __rip;
+	unsigned long __rsp;
+	unsigned long __rbp;
+	unsigned long __rbx;
+	unsigned long __r12;
+	unsigned long __r13;
+	unsigned long __r14;
+	unsigned long __r15;
+#elif defined CONFIG_X86_32
+#define __sjlj_attr	__attribute__((regparm(3)))
+	unsigned int __ebx;
+	unsigned int __esp;
+	unsigned int __ebp;
+	unsigned int __esi;
+	unsigned int __edi;
+	unsigned int __eip;
+#else
+#error "Unsupported configuration"
+#endif
+};
+
+typedef struct jmp_buf_data jmp_buf[1];
+
+int setjmp(jmp_buf jmp) __attribute__((returns_twice)) __sjlj_attr;
+void longjmp(jmp_buf jmp, int ret) __attribute__((noreturn)) __sjlj_attr;
+
+int initjmp(jmp_buf jmp, void __noreturn (*func)(void), void *stack_top) __sjlj_attr;
+
+#endif
diff --git a/arch/x86/lib/Makefile b/arch/x86/lib/Makefile
index 05e43f0f2b34..6a8fa7c0ff3e 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 000000000000..38dcb68c1b59
--- /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 at 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 000000000000..28ea576cd22e
--- /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)
-- 
2.29.2