unified MX27, MX25, MX37 trees
[karo-tx-redboot.git] / packages / kernel / v2_0 / src / sched / mlqueue.cxx
1 //==========================================================================
2 //
3 //      sched/mlqueue.cxx
4 //
5 //      Multi-level queue scheduler class implementation
6 //
7 //==========================================================================
8 //####ECOSGPLCOPYRIGHTBEGIN####
9 // -------------------------------------------
10 // This file is part of eCos, the Embedded Configurable Operating System.
11 // Copyright (C) 1998, 1999, 2000, 2001, 2002 Red Hat, Inc.
12 //
13 // eCos is free software; you can redistribute it and/or modify it under
14 // the terms of the GNU General Public License as published by the Free
15 // Software Foundation; either version 2 or (at your option) any later version.
16 //
17 // eCos is distributed in the hope that it will be useful, but WITHOUT ANY
18 // WARRANTY; without even the implied warranty of MERCHANTABILITY or
19 // FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
20 // for more details.
21 //
22 // You should have received a copy of the GNU General Public License along
23 // with eCos; if not, write to the Free Software Foundation, Inc.,
24 // 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
25 //
26 // As a special exception, if other files instantiate templates or use macros
27 // or inline functions from this file, or you compile this file and link it
28 // with other works to produce a work based on this file, this file does not
29 // by itself cause the resulting work to be covered by the GNU General Public
30 // License. However the source code for this file must still be made available
31 // in accordance with section (3) of the GNU General Public License.
32 //
33 // This exception does not invalidate any other reasons why a work based on
34 // this file might be covered by the GNU General Public License.
35 //
36 // Alternative licenses for eCos may be arranged by contacting Red Hat, Inc.
37 // at http://sources.redhat.com/ecos/ecos-license/
38 // -------------------------------------------
39 //####ECOSGPLCOPYRIGHTEND####
40 //==========================================================================
41 //#####DESCRIPTIONBEGIN####
42 //
43 // Author(s):    nickg
44 // Contributors: jlarmour
45 // Date:         1999-02-17
46 // Purpose:      Multilevel queue scheduler class implementation
47 // Description:  This file contains the implementations of
48 //               Cyg_Scheduler_Implementation and
49 //               Cyg_SchedThread_Implementation.
50 //              
51 //
52 //####DESCRIPTIONEND####
53 //
54 //==========================================================================
55
56 #include <pkgconf/kernel.h>
57
58 #include <cyg/kernel/ktypes.h>         // base kernel types
59 #include <cyg/infra/cyg_trac.h>        // tracing macros
60 #include <cyg/infra/cyg_ass.h>         // assertion macros
61
62 #include <cyg/kernel/sched.hxx>        // our header
63
64 #include <cyg/hal/hal_arch.h>          // Architecture specific definitions
65
66 #include <cyg/kernel/thread.inl>       // thread inlines
67 #include <cyg/kernel/sched.inl>        // scheduler inlines
68
69 #ifdef CYGSEM_KERNEL_SCHED_MLQUEUE
70
71 //==========================================================================
72 // Cyg_Scheduler_Implementation class static members
73
74 #ifdef CYGSEM_KERNEL_SCHED_TIMESLICE
75
76 cyg_ucount32 Cyg_Scheduler_Implementation::timeslice_count[CYGNUM_KERNEL_CPU_MAX];
77
78 #endif
79
80
81 //==========================================================================
82 // Cyg_Scheduler_Implementation class members
83
84 // -------------------------------------------------------------------------
85 // Constructor.
86
87 Cyg_Scheduler_Implementation::Cyg_Scheduler_Implementation()
88 {
89     CYG_REPORT_FUNCTION();
90         
91     queue_map   = 0;
92
93 #ifdef CYGPKG_KERNEL_SMP_SUPPORT
94
95     pending_map = 0;
96     
97     for( int i = 0; i < CYGNUM_KERNEL_SCHED_PRIORITIES; i++ )
98         pending[i] = 0;
99
100 #endif
101     
102     for( int i = 0; i < CYGNUM_KERNEL_CPU_MAX; i++ )
103     {
104 #ifdef CYGSEM_KERNEL_SCHED_TIMESLICE        
105         timeslice_count[i] = CYGNUM_KERNEL_SCHED_TIMESLICE_TICKS;
106 #endif        
107         need_reschedule[i] = true;
108     }
109     
110     CYG_REPORT_RETURN();
111 }
112
113 // -------------------------------------------------------------------------
114 // Choose the best thread to run next
115
116 Cyg_Thread *
117 Cyg_Scheduler_Implementation::schedule(void)
118 {
119     CYG_REPORT_FUNCTYPE("returning thread %08x");
120
121     // The run queue may _never_ be empty, there is always
122     // an idle thread at the lowest priority.
123
124     CYG_ASSERT( queue_map != 0, "Run queue empty");
125     CYG_ASSERT( queue_map & (1<<CYG_THREAD_MIN_PRIORITY), "Idle thread vanished!!!");
126     CYG_ASSERT( !run_queue[CYG_THREAD_MIN_PRIORITY].empty(), "Idle thread vanished!!!");
127
128 #ifdef CYGPKG_KERNEL_SMP_SUPPORT
129
130     Cyg_Thread *current = get_current_thread();
131     register cyg_uint32 index;
132
133     CYG_ASSERT( current->cpu != CYG_KERNEL_CPU_NONE, "Current thread does not have CPU set!");
134
135     // If the current thread is still runnable, return it to pending
136     // state so that it can be considered alongside any other threads
137     // for execution.
138     if( current->get_state() == Cyg_Thread::RUNNING )
139     {
140         current->cpu = CYG_KERNEL_CPU_NONE;
141         pending[current->priority]++;
142         pending_map |= (1<<current->priority);
143     }
144     else
145     {
146         // Otherwise, ensure that the thread is no longer marked as
147         // running.
148         current->cpu = CYG_KERNEL_CPU_NONE;        
149     }
150
151     
152     HAL_LSBIT_INDEX(index, pending_map);
153
154     Cyg_RunQueue *queue = &run_queue[index];
155     
156     CYG_ASSERT( !queue->empty(), "Queue for index empty");
157     CYG_ASSERT( pending[index] > 0, "Pending array and map disagree");
158
159     Cyg_Thread *thread = queue->get_head();
160
161     // We know there is a runnable thread in this queue, If the thread
162     // we got is not it, scan until we find it. While not constant time,
163     // this search has an upper bound of the number of CPUs in the system.
164     
165     while( thread->cpu != CYG_KERNEL_CPU_NONE )
166         thread = thread->get_next();
167
168     // Take newly scheduled thread out of pending map
169     thread->cpu = CYG_KERNEL_CPU_THIS();
170     if( --pending[index] == 0 )
171         pending_map &= ~(1<<index);
172     
173 #else    
174
175     register cyg_uint32 index;
176
177     HAL_LSBIT_INDEX(index, queue_map);
178
179     Cyg_RunQueue *queue = &run_queue[index];
180     
181     CYG_ASSERT( !queue->empty(), "Queue for index empty");
182
183     Cyg_Thread *thread = queue->get_head();
184
185 #endif
186     
187     CYG_INSTRUMENT_MLQ( SCHEDULE, thread, index);
188     
189     CYG_ASSERT( thread != NULL , "No threads in run queue");
190     CYG_ASSERT( thread->queue == NULL , "Runnable thread on a queue!");
191    
192     CYG_REPORT_RETVAL(thread);
193
194     return thread;
195 }
196
197 // -------------------------------------------------------------------------
198
199 void
200 Cyg_Scheduler_Implementation::add_thread(Cyg_Thread *thread)
201 {
202     CYG_REPORT_FUNCTION();
203     CYG_REPORT_FUNCARG1("thread=%08x", thread);
204
205     cyg_priority pri                               = thread->priority;
206     Cyg_RunQueue *queue = &run_queue[pri];
207
208     CYG_INSTRUMENT_MLQ( ADD, thread, pri);
209     
210     CYG_ASSERT((CYG_THREAD_MIN_PRIORITY >= pri) 
211                && (CYG_THREAD_MAX_PRIORITY <= pri),
212                "Priority out of range!");
213
214     CYG_ASSERT( ((queue_map & (1<<pri))!=0) == ((!run_queue[pri].empty())!=0), "Map and queue disagree");
215
216     // If the thread is on some other queue, remove it
217     // here.
218     if( thread->queue != NULL )
219     {
220         thread->queue->remove(thread);
221     }
222     
223     if( queue->empty() )
224     {
225         // set the map bit and ask for a reschedule if this is a
226         // new highest priority thread.
227       
228         queue_map |= (1<<pri);
229
230     }
231     // else the queue already has an occupant, queue behind him
232
233     queue->add_tail(thread);
234
235     // If the new thread is higher priority than any
236     // current thread, request a reschedule.
237
238     set_need_reschedule(thread);
239
240     // Also reset the timeslice_count so that this thread gets a full
241     // timeslice once it begins to run.
242     
243     thread->timeslice_reset();
244     
245 #ifdef CYGPKG_KERNEL_SMP_SUPPORT
246
247     // If the thread is not currently running, increment the pending
248     // count for the priority, and if necessary set the bit in the
249     // pending map.
250
251     if( thread->cpu == CYG_KERNEL_CPU_NONE )
252     {
253         if( pending[pri]++ == 0 )
254             pending_map |= (1<<pri);
255     }
256     // Otherwise the pending count will be dealt with in schedule().
257     
258 #endif    
259     
260     CYG_ASSERT( thread->queue == NULL , "Runnable thread on a queue!");
261     CYG_ASSERT( queue_map != 0, "Run queue empty");
262     CYG_ASSERT( queue_map & (1<<pri), "Queue map bit not set for pri");
263     CYG_ASSERT( !run_queue[pri].empty(), "Queue for pri empty");
264     CYG_ASSERT( ((queue_map & (1<<pri))!=0) == ((!run_queue[pri].empty())!=0), "Map and queue disagree");    
265     CYG_ASSERT( queue_map & (1<<CYG_THREAD_MIN_PRIORITY), "Idle thread vanished!!!");
266     CYG_ASSERT( !run_queue[CYG_THREAD_MIN_PRIORITY].empty(), "Idle thread vanished!!!");
267     
268     CYG_REPORT_RETURN();
269 }
270
271 // -------------------------------------------------------------------------
272
273 void
274 Cyg_Scheduler_Implementation::rem_thread(Cyg_Thread *thread)
275 {
276     CYG_REPORT_FUNCTION();
277     CYG_REPORT_FUNCARG1("thread=%08x", thread);
278         
279     CYG_ASSERT( queue_map != 0, "Run queue empty");
280
281     cyg_priority pri    = thread->priority;
282     Cyg_RunQueue *queue = &run_queue[pri];
283
284     CYG_INSTRUMENT_MLQ( REM, thread, pri);
285     
286     CYG_ASSERT( pri != CYG_THREAD_MIN_PRIORITY, "Idle thread trying to sleep!");
287     CYG_ASSERT( !run_queue[CYG_THREAD_MIN_PRIORITY].empty(), "Idle thread vanished!!!");
288
289 #ifdef CYGPKG_KERNEL_SMP_SUPPORT
290
291     if( thread->cpu == CYG_KERNEL_CPU_NONE )
292     {
293         // If the thread is not running, then we need to adjust the
294         // pending count array and map if necessary.
295
296         if( --pending[pri] == 0 )
297             pending_map &= ~(1<<pri);
298     }
299     else
300     {
301         // If the target thread is currently running on a different
302         // CPU, send a reschedule interrupt there to deschedule it.        
303         if( thread->cpu != CYG_KERNEL_CPU_THIS() )
304             CYG_KERNEL_CPU_RESCHEDULE_INTERRUPT( thread->cpu, 0 );
305     }
306     // If the thread is current running on this CPU, then the pending
307     // count will be dealt with in schedule().
308     
309 #endif
310         
311     CYG_ASSERT( queue_map & (1<<pri), "Queue map bit not set for pri");
312     CYG_ASSERT( !run_queue[pri].empty(), "Queue for pri empty");
313     
314     // remove thread from queue
315     queue->remove(thread);
316
317     if( queue->empty() )
318     {
319         // If this was only thread in
320         // queue, clear map.
321       
322         queue_map &= ~(1<<pri);
323     }
324
325     CYG_ASSERT( queue_map != 0, "Run queue empty");
326     CYG_ASSERT( queue_map & (1<<CYG_THREAD_MIN_PRIORITY), "Idle thread vanished!!!");
327     CYG_ASSERT( !run_queue[CYG_THREAD_MIN_PRIORITY].empty(), "Idle thread vanished!!!");
328     CYG_ASSERT( ((queue_map & (1<<pri))!=0) == ((!run_queue[pri].empty())!=0), "Map and queue disagree");
329     
330     CYG_REPORT_RETURN();
331 }
332
333 // -------------------------------------------------------------------------
334 // Set the need_reschedule flag
335 // This function overrides the definition in Cyg_Scheduler_Base and tests
336 // for a reschedule condition based on the priorities of the given thread
337 // and the current thread(s).
338
339 void Cyg_Scheduler_Implementation::set_need_reschedule(Cyg_Thread *thread)
340 {
341 #ifndef CYGPKG_KERNEL_SMP_SUPPORT
342
343     if( current_thread[0]->priority > thread->priority ||
344         current_thread[0]->get_state() != Cyg_Thread::RUNNING )
345         need_reschedule[0] = true;
346     
347 #else
348
349     HAL_SMP_CPU_TYPE cpu_this = CYG_KERNEL_CPU_THIS();
350     HAL_SMP_CPU_TYPE cpu_count = CYG_KERNEL_CPU_COUNT();
351
352     // Start with current CPU. If we can do the job locally then
353     // that is most efficient. Only go on to other CPUs if that is
354     // not possible.
355
356     for(int i = 0; i < cpu_count; i++)
357     {
358         HAL_SMP_CPU_TYPE cpu = (i + cpu_this) % cpu_count;
359        
360         // If a CPU is not already marked for rescheduling, and its
361         // current thread is of lower priority than _thread_, then
362         // set its need_reschedule flag.
363
364         Cyg_Thread *cur = current_thread[cpu];
365
366         if( (!need_reschedule[cpu]) &&
367             (cur->priority > thread->priority)
368           )
369         {
370             need_reschedule[cpu] = true;
371
372             if( cpu != cpu_this )
373             {
374                 // All processors other than this one need to be sent
375                 // a reschedule interrupt.
376                 
377                 CYG_INSTRUMENT_SMP( RESCHED_SEND, cpu, 0 );
378                 CYG_KERNEL_CPU_RESCHEDULE_INTERRUPT( cpu, 0 );
379             }
380
381             // Having notionally rescheduled _thread_ onto the cpu, we
382             // now see if we can reschedule the former current thread of
383             // that CPU onto another.
384             
385             thread = cur;
386         }
387     } 
388
389 #endif  
390 }
391
392 // -------------------------------------------------------------------------
393 // Set up initial idle thread
394
395 void Cyg_Scheduler_Implementation::set_idle_thread( Cyg_Thread *thread, HAL_SMP_CPU_TYPE cpu )
396 {
397     // Make the thread the current thread for this CPU.
398     
399     current_thread[cpu] = thread;
400
401     // This will insert the thread in the run queues and make it
402     // available to execute.
403     thread->resume();
404
405 #ifdef CYGPKG_KERNEL_SMP_SUPPORT
406
407     thread->cpu = cpu;
408     
409     // In SMP, we need to take this thread out of the pending array
410     // and map.
411     
412     cyg_priority pri    = thread->priority;
413     if( --pending[pri] == 0 )
414         pending_map &= ~(1<<pri);
415 #endif    
416     
417 }
418
419 // -------------------------------------------------------------------------
420 // register thread with scheduler
421
422 void
423 Cyg_Scheduler_Implementation::register_thread(Cyg_Thread *thread)
424 {
425     CYG_REPORT_FUNCTION();
426     CYG_REPORT_FUNCARG1("thread=%08x", thread);
427     // No registration necessary in this scheduler
428     CYG_REPORT_RETURN();
429 }
430
431 // -------------------------------------------------------------------------
432
433 // deregister thread
434 void
435 Cyg_Scheduler_Implementation::deregister_thread(Cyg_Thread *thread)
436 {
437     CYG_REPORT_FUNCTION();
438     CYG_REPORT_FUNCARG1("thread=%08x", thread);
439     // No registration necessary in this scheduler    
440     CYG_REPORT_RETURN();
441 }
442     
443 // -------------------------------------------------------------------------
444 // Test the given priority for uniqueness
445
446 cyg_bool
447 Cyg_Scheduler_Implementation::unique( cyg_priority priority)
448 {
449     CYG_REPORT_FUNCTYPE("returning %d");
450     CYG_REPORT_FUNCARG1("priority=%d", priority);
451     // Priorities are not unique
452     CYG_REPORT_RETVAL(true);
453     return true;
454 }
455
456 //==========================================================================
457 // Support for timeslicing option
458
459 #ifdef CYGSEM_KERNEL_SCHED_TIMESLICE
460
461 // -------------------------------------------------------------------------
462
463 void
464 Cyg_Scheduler_Implementation::timeslice(void)
465 {
466 #ifdef CYGDBG_KERNEL_TRACE_TIMESLICE
467     CYG_REPORT_FUNCTION();
468 #endif
469
470 #ifdef CYGPKG_KERNEL_SMP_SUPPORT
471
472     HAL_SMP_CPU_TYPE cpu;
473     HAL_SMP_CPU_TYPE cpu_count = CYG_KERNEL_CPU_COUNT();
474     HAL_SMP_CPU_TYPE cpu_this = CYG_KERNEL_CPU_THIS();
475     
476     for( cpu = 0; cpu < cpu_count; cpu++ )
477     {
478         if( --timeslice_count[cpu] == 0 )
479             if( cpu == cpu_this )
480                 timeslice_cpu();
481             else CYG_KERNEL_CPU_TIMESLICE_INTERRUPT( cpu, 0 );
482     }
483
484 #else    
485
486     if( --timeslice_count[CYG_KERNEL_CPU_THIS()] == 0 )
487         timeslice_cpu();
488     
489 #endif
490
491 #ifdef CYGDBG_KERNEL_TRACE_TIMESLICE
492     CYG_REPORT_RETURN();
493 #endif    
494 }
495
496 // -------------------------------------------------------------------------
497
498 void
499 Cyg_Scheduler_Implementation::timeslice_cpu(void)
500 {
501 #ifdef CYGDBG_KERNEL_TRACE_TIMESLICE
502     CYG_REPORT_FUNCTION();
503 #endif
504
505     Cyg_Thread *thread = get_current_thread();
506     HAL_SMP_CPU_TYPE cpu_this = CYG_KERNEL_CPU_THIS();
507     
508     CYG_ASSERT( queue_map != 0, "Run queue empty");
509     CYG_ASSERT( queue_map & (1<<CYG_THREAD_MIN_PRIORITY), "Idle thread vanished!!!");
510
511 #ifdef CYGSEM_KERNEL_SCHED_TIMESLICE_ENABLE
512     if( thread->timeslice_enabled &&
513         timeslice_count[cpu_this] == 0 )
514 #else    
515     if( timeslice_count[cpu_this] == 0 )
516 #endif
517     {
518         CYG_INSTRUMENT_SCHED(TIMESLICE,0,0);
519 #ifdef CYGDBG_KERNEL_TRACE_TIMESLICE
520         CYG_TRACE0( true, "quantum consumed, time to reschedule" );
521 #endif
522
523         CYG_ASSERT( get_sched_lock() > 0 , "Timeslice called with zero sched_lock");
524
525         // Only try to rotate the run queue if the current thread is running.
526         // Otherwise we are going to reschedule anyway.
527         if( thread->get_state() == Cyg_Thread::RUNNING )
528         {
529             Cyg_Scheduler *sched = &Cyg_Scheduler::scheduler;
530
531             CYG_INSTRUMENT_MLQ( TIMESLICE, thread, 0);
532                 
533             CYG_ASSERTCLASS( thread, "Bad current thread");
534             CYG_ASSERTCLASS( sched, "Bad scheduler");
535     
536             cyg_priority pri    = thread->priority;
537             Cyg_RunQueue *queue = &sched->run_queue[pri];
538
539 #ifdef CYGPKG_KERNEL_SMP_SUPPORT
540
541             // In SMP systems we set the head of the queue to point to
542             // the thread immediately after the current
543             // thread. schedule() will then pick that thread, or one
544             // after it to run next.
545             
546             queue->to_head( thread->get_next() );
547 #else            
548             queue->rotate();
549 #endif
550             
551             if( queue->get_head() != thread )
552                 sched->set_need_reschedule();
553
554             timeslice_count[cpu_this] = CYGNUM_KERNEL_SCHED_TIMESLICE_TICKS;
555         }
556     }
557
558     
559     CYG_ASSERT( queue_map & (1<<CYG_THREAD_MIN_PRIORITY), "Idle thread vanished!!!");
560     CYG_ASSERT( !run_queue[CYG_THREAD_MIN_PRIORITY].empty(), "Idle thread vanished!!!");
561 #ifdef CYGDBG_KERNEL_TRACE_TIMESLICE
562     CYG_REPORT_RETURN();
563 #endif
564 }
565
566 // -------------------------------------------------------------------------
567
568 __externC void cyg_scheduler_timeslice_cpu(void)
569 {
570     Cyg_Scheduler::scheduler.timeslice_cpu();
571 }
572
573 #endif
574
575 //==========================================================================
576 // Cyg_SchedThread_Implementation class members
577
578 Cyg_SchedThread_Implementation::Cyg_SchedThread_Implementation
579 (
580     CYG_ADDRWORD sched_info
581 )
582 {
583     CYG_REPORT_FUNCTION();
584     CYG_REPORT_FUNCARG1("sched_info=%08x", sched_info);
585         
586     // Set priority to the supplied value.
587     priority = (cyg_priority)sched_info;
588
589 #ifdef CYGSEM_KERNEL_SCHED_TIMESLICE_ENABLE
590     // If timeslice_enabled exists, set it true by default
591     timeslice_enabled = true;
592 #endif
593 #ifdef CYGPKG_KERNEL_SMP_SUPPORT
594     cpu = CYG_KERNEL_CPU_NONE;
595 #endif
596     
597     CYG_REPORT_RETURN();
598 }
599
600 // -------------------------------------------------------------------------
601 // Yield the processor to another thread
602
603 void
604 Cyg_SchedThread_Implementation::yield(void)
605 {
606     CYG_REPORT_FUNCTION();
607         
608     // Prevent preemption
609     Cyg_Scheduler::lock();
610
611     Cyg_Thread *thread  = CYG_CLASSFROMBASE(Cyg_Thread,
612                                             Cyg_SchedThread_Implementation,
613                                             this);
614
615     // Only do this if this thread is running. If it is not, there
616     // is no point.
617     
618     if( thread->get_state() == Cyg_Thread::RUNNING )
619     {
620         // To yield we simply rotate the appropriate
621         // run queue to the next thread and reschedule.
622
623         CYG_INSTRUMENT_MLQ( YIELD, thread, 0);
624         
625         CYG_ASSERTCLASS( thread, "Bad current thread");
626     
627         Cyg_Scheduler *sched = &Cyg_Scheduler::scheduler;
628
629         CYG_ASSERTCLASS( sched, "Bad scheduler");
630     
631         cyg_priority pri    = thread->priority;
632         Cyg_RunQueue *queue = &sched->run_queue[pri];
633
634 #ifdef CYGPKG_KERNEL_SMP_SUPPORT
635
636             // In SMP systems we set the head of the queue to point to
637             // the thread immediately after the current
638             // thread. schedule() will then pick that thread, or one
639             // after it to run next.
640             
641             queue->to_head( thread->get_next() );
642 #else            
643             queue->rotate();
644 #endif
645         
646         if( queue->get_head() != thread )
647             sched->set_need_reschedule();
648         else
649         {
650             // Reset the timeslice counter so that this thread gets a
651             // full quantum as a reward for yielding when it is
652             // eventually rescheduled.
653             thread->timeslice_reset();
654         }
655
656     }
657     
658     // Unlock the scheduler and switch threads
659 #ifdef CYGDBG_USE_ASSERTS
660     // This test keeps the assertions in unlock_inner() happy if
661     // need_reschedule was not set above.
662     if( !Cyg_Scheduler::get_need_reschedule() )
663         Cyg_Scheduler::unlock();
664     else 
665 #endif    
666     Cyg_Scheduler::unlock_reschedule();
667
668     
669     CYG_REPORT_RETURN();
670 }
671
672 // -------------------------------------------------------------------------
673 // Rotate the run queue at a specified priority.
674 // (pri is the decider, not this, so the routine is static)
675
676 void
677 Cyg_SchedThread_Implementation::rotate_queue( cyg_priority pri )
678 {
679     CYG_REPORT_FUNCTION();
680     CYG_REPORT_FUNCARG1("priority=%d", pri);
681         
682     // Prevent preemption
683     Cyg_Scheduler::lock();
684
685     Cyg_Scheduler *sched = &Cyg_Scheduler::scheduler;
686
687     CYG_ASSERTCLASS( sched, "Bad scheduler");
688     
689     Cyg_RunQueue *queue = &sched->run_queue[pri];
690
691     if ( !queue->empty() ) {
692         queue->rotate();
693         sched->set_need_reschedule();
694     }
695
696     // Unlock the scheduler and switch threads
697     Cyg_Scheduler::unlock();
698
699     CYG_REPORT_RETURN();
700 }
701
702 // -------------------------------------------------------------------------
703 // Move this thread to the head of its queue
704 // (not necessarily a scheduler queue)
705
706 void
707 Cyg_SchedThread_Implementation::to_queue_head( void )
708 {
709     CYG_REPORT_FUNCTION();
710         
711     // Prevent preemption
712     Cyg_Scheduler::lock();
713
714     Cyg_Thread *thread  = CYG_CLASSFROMBASE(Cyg_Thread,
715                                             Cyg_SchedThread_Implementation,
716                                             this);
717
718     CYG_ASSERTCLASS( thread, "Bad current thread");
719     
720     Cyg_ThreadQueue *q = thread->get_current_queue();
721     if( q != NULL )
722         q->to_head( thread );
723     else if( thread->in_list() )
724     {
725         // If the queue pointer is NULL then it is on a run
726         // queue. Move the thread to the head of it's priority list
727         // and force a reschedule.
728         
729         Cyg_Scheduler *sched = &Cyg_Scheduler::scheduler;
730         sched->run_queue[thread->priority].to_head( thread );
731         sched->set_need_reschedule( thread );
732     }
733
734     // Unlock the scheduler and switch threads
735     Cyg_Scheduler::unlock();
736
737     CYG_REPORT_RETURN();
738 }
739
740 //==========================================================================
741 // Cyg_ThreadQueue_Implementation class members
742
743 // -------------------------------------------------------------------------        
744
745 void
746 Cyg_ThreadQueue_Implementation::enqueue(Cyg_Thread *thread)
747 {
748     CYG_REPORT_FUNCTION();
749     CYG_REPORT_FUNCARG1("thread=%08x", thread);
750
751     CYG_INSTRUMENT_MLQ( ENQUEUE, this, thread );
752     
753 #ifdef CYGIMP_KERNEL_SCHED_SORTED_QUEUES
754
755     // Insert the thread into the queue in priority order.
756
757     Cyg_Thread *qhead = get_head();
758
759     if( qhead == NULL ) add_tail( thread );
760     else if( qhead == qhead->get_next() )
761     {
762         // There is currently only one thread in the queue, join it
763         // and adjust the queue pointer to point to the highest
764         // priority of the two. If they are the same priority,
765         // leave the pointer pointing to the oldest.
766
767         qhead->insert( thread );
768
769         if( thread->priority < qhead->priority )
770             to_head(thread);
771     }
772     else
773     {
774         // There is more than one thread in the queue. First check
775         // whether we are of higher priority than the head and if
776         // so just jump in at the front. Also check whether we are
777         // lower priority than the tail and jump onto the end.
778         // Otherwise we really have to search the queue to find
779         // our place.
780
781         if( thread->priority < qhead->priority )
782         {
783             qhead->insert( thread );
784             to_head(thread);
785         }
786         else if( thread->priority > get_tail()->priority )
787         {
788             // We are lower priority than any thread in the queue,
789             // go in at the end.
790
791             add_tail( thread );
792         }
793         else
794         {
795             // Search the queue. We do this backwards so that we
796             // always add new threads after any that have the same
797             // priority.
798
799             // Because of the previous tests we know that this
800             // search will terminate before we hit the head of the
801             // queue, hence we do not need to check for that
802             // condition.
803                 
804             Cyg_Thread *qtmp = get_tail();
805
806             // Scan the queue until we find a higher or equal
807             // priority thread.
808
809             while( qtmp->priority > thread->priority )
810                 qtmp = qtmp->get_prev();
811
812             // Append ourself after the node pointed to by qtmp.
813                 
814             qtmp->append( thread );
815         }
816     }
817 #else
818     // Just add the thread to the tail of the list
819     add_tail( thread );
820 #endif
821     
822     thread->queue = CYG_CLASSFROMBASE(Cyg_ThreadQueue,
823                                       Cyg_ThreadQueue_Implementation,
824                                       this);
825     CYG_REPORT_RETURN();
826 }
827
828 // -------------------------------------------------------------------------
829
830 Cyg_Thread *
831 Cyg_ThreadQueue_Implementation::dequeue(void)
832 {
833     CYG_REPORT_FUNCTYPE("returning thread %08x");
834         
835     Cyg_Thread *thread = rem_head();
836
837     CYG_INSTRUMENT_MLQ( DEQUEUE, this, thread );
838     
839     if( thread != NULL )
840         thread->queue = NULL;
841
842     CYG_REPORT_RETVAL(thread);
843     return thread;
844 }
845
846 // -------------------------------------------------------------------------
847
848 void
849 Cyg_ThreadQueue_Implementation::remove( Cyg_Thread *thread )
850 {
851     CYG_REPORT_FUNCTION();
852     CYG_REPORT_FUNCARG1("thread=%08x", thread);
853
854     CYG_INSTRUMENT_MLQ( REMOVE, this, thread );
855     
856     thread->queue = NULL;
857
858     Cyg_CList_T<Cyg_Thread>::remove( thread );
859
860     CYG_REPORT_RETURN();
861 }
862
863 // -------------------------------------------------------------------------
864
865 Cyg_Thread *
866 Cyg_ThreadQueue_Implementation::highpri(void)
867 {
868     CYG_REPORT_FUNCTYPE("returning thread %08x");
869     CYG_REPORT_RETVAL(get_head());
870     return get_head();
871 }
872
873 // -------------------------------------------------------------------------
874
875 inline void
876 Cyg_ThreadQueue_Implementation::set_thread_queue(Cyg_Thread *thread,
877                                                  Cyg_ThreadQueue *tq )
878
879 {
880     thread->queue = tq;
881 }
882
883 // -------------------------------------------------------------------------
884
885 #endif
886
887 // -------------------------------------------------------------------------
888 // EOF sched/mlqueue.cxx