Index: sys/arm64/linux/linux_support.s
===================================================================
--- sys/arm64/linux/linux_support.s
+++ sys/arm64/linux/linux_support.s
@@ -2,6 +2,7 @@
  * SPDX-License-Identifier: BSD-2-Clause-FreeBSD
  *
  * Copyright (C) 2018 Turing Robotic Industries Inc.
+ * Copyright (C) 2022 Dmitry Chagin <dchagin@FreeBSD.org>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -27,36 +28,147 @@
  * $FreeBSD$
  */
 
-#include "linux_assym.h"
 #include <machine/asm.h>
+__FBSDID("$FreeBSD$");
+
+#include <machine/param.h>
+#include <machine/vmparam.h>
+
+#include <sys/errno.h>
 
 #include "assym.inc"
 
-/*
- * LINUXTODO: implement futex_*
- */
+.macro check_user_access user_arg, limit, bad_addr_func
+	ldr	x7, =(\limit)
+	cmp	x\user_arg, x7
+	b.cs	\bad_addr_func
+.endm
+
+futex_fault:
+	SET_FAULT_HANDLER(xzr, x1)
+	EXIT_USER_ACCESS_CHECK(w0, x1)
+futex_fault_nopcb:
+	mov	x0, #EFAULT
+	ret
+
+#define	LINUX_FUTEX_MAX_LOOPS	128
+/* int oparg, uint32_t *uaddr, int *oldval */
 
+/* (int *)uaddr2 = oparg */
 ENTRY(futex_xchgl)
-	brk #0
+	check_user_access 1, (VM_MAXUSER_ADDRESS-3), futex_fault_nopcb
+	adr	x6, futex_fault		/* Load the fault handler */
+	mov	w5, #1
+	SET_FAULT_HANDLER(x6, x4)	/* And set it */
+	ENTER_USER_ACCESS(w6, x4)
+	mov	w5, #LINUX_FUTEX_MAX_LOOPS
+	prfm	pstl1strm, [x1]
+	mov	w6, w0			/* Save oparg */
+1:	ldxr	w4, [x1]		/* Load oldval from uaddr */
+	stlxr	w0, w6, [x1]		/* Store newval to the uaddr */
+	cbz	w0, 3f			/* Exit on success */
+	sub	w5, w5, w0		/* Dec loop counter */
+	cbnz	w5, 1b			/* Loop */
+	mov	x0, #EAGAIN		/* Store of newval failed */
+3:	dmb	ish
+	EXIT_USER_ACCESS(w6)
+	SET_FAULT_HANDLER(xzr, x6)	/* Reset the fault handler */
+	str w4, [x2]			/* Store oldval */
 	ret
 END(futex_xchgl)
 
+/* (int *)uaddr2 += oparg */
 ENTRY(futex_addl)
-	brk #0
+	check_user_access 1, (VM_MAXUSER_ADDRESS-3), futex_fault_nopcb
+	adr	x6, futex_fault		/* Load the fault handler */
+	mov	w5, #1
+	SET_FAULT_HANDLER(x6, x4)	/* And set it */
+	ENTER_USER_ACCESS(w6, x4)
+	mov	w5, #LINUX_FUTEX_MAX_LOOPS
+	prfm	pstl1strm, [x1]
+	mov	w6, w0			/* Save oparg  */
+1:	ldxr	w4, [x1]		/* Load oldval from uaddr */
+	add	w3, w4, w6		/* oldval + oparg */
+	stlxr	w0, w3, [x1]		/* Store newval to the uaddr */
+	cbz	w0, 3f			/* Exit on success */
+	sub	w5, w5, w0		/* Dec loop counter */
+	cbnz	w5, 1b			/* Loop */
+	mov	x0, #EAGAIN		/* Store of newval failed */
+3:	dmb	ish
+	EXIT_USER_ACCESS(w6)
+	SET_FAULT_HANDLER(xzr, x6)	/* Reset the fault handler */
+	str w4, [x2]			/* Store oldval */
 	ret
 END(futex_addl)
 
+/* (int *)uaddr2 |= oparg */
 ENTRY(futex_orl)
-	brk #0
+	check_user_access 1, (VM_MAXUSER_ADDRESS-3), futex_fault_nopcb
+	adr	x6, futex_fault		/* Load the fault handler */
+	mov	w5, #1
+	SET_FAULT_HANDLER(x6, x4)	/* And set it */
+	ENTER_USER_ACCESS(w6, x4)
+	mov	w5, #LINUX_FUTEX_MAX_LOOPS
+	prfm	pstl1strm, [x1]
+	mov	w6, w0			/* Save oparg */
+1:	ldxr	w4, [x1]		/* Load oldval from uaddr */
+	orr	w3, w4, w6		/* oldavl |= oparg */
+	stlxr	w0, w3, [x1]		/* Store newval to the uaddr */
+	cbz	w0, 3f			/* Exit on success */
+	sub	w5, w5, w0		/* Dec loop counter */
+	cbnz	w5, 1b			/* Loop */
+	mov	x0, #EAGAIN		/* Store of newval failed */
+3:	dmb	ish
+	EXIT_USER_ACCESS(w6)
+	SET_FAULT_HANDLER(xzr, x6)	/* Reset the fault handler */
+	str w4, [x2]			/* Store oldval */
 	ret
 END(futex_orl)
 
+/* (int *)uaddr2 &= oparg */
 ENTRY(futex_andl)
-	brk #0
+	check_user_access 1, (VM_MAXUSER_ADDRESS-3), futex_fault_nopcb
+	adr	x6, futex_fault		/* Load the fault handler */
+	mov	w5, #1
+	SET_FAULT_HANDLER(x6, x4)	/* And set it */
+	ENTER_USER_ACCESS(w6, x4)
+	mov	w5, #LINUX_FUTEX_MAX_LOOPS
+	prfm	pstl1strm, [x1]
+	mov	w6, w0			/* Save oparg */
+1:	ldxr	w4, [x1]		/* Load oldval from uaddr */
+	and	w3, w4, w6		/* oldval &= oparg */
+	stlxr	w0, w3, [x1]		/* Store newval to the uaddr */
+	cbz	w0, 3f			/* Exit on success */
+	sub	w5, w5, w0		/* Dec loop counter */
+	cbnz	w5, 1b			/* Loop */
+	mov	x0, #EAGAIN		/* Store of newval failed */
+3:	dmb	ish
+	EXIT_USER_ACCESS(w6)
+	SET_FAULT_HANDLER(xzr, x6)	/* Reset the fault handler */
+	str w4, [x2]			/* Return oldval */
 	ret
 END(futex_andl)
 
+/* (int *)uaddr2 ^= oparg */
 ENTRY(futex_xorl)
-	brk #0
+	check_user_access 1, (VM_MAXUSER_ADDRESS-3), futex_fault_nopcb
+	adr	x6, futex_fault		/* Load the fault handler */
+	mov	w5, #1
+	SET_FAULT_HANDLER(x6, x4)	/* And set it */
+	ENTER_USER_ACCESS(w6, x4)
+	mov	w5, #LINUX_FUTEX_MAX_LOOPS
+	prfm	pstl1strm, [x1]
+	mov	w6, w0			/* Save oparg */
+1:	ldxr	w4, [x1]		/* Load oldval from uaddr */
+	eor	w3, w4, w6		/* oldval ^= oparg */
+	stlxr	w0, w3, [x1]		/* Store newval to the uaddr */
+	cbz	w0, 3f			/* Exit on success */
+	sub	w5, w5, w0		/* Dec loop counter */
+	cbnz	w5, 1b			/* Loop */
+	mov	x0, #EAGAIN		/* Store of newval failed */
+3:	dmb	ish
+	EXIT_USER_ACCESS(w6)
+	SET_FAULT_HANDLER(xzr, x6)	/* Reset the fault handler */
+	str w4, [x2]			/* Return oldval */
 	ret
 END(futex_xorl)