3 files changed, 106 insertions, 54 deletions

diff --git a/lib/libthr/thread/thr_mutex.c b/lib/libthr/thread/thr_mutex.c
index 62d7ac68a56a..5b69952fa86e 100644
--- a/lib/libthr/thread/thr_mutex.c
+++ b/lib/libthr/thread/thr_mutex.c
@@ -93,6 +93,8 @@ static int	mutex_self_trylock(pthread_mutex_t);
 static int	mutex_self_lock(pthread_mutex_t,
 				const struct timespec *abstime);
 static int	mutex_unlock_common(pthread_mutex_t *);
+static int	mutex_lock_sleep(struct pthread *, pthread_mutex_t,
+				const struct timespec *);
 
 __weak_reference(__pthread_mutex_init, pthread_mutex_init);
 __strong_reference(__pthread_mutex_init, _pthread_mutex_init);
@@ -346,25 +348,24 @@ __pthread_mutex_trylock(pthread_mutex_t *mutex)
 }
 
 static int
-mutex_lock_sleep(struct pthread *curthread, pthread_mutex_t m,
-	const struct timespec * abstime)
+mutex_lock_sleep(struct pthread *curthread, struct pthread_mutex *m,
+	const struct timespec *abstime)
 {
-	struct  timespec ts, ts2;
-	uint32_t	id;
-	int	ret;
+	uint32_t	id, owner;
 	int	count;
+	int	ret;
 
-	id = TID(curthread);
-	if (__predict_false(m->m_owner == curthread))
-		return  mutex_self_lock(m, abstime);
+	if (m->m_owner == curthread)
+		return mutex_self_lock(m, abstime);
 
+	id = TID(curthread);
 	/*
 	 * For adaptive mutexes, spin for a bit in the expectation
 	 * that if the application requests this mutex type then
 	 * the lock is likely to be released quickly and it is
 	 * faster than entering the kernel
 	 */
-	if (m->m_lock.m_flags & UMUTEX_PRIO_PROTECT)
+	if (m->m_lock.m_flags & (UMUTEX_PRIO_PROTECT | UMUTEX_PRIO_INHERIT))
 		goto sleep_in_kernel;
 
 	if (!_thr_is_smp)
@@ -372,10 +373,12 @@ mutex_lock_sleep(struct pthread *curthread, pthread_mutex_t m,
 
 	count = m->m_spinloops;
 	while (count--) {
-		if (m->m_lock.m_owner == UMUTEX_UNOWNED) {
-			ret = _thr_umutex_trylock2(&m->m_lock, id);
-			if (ret == 0)
+		owner = m->m_lock.m_owner;
+		if ((owner & ~UMUTEX_CONTESTED) == 0) {
+			if (atomic_cmpset_acq_32(&m->m_lock.m_owner, owner, id|owner)) {
+				ret = 0;
 				goto done;
+			}
 		}
 		CPU_SPINWAIT;
 	}
@@ -384,49 +387,43 @@ yield_loop:
 	count = m->m_yieldloops;
 	while (count--) {
 		_sched_yield();
-		ret = _thr_umutex_trylock2(&m->m_lock, id);
-		if (ret == 0)
-			goto done;
+		owner = m->m_lock.m_owner;
+		if ((owner & ~UMUTEX_CONTESTED) == 0) {
+			if (atomic_cmpset_acq_32(&m->m_lock.m_owner, owner, id|owner)) {
+				ret = 0;
+				goto done;
+			}
+		}
 	}
 
 sleep_in_kernel:
 	if (abstime == NULL) {
-		ret = __thr_umutex_lock(&m->m_lock);
+		ret = __thr_umutex_lock(&m->m_lock, id);
 	} else if (__predict_false(
-		   abstime->tv_sec < 0 || abstime->tv_nsec < 0 ||
+		   abstime->tv_nsec < 0 ||
 		   abstime->tv_nsec >= 1000000000)) {
 		ret = EINVAL;
 	} else {
-		clock_gettime(CLOCK_REALTIME, &ts);
-		TIMESPEC_SUB(&ts2, abstime, &ts);
-		ret = __thr_umutex_timedlock(&m->m_lock, &ts2);
-		/*
-		 * Timed out wait is not restarted if
-		 * it was interrupted, not worth to do it.
-		 */
-		if (ret == EINTR)
-			ret = ETIMEDOUT;
+		ret = __thr_umutex_timedlock(&m->m_lock, id, abstime);
 	}
 done:
 	if (ret == 0)
 		ENQUEUE_MUTEX(curthread, m);
+
 	return (ret);
 }
 
 static inline int
 mutex_lock_common(struct pthread *curthread, struct pthread_mutex *m,
-	const struct timespec * abstime)
+	const struct timespec *abstime)
 {
-	uint32_t	id;
-	int	ret;
 
-	id = TID(curthread);
-	ret = _thr_umutex_trylock2(&m->m_lock, id);
-	if (ret == 0)
+	if (_thr_umutex_trylock2(&m->m_lock, TID(curthread)) == 0) {
 		ENQUEUE_MUTEX(curthread, m);
-	else
-		ret = mutex_lock_sleep(curthread, m, abstime);
-	return (ret);
+		return (0);
+	}
+	
+	return (mutex_lock_sleep(curthread, m, abstime));
 }
 
 int
@@ -450,6 +447,7 @@ __pthread_mutex_lock(pthread_mutex_t *mutex)
 			return (ret);
 		m = *mutex;
 	}
+
 	return (mutex_lock_common(curthread, m, NULL));
 }
 
diff --git a/lib/libthr/thread/thr_umtx.c b/lib/libthr/thread/thr_umtx.c
index 3efe8847a1a8..7c59bb78ccb8 100644
--- a/lib/libthr/thread/thr_umtx.c
+++ b/lib/libthr/thread/thr_umtx.c
@@ -48,25 +48,74 @@ _thr_umutex_init(struct umutex *mtx)
 }
 
 int
-__thr_umutex_lock(struct umutex *mtx)
+__thr_umutex_lock(struct umutex *mtx, uint32_t id)
 {
-	return _umtx_op_err(mtx, UMTX_OP_MUTEX_LOCK, 0, 0, 0);
+	uint32_t owner;
+
+	if ((mtx->m_flags & (UMUTEX_PRIO_PROTECT | UMUTEX_PRIO_INHERIT)) == 0) {
+		for (;;) {
+			/* wait in kernel */
+			_umtx_op_err(mtx, UMTX_OP_MUTEX_WAIT, 0, 0, 0);
+
+			owner = mtx->m_owner;
+			if ((owner & ~UMUTEX_CONTESTED) == 0 &&
+			     atomic_cmpset_acq_32(&mtx->m_owner, owner, id|owner))
+				return (0);
+		}
+	}
+
+	return	_umtx_op_err(mtx, UMTX_OP_MUTEX_LOCK, 0, 0, 0);
 }
 
 int
-__thr_umutex_timedlock(struct umutex *mtx,
-	const struct timespec *timeout)
+__thr_umutex_timedlock(struct umutex *mtx, uint32_t id,
+	const struct timespec *ets)
 {
-	if (timeout && (timeout->tv_sec < 0 || (timeout->tv_sec == 0 &&
-		timeout->tv_nsec <= 0)))
+	struct timespec timo, cts;
+	uint32_t owner;
+	int ret;
+
+	clock_gettime(CLOCK_REALTIME, &cts);
+	TIMESPEC_SUB(&timo, ets, &cts);
+
+	if (timo.tv_sec < 0)
 		return (ETIMEDOUT);
-	return _umtx_op_err(mtx, UMTX_OP_MUTEX_LOCK, 0, 0,
-		__DECONST(void *, timeout));
+
+	for (;;) {
+		if ((mtx->m_flags & (UMUTEX_PRIO_PROTECT | UMUTEX_PRIO_INHERIT)) == 0) {
+
+			/* wait in kernel */
+			ret = _umtx_op_err(mtx, UMTX_OP_MUTEX_WAIT, 0, 0, &timo);
+
+			/* now try to lock it */
+			owner = mtx->m_owner;
+			if ((owner & ~UMUTEX_CONTESTED) == 0 &&
+			     atomic_cmpset_acq_32(&mtx->m_owner, owner, id|owner))
+				return (0);
+		} else {
+			ret = _umtx_op_err(mtx, UMTX_OP_MUTEX_LOCK, 0, 0, &timo);
+			if (ret == 0)
+				break;
+		}
+		if (ret == ETIMEDOUT)
+			break;
+		clock_gettime(CLOCK_REALTIME, &cts);
+		TIMESPEC_SUB(&timo, ets, &cts);
+		if (timo.tv_sec < 0 || (timo.tv_sec == 0 && timo.tv_nsec == 0)) {
+			ret = ETIMEDOUT;
+			break;
+		}
+	}
+	return (ret);
 }
 
 int
-__thr_umutex_unlock(struct umutex *mtx)
+__thr_umutex_unlock(struct umutex *mtx, uint32_t id)
 {
+	if ((mtx->m_flags & (UMUTEX_PRIO_PROTECT | UMUTEX_PRIO_INHERIT)) == 0) {
+		atomic_cmpset_rel_32(&mtx->m_owner, id | UMUTEX_CONTESTED, UMUTEX_CONTESTED);
+		return _umtx_op_err(mtx, UMTX_OP_MUTEX_WAKE, 0, 0, 0);
+	}
 	return _umtx_op_err(mtx, UMTX_OP_MUTEX_UNLOCK, 0, 0, 0);
 }
 
@@ -123,7 +172,8 @@ _thr_ucond_wait(struct ucond *cv, struct umutex *m,
 {
 	if (timeout && (timeout->tv_sec < 0 || (timeout->tv_sec == 0 &&
 	    timeout->tv_nsec <= 0))) {
-		__thr_umutex_unlock(m);
+		struct pthread *curthread = _get_curthread();
+		_thr_umutex_unlock(m, TID(curthread));
                 return (ETIMEDOUT);
 	}
 	return _umtx_op_err(cv, UMTX_OP_CV_WAIT,
diff --git a/lib/libthr/thread/thr_umtx.h b/lib/libthr/thread/thr_umtx.h
index 0ef75a7e49e2..41b5f9686783 100644
--- a/lib/libthr/thread/thr_umtx.h
+++ b/lib/libthr/thread/thr_umtx.h
@@ -34,10 +34,10 @@
 
 #define DEFAULT_UMUTEX	{0,0, {0,0},{0,0,0,0}}
 
-int __thr_umutex_lock(struct umutex *mtx) __hidden;
-int __thr_umutex_timedlock(struct umutex *mtx,
+int __thr_umutex_lock(struct umutex *mtx, uint32_t id) __hidden;
+int __thr_umutex_timedlock(struct umutex *mtx, uint32_t id,
 	const struct timespec *timeout) __hidden;
-int __thr_umutex_unlock(struct umutex *mtx) __hidden;
+int __thr_umutex_unlock(struct umutex *mtx, uint32_t id) __hidden;
 int __thr_umutex_trylock(struct umutex *mtx) __hidden;
 int __thr_umutex_set_ceiling(struct umutex *mtx, uint32_t ceiling,
 	uint32_t *oldceiling) __hidden;
@@ -71,26 +71,30 @@ _thr_umutex_trylock(struct umutex *mtx, uint32_t id)
 static inline int
 _thr_umutex_trylock2(struct umutex *mtx, uint32_t id)
 {
-    if (atomic_cmpset_acq_32(&mtx->m_owner, UMUTEX_UNOWNED, id))
+    if (atomic_cmpset_acq_32(&mtx->m_owner, UMUTEX_UNOWNED, id) != 0)
 	return (0);
+    if ((uint32_t)mtx->m_owner == UMUTEX_CONTESTED &&
+        __predict_true((mtx->m_flags & (UMUTEX_PRIO_PROTECT | UMUTEX_PRIO_INHERIT)) == 0))
+    	if (atomic_cmpset_acq_32(&mtx->m_owner, UMUTEX_CONTESTED, id | UMUTEX_CONTESTED))
+		return (0);
     return (EBUSY);
 }
 
 static inline int
 _thr_umutex_lock(struct umutex *mtx, uint32_t id)
 {
-    if (atomic_cmpset_acq_32(&mtx->m_owner, UMUTEX_UNOWNED, id))
+    if (_thr_umutex_trylock2(mtx, id) == 0)
 	return (0);
-    return (__thr_umutex_lock(mtx));
+    return (__thr_umutex_lock(mtx, id));
 }
 
 static inline int
 _thr_umutex_timedlock(struct umutex *mtx, uint32_t id,
 	const struct timespec *timeout)
 {
-    if (atomic_cmpset_acq_32(&mtx->m_owner, UMUTEX_UNOWNED, id))
+    if (_thr_umutex_trylock2(mtx, id) == 0)
 	return (0);
-    return (__thr_umutex_timedlock(mtx, timeout));
+    return (__thr_umutex_timedlock(mtx, id, timeout));
 }
 
 static inline int
@@ -98,7 +102,7 @@ _thr_umutex_unlock(struct umutex *mtx, uint32_t id)
 {
     if (atomic_cmpset_rel_32(&mtx->m_owner, id, UMUTEX_UNOWNED))
 	return (0);
-    return (__thr_umutex_unlock(mtx));
+    return (__thr_umutex_unlock(mtx, id));
 }
 
 static inline int