2 files changed, 63 insertions, 2 deletions

diff --git a/sysdeps/unix/sysv/linux/aarch64/makecontext.c b/sysdeps/unix/sysv/linux/aarch64/makecontext.c
index 5aec182013..9e66b6761c 100644
--- a/sysdeps/unix/sysv/linux/aarch64/makecontext.c
+++ b/sysdeps/unix/sysv/linux/aarch64/makecontext.c
@@ -22,6 +22,52 @@
 #include <stdint.h>
 #include <ucontext.h>
 
+#define GCS_MAGIC 0x47435300
+
+static struct _aarch64_ctx *extension (void *p)
+{
+  return p;
+}
+
+#ifndef __NR_map_shadow_stack
+# define __NR_map_shadow_stack 453
+#endif
+#ifndef SHADOW_STACK_SET_TOKEN
+# define SHADOW_STACK_SET_TOKEN (1UL << 0)
+# define SHADOW_STACK_SET_MARKER (1UL << 1)
+#endif
+
+static void *
+map_shadow_stack (void *addr, size_t size, unsigned long flags)
+{
+  return (void *) INLINE_SYSCALL_CALL (map_shadow_stack, addr, size, flags);
+}
+
+#define GCS_MAX_SIZE (1UL << 31)
+#define GCS_ALTSTACK_RESERVE 160
+
+static void *
+alloc_makecontext_gcs (size_t stack_size)
+{
+  size_t size = (stack_size / 2 + GCS_ALTSTACK_RESERVE) & -8UL;
+  if (size > GCS_MAX_SIZE)
+    size = GCS_MAX_SIZE;
+
+  unsigned long flags = SHADOW_STACK_SET_MARKER | SHADOW_STACK_SET_TOKEN;
+  void *base = map_shadow_stack (NULL, size, flags);
+  if (base == (void *) -1)
+    /* ENOSYS, bad size or OOM.  */
+    abort ();
+  uint64_t *gcsp = (uint64_t *) ((char *) base + size);
+  /* Skip end of GCS token.  */
+  gcsp--;
+  /* Verify GCS cap token.  */
+  gcsp--;
+  if (((uint64_t)gcsp & 0xfffffffffffff000) + 1 != *gcsp)
+    abort ();
+  /* Return the target GCS pointer for context switch.  */
+  return gcsp + 1;
+}
 
 /* makecontext sets up a stack and the registers for the
    user context.  The stack looks like this:
@@ -56,10 +102,21 @@ __makecontext (ucontext_t *ucp, void (*func) (void), int argc, ...)
   sp = (uint64_t *) (((uintptr_t) sp) & -16L);
 
   ucp->uc_mcontext.regs[19] = (uintptr_t) ucp->uc_link;
+  ucp->uc_mcontext.regs[20] = (uintptr_t) func;
   ucp->uc_mcontext.sp = (uintptr_t) sp;
-  ucp->uc_mcontext.pc = (uintptr_t) func;
+  ucp->uc_mcontext.pc = (uintptr_t) __startcontext;
   ucp->uc_mcontext.regs[29] = (uintptr_t) 0;
-  ucp->uc_mcontext.regs[30] = (uintptr_t) &__startcontext;
+  ucp->uc_mcontext.regs[30] = (uintptr_t) 0;
+
+  void *p = ucp->uc_mcontext.__reserved;
+  if (extension (p)->magic == FPSIMD_MAGIC)
+    p = (char *)p + extension (p)->size;
+  if (extension (p)->magic == GCS_MAGIC)
+    {
+      /* Using the kernel struct gcs_context layout.  */
+      struct { uint64_t x, gcspr, y, z; } *q = p;
+      q->gcspr = (uint64_t) alloc_makecontext_gcs (ucp->uc_stack.ss_size);
+    }
 
   va_start (ap, argc);
   for (i = 0; i < argc; ++i)
diff --git a/sysdeps/unix/sysv/linux/aarch64/setcontext.S b/sysdeps/unix/sysv/linux/aarch64/setcontext.S
index c08e83ee60..6aa7236693 100644
--- a/sysdeps/unix/sysv/linux/aarch64/setcontext.S
+++ b/sysdeps/unix/sysv/linux/aarch64/setcontext.S
@@ -181,7 +181,11 @@ L(gcs_done):
 PSEUDO_END (__setcontext)
 weak_alias (__setcontext, setcontext)
 
+/* makecontext start function: receives uc_link in x19 and func in x20.
+   Arguments of func, x29, x30 and sp are set up by the caller.  */
 ENTRY (__startcontext)
+	cfi_undefined (x30)
+	blr	x20
 	mov	x0, x19
 	cbnz	x0, __setcontext
 1:	b       HIDDEN_JUMPTARGET (exit)