scheduler: New simple SMP scheduler implementation

The new Simple SMP Scheduler allocates a processor for the processor
count highest priority ready threads.  The thread priority and position
in the ready chain are the only information to determine the scheduling
decision.  Threads with an allocated processor are in the scheduled
chain.  After initialization the scheduled chain has exactly processor
count nodes.  Each processor has exactly one allocated thread after
initialization.  All enqueue and extract operations may exchange threads
with the scheduled chain.  One thread will be added and another will be
removed.  The scheduled and ready chain is ordered according to the
thread priority order.  The chain insert operations are O(count of ready
threads), thus this scheduler is unsuitable for most real-time
applications.

The thread preempt mode will be ignored.

8 files changed, 200 insertions, 337 deletions

diff --git a/cpukit/score/src/schedulersimplesmp.c b/cpukit/score/src/schedulersimplesmp.c
new file mode 100644
index 0000000000..ec1b0e27f4
--- /dev/null
+++ b/cpukit/score/src/schedulersimplesmp.c
@@ -0,0 +1,182 @@
+/**
+ * @file
+ *
+ * @brief Simple SMP Scheduler Implementation
+ *
+ * @ingroup ScoreSchedulerSMP
+ */
+
+/*
+ * Copyright (c) 2013 embedded brains GmbH.
+ *
+ * The license and distribution terms for this file may be
+ * found in the file LICENSE in this distribution or at
+ * http://www.rtems.com/license/LICENSE.
+ */
+
+#if HAVE_CONFIG_H
+  #include "config.h"
+#endif
+
+#include <rtems/score/schedulersimplesmp.h>
+
+static Scheduler_simple_smp_Control *_Scheduler_simple_smp_Instance( void )
+{
+  return _Scheduler.information;
+}
+
+void _Scheduler_simple_smp_Initialize( void )
+{
+  Scheduler_simple_smp_Control *self =
+    _Workspace_Allocate_or_fatal_error( sizeof( *self ) );
+
+  _Chain_Initialize_empty( &self->ready );
+  _Chain_Initialize_empty( &self->scheduled );
+
+  _Scheduler.information = self;
+}
+
+static void _Scheduler_simple_smp_Allocate_processor(
+  Thread_Control *scheduled,
+  Thread_Control *victim
+)
+{
+  Per_CPU_Control *cpu_of_scheduled = scheduled->cpu;
+  Per_CPU_Control *cpu_of_victim = victim->cpu;
+  Thread_Control *heir;
+
+  scheduled->is_scheduled = true;
+  victim->is_scheduled = false;
+
+  if ( scheduled->is_executing ) {
+    heir = cpu_of_scheduled->heir;
+    cpu_of_scheduled->heir = scheduled;
+  } else {
+    heir = scheduled;
+  }
+
+  if ( heir != victim ) {
+    heir->cpu = cpu_of_victim;
+    cpu_of_victim->heir = heir;
+    cpu_of_victim->dispatch_necessary = true;
+  }
+}
+
+static void _Scheduler_simple_smp_Move_from_scheduled_to_ready(
+  Chain_Control *ready_chain,
+  Thread_Control *scheduled_to_ready
+)
+{
+  _Chain_Extract_unprotected( &scheduled_to_ready->Object.Node );
+  _Scheduler_simple_Insert_priority_lifo( ready_chain, scheduled_to_ready );
+}
+
+static void _Scheduler_simple_smp_Move_from_ready_to_scheduled(
+  Chain_Control *scheduled_chain,
+  Thread_Control *ready_to_scheduled
+)
+{
+  _Chain_Extract_unprotected( &ready_to_scheduled->Object.Node );
+  _Scheduler_simple_Insert_priority_fifo( scheduled_chain, ready_to_scheduled );
+}
+
+static void _Scheduler_simple_smp_Insert(
+  Chain_Control *chain,
+  Thread_Control *thread,
+  Chain_Node_order order
+)
+{
+  _Chain_Insert_ordered_unprotected( chain, &thread->Object.Node, order );
+}
+
+static void _Scheduler_simple_smp_Enqueue_ordered(
+  Thread_Control *thread,
+  Chain_Node_order order
+)
+{
+  Scheduler_simple_smp_Control *self = _Scheduler_simple_smp_Instance();
+
+  /*
+   * The scheduled chain has exactly processor count nodes after
+   * initialization, thus the lowest priority scheduled thread exists.
+   */
+  Thread_Control *lowest_scheduled =
+    (Thread_Control *) _Chain_Last( &self->scheduled );
+
+  if ( ( *order )( &thread->Object.Node, &lowest_scheduled->Object.Node ) ) {
+    _Scheduler_simple_smp_Allocate_processor( thread, lowest_scheduled );
+
+    _Scheduler_simple_smp_Insert( &self->scheduled, thread, order );
+
+    _Scheduler_simple_smp_Move_from_scheduled_to_ready(
+      &self->ready,
+      lowest_scheduled
+    );
+  } else {
+    _Scheduler_simple_smp_Insert( &self->ready, thread, order );
+  }
+}
+
+void _Scheduler_simple_smp_Enqueue_priority_lifo( Thread_Control *thread )
+{
+  _Scheduler_simple_smp_Enqueue_ordered(
+    thread,
+    _Scheduler_simple_Insert_priority_lifo_order
+  );
+}
+
+void _Scheduler_simple_smp_Enqueue_priority_fifo( Thread_Control *thread )
+{
+  _Scheduler_simple_smp_Enqueue_ordered(
+    thread,
+    _Scheduler_simple_Insert_priority_fifo_order
+  );
+}
+
+void _Scheduler_simple_smp_Extract( Thread_Control *thread )
+{
+  Scheduler_simple_smp_Control *self = _Scheduler_simple_smp_Instance();
+
+  _Chain_Extract_unprotected( &thread->Object.Node );
+
+  if ( thread->is_scheduled ) {
+    Thread_Control *highest_ready =
+      (Thread_Control *) _Chain_First( &self->ready );
+
+    _Scheduler_simple_smp_Allocate_processor( highest_ready, thread );
+
+    _Scheduler_simple_smp_Move_from_ready_to_scheduled(
+      &self->scheduled,
+      highest_ready
+    );
+  }
+}
+
+void _Scheduler_simple_smp_Yield( Thread_Control *thread )
+{
+  ISR_Level level;
+
+  _ISR_Disable( level );
+
+  _Scheduler_simple_smp_Extract( thread );
+  _Scheduler_simple_smp_Enqueue_priority_fifo( thread );
+
+  _ISR_Enable( level );
+}
+
+void _Scheduler_simple_smp_Schedule( void )
+{
+  /* Nothing to do */
+}
+
+void _Scheduler_simple_smp_Start_idle(
+  Thread_Control *thread,
+  Per_CPU_Control *cpu
+)
+{
+  Scheduler_simple_smp_Control *self = _Scheduler_simple_smp_Instance();
+
+  thread->is_scheduled = true;
+  thread->cpu = cpu;
+  _Chain_Append_unprotected( &self->scheduled, &thread->Object.Node );
+}
diff --git a/cpukit/score/src/schedulersimplesmpblock.c b/cpukit/score/src/schedulersimplesmpblock.c
deleted file mode 100644
index 9d6d979684..0000000000
--- a/cpukit/score/src/schedulersimplesmpblock.c
+++ /dev/null
@@ -1,31 +0,0 @@
-/**
- * @file
- *
- * @brief Scheduler Simple Block
- * @ingroup ScoreScheduler
- */
-
-/*
- *  COPYRIGHT (c) 2011.
- *  On-Line Applications Research Corporation (OAR).
- *
- *  The license and distribution terms for this file may be
- *  found in found in the file LICENSE in this distribution or at
- *  http://www.rtems.com/license/LICENSE.
- */
-
-#if HAVE_CONFIG_H
-#include "config.h"
-#endif
-
-#include <rtems/system.h>
-#include <rtems/score/schedulersimplesmp.h>
-
-void _Scheduler_simple_smp_Block(
-  Thread_Control   *the_thread
-)
-{
-  _Scheduler_simple_Extract( the_thread );
-
-  _Scheduler_simple_smp_Schedule();
-}
diff --git a/cpukit/score/src/schedulersimplesmpschedule.c b/cpukit/score/src/schedulersimplesmpschedule.c
deleted file mode 100644
index 471f710ede..0000000000
--- a/cpukit/score/src/schedulersimplesmpschedule.c
+++ /dev/null
@@ -1,271 +0,0 @@
-/**
- *  @file
- *
- *  Scheduler Simple SMP Handler / Schedule
- */
-
-/*
- *  COPYRIGHT (c) 2011- 2012.
- *  On-Line Applications Research Corporation (OAR).
- *
- *  The license and distribution terms for this file may be
- *  found in found in the file LICENSE in this distribution or at
- *  http://www.rtems.com/license/LICENSE.
- */
-
-#if HAVE_CONFIG_H
-#include "config.h"
-#endif
-
-#include <rtems/system.h>
-#include <rtems/score/smp.h>
-#include <rtems/score/schedulersimplesmp.h>
-
-#include <stdio.h>
-
-/*
- *  The following is very useful on the scheduler simulator when debugging
- *  this algorithm.  You are not likely to be able to print like this when
- *  running on a real system.
- *
- *  NOTE: This is NOT using RTEMS_DEBUG because this is not consistency
- *        checking and is VERY VERY noisy.  It is just for debugging
- *        the algorithm.
- */
-#if 0
-#define D(format,...) printk( format, __VA_ARGS__)
-#else
-#define D(format,...)
-#endif
-
-/* Declaration to avoid warnings */
-bool _Scheduler_simple_smp_Assign(
-  Thread_Control *consider
-);
-
-/**
- *  @brief Assign Heir Thread to CPU
- *
- *  This method attempts to find a core for the thread under consideration
- *  (@a consider) to become heir of.  The placement takes into account
- *  priority, preemptibility, and length of time on core.
- *
- *  Combined with the loop in _Scheduler_simple_smp_Schedule, it attempts
- *  to faithfully implement the behaviour of the Deterministic Priority
- *  Scheduler while spreading the threads across multiple cores.
- *
- *  @param[in] consider is the thread under consideration.  This means
- *             that the method is looking for the best core to place it
- *             as heir.
- *
- *  @return This method returns true if it was able to make @a consider
- *          the heir on a core and false otherwise.  When this returns
- *          false, there is no point in attempting to place an heir of
- *          of lower priority.
- */
-bool _Scheduler_simple_smp_Assign(
-  Thread_Control *consider
-)
-{
-  bool            found;       /* have we found a cpu to place it on? */
-  uint32_t        found_cpu;   /* CPU to place this thread */
-  bool            blocked;     /* CPU has blocked thread? */
-  Thread_Control *pheir;       /* heir on found cpu to potentially replace */
-  Thread_Control *h;
-  Thread_Control *e;
-  uint32_t        cpu;
-
-  /*
-   *  Initialize various variables to indicate we have not found
-   *  a potential core for the thread under consideration.
-   */
-  found     = false;
-  blocked   = false;
-  found_cpu = 0;
-  pheir     = NULL;
-
-  for ( cpu=0 ; cpu < _SMP_Processor_count ; cpu++ ) {
-    D( "SCHED CPU=%d consider=0x%08x ASSIGN\n", cpu, consider->Object.id );
-
-    /*
-     *  If the thread under consideration is already executing or
-     *  heir, then we don't have a better option for it.
-     */
-    e = _Per_CPU_Information[cpu].executing;
-    if ( e == consider ) {
-      D( "SCHED CPU=%d Executing=0x%08x considering=0x%08x ASSIGNED\n",
-        cpu, e->Object.id, consider->Object.id );
-      return true;
-    }
-
-    if ( !_States_Is_ready( e->current_state ) ) {
-      pheir     = e;
-      found_cpu = cpu;
-      found     = true;
-      blocked   = true;
-      D( "SCHED CPU=%d PHeir=0x%08x considering=0x%08x BLOCKED\n",
-        cpu, e->Object.id, consider->Object.id );
-      continue;
-    }
-
-    h = _Per_CPU_Information[cpu].heir;
-    if ( h == consider ) {
-      D( "SCHED CPU=%d Heir=0x%08x considering=0x%08x ASSIGNED\n",
-        cpu, h->Object.id, consider->Object.id );
-      return true;
-    }
-
-    if ( blocked )
-      continue;
-
-    /*
-     *  If we haven't found a potential CPU to locate the thread
-     *  under consideration on, we need to consider if this is a
-     *  more important threads first (e.g. priority <).
-     *
-     *  But when a thread changes its priority and ends up at the end of
-     *  the priority group for the new heir, we also need to schedule
-     *  a new heir.  This is the "=" part of the check.
-     */
-    if ( !found ) {
-      if ( consider->current_priority <= h->current_priority ) {
-        pheir     = h;
-        found_cpu = cpu;
-        found     = true;
-        D( "SCHED CPU=%d PHeir=0x%08x considering=0x%08x MAYBE #1\n",
-          cpu, h->Object.id, consider->Object.id );
-      }
-
-      continue;
-    }
-
-    /*
-     *  ASSERTION: (found == true)
-     *
-     *  Past this point, we have found a potential heir and are considering
-     *  whether the thread is better placed on another core. It is desirable
-     *  to preempt the lowest priority thread using time on core and
-     *  preemptibility as additional factors.
-     */
-
-    /*
-     *  If we have found a potential CPU on which to make the
-     *  thread under consideration the heir, then we need to
-     *  check if the current CPU is a more appropriate place
-     *  for this thread to be placed.
-     *
-     *  Check 1: heir of potential CPU is more important
-     *           then heir of current CPU.  We want to
-     *           replace the least important thread possible.
-     */
-    if ( h->current_priority > pheir->current_priority ) {
-      pheir = h;
-      found_cpu = cpu;
-      D( "SCHED CPU=%d PHeir=0x%08x considering=0x%08x MAYBE #2\n",
-        cpu, h->Object.id, consider->Object.id );
-      continue;
-    }
-
-    /*
-     *  If the current heir is more important than the potential
-     *  heir, then we should not consider it further in scheduling.
-     */
-    if ( h->current_priority < pheir->current_priority )
-      continue;
-
-    /*
-     *  ASSERTION: (h->current_priority == pheir->current_priority).
-     *
-     *  Past this point, this means we are considering the length of time
-     *  the thread has spent on the time on the CPU core and if it is
-     *  preemptible.
-     */
-
-    /*
-     *  If heir of potential CPU and of the current CPU are of the SAME
-     *  priority, then which has been running longer?
-     *
-     *           Which CPU has had its executing thread longer?
-     */
-    if ( _Timestamp_Less_than(
-           &_Per_CPU_Information[cpu].time_of_last_context_switch,
-           &_Per_CPU_Information[found_cpu].time_of_last_context_switch
-         ) ) {
-      pheir = h;
-      found_cpu = cpu;
-      D( "SCHED CPU=%d PHeir=0x%08x considering=0x%08x LONGER\n",
-        cpu, h->Object.id, consider->Object.id );
-      continue;
-    }
-
-    /*
-     *  If we are looking at a core with a non-preemptible thread
-     *  for potential placement, then favor a core with a preeemtible
-     *  thread of the same priority.  This should help avoid priority
-     *  inversions and let threads run earlier.
-     */
-    if ( !pheir->is_preemptible && h->is_preemptible ) {
-      D( "SCHED CPU=%d PHeir==0x%08x is NOT PREEMPTIBLE\n", 
-        cpu, pheir->Object.id );
-      pheir = h;
-      found_cpu = cpu;
-      D( "SCHED CPU=%d PHeir=0x%08x considering=0x%08x PREEMPTIBLE\n",
-        cpu, h->Object.id, consider->Object.id );
-      continue;
-
-    }
-  }
-
-  /*
-   *  If we found a cpu to make this thread heir of, then we
-   *  need to consider whether we need a dispatch on that CPU.
-   */
-  if ( found ) {
-    e = _Per_CPU_Information[found_cpu].executing;
-    D( "SCHED CPU=%d executing=0x%08x considering=0x%08x FOUND\n",
-      found_cpu, e->Object.id, consider->Object.id );
-    _Per_CPU_Information[found_cpu].heir = consider;
-    if ( !_States_Is_ready( e->current_state ) ) {
-       D( "SCHED CPU=%d executing not ready dispatch needed\n", found_cpu);
-      _Per_CPU_Information[found_cpu].dispatch_necessary = true;
-    } else if ( consider->current_priority < e->current_priority ) {
-      if ( e->is_preemptible || consider->current_priority == 0 ) {
-         D( "SCHED CPU=%d preempting\n", found_cpu);
-        _Per_CPU_Information[found_cpu].dispatch_necessary = true;
-      }
-    }
-  }
-
-  /*
-   *  Return true to indicate we changed an heir.  This indicates
-   *  scheduling needs to examine more threads.
-   */
-  return found;
-}
-
-/*
- *  Reschedule threads -- select heirs for all cores
- */
-void _Scheduler_simple_smp_Schedule(void)
-{
-  Chain_Control  *c;
-  Chain_Node     *n;
-  Thread_Control *t;
-  uint32_t        cpu;
-
-  c   = (Chain_Control *)_Scheduler.information;
-  cpu = 0;
-
-  /*
-   *  Iterate over the first N (where N is the number of CPU cores) threads
-   *  on the ready chain.  Attempt to assign each as heir on a core.  When
-   *  unable to assign a thread as a new heir, then stop.
-   */
-  for (n = _Chain_First(c); !_Chain_Is_tail(c, n); n = _Chain_Next(n)) {
-    t = (Thread_Control *)n;
-    if ( !_Scheduler_simple_smp_Assign( t ) )
-      break;
-    if ( ++cpu >= _SMP_Processor_count )
-      break;
-  }
-}
diff --git a/cpukit/score/src/schedulersimplesmpunblock.c b/cpukit/score/src/schedulersimplesmpunblock.c
deleted file mode 100644
index ef61d483e8..0000000000
--- a/cpukit/score/src/schedulersimplesmpunblock.c
+++ /dev/null
@@ -1,34 +0,0 @@
-/**
- * @file
- *
- * @brief Scheduler Simple SMP Unblock Method
- * @ingroup ScoreScheduler
- */
- 
-/*
- *  COPYRIGHT (c) 2011.
- *  On-Line Applications Research Corporation (OAR).
- *
- *  The license and distribution terms for this file may be
- *  found in found in the file LICENSE in this distribution or at
- *  http://www.rtems.com/license/LICENSE.
- */
-
-#if HAVE_CONFIG_H
-#include "config.h"
-#endif
-
-#include <rtems/system.h>
-#include <rtems/score/schedulersimplesmp.h>
-
-void _Scheduler_simple_smp_Unblock(
-  Thread_Control    *the_thread
-)
-{
-  _Scheduler_simple_Ready_queue_enqueue(the_thread);
-
-  /*
-   *  Evaluate all CPUs and pick heirs
-   */
-  _Scheduler_simple_smp_Schedule();
-}
diff --git a/cpukit/score/src/smp.c b/cpukit/score/src/smp.c
index 10acbe25de..3081062f87 100644
--- a/cpukit/score/src/smp.c
+++ b/cpukit/score/src/smp.c
@@ -51,7 +51,10 @@ void rtems_smp_secondary_cpu_initialize( void )
    *  THIS core.
    */
   heir = per_cpu->heir;
+  heir->is_executing = true;
+  per_cpu->executing->is_executing = false;
   per_cpu->executing = heir;
+  per_cpu->dispatch_necessary = false;
 
   /*
    * Threads begin execution in the _Thread_Handler() function.   This function
diff --git a/cpukit/score/src/threaddispatch.c b/cpukit/score/src/threaddispatch.c
index 4856f8e67b..4cd449d55b 100644
--- a/cpukit/score/src/threaddispatch.c
+++ b/cpukit/score/src/threaddispatch.c
@@ -94,7 +94,10 @@ void _Thread_Dispatch( void )
   _ISR_Disable( level );
   while ( _Thread_Dispatch_necessary == true ) {
     heir = _Thread_Heir;
-    #ifndef RTEMS_SMP
+    #if defined(RTEMS_SMP)
+      executing->is_executing = false;
+      heir->is_executing = true;
+    #else
       _Thread_Dispatch_set_disable_level( 1 );
     #endif
     _Thread_Dispatch_necessary = false;
diff --git a/cpukit/score/src/threadinitialize.c b/cpukit/score/src/threadinitialize.c
index fb22578c3d..4c40ae87b8 100644
--- a/cpukit/score/src/threadinitialize.c
+++ b/cpukit/score/src/threadinitialize.c
@@ -180,6 +180,12 @@ bool _Thread_Initialize(
 
   the_thread->Start.isr_level         = isr_level;
 
+#if defined(RTEMS_SMP)
+  the_thread->is_scheduled            = false;
+  the_thread->is_executing            = false;
+  the_thread->cpu                     = NULL;
+#endif
+
   the_thread->current_state           = STATES_DORMANT;
   the_thread->Wait.queue              = NULL;
   the_thread->resource_count          = 0;
diff --git a/cpukit/score/src/threadstartmultitasking.c b/cpukit/score/src/threadstartmultitasking.c
index 32a594bd3b..16d1328148 100644
--- a/cpukit/score/src/threadstartmultitasking.c
+++ b/cpukit/score/src/threadstartmultitasking.c
@@ -43,6 +43,11 @@ void _Thread_Start_multitasking( void )
 
   _Thread_Dispatch_necessary = false;
 
+  #if defined(RTEMS_SMP)
+    _Thread_Executing->is_executing = false;
+    _Thread_Heir->is_executing = true;
+  #endif
+
   _Thread_Executing = _Thread_Heir;
 
    /*