implement ucontext functions on x86_64

.gitignore

@@ -31,3 +31,7 @@ Makefile.in
 # http://www.gnu.org/software/texinfo
 
 /texinfo.tex
+
+*.o
+test_libucontext
+libucontext.so.*

Makefile

@@ -1,8 +1,15 @@
 ARCH := x86_64
-LIBUCONTEXT_C_SRC =
+
+CFLAGS = -ggdb -O2 -Wall
+
+LIBUCONTEXT_C_SRC = \
+	arch/${ARCH}/makecontext.c
+
 LIBUCONTEXT_S_SRC = \
 	arch/${ARCH}/getcontext.S \
-	arch/${ARCH}/setcontext.S
+	arch/${ARCH}/setcontext.S \
+	arch/${ARCH}/swapcontext.S \
+	arch/${ARCH}/startcontext.S
 
 LIBUCONTEXT_OBJ = ${LIBUCONTEXT_C_SRC:.c=.o} ${LIBUCONTEXT_S_SRC:.S=.o}
 LIBUCONTEXT_SOVERSION = 0

arch/x86_64/getcontext.S

@@ -1,29 +1,51 @@
+/*
+ * Copyright (c) 2018 William Pitcock <nenolod@dereferenced.org>
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * This software is provided 'as is' and without any warranty, express or
+ * implied.  In no event shall the authors be liable for any damages arising
+ * from the use of this software.
+ */
+
 .globl __getcontext;
 __getcontext:
-	movq	%rdi, 8(%rdi)
+	/* copy all of the current registers into the ucontext structure */
-	movq	%rsi, 16(%rdi)
-	movq	%rdx, 24(%rdi)
-	movq	%rcx, 32(%rdi)
 	movq	%r8, 40(%rdi)
 	movq	%r9, 48(%rdi)
-	movq	$1, 56(%rdi)	/* %rax */
+	movq	%r10, 56(%rdi)
-	movq	%rbx, 64(%rdi)
+	movq	%r11, 64(%rdi)
-	movq	%rbp, 72(%rdi)
+	movq	%r12, 72(%rdi)
-	movq	%r10, 80(%rdi)
+	movq	%r13, 80(%rdi)
-	movq	%r11, 88(%rdi)
+	movq	%r14, 88(%rdi)
-	movq	%r12, 96(%rdi)
+	movq	%r15, 96(%rdi)
-	movq	%r13, 104(%rdi)
+	movq	%rdi, 104(%rdi)
-	movq	%r14, 112(%rdi)
+	movq	%rbp, 112(%rdi)
-	movq	%r15, 120(%rdi)
+	movq	%rsi, 120(%rdi)
+	movq	%rbx, 128(%rdi)
+	movq	%rdx, 136(%rdi)
+	movq	$1, 144(%rdi)		/* $1 is %rax */
+	movq	%rcx, 152(%rdi)
 
-	movq	(%rsp), %rcx	/* %rip */
+	/* the first argument on the stack is the jump target (%rip), so we store it in the RIP
+	   register in the ucontext structure. */
+	movq	(%rsp), %rcx
+	movq	%rcx, 168(%rdi)
+
+	/* finally take the stack pointer address (%rsp) offsetting by 8 to skip over the jump
+	   target. */
+	leaq	8(%rsp), %rcx
 	movq	%rcx, 160(%rdi)
-	leaq	8(%rsp), %rcx	/* %rsp */
+
-	movq	%rcx, 184(%rdi)
+	/* we need to restore %rcx because we clobbered it earlier */	
-	
+	movq	152(%rdi), %rcx
-	movq	32(%rdi), %rcx	/* restore %rcx */
+
-	movq	$0, %rax
+	/* we're all done here, return 0 */
+	xorl	%eax, %eax
 	ret
 
+
 .weak getcontext;
 getcontext = __getcontext;

arch/x86_64/makecontext.c

@@ -0,0 +1,77 @@
+/*
+ * Copyright (c) 2018 William Pitcock <nenolod@dereferenced.org>
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * This software is provided 'as is' and without any warranty, express or
+ * implied.  In no event shall the authors be liable for any damages arising
+ * from the use of this software.
+ */
+
+#define _GNU_SOURCE
+#include <stddef.h>
+#include <stdarg.h>
+#include <signal.h>
+#include <string.h>
+#include <stdint.h>
+
+
+extern void __start_context(void);
+
+
+void
+__makecontext(ucontext_t *ucp, void (*func)(void), int argc, ...)
+{
+	greg_t *sp;
+	va_list va;
+	int i;
+	unsigned int uc_link;
+
+	uc_link = (argc > 6 ? argc - 6 : 0) + 1;
+
+	sp = (greg_t *) ((uintptr_t) ucp->uc_stack.ss_sp + ucp->uc_stack.ss_size);
+	sp -= uc_link;
+	sp = (greg_t *) (((uintptr_t) sp & -16L) - 8);
+
+	ucp->uc_mcontext.gregs[REG_RIP] = (uintptr_t) func;
+	ucp->uc_mcontext.gregs[REG_RBX] = (uintptr_t) &sp[uc_link];
+	ucp->uc_mcontext.gregs[REG_RSP] = (uintptr_t) sp;
+
+	sp[0] = (uintptr_t) &__start_context;
+	sp[uc_link] = (uintptr_t) ucp->uc_link;
+
+	va_start(va, argc);
+
+	for (i = 0; i < argc; i++)
+		switch (i)
+		{
+		case 0:
+			ucp->uc_mcontext.gregs[REG_RDI] = va_arg (va, greg_t);
+			break;
+		case 1:
+			ucp->uc_mcontext.gregs[REG_RSI] = va_arg (va, greg_t);
+			break;
+		case 2:
+			ucp->uc_mcontext.gregs[REG_RDX] = va_arg (va, greg_t);
+			break;
+		case 3:
+			ucp->uc_mcontext.gregs[REG_RCX] = va_arg (va, greg_t);
+			break;
+		case 4:
+			ucp->uc_mcontext.gregs[REG_R8] = va_arg (va, greg_t);
+			break;
+		case 5:
+			ucp->uc_mcontext.gregs[REG_R9] = va_arg (va, greg_t);
+			break;
+		default:
+			sp[i - 5] = va_arg (va, greg_t);
+			break;
+		}
+
+	va_end(va);
+}
+
+
+extern __typeof(__makecontext) makecontext __attribute__((weak, __alias__("__makecontext")));

arch/x86_64/setcontext.S

@@ -1,22 +1,43 @@
+/*
+ * Copyright (c) 2018 William Pitcock <nenolod@dereferenced.org>
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * This software is provided 'as is' and without any warranty, express or
+ * implied.  In no event shall the authors be liable for any damages arising
+ * from the use of this software.
+ */
+
 .globl __setcontext;
 __setcontext:
-	movq	16(%rdi), %rsi
+	/* set all of the registers */
-	movq	24(%rdi), %rdx
+	movq    40(%rdi), %r8
-	movq	32(%rdi), %rcx
+	movq    48(%rdi), %r9
-	movq	40(%rdi), %r8
+	movq    56(%rdi), %r10
-	movq	48(%rdi), %r9
+	movq    64(%rdi), %r11
-	movq	56(%rdi), %rax
+	movq    72(%rdi), %r12
-	movq	64(%rdi), %rbx
+	movq    80(%rdi), %r13
-	movq	72(%rdi), %rbp
+	movq    88(%rdi), %r14
-	movq	80(%rdi), %r10
+	movq    96(%rdi), %r15
-	movq	88(%rdi), %r11
+	movq    112(%rdi), %rbp
-	movq	96(%rdi), %r12
+	movq    120(%rdi), %rsi
-	movq	104(%rdi), %r13
+	movq    128(%rdi), %rbx
-	movq	112(%rdi), %r14
+	movq    136(%rdi), %rdx
-	movq	120(%rdi), %r15
+	movq    144(%rdi), %rax
-	movq	184(%rdi), %rsp
+	movq    152(%rdi), %rcx
-	pushq	160(%rdi)	/* new %rip */
+	movq    160(%rdi), %rsp
-	movq	8(%rdi), %rdi
+
+	/* set the jump target by pushing it to the stack.
+	   ret will pop the new %rip from the stack, causing us to jump there. */
+	pushq   168(%rdi)
+
+	/* finally, set %rdi correctly. */
+	movq    104(%rdi), %rdi
+
+	/* we're all done here, return 0 */
+	xorl    %eax, %eax
 	ret
 
 .weak setcontext;

arch/x86_64/startcontext.S

@@ -0,0 +1,30 @@
+/*
+ * Copyright (c) 2018 William Pitcock <nenolod@dereferenced.org>
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * This software is provided 'as is' and without any warranty, express or
+ * implied.  In no event shall the authors be liable for any damages arising
+ * from the use of this software.
+ */
+
+.globl __start_context;
+__start_context:
+	/* get the proper context into position and test for NULL */
+	movq	%rbx, %rsp
+	movq	(%rsp), %rdi
+	testq	%rdi, %rdi
+	je	hosed
+
+	/* call setcontext to switch to the linked context */
+	call	__setcontext@plt
+	movq	%rax, %rdi
+
+hosed:
+	/* we are returning into a null context, it seems, so maybe we should exit */
+	call	exit@plt
+
+	/* something is really hosed, call hlt to force termination */
+	hlt

arch/x86_64/swapcontext.S

@@ -0,0 +1,74 @@
+/*
+ * Copyright (c) 2018 William Pitcock <nenolod@dereferenced.org>
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * This software is provided 'as is' and without any warranty, express or
+ * implied.  In no event shall the authors be liable for any damages arising
+ * from the use of this software.
+ */
+
+.globl __swapcontext;
+__swapcontext:
+	/* copy all of the current registers into the ucontext structure pointed by
+	   the first argument */
+	movq	%r8, 40(%rdi)
+	movq	%r9, 48(%rdi)
+	movq	%r10, 56(%rdi)
+	movq	%r11, 64(%rdi)
+	movq	%r12, 72(%rdi)
+	movq	%r13, 80(%rdi)
+	movq	%r14, 88(%rdi)
+	movq	%r15, 96(%rdi)
+	movq	%rdi, 104(%rdi)
+	movq	%rbp, 112(%rdi)
+	movq	%rsi, 120(%rdi)
+	movq	%rbx, 128(%rdi)
+	movq	%rdx, 136(%rdi)
+	movq	$1, 144(%rdi)		/* $1 is %rax */
+	movq	%rcx, 152(%rdi)
+
+	/* the first argument on the stack is the jump target (%rip), so we store it in the RIP
+	   register in the ucontext structure. */
+	movq	(%rsp), %rcx
+	movq	%rcx, 168(%rdi)
+
+	/* finally take the stack pointer address (%rsp) offsetting by 8 to skip over the jump
+	   target. */
+	leaq	8(%rsp), %rcx
+	movq	%rcx, 160(%rdi)
+
+	/* set all of the registers to their new states, stored in the second
+	   ucontext structure */	
+	movq	40(%rsi), %r8
+	movq	48(%rsi), %r9
+	movq	56(%rsi), %r10
+	movq	64(%rsi), %r11
+	movq	72(%rsi), %r12
+	movq	80(%rsi), %r13
+	movq	88(%rsi), %r14
+	movq	96(%rsi), %r15
+	movq	104(%rsi), %rdi
+	movq	112(%rsi), %rbp
+	movq	128(%rsi), %rbx
+	movq	136(%rsi), %rdx
+	movq	144(%rsi), %rax
+	movq	152(%rsi), %rcx
+	movq	160(%rsi), %rsp
+
+	/* set the jump target by pushing it to the stack.
+	   ret will pop the new %rip from the stack, causing us to jump there. */
+	pushq	168(%rsi)
+
+	/* finally, set %rsi correctly since we do not need it anymore. */
+	movq	120(%rsi), %rsi
+
+	/* we're all done here, return 0 */
+	xorl	%eax, %eax
+	ret
+
+
+.weak swapcontext;
+swapcontext = __swapcontext;