score: Implement scheduler helping protocol

The following scheduler operations return a thread in need for help

    - unblock,
    - change priority, and
    - yield.

A thread in need for help is a thread that encounters a scheduler state
change from scheduled to ready or a thread that cannot be scheduled in
an unblock operation.  Such a thread can ask threads which depend on
resources owned by this thread for help.

Add a new ask for help scheduler operation.  This operation is used by
_Scheduler_Ask_for_help() to help threads in need for help returned by
the operations mentioned above.  This operation is also used by
_Scheduler_Thread_change_resource_root() in case the root of a resource
sub-tree changes.  A use case is the ownership change of a resource.

In case it is not possible to schedule a thread in need for help, then
the corresponding scheduler node will be placed into the set of ready
scheduler nodes of the scheduler instance.  Once a state change from
ready to scheduled happens for this scheduler node it may be used to
schedule the thread in need for help.

11 files changed, 266 insertions, 22 deletions

diff --git a/cpukit/score/src/schedulercbsnodeinit.c b/cpukit/score/src/schedulercbsnodeinit.c
index 1a5299a863..d16f3fa035 100644
--- a/cpukit/score/src/schedulercbsnodeinit.c
+++ b/cpukit/score/src/schedulercbsnodeinit.c
@@ -29,6 +29,8 @@ void _Scheduler_CBS_Node_initialize(
 
   (void) scheduler;
 
+  _Scheduler_Node_do_initialize( &node->Base.Base, the_thread );
+
   node->Base.thread = the_thread;
   node->Base.queue_state = SCHEDULER_EDF_QUEUE_STATE_NEVER_HAS_BEEN;
   node->cbs_server = NULL;
diff --git a/cpukit/score/src/schedulerchangeroot.c b/cpukit/score/src/schedulerchangeroot.c
new file mode 100644
index 0000000000..eba852ba5e
--- /dev/null
+++ b/cpukit/score/src/schedulerchangeroot.c
@@ -0,0 +1,85 @@
+/*
+ * Copyright (c) 2014 embedded brains GmbH.  All rights reserved.
+ *
+ *  embedded brains GmbH
+ *  Dornierstr. 4
+ *  82178 Puchheim
+ *  Germany
+ *  <rtems@embedded-brains.de>
+ *
+ * The license and distribution terms for this file may be
+ * found in the file LICENSE in this distribution or at
+ * http://www.rtems.org/license/LICENSE.
+ */
+
+#if HAVE_CONFIG_H
+  #include "config.h"
+#endif
+
+#include <rtems/score/schedulerimpl.h>
+
+typedef struct {
+  Thread_Control *root;
+  Thread_Control *needs_help;
+} Scheduler_Set_root_context;
+
+RTEMS_INLINE_ROUTINE bool _Scheduler_Set_root_visitor(
+  Resource_Node *resource_node,
+  void          *arg
+)
+{
+  Scheduler_Set_root_context *ctx = arg;
+  Thread_Control *root = ctx->root;
+  Thread_Control *needs_help = root;
+  Thread_Control *offers_help =
+    _Thread_Resource_node_to_thread( resource_node );
+  const Scheduler_Control *scheduler = _Scheduler_Get_own( offers_help );
+  Thread_Control *needs_help_too;
+
+  _Resource_Node_set_root( resource_node, &root->Resource_node );
+
+  needs_help_too = ( *scheduler->Operations.ask_for_help )(
+    scheduler,
+    offers_help,
+    needs_help
+  );
+
+  if ( needs_help_too != needs_help && needs_help_too != NULL ) {
+    _Assert( ctx->needs_help == NULL );
+    ctx->needs_help = needs_help_too;
+  }
+
+  return false;
+}
+
+void _Scheduler_Thread_change_resource_root(
+  Thread_Control *top,
+  Thread_Control *root
+)
+{
+  Scheduler_Set_root_context ctx = { root, NULL };
+  Thread_Control *offers_help = top;
+  Scheduler_Node *offers_help_node;
+  Thread_Control *offers_help_too;
+  ISR_Level level;
+
+  _ISR_Disable( level );
+
+  offers_help_node = _Scheduler_Thread_get_node( offers_help );
+  offers_help_too = _Scheduler_Node_get_owner( offers_help_node );
+
+  if ( offers_help != offers_help_too ) {
+    _Scheduler_Set_root_visitor( &offers_help_too->Resource_node, &ctx );
+    _Assert( ctx.needs_help == offers_help );
+    ctx.needs_help = NULL;
+  }
+
+  _Scheduler_Set_root_visitor( &top->Resource_node, &ctx );
+  _Resource_Iterate( &top->Resource_node, _Scheduler_Set_root_visitor, &ctx );
+
+  if ( ctx.needs_help != NULL ) {
+    _Scheduler_Ask_for_help( ctx.needs_help );
+  }
+
+  _ISR_Enable( level );
+}
diff --git a/cpukit/score/src/schedulerdefaultaskforhelp.c b/cpukit/score/src/schedulerdefaultaskforhelp.c
new file mode 100644
index 0000000000..b69524846b
--- /dev/null
+++ b/cpukit/score/src/schedulerdefaultaskforhelp.c
@@ -0,0 +1,26 @@
+/*
+ * Copyright (c) 2014 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.org/license/LICENSE.
+ */
+
+#if HAVE_CONFIG_H
+  #include "config.h"
+#endif
+
+#include <rtems/score/scheduler.h>
+
+Thread_Control *_Scheduler_default_Ask_for_help(
+  const Scheduler_Control *scheduler,
+  Thread_Control          *offers_help,
+  Thread_Control          *needs_help
+)
+{
+  (void) scheduler;
+  (void) offers_help;
+  (void) needs_help;
+
+  return NULL;
+}
diff --git a/cpukit/score/src/schedulerdefaultnodeinit.c b/cpukit/score/src/schedulerdefaultnodeinit.c
index ab371bd7fe..a96a528ee4 100644
--- a/cpukit/score/src/schedulerdefaultnodeinit.c
+++ b/cpukit/score/src/schedulerdefaultnodeinit.c
@@ -19,13 +19,16 @@
 #include "config.h"
 #endif
 
-#include <rtems/score/scheduler.h>
+#include <rtems/score/schedulerimpl.h>
 
 void _Scheduler_default_Node_initialize(
   const Scheduler_Control *scheduler,
   Thread_Control          *the_thread
 )
 {
+  Scheduler_Node *node = _Scheduler_Thread_get_own_node( the_thread );
+
   (void) scheduler;
-  (void) the_thread;
+
+  _Scheduler_Node_do_initialize( node, the_thread );
 }
diff --git a/cpukit/score/src/scheduleredfnodeinit.c b/cpukit/score/src/scheduleredfnodeinit.c
index b23f5acda8..e7f8af70a5 100644
--- a/cpukit/score/src/scheduleredfnodeinit.c
+++ b/cpukit/score/src/scheduleredfnodeinit.c
@@ -29,6 +29,8 @@ void _Scheduler_EDF_Node_initialize(
 
   (void) scheduler;
 
+  _Scheduler_Node_do_initialize( &node->Base, the_thread );
+
   node->thread = the_thread;
   node->queue_state = SCHEDULER_EDF_QUEUE_STATE_NEVER_HAS_BEEN;
 }
diff --git a/cpukit/score/src/schedulerpriorityaffinitysmp.c b/cpukit/score/src/schedulerpriorityaffinitysmp.c
index 24fae3cfee..39a0a481ea 100644
--- a/cpukit/score/src/schedulerpriorityaffinitysmp.c
+++ b/cpukit/score/src/schedulerpriorityaffinitysmp.c
@@ -60,6 +60,15 @@ static bool _Scheduler_priority_affinity_SMP_Insert_priority_fifo_order(
     && _Scheduler_SMP_Insert_priority_fifo_order( to_insert, next );
 }
 
+static Scheduler_priority_affinity_SMP_Node *
+_Scheduler_priority_affinity_SMP_Thread_get_own_node(
+  Thread_Control *thread
+)
+{
+  return (Scheduler_priority_affinity_SMP_Node *)
+    _Scheduler_Thread_get_own_node( thread );
+}
+
 /*
  * This method returns the scheduler node for the specified thread
  * as a scheduler specific type.
@@ -69,7 +78,8 @@ _Scheduler_priority_affinity_SMP_Thread_get_node(
   Thread_Control *thread
 )
 {
-  return (Scheduler_priority_affinity_SMP_Node *) _Scheduler_Thread_get_node( thread );
+  return (Scheduler_priority_affinity_SMP_Node *)
+    _Scheduler_Thread_get_node( thread );
 }
 
 static Scheduler_priority_affinity_SMP_Node *
@@ -90,7 +100,7 @@ void _Scheduler_priority_affinity_SMP_Node_initialize(
 )
 {
   Scheduler_priority_affinity_SMP_Node *node =
-    _Scheduler_priority_affinity_SMP_Thread_get_node( thread );
+    _Scheduler_priority_affinity_SMP_Thread_get_own_node( thread );
 
   (void) scheduler;
 
@@ -221,7 +231,8 @@ void _Scheduler_priority_affinity_SMP_Block(
     _Scheduler_priority_SMP_Extract_from_ready,
     _Scheduler_priority_affinity_SMP_Get_highest_ready,
     _Scheduler_priority_SMP_Move_from_ready_to_scheduled,
-    _Scheduler_SMP_Allocate_processor_exact
+    _Scheduler_SMP_Allocate_processor_exact,
+    _Scheduler_priority_SMP_Get_idle_thread
   );
 
   /*
@@ -303,7 +314,8 @@ static Thread_Control *_Scheduler_priority_affinity_SMP_Enqueue_fifo(
     _Scheduler_SMP_Insert_scheduled_fifo,
     _Scheduler_priority_SMP_Move_from_scheduled_to_ready,
     _Scheduler_priority_affinity_SMP_Get_lowest_scheduled,
-    _Scheduler_SMP_Allocate_processor_exact
+    _Scheduler_SMP_Allocate_processor_exact,
+    _Scheduler_priority_SMP_Release_idle_thread
   );
 }
 
@@ -387,7 +399,8 @@ Thread_Control *_Scheduler_priority_affinity_SMP_Unblock(
   needs_help = _Scheduler_SMP_Unblock(
     context,
     thread,
-    _Scheduler_priority_affinity_SMP_Enqueue_fifo
+    _Scheduler_priority_affinity_SMP_Enqueue_fifo,
+    _Scheduler_priority_SMP_Release_idle_thread
   );
 
   /*
@@ -420,7 +433,8 @@ static Thread_Control *_Scheduler_priority_affinity_SMP_Enqueue_ordered(
     insert_scheduled,
     _Scheduler_priority_SMP_Move_from_scheduled_to_ready,
     _Scheduler_priority_affinity_SMP_Get_lowest_scheduled,
-    _Scheduler_SMP_Allocate_processor_exact
+    _Scheduler_SMP_Allocate_processor_exact,
+    _Scheduler_priority_SMP_Release_idle_thread
   );
 }
 
@@ -463,11 +477,14 @@ _Scheduler_priority_affinity_SMP_Enqueue_scheduled_ordered(
     context,
     node,
     order,
+    _Scheduler_priority_SMP_Extract_from_ready,
     _Scheduler_priority_affinity_SMP_Get_highest_ready,
     insert_ready,
     insert_scheduled,
     _Scheduler_priority_SMP_Move_from_ready_to_scheduled,
-    _Scheduler_SMP_Allocate_processor_exact
+    _Scheduler_SMP_Allocate_processor_exact,
+    _Scheduler_priority_SMP_Get_idle_thread,
+    _Scheduler_priority_SMP_Release_idle_thread
   );
 }
 
@@ -543,6 +560,27 @@ Thread_Control *_Scheduler_priority_affinity_SMP_Change_priority(
   return displaced;
 }
 
+Thread_Control *_Scheduler_priority_affinity_SMP_Ask_for_help(
+  const Scheduler_Control *scheduler,
+  Thread_Control          *offers_help,
+  Thread_Control          *needs_help
+)
+{
+  Scheduler_Context *context = _Scheduler_Get_context( scheduler );
+
+  needs_help = _Scheduler_SMP_Ask_for_help(
+    context,
+    offers_help,
+    needs_help,
+    _Scheduler_priority_affinity_SMP_Enqueue_fifo,
+    _Scheduler_priority_SMP_Release_idle_thread
+  );
+
+  _Scheduler_priority_affinity_SMP_Check_for_migrations( context );
+
+  return needs_help;
+}
+
 /*
  * This is the public scheduler specific Change Priority operation.
  */
diff --git a/cpukit/score/src/schedulerprioritysmp.c b/cpukit/score/src/schedulerprioritysmp.c
index b642c5d6f3..93d1c76d30 100644
--- a/cpukit/score/src/schedulerprioritysmp.c
+++ b/cpukit/score/src/schedulerprioritysmp.c
@@ -47,7 +47,7 @@ void _Scheduler_priority_SMP_Node_initialize(
   Thread_Control *thread
 )
 {
-  Scheduler_SMP_Node *node = _Scheduler_SMP_Thread_get_node( thread );
+  Scheduler_SMP_Node *node = _Scheduler_SMP_Thread_get_own_node( thread );
 
   _Scheduler_SMP_Node_initialize( node, thread );
 }
@@ -93,7 +93,8 @@ void _Scheduler_priority_SMP_Block(
     _Scheduler_priority_SMP_Extract_from_ready,
     _Scheduler_priority_SMP_Get_highest_ready,
     _Scheduler_priority_SMP_Move_from_ready_to_scheduled,
-    _Scheduler_SMP_Allocate_processor_lazy
+    _Scheduler_SMP_Allocate_processor_lazy,
+    _Scheduler_priority_SMP_Get_idle_thread
   );
 }
 
@@ -115,7 +116,8 @@ static Thread_Control *_Scheduler_priority_SMP_Enqueue_ordered(
     insert_scheduled,
     _Scheduler_priority_SMP_Move_from_scheduled_to_ready,
     _Scheduler_SMP_Get_lowest_scheduled,
-    _Scheduler_SMP_Allocate_processor_lazy
+    _Scheduler_SMP_Allocate_processor_lazy,
+    _Scheduler_priority_SMP_Release_idle_thread
   );
 }
 
@@ -163,11 +165,14 @@ static Thread_Control *_Scheduler_priority_SMP_Enqueue_scheduled_ordered(
     context,
     node,
     order,
+    _Scheduler_priority_SMP_Extract_from_ready,
     _Scheduler_priority_SMP_Get_highest_ready,
     insert_ready,
     insert_scheduled,
     _Scheduler_priority_SMP_Move_from_ready_to_scheduled,
-    _Scheduler_SMP_Allocate_processor_lazy
+    _Scheduler_SMP_Allocate_processor_lazy,
+    _Scheduler_priority_SMP_Get_idle_thread,
+    _Scheduler_priority_SMP_Release_idle_thread
   );
 }
 
@@ -209,7 +214,8 @@ Thread_Control *_Scheduler_priority_SMP_Unblock(
   return _Scheduler_SMP_Unblock(
     context,
     thread,
-    _Scheduler_priority_SMP_Enqueue_fifo
+    _Scheduler_priority_SMP_Enqueue_fifo,
+    _Scheduler_priority_SMP_Release_idle_thread
   );
 }
 
@@ -236,6 +242,23 @@ Thread_Control *_Scheduler_priority_SMP_Change_priority(
   );
 }
 
+Thread_Control *_Scheduler_priority_SMP_Ask_for_help(
+  const Scheduler_Control *scheduler,
+  Thread_Control          *offers_help,
+  Thread_Control          *needs_help
+)
+{
+  Scheduler_Context *context = _Scheduler_Get_context( scheduler );
+
+  return _Scheduler_SMP_Ask_for_help(
+    context,
+    offers_help,
+    needs_help,
+    _Scheduler_priority_SMP_Enqueue_fifo,
+    _Scheduler_priority_SMP_Release_idle_thread
+  );
+}
+
 Thread_Control *_Scheduler_priority_SMP_Yield(
   const Scheduler_Control *scheduler,
   Thread_Control *thread
diff --git a/cpukit/score/src/schedulersimplesmp.c b/cpukit/score/src/schedulersimplesmp.c
index ee540bebd0..084d78251f 100644
--- a/cpukit/score/src/schedulersimplesmp.c
+++ b/cpukit/score/src/schedulersimplesmp.c
@@ -47,7 +47,7 @@ void _Scheduler_simple_SMP_Node_initialize(
   Thread_Control          *the_thread
 )
 {
-  Scheduler_SMP_Node *node = _Scheduler_SMP_Thread_get_node( the_thread );
+  Scheduler_SMP_Node *node = _Scheduler_SMP_Thread_get_own_node( the_thread );
 
   _Scheduler_SMP_Node_initialize( node, the_thread );
 }
@@ -162,6 +162,28 @@ static void _Scheduler_simple_SMP_Extract_from_ready(
   _Chain_Extract_unprotected( &node_to_extract->Node );
 }
 
+static Thread_Control *_Scheduler_simple_SMP_Get_idle_thread(
+  Scheduler_Context *context
+)
+{
+  return _Scheduler_SMP_Get_idle_thread(
+    context,
+    _Scheduler_simple_SMP_Extract_from_ready
+  );
+}
+
+static void _Scheduler_simple_SMP_Release_idle_thread(
+  Scheduler_Context *context,
+  Thread_Control    *idle
+)
+{
+  _Scheduler_SMP_Release_idle_thread(
+    context,
+    idle,
+    _Scheduler_simple_SMP_Insert_ready_fifo
+  );
+}
+
 void _Scheduler_simple_SMP_Block(
   const Scheduler_Control *scheduler,
   Thread_Control *thread
@@ -175,7 +197,8 @@ void _Scheduler_simple_SMP_Block(
     _Scheduler_simple_SMP_Extract_from_ready,
     _Scheduler_simple_SMP_Get_highest_ready,
     _Scheduler_simple_SMP_Move_from_ready_to_scheduled,
-    _Scheduler_SMP_Allocate_processor_lazy
+    _Scheduler_SMP_Allocate_processor_lazy,
+    _Scheduler_simple_SMP_Get_idle_thread
   );
 }
 
@@ -197,7 +220,8 @@ static Thread_Control *_Scheduler_simple_SMP_Enqueue_ordered(
     insert_scheduled,
     _Scheduler_simple_SMP_Move_from_scheduled_to_ready,
     _Scheduler_SMP_Get_lowest_scheduled,
-    _Scheduler_SMP_Allocate_processor_lazy
+    _Scheduler_SMP_Allocate_processor_lazy,
+    _Scheduler_simple_SMP_Release_idle_thread
   );
 }
 
@@ -245,11 +269,14 @@ static Thread_Control *_Scheduler_simple_SMP_Enqueue_scheduled_ordered(
     context,
     node,
     order,
+    _Scheduler_simple_SMP_Extract_from_ready,
     _Scheduler_simple_SMP_Get_highest_ready,
     insert_ready,
     insert_scheduled,
     _Scheduler_simple_SMP_Move_from_ready_to_scheduled,
-    _Scheduler_SMP_Allocate_processor_lazy
+    _Scheduler_SMP_Allocate_processor_lazy,
+    _Scheduler_simple_SMP_Get_idle_thread,
+    _Scheduler_simple_SMP_Release_idle_thread
   );
 }
 
@@ -291,7 +318,8 @@ Thread_Control *_Scheduler_simple_SMP_Unblock(
   return _Scheduler_SMP_Unblock(
     context,
     thread,
-    _Scheduler_simple_SMP_Enqueue_fifo
+    _Scheduler_simple_SMP_Enqueue_fifo,
+    _Scheduler_simple_SMP_Release_idle_thread
   );
 }
 
@@ -318,6 +346,23 @@ Thread_Control *_Scheduler_simple_SMP_Change_priority(
   );
 }
 
+Thread_Control *_Scheduler_simple_SMP_Ask_for_help(
+  const Scheduler_Control *scheduler,
+  Thread_Control          *offers_help,
+  Thread_Control          *needs_help
+)
+{
+  Scheduler_Context *context = _Scheduler_Get_context( scheduler );
+
+  return _Scheduler_SMP_Ask_for_help(
+    context,
+    offers_help,
+    needs_help,
+    _Scheduler_simple_SMP_Enqueue_fifo,
+    _Scheduler_simple_SMP_Release_idle_thread
+  );
+}
+
 Thread_Control *_Scheduler_simple_SMP_Yield(
   const Scheduler_Control *scheduler,
   Thread_Control *thread
diff --git a/cpukit/score/src/schedulersmpvalidstatechanges.c b/cpukit/score/src/schedulersmpdebug.c
index 6a5dcc6de4..4a45d2095f 100644
--- a/cpukit/score/src/schedulersmpvalidstatechanges.c
+++ b/cpukit/score/src/schedulersmpdebug.c
@@ -1,9 +1,9 @@
 /**
  * @file
  *
- * @ingroup ScoreSchedulerSMP
+ * @ingroup ScoreScheduler
  *
- * @brief SMP Scheduler Implementation
+ * @brief Scheduler SMP Debug Implementation
  */
 
 /*
@@ -24,10 +24,24 @@
   #include "config.h"
 #endif
 
+#include <rtems/score/schedulerimpl.h>
 #include <rtems/score/schedulerpriorityimpl.h>
 
+#if defined(RTEMS_DEBUG)
+
+/*
+ * Table with all valid state transitions for _Scheduler_Thread_change_state()
+ * in case RTEMS_DEBUG is defined.
+ */
+const bool _Scheduler_Thread_state_valid_state_changes[ 3 ][ 3 ] = {
+  /* FROM / TO       BLOCKED SCHEDULED READY */
+  /* BLOCKED    */ { false,  true,     true },
+  /* SCHEDULED  */ { true,   false,    true },
+  /* READY      */ { true,   true,     true }
+};
+
 /*
- * Table with all valid state transitions.  It is used in
+ * Table with all valid state transitions for
  * _Scheduler_SMP_Node_change_state() in case RTEMS_DEBUG is defined.
  */
 const bool _Scheduler_SMP_Node_valid_state_changes[ 3 ][ 3 ] = {
@@ -36,3 +50,5 @@ const bool _Scheduler_SMP_Node_valid_state_changes[ 3 ][ 3 ] = {
   /* SCHEDULED  */ { true,   false,    true },
   /* READY      */ { true,   true,     false }
 };
+
+#endif
diff --git a/cpukit/score/src/schedulersmpstartidle.c b/cpukit/score/src/schedulersmpstartidle.c
index 6809fd81a3..de125d3323 100644
--- a/cpukit/score/src/schedulersmpstartidle.c
+++ b/cpukit/score/src/schedulersmpstartidle.c
@@ -26,4 +26,5 @@ void _Scheduler_SMP_Start_idle(
 
   _Thread_Set_CPU( thread, cpu );
   _Chain_Append_unprotected( &self->Scheduled, &node->Base.Node );
+  _Chain_Prepend_unprotected( &self->Idle_threads, &thread->Object.Node );
 }
diff --git a/cpukit/score/src/threadinitialize.c b/cpukit/score/src/threadinitialize.c
index e6c4985591..e56e4e6cda 100644
--- a/cpukit/score/src/threadinitialize.c
+++ b/cpukit/score/src/threadinitialize.c
@@ -181,7 +181,10 @@ bool _Thread_Initialize(
   }
 
 #if defined(RTEMS_SMP)
+  the_thread->Scheduler.state = THREAD_SCHEDULER_BLOCKED;
+  the_thread->Scheduler.own_control = scheduler;
   the_thread->Scheduler.control = scheduler;
+  the_thread->Scheduler.own_node = the_thread->Scheduler.node;
   _Resource_Node_initialize( &the_thread->Resource_node );
   _CPU_Context_Set_is_executing( &the_thread->Registers, false );
 #endif