aarch64: Add GCS support for setcontext

Userspace ucontext needs to store GCSPR, it does not have to be
compatible with the kernel ucontext. For now we use the linux
struct gcs_context layout but only use the gcspr field from it.

Similar implementation to the longjmp code, supports switching GCS
if the target GCS is capped, and unwinding a continous GCS to a
previous state.

4 files changed, 83 insertions, 9 deletions

diff --git a/sysdeps/unix/sysv/linux/aarch64/getcontext.S b/sysdeps/unix/sysv/linux/aarch64/getcontext.S
index e5b69c9..30e2b39 100644
--- a/sysdeps/unix/sysv/linux/aarch64/getcontext.S
+++ b/sysdeps/unix/sysv/linux/aarch64/getcontext.S
@@ -83,9 +83,24 @@ ENTRY(__getcontext)
 	mrs	x4, fpcr
 	str	w4, [x3, oFPCR - oFPSR]
 
-	/* Write the termination context extension header.  */
 	add	x2, x2, #FPSIMD_CONTEXT_SIZE
 
+	/* Save the GCSPR.  */
+	mov	x16, 1
+	CHKFEAT_X16
+	tbnz	x16, 0, L(gcs_done)
+	mov	w3, #(GCS_MAGIC & 0xffff)
+	movk	w3, #(GCS_MAGIC >> 16), lsl #16
+	str	w3, [x2, #oHEAD + oMAGIC]
+	mov	w3, #GCS_CONTEXT_SIZE
+	str	w3, [x2, #oHEAD + oSIZE]
+	MRS_GCSPR (x4)
+	add	x4, x4, 8 /* GCS state right after getcontext returns.  */
+	str	x4, [x2, #oGCSPR]
+	add	x2, x2, #GCS_CONTEXT_SIZE
+L(gcs_done):
+
+	/* Write the termination context extension header.  */
 	str	wzr, [x2, #oHEAD + oMAGIC]
 	str	wzr, [x2, #oHEAD + oSIZE]
 
diff --git a/sysdeps/unix/sysv/linux/aarch64/setcontext.S b/sysdeps/unix/sysv/linux/aarch64/setcontext.S
index ba65943..bdfd458 100644
--- a/sysdeps/unix/sysv/linux/aarch64/setcontext.S
+++ b/sysdeps/unix/sysv/linux/aarch64/setcontext.S
@@ -130,6 +130,44 @@ ENTRY (__setcontext)
 	ldr	w4, [x3, oFPCR - oFPSR]
 	msr	fpcr, x4
 
+	/* Restore the GCS.  */
+	mov	x16, 1
+	CHKFEAT_X16
+	tbnz	x16, 0, L(gcs_done)
+	/* Get target GCS from GCS context.  */
+	ldr	w1, [x2, #oHEAD + oSIZE]
+	add	x2, x2, x1
+	mov	w3, #(GCS_MAGIC & 0xffff)
+	movk	w3, #(GCS_MAGIC >> 16), lsl #16
+	ldr	w1, [x2, #oHEAD + oMAGIC]
+	cmp	w1, w3
+	b.ne	L(gcs_done)
+	ldr	x3, [x2, #oGCSPR]
+	MRS_GCSPR (x2)
+	mov	x4, x3
+	/* x2: GCSPR now.  x3, x4: target GCSPR.  x5, x6: tmp regs.  */
+L(gcs_scan):
+	cmp	x2, x4
+	b.eq	L(gcs_pop)
+	sub	x4, x4, 8
+	/* Check for a cap token.  */
+	ldr	x5, [x4]
+	and	x6, x4, 0xfffffffffffff000
+	orr	x6, x6, 1
+	cmp	x5, x6
+	b.ne	L(gcs_scan)
+L(gcs_switch):
+	add	x2, x4, 8
+	GCSSS1 (x4)
+	GCSSS2 (xzr)
+L(gcs_pop):
+	cmp	x2, x3
+	b.eq	L(gcs_done)
+	GCSPOPM (xzr)
+	add	x2, x2, 8
+	b	L(gcs_pop)
+L(gcs_done):
+
 2:
 	ldr     x16, [x0, oPC]
 	/* Restore arg registers.  */
diff --git a/sysdeps/unix/sysv/linux/aarch64/swapcontext.S b/sysdeps/unix/sysv/linux/aarch64/swapcontext.S
index f049140..45b1277 100644
--- a/sysdeps/unix/sysv/linux/aarch64/swapcontext.S
+++ b/sysdeps/unix/sysv/linux/aarch64/swapcontext.S
@@ -32,8 +32,15 @@ ENTRY(__swapcontext)
 	   And set up x1 to become the return address of the caller, so we
 	   can return there with a normal RET instead of an indirect jump.  */
 	stp	xzr, x30, [x0, oX0 +  0 * SZREG]
+
+	/* With GCS, swapcontext calls are followed by BTI J, otherwise
+	   we have to be compatible with old BTI enabled binaries.  */
+	mov	x16, 1
+	CHKFEAT_X16
+	tbz	x16, 0, L(skip_x30_redirect)
 	/* Arrange the oucp context to return to 2f.  */
 	adr	x30, 2f
+L(skip_x30_redirect):
 
 	stp	x18, x19, [x0, oX0 + 18 * SZREG]
 	stp	x20, x21, [x0, oX0 + 20 * SZREG]
@@ -72,14 +79,27 @@ ENTRY(__swapcontext)
 	mrs	x4, fpcr
 	str	w4, [x3, #oFPCR - oFPSR]
 
-	/* Write the termination context extension header.  */
 	add	x2, x2, #FPSIMD_CONTEXT_SIZE
 
+	/* Save the GCSPR.  */
+	tbnz	x16, 0, L(gcs_done)
+	mov	w3, #(GCS_MAGIC & 0xffff)
+	movk	w3, #(GCS_MAGIC >> 16), lsl #16
+	str	w3, [x2, #oHEAD + oMAGIC]
+	mov	w3, #GCS_CONTEXT_SIZE
+	str	w3, [x2, #oHEAD + oSIZE]
+	MRS_GCSPR (x4)
+	add	x4, x4, 8 /* GCSPR of the caller.  */
+	str	x4, [x2, #oGCSPR]
+	add	x2, x2, #GCS_CONTEXT_SIZE
+L(gcs_done):
+
+	/* Write the termination context extension header.  */
 	str	wzr, [x2, #oHEAD + oMAGIC]
 	str	wzr, [x2, #oHEAD + oSIZE]
 
 	/* Preserve ucp.  */
-	mov	x21, x1
+	mov	x9, x1
 
 	/* rt_sigprocmask (SIG_SETMASK, &ucp->uc_sigmask, &oucp->uc_sigmask,
 			   _NSIG8) */
@@ -93,12 +113,8 @@ ENTRY(__swapcontext)
 	svc	0
 	cbnz	x0, 1f
 
-	mov     x22, x30
-	mov	x0, x21
-	bl	JUMPTARGET (__setcontext)
-	mov     x30, x22
-	RET
-
+	mov	x0, x9
+	b	JUMPTARGET (__setcontext)
 1:
 	b	C_SYMBOL_NAME(__syscall_error)
 2:
diff --git a/sysdeps/unix/sysv/linux/aarch64/ucontext-internal.h b/sysdeps/unix/sysv/linux/aarch64/ucontext-internal.h
index 096d5fb..84f5365 100644
--- a/sysdeps/unix/sysv/linux/aarch64/ucontext-internal.h
+++ b/sysdeps/unix/sysv/linux/aarch64/ucontext-internal.h
@@ -43,3 +43,8 @@
 #define oX21 (oX0 + 21*8)
 #define oFP  (oX0 + 29*8)
 #define oLR  (oX0 + 30*8)
+
+/* Use kernel layout for saving GCSPR in ucontext.  */
+#define GCS_MAGIC 0x47435300
+#define GCS_CONTEXT_SIZE 32
+#define oGCSPR 8