xref: /onnv/onnv-gate/usr/src/lib/libc/port/threads/synch.c

/*
 * CDDL HEADER START
 *
 * The contents of this file are subject to the terms of the
 * Common Development and Distribution License (the "License").
 * You may not use this file except in compliance with the License.
 *
 * You can obtain a copy of the license at usr/src/OPENSOLARIS.LICENSE
 * or http://www.opensolaris.org/os/licensing.
 * See the License for the specific language governing permissions
 * and limitations under the License.
 *
 * When distributing Covered Code, include this CDDL HEADER in each
 * file and include the License file at usr/src/OPENSOLARIS.LICENSE.
 * If applicable, add the following below this CDDL HEADER, with the
 * fields enclosed by brackets "[]" replaced with your own identifying
 * information: Portions Copyright [yyyy] [name of copyright owner]
 *
 * CDDL HEADER END
 */

/*
 * Copyright 2008 Sun Microsystems, Inc.  All rights reserved.
 * Use is subject to license terms.
 */

#pragma ident	"%Z%%M%	%I%	%E% SMI"

#include "lint.h"
#include "thr_uberdata.h"
#include <sys/rtpriocntl.h>
#include <sys/sdt.h>
#include <atomic.h>

#if defined(THREAD_DEBUG)
#define	INCR32(x)	(((x) != UINT32_MAX)? (x)++ : 0)
#define	INCR(x)		((x)++)
#define	DECR(x)		((x)--)
#define	MAXINCR(m, x)	((m < ++x)? (m = x) : 0)
#else
#define	INCR32(x)
#define	INCR(x)
#define	DECR(x)
#define	MAXINCR(m, x)
#endif

/*
 * This mutex is initialized to be held by lwp#1.
 * It is used to block a thread that has returned from a mutex_lock()
 * of a LOCK_PRIO_INHERIT mutex with an unrecoverable error.
 */
mutex_t	stall_mutex = DEFAULTMUTEX;

static int shared_mutex_held(mutex_t *);
static int mutex_queuelock_adaptive(mutex_t *);
static void mutex_wakeup_all(mutex_t *);

/*
 * Lock statistics support functions.
 */
void
record_begin_hold(tdb_mutex_stats_t *msp)
{
	tdb_incr(msp->mutex_lock);
	msp->mutex_begin_hold = gethrtime();
}

hrtime_t
record_hold_time(tdb_mutex_stats_t *msp)
{
	hrtime_t now = gethrtime();

	if (msp->mutex_begin_hold)
		msp->mutex_hold_time += now - msp->mutex_begin_hold;
	msp->mutex_begin_hold = 0;
	return (now);
}

/*
 * Called once at library initialization.
 */
void
mutex_setup(void)
{
	if (set_lock_byte(&stall_mutex.mutex_lockw))
		thr_panic("mutex_setup() cannot acquire stall_mutex");
	stall_mutex.mutex_owner = (uintptr_t)curthread;
}

/*
 * The default spin count of 1000 is experimentally determined.
 * On sun4u machines with any number of processors it could be raised
 * to 10,000 but that (experimentally) makes almost no difference.
 * The environment variable:
 *	_THREAD_ADAPTIVE_SPIN=count
 * can be used to override and set the count in the range [0 .. 1,000,000].
 */
int	thread_adaptive_spin = 1000;
uint_t	thread_max_spinners = 100;
int	thread_queue_verify = 0;
static	int	ncpus;

/*
 * Distinguish spinning for queue locks from spinning for regular locks.
 * We try harder to acquire queue locks by spinning.
 * The environment variable:
 *	_THREAD_QUEUE_SPIN=count
 * can be used to override and set the count in the range [0 .. 1,000,000].
 */
int	thread_queue_spin = 10000;

#define	ALL_ATTRIBUTES				\
	(LOCK_RECURSIVE | LOCK_ERRORCHECK |	\
	LOCK_PRIO_INHERIT | LOCK_PRIO_PROTECT |	\
	LOCK_ROBUST)

/*
 * 'type' can be one of USYNC_THREAD, USYNC_PROCESS, or USYNC_PROCESS_ROBUST,
 * augmented by zero or more the flags:
 *	LOCK_RECURSIVE
 *	LOCK_ERRORCHECK
 *	LOCK_PRIO_INHERIT
 *	LOCK_PRIO_PROTECT
 *	LOCK_ROBUST
 */
#pragma weak _mutex_init = mutex_init
/* ARGSUSED2 */
int
mutex_init(mutex_t *mp, int type, void *arg)
{
	int basetype = (type & ~ALL_ATTRIBUTES);
	const pcclass_t *pccp;
	int error = 0;
	int ceil;

	if (basetype == USYNC_PROCESS_ROBUST) {
		/*
		 * USYNC_PROCESS_ROBUST is a deprecated historical type.
		 * We change it into (USYNC_PROCESS | LOCK_ROBUST) but
		 * retain the USYNC_PROCESS_ROBUST flag so we can return
		 * ELOCKUNMAPPED when necessary (only USYNC_PROCESS_ROBUST
		 * mutexes will ever draw ELOCKUNMAPPED).
		 */
		type |= (USYNC_PROCESS | LOCK_ROBUST);
		basetype = USYNC_PROCESS;
	}

	if (type & LOCK_PRIO_PROTECT)
		pccp = get_info_by_policy(SCHED_FIFO);
	if ((basetype != USYNC_THREAD && basetype != USYNC_PROCESS) ||
	    (type & (LOCK_PRIO_INHERIT | LOCK_PRIO_PROTECT))
	    == (LOCK_PRIO_INHERIT | LOCK_PRIO_PROTECT) ||
	    ((type & LOCK_PRIO_PROTECT) &&
	    ((ceil = *(int *)arg) < pccp->pcc_primin ||
	    ceil > pccp->pcc_primax))) {
		error = EINVAL;
	} else if (type & LOCK_ROBUST) {
		/*
		 * Callers of mutex_init() with the LOCK_ROBUST attribute
		 * are required to pass an initially all-zero mutex.
		 * Multiple calls to mutex_init() are allowed; all but
		 * the first return EBUSY.  A call to mutex_init() is
		 * allowed to make an inconsistent robust lock consistent
		 * (for historical usage, even though the proper interface
		 * for this is mutex_consistent()).  Note that we use
		 * atomic_or_16() to set the LOCK_INITED flag so as
		 * not to disturb surrounding bits (LOCK_OWNERDEAD, etc).
		 */
		if (!(mp->mutex_flag & LOCK_INITED)) {
			mp->mutex_type = (uint8_t)type;
			atomic_or_16(&mp->mutex_flag, LOCK_INITED);
			mp->mutex_magic = MUTEX_MAGIC;
		} else if (type != mp->mutex_type ||
		    ((type & LOCK_PRIO_PROTECT) && mp->mutex_ceiling != ceil)) {
			error = EINVAL;
		} else if (mutex_consistent(mp) != 0) {
			error = EBUSY;
		}
		/* register a process robust mutex with the kernel */
		if (basetype == USYNC_PROCESS)
			register_lock(mp);
	} else {
		(void) memset(mp, 0, sizeof (*mp));
		mp->mutex_type = (uint8_t)type;
		mp->mutex_flag = LOCK_INITED;
		mp->mutex_magic = MUTEX_MAGIC;
	}

	if (error == 0 && (type & LOCK_PRIO_PROTECT)) {
		mp->mutex_ceiling = ceil;
	}

	return (error);
}

/*
 * Delete mp from list of ceiling mutexes owned by curthread.
 * Return 1 if the head of the chain was updated.
 */
int
_ceil_mylist_del(mutex_t *mp)
{
	ulwp_t *self = curthread;
	mxchain_t **mcpp;
	mxchain_t *mcp;

	for (mcpp = &self->ul_mxchain;
	    (mcp = *mcpp) != NULL;
	    mcpp = &mcp->mxchain_next) {
		if (mcp->mxchain_mx == mp) {
			*mcpp = mcp->mxchain_next;
			lfree(mcp, sizeof (*mcp));
			return (mcpp == &self->ul_mxchain);
		}
	}
	return (0);
}

/*
 * Add mp to the list of ceiling mutexes owned by curthread.
 * Return ENOMEM if no memory could be allocated.
 */
int
_ceil_mylist_add(mutex_t *mp)
{
	ulwp_t *self = curthread;
	mxchain_t *mcp;

	if ((mcp = lmalloc(sizeof (*mcp))) == NULL)
		return (ENOMEM);
	mcp->mxchain_mx = mp;
	mcp->mxchain_next = self->ul_mxchain;
	self->ul_mxchain = mcp;
	return (0);
}

/*
 * Helper function for _ceil_prio_inherit() and _ceil_prio_waive(), below.
 */
static void
set_rt_priority(ulwp_t *self, int prio)
{
	pcparms_t pcparm;

	pcparm.pc_cid = self->ul_rtclassid;
	((rtparms_t *)pcparm.pc_clparms)->rt_tqnsecs = RT_NOCHANGE;
	((rtparms_t *)pcparm.pc_clparms)->rt_pri = prio;
	(void) priocntl(P_LWPID, self->ul_lwpid, PC_SETPARMS, &pcparm);
}

/*
 * Inherit priority from ceiling.
 * This changes the effective priority, not the assigned priority.
 */
void
_ceil_prio_inherit(int prio)
{
	ulwp_t *self = curthread;

	self->ul_epri = prio;
	set_rt_priority(self, prio);
}

/*
 * Waive inherited ceiling priority.  Inherit from head of owned ceiling locks
 * if holding at least one ceiling lock.  If no ceiling locks are held at this
 * point, disinherit completely, reverting back to assigned priority.
 */
void
_ceil_prio_waive(void)
{
	ulwp_t *self = curthread;
	mxchain_t *mcp = self->ul_mxchain;
	int prio;

	if (mcp == NULL) {
		prio = self->ul_pri;
		self->ul_epri = 0;
	} else {
		prio = mcp->mxchain_mx->mutex_ceiling;
		self->ul_epri = prio;
	}
	set_rt_priority(self, prio);
}

/*
 * Clear the lock byte.  Retain the waiters byte and the spinners byte.
 * Return the old value of the lock word.
 */
static uint32_t
clear_lockbyte(volatile uint32_t *lockword)
{
	uint32_t old;
	uint32_t new;

	do {
		old = *lockword;
		new = old & ~LOCKMASK;
	} while (atomic_cas_32(lockword, old, new) != old);

	return (old);
}

/*
 * Same as clear_lockbyte(), but operates on mutex_lockword64.
 * The mutex_ownerpid field is cleared along with the lock byte.
 */
static uint64_t
clear_lockbyte64(volatile uint64_t *lockword64)
{
	uint64_t old;
	uint64_t new;

	do {
		old = *lockword64;
		new = old & ~LOCKMASK64;
	} while (atomic_cas_64(lockword64, old, new) != old);

	return (old);
}

/*
 * Similar to set_lock_byte(), which only tries to set the lock byte.
 * Here, we attempt to set the lock byte AND the mutex_ownerpid,
 * keeping the remaining bytes constant.
 */
static int
set_lock_byte64(volatile uint64_t *lockword64, pid_t ownerpid)
{
	uint64_t old;
	uint64_t new;

	old = *lockword64 & ~LOCKMASK64;
	new = old | ((uint64_t)(uint_t)ownerpid << PIDSHIFT) | LOCKBYTE64;
	if (atomic_cas_64(lockword64, old, new) == old)
		return (LOCKCLEAR);

	return (LOCKSET);
}

/*
 * Increment the spinners count in the mutex lock word.
 * Return 0 on success.  Return -1 if the count would overflow.
 */
static int
spinners_incr(volatile uint32_t *lockword, uint8_t max_spinners)
{
	uint32_t old;
	uint32_t new;

	do {
		old = *lockword;
		if (((old & SPINNERMASK) >> SPINNERSHIFT) >= max_spinners)
			return (-1);
		new = old + (1 << SPINNERSHIFT);
	} while (atomic_cas_32(lockword, old, new) != old);

	return (0);
}

/*
 * Decrement the spinners count in the mutex lock word.
 * Return the new value of the lock word.
 */
static uint32_t
spinners_decr(volatile uint32_t *lockword)
{
	uint32_t old;
	uint32_t new;

	do {
		new = old = *lockword;
		if (new & SPINNERMASK)
			new -= (1 << SPINNERSHIFT);
	} while (atomic_cas_32(lockword, old, new) != old);

	return (new);
}

/*
 * Non-preemptive spin locks.  Used by queue_lock().
 * No lock statistics are gathered for these locks.
 * No DTrace probes are provided for these locks.
 */
void
spin_lock_set(mutex_t *mp)
{
	ulwp_t *self = curthread;

	no_preempt(self);
	if (set_lock_byte(&mp->mutex_lockw) == 0) {
		mp->mutex_owner = (uintptr_t)self;
		return;
	}
	/*
	 * Spin for a while, attempting to acquire the lock.
	 */
	INCR32(self->ul_spin_lock_spin);
	if (mutex_queuelock_adaptive(mp) == 0 ||
	    set_lock_byte(&mp->mutex_lockw) == 0) {
		mp->mutex_owner = (uintptr_t)self;
		return;
	}
	/*
	 * Try harder if we were previously at a no premption level.
	 */
	if (self->ul_preempt > 1) {
		INCR32(self->ul_spin_lock_spin2);
		if (mutex_queuelock_adaptive(mp) == 0 ||
		    set_lock_byte(&mp->mutex_lockw) == 0) {
			mp->mutex_owner = (uintptr_t)self;
			return;
		}
	}
	/*
	 * Give up and block in the kernel for the mutex.
	 */
	INCR32(self->ul_spin_lock_sleep);
	(void) ___lwp_mutex_timedlock(mp, NULL);
	mp->mutex_owner = (uintptr_t)self;
}

void
spin_lock_clear(mutex_t *mp)
{
	ulwp_t *self = curthread;

	mp->mutex_owner = 0;
	if (atomic_swap_32(&mp->mutex_lockword, 0) & WAITERMASK) {
		(void) ___lwp_mutex_wakeup(mp, 0);
		INCR32(self->ul_spin_lock_wakeup);
	}
	preempt(self);
}

/*
 * Allocate the sleep queue hash table.
 */
void
queue_alloc(void)
{
	ulwp_t *self = curthread;
	uberdata_t *udp = self->ul_uberdata;
	queue_head_t *qp;
	void *data;
	int i;

	/*
	 * No locks are needed; we call here only when single-threaded.
	 */
	ASSERT(self == udp->ulwp_one);
	ASSERT(!udp->uberflags.uf_mt);
	if ((data = mmap(NULL, 2 * QHASHSIZE * sizeof (queue_head_t),
	    PROT_READ|PROT_WRITE, MAP_PRIVATE|MAP_ANON, -1, (off_t)0))
	    == MAP_FAILED)
		thr_panic("cannot allocate thread queue_head table");
	udp->queue_head = qp = (queue_head_t *)data;
	for (i = 0; i < 2 * QHASHSIZE; qp++, i++) {
		qp->qh_type = (i < QHASHSIZE)? MX : CV;
		qp->qh_lock.mutex_flag = LOCK_INITED;
		qp->qh_lock.mutex_magic = MUTEX_MAGIC;
		qp->qh_hlist = &qp->qh_def_root;
#if defined(THREAD_DEBUG)
		qp->qh_hlen = 1;
		qp->qh_hmax = 1;
#endif
	}
}

#if defined(THREAD_DEBUG)

/*
 * Debugging: verify correctness of a sleep queue.
 */
void
QVERIFY(queue_head_t *qp)
{
	ulwp_t *self = curthread;
	uberdata_t *udp = self->ul_uberdata;
	queue_root_t *qrp;
	ulwp_t *ulwp;
	ulwp_t *prev;
	uint_t index;
	uint32_t cnt;
	char qtype;
	void *wchan;

	ASSERT(qp >= udp->queue_head && (qp - udp->queue_head) < 2 * QHASHSIZE);
	ASSERT(MUTEX_OWNED(&qp->qh_lock, self));
	for (cnt = 0, qrp = qp->qh_hlist; qrp != NULL; qrp = qrp->qr_next) {
		cnt++;
		ASSERT((qrp->qr_head != NULL && qrp->qr_tail != NULL) ||
		    (qrp->qr_head == NULL && qrp->qr_tail == NULL));
	}
	ASSERT(qp->qh_hlen == cnt && qp->qh_hmax >= cnt);
	qtype = ((qp - udp->queue_head) < QHASHSIZE)? MX : CV;
	ASSERT(qp->qh_type == qtype);
	if (!thread_queue_verify)
		return;
	/* real expensive stuff, only for _THREAD_QUEUE_VERIFY */
	for (cnt = 0, qrp = qp->qh_hlist; qrp != NULL; qrp = qrp->qr_next) {
		for (prev = NULL, ulwp = qrp->qr_head; ulwp != NULL;
		    prev = ulwp, ulwp = ulwp->ul_link) {
			cnt++;
			if (ulwp->ul_writer)
				ASSERT(prev == NULL || prev->ul_writer);
			ASSERT(ulwp->ul_qtype == qtype);
			ASSERT(ulwp->ul_wchan != NULL);
			ASSERT(ulwp->ul_sleepq == qp);
			wchan = ulwp->ul_wchan;
			ASSERT(qrp->qr_wchan == wchan);
			index = QUEUE_HASH(wchan, qtype);
			ASSERT(&udp->queue_head[index] == qp);
		}
		ASSERT(qrp->qr_tail == prev);
	}
	ASSERT(qp->qh_qlen == cnt);
}

#else	/* THREAD_DEBUG */

#define	QVERIFY(qp)

#endif	/* THREAD_DEBUG */

/*
 * Acquire a queue head.
 */
queue_head_t *
queue_lock(void *wchan, int qtype)
{
	uberdata_t *udp = curthread->ul_uberdata;
	queue_head_t *qp;
	queue_root_t *qrp;

	ASSERT(qtype == MX || qtype == CV);

	/*
	 * It is possible that we could be called while still single-threaded.
	 * If so, we call queue_alloc() to allocate the queue_head[] array.
	 */
	if ((qp = udp->queue_head) == NULL) {
		queue_alloc();
		qp = udp->queue_head;
	}
	qp += QUEUE_HASH(wchan, qtype);
	spin_lock_set(&qp->qh_lock);
	for (qrp = qp->qh_hlist; qrp != NULL; qrp = qrp->qr_next)
		if (qrp->qr_wchan == wchan)
			break;
	if (qrp == NULL && qp->qh_def_root.qr_head == NULL) {
		/* the default queue root is available; use it */
		qrp = &qp->qh_def_root;
		qrp->qr_wchan = wchan;
		ASSERT(qrp->qr_next == NULL);
		ASSERT(qrp->qr_tail == NULL &&
		    qrp->qr_rtcount == 0 && qrp->qr_qlen == 0);
	}
	qp->qh_wchan = wchan;	/* valid until queue_unlock() is called */
	qp->qh_root = qrp;	/* valid until queue_unlock() is called */
	INCR32(qp->qh_lockcount);
	QVERIFY(qp);
	return (qp);
}

/*
 * Release a queue head.
 */
void
queue_unlock(queue_head_t *qp)
{
	QVERIFY(qp);
	spin_lock_clear(&qp->qh_lock);
}

/*
 * For rwlock queueing, we must queue writers ahead of readers of the
 * same priority.  We do this by making writers appear to have a half
 * point higher priority for purposes of priority comparisons below.
 */
#define	CMP_PRIO(ulwp)	((real_priority(ulwp) << 1) + (ulwp)->ul_writer)

void
enqueue(queue_head_t *qp, ulwp_t *ulwp, int force_fifo)
{
	queue_root_t *qrp;
	ulwp_t **ulwpp;
	ulwp_t *next;
	int pri = CMP_PRIO(ulwp);

	ASSERT(MUTEX_OWNED(&qp->qh_lock, curthread));
	ASSERT(ulwp->ul_sleepq != qp);

	if ((qrp = qp->qh_root) == NULL) {
		/* use the thread's queue root for the linkage */
		qrp = &ulwp->ul_queue_root;
		qrp->qr_next = qp->qh_hlist;
		qrp->qr_prev = NULL;
		qrp->qr_head = NULL;
		qrp->qr_tail = NULL;
		qrp->qr_wchan = qp->qh_wchan;
		qrp->qr_rtcount = 0;
		qrp->qr_qlen = 0;
		qrp->qr_qmax = 0;
		qp->qh_hlist->qr_prev = qrp;
		qp->qh_hlist = qrp;
		qp->qh_root = qrp;
		MAXINCR(qp->qh_hmax, qp->qh_hlen);
	}

	/*
	 * LIFO queue ordering is unfair and can lead to starvation,
	 * but it gives better performance for heavily contended locks.
	 * We use thread_queue_fifo (range is 0..8) to determine
	 * the frequency of FIFO vs LIFO queuing:
	 *	0 : every 256th time	(almost always LIFO)
	 *	1 : every 128th time
	 *	2 : every 64th  time
	 *	3 : every 32nd  time
	 *	4 : every 16th  time	(the default value, mostly LIFO)
	 *	5 : every 8th   time
	 *	6 : every 4th   time
	 *	7 : every 2nd   time
	 *	8 : every time		(never LIFO, always FIFO)
	 * Note that there is always some degree of FIFO ordering.
	 * This breaks live lock conditions that occur in applications
	 * that are written assuming (incorrectly) that threads acquire
	 * locks fairly, that is, in roughly round-robin order.
	 * In any event, the queue is maintained in kernel priority order.
	 *
	 * If force_fifo is non-zero, fifo queueing is forced.
	 * SUSV3 requires this for semaphores.
	 */
	if (qrp->qr_head == NULL) {
		/*
		 * The queue is empty.  LIFO/FIFO doesn't matter.
		 */
		ASSERT(qrp->qr_tail == NULL);
		ulwpp = &qrp->qr_head;
	} else if (force_fifo |
	    (((++qp->qh_qcnt << curthread->ul_queue_fifo) & 0xff) == 0)) {
		/*
		 * Enqueue after the last thread whose priority is greater
		 * than or equal to the priority of the thread being queued.
		 * Attempt first to go directly onto the tail of the queue.
		 */
		if (pri <= CMP_PRIO(qrp->qr_tail))
			ulwpp = &qrp->qr_tail->ul_link;
		else {
			for (ulwpp = &qrp->qr_head; (next = *ulwpp) != NULL;
			    ulwpp = &next->ul_link)
				if (pri > CMP_PRIO(next))
					break;
		}
	} else {
		/*
		 * Enqueue before the first thread whose priority is less
		 * than or equal to the priority of the thread being queued.
		 * Hopefully we can go directly onto the head of the queue.
		 */
		for (ulwpp = &qrp->qr_head; (next = *ulwpp) != NULL;
		    ulwpp = &next->ul_link)
			if (pri >= CMP_PRIO(next))
				break;
	}
	if ((ulwp->ul_link = *ulwpp) == NULL)
		qrp->qr_tail = ulwp;
	*ulwpp = ulwp;

	ulwp->ul_sleepq = qp;
	ulwp->ul_wchan = qp->qh_wchan;
	ulwp->ul_qtype = qp->qh_type;
	if ((ulwp->ul_schedctl != NULL &&
	    ulwp->ul_schedctl->sc_cid == ulwp->ul_rtclassid) |
	    ulwp->ul_pilocks) {
		ulwp->ul_rtqueued = 1;
		qrp->qr_rtcount++;
	}
	MAXINCR(qrp->qr_qmax, qrp->qr_qlen);
	MAXINCR(qp->qh_qmax, qp->qh_qlen);
}

/*
 * Helper function for queue_slot() and queue_slot_rt().
 * Try to find a non-suspended thread on the queue.
 */
static ulwp_t **
queue_slot_runnable(ulwp_t **ulwpp, ulwp_t **prevp, int rt)
{
	ulwp_t *ulwp;
	ulwp_t **foundpp = NULL;
	int priority = -1;
	ulwp_t *prev;
	int tpri;

	for (prev = NULL;
	    (ulwp = *ulwpp) != NULL;
	    prev = ulwp, ulwpp = &ulwp->ul_link) {
		if (ulwp->ul_stop)	/* skip suspended threads */
			continue;
		tpri = rt? CMP_PRIO(ulwp) : 0;
		if (tpri > priority) {
			foundpp = ulwpp;
			*prevp = prev;
			priority = tpri;
			if (!rt)
				break;
		}
	}
	return (foundpp);
}

/*
 * For real-time, we search the entire queue because the dispatch
 * (kernel) priorities may have changed since enqueueing.
 */
static ulwp_t **
queue_slot_rt(ulwp_t **ulwpp_org, ulwp_t **prevp)
{
	ulwp_t **ulwpp = ulwpp_org;
	ulwp_t *ulwp = *ulwpp;
	ulwp_t **foundpp = ulwpp;
	int priority = CMP_PRIO(ulwp);
	ulwp_t *prev;
	int tpri;

	for (prev = ulwp, ulwpp = &ulwp->ul_link;
	    (ulwp = *ulwpp) != NULL;
	    prev = ulwp, ulwpp = &ulwp->ul_link) {
		tpri = CMP_PRIO(ulwp);
		if (tpri > priority) {
			foundpp = ulwpp;
			*prevp = prev;
			priority = tpri;
		}
	}
	ulwp = *foundpp;

	/*
	 * Try not to return a suspended thread.
	 * This mimics the old libthread's behavior.
	 */
	if (ulwp->ul_stop &&
	    (ulwpp = queue_slot_runnable(ulwpp_org, prevp, 1)) != NULL) {
		foundpp = ulwpp;
		ulwp = *foundpp;
	}
	ulwp->ul_rt = 1;
	return (foundpp);
}

ulwp_t **
queue_slot(queue_head_t *qp, ulwp_t **prevp, int *more)
{
	queue_root_t *qrp;
	ulwp_t **ulwpp;
	ulwp_t *ulwp;
	int rt;

	ASSERT(MUTEX_OWNED(&qp->qh_lock, curthread));

	if ((qrp = qp->qh_root) == NULL || (ulwp = qrp->qr_head) == NULL) {
		*more = 0;
		return (NULL);		/* no lwps on the queue */
	}
	rt = (qrp->qr_rtcount != 0);
	*prevp = NULL;
	if (ulwp->ul_link == NULL) {	/* only one lwp on the queue */
		*more = 0;
		ulwp->ul_rt = rt;
		return (&qrp->qr_head);
	}
	*more = 1;

	if (rt)		/* real-time queue */
		return (queue_slot_rt(&qrp->qr_head, prevp));
	/*
	 * Try not to return a suspended thread.
	 * This mimics the old libthread's behavior.
	 */
	if (ulwp->ul_stop &&
	    (ulwpp = queue_slot_runnable(&qrp->qr_head, prevp, 0)) != NULL) {
		ulwp = *ulwpp;
		ulwp->ul_rt = 0;
		return (ulwpp);
	}
	/*
	 * The common case; just pick the first thread on the queue.
	 */
	ulwp->ul_rt = 0;
	return (&qrp->qr_head);
}

/*
 * Common code for unlinking an lwp from a user-level sleep queue.
 */
void
queue_unlink(queue_head_t *qp, ulwp_t **ulwpp, ulwp_t *prev)
{
	queue_root_t *qrp = qp->qh_root;
	queue_root_t *nqrp;
	ulwp_t *ulwp = *ulwpp;
	ulwp_t *next;

	ASSERT(MUTEX_OWNED(&qp->qh_lock, curthread));
	ASSERT(qp->qh_wchan != NULL && ulwp->ul_wchan == qp->qh_wchan);

	DECR(qp->qh_qlen);
	DECR(qrp->qr_qlen);
	if (ulwp->ul_rtqueued) {
		ulwp->ul_rtqueued = 0;
		qrp->qr_rtcount--;
	}
	next = ulwp->ul_link;
	*ulwpp = next;
	ulwp->ul_link = NULL;
	if (qrp->qr_tail == ulwp)
		qrp->qr_tail = prev;
	if (qrp == &ulwp->ul_queue_root) {
		/*
		 * We can't continue to use the unlinked thread's
		 * queue root for the linkage.
		 */
		queue_root_t *qr_next = qrp->qr_next;
		queue_root_t *qr_prev = qrp->qr_prev;

		if (qrp->qr_tail) {
			/* switch to using the last thread's queue root */
			ASSERT(qrp->qr_qlen != 0);
			nqrp = &qrp->qr_tail->ul_queue_root;
			*nqrp = *qrp;
			if (qr_next)
				qr_next->qr_prev = nqrp;
			if (qr_prev)
				qr_prev->qr_next = nqrp;
			else
				qp->qh_hlist = nqrp;
			qp->qh_root = nqrp;
		} else {
			/* empty queue root; just delete from the hash list */
			ASSERT(qrp->qr_qlen == 0);
			if (qr_next)
				qr_next->qr_prev = qr_prev;
			if (qr_prev)
				qr_prev->qr_next = qr_next;
			else
				qp->qh_hlist = qr_next;
			qp->qh_root = NULL;
			DECR(qp->qh_hlen);
		}
	}
}

ulwp_t *
dequeue(queue_head_t *qp, int *more)
{
	ulwp_t **ulwpp;
	ulwp_t *ulwp;
	ulwp_t *prev;

	if ((ulwpp = queue_slot(qp, &prev, more)) == NULL)
		return (NULL);
	ulwp = *ulwpp;
	queue_unlink(qp, ulwpp, prev);
	ulwp->ul_sleepq = NULL;
	ulwp->ul_wchan = NULL;
	return (ulwp);
}

/*
 * Return a pointer to the highest priority thread sleeping on wchan.
 */
ulwp_t *
queue_waiter(queue_head_t *qp)
{
	ulwp_t **ulwpp;
	ulwp_t *prev;
	int more;

	if ((ulwpp = queue_slot(qp, &prev, &more)) == NULL)
		return (NULL);
	return (*ulwpp);
}

int
dequeue_self(queue_head_t *qp)
{
	ulwp_t *self = curthread;
	queue_root_t *qrp;
	ulwp_t **ulwpp;
	ulwp_t *ulwp;
	ulwp_t *prev;
	int found = 0;

	ASSERT(MUTEX_OWNED(&qp->qh_lock, self));

	/* find self on the sleep queue */
	if ((qrp = qp->qh_root) != NULL) {
		for (prev = NULL, ulwpp = &qrp->qr_head;
		    (ulwp = *ulwpp) != NULL;
		    prev = ulwp, ulwpp = &ulwp->ul_link) {
			if (ulwp == self) {
				queue_unlink(qp, ulwpp, prev);
				self->ul_cvmutex = NULL;
				self->ul_sleepq = NULL;
				self->ul_wchan = NULL;
				found = 1;
				break;
			}
		}
	}

	if (!found)
		thr_panic("dequeue_self(): curthread not found on queue");

	return ((qrp = qp->qh_root) != NULL && qrp->qr_head != NULL);
}

/*
 * Called from call_user_handler() and _thrp_suspend() to take
 * ourself off of our sleep queue so we can grab locks.
 */
void
unsleep_self(void)
{
	ulwp_t *self = curthread;
	queue_head_t *qp;

	/*
	 * Calling enter_critical()/exit_critical() here would lead
	 * to recursion.  Just manipulate self->ul_critical directly.
	 */
	self->ul_critical++;
	while (self->ul_sleepq != NULL) {
		qp = queue_lock(self->ul_wchan, self->ul_qtype);
		/*
		 * We may have been moved from a CV queue to a
		 * mutex queue while we were attempting queue_lock().
		 * If so, just loop around and try again.
		 * dequeue_self() clears self->ul_sleepq.
		 */
		if (qp == self->ul_sleepq)
			(void) dequeue_self(qp);
		queue_unlock(qp);
	}
	self->ul_writer = 0;
	self->ul_critical--;
}

/*
 * Common code for calling the the ___lwp_mutex_timedlock() system call.
 * Returns with mutex_owner and mutex_ownerpid set correctly.
 */
static int
mutex_lock_kernel(mutex_t *mp, timespec_t *tsp, tdb_mutex_stats_t *msp)
{
	ulwp_t *self = curthread;
	uberdata_t *udp = self->ul_uberdata;
	int mtype = mp->mutex_type;
	hrtime_t begin_sleep;
	int acquired;
	int error;

	self->ul_sp = stkptr();
	self->ul_wchan = mp;
	if (__td_event_report(self, TD_SLEEP, udp)) {
		self->ul_td_evbuf.eventnum = TD_SLEEP;
		self->ul_td_evbuf.eventdata = mp;
		tdb_event(TD_SLEEP, udp);
	}
	if (msp) {
		tdb_incr(msp->mutex_sleep);
		begin_sleep = gethrtime();
	}

	DTRACE_PROBE1(plockstat, mutex__block, mp);

	for (;;) {
		/*
		 * A return value of EOWNERDEAD or ELOCKUNMAPPED
		 * means we successfully acquired the lock.
		 */
		if ((error = ___lwp_mutex_timedlock(mp, tsp)) != 0 &&
		    error != EOWNERDEAD && error != ELOCKUNMAPPED) {
			acquired = 0;
			break;
		}

		if (mtype & USYNC_PROCESS) {
			/*
			 * Defend against forkall().  We may be the child,
			 * in which case we don't actually own the mutex.
			 */
			enter_critical(self);
			if (mp->mutex_ownerpid == udp->pid) {
				mp->mutex_owner = (uintptr_t)self;
				exit_critical(self);
				acquired = 1;
				break;
			}
			exit_critical(self);
		} else {
			mp->mutex_owner = (uintptr_t)self;
			acquired = 1;
			break;
		}
	}
	if (msp)
		msp->mutex_sleep_time += gethrtime() - begin_sleep;
	self->ul_wchan = NULL;
	self->ul_sp = 0;

	if (acquired) {
		DTRACE_PROBE2(plockstat, mutex__blocked, mp, 1);
		DTRACE_PROBE3(plockstat, mutex__acquire, mp, 0, 0);
	} else {
		DTRACE_PROBE2(plockstat, mutex__blocked, mp, 0);
		DTRACE_PROBE2(plockstat, mutex__error, mp, error);
	}

	return (error);
}

/*
 * Common code for calling the ___lwp_mutex_trylock() system call.
 * Returns with mutex_owner and mutex_ownerpid set correctly.
 */
int
mutex_trylock_kernel(mutex_t *mp)
{
	ulwp_t *self = curthread;
	uberdata_t *udp = self->ul_uberdata;
	int mtype = mp->mutex_type;
	int error;
	int acquired;

	for (;;) {
		/*
		 * A return value of EOWNERDEAD or ELOCKUNMAPPED
		 * means we successfully acquired the lock.
		 */
		if ((error = ___lwp_mutex_trylock(mp)) != 0 &&
		    error != EOWNERDEAD && error != ELOCKUNMAPPED) {
			acquired = 0;
			break;
		}

		if (mtype & USYNC_PROCESS) {
			/*
			 * Defend against forkall().  We may be the child,
			 * in which case we don't actually own the mutex.
			 */
			enter_critical(self);
			if (mp->mutex_ownerpid == udp->pid) {
				mp->mutex_owner = (uintptr_t)self;
				exit_critical(self);
				acquired = 1;
				break;
			}
			exit_critical(self);
		} else {
			mp->mutex_owner = (uintptr_t)self;
			acquired = 1;
			break;
		}
	}

	if (acquired) {
		DTRACE_PROBE3(plockstat, mutex__acquire, mp, 0, 0);
	} else if (error != EBUSY) {
		DTRACE_PROBE2(plockstat, mutex__error, mp, error);
	}

	return (error);
}

volatile sc_shared_t *
setup_schedctl(void)
{
	ulwp_t *self = curthread;
	volatile