kernel - All lwkt thread now start out mpsafe part 1/2
[dragonfly.git] / sys / kern / lwkt_thread.c
index a1ce259..1311f37 100644 (file)
@@ -1,13 +1,13 @@
 /*
- * Copyright (c) 2003,2004 The DragonFly Project.  All rights reserved.
- * 
+ * Copyright (c) 2003-2010 The DragonFly Project.  All rights reserved.
+ *
  * This code is derived from software contributed to The DragonFly Project
  * by Matthew Dillon <dillon@backplane.com>
- * 
+ *
  * 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.
  * 2. Redistributions in binary form must reproduce the above copyright
@@ -17,7 +17,7 @@
  * 3. Neither the name of The DragonFly Project nor the names of its
  *    contributors may be used to endorse or promote products derived
  *    from this software without specific, prior written permission.
- * 
+ *
  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
  * ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
@@ -30,8 +30,6 @@
  * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
  * OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  * SUCH DAMAGE.
- * 
- * $DragonFly: src/sys/kern/lwkt_thread.c,v 1.120 2008/10/26 04:29:19 sephe Exp $
  */
 
 /*
  * to use a critical section to avoid problems.  Foreign thread 
  * scheduling is queued via (async) IPIs.
  */
-#include "opt_ddb.h"
 
 #include <sys/param.h>
 #include <sys/systm.h>
 #include <sys/kernel.h>
 #include <sys/proc.h>
 #include <sys/rtprio.h>
+#include <sys/kinfo.h>
 #include <sys/queue.h>
 #include <sys/sysctl.h>
 #include <sys/kthread.h>
@@ -58,6 +56,9 @@
 
 #include <sys/thread2.h>
 #include <sys/spinlock2.h>
+#include <sys/mplock2.h>
+
+#include <sys/dsched.h>
 
 #include <vm/vm.h>
 #include <vm/vm_param.h>
 #include <machine/stdarg.h>
 #include <machine/smp.h>
 
-#ifdef DDB
-#include <ddb/ddb.h>
+#if !defined(KTR_CTXSW)
+#define KTR_CTXSW KTR_ALL
 #endif
+KTR_INFO_MASTER(ctxsw);
+KTR_INFO(KTR_CTXSW, ctxsw, sw, 0, "#cpu[%d].td = %p",
+        sizeof(int) + sizeof(struct thread *));
+KTR_INFO(KTR_CTXSW, ctxsw, pre, 1, "#cpu[%d].td = %p",
+        sizeof(int) + sizeof(struct thread *));
+KTR_INFO(KTR_CTXSW, ctxsw, newtd, 2, "#threads[%p].name = %s",
+        sizeof (struct thread *) + sizeof(char *));
+KTR_INFO(KTR_CTXSW, ctxsw, deadtd, 3, "#threads[%p].name = <dead>", sizeof (struct thread *));
 
 static MALLOC_DEFINE(M_THREAD, "thread", "lwkt threads");
 
-static int untimely_switch = 0;
 #ifdef INVARIANTS
 static int panic_on_cscount = 0;
 #endif
@@ -85,73 +93,66 @@ static __int64_t switch_count = 0;
 static __int64_t preempt_hit = 0;
 static __int64_t preempt_miss = 0;
 static __int64_t preempt_weird = 0;
-static __int64_t token_contention_count = 0;
-static __int64_t mplock_contention_count = 0;
+static __int64_t token_contention_count __debugvar = 0;
 static int lwkt_use_spin_port;
-#ifdef SMP
-static int chain_mplock = 0;
-#endif
 static struct objcache *thread_cache;
 
-volatile cpumask_t mp_lock_contention_mask;
+#ifdef SMP
+static void lwkt_schedule_remote(void *arg, int arg2, struct intrframe *frame);
+#endif
+static void lwkt_fairq_accumulate(globaldata_t gd, thread_t td);
 
 extern void cpu_heavy_restore(void);
 extern void cpu_lwkt_restore(void);
 extern void cpu_kthread_restore(void);
 extern void cpu_idle_restore(void);
 
-int
+#ifdef __x86_64__
+
+static int
 jg_tos_ok(struct thread *td)
 {
+       void *tos;
+       int tos_ok;
+
        if (td == NULL) {
                return 1;
        }
        KKASSERT(td->td_sp != NULL);
-       unsigned long tos = ((unsigned long *)td->td_sp)[0];
-       int tos_ok = 0;
-       if ((tos == cpu_heavy_restore) || (tos == cpu_lwkt_restore)
-               || (tos == cpu_kthread_restore) || (tos == cpu_idle_restore)) {
+       tos = ((void **)td->td_sp)[0];
+       tos_ok = 0;
+       if ((tos == cpu_heavy_restore) || (tos == cpu_lwkt_restore) ||
+           (tos == cpu_kthread_restore) || (tos == cpu_idle_restore)) {
                tos_ok = 1;
        }
        return tos_ok;
 }
 
+#endif
+
 /*
  * We can make all thread ports use the spin backend instead of the thread
  * backend.  This should only be set to debug the spin backend.
  */
 TUNABLE_INT("lwkt.use_spin_port", &lwkt_use_spin_port);
 
-SYSCTL_INT(_lwkt, OID_AUTO, untimely_switch, CTLFLAG_RW, &untimely_switch, 0, "");
 #ifdef INVARIANTS
 SYSCTL_INT(_lwkt, OID_AUTO, panic_on_cscount, CTLFLAG_RW, &panic_on_cscount, 0, "");
 #endif
-#ifdef SMP
-SYSCTL_INT(_lwkt, OID_AUTO, chain_mplock, CTLFLAG_RW, &chain_mplock, 0, "");
-#endif
 SYSCTL_QUAD(_lwkt, OID_AUTO, switch_count, CTLFLAG_RW, &switch_count, 0, "");
-SYSCTL_QUAD(_lwkt, OID_AUTO, preempt_hit, CTLFLAG_RW, &preempt_hit, 0, "");
-SYSCTL_QUAD(_lwkt, OID_AUTO, preempt_miss, CTLFLAG_RW, &preempt_miss, 0, "");
+SYSCTL_QUAD(_lwkt, OID_AUTO, preempt_hit, CTLFLAG_RW, &preempt_hit, 0, 
+           "Successful preemption events");
+SYSCTL_QUAD(_lwkt, OID_AUTO, preempt_miss, CTLFLAG_RW, &preempt_miss, 0, 
+           "Failed preemption events");
 SYSCTL_QUAD(_lwkt, OID_AUTO, preempt_weird, CTLFLAG_RW, &preempt_weird, 0, "");
 #ifdef INVARIANTS
 SYSCTL_QUAD(_lwkt, OID_AUTO, token_contention_count, CTLFLAG_RW,
        &token_contention_count, 0, "spinning due to token contention");
-SYSCTL_QUAD(_lwkt, OID_AUTO, mplock_contention_count, CTLFLAG_RW,
-       &mplock_contention_count, 0, "spinning due to MPLOCK contention");
 #endif
-
-/*
- * Kernel Trace
- */
-#if !defined(KTR_GIANT_CONTENTION)
-#define KTR_GIANT_CONTENTION   KTR_ALL
-#endif
-
-KTR_INFO_MASTER(giant);
-KTR_INFO(KTR_GIANT_CONTENTION, giant, beg, 0, "thread=%p", sizeof(void *));
-KTR_INFO(KTR_GIANT_CONTENTION, giant, end, 1, "thread=%p", sizeof(void *));
-
-#define loggiant(name) KTR_LOG(giant_ ## name, curthread)
+static int fairq_enable = 1;
+SYSCTL_INT(_lwkt, OID_AUTO, fairq_enable, CTLFLAG_RW, &fairq_enable, 0, "");
+static int user_pri_sched = 0;
+SYSCTL_INT(_lwkt, OID_AUTO, user_pri_sched, CTLFLAG_RW, &user_pri_sched, 0, "");
 
 /*
  * These helper procedures handle the runq, they can only be called from
@@ -167,26 +168,45 @@ void
 _lwkt_dequeue(thread_t td)
 {
     if (td->td_flags & TDF_RUNQ) {
-       int nq = td->td_pri & TDPRI_MASK;
        struct globaldata *gd = td->td_gd;
 
        td->td_flags &= ~TDF_RUNQ;
-       TAILQ_REMOVE(&gd->gd_tdrunq[nq], td, td_threadq);
-       /* runqmask is passively cleaned up by the switcher */
+       TAILQ_REMOVE(&gd->gd_tdrunq, td, td_threadq);
+       gd->gd_fairq_total_pri -= td->td_pri;
+       if (TAILQ_FIRST(&gd->gd_tdrunq) == NULL)
+               atomic_clear_int_nonlocked(&gd->gd_reqflags, RQF_RUNNING);
     }
 }
 
+/*
+ * Priority enqueue.
+ *
+ * NOTE: There are a limited number of lwkt threads runnable since user
+ *      processes only schedule one at a time per cpu.
+ */
 static __inline
 void
 _lwkt_enqueue(thread_t td)
 {
-    if ((td->td_flags & (TDF_RUNQ|TDF_MIGRATING|TDF_TSLEEPQ|TDF_BLOCKQ)) == 0) {
-       int nq = td->td_pri & TDPRI_MASK;
+    thread_t xtd;
+
+    if ((td->td_flags & (TDF_RUNQ|TDF_MIGRATING|TDF_BLOCKQ)) == 0) {
        struct globaldata *gd = td->td_gd;
 
        td->td_flags |= TDF_RUNQ;
-       TAILQ_INSERT_TAIL(&gd->gd_tdrunq[nq], td, td_threadq);
-       gd->gd_runqmask |= 1 << nq;
+       xtd = TAILQ_FIRST(&gd->gd_tdrunq);
+       if (xtd == NULL) {
+               TAILQ_INSERT_TAIL(&gd->gd_tdrunq, td, td_threadq);
+               atomic_set_int_nonlocked(&gd->gd_reqflags, RQF_RUNNING);
+       } else {
+               while (xtd && xtd->td_pri > td->td_pri)
+                       xtd = TAILQ_NEXT(xtd, td_threadq);
+               if (xtd)
+                       TAILQ_INSERT_BEFORE(xtd, td, td_threadq);
+               else
+                       TAILQ_INSERT_TAIL(&gd->gd_tdrunq, td, td_threadq);
+       }
+       gd->gd_fairq_total_pri += td->td_pri;
     }
 }
 
@@ -237,7 +257,8 @@ void
 lwkt_schedule_self(thread_t td)
 {
     crit_enter_quick(td);
-    KASSERT(td != &td->td_gd->gd_idlethread, ("lwkt_schedule_self(): scheduling gd_idlethread is illegal!"));
+    KASSERT(td != &td->td_gd->gd_idlethread,
+           ("lwkt_schedule_self(): scheduling gd_idlethread is illegal!"));
     KKASSERT(td->td_lwp == NULL || (td->td_lwp->lwp_flag & LWP_ONRUNQ) == 0);
     _lwkt_enqueue(td);
     crit_exit_quick(td);
@@ -264,11 +285,7 @@ lwkt_deschedule_self(thread_t td)
 void
 lwkt_gdinit(struct globaldata *gd)
 {
-    int i;
-
-    for (i = 0; i < sizeof(gd->gd_tdrunq)/sizeof(gd->gd_tdrunq[0]); ++i)
-       TAILQ_INIT(&gd->gd_tdrunq[i]);
-    gd->gd_runqmask = 0;
+    TAILQ_INIT(&gd->gd_tdrunq);
     TAILQ_INIT(&gd->gd_tdallq);
 }
 
@@ -357,12 +374,17 @@ lwkt_init_thread(thread_t td, void *stack, int stksize, int flags,
 {
     globaldata_t mygd = mycpu;
 
+    /* all threads start mpsafe now */
+    KKASSERT(flags & TDF_MPSAFE);
+
     bzero(td, sizeof(struct thread));
     td->td_kstack = stack;
     td->td_kstack_size = stksize;
     td->td_flags = flags;
     td->td_gd = gd;
-    td->td_pri = TDPRI_KERN_DAEMON + TDPRI_CRIT;
+    td->td_pri = TDPRI_KERN_DAEMON;
+    td->td_critcount = 1;
+    td->td_toks_stop = &td->td_toks_base;
 #ifdef SMP
     if ((flags & TDF_MPSAFE) == 0)
        td->td_mpcount = 1;
@@ -391,6 +413,8 @@ lwkt_init_thread(thread_t td, void *stack, int stksize, int flags,
     TAILQ_INSERT_TAIL(&gd->gd_tdallq, td, td_allq);
     crit_exit_gd(mygd);
 #endif
+
+    dsched_new_thread(td);
 }
 
 void
@@ -401,6 +425,7 @@ lwkt_set_comm(thread_t td, const char *ctl, ...)
     __va_start(va, ctl);
     kvsnprintf(td->td_comm, sizeof(td->td_comm), ctl, va);
     __va_end(va);
+    KTR_LOG(ctxsw_newtd, td, &td->td_comm[0]);
 }
 
 void
@@ -439,6 +464,7 @@ lwkt_free_thread(thread_t td)
        td->td_kstack = NULL;
        td->td_kstack_size = 0;
     }
+    KTR_LOG(ctxsw_deadtd, td);
 }
 
 
@@ -474,9 +500,15 @@ lwkt_switch(void)
     globaldata_t gd = mycpu;
     thread_t td = gd->gd_curthread;
     thread_t ntd;
+    thread_t xtd;
+    thread_t nlast;
+    int nquserok;
 #ifdef SMP
     int mpheld;
 #endif
+    int didaccumulate;
+    const char *lmsg;  /* diagnostic - 'systat -pv 1' */
+    const void *laddr;
 
     /*
      * Switching from within a 'fast' (non thread switched) interrupt or IPI
@@ -501,9 +533,7 @@ lwkt_switch(void)
                td->td_flags |= TDF_PANICWARN;
                kprintf("Warning: thread switch from interrupt or IPI, "
                        "thread %p (%s)\n", td, td->td_comm);
-#ifdef DDB
-               db_print_backtrace();
-#endif
+               print_backtrace(-1);
            }
            lwkt_switch();
            gd->gd_intr_nesting_level = savegdnest;
@@ -525,7 +555,7 @@ lwkt_switch(void)
            td->td_release(td);
 
     crit_enter_gd(gd);
-    if (td->td_toks)
+    if (TD_TOKS_HELD(td))
            lwkt_relalltokens(td);
 
     /*
@@ -549,7 +579,7 @@ lwkt_switch(void)
      * (but, of course, another cpu may own or release the lock so the
      * actual value of mp_lock is not stable).
      */
-    mpheld = MP_LOCK_HELD();
+    mpheld = MP_LOCK_HELD(gd);
 #ifdef INVARIANTS
     if (td->td_cscount) {
        kprintf("Diagnostic: attempt to switch while mastering cpusync: %p\n",
@@ -559,17 +589,18 @@ lwkt_switch(void)
     }
 #endif
 #endif
+
+    /*
+     * If we had preempted another thread on this cpu, resume the preempted
+     * thread.  This occurs transparently, whether the preempted thread
+     * was scheduled or not (it may have been preempted after descheduling
+     * itself).
+     *
+     * We have to setup the MP lock for the original thread after backing
+     * out the adjustment that was made to curthread when the original
+     * was preempted.
+     */
     if ((ntd = td->td_preempted) != NULL) {
-       /*
-        * We had preempted another thread on this cpu, resume the preempted
-        * thread.  This occurs transparently, whether the preempted thread
-        * was scheduled or not (it may have been preempted after descheduling
-        * itself). 
-        *
-        * We have to setup the MP lock for the original thread after backing
-        * out the adjustment that was made to curthread when the original
-        * was preempted.
-        */
        KKASSERT(ntd->td_flags & TDF_PREEMPT_LOCK);
 #ifdef SMP
        if (ntd->td_mpcount && mpheld == 0) {
@@ -588,185 +619,286 @@ lwkt_switch(void)
         * set the reschedule flag if the originally interrupted thread is
         * at a lower priority.
         */
-       if (gd->gd_runqmask > (2 << (ntd->td_pri & TDPRI_MASK)) - 1)
+       if (TAILQ_FIRST(&gd->gd_tdrunq) &&
+           TAILQ_FIRST(&gd->gd_tdrunq)->td_pri > ntd->td_pri) {
            need_lwkt_resched();
+       }
        /* YYY release mp lock on switchback if original doesn't need it */
-    } else {
+       goto havethread_preempted;
+    }
+
+    /*
+     * Implement round-robin fairq with priority insertion.  The priority
+     * insertion is handled by _lwkt_enqueue()
+     *
+     * We have to adjust the MP lock for the target thread.  If we
+     * need the MP lock and cannot obtain it we try to locate a
+     * thread that does not need the MP lock.  If we cannot, we spin
+     * instead of HLT.
+     *
+     * A similar issue exists for the tokens held by the target thread.
+     * If we cannot obtain ownership of the tokens we cannot immediately
+     * schedule the thread.
+     */
+    for (;;) {
+       clear_lwkt_resched();
+       didaccumulate = 0;
+       ntd = TAILQ_FIRST(&gd->gd_tdrunq);
+
        /*
-        * Priority queue / round-robin at each priority.  Note that user
-        * processes run at a fixed, low priority and the user process
-        * scheduler deals with interactions between user processes
-        * by scheduling and descheduling them from the LWKT queue as
-        * necessary.
+        * Hotpath if we can get all necessary resources.
         *
-        * We have to adjust the MP lock for the target thread.  If we 
-        * need the MP lock and cannot obtain it we try to locate a
-        * thread that does not need the MP lock.  If we cannot, we spin
-        * instead of HLT.
+        * If nothing is runnable switch to the idle thread
+        */
+       if (ntd == NULL) {
+           ntd = &gd->gd_idlethread;
+           if (gd->gd_reqflags & RQF_IDLECHECK_MASK)
+                   ntd->td_flags |= TDF_IDLE_NOHLT;
+#ifdef SMP
+           if (ntd->td_mpcount) {
+               if (gd->gd_trap_nesting_level == 0 && panicstr == NULL)
+                   panic("Idle thread %p was holding the BGL!", ntd);
+               if (mpheld == 0) {
+                   set_cpu_contention_mask(gd);
+                   handle_cpu_contention_mask();
+                   cpu_try_mplock();
+                   mpheld = MP_LOCK_HELD(gd);
+                   cpu_pause();
+                   continue;
+               }
+           }
+           clr_cpu_contention_mask(gd);
+#endif
+           cpu_time.cp_msg[0] = 0;
+           cpu_time.cp_stallpc = 0;
+           goto haveidle;
+       }
+
+       /*
+        * Hotpath schedule
         *
-        * A similar issue exists for the tokens held by the target thread.
-        * If we cannot obtain ownership of the tokens we cannot immediately
-        * schedule the thread.
+        * NOTE: For UP there is no mplock and lwkt_getalltokens()
+        *           always succeeds.
         */
+       if (ntd->td_fairq_accum >= 0 &&
+#ifdef SMP
+           (ntd->td_mpcount == 0 || mpheld || cpu_try_mplock()) &&
+#endif
+           (!TD_TOKS_HELD(ntd) || lwkt_getalltokens(ntd, &lmsg, &laddr))
+       ) {
+#ifdef SMP
+           clr_cpu_contention_mask(gd);
+#endif
+           goto havethread;
+       }
+
+       lmsg = NULL;
+       laddr = NULL;
+
+#ifdef SMP
+       if (ntd->td_fairq_accum >= 0)
+               set_cpu_contention_mask(gd);
+       /* Reload mpheld (it become stale after mplock/token ops) */
+       mpheld = MP_LOCK_HELD(gd);
+       if (ntd->td_mpcount && mpheld == 0) {
+           lmsg = "mplock";
+           laddr = ntd->td_mplock_stallpc;
+       }
+#endif
 
        /*
-        * If an LWKT reschedule was requested, well that is what we are
-        * doing now so clear it.
+        * Coldpath - unable to schedule ntd, continue looking for threads
+        * to schedule.  This is only allowed of the (presumably) kernel
+        * thread exhausted its fair share.  A kernel thread stuck on
+        * resources does not currently allow a user thread to get in
+        * front of it.
         */
-       clear_lwkt_resched();
-again:
-       if (gd->gd_runqmask) {
-           int nq = bsrl(gd->gd_runqmask);
-           if ((ntd = TAILQ_FIRST(&gd->gd_tdrunq[nq])) == NULL) {
-               gd->gd_runqmask &= ~(1 << nq);
-               goto again;
-           }
 #ifdef SMP
+       nquserok = ((ntd->td_pri < TDPRI_KERN_LPSCHED) ||
+                   (ntd->td_fairq_accum < 0));
+#else
+       nquserok = 1;
+#endif
+       nlast = NULL;
+
+       for (;;) {
            /*
-            * THREAD SELECTION FOR AN SMP MACHINE BUILD
-            *
-            * If the target needs the MP lock and we couldn't get it,
-            * or if the target is holding tokens and we could not 
-            * gain ownership of the tokens, continue looking for a
-            * thread to schedule and spin instead of HLT if we can't.
+            * If the fair-share scheduler ran out ntd gets moved to the
+            * end and its accumulator will be bumped, if it didn't we
+            * maintain the same queue position.
             *
-            * NOTE: the mpheld variable invalid after this conditional, it
-            * can change due to both cpu_try_mplock() returning success
-            * AND interactions in lwkt_getalltokens() due to the fact that
-            * we are trying to check the mpcount of a thread other then
-            * the current thread.  Because of this, if the current thread
-            * is not holding td_mpcount, an IPI indirectly run via
-            * lwkt_getalltokens() can obtain and release the MP lock and
-            * cause the core MP lock to be released. 
+            * nlast keeps track of the last element prior to any moves.
             */
-           if ((ntd->td_mpcount && mpheld == 0 && !cpu_try_mplock()) ||
-               (ntd->td_toks && lwkt_getalltokens(ntd) == 0)
-           ) {
-               u_int32_t rqmask = gd->gd_runqmask;
-
-               mpheld = MP_LOCK_HELD();
-               ntd = NULL;
-               while (rqmask) {
-                   TAILQ_FOREACH(ntd, &gd->gd_tdrunq[nq], td_threadq) {
-                       if (ntd->td_mpcount && !mpheld && !cpu_try_mplock()) {
-                           /* spinning due to MP lock being held */
-#ifdef INVARIANTS
-                           ++mplock_contention_count;
-#endif
-                           /* mplock still not held, 'mpheld' still valid */
-                           continue;
-                       }
-
-                       /*
-                        * mpheld state invalid after getalltokens call returns
-                        * failure, but the variable is only needed for
-                        * the loop.
-                        */
-                       if (ntd->td_toks && !lwkt_getalltokens(ntd)) {
-                           /* spinning due to token contention */
-#ifdef INVARIANTS
-                           ++token_contention_count;
-#endif
-                           mpheld = MP_LOCK_HELD();
-                           continue;
-                       }
-                       break;
-                   }
-                   if (ntd)
-                       break;
-                   rqmask &= ~(1 << nq);
-                   nq = bsrl(rqmask);
-
-                   /*
-                    * We have two choices. We can either refuse to run a
-                    * user thread when a kernel thread needs the MP lock
-                    * but could not get it, or we can allow it to run but
-                    * then expect an IPI (hopefully) later on to force a
-                    * reschedule when the MP lock might become available.
-                    */
-                   if (nq < TDPRI_KERN_LPSCHED) {
-                       if (chain_mplock == 0)
-                               break;
-                       atomic_set_int(&mp_lock_contention_mask,
-                                      gd->gd_cpumask);
-                       /* continue loop, allow user threads to be scheduled */
-                   }
-               }
-               if (ntd == NULL) {
-                   cpu_mplock_contested();
-                   ntd = &gd->gd_idlethread;
-                   ntd->td_flags |= TDF_IDLE_NOHLT;
-                   goto using_idle_thread;
-               } else {
-                   ++gd->gd_cnt.v_swtch;
-                   TAILQ_REMOVE(&gd->gd_tdrunq[nq], ntd, td_threadq);
-                   TAILQ_INSERT_TAIL(&gd->gd_tdrunq[nq], ntd, td_threadq);
+           if (ntd->td_fairq_accum < 0) {
+               lwkt_fairq_accumulate(gd, ntd);
+               didaccumulate = 1;
+
+               /*
+                * Move to end
+                */
+               xtd = TAILQ_NEXT(ntd, td_threadq);
+               TAILQ_REMOVE(&gd->gd_tdrunq, ntd, td_threadq);
+               TAILQ_INSERT_TAIL(&gd->gd_tdrunq, ntd, td_threadq);
+
+               /*
+                * Set terminal element (nlast)
+                */
+               if (nlast == NULL) {
+                   nlast = ntd;
+                   if (xtd == NULL)
+                       xtd = ntd;
                }
+               ntd = xtd;
            } else {
-               ++gd->gd_cnt.v_swtch;
-               TAILQ_REMOVE(&gd->gd_tdrunq[nq], ntd, td_threadq);
-               TAILQ_INSERT_TAIL(&gd->gd_tdrunq[nq], ntd, td_threadq);
+               ntd = TAILQ_NEXT(ntd, td_threadq);
            }
-#else
+
            /*
-            * THREAD SELECTION FOR A UP MACHINE BUILD.  We don't have to
-            * worry about tokens or the BGL.  However, we still have
-            * to call lwkt_getalltokens() in order to properly detect
-            * stale tokens.  This call cannot fail for a UP build!
+            * If we exhausted the run list switch to the idle thread.
+            * Since one or more threads had resource acquisition issues
+            * we do not allow the idle thread to halt.
+            *
+            * NOTE: nlast can be NULL.
             */
-           lwkt_getalltokens(ntd);
-           ++gd->gd_cnt.v_swtch;
-           TAILQ_REMOVE(&gd->gd_tdrunq[nq], ntd, td_threadq);
-           TAILQ_INSERT_TAIL(&gd->gd_tdrunq[nq], ntd, td_threadq);
+           if (ntd == nlast) {
+               cpu_pause();
+               ntd = &gd->gd_idlethread;
+               ntd->td_flags |= TDF_IDLE_NOHLT;
+#ifdef SMP
+               if (ntd->td_mpcount) {
+                   mpheld = MP_LOCK_HELD(gd);
+                   if (gd->gd_trap_nesting_level == 0 && panicstr == NULL)
+                       panic("Idle thread %p was holding the BGL!", ntd);
+                   if (mpheld == 0) {
+                       set_cpu_contention_mask(gd);
+                       handle_cpu_contention_mask();
+                       cpu_try_mplock();
+                       mpheld = MP_LOCK_HELD(gd);
+                       cpu_pause();
+                       break;          /* try again from the top, almost */
+                   }
+               }
 #endif
-       } else {
+
+               /*
+                * If fairq accumulations occured we do not schedule the
+                * idle thread.  This will cause us to try again from
+                * the (almost) top.
+                */
+               if (didaccumulate)
+                       break;          /* try again from the top, almost */
+               if (lmsg)
+                   strlcpy(cpu_time.cp_msg, lmsg, sizeof(cpu_time.cp_msg));
+               cpu_time.cp_stallpc = (uintptr_t)laddr;
+               goto haveidle;
+           }
+
            /*
-            * We have nothing to run but only let the idle loop halt
-            * the cpu if there are no pending interrupts.
+            * Try to switch to this thread.
+            *
+            * NOTE: For UP there is no mplock and lwkt_getalltokens()
+            *       always succeeds.
             */
-           ntd = &gd->gd_idlethread;
-           if (gd->gd_reqflags & RQF_IDLECHECK_MASK)
-               ntd->td_flags |= TDF_IDLE_NOHLT;
+           if ((ntd->td_pri >= TDPRI_KERN_LPSCHED || nquserok ||
+               user_pri_sched) && ntd->td_fairq_accum >= 0 &&
+#ifdef SMP
+               (ntd->td_mpcount == 0 || mpheld || cpu_try_mplock()) &&
+#endif
+               (!TD_TOKS_HELD(ntd) || lwkt_getalltokens(ntd, &lmsg, &laddr))
+           ) {
 #ifdef SMP
-using_idle_thread:
+                   clr_cpu_contention_mask(gd);
+#endif
+                   goto havethread;
+           }
+#ifdef SMP
+           if (ntd->td_fairq_accum >= 0)
+                   set_cpu_contention_mask(gd);
            /*
-            * The idle thread should not be holding the MP lock unless we
-            * are trapping in the kernel or in a panic.  Since we select the
-            * idle thread unconditionally when no other thread is available,
-            * if the MP lock is desired during a panic or kernel trap, we
-            * have to loop in the scheduler until we get it.
+            * Reload mpheld (it become stale after mplock/token ops).
             */
-           if (ntd->td_mpcount) {
-               mpheld = MP_LOCK_HELD();
-               if (gd->gd_trap_nesting_level == 0 && panicstr == NULL) {
-                   panic("Idle thread %p was holding the BGL!", ntd);
-               } else if (mpheld == 0) {
-                   cpu_mplock_contested();
-                   goto again;
-               }
+           mpheld = MP_LOCK_HELD(gd);
+           if (ntd->td_mpcount && mpheld == 0) {
+               lmsg = "mplock";
+               laddr = ntd->td_mplock_stallpc;
            }
+           if (ntd->td_pri >= TDPRI_KERN_LPSCHED && ntd->td_fairq_accum >= 0)
+               nquserok = 0;
 #endif
        }
+
+       /*
+        * All threads exhausted but we can loop due to a negative
+        * accumulator.
+        *
+        * While we are looping in the scheduler be sure to service
+        * any interrupts which were made pending due to our critical
+        * section, otherwise we could livelock (e.g.) IPIs.
+        *
+        * NOTE: splz can enter and exit the mplock so mpheld is
+        * stale after this call.
+        */
+       splz_check();
+
+#ifdef SMP
+       /*
+        * Our mplock can be cached and cause other cpus to livelock
+        * if we loop due to e.g. not being able to acquire tokens.
+        */
+       if (MP_LOCK_HELD(gd))
+           cpu_rel_mplock(gd->gd_cpuid);
+       mpheld = 0;
+#endif
     }
-    KASSERT(ntd->td_pri >= TDPRI_CRIT,
-       ("priority problem in lwkt_switch %d %d", td->td_pri, ntd->td_pri));
 
     /*
-     * Do the actual switch.  If the new target does not need the MP lock
-     * and we are holding it, release the MP lock.  If the new target requires
-     * the MP lock we have already acquired it for the target.
+     * Do the actual switch.  WARNING: mpheld is stale here.
+     *
+     * We must always decrement td_fairq_accum on non-idle threads just
+     * in case a thread never gets a tick due to being in a continuous
+     * critical section.  The page-zeroing code does that.
+     *
+     * If the thread we came up with is a higher or equal priority verses
+     * the thread at the head of the queue we move our thread to the
+     * front.  This way we can always check the front of the queue.
+     */
+havethread:
+    ++gd->gd_cnt.v_swtch;
+    --ntd->td_fairq_accum;
+    xtd = TAILQ_FIRST(&gd->gd_tdrunq);
+    if (ntd != xtd && ntd->td_pri >= xtd->td_pri) {
+       TAILQ_REMOVE(&gd->gd_tdrunq, ntd, td_threadq);
+       TAILQ_INSERT_HEAD(&gd->gd_tdrunq, ntd, td_threadq);
+    }
+havethread_preempted:
+    ;
+    /*
+     * If the new target does not need the MP lock and we are holding it,
+     * release the MP lock.  If the new target requires the MP lock we have
+     * already acquired it for the target.
+     *
+     * WARNING: mpheld is stale here.
      */
+haveidle:
+    KASSERT(ntd->td_critcount,
+           ("priority problem in lwkt_switch %d %d", td->td_pri, ntd->td_pri));
 #ifdef SMP
     if (ntd->td_mpcount == 0 ) {
-       if (MP_LOCK_HELD())
-           cpu_rel_mplock();
+       if (MP_LOCK_HELD(gd))
+           cpu_rel_mplock(gd->gd_cpuid);
     } else {
        ASSERT_MP_LOCK_HELD(ntd);
     }
 #endif
     if (td != ntd) {
        ++switch_count;
-       KKASSERT(jg_tos_ok(ntd));
+#ifdef __x86_64__
+       {
+           int tos_ok __debugvar = jg_tos_ok(ntd);
+           KKASSERT(tos_ok);
+       }
+#endif
+       KTR_LOG(ctxsw_sw, gd->gd_cpuid, ntd);
        td->td_switch(ntd);
     }
     /* NOTE: current cpu may have changed after switch */
@@ -787,7 +919,7 @@ using_idle_thread:
  *
  * THE CALLER OF LWKT_PREEMPT() MUST BE IN A CRITICAL SECTION.  Typically
  * this is called via lwkt_schedule() through the td_preemptable callback.
- * critpri is the managed critical priority that we should ignore in order
+ * critcount is the managed critical priority that we should ignore in order
  * to determine whether preemption is possible (aka usually just the crit
  * priority of lwkt_schedule() itself).
  *
@@ -808,7 +940,7 @@ using_idle_thread:
  * can leave it synchronized on return).
  */
 void
-lwkt_preempt(thread_t ntd, int critpri)
+lwkt_preempt(thread_t ntd, int critcount)
 {
     struct globaldata *gd = mycpu;
     thread_t td;
@@ -820,7 +952,7 @@ lwkt_preempt(thread_t ntd, int critpri)
     /*
      * The caller has put us in a critical section.  We can only preempt
      * if the caller of the caller was not in a critical section (basically
-     * a local interrupt), as determined by the 'critpri' parameter.  We
+     * a local interrupt), as determined by the 'critcount' parameter.  We
      * also can't preempt if the caller is holding any spinlocks (even if
      * he isn't in a critical section).  This also handles the tokens test.
      *
@@ -829,14 +961,14 @@ lwkt_preempt(thread_t ntd, int critpri)
      *
      * Set need_lwkt_resched() unconditionally for now YYY.
      */
-    KASSERT(ntd->td_pri >= TDPRI_CRIT, ("BADCRIT0 %d", ntd->td_pri));
+    KASSERT(ntd->td_critcount, ("BADCRIT0 %d", ntd->td_pri));
 
     td = gd->gd_curthread;
-    if ((ntd->td_pri & TDPRI_MASK) <= (td->td_pri & TDPRI_MASK)) {
+    if (ntd->td_pri <= td->td_pri) {
        ++preempt_miss;
        return;
     }
-    if ((td->td_pri & ~TDPRI_MASK) > critpri) {
+    if (td->td_critcount > critcount) {
        ++preempt_miss;
        need_lwkt_resched();
        return;
@@ -849,24 +981,17 @@ lwkt_preempt(thread_t ntd, int critpri)
     }
 #endif
     /*
-     * Take the easy way out and do not preempt if we are holding
-     * any spinlocks.  We could test whether the thread(s) being
-     * preempted interlock against the target thread's tokens and whether
-     * we can get all the target thread's tokens, but this situation 
-     * should not occur very often so its easier to simply not preempt.
-     * Also, plain spinlocks are impossible to figure out at this point so 
-     * just don't preempt.
+     * We don't have to check spinlocks here as they will also bump
+     * td_critcount.
      *
      * Do not try to preempt if the target thread is holding any tokens.
      * We could try to acquire the tokens but this case is so rare there
      * is no need to support it.
      */
-    if (gd->gd_spinlock_rd || gd->gd_spinlocks_wr) {
-       ++preempt_miss;
-       need_lwkt_resched();
-       return;
-    }
-    if (ntd->td_toks) {
+    KKASSERT(gd->gd_spinlock_rd == NULL);
+    KKASSERT(gd->gd_spinlocks_wr == 0);
+
+    if (TD_TOKS_HELD(ntd)) {
        ++preempt_miss;
        need_lwkt_resched();
        return;
@@ -891,7 +1016,7 @@ lwkt_preempt(thread_t ntd, int critpri)
      * or not.
      */
     savecnt = td->td_mpcount;
-    mpheld = MP_LOCK_HELD();
+    mpheld = MP_LOCK_HELD(gd);
     ntd->td_mpcount += td->td_mpcount;
     if (mpheld == 0 && ntd->td_mpcount && !cpu_try_mplock()) {
        ntd->td_mpcount -= td->td_mpcount;
@@ -908,14 +1033,15 @@ lwkt_preempt(thread_t ntd, int critpri)
     ++preempt_hit;
     ntd->td_preempted = td;
     td->td_flags |= TDF_PREEMPT_LOCK;
+    KTR_LOG(ctxsw_pre, gd->gd_cpuid, ntd);
     td->td_switch(ntd);
 
     KKASSERT(ntd->td_preempted && (td->td_flags & TDF_PREEMPT_DONE));
 #ifdef SMP
     KKASSERT(savecnt == td->td_mpcount);
-    mpheld = MP_LOCK_HELD();
+    mpheld = MP_LOCK_HELD(gd);
     if (mpheld && td->td_mpcount == 0)
-       cpu_rel_mplock();
+       cpu_rel_mplock(gd->gd_cpuid);
     else if (mpheld == 0 && td->td_mpcount)
        panic("lwkt_preempt(): MP lock was not held through");
 #endif
@@ -924,100 +1050,134 @@ lwkt_preempt(thread_t ntd, int critpri)
 }
 
 /*
- * Yield our thread while higher priority threads are pending.  This is
- * typically called when we leave a critical section but it can be safely
- * called while we are in a critical section.
- *
- * This function will not generally yield to equal priority threads but it
- * can occur as a side effect.  Note that lwkt_switch() is called from
- * inside the critical section to prevent its own crit_exit() from reentering
- * lwkt_yield_quick().
+ * Conditionally call splz() if gd_reqflags indicates work is pending.
  *
- * gd_reqflags indicates that *something* changed, e.g. an interrupt or softint
- * came along but was blocked and made pending.
+ * td_nest_count prevents deep nesting via splz() or doreti() which
+ * might otherwise blow out the kernel stack.  Note that except for
+ * this special case, we MUST call splz() here to handle any
+ * pending ints, particularly after we switch, or we might accidently
+ * halt the cpu with interrupts pending.
  *
  * (self contained on a per cpu basis)
  */
 void
-lwkt_yield_quick(void)
+splz_check(void)
 {
     globaldata_t gd = mycpu;
     thread_t td = gd->gd_curthread;
 
-    /*
-     * gd_reqflags is cleared in splz if the cpl is 0.  If we were to clear
-     * it with a non-zero cpl then we might not wind up calling splz after
-     * a task switch when the critical section is exited even though the
-     * new task could accept the interrupt.
-     *
-     * XXX from crit_exit() only called after last crit section is released.
-     * If called directly will run splz() even if in a critical section.
-     *
-     * td_nest_count prevent deep nesting via splz() or doreti().  Note that
-     * except for this special case, we MUST call splz() here to handle any
-     * pending ints, particularly after we switch, or we might accidently
-     * halt the cpu with interrupts pending.
-     */
-    if (gd->gd_reqflags && td->td_nest_count < 2)
+    if ((gd->gd_reqflags & RQF_IDLECHECK_MASK) && td->td_nest_count < 2)
        splz();
+}
 
-    /*
-     * YYY enabling will cause wakeup() to task-switch, which really
-     * confused the old 4.x code.  This is a good way to simulate
-     * preemption and MP without actually doing preemption or MP, because a
-     * lot of code assumes that wakeup() does not block.
-     */
-    if (untimely_switch && td->td_nest_count == 0 &&
-       gd->gd_intr_nesting_level == 0
-    ) {
-       crit_enter_quick(td);
-       /*
-        * YYY temporary hacks until we disassociate the userland scheduler
-        * from the LWKT scheduler.
-        */
-       if (td->td_flags & TDF_RUNQ) {
-           lwkt_switch();              /* will not reenter yield function */
-       } else {
-           lwkt_schedule_self(td);     /* make sure we are scheduled */
-           lwkt_switch();              /* will not reenter yield function */
-           lwkt_deschedule_self(td);   /* make sure we are descheduled */
-       }
-       crit_exit_noyield(td);
-    }
+/*
+ * This function is used to negotiate a passive release of the current
+ * process/lwp designation with the user scheduler, allowing the user
+ * scheduler to schedule another user thread.  The related kernel thread
+ * (curthread) continues running in the released state.
+ */
+void
+lwkt_passive_release(struct thread *td)
+{
+    struct lwp *lp = td->td_lwp;
+
+    td->td_release = NULL;
+    lwkt_setpri_self(TDPRI_KERN_USER);
+    lp->lwp_proc->p_usched->release_curproc(lp);
 }
 
+
 /*
- * This implements a normal yield which, unlike _quick, will yield to equal
- * priority threads as well.  Note that gd_reqflags tests will be handled by
- * the crit_exit() call in lwkt_switch().
+ * This implements a normal yield.  This routine is virtually a nop if
+ * there is nothing to yield to but it will always run any pending interrupts
+ * if called from a critical section.
+ *
+ * This yield is designed for kernel threads without a user context.
  *
  * (self contained on a per cpu basis)
  */
 void
 lwkt_yield(void)
 {
-    lwkt_schedule_self(curthread);
-    lwkt_switch();
+    globaldata_t gd = mycpu;
+    thread_t td = gd->gd_curthread;
+    thread_t xtd;
+
+    if ((gd->gd_reqflags & RQF_IDLECHECK_MASK) && td->td_nest_count < 2)
+       splz();
+    if (td->td_fairq_accum < 0) {
+       lwkt_schedule_self(curthread);
+       lwkt_switch();
+    } else {
+       xtd = TAILQ_FIRST(&gd->gd_tdrunq);
+       if (xtd && xtd->td_pri > td->td_pri) {
+           lwkt_schedule_self(curthread);
+           lwkt_switch();
+       }
+    }
 }
 
 /*
- * Return 0 if no runnable threads are pending at the same or higher
- * priority as the passed thread.
+ * This yield is designed for kernel threads with a user context.
  *
- * Return 1 if runnable threads are pending at the same priority.
+ * The kernel acting on behalf of the user is potentially cpu-bound,
+ * this function will efficiently allow other threads to run and also
+ * switch to other processes by releasing.
  *
- * Return 2 if runnable threads are pending at a higher priority.
+ * The lwkt_user_yield() function is designed to have very low overhead
+ * if no yield is determined to be needed.
  */
-int
-lwkt_check_resched(thread_t td)
+void
+lwkt_user_yield(void)
 {
-       int pri = td->td_pri & TDPRI_MASK;
+    globaldata_t gd = mycpu;
+    thread_t td = gd->gd_curthread;
+
+    /*
+     * Always run any pending interrupts in case we are in a critical
+     * section.
+     */
+    if ((gd->gd_reqflags & RQF_IDLECHECK_MASK) && td->td_nest_count < 2)
+       splz();
 
-       if (td->td_gd->gd_runqmask > (2 << pri) - 1)
-               return(2);
-       if (TAILQ_NEXT(td, td_threadq))
-               return(1);
-       return(0);
+#ifdef SMP
+    /*
+     * XXX SEVERE TEMPORARY HACK.  A cpu-bound operation running in the
+     * kernel can prevent other cpus from servicing interrupt threads
+     * which still require the MP lock (which is a lot of them).  This
+     * has a chaining effect since if the interrupt is blocked, so is
+     * the event, so normal scheduling will not pick up on the problem.
+     */
+    if (cpu_contention_mask && td->td_mpcount) {
+       yield_mplock(td);
+    }
+#endif
+
+    /*
+     * Switch (which forces a release) if another kernel thread needs
+     * the cpu, if userland wants us to resched, or if our kernel
+     * quantum has run out.
+     */
+    if (lwkt_resched_wanted() ||
+       user_resched_wanted() ||
+       td->td_fairq_accum < 0)
+    {
+       lwkt_switch();
+    }
+
+#if 0
+    /*
+     * Reacquire the current process if we are released.
+     *
+     * XXX not implemented atm.  The kernel may be holding locks and such,
+     *     so we want the thread to continue to receive cpu.
+     */
+    if (td->td_release == NULL && lp) {
+       lp->lwp_proc->p_usched->acquire_curproc(lp);
+       td->td_release = lwkt_passive_release;
+       lwkt_setpri_self(TDPRI_USER_NORM);
+    }
+#endif
 }
 
 /*
@@ -1041,18 +1201,31 @@ lwkt_check_resched(thread_t td)
  */
 static __inline
 void
-_lwkt_schedule_post(globaldata_t gd, thread_t ntd, int cpri, int reschedok)
+_lwkt_schedule_post(globaldata_t gd, thread_t ntd, int ccount, int reschedok)
 {
     thread_t otd;
 
     if (ntd->td_flags & TDF_RUNQ) {
        if (ntd->td_preemptable && reschedok) {
-           ntd->td_preemptable(ntd, cpri);     /* YYY +token */
+           ntd->td_preemptable(ntd, ccount);   /* YYY +token */
        } else if (reschedok) {
            otd = curthread;
-           if ((ntd->td_pri & TDPRI_MASK) > (otd->td_pri & TDPRI_MASK))
+           if (ntd->td_pri > otd->td_pri)
                need_lwkt_resched();
        }
+
+       /*
+        * Give the thread a little fair share scheduler bump if it
+        * has been asleep for a while.  This is primarily to avoid
+        * a degenerate case for interrupt threads where accumulator
+        * crosses into negative territory unnecessarily.
+        */
+       if (ntd->td_fairq_lticks != ticks) {
+           ntd->td_fairq_lticks = ticks;
+           ntd->td_fairq_accum += gd->gd_fairq_total_pri;
+           if (ntd->td_fairq_accum > TDFAIRQ_MAX(gd))
+                   ntd->td_fairq_accum = TDFAIRQ_MAX(gd);
+       }
     }
 }
 
@@ -1076,13 +1249,13 @@ _lwkt_schedule(thread_t td, int reschedok)
 #ifdef SMP
        if (td->td_gd == mygd) {
            _lwkt_enqueue(td);
-           _lwkt_schedule_post(mygd, td, TDPRI_CRIT, reschedok);
+           _lwkt_schedule_post(mygd, td, 1, reschedok);
        } else {
-           lwkt_send_ipiq(td->td_gd, (ipifunc1_t)lwkt_schedule, td);
+           lwkt_send_ipiq3(td->td_gd, lwkt_schedule_remote, td, 0);
        }
 #else
        _lwkt_enqueue(td);
-       _lwkt_schedule_post(mygd, td, TDPRI_CRIT, reschedok);
+       _lwkt_schedule_post(mygd, td, 1, reschedok);
 #endif
     }
     crit_exit_gd(mygd);
@@ -1102,6 +1275,28 @@ lwkt_schedule_noresched(thread_t td)
 
 #ifdef SMP
 
+/*
+ * When scheduled remotely if frame != NULL the IPIQ is being
+ * run via doreti or an interrupt then preemption can be allowed.
+ *
+ * To allow preemption we have to drop the critical section so only
+ * one is present in _lwkt_schedule_post.
+ */
+static void
+lwkt_schedule_remote(void *arg, int arg2, struct intrframe *frame)
+{
+    thread_t td = curthread;
+    thread_t ntd = arg;
+
+    if (frame && ntd->td_preemptable) {
+       crit_exit_noyield(td);
+       _lwkt_schedule(ntd, 1);
+       crit_enter_quick(td);
+    } else {
+       _lwkt_schedule(ntd, 1);
+    }
+}
+
 /*
  * Thread migration using a 'Pull' method.  The thread may or may not be
  * the current thread.  It MUST be descheduled and in a stable state.
@@ -1110,18 +1305,23 @@ lwkt_schedule_noresched(thread_t td)
  * At any point after lwkt_giveaway() is called, the target cpu may
  * 'pull' the thread by calling lwkt_acquire().
  *
+ * We have to make sure the thread is not sitting on a per-cpu tsleep
+ * queue or it will blow up when it moves to another cpu.
+ *
  * MPSAFE - must be called under very specific conditions.
  */
 void
 lwkt_giveaway(thread_t td)
 {
-       globaldata_t gd = mycpu;
+    globaldata_t gd = mycpu;
 
-       crit_enter_gd(gd);
-       KKASSERT(td->td_gd == gd);
-       TAILQ_REMOVE(&gd->gd_tdallq, td, td_allq);
-       td->td_flags |= TDF_MIGRATING;
-       crit_exit_gd(gd);
+    crit_enter_gd(gd);
+    if (td->td_flags & TDF_TSLEEPQ)
+       tsleep_remove(td);
+    KKASSERT(td->td_gd == gd);
+    TAILQ_REMOVE(&gd->gd_tdallq, td, td_allq);
+    td->td_flags |= TDF_MIGRATING;
+    crit_exit_gd(gd);
 }
 
 void
@@ -1188,29 +1388,40 @@ lwkt_deschedule(thread_t td)
  * Set the target thread's priority.  This routine does not automatically
  * switch to a higher priority thread, LWKT threads are not designed for
  * continuous priority changes.  Yield if you want to switch.
- *
- * We have to retain the critical section count which uses the high bits
- * of the td_pri field.  The specified priority may also indicate zero or
- * more critical sections by adding TDPRI_CRIT*N.
- *
- * Note that we requeue the thread whether it winds up on a different runq
- * or not.  uio_yield() depends on this and the routine is not normally
- * called with the same priority otherwise.
  */
 void
 lwkt_setpri(thread_t td, int pri)
 {
-    KKASSERT(pri >= 0);
     KKASSERT(td->td_gd == mycpu);
-    crit_enter();
-    if (td->td_flags & TDF_RUNQ) {
-       _lwkt_dequeue(td);
-       td->td_pri = (td->td_pri & ~TDPRI_MASK) + pri;
-       _lwkt_enqueue(td);
-    } else {
-       td->td_pri = (td->td_pri & ~TDPRI_MASK) + pri;
+    if (td->td_pri != pri) {
+       KKASSERT(pri >= 0);
+       crit_enter();
+       if (td->td_flags & TDF_RUNQ) {
+           _lwkt_dequeue(td);
+           td->td_pri = pri;
+           _lwkt_enqueue(td);
+       } else {
+           td->td_pri = pri;
+       }
+       crit_exit();
     }
-    crit_exit();
+}
+
+/*
+ * Set the initial priority for a thread prior to it being scheduled for
+ * the first time.  The thread MUST NOT be scheduled before or during
+ * this call.  The thread may be assigned to a cpu other then the current
+ * cpu.
+ *
+ * Typically used after a thread has been created with TDF_STOPPREQ,
+ * and before the thread is initially scheduled.
+ */
+void
+lwkt_setpri_initial(thread_t td, int pri)
+{
+    KKASSERT(pri >= 0);
+    KKASSERT((td->td_flags & TDF_RUNQ) == 0);
+    td->td_pri = pri;
 }
 
 void
@@ -1222,14 +1433,46 @@ lwkt_setpri_self(int pri)
     crit_enter();
     if (td->td_flags & TDF_RUNQ) {
        _lwkt_dequeue(td);
-       td->td_pri = (td->td_pri & ~TDPRI_MASK) + pri;
+       td->td_pri = pri;
        _lwkt_enqueue(td);
     } else {
-       td->td_pri = (td->td_pri & ~TDPRI_MASK) + pri;
+       td->td_pri = pri;
     }
     crit_exit();
 }
 
+/*
+ * 1/hz tick (typically 10ms) x TDFAIRQ_SCALE (typ 8) = 80ms full cycle.
+ *
+ * Example: two competing threads, same priority N.  decrement by (2*N)
+ * increment by N*8, each thread will get 4 ticks.
+ */
+void
+lwkt_fairq_schedulerclock(thread_t td)
+{
+    if (fairq_enable) {
+       while (td) {
+           if (td != &td->td_gd->gd_idlethread) {
+               td->td_fairq_accum -= td->td_gd->gd_fairq_total_pri;
+               if (td->td_fairq_accum < -TDFAIRQ_MAX(td->td_gd))
+                       td->td_fairq_accum = -TDFAIRQ_MAX(td->td_gd);
+               if (td->td_fairq_accum < 0)
+                       need_lwkt_resched();
+               td->td_fairq_lticks = ticks;
+           }
+           td = td->td_preempted;
+       }
+    }
+}
+
+static void
+lwkt_fairq_accumulate(globaldata_t gd, thread_t td)
+{
+       td->td_fairq_accum += td->td_pri * TDFAIRQ_SCALE;
+       if (td->td_fairq_accum > TDFAIRQ_MAX(td->td_gd))
+               td->td_fairq_accum = TDFAIRQ_MAX(td->td_gd);
+}
+
 /*
  * Migrate the current thread to the specified cpu. 
  *
@@ -1237,6 +1480,10 @@ lwkt_setpri_self(int pri)
  * moving our thread to the tdallq of the target cpu, IPI messaging the
  * target cpu, and switching out.  TDF_MIGRATING prevents scheduling
  * races while the thread is being migrated.
+ *
+ * We must be sure to remove ourselves from the current cpu's tsleepq
+ * before potentially moving to another queue.  The thread can be on
+ * a tsleepq due to a left-over tsleep_interlock().
  */
 #ifdef SMP
 static void lwkt_setcpu_remote(void *arg);
@@ -1250,6 +1497,8 @@ lwkt_setcpu_self(globaldata_t rgd)
 
     if (td->td_gd != rgd) {
        crit_enter_quick(td);
+       if (td->td_flags & TDF_TSLEEPQ)
+           tsleep_remove(td);
        td->td_flags |= TDF_MIGRATING;
        lwkt_deschedule_self(td);
        TAILQ_REMOVE(&td->td_gd->gd_tdallq, td, td_allq);
@@ -1324,9 +1573,8 @@ lwkt_preempted_proc(void)
  * rel_mplock() at the start of the new thread.
  */
 int
-lwkt_create(void (*func)(void *), void *arg,
-    struct thread **tdp, thread_t template, int tdflags, int cpu,
-    const char *fmt, ...)
+lwkt_create(void (*func)(void *), void *arg, struct thread **tdp,
+           thread_t template, int tdflags, int cpu, const char *fmt, ...)
 {
     thread_t td;
     __va_list ap;
@@ -1383,6 +1631,15 @@ lwkt_exit(void)
        gd->gd_freetd = NULL;
        objcache_put(thread_cache, std);
     }
+
+    /*
+     * Remove thread resources from kernel lists and deschedule us for
+     * the last time.
+     */
+    if (td->td_flags & TDF_TSLEEPQ)
+       tsleep_remove(td);
+    biosched_done(td);
+    dsched_exit_thread(td);
     lwkt_deschedule_self(td);
     lwkt_remove_tdallq(td);
     if (td->td_flags & TDF_ALLOCATED_THREAD)
@@ -1401,10 +1658,10 @@ void
 crit_panic(void)
 {
     thread_t td = curthread;
-    int lpri = td->td_pri;
+    int lcrit = td->td_critcount;
 
-    td->td_pri = 0;
-    panic("td_pri is/would-go negative! %p %d", td, lpri);
+    td->td_critcount = 0;
+    panic("td_critcount is/would-go negative! %p %d", td, lcrit);
 }
 
 #ifdef SMP
@@ -1433,72 +1690,4 @@ lwkt_smp_stopped(void)
     crit_exit_gd(gd);
 }
 
-/*
- * get_mplock() calls this routine if it is unable to obtain the MP lock.
- * get_mplock() has already incremented td_mpcount.  We must block and
- * not return until giant is held.
- *
- * All we have to do is lwkt_switch() away.  The LWKT scheduler will not
- * reschedule the thread until it can obtain the giant lock for it.
- */
-void
-lwkt_mp_lock_contested(void)
-{
-    loggiant(beg);
-    lwkt_switch();
-    loggiant(end);
-}
-
-/*
- * The rel_mplock() code will call this function after releasing the
- * last reference on the MP lock if mp_lock_contention_mask is non-zero.
- *
- * We then chain an IPI to a single other cpu potentially needing the
- * lock.  This is a bit heuristical and we can wind up with IPIs flying
- * all over the place.
- */
-static void lwkt_mp_lock_uncontested_remote(void *arg __unused);
-
-void
-lwkt_mp_lock_uncontested(void)
-{
-    globaldata_t gd;
-    globaldata_t dgd;
-    cpumask_t mask;
-    cpumask_t tmpmask;
-    int cpuid;
-
-    if (chain_mplock) {
-       gd = mycpu;
-       atomic_clear_int(&mp_lock_contention_mask, gd->gd_cpumask);
-       mask = mp_lock_contention_mask;
-       tmpmask = ~((1 << gd->gd_cpuid) - 1);
-
-       if (mask) {
-           if (mask & tmpmask)
-                   cpuid = bsfl(mask & tmpmask);
-           else
-                   cpuid = bsfl(mask);
-           atomic_clear_int(&mp_lock_contention_mask, 1 << cpuid);
-           dgd = globaldata_find(cpuid);
-           lwkt_send_ipiq(dgd, lwkt_mp_lock_uncontested_remote, NULL);
-       }
-    }
-}
-
-/*
- * The idea is for this IPI to interrupt a potentially lower priority
- * thread, such as a user thread, to allow the scheduler to reschedule
- * a higher priority kernel thread that needs the MP lock.
- *
- * For now we set the LWKT reschedule flag which generates an AST in
- * doreti, though theoretically it is also possible to possibly preempt
- * here if the underlying thread was operating in user mode.  Nah.
- */
-static void
-lwkt_mp_lock_uncontested_remote(void *arg __unused)
-{
-       need_lwkt_resched();
-}
-
 #endif