[RFC PATCH 0/5] futex: introduce an optimistic spinning futex

@@ -20,6 +20,8 @@
 #define FUTEX_WAKE_BITSET	10
 #define FUTEX_WAIT_REQUEUE_PI	11
 #define FUTEX_CMP_REQUEUE_PI	12
+#define FUTEX_SPIN_LOCK		13
+#define FUTEX_SPIN_UNLOCK	14
 
 #define FUTEX_PRIVATE_FLAG	128
 #define FUTEX_CLOCK_REALTIME	256

@@ -39,6 +41,8 @@
 					 FUTEX_PRIVATE_FLAG)
 #define FUTEX_CMP_REQUEUE_PI_PRIVATE	(FUTEX_CMP_REQUEUE_PI | \
 					 FUTEX_PRIVATE_FLAG)
+#define FUTEX_SPIN_LOCK_PRIVATE		(FUTEX_SPIN_LOCK | FUTEX_PRIVATE_FLAG)
+#define FUTEX_SPIN_UNLOCK_PRIVATE	(FUTEX_SPIN_UNLOCK | FUTEX_PRIVATE_FLAG)
 
 /*
  * Support for robust futexes: the kernel cleans up held futexes at

@@ -23,6 +23,9 @@
  *  Copyright (C) IBM Corporation, 2009
  *  Thanks to Thomas Gleixner for conceptual design and careful reviews.
  *
+ *  Optimistic-spinning futexes by Waiman Long
+ *  Copyright (C) 2014 Hewlett-Packard Development Company, L.P.
+ *
  *  Thanks to Ben LaHaise for yelling "hashed waitqueues" loudly
  *  enough at me, Linus for the original (flawed) idea, Matthew
  *  Kirkwood for proof-of-concept implementation.

@@ -253,6 +256,8 @@ struct futex_hash_bucket {
 	atomic_t waiters;
 	spinlock_t lock;
 	struct plist_head chain;
+	struct list_head oslist;	/* Optimistic spinning list */
+	void *qhcache;			/* Cache for a free queue head */
 } ____cacheline_aligned_in_smp;
 
 static unsigned long __read_mostly futex_hashsize;

@@ -2939,6 +2944,446 @@ void exit_robust_list(struct task_struct *curr)
 				   curr, pip);
 }
 
+/*=========================================
+ *	Optimistic Spinning Futexes
+ *=========================================
+ *
+ * A major difference between the optimistic spinning (optspin) futex and a
+ * wait-wake futex is the fact we need a central data structure to hold
+ * information on all the spinning and waiting tasks for a given optspin
+ * futex. We can't use the futex_q structure for that purpose. Instead,
+ * we have to dynamically allocate a new futex_q_head structure to hold that
+ * information.
+ *
+ * To ease implementation, these new futex_q_head structures are queued in a
+ * different list within the futex hash bucket. To reduce the memory allocation
+ * and deallocation overhead, another field is added to the hash bucket to
+ * cache the last freed futex_q_head structure. This improves performance
+ * at the expense of a bit higher memory overhead. As long as there is no futex
+ * address hash collision, there is no need to do a lot of memory allocation
+ * and deallocation for using optspin futex.
+ *
+ * The states of each spinning or waiting task are stored in a futex_q_node
+ * structure allocated from the task's kernel stack, just like futex_q.
+ *
+ * Data structure diagram:
+ *
+ * hash	       futex	    futex
+ * table       qhead 0      qhead 1
+ * |    |
+ * +----+     +------+     +------+
+ * | p  |---->| next |---->| next |----> ....
+ * +----+     +------+     +------+
+ * |    |        |            |
+ *               |
+ *            +------+     +------+
+ *            |node 0|---->|node 1|----> ....
+ *            +------+     +------+
+ *
+ * Locking
+ * -------
+ * Two spinlocks are used by the optspin futexes - the hash bucket spinlock
+ * and the queue head spinlock.
+ *
+ * The hash bucket spinlock protects against the searching and modification
+ * of the futex queue head list as well as the increment of the atomic locker
+ * count. The decrement of locker count is done outside the lock. When the
+ * count reaches 0, the queue head structure can then be deallocated.
+ *
+ * The queue head spinlock protects the futex wait queue from modification.
+ */
+#define FUTEX_TID(u)		(pid_t)((u) & FUTEX_TID_MASK)
+#define FUTEX_HAS_WAITERS(u)	((u) & FUTEX_WAITERS)
+
+/**
+ * struct futex_q_head - head of the optspin futex queue, one per unique key
+ * @hnode:	list entry from the hash bucket
+ * @waitq:	a list of waiting tasks
+ * @key:	the key the futex is hashed on
+ * @wlock:	spinlock for managing wait queue
+ * @lcnt:	locker count
+ *
+ * Locking sequence
+ * ----------------
+ * 1) Lock hash bucket spinlock, locate the futex queue head
+ * 2) Inc lcnt (lock) or read lcnt (unlock), release hash bucket spinlock
+ * 3) For waiter:
+ *    - lock futex queue head spinlock
+ *    - enqueue into the wait queue
+ *    - release the lock & sleep
+ * 4) For unlocker:
+ *    - locate the queue head just like a locker does
+ *    - Take the queue head lock and wake up the first waiter there.
+ */
+struct futex_q_head {
+	struct list_head	      hnode;
+	struct list_head	      waitq;
+	union futex_key		      key;
+	pid_t			      otid;
+	spinlock_t		      wlock;
+	atomic_t		      lcnt;
+};
+
+/**
+ * struct futex_q_node - a node in the optspin futex queue
+ * @wnode:	a list entry for the waiting queue
+ * @task:	task_struct pointer of the current task
+ */
+struct futex_q_node {
+	struct list_head	wnode;
+	struct task_struct     *task;
+};
+
+
+/*
+ * find_qhead - Find a queue head structure with the matching key
+ *
+ * The caller must hold the hash bucket spinlock.
+ */
+static inline struct futex_q_head *
+find_qhead(struct futex_hash_bucket *hb, union futex_key *key)
+{
+	struct futex_q_head *qh;
+
+	list_for_each_entry(qh, &hb->oslist, hnode)
+		if (match_futex(&qh->key, key))
+			return qh;
+	return NULL;
+}
+
+/*
+ * qhead_alloc_init - allocate and initialize a queue head structure.
+ *
+ * The caller must hold the hash bucket spinlock.
+ *
+ * A one-slot queue head structure cache is added to the futex hash bucket
+ * to minimize overhead due to memory allocation and deallocation under light
+ * contention with no hash bucket collision.
+ */
+static inline struct futex_q_head *
+qhead_alloc_init(struct futex_hash_bucket *hb, union futex_key *key)
+{
+	struct futex_q_head *qh = NULL;
+	static const struct futex_q_head qh0 = { { 0 } };
+
+	if (hb->qhcache)
+		qh = xchg(&hb->qhcache, NULL);
+	if (!qh)
+		qh = kmalloc(sizeof(struct futex_q_head), GFP_KERNEL);
+
+	/*
+	 * Initialize the queue head structure
+	 */
+	if (qh) {
+		*qh = qh0;
+		qh->key = *key;
+		atomic_set(&qh->lcnt, 1);
+		INIT_LIST_HEAD(&qh->waitq);
+		spin_lock_init(&qh->wlock);
+		list_add(&qh->hnode, &hb->oslist);
+	}
+	return qh;
+}
+
+/*
+ * free_qhead - put queue head structure back to the free qh cache or free it
+ */
+static inline void
+qhead_free(struct futex_q_head *qh, struct futex_hash_bucket *hb)
+{
+	bool found;
+	struct futex_q_head *qhead;
+
+	/*
+	 * Try to free the queue head structure if no one is using it
+	 * Must take the hash bucket lock to make sure that no one
+	 * else is touching the locker count.
+	 */
+	spin_lock(&hb->lock);
+
+	/*
+	 * First make sure that qh is still there and unused in the
+	 * hashed list.
+	 */
+	found = false;
+	list_for_each_entry(qhead, &hb->oslist, hnode)
+		if (qhead == qh) {
+			found = true;
+			break;
+		}
+
+	if (!found || atomic_read(&qh->lcnt)) {
+		spin_unlock(&hb->lock);
+		return;
+	}
+
+	/*
+	 * Free the queue head structure
+	 */
+	BUG_ON(!list_empty(&qh->waitq));
+	list_del(&qh->hnode);
+	spin_unlock(&hb->lock);
+
+	if (!hb->qhcache && (cmpxchg(&hb->qhcache, NULL, qh) == NULL))
+		return;
+	kfree(qh);
+}
+
+/*
+ * futex_spin_trylock - attempt to take the lock
+ * Return: 1 if successful or an error happen
+ *	   0 otherwise
+ *
+ * Side effect: The uval and ret will be updated.
+ */
+static inline int futex_spin_trylock(u32 __user *uaddr, u32 *puval,
+				       int *pret, u32 vpid)
+{
+	u32	  old;
+
+	*pret = get_futex_value_locked(puval, uaddr);
+	if (*pret)
+		return 1;
+
+	if (FUTEX_TID(*puval))
+		return 0;	/* The mutex is not free */
+
+	old = *puval;
+
+	*pret = cmpxchg_futex_value_locked(puval, uaddr, old, vpid | old);
+	if (*pret)
+		return 1;
+	if (*puval == old) {
+		/* Adjust uval to reflect current value */
+		*puval = vpid | old;
+		return 1;
+	}
+	return 0;
+}
+
+/*
+ * futex_spin_lock
+ */
+static noinline int futex_spin_lock(u32 __user *uaddr, unsigned int flags)
+{
+	struct futex_hash_bucket *hb;
+	struct futex_q_head	 *qh = NULL;
+	struct futex_q_node	  qnode;
+	union futex_key		  key;
+	bool			  gotlock;
+	int			  ret, cnt;
+	u32			  uval, vpid, old;
+
+	qnode.task  = current;
+	vpid = task_pid_vnr(qnode.task);
+
+	ret = get_futex_key(uaddr, flags & FLAGS_SHARED, &key, VERIFY_WRITE);
+	if (unlikely(ret))
+		return ret;
+
+	hb = hash_futex(&key);
+	spin_lock(&hb->lock);
+
+	/*
+	 * Locate the queue head for the given key
+	 */
+	qh = find_qhead(hb, &key);
+
+	/*
+	 * Check the futex value under the hash bucket lock as it might
+	 * be changed.
+	 */
+	if (futex_spin_trylock(uaddr, &uval, &ret, vpid))
+		goto hbunlock_out;
+
+	if (!qh) {
+		/*
+		 * First waiter:
+		 * Allocate a queue head structure & initialize it
+		 */
+		qh = qhead_alloc_init(hb, &key);
+		if (unlikely(!qh)) {
+			ret = -ENOMEM;
+			goto hbunlock_out;
+		}
+	} else {
+		atomic_inc(&qh->lcnt);
+	}
+	spin_unlock(&hb->lock);
+
+	/*
+	 * Put the task into the wait queue and sleep
+	 */
+	preempt_disable();
+	get_task_struct(qnode.task);
+	spin_lock(&qh->wlock);
+	list_add_tail(&qnode.wnode, &qh->waitq);
+	__set_current_state(TASK_INTERRUPTIBLE);
+	spin_unlock(&qh->wlock);
+	gotlock = false;
+	for (;;) {
+		ret = get_user(uval, uaddr);
+		if (ret)
+			break;
+		while ((FUTEX_TID(uval) == 0) || !FUTEX_HAS_WAITERS(uval)) {
+			/*
+			 * Make sure the waiter bit is set before sleeping
+			 * and get the lock if it is available.
+			 */
+			if (FUTEX_TID(uval) == 0) {
+				old = uval;
+				ret = futex_atomic_cmpxchg_inatomic(&uval,
+					uaddr, old, vpid);
+				if (ret) {
+					goto dequeue;
+				} else if (uval == old) {
+					gotlock = true;
+					goto dequeue;
+				}
+				continue;
+			}
+
+			old = uval;
+			ret = futex_atomic_cmpxchg_inatomic(&uval, uaddr, old,
+					FUTEX_TID(old) | FUTEX_WAITERS);
+			if (ret)
+				goto dequeue;
+			if (uval == old)
+				break;
+		}
+
+		schedule_preempt_disabled();
+
+		/*
+		 * Got a signal? Return EINTR
+		 */
+		if (unlikely(signal_pending(qnode.task))) {
+			ret = -EINTR;
+			break;	/* Remove from queue */
+		}
+	}
+dequeue:
+	__set_current_state(TASK_RUNNING);
+	/*
+	 * Remove itself from the wait queue and go back to optimistic
+	 * spinning if it hasn't got the lock yet.
+	 */
+	put_task_struct(qnode.task);
+	spin_lock(&qh->wlock);
+	list_del(&qnode.wnode);
+
+	/*
+	 * Try to clear the waiter bit if the wait queue is empty
+	 */
+	if (list_empty(&qh->waitq)) {
+		int retval = get_futex_value_locked(&uval, uaddr);
+
+		if (!retval && FUTEX_HAS_WAITERS(uval)) {
+			old   = uval;
+			uval &= ~FUTEX_WAITERS;
+			(void)cmpxchg_futex_value_locked(&uval, uaddr, old,
+							  uval);
+		}
+	}
+	spin_unlock(&qh->wlock);
+	preempt_enable();
+
+	cnt = atomic_dec_return(&qh->lcnt);
+	if (cnt == 0)
+		qhead_free(qh, hb);
+	/*
+	 * Need to set the waiters bit there are still waiters
+	 */
+	else if (!ret)
+		ret = put_user(vpid | FUTEX_WAITERS, uaddr);
+out:
+	put_futex_key(&key);
+	return ret;
+
+hbunlock_out:
+	spin_unlock(&hb->lock);
+	goto out;
+}
+
+/*
+ * futex_spin_unlock
+ */
+static noinline int futex_spin_unlock(u32 __user *uaddr, unsigned int flags)
+{
+	struct futex_hash_bucket *hb;
+	struct futex_q_head	 *qh;
+	union futex_key		  key;
+	struct task_struct	 *wtask;	/* Task to be woken */
+	int			  ret, lcnt;
+	u32			  uval, old, vpid = task_pid_vnr(current);
+
+	ret = get_user(uval, uaddr);
+	if (ret)
+		return ret;
+
+	/*
+	 * The unlocker may have cleared the TID value and another task may
+	 * steal it. However, if its TID is still set, we need to clear
+	 * it as well as the FUTEX_WAITERS bit.
+	 */
+	while (FUTEX_TID(uval) == vpid) {
+		old   = uval;
+		uval &= ~(FUTEX_TID_MASK | FUTEX_WAITERS);
+		ret   = futex_atomic_cmpxchg_inatomic(&uval, uaddr, old, uval);
+		if (ret)
+			return ret;
+		if (uval == old)
+			break;
+	}
+	/*
+	 * Need to wake up a waiter
+	 */
+	ret = get_futex_key(uaddr, flags & FLAGS_SHARED, &key, VERIFY_WRITE);
+	if (unlikely(ret))
+		return ret;
+
+	hb = hash_futex(&key);
+	spin_lock(&hb->lock);
+
+	/*
+	 * Locate the queue head for the given key
+	 */
+	qh = find_qhead(hb, &key);
+	if (!qh) {
+		spin_unlock(&hb->lock);
+		ret = -ESRCH;	/* Invalid futex address */
+		goto out;
+	}
+
+	/*
+	 * The hash bucket lock is being hold while retrieving the task
+	 * structure from the queue head to make sure that the qh structure
+	 * won't go away under the hood.
+	 * Preemption is disabled during the wakeup process to avoid having
+	 * a long gap between getting the task structure and waking it up.
+	 */
+	wtask = NULL;
+	preempt_disable();
+	lcnt = atomic_read(&qh->lcnt);
+	if (lcnt) {
+		spin_lock(&qh->wlock);
+		if (!list_empty(&qh->waitq))
+			wtask = list_entry(qh->waitq.next, struct futex_q_node,
+					   wnode)->task;
+		spin_unlock(&qh->wlock);
+	}
+	spin_unlock(&hb->lock);
+	if (wtask && wake_up_process(wtask))
+		ret = 1;
+	else if (!wtask)
+		ret = -ESRCH;
+	preempt_enable();
+out:
+	put_futex_key(&key);
+	return ret;
+}
+
+
 long do_futex(u32 __user *uaddr, int op, u32 val, ktime_t *timeout,
 		u32 __user *uaddr2, u32 val2, u32 val3)
 {

@@ -2960,6 +3405,8 @@ long do_futex(u32 __user *uaddr, int op, u32 val, ktime_t *timeout,
 	case FUTEX_TRYLOCK_PI:
 	case FUTEX_WAIT_REQUEUE_PI:
 	case FUTEX_CMP_REQUEUE_PI:
+	case FUTEX_SPIN_LOCK:
+	case FUTEX_SPIN_UNLOCK:
 		if (!futex_cmpxchg_enabled)
 			return -ENOSYS;
 	}

@@ -2991,6 +3438,10 @@ long do_futex(u32 __user *uaddr, int op, u32 val, ktime_t *timeout,
 					     uaddr2);
 	case FUTEX_CMP_REQUEUE_PI:
 		return futex_requeue(uaddr, flags, uaddr2, val, val2, &val3, 1);
+	case FUTEX_SPIN_LOCK:
+		return futex_spin_lock(uaddr, flags);
+	case FUTEX_SPIN_UNLOCK:
+		return futex_spin_unlock(uaddr, flags);
 	}
 	return -ENOSYS;
 }

@@ -3072,7 +3523,9 @@ static int __init futex_init(void)
 	for (i = 0; i < futex_hashsize; i++) {
 		atomic_set(&futex_queues[i].waiters, 0);
 		plist_head_init(&futex_queues[i].chain);
+		INIT_LIST_HEAD(&futex_queues[i].oslist);
 		spin_lock_init(&futex_queues[i].lock);
+		futex_queues[i].qhcache = NULL;
 	}
 
 	return 0;

@@ -3215,8 +3215,8 @@ static inline int futex_spin_trylock(u32 __user *uaddr, u32 *puval,
  * loop. Preemption should have been disabled before calling this function.
  *
  * The number of spinners may temporarily exceed the threshold due to
- * racing (the spin count check and add aren't atomic), but that shouldn't
- * be a big problem.
+ * racing and from waiter joining the OSQ(the spin count check and add
+ * aren't atomic), but that shouldn't be a real problem.
  */
 static inline int futex_optspin(struct futex_q_head *qh,
 				struct futex_q_node *qn,

@@ -3297,7 +3297,7 @@ static noinline int futex_spin_lock(u32 __user *uaddr, unsigned int flags)
 	struct futex_q_node	  qnode;
 	union futex_key		  key;
 	struct task_struct	 *owner;
-	bool			  gotlock;
+	bool			  gotlock, slept;
 	int			  ret, cnt;
 	u32			  uval, vpid, old;
 

@@ -3345,6 +3345,7 @@ static noinline int futex_spin_lock(u32 __user *uaddr, unsigned int flags)
 	owner = ACCESS_ONCE(qh->owner);
 	if ((FUTEX_SPINCNT(qh) < futex_spincnt_max) &&
 	    (!owner || owner->on_cpu))
+optspin:
 		if (futex_optspin(qh, &qnode, uaddr, vpid))
 			goto penable_out;
 

@@ -3356,7 +3357,7 @@ static noinline int futex_spin_lock(u32 __user *uaddr, unsigned int flags)
 	list_add_tail(&qnode.wnode, &qh->waitq);
 	__set_current_state(TASK_INTERRUPTIBLE);
 	spin_unlock(&qh->wlock);
-	gotlock = false;
+	slept = gotlock = false;
 	for (;;) {
 		ret = get_user(uval, uaddr);
 		if (ret)

@@ -3393,7 +3394,16 @@ static noinline int futex_spin_lock(u32 __user *uaddr, unsigned int flags)
 				break;
 		}
 
+		/*
+		 * Go back to spinning if the lock owner is running and the
+		 * spinner limit hasn't been reached.
+		 */
+		if (slept && (!owner || owner->on_cpu) &&
+		   (FUTEX_SPINCNT(qh) < futex_spincnt_max))
+			break;
+
 		schedule_preempt_disabled();
+		slept = true;
 
 		/*
 		 * Got a signal? Return EINTR

@@ -3427,6 +3437,8 @@ dequeue:
 		}
 	}
 	spin_unlock(&qh->wlock);
+	if (!ret && !gotlock)
+		goto optspin;
 
 penable_out:
 	preempt_enable();

@@ -0,0 +1,109 @@
+Started by: Waiman Long <waiman.long@hp.com>
+
+Spinning Futex
+--------------
+
+There are two main problems for a wait-wake futex (FUTEX_WAIT and
+FUTEX_WAKE) when used for creating user-space lock primitives:
+
+ 1) With a wait-wake futex, tasks waiting for a lock are put to sleep
+    in the futex queue to be woken up by the lock owner when it is done
+    with the lock. Waking up a sleeping task, however, introduces some
+    additional latency which can be large especially if the critical
+    section protected by the lock is relatively short. This may cause
+    a performance bottleneck on large systems with many CPUs running
+    applications that need a lot of inter-thread synchronization.
+
+ 2) The performance of the wait-wake futex is currently
+    spinlock-constrained.  When many threads are contending for a
+    futex in a large system with many CPUs, it is not unusual to have
+    spinlock contention accounting for more than 90% of the total
+    CPU cycles consumed at various points in time.
+
+Spinning futex is a solution to both the wakeup latency and spinlock
+contention problems by optimistically spinning on a locked futex
+when the lock owner is running within the kernel until the lock is
+free. This is the same optimistic spinning mechanism used by the kernel
+mutex and rw semaphore implementations to improve performance. The
+optimistic spinning was done without taking any lock.
+
+Implementation
+--------------
+
+Like the PI and robust futexes, a lock acquirer has to atomically
+put its thread ID (TID) into the lower 30 bits of the 32-bit futex
+which should has an original value of 0. If it succeeds, it will be
+the owner of the futex. Otherwise, it has to call into the kernel
+using the new FUTEX_SPIN_LOCK futex(2) syscall.
+
+The kernel will use the setting of the most significant bit
+(FUTEX_WAITERS) in the futex value to indicate one or more waiters
+are sleeping and need to be woken up later on.
+
+When it is time to unlock, the lock owner has to atomically clear
+the TID portion of the futex value. If the FUTEX_WAITERS bit is set,
+it has to issue a FUTEX_SPIN_UNLOCK futex system call to wake up the
+sleeping task.
+
+A return value of 1 from the FUTEX_SPIN_UNLOCK futex(2) syscall
+indicates a task has been woken up. The syscall returns 0 if no
+sleeping task is found or spinners are present to take the lock.
+
+The error number returned by a FUTEX_SPIN_UNLOCK call on an empty
+futex can be used to decide if the spinning futex functionality is
+implemented in the kernel. If it is present, the returned error number
+should be ESRCH. Otherwise it will be ENOSYS.
+
+Currently, only the first and the second arguments (the futex address
+and the opcode) of the futex(2) syscall is used. All the other
+arguments must be set to 0 or NULL to avoid forward compatibility
+problem.
+
+The spinning futex requires the kernel to have support for the cmpxchg
+functionality. For architectures that don't support cmpxchg, spinning
+futex will not be supported as well.
+
+Usage Scenario
+--------------
+
+A spinning futex can be used as an exclusive lock to guard a critical
+section which are unlikely to go to sleep in the kernel. The spinners
+in a spinning futex, however, will fall back to sleep in a wait queue
+if the lock owner isn't running. Therefore, it can also be used when
+the critical section is long and prone to sleeping. However, it may
+not have the performance benefit when compared with a wait-wake futex
+in this case.
+
+Sample Code
+-----------
+
+The following are sample code to implement a simple lock and unlock
+function.
+
+__thread int tid;	/* Thread ID */
+
+void mutex_lock(int *faddr)
+{
+	if (cmpxchg(faddr, 0, tid) == 0)
+		return;
+	for (;;)
+		if (futex(faddr, FUTEX_SPIN_LOCK, ...) == 0)
+			break;
+}
+
+void mutex_unlock(int *faddr)
+{
+	int old, fval;
+
+	if ((fval = cmpxchg(faddr, tid, 0)) == tid)
+		return;
+	/* Clear only the TID portion of the futex */
+	for (;;) {
+		old  = fval;
+		fval = cmpxchg(faddr, old, old & ~FUTEX_TID_MASK);
+		if (fval == old)
+			break;
+	}
+	if (fval & FUTEX_WAITERS)
+		futex(faddr, FUTEX_SPIN_UNLOCK, ...);
+}

@@ -3011,10 +3011,11 @@ void exit_robust_list(struct task_struct *curr)
 /**
  * struct futex_q_head - head of the optspin futex queue, one per unique key
  * @hnode:	list entry from the hash bucket
- * @waitq:	a list of waiting tasks
+ * @waitq:	a rbtree of waiting tasks sorted by priority & sequence number
  * @key:	the key the futex is hashed on
  * @osq:	pointer to optimisitic spinning queue
  * @owner:	task_struct pointer of the futex owner
+ * @seq:	last used sequence number + 1
  * @otid:	TID of the futex owner
  * @wlock:	spinlock for managing wait queue
  * @lcnt:	locker count (spinners + waiters)

@@ -3027,7 +3028,7 @@ void exit_robust_list(struct task_struct *curr)
  *    - enqueue into the spinner queue and wait for its turn.
  * 4) For waiter:
  *    - lock futex queue head spinlock
- *    - enqueue into the wait queue
+ *    - enqueue into the wait rbtree queue
  *    - release the lock & sleep
  * 5) For unlocker:
  *    - locate the queue head just like a locker does

@@ -3037,10 +3038,11 @@ void exit_robust_list(struct task_struct *curr)
  */
 struct futex_q_head {
 	struct list_head	      hnode;
-	struct list_head	      waitq;
+	struct rb_root		      waitq;
 	union futex_key		      key;
 	struct optimistic_spin_queue *osq;
 	struct task_struct	     *owner;
+	unsigned long		      seq;
 	pid_t			      otid;
 	spinlock_t		      wlock;
 	atomic_t		      lcnt;

@@ -3050,10 +3052,14 @@ struct futex_q_head {
  * struct futex_q_node - a node in the optspin futex queue
  * @wnode:	a list entry for the waiting queue
  * @task:	task_struct pointer of the current task
+ * @seq:	unique sequence number that shows order of kernel entry
+ * @prio:	process priority
  */
 struct futex_q_node {
-	struct list_head	wnode;
+	struct rb_node		wnode;
 	struct task_struct     *task;
+	unsigned long		seq;
+	int			prio;
 };
 
 /*

@@ -3113,7 +3119,7 @@ qhead_alloc_init(struct futex_hash_bucket *hb, union futex_key *key, u32 uval)
 		if (unlikely(!qh->owner))
 			qh->otid = 0;
 		atomic_set(&qh->lcnt, 1);
-		INIT_LIST_HEAD(&qh->waitq);
+		qh->waitq = RB_ROOT;
 		spin_lock_init(&qh->wlock);
 		list_add(&qh->hnode, &hb->oslist);
 	}

@@ -3155,7 +3161,7 @@ qhead_free(struct futex_q_head *qh, struct futex_hash_bucket *hb)
 	/*
 	 * Free the queue head structure
 	 */
-	BUG_ON(!list_empty(&qh->waitq) || qh->osq);
+	BUG_ON(!RB_EMPTY_ROOT(&qh->waitq) || qh->osq);
 	list_del(&qh->hnode);
 	spin_unlock(&hb->lock);
 	if (qh->owner)

@@ -3167,6 +3173,38 @@ qhead_free(struct futex_q_head *qh, struct futex_hash_bucket *hb)
 }
 
 /*
+ * qnode_insert - insert queue node into an rbtree sorted by priority
+ * 		  and then the unique sequence number
+ */
+static inline void qnode_insert(struct rb_root *root, struct futex_q_node *node)
+{
+	struct rb_node **new = &(root->rb_node), *parent = NULL;
+
+	/*
+	 * Locate where to put the new node
+	 */
+	while (*new) {
+		struct futex_q_node *this = container_of(*new,
+					    struct futex_q_node, wnode);
+		parent = *new;
+		if (likely(node->prio == this->prio)) {
+			if (node->seq < this->seq)
+				new = &(parent->rb_left);
+			else /* node->seq > this->seq */
+				new = &(parent->rb_right);
+		} else if (node->prio < this->prio) {
+			new =  &(parent->rb_left);
+		} else {
+			new = &(parent->rb_right);
+		}
+	}
+
+	/* Add new node and rebalance tree */
+	rb_link_node(&node->wnode, parent, new);
+	rb_insert_color(&node->wnode, root);
+}
+
+/*
  * futex_spin_trylock - attempt to take the lock
  * Return: 1 if successful or an error happen
  *	   0 otherwise

@@ -3336,6 +3374,7 @@ static noinline int futex_spin_lock(u32 __user *uaddr, unsigned int flags)
 	} else {
 		atomic_inc(&qh->lcnt);
 	}
+	qnode.seq = qh->seq++;
 	spin_unlock(&hb->lock);
 
 	/*

@@ -3353,8 +3392,9 @@ optspin:
 	 * Put the task into the wait queue and sleep
 	 */
 	get_task_struct(qnode.task);
+	qnode.prio = min(qnode.task->normal_prio, MAX_RT_PRIO);
 	spin_lock(&qh->wlock);
-	list_add_tail(&qnode.wnode, &qh->waitq);
+	qnode_insert(&qh->waitq, &qnode);
 	__set_current_state(TASK_INTERRUPTIBLE);
 	spin_unlock(&qh->wlock);
 	slept = gotlock = false;

@@ -3421,12 +3461,12 @@ dequeue:
 	 */
 	put_task_struct(qnode.task);
 	spin_lock(&qh->wlock);
-	list_del(&qnode.wnode);
+	rb_erase(&qnode.wnode, &qh->waitq);
 
 	/*
 	 * Try to clear the waiter bit if the wait queue is empty
 	 */
-	if (list_empty(&qh->waitq)) {
+	if (RB_EMPTY_ROOT(&qh->waitq)) {
 		int retval = get_futex_value_locked(&uval, uaddr);
 
 		if (!retval && FUTEX_HAS_WAITERS(uval)) {

@@ -3529,9 +3569,9 @@ static noinline int futex_spin_unlock(u32 __user *uaddr, unsigned int flags)
 	lcnt = atomic_read(&qh->lcnt);
 	if (lcnt) {
 		spin_lock(&qh->wlock);
-		if (!list_empty(&qh->waitq))
-			wtask = list_entry(qh->waitq.next, struct futex_q_node,
-					   wnode)->task;
+		if (!RB_EMPTY_ROOT(&qh->waitq))
+			wtask = container_of(rb_first(&qh->waitq),
+				struct futex_q_node, wnode)->task;
 		spin_unlock(&qh->wlock);
 	}
 	spin_unlock(&hb->lock);

@@ -71,6 +71,7 @@
 #include <asm/futex.h>
 
 #include "locking/rtmutex_common.h"
+#include "locking/mcs_spinlock.h"
 
 /*
  * READ this before attempting to hack on futexes!

@@ -2995,30 +2996,51 @@ void exit_robust_list(struct task_struct *curr)
 #define FUTEX_TID(u)		(pid_t)((u) & FUTEX_TID_MASK)
 #define FUTEX_HAS_WAITERS(u)	((u) & FUTEX_WAITERS)
 
+/*
+ * Bit usage of the locker count:
+ * bit  0-23: number of lockers (spinners + waiters)
+ * bit 24-30: number of spinners
+ */
+#define FUTEX_SPINCNT_MAX	64	/* Maximum # of spinners */
+#define FUTEX_SPINCNT_SHIFT	24
+#define FUTEX_SPINCNT_BIAS	(1U << FUTEX_SPINCNT_SHIFT)
+#define FUTEX_LOCKCNT_MASK	(FUTEX_SPINCNT_BIAS - 1)
+#define FUTEX_LOCKCNT(qh)	(atomic_read(&(qh)->lcnt) & FUTEX_LOCKCNT_MASK)
+#define FUTEX_SPINCNT(qh)	(atomic_read(&(qh)->lcnt)>>FUTEX_SPINCNT_SHIFT)
+
 /**
  * struct futex_q_head - head of the optspin futex queue, one per unique key
  * @hnode:	list entry from the hash bucket
  * @waitq:	a list of waiting tasks
  * @key:	the key the futex is hashed on
+ * @osq:	pointer to optimisitic spinning queue
+ * @owner:	task_struct pointer of the futex owner
+ * @otid:	TID of the futex owner
  * @wlock:	spinlock for managing wait queue
- * @lcnt:	locker count
+ * @lcnt:	locker count (spinners + waiters)
  *
  * Locking sequence
  * ----------------
  * 1) Lock hash bucket spinlock, locate the futex queue head
  * 2) Inc lcnt (lock) or read lcnt (unlock), release hash bucket spinlock
- * 3) For waiter:
+ * 3) For spinner:
+ *    - enqueue into the spinner queue and wait for its turn.
+ * 4) For waiter:
  *    - lock futex queue head spinlock
  *    - enqueue into the wait queue
  *    - release the lock & sleep
- * 4) For unlocker:
+ * 5) For unlocker:
  *    - locate the queue head just like a locker does
- *    - Take the queue head lock and wake up the first waiter there.
+ *    - clear the owner field if it is the current owner
+ *    - if the locker count is not 0 & osq is empty, take the queue head lock
+ *      and wake up the first waiter.
  */
 struct futex_q_head {
 	struct list_head	      hnode;
 	struct list_head	      waitq;
 	union futex_key		      key;
+	struct optimistic_spin_queue *osq;
+	struct task_struct	     *owner;
 	pid_t			      otid;
 	spinlock_t		      wlock;
 	atomic_t		      lcnt;