V3 [PATCH 06/12] x86-64/CET: Extend ucontext_t to save shadow stack

["H.J. Lu" <hjl dot tools at gmail dot com>]

On Tue, Jul 24, 2018 at 09:47:50PM -0400, Carlos O'Donell wrote:
> On 07/24/2018 06:38 PM, H.J. Lu wrote:
> > On Tue, Jul 24, 2018 at 1:48 PM, Carlos O'Donell <carlos@redhat.com> wrote:
> >> On 07/21/2018 10:20 AM, H.J. Lu wrote:
> >>> This patch adds a field to ucontext_t to save shadow stack:
> >>>
> >>> 1. getcontext and swapcontext are updated to save the caller's shadow
> >>> stack pointer and return addresses.
> >>> 2. setcontext and swapcontext are updated to restore shadow stack and
> >>> jump to new context directly.
> >>> 3. makecontext is updated to allocate a new shadow stack and set the
> >>> caller's return address to __start_context.
> >>>
> >>> Since makecontext allocates a new shadow stack when making a new
> >>> context and kernel allocates a new shadow stack for clone/fork/vfork
> >>> syscalls, we keep track the lowest shadow stack base.  In setcontext
> >>> and swapcontext, when searching for shadow stack restore token, if the
> >>> lowest shadow stack base is reached, we assume both the current and
> >>> target shadow stack pointers are on the same shadow stack.
> >>>
> >>> We enable shadow stack at run-time only if program and all used shared
> >>> objects, including dlopened ones, are shadow stack enabled, which means
> >>> that they must be compiled with GCC 8 or above and glibc 2.28 or above.
> >>> We need to save and restore shadow stack only if shadow stack is enabled.
> >>> When caller of getcontext, setcontext, swapcontext and makecontext is
> >>> compiled with smaller ucontext_t, shadow stack won't be enabled at
> >>> run-time.  We check if shadow stack is enabled before accessing the
> >>> extended field in ucontext_t.
> >>>
> >> Right, this is a flag day ABI change.
> >>
> >>> 2018-05-21  Vedvyas Shanbhogue  <vedvyas.shanbhogue@intel.com>
> >>>           H.J. Lu  <hongjiu.lu@intel.com>
> >>>
> >>>       * sysdeps/unix/sysv/linux/x86/sys/ucontext.h (ucontext_t): Add
> >>>       __ssp.
> >>>       * sysdeps/unix/sysv/linux/x86_64/__start_context.S: Include
> >>>       <bits/prctl.h> and "ucontext_i.h" when shadow stack is enabled.
> >>>       (__push___start_context): New.
> >>>       * sysdeps/unix/sysv/linux/x86_64/getcontext.S (__getcontext):
> >>>       Save the caller's shadow stack pointer when shadow stack is in
> >>>       use.
> >>>       * sysdeps/unix/sysv/linux/x86_64/makecontext.c Include
> >>>       <pthread.h>, <libc-pointer-arith.h> and <sys/prctl.h>.
> >>>       (__push___start_context): New prototype.
> >>>       (__makecontext): Call __push___start_context to allocate a new
> >>>       shadow stack, push __start_context onto the new stack as well
> >>>       as the new shadow stack.  Set the lowest shadow stack base.
> >>>       * sysdeps/unix/sysv/linux/x86_64/setcontext.S: Include
> >>>       <bits/prctl.h>.
> >>>       (__setcontext): Use the restore token to restore shadow stack
> >>>       if available.  Otherwise unwind shadow stack.  Check if the
> >>>       target shadow stack pointer came from __push___start_context.
> >>>       Don't search for shadow stack restore token below the lowest
> >>>       shadow stack base.
> >>>       * sysdeps/unix/sysv/linux/x86_64/swapcontext.S: Include
> >>>       <bits/prctl.h>.
> >>>       (__swapcontext): Save the current shadow stack pointer.
> >>>       Use the restore token to restore shadow stack if available.
> >>>       Otherwise unwind shadow stack.  Check if the target shadow
> >>>       stack pointer came from __push___start_context.  Don't search
> >>>       for shadow stack restore token below the lowest shadow stack
> >>>       base.
> >>>       * sysdeps/unix/sysv/linux/x86_64/sysdep.h
> >>>       (STACK_SIZE_TO_SHADOW_STACK_SIZE_SHIFT): New.
> >>>       * sysdeps/unix/sysv/linux/x86_64/ucontext_i.sym (oSSP): New.
> >>> ---
> >> OK for 2.28, but should we increase the size we allocate in the
> >> new structure?
> >>
> >> The hardest part to review was the CET token save/restore state
> >> transitions between context changes and for example I still don't
> >> clearly understand why we need to check if the restored state is
> >> on the same shadow stack and unwind? Is this because we are trying
> >> to unwind the shadow stack as often as possible to avoid overflow
> >> (as we do in the unwind_shadow_stack/loop sequence). Why do we do
> >> this unwind process?
> >>
> >> Reviewed-by: Carlos O'Donell <carlos@redhat.com>
> >>
> > Here is a revamp of x86-64 ucontext functions.  It should be
> > much easier to read now.  The key things are:
> > 
> > 1. We keep track the base and limit of the current shadow stack
> > in tcbhead_t.
> > 2.  In setcontext and swapcontext, if the target shadow stack
> > pointer is above the current shadow stack pointer and below the upper
> > limit of the current shadow stack, we unwind the shadow stack.
> > Otherwise it is a stack switch and we look for a restore token to
> > restore the target shadow stack.
> > 
> > OK for master branch?
> 
> OK for 2.28.
> 
> The algorithm is basically the same but with a length, so this still
> seems good to me.

Furthe similify the algorithm.  There is no need to check length.  If
the target shadow stack base is the same as the current shadow stack
base, we unwind the shadow stack.  Otherwise it is a stack switch and
we look for a restore token to restore the target shadow stack.

> 
> Please simplify the Changelog, it is too verbose.
> 
> Reviewed-by: Carlos O'Donell <carlos@redhat.com>

Here is the updated patch with shortened ChangeLog entries.

OK for master?

Thanks.

H.J.
---
This patch adds a field to ucontext_t to save shadow stack:

1. getcontext and swapcontext are updated to save the caller's shadow
stack pointer and return addresses.
2. setcontext and swapcontext are updated to restore shadow stack and
jump to new context directly.
3. makecontext is updated to allocate a new shadow stack and set the
caller's return address to __start_context.

Since makecontext allocates a new shadow stack when making a new
context and kernel allocates a new shadow stack for clone/fork/vfork
syscalls, we track the current shadow stack base.  In setcontext and
swapcontext, if the target shadow stack base is the same as the current
shadow stack base, we unwind the shadow stack.  Otherwise it is a stack
switch and we look for a restore token.

We enable shadow stack at run-time only if program and all used shared
objects, including dlopened ones, are shadow stack enabled, which means
that they must be compiled with GCC 8 or above and glibc 2.28 or above.
We need to save and restore shadow stack only if shadow stack is enabled.
When caller of getcontext, setcontext, swapcontext and makecontext is
compiled with smaller ucontext_t, shadow stack won't be enabled at
run-time.  We check if shadow stack is enabled before accessing the
extended field in ucontext_t.

2018-05-21  Vedvyas Shanbhogue  <vedvyas.shanbhogue@intel.com>
	    H.J. Lu  <hongjiu.lu@intel.com>

	* sysdeps/unix/sysv/linux/x86/sys/ucontext.h (ucontext_t): Add
	__ssp.
	* sysdeps/unix/sysv/linux/x86_64/__start_context.S: Include
	<asm/prctl.h> and "ucontext_i.h" when shadow stack is enabled.
	(__push___start_context): New.
	* sysdeps/unix/sysv/linux/x86_64/getcontext.S: Include
	<asm/prctl.h>.
	(__getcontext): Record the current shadow stack base.  Save the
	caller's shadow stack pointer and base.
	* sysdeps/unix/sysv/linux/x86_64/makecontext.c: Include
	<pthread.h>, <libc-pointer-arith.h> and <sys/prctl.h>.
	(__push___start_context): New prototype.
	(__makecontext): Call __push___start_context to allocate a new
	shadow stack, push __start_context onto the new stack as well
	as the new shadow stack.
	* sysdeps/unix/sysv/linux/x86_64/setcontext.S: Include
	<asm/prctl.h>.
	(__setcontext): Restore the target shadow stack.
	* sysdeps/unix/sysv/linux/x86_64/swapcontext.S: Include
	<asm/prctl.h>.
	(__swapcontext): Record the current shadow stack base.  Save
	the caller's shadow stack pointer and base.  Restore the target
	shadow stack.
	* sysdeps/unix/sysv/linux/x86_64/sysdep.h
	(STACK_SIZE_TO_SHADOW_STACK_SIZE_SHIFT): New.
	* sysdeps/unix/sysv/linux/x86_64/ucontext_i.sym (oSSP): New.
---
 sysdeps/unix/sysv/linux/x86/sys/ucontext.h    |   2 +
 .../unix/sysv/linux/x86_64/__start_context.S  |  75 ++++++++++
 sysdeps/unix/sysv/linux/x86_64/getcontext.S   |  50 +++++++
 sysdeps/unix/sysv/linux/x86_64/makecontext.c  |  38 +++++-
 sysdeps/unix/sysv/linux/x86_64/setcontext.S   |  92 +++++++++++++
 sysdeps/unix/sysv/linux/x86_64/swapcontext.S  | 129 ++++++++++++++++++
 sysdeps/unix/sysv/linux/x86_64/sysdep.h       |   5 +
 sysdeps/unix/sysv/linux/x86_64/ucontext_i.sym |   1 +
 8 files changed, 391 insertions(+), 1 deletion(-)

diff --git a/sysdeps/unix/sysv/linux/x86/sys/ucontext.h b/sysdeps/unix/sysv/linux/x86/sys/ucontext.h
index afb7c181bf..7367726a50 100644
--- a/sysdeps/unix/sysv/linux/x86/sys/ucontext.h
+++ b/sysdeps/unix/sysv/linux/x86/sys/ucontext.h
@@ -147,6 +147,7 @@ typedef struct ucontext_t
     mcontext_t uc_mcontext;
     sigset_t uc_sigmask;
     struct _libc_fpstate __fpregs_mem;
+    __extension__ unsigned long long int __ssp[4];
   } ucontext_t;
 
 #else /* !__x86_64__ */
@@ -251,6 +252,7 @@ typedef struct ucontext_t
     mcontext_t uc_mcontext;
     sigset_t uc_sigmask;
     struct _libc_fpstate __fpregs_mem;
+    unsigned long int __ssp[4];
   } ucontext_t;
 
 #endif /* !__x86_64__ */
diff --git a/sysdeps/unix/sysv/linux/x86_64/__start_context.S b/sysdeps/unix/sysv/linux/x86_64/__start_context.S
index 0bfde5fc31..87de0e5996 100644
--- a/sysdeps/unix/sysv/linux/x86_64/__start_context.S
+++ b/sysdeps/unix/sysv/linux/x86_64/__start_context.S
@@ -18,6 +18,80 @@
 
 #include <sysdep.h>
 
+#if SHSTK_ENABLED
+# include <asm/prctl.h>
+# include "ucontext_i.h"
+
+/* Use CALL to push __start_context onto the new stack as well as the new
+   shadow stack.  RDI points to ucontext:
+   Incoming:
+     __ssp[0]: The original caller's shadow stack pointer.
+     __ssp[1]: The size of the new shadow stack.
+     __ssp[2]: The size of the new shadow stack.
+   Outgoing:
+     __ssp[0]: The new shadow stack pointer.
+     __ssp[1]: The base address of the new shadow stack.
+     __ssp[2]: The size of the new shadow stack.
+ */
+
+ENTRY(__push___start_context)
+	/* Save the pointer to ucontext.  */
+	movq	%rdi, %r9
+	/* Get the original shadow stack pointer.  */
+	rdsspq	%r8
+	/* Save the original stack pointer.  */
+	movq	%rsp, %rdx
+	/* Load the top of the new stack into RSI.  */
+	movq 	oRSP(%rdi), %rsi
+	/* Add 8 bytes to RSI since CALL will push the 8-byte return
+	   address onto stack.  */
+	leaq	8(%rsi), %rsp
+	/* Allocate the new shadow stack.  The size of the new shadow
+	   stack is passed in __ssp[1].  */
+	lea	(oSSP + 8)(%rdi), %RSI_LP
+	movl	$ARCH_CET_ALLOC_SHSTK, %edi
+	movl	$__NR_arch_prctl, %eax
+	/* The new shadow stack base is returned in __ssp[1].  */
+	syscall
+	testq	%rax, %rax
+	jne	L(hlt)		/* This should never happen.  */
+
+	/* Get the size of the new shadow stack.  */
+	movq	8(%rsi), %rdi
+
+	/* Get the base address of the new shadow stack.  */
+	movq	(%rsi), %rsi
+
+	/* Use the restore stoken to restore the new shadow stack.  */
+	rstorssp -8(%rsi, %rdi)
+
+	/* Save the restore token on the original shadow stack.  */
+	saveprevssp
+
+	/* Push the address of "jmp __start_context" onto the new stack
+	   as well as the new shadow stack.  */
+	call	1f
+	jmp	__start_context
+1:
+
+	/* Get the new shadow stack pointer.  */
+	rdsspq	%rdi
+
+	/* Use the restore stoken to restore the original shadow stack.  */
+	rstorssp -8(%r8)
+
+	/* Save the restore token on the new shadow stack.  */
+	saveprevssp
+
+	/* Store the new shadow stack pointer in __ssp[0].  */
+	movq	%rdi, oSSP(%r9)
+
+	/* Restore the original stack.  */
+	mov	%rdx, %rsp
+	ret
+END(__push___start_context)
+#endif
+
 /* This is the helper code which gets called if a function which is
    registered with 'makecontext' returns.  In this case we have to
    install the context listed in the uc_link element of the context
@@ -45,5 +119,6 @@ ENTRY(__start_context)
 	call	HIDDEN_JUMPTARGET(exit)
 	/* The 'exit' call should never return.  In case it does cause
 	   the process to terminate.  */
+L(hlt):
 	hlt
 END(__start_context)
diff --git a/sysdeps/unix/sysv/linux/x86_64/getcontext.S b/sysdeps/unix/sysv/linux/x86_64/getcontext.S
index 33347bc02e..84b986ca98 100644
--- a/sysdeps/unix/sysv/linux/x86_64/getcontext.S
+++ b/sysdeps/unix/sysv/linux/x86_64/getcontext.S
@@ -18,6 +18,7 @@
    <http://www.gnu.org/licenses/>.  */
 
 #include <sysdep.h>
+#include <asm/prctl.h>
 
 #include "ucontext_i.h"
 
@@ -53,6 +54,55 @@ ENTRY(__getcontext)
 	leaq	8(%rsp), %rcx		/* Exclude the return address.  */
 	movq	%rcx, oRSP(%rdi)
 
+#if SHSTK_ENABLED
+	/* Check if shadow stack is enabled.  */
+	testl	$X86_FEATURE_1_SHSTK, %fs:FEATURE_1_OFFSET
+	jz	L(no_shstk)
+
+	/* Save RDI in RDX which won't be clobbered by syscall.  */
+	movq	%rdi, %rdx
+
+	xorl	%eax, %eax
+	cmpq	%fs:SSP_BASE_OFFSET, %rax
+	jnz	L(shadow_stack_bound_recorded)
+
+	/* Get the base address and size of the default shadow stack
+	   which must be the current shadow stack since nothing has
+	   been recorded yet.  */
+	sub	$24, %RSP_LP
+	mov	%RSP_LP, %RSI_LP
+	movl	$ARCH_CET_STATUS, %edi
+	movl	$__NR_arch_prctl, %eax
+	syscall
+	testq	%rax, %rax
+	jz	L(continue_no_err)
+
+	/* This should never happen.  */
+	hlt
+
+L(continue_no_err):
+	/* Record the base of the current shadow stack.  */
+	movq	8(%rsp), %rax
+	movq	%rax, %fs:SSP_BASE_OFFSET
+	add	$24, %RSP_LP
+
+	/* Restore RDI.  */
+	movq	%rdx, %rdi
+
+L(shadow_stack_bound_recorded):
+	/* Get the current shadow stack pointer.  */
+	rdsspq	%rax
+	/* NB: Save the caller's shadow stack so that we can jump back
+	   to the caller directly.  */
+	addq	$8, %rax
+	movq	%rax, oSSP(%rdx)
+
+	/* Save the current shadow stack base in ucontext.  */
+	movq	%fs:SSP_BASE_OFFSET, %rax
+	movq	%rax, (oSSP + 8)(%rdi)
+
+L(no_shstk):
+#endif
 	/* We have separate floating-point register content memory on the
 	   stack.  We use the __fpregs_mem block in the context.  Set the
 	   links up correctly.  */
diff --git a/sysdeps/unix/sysv/linux/x86_64/makecontext.c b/sysdeps/unix/sysv/linux/x86_64/makecontext.c
index 0d0802bf43..3eb4c59a5d 100644
--- a/sysdeps/unix/sysv/linux/x86_64/makecontext.c
+++ b/sysdeps/unix/sysv/linux/x86_64/makecontext.c
@@ -21,6 +21,11 @@
 #include <stdarg.h>
 #include <stdint.h>
 #include <ucontext.h>
+#if SHSTK_ENABLED
+# include <pthread.h>
+# include <libc-pointer-arith.h>
+# include <sys/prctl.h>
+#endif
 
 #include "ucontext_i.h"
 
@@ -52,6 +57,8 @@ void
 __makecontext (ucontext_t *ucp, void (*func) (void), int argc, ...)
 {
   extern void __start_context (void) attribute_hidden;
+  extern void __push___start_context (ucontext_t *)
+    attribute_hidden;
   greg_t *sp;
   unsigned int idx_uc_link;
   va_list ap;
@@ -74,7 +81,36 @@ __makecontext (ucontext_t *ucp, void (*func) (void), int argc, ...)
   ucp->uc_mcontext.gregs[REG_RSP] = (uintptr_t) sp;
 
   /* Setup stack.  */
-  sp[0] = (uintptr_t) &__start_context;
+#if SHSTK_ENABLED
+  struct pthread *self = THREAD_SELF;
+  unsigned int feature_1 = THREAD_GETMEM (self, header.feature_1);
+  /* NB: We must check feature_1 before accessing __ssp since caller
+	 may be compiled against ucontext_t without __ssp.  */
+  if ((feature_1 & X86_FEATURE_1_SHSTK) != 0)
+    {
+      /* Shadow stack is enabled.  We need to allocate a new shadow
+         stack.  */
+      unsigned long ssp_size = (((uintptr_t) sp
+				 - (uintptr_t) ucp->uc_stack.ss_sp)
+				>> STACK_SIZE_TO_SHADOW_STACK_SIZE_SHIFT);
+      /* Align shadow stack to 8 bytes.  */
+      ssp_size = ALIGN_UP (ssp_size, 8);
+
+      ucp->__ssp[1] = ssp_size;
+      ucp->__ssp[2] = ssp_size;
+
+      /* Call __push___start_context to allocate a new shadow stack,
+	 push __start_context onto the new stack as well as the new
+	 shadow stack.  NB: After __push___start_context returns,
+	   ucp->__ssp[0]: The new shadow stack pointer.
+	   ucp->__ssp[1]: The base address of the new shadow stack.
+	   ucp->__ssp[2]: The size of the new shadow stack.
+       */
+      __push___start_context (ucp);
+    }
+  else
+#endif
+    sp[0] = (uintptr_t) &__start_context;
   sp[idx_uc_link] = (uintptr_t) ucp->uc_link;
 
   va_start (ap, argc);
diff --git a/sysdeps/unix/sysv/linux/x86_64/setcontext.S b/sysdeps/unix/sysv/linux/x86_64/setcontext.S
index b42af8e291..0afdf8ce0a 100644
--- a/sysdeps/unix/sysv/linux/x86_64/setcontext.S
+++ b/sysdeps/unix/sysv/linux/x86_64/setcontext.S
@@ -18,6 +18,7 @@
    <http://www.gnu.org/licenses/>.  */
 
 #include <sysdep.h>
+#include <asm/prctl.h>
 
 #include "ucontext_i.h"
 
@@ -79,6 +80,97 @@ ENTRY(__setcontext)
 	movq	oR14(%rdx), %r14
 	movq	oR15(%rdx), %r15
 
+#if SHSTK_ENABLED
+	/* Check if shadow stack is enabled.  */
+	testl	$X86_FEATURE_1_SHSTK, %fs:FEATURE_1_OFFSET
+	jz	L(no_shstk)
+
+	/* If the base of the target shadow stack is the same as the
+	   base of the current shadow stack, we unwind the shadow
+	   stack.  Otherwise it is a stack switch and we look for a
+	   restore token.  */
+	movq	oSSP(%rdx), %rsi
+	movq	%rsi, %rdi
+
+	/* Get the base of the target shadow stack.  */
+	movq	(oSSP + 8)(%rdx), %rcx
+	cmpq	%fs:SSP_BASE_OFFSET, %rcx
+	je	L(unwind_shadow_stack)
+
+L(find_restore_token_loop):
+	/* Look for a restore token.  */
+	movq	-8(%rsi), %rax
+	andq	$-8, %rax
+	cmpq	%rsi, %rax
+	je	L(restore_shadow_stack)
+
+	/* Try the next slot.  */
+	subq	$8, %rsi
+	jmp	L(find_restore_token_loop)
+
+L(restore_shadow_stack):
+	/* Pop return address from the shadow stack since setcontext
+	   will not return.  */
+	movq	$1, %rax
+	incsspq	%rax
+
+	/* Use the restore stoken to restore the target shadow stack.  */
+	rstorssp -8(%rsi)
+
+	/* Save the restore token on the old shadow stack.  NB: This
+	   restore token may be checked by setcontext or swapcontext
+	   later.  */
+	saveprevssp
+
+	/* Record the new shadow stack base that was switched to.  */
+	movq	(oSSP + 8)(%rdx), %rax
+	movq	%rax, %fs:SSP_BASE_OFFSET
+
+L(unwind_shadow_stack):
+	rdsspq	%rcx
+	subq	%rdi, %rcx
+	je	L(skip_unwind_shadow_stack)
+	negq	%rcx
+	shrq	$3, %rcx
+	movl	$255, %esi
+L(loop):
+	cmpq	%rsi, %rcx
+	cmovb	%rcx, %rsi
+	incsspq	%rsi
+	subq	%rsi, %rcx
+	ja	L(loop)
+
+L(skip_unwind_shadow_stack):
+	movq	oRSI(%rdx), %rsi
+	movq	oRDI(%rdx), %rdi
+	movq	oRCX(%rdx), %rcx
+	movq	oR8(%rdx), %r8
+	movq	oR9(%rdx), %r9
+
+	/* Get the return address set with getcontext.  */
+	movq	oRIP(%rdx), %r10
+
+	/* Setup finally %rdx.  */
+	movq	oRDX(%rdx), %rdx
+
+	/* Check if return address is valid for the case when setcontext
+	   is invoked from __start_context with linked context.  */
+	rdsspq	%rax
+	cmpq	(%rax), %r10
+	/* Clear RAX to indicate success.  NB: Don't use xorl to keep
+	   EFLAGS for jne.  */
+	movl	$0, %eax
+	jne	L(jmp)
+	/* Return to the new context if return address valid.  */
+	pushq	%r10
+	ret
+
+L(jmp):
+	/* Jump to the new context directly.  */
+	jmp	*%r10
+
+L(no_shstk):
+#endif
 	/* The following ret should return to the address set with
 	getcontext.  Therefore push the address on the stack.  */
 	movq	oRIP(%rdx), %rcx
diff --git a/sysdeps/unix/sysv/linux/x86_64/swapcontext.S b/sysdeps/unix/sysv/linux/x86_64/swapcontext.S
index 1110c479fa..92d3371807 100644
--- a/sysdeps/unix/sysv/linux/x86_64/swapcontext.S
+++ b/sysdeps/unix/sysv/linux/x86_64/swapcontext.S
@@ -18,6 +18,7 @@
    <http://www.gnu.org/licenses/>.  */
 
 #include <sysdep.h>
+#include <asm/prctl.h>
 
 #include "ucontext_i.h"
 
@@ -67,6 +68,7 @@ ENTRY(__swapcontext)
 
 	/* The syscall destroys some registers, save them.  */
 	movq	%rsi, %r12
+	movq	%rdi, %r9
 
 	/* Save the current signal mask and install the new one with
 	   rt_sigprocmask (SIG_BLOCK, newset, oldset,_NSIG/8).  */
@@ -99,6 +101,133 @@ ENTRY(__swapcontext)
 	movq	oR14(%rdx), %r14
 	movq	oR15(%rdx), %r15
 
+#if SHSTK_ENABLED
+	/* Check if shadow stack is enabled.  */
+	testl	$X86_FEATURE_1_SHSTK, %fs:FEATURE_1_OFFSET
+	jz	L(no_shstk)
+
+	xorl	%eax, %eax
+	cmpq	%fs:SSP_BASE_OFFSET, %rax
+	jnz	L(shadow_stack_bound_recorded)
+
+	/* Get the base address and size of the default shadow stack
+	   which must be the current shadow stack since nothing has
+	   been recorded yet.  */
+	sub	$24, %RSP_LP
+	mov	%RSP_LP, %RSI_LP
+	movl	$ARCH_CET_STATUS, %edi
+	movl	$__NR_arch_prctl, %eax
+	syscall
+	testq	%rax, %rax
+	jz	L(continue_no_err)
+
+	/* This should never happen.  */
+	hlt
+
+L(continue_no_err):
+	/* Record the base of the current shadow stack.  */
+	movq	8(%rsp), %rax
+	movq	%rax, %fs:SSP_BASE_OFFSET
+	add	$24, %RSP_LP
+
+L(shadow_stack_bound_recorded):
+        /* If we unwind the stack, we can't undo stack unwinding.  Just
+	   save the target shadow stack pointer as the current shadow
+	   stack pointer.   */
+	movq	oSSP(%rdx), %rcx
+	movq	%rcx, oSSP(%r9)
+
+	/* Save the base of the current shadow stack.  */
+	movq	%fs:SSP_BASE_OFFSET, %rax
+	movq	%rax, (oSSP + 8)(%r9)
+
+	/* If the base of the target shadow stack is the same as the
+	   base of the current shadow stack, we unwind the shadow
+	   stack.  Otherwise it is a stack switch and we look for a
+	   restore token.  */
+	movq	oSSP(%rdx), %rsi
+	movq	%rsi, %rdi
+
+	/* Get the base of the target shadow stack.  */
+	movq	(oSSP + 8)(%rdx), %rcx
+	cmpq	%fs:SSP_BASE_OFFSET, %rcx
+	je	L(unwind_shadow_stack)
+
+L(find_restore_token_loop):
+	/* Look for a restore token.  */
+	movq	-8(%rsi), %rax
+	andq	$-8, %rax
+	cmpq	%rsi, %rax
+	je	L(restore_shadow_stack)
+
+	/* Try the next slot.  */
+	subq	$8, %rsi
+	jmp	L(find_restore_token_loop)
+
+L(restore_shadow_stack):
+        /* The target shadow stack will be restored.  Save the current
+	   shadow stack pointer.  */
+	rdsspq	%rcx
+	movq	%rcx, oSSP(%r9)
+
+	/* Restore the target shadow stack.  */
+	rstorssp -8(%rsi)
+
+	/* Save the restore token on the old shadow stack.  NB: This
+	   restore token may be checked by setcontext or swapcontext
+	   later.  */
+	saveprevssp
+
+	/* Record the new shadow stack base that was switched to.   */
+	movq	(oSSP + 8)(%rdx), %rax
+	movq	%rax, %fs:SSP_BASE_OFFSET
+
+L(unwind_shadow_stack):
+	rdsspq	%rcx
+	subq	%rdi, %rcx
+	je	L(skip_unwind_shadow_stack)
+	negq	%rcx
+	shrq	$3, %rcx
+	movl	$255, %esi
+L(loop):
+	cmpq	%rsi, %rcx
+	cmovb	%rcx, %rsi
+	incsspq	%rsi
+	subq	%rsi, %rcx
+	ja	L(loop)
+
+L(skip_unwind_shadow_stack):
+	/* Setup registers used for passing args.  */
+	movq	oRDI(%rdx), %rdi
+	movq	oRSI(%rdx), %rsi
+	movq	oRCX(%rdx), %rcx
+	movq	oR8(%rdx), %r8
+	movq	oR9(%rdx), %r9
+
+	/* Get the return address set with getcontext.  */
+	movq	oRIP(%rdx), %r10
+
+	/* Setup finally %rdx.  */
+	movq	oRDX(%rdx), %rdx
+
+	/* Check if return address is valid for the case when setcontext
+	   is invoked from __start_context with linked context.  */
+	rdsspq	%rax
+	cmpq	(%rax), %r10
+	/* Clear rax to indicate success.  NB: Don't use xorl to keep
+	   EFLAGS for jne.  */
+	movl	$0, %eax
+	jne	L(jmp)
+	/* Return to the new context if return address valid.  */
+	pushq	%r10
+	ret
+
+L(jmp):
+	/* Jump to the new context directly.  */
+	jmp	*%r10
+
+L(no_shstk):
+#endif
 	/* The following ret should return to the address set with
 	getcontext.  Therefore push the address on the stack.  */
 	movq	oRIP(%rdx), %rcx
diff --git a/sysdeps/unix/sysv/linux/x86_64/sysdep.h b/sysdeps/unix/sysv/linux/x86_64/sysdep.h
index 1ef0f742ae..f07eb04962 100644
--- a/sysdeps/unix/sysv/linux/x86_64/sysdep.h
+++ b/sysdeps/unix/sysv/linux/x86_64/sysdep.h
@@ -423,4 +423,9 @@
 #undef LO_HI_LONG
 #define LO_HI_LONG(val) (val), 0
 
+/* Each shadow stack slot takes 8 bytes.  Assuming that each stack
+   frame takes 256 bytes, this is used to compute shadow stack size
+   from stack size.  */
+#define STACK_SIZE_TO_SHADOW_STACK_SIZE_SHIFT 5
+
 #endif /* linux/x86_64/sysdep.h */
diff --git a/sysdeps/unix/sysv/linux/x86_64/ucontext_i.sym b/sysdeps/unix/sysv/linux/x86_64/ucontext_i.sym
index af3e0e544b..c08b3b8b47 100644
--- a/sysdeps/unix/sysv/linux/x86_64/ucontext_i.sym
+++ b/sysdeps/unix/sysv/linux/x86_64/ucontext_i.sym
@@ -35,3 +35,4 @@ oFPREGS		mcontext (fpregs)
 oSIGMASK	ucontext (uc_sigmask)
 oFPREGSMEM	ucontext (__fpregs_mem)
 oMXCSR		ucontext (__fpregs_mem.mxcsr)
+oSSP		ucontext (__ssp)
-- 
2.17.1