0494e91aa83d6610c3b3d66425babd405eee2f3d
[dragonfly.git] / sys / kern / usched_bsd4.c
1 /*
2  * Copyright (c) 1999 Peter Wemm <peter@FreeBSD.org>
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions
7  * are met:
8  * 1. Redistributions of source code must retain the above copyright
9  *    notice, this list of conditions and the following disclaimer.
10  * 2. Redistributions in binary form must reproduce the above copyright
11  *    notice, this list of conditions and the following disclaimer in the
12  *    documentation and/or other materials provided with the distribution.
13  *
14  * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
15  * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
16  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
17  * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
18  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
19  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
20  * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
21  * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
22  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
23  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
24  * SUCH DAMAGE.
25  *
26  * $DragonFly: src/sys/kern/usched_bsd4.c,v 1.26 2008/11/01 23:31:19 dillon Exp $
27  */
28
29 #include <sys/param.h>
30 #include <sys/systm.h>
31 #include <sys/kernel.h>
32 #include <sys/lock.h>
33 #include <sys/queue.h>
34 #include <sys/proc.h>
35 #include <sys/rtprio.h>
36 #include <sys/uio.h>
37 #include <sys/sysctl.h>
38 #include <sys/resourcevar.h>
39 #include <sys/spinlock.h>
40 #include <machine/cpu.h>
41 #include <machine/smp.h>
42
43 #include <sys/thread2.h>
44 #include <sys/spinlock2.h>
45 #include <sys/mplock2.h>
46
47 /*
48  * Priorities.  Note that with 32 run queues per scheduler each queue
49  * represents four priority levels.
50  */
51
52 #define MAXPRI                  128
53 #define PRIMASK                 (MAXPRI - 1)
54 #define PRIBASE_REALTIME        0
55 #define PRIBASE_NORMAL          MAXPRI
56 #define PRIBASE_IDLE            (MAXPRI * 2)
57 #define PRIBASE_THREAD          (MAXPRI * 3)
58 #define PRIBASE_NULL            (MAXPRI * 4)
59
60 #define NQS     32                      /* 32 run queues. */
61 #define PPQ     (MAXPRI / NQS)          /* priorities per queue */
62 #define PPQMASK (PPQ - 1)
63
64 /*
65  * NICEPPQ      - number of nice units per priority queue
66  * ESTCPURAMP   - number of scheduler ticks for estcpu to switch queues
67  *
68  * ESTCPUPPQ    - number of estcpu units per priority queue
69  * ESTCPUMAX    - number of estcpu units
70  * ESTCPUINCR   - amount we have to increment p_estcpu per scheduling tick at
71  *                100% cpu.
72  */
73 #define NICEPPQ         2
74 #define ESTCPURAMP      4
75 #define ESTCPUPPQ       512
76 #define ESTCPUMAX       (ESTCPUPPQ * NQS)
77 #define ESTCPUINCR      (ESTCPUPPQ / ESTCPURAMP)
78 #define PRIO_RANGE      (PRIO_MAX - PRIO_MIN + 1)
79
80 #define ESTCPULIM(v)    min((v), ESTCPUMAX)
81
82 TAILQ_HEAD(rq, lwp);
83
84 #define lwp_priority    lwp_usdata.bsd4.priority
85 #define lwp_rqindex     lwp_usdata.bsd4.rqindex
86 #define lwp_origcpu     lwp_usdata.bsd4.origcpu
87 #define lwp_estcpu      lwp_usdata.bsd4.estcpu
88 #define lwp_rqtype      lwp_usdata.bsd4.rqtype
89
90 static void bsd4_acquire_curproc(struct lwp *lp);
91 static void bsd4_release_curproc(struct lwp *lp);
92 static void bsd4_select_curproc(globaldata_t gd);
93 static void bsd4_setrunqueue(struct lwp *lp);
94 static void bsd4_schedulerclock(struct lwp *lp, sysclock_t period,
95                                 sysclock_t cpstamp);
96 static void bsd4_recalculate_estcpu(struct lwp *lp);
97 static void bsd4_resetpriority(struct lwp *lp);
98 static void bsd4_forking(struct lwp *plp, struct lwp *lp);
99 static void bsd4_exiting(struct lwp *plp, struct lwp *lp);
100 static void bsd4_yield(struct lwp *lp);
101
102 #ifdef SMP
103 static void need_user_resched_remote(void *dummy);
104 #endif
105 static struct lwp *chooseproc_locked(struct lwp *chklp);
106 static void bsd4_remrunqueue_locked(struct lwp *lp);
107 static void bsd4_setrunqueue_locked(struct lwp *lp);
108
109 struct usched usched_bsd4 = {
110         { NULL },
111         "bsd4", "Original DragonFly Scheduler",
112         NULL,                   /* default registration */
113         NULL,                   /* default deregistration */
114         bsd4_acquire_curproc,
115         bsd4_release_curproc,
116         bsd4_setrunqueue,
117         bsd4_schedulerclock,
118         bsd4_recalculate_estcpu,
119         bsd4_resetpriority,
120         bsd4_forking,
121         bsd4_exiting,
122         NULL,                   /* setcpumask not supported */
123         bsd4_yield
124 };
125
126 struct usched_bsd4_pcpu {
127         struct thread helper_thread;
128         short   rrcount;
129         short   upri;
130         struct lwp *uschedcp;
131 };
132
133 typedef struct usched_bsd4_pcpu *bsd4_pcpu_t;
134
135 /*
136  * We have NQS (32) run queues per scheduling class.  For the normal
137  * class, there are 128 priorities scaled onto these 32 queues.  New
138  * processes are added to the last entry in each queue, and processes
139  * are selected for running by taking them from the head and maintaining
140  * a simple FIFO arrangement.  Realtime and Idle priority processes have
141  * and explicit 0-31 priority which maps directly onto their class queue
142  * index.  When a queue has something in it, the corresponding bit is
143  * set in the queuebits variable, allowing a single read to determine
144  * the state of all 32 queues and then a ffs() to find the first busy
145  * queue.
146  */
147 static struct rq bsd4_queues[NQS];
148 static struct rq bsd4_rtqueues[NQS];
149 static struct rq bsd4_idqueues[NQS];
150 static u_int32_t bsd4_queuebits;
151 static u_int32_t bsd4_rtqueuebits;
152 static u_int32_t bsd4_idqueuebits;
153 static cpumask_t bsd4_curprocmask = -1; /* currently running a user process */
154 static cpumask_t bsd4_rdyprocmask;      /* ready to accept a user process */
155 static int       bsd4_runqcount;
156 #ifdef SMP
157 static volatile int bsd4_scancpu;
158 #endif
159 static struct spinlock bsd4_spin;
160 static struct usched_bsd4_pcpu bsd4_pcpu[MAXCPU];
161
162 SYSCTL_INT(_debug, OID_AUTO, bsd4_runqcount, CTLFLAG_RD, &bsd4_runqcount, 0, "");
163 #ifdef INVARIANTS
164 static int usched_nonoptimal;
165 SYSCTL_INT(_debug, OID_AUTO, usched_nonoptimal, CTLFLAG_RW,
166         &usched_nonoptimal, 0, "acquire_curproc() was not optimal");
167 static int usched_optimal;
168 SYSCTL_INT(_debug, OID_AUTO, usched_optimal, CTLFLAG_RW,
169         &usched_optimal, 0, "acquire_curproc() was optimal");
170 #endif
171 static int usched_debug = -1;
172 SYSCTL_INT(_debug, OID_AUTO, scdebug, CTLFLAG_RW, &usched_debug, 0, "");
173 #ifdef SMP
174 static int remote_resched_nonaffinity;
175 static int remote_resched_affinity;
176 static int choose_affinity;
177 SYSCTL_INT(_debug, OID_AUTO, remote_resched_nonaffinity, CTLFLAG_RD,
178         &remote_resched_nonaffinity, 0, "Number of remote rescheds");
179 SYSCTL_INT(_debug, OID_AUTO, remote_resched_affinity, CTLFLAG_RD,
180         &remote_resched_affinity, 0, "Number of remote rescheds");
181 SYSCTL_INT(_debug, OID_AUTO, choose_affinity, CTLFLAG_RD,
182         &choose_affinity, 0, "chooseproc() was smart");
183 #endif
184
185 static int usched_bsd4_rrinterval = (ESTCPUFREQ + 9) / 10;
186 SYSCTL_INT(_kern, OID_AUTO, usched_bsd4_rrinterval, CTLFLAG_RW,
187         &usched_bsd4_rrinterval, 0, "");
188 static int usched_bsd4_decay = ESTCPUINCR / 2;
189 SYSCTL_INT(_kern, OID_AUTO, usched_bsd4_decay, CTLFLAG_RW,
190         &usched_bsd4_decay, 0, "");
191
192 /*
193  * Initialize the run queues at boot time.
194  */
195 static void
196 rqinit(void *dummy)
197 {
198         int i;
199
200         spin_init(&bsd4_spin);
201         for (i = 0; i < NQS; i++) {
202                 TAILQ_INIT(&bsd4_queues[i]);
203                 TAILQ_INIT(&bsd4_rtqueues[i]);
204                 TAILQ_INIT(&bsd4_idqueues[i]);
205         }
206         atomic_clear_int(&bsd4_curprocmask, 1);
207 }
208 SYSINIT(runqueue, SI_BOOT2_USCHED, SI_ORDER_FIRST, rqinit, NULL)
209
210 /*
211  * BSD4_ACQUIRE_CURPROC
212  *
213  * This function is called when the kernel intends to return to userland.
214  * It is responsible for making the thread the current designated userland
215  * thread for this cpu, blocking if necessary.
216  *
217  * The kernel has already depressed our LWKT priority so we must not switch
218  * until we have either assigned or disposed of the thread.
219  *
220  * WARNING! THIS FUNCTION IS ALLOWED TO CAUSE THE CURRENT THREAD TO MIGRATE
221  * TO ANOTHER CPU!  Because most of the kernel assumes that no migration will
222  * occur, this function is called only under very controlled circumstances.
223  *
224  * MPSAFE
225  */
226 static void
227 bsd4_acquire_curproc(struct lwp *lp)
228 {
229         globaldata_t gd;
230         bsd4_pcpu_t dd;
231         struct lwp *olp;
232
233         crit_enter();
234         bsd4_recalculate_estcpu(lp);
235
236         /*
237          * If a reschedule was requested give another thread the
238          * driver's seat.
239          */
240         if (user_resched_wanted()) {
241                 clear_user_resched();
242                 bsd4_release_curproc(lp);
243         }
244
245         /*
246          * Loop until we are the current user thread
247          */
248         do {
249                 /*
250                  * Reload after a switch or setrunqueue/switch possibly
251                  * moved us to another cpu.
252                  */
253                 clear_lwkt_resched();
254                 gd = mycpu;
255                 dd = &bsd4_pcpu[gd->gd_cpuid];
256
257                 /*
258                  * Become the currently scheduled user thread for this cpu
259                  * if we can do so trivially.
260                  *
261                  * We can steal another thread's current thread designation
262                  * on this cpu since if we are running that other thread
263                  * must not be, so we can safely deschedule it.
264                  */
265                 if (dd->uschedcp == lp) {
266                         dd->upri = lp->lwp_priority;
267                 } else if (dd->uschedcp == NULL) {
268                         atomic_set_int(&bsd4_curprocmask, gd->gd_cpumask);
269                         dd->uschedcp = lp;
270                         dd->upri = lp->lwp_priority;
271                 } else if (dd->upri > lp->lwp_priority) {
272                         olp = dd->uschedcp;
273                         dd->uschedcp = lp;
274                         dd->upri = lp->lwp_priority;
275                         lwkt_deschedule(olp->lwp_thread);
276                         bsd4_setrunqueue(olp);
277                 } else {
278                         lwkt_deschedule(lp->lwp_thread);
279                         bsd4_setrunqueue(lp);
280                         lwkt_switch();
281                 }
282
283                 /*
284                  * Other threads at our current user priority have already
285                  * put in their bids, but we must run any kernel threads
286                  * at higher priorities, and we could lose our bid to
287                  * another thread trying to return to user mode in the
288                  * process.
289                  *
290                  * If we lose our bid we will be descheduled and put on
291                  * the run queue.  When we are reactivated we will have
292                  * another chance.
293                  */
294                 lwkt_switch();
295         } while (dd->uschedcp != lp);
296
297         crit_exit();
298         KKASSERT((lp->lwp_flag & LWP_ONRUNQ) == 0);
299 }
300
301 /*
302  * BSD4_RELEASE_CURPROC
303  *
304  * This routine detaches the current thread from the userland scheduler,
305  * usually because the thread needs to run or block in the kernel (at
306  * kernel priority) for a while.
307  *
308  * This routine is also responsible for selecting a new thread to
309  * make the current thread.
310  *
311  * NOTE: This implementation differs from the dummy example in that
312  * bsd4_select_curproc() is able to select the current process, whereas
313  * dummy_select_curproc() is not able to select the current process.
314  * This means we have to NULL out uschedcp.
315  *
316  * Additionally, note that we may already be on a run queue if releasing
317  * via the lwkt_switch() in bsd4_setrunqueue().
318  *
319  * WARNING!  The MP lock may be in an unsynchronized state due to the
320  * way get_mplock() works and the fact that this function may be called
321  * from a passive release during a lwkt_switch().   try_mplock() will deal 
322  * with this for us but you should be aware that td_mpcount may not be
323  * useable.
324  *
325  * MPSAFE
326  */
327 static void
328 bsd4_release_curproc(struct lwp *lp)
329 {
330         globaldata_t gd = mycpu;
331         bsd4_pcpu_t dd = &bsd4_pcpu[gd->gd_cpuid];
332
333         if (dd->uschedcp == lp) {
334                 crit_enter();
335                 KKASSERT((lp->lwp_flag & LWP_ONRUNQ) == 0);
336                 dd->uschedcp = NULL;    /* don't let lp be selected */
337                 dd->upri = PRIBASE_NULL;
338                 atomic_clear_int(&bsd4_curprocmask, gd->gd_cpumask);
339                 bsd4_select_curproc(gd);
340                 crit_exit();
341         }
342 }
343
344 /*
345  * BSD4_SELECT_CURPROC
346  *
347  * Select a new current process for this cpu and clear any pending user
348  * reschedule request.  The cpu currently has no current process.
349  *
350  * This routine is also responsible for equal-priority round-robining,
351  * typically triggered from bsd4_schedulerclock().  In our dummy example
352  * all the 'user' threads are LWKT scheduled all at once and we just
353  * call lwkt_switch().
354  *
355  * The calling process is not on the queue and cannot be selected.
356  *
357  * MPSAFE
358  */
359 static
360 void
361 bsd4_select_curproc(globaldata_t gd)
362 {
363         bsd4_pcpu_t dd = &bsd4_pcpu[gd->gd_cpuid];
364         struct lwp *nlp;
365         int cpuid = gd->gd_cpuid;
366
367         crit_enter_gd(gd);
368
369         spin_lock(&bsd4_spin);
370         if ((nlp = chooseproc_locked(dd->uschedcp)) != NULL) {
371                 atomic_set_int(&bsd4_curprocmask, 1 << cpuid);
372                 dd->upri = nlp->lwp_priority;
373                 dd->uschedcp = nlp;
374                 spin_unlock(&bsd4_spin);
375 #ifdef SMP
376                 lwkt_acquire(nlp->lwp_thread);
377 #endif
378                 lwkt_schedule(nlp->lwp_thread);
379         } else if (bsd4_runqcount && (bsd4_rdyprocmask & (1 << cpuid))) {
380                 atomic_clear_int(&bsd4_rdyprocmask, 1 << cpuid);
381                 spin_unlock(&bsd4_spin);
382                 lwkt_schedule(&dd->helper_thread);
383         } else {
384                 spin_unlock(&bsd4_spin);
385         }
386         crit_exit_gd(gd);
387 }
388
389 /*
390  * BSD4_SETRUNQUEUE
391  *
392  * Place the specified lwp on the user scheduler's run queue.  This routine
393  * must be called with the thread descheduled.  The lwp must be runnable.
394  *
395  * The thread may be the current thread as a special case.
396  *
397  * MPSAFE
398  */
399 static void
400 bsd4_setrunqueue(struct lwp *lp)
401 {
402         globaldata_t gd;
403         bsd4_pcpu_t dd;
404 #ifdef SMP
405         int cpuid;
406         cpumask_t mask;
407         cpumask_t tmpmask;
408 #endif
409
410         /*
411          * First validate the process state relative to the current cpu.
412          * We don't need the spinlock for this, just a critical section.
413          * We are in control of the process.
414          */
415         crit_enter();
416         KASSERT(lp->lwp_stat == LSRUN, ("setrunqueue: lwp not LSRUN"));
417         KASSERT((lp->lwp_flag & LWP_ONRUNQ) == 0,
418             ("lwp %d/%d already on runq! flag %08x/%08x", lp->lwp_proc->p_pid,
419              lp->lwp_tid, lp->lwp_proc->p_flag, lp->lwp_flag));
420         KKASSERT((lp->lwp_thread->td_flags & TDF_RUNQ) == 0);
421
422         /*
423          * Note: gd and dd are relative to the target thread's last cpu,
424          * NOT our current cpu.
425          */
426         gd = lp->lwp_thread->td_gd;
427         dd = &bsd4_pcpu[gd->gd_cpuid];
428
429         /*
430          * This process is not supposed to be scheduled anywhere or assigned
431          * as the current process anywhere.  Assert the condition.
432          */
433         KKASSERT(dd->uschedcp != lp);
434
435 #ifndef SMP
436         /*
437          * If we are not SMP we do not have a scheduler helper to kick
438          * and must directly activate the process if none are scheduled.
439          *
440          * This is really only an issue when bootstrapping init since
441          * the caller in all other cases will be a user process, and
442          * even if released (dd->uschedcp == NULL), that process will
443          * kickstart the scheduler when it returns to user mode from
444          * the kernel.
445          */
446         if (dd->uschedcp == NULL) {
447                 atomic_set_int(&bsd4_curprocmask, gd->gd_cpumask);
448                 dd->uschedcp = lp;
449                 dd->upri = lp->lwp_priority;
450                 lwkt_schedule(lp->lwp_thread);
451                 crit_exit();
452                 return;
453         }
454 #endif
455
456 #ifdef SMP
457         /*
458          * XXX fixme.  Could be part of a remrunqueue/setrunqueue
459          * operation when the priority is recalculated, so TDF_MIGRATING
460          * may already be set.
461          */
462         if ((lp->lwp_thread->td_flags & TDF_MIGRATING) == 0)
463                 lwkt_giveaway(lp->lwp_thread);
464 #endif
465
466         /*
467          * We lose control of lp the moment we release the spinlock after
468          * having placed lp on the queue.  i.e. another cpu could pick it
469          * up and it could exit, or its priority could be further adjusted,
470          * or something like that.
471          */
472         spin_lock(&bsd4_spin);
473         bsd4_setrunqueue_locked(lp);
474
475 #ifdef SMP
476         /*
477          * Kick the scheduler helper on one of the other cpu's
478          * and request a reschedule if appropriate.
479          */
480         cpuid = (bsd4_scancpu & 0xFFFF) % ncpus;
481         ++bsd4_scancpu;
482         mask = ~bsd4_curprocmask & bsd4_rdyprocmask &
483                 lp->lwp_cpumask & smp_active_mask;
484         spin_unlock(&bsd4_spin);
485
486         while (mask) {
487                 tmpmask = ~((1 << cpuid) - 1);
488                 if (mask & tmpmask)
489                         cpuid = bsfl(mask & tmpmask);
490                 else
491                         cpuid = bsfl(mask);
492                 gd = globaldata_find(cpuid);
493                 dd = &bsd4_pcpu[cpuid];
494
495                 if ((dd->upri & ~PPQMASK) > (lp->lwp_priority & ~PPQMASK)) {
496                         if (gd == mycpu)
497                                 need_user_resched_remote(NULL);
498                         else
499                                 lwkt_send_ipiq(gd, need_user_resched_remote, NULL);
500                         break;
501                 }
502                 mask &= ~(1 << cpuid);
503         }
504 #else
505         /*
506          * Request a reschedule if appropriate.
507          */
508         spin_unlock(&bsd4_spin);
509         if ((dd->upri & ~PPQMASK) > (lp->lwp_priority & ~PPQMASK)) {
510                 need_user_resched();
511         }
512 #endif
513         crit_exit();
514 }
515
516 /*
517  * This routine is called from a systimer IPI.  It MUST be MP-safe and
518  * the BGL IS NOT HELD ON ENTRY.  This routine is called at ESTCPUFREQ on
519  * each cpu.
520  *
521  * MPSAFE
522  */
523 static
524 void
525 bsd4_schedulerclock(struct lwp *lp, sysclock_t period, sysclock_t cpstamp)
526 {
527         globaldata_t gd = mycpu;
528         bsd4_pcpu_t dd = &bsd4_pcpu[gd->gd_cpuid];
529
530         /*
531          * Do we need to round-robin?  We round-robin 10 times a second.
532          * This should only occur for cpu-bound batch processes.
533          */
534         if (++dd->rrcount >= usched_bsd4_rrinterval) {
535                 dd->rrcount = 0;
536                 need_user_resched();
537         }
538
539         /*
540          * As the process accumulates cpu time p_estcpu is bumped and may
541          * push the process into another scheduling queue.  It typically
542          * takes 4 ticks to bump the queue.
543          */
544         lp->lwp_estcpu = ESTCPULIM(lp->lwp_estcpu + ESTCPUINCR);
545
546         /*
547          * Reducing p_origcpu over time causes more of our estcpu to be
548          * returned to the parent when we exit.  This is a small tweak
549          * for the batch detection heuristic.
550          */
551         if (lp->lwp_origcpu)
552                 --lp->lwp_origcpu;
553
554         /*
555          * Spinlocks also hold a critical section so there should not be
556          * any active.
557          */
558         KKASSERT(gd->gd_spinlocks_wr == 0);
559
560         bsd4_resetpriority(lp);
561 #if 0
562         /*
563         * if we can't call bsd4_resetpriority for some reason we must call
564          * need user_resched().
565          */
566         need_user_resched();
567 #endif
568 }
569
570 /*
571  * Called from acquire and from kern_synch's one-second timer (one of the
572  * callout helper threads) with a critical section held. 
573  *
574  * Decay p_estcpu based on the number of ticks we haven't been running
575  * and our p_nice.  As the load increases each process observes a larger
576  * number of idle ticks (because other processes are running in them).
577  * This observation leads to a larger correction which tends to make the
578  * system more 'batchy'.
579  *
580  * Note that no recalculation occurs for a process which sleeps and wakes
581  * up in the same tick.  That is, a system doing thousands of context
582  * switches per second will still only do serious estcpu calculations
583  * ESTCPUFREQ times per second.
584  *
585  * MPSAFE
586  */
587 static
588 void 
589 bsd4_recalculate_estcpu(struct lwp *lp)
590 {
591         globaldata_t gd = mycpu;
592         sysclock_t cpbase;
593         int loadfac;
594         int ndecay;
595         int nticks;
596         int nleft;
597
598         /*
599          * We have to subtract periodic to get the last schedclock
600          * timeout time, otherwise we would get the upcoming timeout.
601          * Keep in mind that a process can migrate between cpus and
602          * while the scheduler clock should be very close, boundary
603          * conditions could lead to a small negative delta.
604          */
605         cpbase = gd->gd_schedclock.time - gd->gd_schedclock.periodic;
606
607         if (lp->lwp_slptime > 1) {
608                 /*
609                  * Too much time has passed, do a coarse correction.
610                  */
611                 lp->lwp_estcpu = lp->lwp_estcpu >> 1;
612                 bsd4_resetpriority(lp);
613                 lp->lwp_cpbase = cpbase;
614                 lp->lwp_cpticks = 0;
615         } else if (lp->lwp_cpbase != cpbase) {
616                 /*
617                  * Adjust estcpu if we are in a different tick.  Don't waste
618                  * time if we are in the same tick. 
619                  * 
620                  * First calculate the number of ticks in the measurement
621                  * interval.  The nticks calculation can wind up 0 due to
622                  * a bug in the handling of lwp_slptime  (as yet not found),
623                  * so make sure we do not get a divide by 0 panic.
624                  */
625                 nticks = (cpbase - lp->lwp_cpbase) / gd->gd_schedclock.periodic;
626                 if (nticks <= 0)
627                         nticks = 1;
628                 updatepcpu(lp, lp->lwp_cpticks, nticks);
629
630                 if ((nleft = nticks - lp->lwp_cpticks) < 0)
631                         nleft = 0;
632                 if (usched_debug == lp->lwp_proc->p_pid) {
633                         kprintf("pid %d tid %d estcpu %d cpticks %d nticks %d nleft %d",
634                                 lp->lwp_proc->p_pid, lp->lwp_tid, lp->lwp_estcpu,
635                                 lp->lwp_cpticks, nticks, nleft);
636                 }
637
638                 /*
639                  * Calculate a decay value based on ticks remaining scaled
640                  * down by the instantanious load and p_nice.
641                  */
642                 if ((loadfac = bsd4_runqcount) < 2)
643                         loadfac = 2;
644                 ndecay = nleft * usched_bsd4_decay * 2 * 
645                         (PRIO_MAX * 2 - lp->lwp_proc->p_nice) / (loadfac * PRIO_MAX * 2);
646
647                 /*
648                  * Adjust p_estcpu.  Handle a border case where batch jobs
649                  * can get stalled long enough to decay to zero when they
650                  * shouldn't.
651                  */
652                 if (lp->lwp_estcpu > ndecay * 2)
653                         lp->lwp_estcpu -= ndecay;
654                 else
655                         lp->lwp_estcpu >>= 1;
656
657                 if (usched_debug == lp->lwp_proc->p_pid)
658                         kprintf(" ndecay %d estcpu %d\n", ndecay, lp->lwp_estcpu);
659                 bsd4_resetpriority(lp);
660                 lp->lwp_cpbase = cpbase;
661                 lp->lwp_cpticks = 0;
662         }
663 }
664
665 /*
666  * Compute the priority of a process when running in user mode.
667  * Arrange to reschedule if the resulting priority is better
668  * than that of the current process.
669  *
670  * This routine may be called with any process.
671  *
672  * This routine is called by fork1() for initial setup with the process
673  * of the run queue, and also may be called normally with the process on or
674  * off the run queue.
675  *
676  * MPSAFE
677  */
678 static void
679 bsd4_resetpriority(struct lwp *lp)
680 {
681         bsd4_pcpu_t dd;
682         int newpriority;
683         u_short newrqtype;
684         int reschedcpu;
685
686         /*
687          * Calculate the new priority and queue type
688          */
689         crit_enter();
690         spin_lock(&bsd4_spin);
691
692         newrqtype = lp->lwp_rtprio.type;
693
694         switch(newrqtype) {
695         case RTP_PRIO_REALTIME:
696         case RTP_PRIO_FIFO:
697                 newpriority = PRIBASE_REALTIME +
698                              (lp->lwp_rtprio.prio & PRIMASK);
699                 break;
700         case RTP_PRIO_NORMAL:
701                 newpriority = (lp->lwp_proc->p_nice - PRIO_MIN) * PPQ / NICEPPQ;
702                 newpriority += lp->lwp_estcpu * PPQ / ESTCPUPPQ;
703                 newpriority = newpriority * MAXPRI / (PRIO_RANGE * PPQ /
704                               NICEPPQ + ESTCPUMAX * PPQ / ESTCPUPPQ);
705                 newpriority = PRIBASE_NORMAL + (newpriority & PRIMASK);
706                 break;
707         case RTP_PRIO_IDLE:
708                 newpriority = PRIBASE_IDLE + (lp->lwp_rtprio.prio & PRIMASK);
709                 break;
710         case RTP_PRIO_THREAD:
711                 newpriority = PRIBASE_THREAD + (lp->lwp_rtprio.prio & PRIMASK);
712                 break;
713         default:
714                 panic("Bad RTP_PRIO %d", newrqtype);
715                 /* NOT REACHED */
716         }
717
718         /*
719          * The newpriority incorporates the queue type so do a simple masked
720          * check to determine if the process has moved to another queue.  If
721          * it has, and it is currently on a run queue, then move it.
722          */
723         if ((lp->lwp_priority ^ newpriority) & ~PPQMASK) {
724                 lp->lwp_priority = newpriority;
725                 if (lp->lwp_flag & LWP_ONRUNQ) {
726                         bsd4_remrunqueue_locked(lp);
727                         lp->lwp_rqtype = newrqtype;
728                         lp->lwp_rqindex = (newpriority & PRIMASK) / PPQ;
729                         bsd4_setrunqueue_locked(lp);
730                         reschedcpu = lp->lwp_thread->td_gd->gd_cpuid;
731                 } else {
732                         lp->lwp_rqtype = newrqtype;
733                         lp->lwp_rqindex = (newpriority & PRIMASK) / PPQ;
734                         reschedcpu = -1;
735                 }
736         } else {
737                 lp->lwp_priority = newpriority;
738                 reschedcpu = -1;
739         }
740         spin_unlock(&bsd4_spin);
741
742         /*
743          * Determine if we need to reschedule the target cpu.  This only
744          * occurs if the LWP is already on a scheduler queue, which means
745          * that idle cpu notification has already occured.  At most we
746          * need only issue a need_user_resched() on the appropriate cpu.
747          *
748          * The LWP may be owned by a CPU different from the current one,
749          * in which case dd->uschedcp may be modified without an MP lock
750          * or a spinlock held.  The worst that happens is that the code
751          * below causes a spurious need_user_resched() on the target CPU
752          * and dd->pri to be wrong for a short period of time, both of
753          * which are harmless.
754          */
755         if (reschedcpu >= 0) {
756                 dd = &bsd4_pcpu[reschedcpu];
757                 if ((dd->upri & ~PRIMASK) > (lp->lwp_priority & ~PRIMASK)) {
758                         dd->upri = lp->lwp_priority;
759 #ifdef SMP
760                         if (reschedcpu == mycpu->gd_cpuid) {
761                                 need_user_resched();
762                         } else {
763                                 lwkt_send_ipiq(lp->lwp_thread->td_gd,
764                                                need_user_resched_remote, NULL);
765                         }
766 #else
767                         need_user_resched();
768 #endif
769                 }
770         }
771         crit_exit();
772 }
773
774 /*
775  * MPSAFE
776  */
777 static
778 void
779 bsd4_yield(struct lwp *lp) 
780 {
781 #if 0
782         /* FUTURE (or something similar) */
783         switch(lp->lwp_rqtype) {
784         case RTP_PRIO_NORMAL:
785                 lp->lwp_estcpu = ESTCPULIM(lp->lwp_estcpu + ESTCPUINCR);
786                 break;
787         default:
788                 break;
789         }
790 #endif
791         need_user_resched();
792 }
793
794 /*
795  * Called from fork1() when a new child process is being created.
796  *
797  * Give the child process an initial estcpu that is more batch then
798  * its parent and dock the parent for the fork (but do not
799  * reschedule the parent).   This comprises the main part of our batch
800  * detection heuristic for both parallel forking and sequential execs.
801  *
802  * Interactive processes will decay the boosted estcpu quickly while batch
803  * processes will tend to compound it.
804  * XXX lwp should be "spawning" instead of "forking"
805  *
806  * MPSAFE
807  */
808 static void
809 bsd4_forking(struct lwp *plp, struct lwp *lp)
810 {
811         lp->lwp_estcpu = ESTCPULIM(plp->lwp_estcpu + ESTCPUPPQ);
812         lp->lwp_origcpu = lp->lwp_estcpu;
813         plp->lwp_estcpu = ESTCPULIM(plp->lwp_estcpu + ESTCPUPPQ);
814 }
815
816 /*
817  * Called when the parent reaps a child.   Propogate cpu use by the child
818  * back to the parent.
819  *
820  * MPSAFE
821  */
822 static void
823 bsd4_exiting(struct lwp *plp, struct lwp *lp)
824 {
825         int delta;
826
827         if (plp->lwp_proc->p_pid != 1) {
828                 delta = lp->lwp_estcpu - lp->lwp_origcpu;
829                 if (delta > 0)
830                         plp->lwp_estcpu = ESTCPULIM(plp->lwp_estcpu + delta);
831         }
832 }
833
834
835 /*
836  * chooseproc() is called when a cpu needs a user process to LWKT schedule,
837  * it selects a user process and returns it.  If chklp is non-NULL and chklp
838  * has a better or equal priority then the process that would otherwise be
839  * chosen, NULL is returned.
840  *
841  * Until we fix the RUNQ code the chklp test has to be strict or we may
842  * bounce between processes trying to acquire the current process designation.
843  *
844  * MPSAFE - must be called with bsd4_spin exclusive held.  The spinlock is
845  *          left intact through the entire routine.
846  */
847 static
848 struct lwp *
849 chooseproc_locked(struct lwp *chklp)
850 {
851         struct lwp *lp;
852         struct rq *q;
853         u_int32_t *which, *which2;
854         u_int32_t pri;
855         u_int32_t rtqbits;
856         u_int32_t tsqbits;
857         u_int32_t idqbits;
858         cpumask_t cpumask;
859
860         rtqbits = bsd4_rtqueuebits;
861         tsqbits = bsd4_queuebits;
862         idqbits = bsd4_idqueuebits;
863         cpumask = mycpu->gd_cpumask;
864
865 #ifdef SMP
866 again:
867 #endif
868         if (rtqbits) {
869                 pri = bsfl(rtqbits);
870                 q = &bsd4_rtqueues[pri];
871                 which = &bsd4_rtqueuebits;
872                 which2 = &rtqbits;
873         } else if (tsqbits) {
874                 pri = bsfl(tsqbits);
875                 q = &bsd4_queues[pri];
876                 which = &bsd4_queuebits;
877                 which2 = &tsqbits;
878         } else if (idqbits) {
879                 pri = bsfl(idqbits);
880                 q = &bsd4_idqueues[pri];
881                 which = &bsd4_idqueuebits;
882                 which2 = &idqbits;
883         } else {
884                 return NULL;
885         }
886         lp = TAILQ_FIRST(q);
887         KASSERT(lp, ("chooseproc: no lwp on busy queue"));
888
889 #ifdef SMP
890         while ((lp->lwp_cpumask & cpumask) == 0) {
891                 lp = TAILQ_NEXT(lp, lwp_procq);
892                 if (lp == NULL) {
893                         *which2 &= ~(1 << pri);
894                         goto again;
895                 }
896         }
897 #endif
898
899         /*
900          * If the passed lwp <chklp> is reasonably close to the selected
901          * lwp <lp>, return NULL (indicating that <chklp> should be kept).
902          * 
903          * Note that we must error on the side of <chklp> to avoid bouncing
904          * between threads in the acquire code.
905          */
906         if (chklp) {
907                 if (chklp->lwp_priority < lp->lwp_priority + PPQ)
908                         return(NULL);
909         }
910
911 #ifdef SMP
912         /*
913          * If the chosen lwp does not reside on this cpu spend a few
914          * cycles looking for a better candidate at the same priority level.
915          * This is a fallback check, setrunqueue() tries to wakeup the
916          * correct cpu and is our front-line affinity.
917          */
918         if (lp->lwp_thread->td_gd != mycpu &&
919             (chklp = TAILQ_NEXT(lp, lwp_procq)) != NULL
920         ) {
921                 if (chklp->lwp_thread->td_gd == mycpu) {
922                         ++choose_affinity;
923                         lp = chklp;
924                 }
925         }
926 #endif
927
928         TAILQ_REMOVE(q, lp, lwp_procq);
929         --bsd4_runqcount;
930         if (TAILQ_EMPTY(q))
931                 *which &= ~(1 << pri);
932         KASSERT((lp->lwp_flag & LWP_ONRUNQ) != 0, ("not on runq6!"));
933         lp->lwp_flag &= ~LWP_ONRUNQ;
934         return lp;
935 }
936
937 #ifdef SMP
938
939 /*
940  * Called via an ipi message to reschedule on another cpu.  If no
941  * user thread is active on the target cpu we wake the scheduler
942  * helper thread up to help schedule one.
943  *
944  * MPSAFE
945  */
946 static
947 void
948 need_user_resched_remote(void *dummy)
949 {
950         globaldata_t gd = mycpu;
951         bsd4_pcpu_t  dd = &bsd4_pcpu[gd->gd_cpuid];
952
953         if (dd->uschedcp == NULL && (bsd4_rdyprocmask & gd->gd_cpumask)) {
954                 atomic_clear_int(&bsd4_rdyprocmask, gd->gd_cpumask);
955                 lwkt_schedule(&dd->helper_thread);
956         } else {
957                 need_user_resched();
958         }
959 }
960
961 #endif
962
963 /*
964  * bsd4_remrunqueue_locked() removes a given process from the run queue
965  * that it is on, clearing the queue busy bit if it becomes empty.
966  *
967  * Note that user process scheduler is different from the LWKT schedule.
968  * The user process scheduler only manages user processes but it uses LWKT
969  * underneath, and a user process operating in the kernel will often be
970  * 'released' from our management.
971  *
972  * MPSAFE - bsd4_spin must be held exclusively on call
973  */
974 static void
975 bsd4_remrunqueue_locked(struct lwp *lp)
976 {
977         struct rq *q;
978         u_int32_t *which;
979         u_int8_t pri;
980
981         KKASSERT(lp->lwp_flag & LWP_ONRUNQ);
982         lp->lwp_flag &= ~LWP_ONRUNQ;
983         --bsd4_runqcount;
984         KKASSERT(bsd4_runqcount >= 0);
985
986         pri = lp->lwp_rqindex;
987         switch(lp->lwp_rqtype) {
988         case RTP_PRIO_NORMAL:
989                 q = &bsd4_queues[pri];
990                 which = &bsd4_queuebits;
991                 break;
992         case RTP_PRIO_REALTIME:
993         case RTP_PRIO_FIFO:
994                 q = &bsd4_rtqueues[pri];
995                 which = &bsd4_rtqueuebits;
996                 break;
997         case RTP_PRIO_IDLE:
998                 q = &bsd4_idqueues[pri];
999                 which = &bsd4_idqueuebits;
1000                 break;
1001         default:
1002                 panic("remrunqueue: invalid rtprio type");
1003                 /* NOT REACHED */
1004         }
1005         TAILQ_REMOVE(q, lp, lwp_procq);
1006         if (TAILQ_EMPTY(q)) {
1007                 KASSERT((*which & (1 << pri)) != 0,
1008                         ("remrunqueue: remove from empty queue"));
1009                 *which &= ~(1 << pri);
1010         }
1011 }
1012
1013 /*
1014  * bsd4_setrunqueue_locked()
1015  *
1016  * Add a process whos rqtype and rqindex had previously been calculated
1017  * onto the appropriate run queue.   Determine if the addition requires
1018  * a reschedule on a cpu and return the cpuid or -1.
1019  *
1020  * NOTE: Lower priorities are better priorities.
1021  *
1022  * MPSAFE - bsd4_spin must be held exclusively on call
1023  */
1024 static void
1025 bsd4_setrunqueue_locked(struct lwp *lp)
1026 {
1027         struct rq *q;
1028         u_int32_t *which;
1029         int pri;
1030
1031         KKASSERT((lp->lwp_flag & LWP_ONRUNQ) == 0);
1032         lp->lwp_flag |= LWP_ONRUNQ;
1033         ++bsd4_runqcount;
1034
1035         pri = lp->lwp_rqindex;
1036
1037         switch(lp->lwp_rqtype) {
1038         case RTP_PRIO_NORMAL:
1039                 q = &bsd4_queues[pri];
1040                 which = &bsd4_queuebits;
1041                 break;
1042         case RTP_PRIO_REALTIME:
1043         case RTP_PRIO_FIFO:
1044                 q = &bsd4_rtqueues[pri];
1045                 which = &bsd4_rtqueuebits;
1046                 break;
1047         case RTP_PRIO_IDLE:
1048                 q = &bsd4_idqueues[pri];
1049                 which = &bsd4_idqueuebits;
1050                 break;
1051         default:
1052                 panic("remrunqueue: invalid rtprio type");
1053                 /* NOT REACHED */
1054         }
1055
1056         /*
1057          * Add to the correct queue and set the appropriate bit.  If no
1058          * lower priority (i.e. better) processes are in the queue then
1059          * we want a reschedule, calculate the best cpu for the job.
1060          *
1061          * Always run reschedules on the LWPs original cpu.
1062          */
1063         TAILQ_INSERT_TAIL(q, lp, lwp_procq);
1064         *which |= 1 << pri;
1065 }
1066
1067 #ifdef SMP
1068
1069 /*
1070  * For SMP systems a user scheduler helper thread is created for each
1071  * cpu and is used to allow one cpu to wakeup another for the purposes of
1072  * scheduling userland threads from setrunqueue().
1073  *
1074  * UP systems do not need the helper since there is only one cpu.
1075  *
1076  * We can't use the idle thread for this because we might block.
1077  * Additionally, doing things this way allows us to HLT idle cpus
1078  * on MP systems.
1079  *
1080  * MPSAFE
1081  */
1082 static void
1083 sched_thread(void *dummy)
1084 {
1085     globaldata_t gd;
1086     bsd4_pcpu_t  dd;
1087     struct lwp *nlp;
1088     cpumask_t cpumask;
1089     int cpuid;
1090 #if 0
1091     cpumask_t tmpmask;
1092     int tmpid;
1093 #endif
1094
1095     gd = mycpu;
1096     cpuid = gd->gd_cpuid;       /* doesn't change */
1097     cpumask = gd->gd_cpumask;   /* doesn't change */
1098     dd = &bsd4_pcpu[cpuid];
1099
1100     /*
1101      * Since we are woken up only when no user processes are scheduled
1102      * on a cpu, we can run at an ultra low priority.
1103      */
1104     lwkt_setpri_self(TDPRI_USER_SCHEDULER);
1105
1106     for (;;) {
1107         /*
1108          * We use the LWKT deschedule-interlock trick to avoid racing
1109          * bsd4_rdyprocmask.  This means we cannot block through to the
1110          * manual lwkt_switch() call we make below.
1111          */
1112         crit_enter_gd(gd);
1113         lwkt_deschedule_self(gd->gd_curthread);
1114         spin_lock(&bsd4_spin);
1115         atomic_set_int(&bsd4_rdyprocmask, cpumask);
1116
1117         clear_user_resched();   /* This satisfied the reschedule request */
1118         dd->rrcount = 0;        /* Reset the round-robin counter */
1119
1120         if ((bsd4_curprocmask & cpumask) == 0) {
1121                 /*
1122                  * No thread is currently scheduled.
1123                  */
1124                 KKASSERT(dd->uschedcp == NULL);
1125                 if ((nlp = chooseproc_locked(NULL)) != NULL) {
1126                         atomic_set_int(&bsd4_curprocmask, cpumask);
1127                         dd->upri = nlp->lwp_priority;
1128                         dd->uschedcp = nlp;
1129                         spin_unlock(&bsd4_spin);
1130                         lwkt_acquire(nlp->lwp_thread);
1131                         lwkt_schedule(nlp->lwp_thread);
1132                 } else {
1133                         spin_unlock(&bsd4_spin);
1134                 }
1135 #if 0
1136         /*
1137          * Disabled for now, this can create an infinite loop.
1138          */
1139         } else if (bsd4_runqcount) {
1140                 /*
1141                  * Someone scheduled us but raced.  In order to not lose
1142                  * track of the fact that there may be a LWP ready to go,
1143                  * forward the request to another cpu if available.
1144                  *
1145                  * Rotate through cpus starting with cpuid + 1.  Since cpuid
1146                  * is already masked out by gd_other_cpus, just use ~cpumask.
1147                  */
1148                 tmpmask = bsd4_rdyprocmask & mycpu->gd_other_cpus &
1149                           ~bsd4_curprocmask;
1150                 if (tmpmask) {
1151                         if (tmpmask & ~(cpumask - 1))
1152                                 tmpid = bsfl(tmpmask & ~(cpumask - 1));
1153                         else
1154                                 tmpid = bsfl(tmpmask);
1155                         bsd4_scancpu = tmpid;
1156                         atomic_clear_int(&bsd4_rdyprocmask, 1 << tmpid);
1157                         spin_unlock_wr(&bsd4_spin);
1158                         lwkt_schedule(&bsd4_pcpu[tmpid].helper_thread);
1159                 } else {
1160                         spin_unlock_wr(&bsd4_spin);
1161                 }
1162 #endif
1163         } else {
1164                 /*
1165                  * The runq is empty.
1166                  */
1167                 spin_unlock(&bsd4_spin);
1168         }
1169         crit_exit_gd(gd);
1170         lwkt_switch();
1171     }
1172 }
1173
1174 /*
1175  * Setup our scheduler helpers.  Note that curprocmask bit 0 has already
1176  * been cleared by rqinit() and we should not mess with it further.
1177  */
1178 static void
1179 sched_thread_cpu_init(void)
1180 {
1181     int i;
1182
1183     if (bootverbose)
1184         kprintf("start scheduler helpers on cpus:");
1185
1186     for (i = 0; i < ncpus; ++i) {
1187         bsd4_pcpu_t dd = &bsd4_pcpu[i];
1188         cpumask_t mask = 1 << i;
1189
1190         if ((mask & smp_active_mask) == 0)
1191             continue;
1192
1193         if (bootverbose)
1194             kprintf(" %d", i);
1195
1196         lwkt_create(sched_thread, NULL, NULL, &dd->helper_thread, 
1197                     TDF_STOPREQ, i, "usched %d", i);
1198
1199         /*
1200          * Allow user scheduling on the target cpu.  cpu #0 has already
1201          * been enabled in rqinit().
1202          */
1203         if (i)
1204             atomic_clear_int(&bsd4_curprocmask, mask);
1205         atomic_set_int(&bsd4_rdyprocmask, mask);
1206         dd->upri = PRIBASE_NULL;
1207     }
1208     if (bootverbose)
1209         kprintf("\n");
1210 }
1211 SYSINIT(uschedtd, SI_BOOT2_USCHED, SI_ORDER_SECOND,
1212         sched_thread_cpu_init, NULL)
1213
1214 #endif
1215