 @@ -1,14 +1,14 @@
-/*	$NetBSD: kern_turnstile.c,v 1.30 2011/07/27 14:35:34 uebayasi Exp $	*/
+/*	$NetBSD: kern_turnstile.c,v 1.31 2011/12/02 12:31:53 yamt Exp $	*/
 /*-
  * Copyright (c) 2002, 2006, 2007, 2009 The NetBSD Foundation, Inc.
  * All rights reserved.
+ *
  * This code is derived from software contributed to The NetBSD Foundation
  * by Jason R. Thorpe and Andrew Doran.
+ *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
  * are met:
  * 1. Redistributions of source code must retain the above copyright
  *    notice, this list of conditions and the following disclaimer.
 @@ -50,27 +50,27 @@
  * queue.  If a thread decides it doesn't need to block after all, then this
  * interlock must be released by explicitly aborting the turnstile
  * operation.
+ *
  * When a thread is awakened, it needs to get its turnstile back.  If there
  * are still other threads waiting in the active turnstile, the thread
  * 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.
  */
 #include <sys/cdefs.h>
-__KERNEL_RCSID(0, "$NetBSD: kern_turnstile.c,v 1.30 2011/07/27 14:35:34 uebayasi Exp $");
+__KERNEL_RCSID(0, "$NetBSD: kern_turnstile.c,v 1.31 2011/12/02 12:31:53 yamt Exp $");
 #include <sys/param.h>
 #include <sys/lockdebug.h>
 #include <sys/pool.h>
 #include <sys/proc.h>
 #include <sys/sleepq.h>
 #include <sys/systm.h>
 #define	TS_HASH_SIZE	64
 #define	TS_HASH_MASK	(TS_HASH_SIZE - 1)
 #define	TS_HASH(obj)	(((uintptr_t)(obj) >> 3) & TS_HASH_MASK)
 static tschain_t	turnstile_tab[TS_HASH_SIZE]	__cacheline_aligned;
 @@ -185,107 +185,51 @@ turnstile_lookup(wchan_t obj)
+ *
  *	Abort a turnstile operation.
  */
 void
 turnstile_exit(wchan_t obj)
+{
 	tschain_t *tc;
 	tc = &turnstile_tab[TS_HASH(obj)];
 	mutex_spin_exit(tc->tc_mutex);
+}
 /*
- * turnstile_block:
+ * turnstile_lendpri:
+ *
  *	Lend our priority to lwps on the blocking chain.
+ *
+ *
  *	 Enter an object into the turnstile chain and prepare the current
  *	 LWP for sleep.
  */
 void
 turnstile_block(turnstile_t *ts, int q, wchan_t obj, syncobj_t *sobj)
 	lwp_t *l;
 	lwp_t *cur; /* cached curlwp */
 	lwp_t *owner;
 	turnstile_t *ots;
 	tschain_t *tc;
 	sleepq_t *sq;
 	pri_t prio, obase;
 	tc = &turnstile_tab[TS_HASH(obj)];
 	l = cur = curlwp;
-	KASSERT(q == TS_READER_Q || q == TS_WRITER_Q);
+static void
-	KASSERT(mutex_owned(tc->tc_mutex));
+turnstile_lendpri(lwp_t *cur)
 	KASSERT(l != NULL && l->l_ts != NULL);
 	lwp_t * l = cur;
-	if (ts == NULL) {
+	pri_t prio;
 		/*
 		 * We are the first thread to wait for this object;
 		 * lend our turnstile to it.
 		 */
 		ts = l->l_ts;
 		KASSERT(TS_ALL_WAITERS(ts) == 0);
 		KASSERT(TAILQ_EMPTY(&ts->ts_sleepq[TS_READER_Q]) &&
 			TAILQ_EMPTY(&ts->ts_sleepq[TS_WRITER_Q]));
 		ts->ts_obj = obj;
 		ts->ts_inheritor = NULL;
 		LIST_INSERT_HEAD(&tc->tc_chain, ts, ts_chain);
 	} else {
 		/*
 		 * Object already has a turnstile.  Put our turnstile
 		 * onto the free list, and reference the existing
 		 * turnstile instead.
 		 */
 		ots = l->l_ts;
 		KASSERT(ots->ts_free == NULL);
 		ots->ts_free = ts->ts_free;
 		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]) ||
 			!TAILQ_EMPTY(&ts->ts_sleepq[TS_WRITER_Q]));
 	sq = &ts->ts_sleepq[q];
 	ts->ts_waiters[q]++;
 	sleepq_enter(sq, l, tc->tc_mutex);
 	LOCKDEBUG_BARRIER(tc->tc_mutex, 1);
 	l->l_kpriority = true;
 	obase = l->l_kpribase;
 	if (obase < PRI_KTHREAD)
 		l->l_kpribase = PRI_KTHREAD;
 	sleepq_enqueue(sq, obj, "tstile", sobj);
 	/*
 	 * Disable preemption across this entire block, as we may drop
 	 * scheduler locks (allowing preemption), and would prefer not
 	 * to be interrupted while in a state of flux.
 	 */
 	KPREEMPT_DISABLE(l);
 	/*
 	 * Lend our priority to lwps on the blocking chain.
 	 * NOTE: if you get a panic in this code block, it is likely that
 	 * a lock has been destroyed or corrupted while still in use.  Try
 	 * compiling a kernel with LOCKDEBUG to pinpoint the problem.
 	 */
 	LOCKDEBUG_BARRIER(l->l_mutex, 1);
 	KASSERT(l == curlwp);
 	prio = lwp_eprio(l);
 	KASSERT(cur == l);
 	KASSERT(tc->tc_mutex == cur->l_mutex);
 	for (;;) {
 		lwp_t *owner;
 		turnstile_t *ts;
 		bool dolock;
 		if (l->l_wchan == NULL)
 			break;
 		owner = (*l->l_syncobj->sobj_owner)(l->l_wchan);
 		if (owner == NULL)
 			break;
 		/* The owner may have changed as we have dropped the tc lock */
 		if (cur == owner) {
 			/*
 			 * we own the lock: stop here, sleepq_block()
 @@ -327,30 +271,155 @@ turnstile_block(turnstile_t *ts, int q,
 			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);
+}
 /*
  * turnstile_unlendpri: undo turnstile_lendpri
  */
 static void
 turnstile_unlendpri(turnstile_t *ts)
+{
 	lwp_t * const l = curlwp;
 	turnstile_t *iter;
 	turnstile_t *next;
 	turnstile_t *prev = NULL;
 	pri_t prio;
 	bool dolock;
 	KASSERT(ts->ts_inheritor != NULL);
 	ts->ts_inheritor = NULL;
 	dolock = l->l_mutex == l->l_cpu->ci_schedstate.spc_lwplock;
 	if (dolock) {
 		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 = -1;
 	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);
 	if (dolock) {
 		lwp_unlock(l);
+	}
+}
 /*
  * turnstile_block:
+ *
  *	 Enter an object into the turnstile chain and prepare the current
  *	 LWP for sleep.
  */
 void
 turnstile_block(turnstile_t *ts, int q, wchan_t obj, syncobj_t *sobj)
+{
 	lwp_t * const l = curlwp; /* cached curlwp */
 	turnstile_t *ots;
 	tschain_t *tc;
 	sleepq_t *sq;
 	pri_t obase;
 	tc = &turnstile_tab[TS_HASH(obj)];
 	KASSERT(q == TS_READER_Q || q == TS_WRITER_Q);
 	KASSERT(mutex_owned(tc->tc_mutex));
 	KASSERT(l != NULL && l->l_ts != NULL);
 	if (ts == NULL) {
 		/*
 		 * We are the first thread to wait for this object;
 		 * lend our turnstile to it.
 		 */
 		ts = l->l_ts;
 		KASSERT(TS_ALL_WAITERS(ts) == 0);
 		KASSERT(TAILQ_EMPTY(&ts->ts_sleepq[TS_READER_Q]) &&
 			TAILQ_EMPTY(&ts->ts_sleepq[TS_WRITER_Q]));
 		ts->ts_obj = obj;
 		ts->ts_inheritor = NULL;
 		LIST_INSERT_HEAD(&tc->tc_chain, ts, ts_chain);
 	} else {
 		/*
 		 * Object already has a turnstile.  Put our turnstile
 		 * onto the free list, and reference the existing
 		 * turnstile instead.
 		 */
 		ots = l->l_ts;
 		KASSERT(ots->ts_free == NULL);
 		ots->ts_free = ts->ts_free;
 		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]) ||
 			!TAILQ_EMPTY(&ts->ts_sleepq[TS_WRITER_Q]));
+	}
 	sq = &ts->ts_sleepq[q];
 	ts->ts_waiters[q]++;
 	sleepq_enter(sq, l, tc->tc_mutex);
 	LOCKDEBUG_BARRIER(tc->tc_mutex, 1);
 	l->l_kpriority = true;
 	obase = l->l_kpribase;
 	if (obase < PRI_KTHREAD)
 		l->l_kpribase = PRI_KTHREAD;
 	sleepq_enqueue(sq, obj, "tstile", sobj);
 	/*
 	 * Disable preemption across this entire block, as we may drop
 	 * scheduler locks (allowing preemption), and would prefer not
 	 * to be interrupted while in a state of flux.
 	 */
 	KPREEMPT_DISABLE(l);
 	KASSERT(tc->tc_mutex == l->l_mutex);
 	turnstile_lendpri(l);
 	sleepq_block(0, false);
-	cur->l_kpribase = obase;
+	l->l_kpribase = obase;
-	KPREEMPT_ENABLE(cur);
+	KPREEMPT_ENABLE(l);
+}
 /*
  * turnstile_wakeup:
+ *
  *	Wake up the specified number of threads that are blocked
  *	in a turnstile.
  */
 void
 turnstile_wakeup(turnstile_t *ts, int q, int count, lwp_t *nl)
+{
 	sleepq_t *sq;
 	tschain_t *tc;
 @@ -359,72 +428,27 @@ turnstile_wakeup(turnstile_t *ts, int q,
 	tc = &turnstile_tab[TS_HASH(ts->ts_obj)];
 	sq = &ts->ts_sleepq[q];
 	KASSERT(q == TS_READER_Q || q == TS_WRITER_Q);
 	KASSERT(count > 0 && count <= TS_WAITERS(ts, q));
 	KASSERT(mutex_owned(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_unlendpri(ts);
 		turnstile_t *next;
 		turnstile_t *prev = NULL;
 		pri_t prio;
 		bool dolock;
 		ts->ts_inheritor = NULL;
 		l = curlwp;
 		dolock = l->l_mutex == l->l_cpu->ci_schedstate.spc_lwplock;
 		if (dolock) {
 			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 = -1;
 		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);
 		if (dolock) {
 			lwp_unlock(l);
+	}
 	if (nl != NULL) {
 #if defined(DEBUG) || defined(LOCKDEBUG)
 		TAILQ_FOREACH(l, sq, l_sleepchain) {
 			if (l == nl)
 				break;
+		}
 		if (l == NULL)
 			panic("turnstile_wakeup: nl not on sleepq");
 #endif
 		turnstile_remove(ts, nl, q);
 	} else {