diff --git a/sys/kern/kern_condvar.c b/sys/kern/kern_condvar.c
index dc7002997f25..19ae63a2a057 100644
--- a/sys/kern/kern_condvar.c
+++ b/sys/kern/kern_condvar.c
@@ -1,4 +1,4 @@
-/*	$NetBSD: kern_condvar.c,v 1.3 2007/02/11 15:41:53 yamt Exp $	*/
+/*	$NetBSD: kern_condvar.c,v 1.4 2007/02/26 09:20:52 yamt Exp $	*/
 
 /*-
  * Copyright (c) 2006, 2007 The NetBSD Foundation, Inc.
@@ -45,7 +45,7 @@
  */
 
 #include <sys/cdefs.h>
-__KERNEL_RCSID(0, "$NetBSD: kern_condvar.c,v 1.3 2007/02/11 15:41:53 yamt Exp $");
+__KERNEL_RCSID(0, "$NetBSD: kern_condvar.c,v 1.4 2007/02/26 09:20:52 yamt Exp $");
 
 #include <sys/param.h>
 #include <sys/proc.h>
@@ -61,6 +61,8 @@ syncobj_t cv_syncobj = {
 	SOBJ_SLEEPQ_SORTED,
 	cv_unsleep,
 	cv_changepri,
+	sleepq_lendpri,
+	syncobj_noowner,
 };
 
 /*
@@ -149,13 +151,13 @@ cv_changepri(struct lwp *l, int pri)
 
 	KASSERT(lwp_locked(l, sq->sq_mutex));
 
-	opri = l->l_priority;
+	opri = lwp_eprio(l);
 	l->l_usrpri = pri;
 	l->l_priority = sched_kpri(l);
 
-	if (l->l_priority != opri) {
+	if (lwp_eprio(l) != opri) {
 		TAILQ_REMOVE(&sq->sq_queue, l, l_sleepchain);
-		sleepq_insert(sq, l, pri, l->l_syncobj);
+		sleepq_insert(sq, l, l->l_syncobj);
 	}
 }
 
diff --git a/sys/kern/kern_ktrace.c b/sys/kern/kern_ktrace.c
index d8bd2aaf5233..d5d1e1e4c814 100644
--- a/sys/kern/kern_ktrace.c
+++ b/sys/kern/kern_ktrace.c
@@ -1,4 +1,4 @@
-/*	$NetBSD: kern_ktrace.c,v 1.116 2007/02/22 06:34:43 thorpej Exp $	*/
+/*	$NetBSD: kern_ktrace.c,v 1.117 2007/02/26 09:20:53 yamt Exp $	*/
 
 /*
  * Copyright (c) 1989, 1993
@@ -32,7 +32,7 @@
  */
 
 #include <sys/cdefs.h>
-__KERNEL_RCSID(0, "$NetBSD: kern_ktrace.c,v 1.116 2007/02/22 06:34:43 thorpej Exp $");
+__KERNEL_RCSID(0, "$NetBSD: kern_ktrace.c,v 1.117 2007/02/26 09:20:53 yamt Exp $");
 
 #include "opt_ktrace.h"
 #include "opt_compat_mach.h"
@@ -688,7 +688,7 @@ ktrcsw(struct lwp *l, int out, int user)
 	 * Don't record context switches resulting from blocking on 
 	 * locks; it's too easy to get duff results.
 	 */
-	if (l->l_syncobj == &turnstile_syncobj)
+	if (l->l_syncobj == &mutex_syncobj || l->l_syncobj == &rw_syncobj)
 		return;
 
 	/*
diff --git a/sys/kern/kern_lwp.c b/sys/kern/kern_lwp.c
index b39ccb689739..98020067f333 100644
--- a/sys/kern/kern_lwp.c
+++ b/sys/kern/kern_lwp.c
@@ -1,4 +1,4 @@
-/*	$NetBSD: kern_lwp.c,v 1.59 2007/02/21 23:48:13 thorpej Exp $	*/
+/*	$NetBSD: kern_lwp.c,v 1.60 2007/02/26 09:20:53 yamt Exp $	*/
 
 /*-
  * Copyright (c) 2001, 2006, 2007 The NetBSD Foundation, Inc.
@@ -204,7 +204,7 @@
  */
 
 #include <sys/cdefs.h>
-__KERNEL_RCSID(0, "$NetBSD: kern_lwp.c,v 1.59 2007/02/21 23:48:13 thorpej Exp $");
+__KERNEL_RCSID(0, "$NetBSD: kern_lwp.c,v 1.60 2007/02/26 09:20:53 yamt Exp $");
 
 #include "opt_multiprocessor.h"
 #include "opt_lockdebug.h"
@@ -504,9 +504,12 @@ newlwp(struct lwp *l1, struct proc *p2, vaddr_t uaddr, bool inmem,
 		l2 = pool_get(&lwp_pool, PR_WAITOK);
 		memset(l2, 0, sizeof(*l2));
 		l2->l_ts = pool_cache_get(&turnstile_cache, PR_WAITOK);
+		SLIST_INIT(&l2->l_pi_lenders);
 	} else {
 		l2 = isfree;
 		ts = l2->l_ts;
+		KASSERT(l2->l_inheritedprio == MAXPRI);
+		KASSERT(SLIST_EMPTY(&l2->l_pi_lenders));
 		memset(l2, 0, sizeof(*l2));
 		l2->l_ts = ts;
 	}
@@ -516,6 +519,7 @@ newlwp(struct lwp *l1, struct proc *p2, vaddr_t uaddr, bool inmem,
 	l2->l_refcnt = 1;
 	l2->l_priority = l1->l_priority;
 	l2->l_usrpri = l1->l_usrpri;
+	l2->l_inheritedprio = MAXPRI;
 	l2->l_mutex = &sched_mutex;
 	l2->l_cpu = l1->l_cpu;
 	l2->l_flag = inmem ? LW_INMEM : 0;
@@ -782,6 +786,8 @@ lwp_free(struct lwp *l, int recycle, int last)
 	cpu_lwp_free2(l);
 #endif
 	uvm_lwp_exit(l);
+	KASSERT(SLIST_EMPTY(&l->l_pi_lenders));
+	KASSERT(l->l_inheritedprio == MAXPRI);
 	if (!recycle)
 		pool_put(&lwp_pool, l);
 	KERNEL_UNLOCK_ONE(curlwp);	/* XXXSMP */
@@ -1065,6 +1071,24 @@ lwp_relock(struct lwp *l, kmutex_t *new)
 #endif
 }
 
+int
+lwp_trylock(struct lwp *l)
+{
+#if defined(MULTIPROCESSOR) || defined(LOCKDEBUG)
+	kmutex_t *old;
+
+	for (;;) {
+		if (!mutex_tryenter(old = l->l_mutex))
+			return 0;
+		if (__predict_true(l->l_mutex == old))
+			return 1;
+		mutex_spin_exit(old);
+	}
+#else
+	return mutex_tryenter(l->l_mutex);
+#endif
+}
+
 /*
  * Handle exceptions for mi_userret().  Called if a member of LW_USERRET is
  * set.
diff --git a/sys/kern/kern_mutex.c b/sys/kern/kern_mutex.c
index 6930422efd2e..49b81f31aaf7 100644
--- a/sys/kern/kern_mutex.c
+++ b/sys/kern/kern_mutex.c
@@ -1,4 +1,4 @@
-/*	$NetBSD: kern_mutex.c,v 1.4 2007/02/15 15:49:27 ad Exp $	*/
+/*	$NetBSD: kern_mutex.c,v 1.5 2007/02/26 09:20:53 yamt Exp $	*/
 
 /*-
  * Copyright (c) 2002, 2006, 2007 The NetBSD Foundation, Inc.
@@ -49,7 +49,7 @@
 #define	__MUTEX_PRIVATE
 
 #include <sys/cdefs.h>
-__KERNEL_RCSID(0, "$NetBSD: kern_mutex.c,v 1.4 2007/02/15 15:49:27 ad Exp $");
+__KERNEL_RCSID(0, "$NetBSD: kern_mutex.c,v 1.5 2007/02/26 09:20:53 yamt Exp $");
 
 #include <sys/param.h>
 #include <sys/proc.h>
@@ -236,6 +236,7 @@ __strong_alias(mutex_spin_exit, mutex_vector_exit);
 void	mutex_abort(kmutex_t *, const char *, const char *);
 void	mutex_dump(volatile void *);
 int	mutex_onproc(uintptr_t, struct cpu_info **);
+static struct lwp *mutex_getowner(wchan_t); /* XXX naming conflict */
 
 lockops_t mutex_spin_lockops = {
 	"Mutex",
@@ -249,6 +250,14 @@ lockops_t mutex_adaptive_lockops = {
 	mutex_dump
 };
 
+syncobj_t mutex_syncobj = {
+	SOBJ_SLEEPQ_SORTED,
+	turnstile_unsleep,
+	turnstile_changepri,
+	sleepq_lendpri,
+	mutex_getowner,
+};
+
 /*
  * mutex_dump:
  *
@@ -637,7 +646,7 @@ mutex_vector_enter(kmutex_t *mtx)
 
 		LOCKSTAT_START_TIMER(lsflag, slptime);
 
-		turnstile_block(ts, TS_WRITER_Q, mtx);
+		turnstile_block(ts, TS_WRITER_Q, mtx, &mutex_syncobj);
 
 		LOCKSTAT_STOP_TIMER(lsflag, slptime);
 		LOCKSTAT_COUNT(slpcnt, 1);
@@ -761,6 +770,14 @@ mutex_owner(kmutex_t *mtx)
 	return (struct lwp *)MUTEX_OWNER(mtx->mtx_owner);
 }
 
+static struct lwp *
+mutex_getowner(wchan_t obj)
+{
+	kmutex_t *mtx = (void *)(uintptr_t)obj; /* discard qualifiers */
+
+	return mutex_owner(mtx);
+}
+
 /*
  * mutex_tryenter:
  *
diff --git a/sys/kern/kern_proc.c b/sys/kern/kern_proc.c
index 7cf4d3ad5bc7..ee0d2e710b70 100644
--- a/sys/kern/kern_proc.c
+++ b/sys/kern/kern_proc.c
@@ -1,4 +1,4 @@
-/*	$NetBSD: kern_proc.c,v 1.104 2007/02/21 23:48:14 thorpej Exp $	*/
+/*	$NetBSD: kern_proc.c,v 1.105 2007/02/26 09:20:53 yamt Exp $	*/
 
 /*-
  * Copyright (c) 1999, 2006, 2007 The NetBSD Foundation, Inc.
@@ -69,7 +69,7 @@
  */
 
 #include <sys/cdefs.h>
-__KERNEL_RCSID(0, "$NetBSD: kern_proc.c,v 1.104 2007/02/21 23:48:14 thorpej Exp $");
+__KERNEL_RCSID(0, "$NetBSD: kern_proc.c,v 1.105 2007/02/26 09:20:53 yamt Exp $");
 
 #include "opt_kstack.h"
 #include "opt_maxuprc.h"
@@ -355,6 +355,8 @@ proc0_init(void)
 	l->l_cpu = curcpu();
 	l->l_priority = PRIBIO;
 	l->l_usrpri = PRIBIO;
+	l->l_inheritedprio = MAXPRI;
+	SLIST_INIT(&l->l_pi_lenders);
 
 	callout_init(&l->l_tsleep_ch);
 	cv_init(&l->l_sigcv, "sigwait");
diff --git a/sys/kern/kern_rwlock.c b/sys/kern/kern_rwlock.c
index 2732eaa53014..df92cdb0cc4a 100644
--- a/sys/kern/kern_rwlock.c
+++ b/sys/kern/kern_rwlock.c
@@ -1,4 +1,4 @@
-/*	$NetBSD: kern_rwlock.c,v 1.3 2007/02/10 21:07:52 ad Exp $	*/
+/*	$NetBSD: kern_rwlock.c,v 1.4 2007/02/26 09:20:53 yamt Exp $	*/
 
 /*-
  * Copyright (c) 2002, 2006, 2007 The NetBSD Foundation, Inc.
@@ -47,7 +47,7 @@
 #include "opt_multiprocessor.h"
 
 #include <sys/cdefs.h>
-__KERNEL_RCSID(0, "$NetBSD: kern_rwlock.c,v 1.3 2007/02/10 21:07:52 ad Exp $");
+__KERNEL_RCSID(0, "$NetBSD: kern_rwlock.c,v 1.4 2007/02/26 09:20:53 yamt Exp $");
 
 #define	__RWLOCK_PRIVATE
 
@@ -145,6 +145,7 @@ __strong_alias(rw_exit, rw_vector_exit);
 #endif
 
 void	rw_dump(volatile void *);
+static struct lwp *rw_owner(wchan_t);
 
 lockops_t rwlock_lockops = {
 	"Reader / writer lock",
@@ -152,6 +153,14 @@ lockops_t rwlock_lockops = {
 	rw_dump
 };
 
+syncobj_t rw_syncobj = {
+	SOBJ_SLEEPQ_SORTED,
+	turnstile_unsleep,
+	turnstile_changepri,
+	sleepq_lendpri,
+	rw_owner,
+};
+
 /*
  * rw_dump:
  *
@@ -299,7 +308,7 @@ rw_vector_enter(krwlock_t *rw, const krw_t op)
 
 		LOCKSTAT_START_TIMER(lsflag, slptime);
 
-		turnstile_block(ts, queue, rw);
+		turnstile_block(ts, queue, rw, &rw_syncobj);
 
 		/* If we wake up and arrive here, we've been handed the lock. */
 		RW_RECEIVE(rw);
@@ -670,3 +679,15 @@ rw_lock_held(krwlock_t *rw)
 
 	return (rw->rw_owner & RW_THREAD) != 0;
 }
+
+static struct lwp *
+rw_owner(wchan_t obj)
+{
+	krwlock_t *rw = (void *)(uintptr_t)obj; /* discard qualifiers */
+	uintptr_t owner = rw->rw_owner;
+
+	if ((owner & RW_WRITE_LOCKED) == 0)
+		return NULL;
+
+	return (void *)(owner & RW_THREAD);
+}
diff --git a/sys/kern/kern_sleepq.c b/sys/kern/kern_sleepq.c
index 850af97d1072..f3b0b4325e22 100644
--- a/sys/kern/kern_sleepq.c
+++ b/sys/kern/kern_sleepq.c
@@ -1,4 +1,4 @@
-/*	$NetBSD: kern_sleepq.c,v 1.5 2007/02/17 22:31:43 pavel Exp $	*/
+/*	$NetBSD: kern_sleepq.c,v 1.6 2007/02/26 09:20:53 yamt Exp $	*/
 
 /*-
  * Copyright (c) 2006, 2007 The NetBSD Foundation, Inc.
@@ -42,7 +42,7 @@
  */
 
 #include <sys/cdefs.h>
-__KERNEL_RCSID(0, "$NetBSD: kern_sleepq.c,v 1.5 2007/02/17 22:31:43 pavel Exp $");
+__KERNEL_RCSID(0, "$NetBSD: kern_sleepq.c,v 1.6 2007/02/26 09:20:53 yamt Exp $");
 
 #include "opt_multiprocessor.h"
 #include "opt_lockdebug.h"
@@ -177,7 +177,7 @@ sleepq_remove(sleepq_t *sq, struct lwp *l)
 	l->l_slptime = 0;
 	if ((l->l_flag & LW_INMEM) != 0) {
 		setrunqueue(l);
-		if (l->l_priority < ci->ci_schedstate.spc_curpriority)
+		if (lwp_eprio(l) < ci->ci_schedstate.spc_curpriority)
 			cpu_need_resched(ci);
 		sched_unlock(1);
 		return 0;
@@ -193,13 +193,14 @@ sleepq_remove(sleepq_t *sq, struct lwp *l)
  *	Insert an LWP into the sleep queue, optionally sorting by priority.
  */
 inline void
-sleepq_insert(sleepq_t *sq, struct lwp *l, int pri, syncobj_t *sobj)
+sleepq_insert(sleepq_t *sq, struct lwp *l, syncobj_t *sobj)
 {
 	struct lwp *l2;
+	const int pri = lwp_eprio(l);
 
 	if ((sobj->sobj_flag & SOBJ_SLEEPQ_SORTED) != 0) {
 		TAILQ_FOREACH(l2, &sq->sq_queue, l_sleepchain) {
-			if (l2->l_priority > pri) {
+			if (lwp_eprio(l2) > pri) {
 				TAILQ_INSERT_BEFORE(l2, l, l_sleepchain);
 				return;
 			}
@@ -209,19 +210,9 @@ sleepq_insert(sleepq_t *sq, struct lwp *l, int pri, syncobj_t *sobj)
 	TAILQ_INSERT_TAIL(&sq->sq_queue, l, l_sleepchain);
 }
 
-/*
- * sleepq_block:
- *
- *	Enter an LWP into the sleep queue and prepare for sleep.  The sleep
- *	queue must already be locked, and any interlock (such as the kernel
- *	lock) must have be released (see sleeptab_lookup(), sleepq_enter()).
- *
- * 	sleepq_block() may return early under exceptional conditions, for
- * 	example if the LWP's containing process is exiting.
- */
 void
-sleepq_block(sleepq_t *sq, int pri, wchan_t wchan, const char *wmesg, int timo,
-	     int catch, syncobj_t *sobj)
+sleepq_enqueue(sleepq_t *sq, int pri, wchan_t wchan, const char *wmesg,
+    syncobj_t *sobj)
 {
 	struct lwp *l = curlwp;
 
@@ -240,7 +231,13 @@ sleepq_block(sleepq_t *sq, int pri, wchan_t wchan, const char *wmesg, int timo,
 	l->l_nvcsw++;
 
 	sq->sq_waiters++;
-	sleepq_insert(sq, l, pri, sobj);
+	sleepq_insert(sq, l, sobj);
+}
+
+void
+sleepq_switch(int timo, int catch)
+{
+	struct lwp *l = curlwp;
 
 #ifdef KTRACE
 	if (KTRPOINT(l->l_proc, KTR_CSW))
@@ -280,6 +277,25 @@ sleepq_block(sleepq_t *sq, int pri, wchan_t wchan, const char *wmesg, int timo,
 	KASSERT(l->l_wchan == NULL && l->l_sleepq == NULL);
 }
 
+/*
+ * sleepq_block:
+ *
+ *	Enter an LWP into the sleep queue and prepare for sleep.  The sleep
+ *	queue must already be locked, and any interlock (such as the kernel
+ *	lock) must have be released (see sleeptab_lookup(), sleepq_enter()).
+ *
+ * 	sleepq_block() may return early under exceptional conditions, for
+ * 	example if the LWP's containing process is exiting.
+ */
+void
+sleepq_block(sleepq_t *sq, int pri, wchan_t wchan, const char *wmesg, int timo,
+	     int catch, syncobj_t *sobj)
+{
+
+	sleepq_enqueue(sq, pri, wchan, wmesg, sobj);
+	sleepq_switch(timo, catch);
+}
+
 /*
  * sleepq_unblock:
  *
@@ -484,3 +500,21 @@ sleepq_changepri(struct lwp *l, int pri)
 	KASSERT(lwp_locked(l, l->l_sleepq->sq_mutex));
 	l->l_usrpri = pri;
 }
+
+void
+sleepq_lendpri(struct lwp *l, int pri)
+{
+	sleepq_t *sq = l->l_sleepq;
+	int opri;
+
+	KASSERT(lwp_locked(l, sq->sq_mutex));
+
+	opri = lwp_eprio(l);
+	l->l_inheritedprio = pri;
+
+	if (lwp_eprio(l) != opri &&
+	    (l->l_syncobj->sobj_flag & SOBJ_SLEEPQ_SORTED) != 0) {
+		TAILQ_REMOVE(&sq->sq_queue, l, l_sleepchain);
+		sleepq_insert(sq, l, l->l_syncobj);
+	}
+}
diff --git a/sys/kern/kern_synch.c b/sys/kern/kern_synch.c
index 9b654f5e9fed..dc1cdd752c75 100644
--- a/sys/kern/kern_synch.c
+++ b/sys/kern/kern_synch.c
@@ -1,4 +1,4 @@
-/*	$NetBSD: kern_synch.c,v 1.183 2007/02/23 16:51:47 ad Exp $	*/
+/*	$NetBSD: kern_synch.c,v 1.184 2007/02/26 09:20:53 yamt Exp $	*/
 
 /*-
  * Copyright (c) 1999, 2000, 2004, 2006, 2007 The NetBSD Foundation, Inc.
@@ -74,7 +74,7 @@
  */
 
 #include <sys/cdefs.h>
-__KERNEL_RCSID(0, "$NetBSD: kern_synch.c,v 1.183 2007/02/23 16:51:47 ad Exp $");
+__KERNEL_RCSID(0, "$NetBSD: kern_synch.c,v 1.184 2007/02/26 09:20:53 yamt Exp $");
 
 #include "opt_ddb.h"
 #include "opt_kstack.h"
@@ -120,6 +120,7 @@ void	updatepri(struct lwp *);
 
 void	sched_unsleep(struct lwp *);
 void	sched_changepri(struct lwp *, int);
+void	sched_lendpri(struct lwp *, int);
 
 struct callout schedcpu_ch = CALLOUT_INITIALIZER_SETFUNC(schedcpu, NULL);
 static unsigned int schedcpu_ticks;
@@ -127,13 +128,17 @@ static unsigned int schedcpu_ticks;
 syncobj_t sleep_syncobj = {
 	SOBJ_SLEEPQ_SORTED,
 	sleepq_unsleep,
-	sleepq_changepri
+	sleepq_changepri,
+	sleepq_lendpri,
+	syncobj_noowner,
 };
 
 syncobj_t sched_syncobj = {
 	SOBJ_SLEEPQ_SORTED,
 	sched_unsleep,
-	sched_changepri
+	sched_changepri,
+	sched_lendpri,
+	syncobj_noowner,
 };
 
 /*
@@ -743,9 +748,10 @@ rqinit()
 }
 
 static inline void
-resched_lwp(struct lwp *l, u_char pri)
+resched_lwp(struct lwp *l)
 {
 	struct cpu_info *ci;
+	const int pri = lwp_eprio(l);
 
 	/*
 	 * XXXSMP
@@ -857,7 +863,7 @@ setrunnable(struct lwp *l)
 
 	if (l->l_flag & LW_INMEM) {
 		setrunqueue(l);
-		resched_lwp(l, l->l_priority);
+		resched_lwp(l);
 		lwp_unlock(l);
 	} else {
 		lwp_unlock(l);
@@ -1108,11 +1114,10 @@ sched_changepri(struct lwp *l, int pri)
 	LOCK_ASSERT(lwp_locked(l, &sched_mutex));
 
 	l->l_usrpri = pri;
-
 	if (l->l_priority < PUSER)
 		return;
-	if (l->l_stat != LSRUN || (l->l_flag & LW_INMEM) == 0 ||
-	    (l->l_priority / PPQ) == (pri / PPQ)) {
+
+	if (l->l_stat != LSRUN || (l->l_flag & LW_INMEM) == 0) {
 		l->l_priority = pri;
 		return;
 	}
@@ -1120,7 +1125,31 @@ sched_changepri(struct lwp *l, int pri)
 	remrunqueue(l);
 	l->l_priority = pri;
 	setrunqueue(l);
-	resched_lwp(l, pri);
+	resched_lwp(l);
+}
+
+void
+sched_lendpri(struct lwp *l, int pri)
+{
+
+	LOCK_ASSERT(lwp_locked(l, &sched_mutex));
+
+	if (l->l_stat != LSRUN || (l->l_flag & LW_INMEM) == 0) {
+		l->l_inheritedprio = pri;
+		return;
+	}
+
+	remrunqueue(l);
+	l->l_inheritedprio = pri;
+	setrunqueue(l);
+	resched_lwp(l);
+}
+
+struct lwp *
+syncobj_noowner(wchan_t wchan)
+{
+
+	return NULL;
 }
 
 /*
@@ -1212,7 +1241,7 @@ setrunqueue(struct lwp *l)
 {
 	struct prochd *rq;
 	struct lwp *prev;
-	const int whichq = l->l_priority / PPQ;
+	const int whichq = lwp_eprio(l) / PPQ;
 
 	LOCK_ASSERT(lwp_locked(l, &sched_mutex));
 
@@ -1245,7 +1274,7 @@ void
 remrunqueue(struct lwp *l)
 {
 	struct lwp *prev, *next;
-	const int whichq = l->l_priority / PPQ;
+	const int whichq = lwp_eprio(l) / PPQ;
 
 	LOCK_ASSERT(lwp_locked(l, &sched_mutex));
 
diff --git a/sys/kern/kern_turnstile.c b/sys/kern/kern_turnstile.c
index 6cdcef276c15..7aa39da09adf 100644
--- a/sys/kern/kern_turnstile.c
+++ b/sys/kern/kern_turnstile.c
@@ -1,4 +1,4 @@
-/*	$NetBSD: kern_turnstile.c,v 1.3 2007/02/15 20:21:13 ad Exp $	*/
+/*	$NetBSD: kern_turnstile.c,v 1.4 2007/02/26 09:20:53 yamt Exp $	*/
 
 /*-
  * Copyright (c) 2002, 2006, 2007 The NetBSD Foundation, Inc.
@@ -63,12 +63,11 @@
  * grabs a free turnstile off the free list.  Otherwise, it can take back
  * the active turnstile from the lock (thus deactivating the turnstile).
  *
- * Turnstiles are the place to do priority inheritence.  However, we do
- * not currently implement that.
+ * Turnstiles are the place to do priority inheritence.
  */
 
 #include <sys/cdefs.h>
-__KERNEL_RCSID(0, "$NetBSD: kern_turnstile.c,v 1.3 2007/02/15 20:21:13 ad Exp $");
+__KERNEL_RCSID(0, "$NetBSD: kern_turnstile.c,v 1.4 2007/02/26 09:20:53 yamt Exp $");
 
 #include "opt_lockdebug.h"
 #include "opt_multiprocessor.h"
@@ -77,6 +76,7 @@ __KERNEL_RCSID(0, "$NetBSD: kern_turnstile.c,v 1.3 2007/02/15 20:21:13 ad Exp $"
 
 #include <sys/param.h>
 #include <sys/lock.h>
+#include <sys/lockdebug.h>
 #include <sys/pool.h>
 #include <sys/proc.h> 
 #include <sys/sleepq.h>
@@ -94,17 +94,9 @@ struct pool turnstile_pool;
 struct pool_cache turnstile_cache;
 
 int	turnstile_ctor(void *, void *, int);
-void	turnstile_unsleep(struct lwp *);
-void	turnstile_changepri(struct lwp *, int);
 
 extern turnstile_t turnstile0;
 
-syncobj_t turnstile_syncobj = {
-	SOBJ_SLEEPQ_FIFO,
-	turnstile_unsleep,
-	turnstile_changepri
-};
-
 /*
  * turnstile_init:
  *
@@ -231,15 +223,18 @@ turnstile_exit(wchan_t obj)
  *	 LWP for sleep.
  */
 void
-turnstile_block(turnstile_t *ts, int q, wchan_t obj)
+turnstile_block(turnstile_t *ts, int q, wchan_t obj, syncobj_t *sobj)
 {
 	struct lwp *l;
+	struct lwp *cur; /* cached curlwp */
+	struct lwp *owner;
 	turnstile_t *ots;
 	tschain_t *tc;
 	sleepq_t *sq;
+	u_char prio;
 
 	tc = &turnstile_tab[TS_HASH(obj)];
-	l = curlwp;
+	l = cur = curlwp;
 
 	KASSERT(q == TS_READER_Q || q == TS_WRITER_Q);
 	KASSERT(mutex_owned(tc->tc_mutex));
@@ -255,6 +250,7 @@ turnstile_block(turnstile_t *ts, int q, wchan_t obj)
 		KASSERT(TAILQ_EMPTY(&ts->ts_sleepq[TS_READER_Q].sq_queue) &&
 			TAILQ_EMPTY(&ts->ts_sleepq[TS_WRITER_Q].sq_queue));
 		ts->ts_obj = obj;
+		ts->ts_inheritor = NULL;
 		ts->ts_sleepq[TS_READER_Q].sq_mutex = tc->tc_mutex;
 		ts->ts_sleepq[TS_WRITER_Q].sq_mutex = tc->tc_mutex;
 		LIST_INSERT_HEAD(&tc->tc_chain, ts, ts_chain);
@@ -269,6 +265,7 @@ turnstile_block(turnstile_t *ts, int q, wchan_t obj)
 		ts->ts_free = ots;
 		l->l_ts = ts;
 
+		KASSERT(ts->ts_obj == obj);
 		KASSERT(TS_ALL_WAITERS(ts) != 0);
 		KASSERT(!TAILQ_EMPTY(&ts->ts_sleepq[TS_READER_Q].sq_queue) ||
 			!TAILQ_EMPTY(&ts->ts_sleepq[TS_WRITER_Q].sq_queue));
@@ -276,8 +273,69 @@ turnstile_block(turnstile_t *ts, int q, wchan_t obj)
 
 	sq = &ts->ts_sleepq[q];
 	sleepq_enter(sq, l);
-	sleepq_block(sq, sched_kpri(l), obj, "tstile", 0, 0,
-	    &turnstile_syncobj);
+	LOCKDEBUG_BARRIER(tc->tc_mutex, 1);
+	prio = lwp_eprio(l);
+	sleepq_enqueue(sq, prio, obj, "tstile", sobj);
+
+	/*
+	 * lend our priority to lwps on the blocking chain.
+	 */
+
+	for (;;) {
+		bool dolock;
+
+		if (l->l_wchan == NULL)
+			break;
+
+		owner = (*l->l_syncobj->sobj_owner)(l->l_wchan);
+		if (owner == NULL)
+			break;
+
+		KASSERT(l != owner);
+		KASSERT(cur != owner);
+
+		if (l->l_mutex != owner->l_mutex)
+			dolock = true;
+		else
+			dolock = false;
+		if (dolock && !lwp_trylock(owner)) {
+			/*
+			 * restart from curlwp.
+			 */
+			lwp_unlock(l);
+			l = cur;
+			lwp_lock(l);
+			prio = lwp_eprio(l);
+			continue;
+		}
+		if (prio >= lwp_eprio(owner)) {
+			if (dolock)
+				lwp_unlock(owner);
+			break;
+		}
+		ts = l->l_ts;
+		KASSERT(ts->ts_inheritor == owner || ts->ts_inheritor == NULL);
+		if (ts->ts_inheritor == NULL) {
+			ts->ts_inheritor = owner;
+			ts->ts_eprio = prio;
+			SLIST_INSERT_HEAD(&owner->l_pi_lenders, ts, ts_pichain);
+			lwp_lendpri(owner, prio);
+		} else if (prio < ts->ts_eprio) {
+			ts->ts_eprio = prio;
+			lwp_lendpri(owner, prio);
+		}
+		if (dolock)
+			lwp_unlock(l);
+		l = owner;
+	}
+	LOCKDEBUG_BARRIER(l->l_mutex, 1);
+	if (cur->l_mutex != l->l_mutex) {
+		lwp_unlock(l);
+		lwp_lock(cur);
+	}
+	LOCKDEBUG_BARRIER(cur->l_mutex, 1);
+
+	sleepq_switch(0, 0);
 }
 
 /*
@@ -301,6 +359,52 @@ turnstile_wakeup(turnstile_t *ts, int q, int count, struct lwp *nl)
 	KASSERT(q == TS_READER_Q || q == TS_WRITER_Q);
 	KASSERT(count > 0 && count <= TS_WAITERS(ts, q));
 	KASSERT(mutex_owned(tc->tc_mutex) && sq->sq_mutex == tc->tc_mutex);
+	KASSERT(ts->ts_inheritor == curlwp || ts->ts_inheritor == NULL);
+
+	/*
+	 * restore inherited priority if necessary.
+	 */
+
+	if (ts->ts_inheritor != NULL) {
+		turnstile_t *iter;
+		turnstile_t *next;
+		turnstile_t *prev = NULL;
+		u_char prio;
+
+		ts->ts_inheritor = NULL;
+		l = curlwp;
+		lwp_lock(l);
+
+		/*
+		 * the following loop does two things.
+		 *
+		 * - remove ts from the list.
+		 *
+		 * - from the rest of the list, find the highest priority.
+		 */
+
+		prio = MAXPRI;
+		KASSERT(!SLIST_EMPTY(&l->l_pi_lenders));
+		for (iter = SLIST_FIRST(&l->l_pi_lenders);
+		    iter != NULL; iter = next) {
+			KASSERT(lwp_eprio(l) <= ts->ts_eprio);
+			next = SLIST_NEXT(iter, ts_pichain);
+			if (iter == ts) {
+				if (prev == NULL) {
+					SLIST_REMOVE_HEAD(&l->l_pi_lenders,
+					    ts_pichain);
+				} else {
+					SLIST_REMOVE_AFTER(prev, ts_pichain);
+				}
+			} else if (prio > iter->ts_eprio) {
+				prio = iter->ts_eprio;
+			}
+			prev = iter;
+		}
+
+		lwp_lendpri(l, prio);
+		lwp_unlock(l);
+	}
 
 	if (nl != NULL) {
 #if defined(DEBUG) || defined(LOCKDEBUG)
@@ -348,16 +452,14 @@ turnstile_unsleep(struct lwp *l)
 /*
  * turnstile_changepri:
  *
- *	Adjust the priority of an LWP residing on a turnstile.  Since we do
- *	not yet do priority inheritance, we mostly ignore this action.
+ *	Adjust the priority of an LWP residing on a turnstile.
  */
 void
 turnstile_changepri(struct lwp *l, int pri)
 {
 
-	/* LWPs on turnstiles always have kernel priority. */
-	l->l_usrpri = pri;
-	l->l_priority = sched_kpri(l);
+	/* XXX priority inheritance */
+	sleepq_changepri(l, pri);
 }
 
 #if defined(LOCKDEBUG)
diff --git a/sys/kern/sys_lwp.c b/sys/kern/sys_lwp.c
index c27e5960da09..5464cb8aa5fd 100644
--- a/sys/kern/sys_lwp.c
+++ b/sys/kern/sys_lwp.c
@@ -1,4 +1,4 @@
-/*	$NetBSD: sys_lwp.c,v 1.6 2007/02/21 23:48:15 thorpej Exp $	*/
+/*	$NetBSD: sys_lwp.c,v 1.7 2007/02/26 09:20:54 yamt Exp $	*/
 
 /*-
  * Copyright (c) 2001, 2006, 2007 The NetBSD Foundation, Inc.
@@ -42,7 +42,7 @@
  */
 
 #include <sys/cdefs.h>
-__KERNEL_RCSID(0, "$NetBSD: sys_lwp.c,v 1.6 2007/02/21 23:48:15 thorpej Exp $");
+__KERNEL_RCSID(0, "$NetBSD: sys_lwp.c,v 1.7 2007/02/26 09:20:54 yamt Exp $");
 
 #include <sys/param.h>
 #include <sys/systm.h>
@@ -61,7 +61,9 @@ __KERNEL_RCSID(0, "$NetBSD: sys_lwp.c,v 1.6 2007/02/21 23:48:15 thorpej Exp $");
 syncobj_t lwp_park_sobj = {
 	SOBJ_SLEEPQ_SORTED,
 	sleepq_unsleep,
-	sleepq_changepri
+	sleepq_changepri,
+	sleepq_lendpri,
+	syncobj_noowner,
 };
 
 sleeptab_t	lwp_park_tab;
diff --git a/sys/sys/lwp.h b/sys/sys/lwp.h
index c15801137c4b..34590e793640 100644
--- a/sys/sys/lwp.h
+++ b/sys/sys/lwp.h
@@ -1,4 +1,4 @@
-/* 	$NetBSD: lwp.h,v 1.51 2007/02/22 04:38:03 matt Exp $	*/
+/* 	$NetBSD: lwp.h,v 1.52 2007/02/26 09:20:52 yamt Exp $	*/
 
 /*-
  * Copyright (c) 2001, 2006, 2007 The NetBSD Foundation, Inc.
@@ -83,8 +83,10 @@ struct	lwp {
 	u_int		l_swtime;	/* l: time swapped in or out */
 	int		l_holdcnt;	/* l: if non-zero, don't swap */
 	int		l_biglocks;	/* l: biglock count before sleep */
-	u_char		l_priority;	/* l: process priority */
-	u_char		l_usrpri;	/* l: user-priority */
+	int		l_priority;	/* l: process priority */
+	int		l_usrpri;	/* l: user-priority */
+	int		l_inheritedprio;/* l: inherited priority */
+	SLIST_HEAD(, turnstile) l_pi_lenders; /* l: ts lending us priority */
 	long		l_nvcsw;	/* l: voluntary context switches */
 	long		l_nivcsw;	/* l: involuntary context switches */
 
@@ -250,6 +252,7 @@ void	lwp_setlock(struct lwp *, kmutex_t *);
 void	lwp_unlock_to(struct lwp *, kmutex_t *);
 void	lwp_lock_retry(struct lwp *, kmutex_t *);
 void	lwp_relock(struct lwp *, kmutex_t *);
+int	lwp_trylock(struct lwp *);
 void	lwp_addref(struct lwp *);
 void	lwp_delref(struct lwp *);
 void	lwp_drainrefs(struct lwp *);
@@ -322,9 +325,23 @@ lwp_changepri(struct lwp *l, int pri)
 {
 	LOCK_ASSERT(mutex_owned(l->l_mutex));
 
+	if (l->l_priority == pri)
+		return;
+
 	(*l->l_syncobj->sobj_changepri)(l, pri);
 }
 
+static inline void
+lwp_lendpri(struct lwp *l, int pri)
+{
+	LOCK_ASSERT(mutex_owned(l->l_mutex));
+
+	if (l->l_inheritedprio == pri)
+		return;
+
+	(*l->l_syncobj->sobj_lendpri)(l, pri);
+}
+
 static inline void
 lwp_unsleep(struct lwp *l)
 {
@@ -333,6 +350,13 @@ lwp_unsleep(struct lwp *l)
 	(*l->l_syncobj->sobj_unsleep)(l);
 }
 
+static inline int
+lwp_eprio(struct lwp *l)
+{
+
+	return MIN(l->l_inheritedprio, l->l_priority);
+}
+
 int newlwp(struct lwp *, struct proc *, vaddr_t, bool, int,
     void *, size_t, void (*)(void *), void *, struct lwp **);
 
diff --git a/sys/sys/queue.h b/sys/sys/queue.h
index f5ce06845ee8..24ccea7cba83 100644
--- a/sys/sys/queue.h
+++ b/sys/sys/queue.h
@@ -1,4 +1,4 @@
-/*	$NetBSD: queue.h,v 1.45 2006/03/07 17:56:00 pooka Exp $	*/
+/*	$NetBSD: queue.h,v 1.46 2007/02/26 09:20:52 yamt Exp $	*/
 
 /*
  * Copyright (c) 1991, 1993
@@ -222,6 +222,11 @@ struct {								\
 	}								\
 } while (/*CONSTCOND*/0)
 
+#define	SLIST_REMOVE_AFTER(slistelm, field) do {			\
+	(slistelm)->field.sle_next =					\
+	    SLIST_NEXT(SLIST_NEXT((slistelm), field), field);		\
+} while (/*CONSTCOND*/0)
+
 #define	SLIST_FOREACH(var, head, field)					\
 	for((var) = (head)->slh_first; (var); (var) = (var)->field.sle_next)
 
diff --git a/sys/sys/sched.h b/sys/sys/sched.h
index 7d58ac2ad07e..429db4cd8367 100644
--- a/sys/sys/sched.h
+++ b/sys/sys/sched.h
@@ -1,4 +1,4 @@
-/* $NetBSD: sched.h,v 1.30 2007/02/15 15:09:16 ad Exp $ */
+/* $NetBSD: sched.h,v 1.31 2007/02/26 09:20:52 yamt Exp $ */
 
 /*-
  * Copyright (c) 1999, 2000, 2001, 2002, 2007 The NetBSD Foundation, Inc.
@@ -195,13 +195,18 @@ typedef struct syncobj {
 	u_int	sobj_flag;
 	void	(*sobj_unsleep)(struct lwp *);
 	void	(*sobj_changepri)(struct lwp *, int);
+	void	(*sobj_lendpri)(struct lwp *, int);
+	struct lwp *(*sobj_owner)(volatile const void *); /* XXX wchan_t */
 } syncobj_t;
 
+struct lwp *syncobj_noowner(volatile const void *); /* XXX wchan_t */
+
 #define	SOBJ_SLEEPQ_SORTED	0x01
 #define	SOBJ_SLEEPQ_FIFO	0x02
 
 extern syncobj_t	sched_syncobj;
-extern syncobj_t	turnstile_syncobj;
+extern syncobj_t	mutex_syncobj;
+extern syncobj_t	rw_syncobj;
 
 #endif	/* _KERNEL */
 #endif	/* _SYS_SCHED_H_ */
diff --git a/sys/sys/sleepq.h b/sys/sys/sleepq.h
index a5e3ba6eb9b6..4b5cf74b286d 100644
--- a/sys/sys/sleepq.h
+++ b/sys/sys/sleepq.h
@@ -1,4 +1,4 @@
-/*	$NetBSD: sleepq.h,v 1.2 2007/02/09 21:55:37 ad Exp $	*/
+/*	$NetBSD: sleepq.h,v 1.3 2007/02/26 09:20:52 yamt Exp $	*/
 
 /*-
  * Copyright (c) 2002, 2006, 2007 The NetBSD Foundation, Inc.
@@ -95,8 +95,12 @@ void	sleepq_timeout(void *);
 void	sleepq_wake(sleepq_t *, wchan_t, u_int);
 int	sleepq_abort(kmutex_t *, int);
 void	sleepq_changepri(struct lwp *, int);
+void	sleepq_lendpri(struct lwp *, int);
 int	sleepq_unblock(int, int);
-void	sleepq_insert(sleepq_t *, struct lwp *, int, syncobj_t *);
+void	sleepq_insert(sleepq_t *, struct lwp *, syncobj_t *);
+
+void	sleepq_enqueue(sleepq_t *, int, wchan_t, const char *, syncobj_t *);
+void	sleepq_switch(int, int);
 
 void	sleeptab_init(sleeptab_t *);
 
@@ -189,6 +193,11 @@ typedef struct turnstile {
 	struct turnstile	*ts_free;	/* turnstile free list */
 	wchan_t			ts_obj;		/* lock object */
 	sleepq_t		ts_sleepq[2];	/* sleep queues */
+
+	/* priority inheritance */
+	u_char			ts_eprio;
+	struct lwp		*ts_inheritor;
+	SLIST_ENTRY(turnstile)	ts_pichain;
 } turnstile_t;
 
 #if defined(MULTIPROCESSOR) || defined(LOCKDEBUG)
@@ -222,7 +231,7 @@ typedef struct tschain {
 void	turnstile_init(void);
 turnstile_t	*turnstile_lookup(wchan_t);
 void	turnstile_exit(wchan_t);
-void	turnstile_block(turnstile_t *, int, wchan_t);
+void	turnstile_block(turnstile_t *, int, wchan_t, syncobj_t *);
 void	turnstile_wakeup(turnstile_t *, int, int, struct lwp *);
 void	turnstile_print(volatile void *, void (*)(const char *, ...));
 
@@ -232,6 +241,9 @@ turnstile_unblock(void)
 	(void)sleepq_unblock(0, 0);
 }
 
+void	turnstile_unsleep(struct lwp *);
+void	turnstile_changepri(struct lwp *, int);
+
 extern struct pool_cache turnstile_cache;
 extern struct turnstile turnstile0;
 
diff --git a/sys/uvm/uvm_meter.c b/sys/uvm/uvm_meter.c
index 0f3c0ef32ad2..04e11f60d9f7 100644
--- a/sys/uvm/uvm_meter.c
+++ b/sys/uvm/uvm_meter.c
@@ -1,4 +1,4 @@
-/*	$NetBSD: uvm_meter.c,v 1.46 2007/02/17 22:31:45 pavel Exp $	*/
+/*	$NetBSD: uvm_meter.c,v 1.47 2007/02/26 09:20:54 yamt Exp $	*/
 
 /*
  * Copyright (c) 1997 Charles D. Cranor and Washington University.
@@ -41,7 +41,7 @@
  */
 
 #include <sys/cdefs.h>
-__KERNEL_RCSID(0, "$NetBSD: uvm_meter.c,v 1.46 2007/02/17 22:31:45 pavel Exp $");
+__KERNEL_RCSID(0, "$NetBSD: uvm_meter.c,v 1.47 2007/02/26 09:20:54 yamt Exp $");
 
 #include <sys/param.h>
 #include <sys/proc.h>
@@ -378,7 +378,7 @@ uvm_total(struct vmtotal *totalp)
 		case LSSLEEP:
 		case LSSTOP:
 			if (l->l_flag & LW_INMEM) {
-				if (l->l_priority <= PZERO)
+				if (lwp_eprio(l) <= PZERO)
 					totalp->t_dw++;
 				else if (l->l_slptime < maxslp)
 					totalp->t_sl++;