score: Rework EDF scheduler

Use inline red-black tree insert.  Do not use shifting priorities since
this is not supported by the thread queues.  Due to the 32-bit
Priority_Control this currently limits the uptime to 49days with a 1ms
clock tick.

Update #2173.

12 files changed, 191 insertions, 183 deletions

diff --git a/cpukit/score/include/rtems/score/scheduleredf.h b/cpukit/score/include/rtems/score/scheduleredf.h
index feaa2efa62..4eb5f49d4b 100644
--- a/cpukit/score/include/rtems/score/scheduleredf.h
+++ b/cpukit/score/include/rtems/score/scheduleredf.h
@@ -35,7 +35,7 @@ extern "C" {
  */
 /**@{*/
 
-#define SCHEDULER_EDF_MAXIMUM_PRIORITY 255
+#define SCHEDULER_EDF_MAXIMUM_PRIORITY 0x7fffffff
 
 /**
  *  Entry points for the Earliest Deadline First Scheduler.
@@ -82,18 +82,6 @@ typedef struct {
 } Scheduler_EDF_Context;
 
 /**
- * @typedef Scheduler_EDF_Queue_state
- *
- * This enumeration distiguishes state of a thread with respect to the
- * ready queue.
- */
-typedef enum {
-  SCHEDULER_EDF_QUEUE_STATE_NOT_PRESENTLY,
-  SCHEDULER_EDF_QUEUE_STATE_YES,
-  SCHEDULER_EDF_QUEUE_STATE_NEVER_HAS_BEEN
-} Scheduler_EDF_Queue_state;
-
-/**
  * @brief Scheduler node specialization for EDF schedulers.
  */
 typedef struct {
@@ -110,10 +98,17 @@ typedef struct {
    * Rbtree node related to this thread.
    */
   RBTree_Node Node;
+
+  /**
+   * @brief The thread priority used by this scheduler instance in case no job
+   * is released.
+   */
+  Priority_Control background_priority;
+
   /**
-   * State of the thread with respect to ready queue.
+   * @brief The thread priority currently used by this scheduler instance.
    */
-  Scheduler_EDF_Queue_state queue_state;
+  Priority_Control current_priority;
 } Scheduler_EDF_Node;
 
 /**
diff --git a/cpukit/score/include/rtems/score/scheduleredfimpl.h b/cpukit/score/include/rtems/score/scheduleredfimpl.h
index e831caf9f9..7ff7aa2c12 100644
--- a/cpukit/score/include/rtems/score/scheduleredfimpl.h
+++ b/cpukit/score/include/rtems/score/scheduleredfimpl.h
@@ -44,44 +44,92 @@ RTEMS_INLINE_ROUTINE Scheduler_EDF_Node *_Scheduler_EDF_Thread_get_node(
   return (Scheduler_EDF_Node *) _Scheduler_Thread_get_node( the_thread );
 }
 
-int _Scheduler_EDF_Priority_compare (
-  Priority_Control p1,
-  Priority_Control p2
-);
+RTEMS_INLINE_ROUTINE bool _Scheduler_EDF_Less(
+  const void        *left,
+  const RBTree_Node *right
+)
+{
+  const Priority_Control   *the_left;
+  const Scheduler_EDF_Node *the_right;
+  Priority_Control          prio_left;
+  Priority_Control          prio_right;
+
+  the_left = left;
+  the_right = RTEMS_CONTAINER_OF( right, Scheduler_EDF_Node, Node );
+
+  prio_left = *the_left;
+  prio_right = the_right->current_priority;
+
+  return prio_left < prio_right;
+}
+
+RTEMS_INLINE_ROUTINE bool _Scheduler_EDF_Less_or_equal(
+  const void        *left,
+  const RBTree_Node *right
+)
+{
+  const Priority_Control   *the_left;
+  const Scheduler_EDF_Node *the_right;
+  Priority_Control          prio_left;
+  Priority_Control          prio_right;
 
-RBTree_Compare_result _Scheduler_EDF_Compare(
-  const RBTree_Node* n1,
-  const RBTree_Node* n2
-);
+  the_left = left;
+  the_right = RTEMS_CONTAINER_OF( right, Scheduler_EDF_Node, Node );
+
+  prio_left = *the_left;
+  prio_right = the_right->current_priority;
+
+  return prio_left <= prio_right;
+}
 
 RTEMS_INLINE_ROUTINE void _Scheduler_EDF_Enqueue(
-  const Scheduler_Control *scheduler,
-  Thread_Control          *the_thread
+  Scheduler_EDF_Context *context,
+  Scheduler_EDF_Node    *node,
+  Priority_Control       current_priority
 )
 {
-  Scheduler_EDF_Context *context =
-    _Scheduler_EDF_Get_context( scheduler );
-  Scheduler_EDF_Node *node = _Scheduler_EDF_Thread_get_node( the_thread );
+  _RBTree_Insert_inline(
+    &context->Ready,
+    &node->Node,
+    &current_priority,
+    _Scheduler_EDF_Less
+  );
+}
 
-  _RBTree_Insert(
+RTEMS_INLINE_ROUTINE void _Scheduler_EDF_Enqueue_first(
+  Scheduler_EDF_Context *context,
+  Scheduler_EDF_Node    *node,
+  Priority_Control       current_priority
+)
+{
+  _RBTree_Insert_inline(
     &context->Ready,
     &node->Node,
-    _Scheduler_EDF_Compare,
-    false
+    &current_priority,
+    _Scheduler_EDF_Less_or_equal
   );
-  node->queue_state = SCHEDULER_EDF_QUEUE_STATE_YES;
 }
 
 RTEMS_INLINE_ROUTINE void _Scheduler_EDF_Extract(
+  Scheduler_EDF_Context *context,
+  Scheduler_EDF_Node    *node
+)
+{
+  _RBTree_Extract( &context->Ready, &node->Node );
+}
+
+RTEMS_INLINE_ROUTINE void _Scheduler_EDF_Extract_body(
   const Scheduler_Control *scheduler,
   Thread_Control          *the_thread
 )
 {
-  Scheduler_EDF_Context *context =
-    _Scheduler_EDF_Get_context( scheduler );
-  Scheduler_EDF_Node *node = _Scheduler_EDF_Thread_get_node( the_thread );
+  Scheduler_EDF_Context *context;
+  Scheduler_EDF_Node    *node;
 
-  _RBTree_Extract( &context->Ready, &node->Node );
+  context = _Scheduler_EDF_Get_context( scheduler );
+  node = _Scheduler_EDF_Thread_get_node( the_thread );
+
+  _Scheduler_EDF_Extract( context, node );
 }
 
 RTEMS_INLINE_ROUTINE void _Scheduler_EDF_Schedule_body(
@@ -90,16 +138,17 @@ RTEMS_INLINE_ROUTINE void _Scheduler_EDF_Schedule_body(
   bool                     force_dispatch
 )
 {
-  Scheduler_EDF_Context *context =
-    _Scheduler_EDF_Get_context( scheduler );
-  RBTree_Node *first = _RBTree_Minimum( &context->Ready );
-  Scheduler_EDF_Node *node =
-    RTEMS_CONTAINER_OF( first, Scheduler_EDF_Node, Node );
-  Thread_Control *heir = node->thread;
+  Scheduler_EDF_Context *context;
+  RBTree_Node           *first;
+  Scheduler_EDF_Node    *node;
+
+  (void) the_thread;
 
-  ( void ) the_thread;
+  context = _Scheduler_EDF_Get_context( scheduler );
+  first = _RBTree_Minimum( &context->Ready );
+  node = RTEMS_CONTAINER_OF( first, Scheduler_EDF_Node, Node );
 
-  _Scheduler_Update_heir( heir, force_dispatch );
+  _Scheduler_Update_heir( node->thread, force_dispatch );
 }
 
 /**@}*/
diff --git a/cpukit/score/src/schedulercbsnodeinit.c b/cpukit/score/src/schedulercbsnodeinit.c
index d16f3fa035..df679b6588 100644
--- a/cpukit/score/src/schedulercbsnodeinit.c
+++ b/cpukit/score/src/schedulercbsnodeinit.c
@@ -25,13 +25,10 @@ void _Scheduler_CBS_Node_initialize(
   Thread_Control          *the_thread
 )
 {
-  Scheduler_CBS_Node *node = _Scheduler_CBS_Thread_get_node( the_thread );
+  Scheduler_CBS_Node *node;
 
-  (void) scheduler;
+  _Scheduler_EDF_Node_initialize( scheduler, the_thread );
 
-  _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 = _Scheduler_CBS_Thread_get_node( the_thread );
   node->cbs_server = NULL;
 }
diff --git a/cpukit/score/src/schedulercbsunblock.c b/cpukit/score/src/schedulercbsunblock.c
index 05e155a4c3..3f5bd33623 100644
--- a/cpukit/score/src/schedulercbsunblock.c
+++ b/cpukit/score/src/schedulercbsunblock.c
@@ -30,12 +30,15 @@ Scheduler_Void_or_thread _Scheduler_CBS_Unblock(
   Thread_Control          *the_thread
 )
 {
-  Scheduler_CBS_Node   *node = _Scheduler_CBS_Thread_get_node( the_thread );
-  Scheduler_CBS_Server *serv_info = node->cbs_server;
-  Priority_Control      new_priority;
+  Scheduler_EDF_Context *context;
+  Scheduler_CBS_Node    *node;
+  Scheduler_CBS_Server  *serv_info;
+  Priority_Control       priority;
 
-  _Scheduler_EDF_Enqueue( scheduler, the_thread );
-  /* TODO: flash critical section? */
+  context = _Scheduler_EDF_Get_context( scheduler );
+  node = _Scheduler_CBS_Thread_get_node( the_thread );
+  serv_info = node->cbs_server;
+  priority = node->Base.current_priority;
 
   /*
    * Late unblock rule for deadline-driven tasks. The remaining time to
@@ -43,29 +46,30 @@ Scheduler_Void_or_thread _Scheduler_CBS_Unblock(
    * without increased utilization of this task. It might cause a deadline
    * miss of another task.
    */
-  if (serv_info) {
+  if ( serv_info != NULL && ( priority & SCHEDULER_EDF_PRIO_MSB ) == 0 ) {
     time_t deadline = serv_info->parameters.deadline;
     time_t budget = serv_info->parameters.budget;
-    time_t deadline_left = the_thread->cpu_time_budget;
-    time_t budget_left = the_thread->real_priority -
-                           _Watchdog_Ticks_since_boot;
+    uint32_t deadline_left = the_thread->cpu_time_budget;
+    Priority_Control budget_left = priority - _Watchdog_Ticks_since_boot;
 
-    if ( deadline*budget_left > budget*deadline_left ) {
+    if ( deadline * budget_left > budget * deadline_left ) {
       /* Put late unblocked task to background until the end of period. */
-      new_priority = the_thread->Start.initial_priority;
-      the_thread->real_priority = new_priority;
-      if ( the_thread->current_priority != new_priority ) {
-        the_thread->current_priority = new_priority;
-        _Scheduler_EDF_Change_priority(
-          scheduler,
-          the_thread,
-          new_priority,
-          true
-        );
+
+      priority = node->Base.background_priority;
+      the_thread->real_priority = priority;
+
+      if (
+        _Thread_Priority_less_than( the_thread->current_priority, priority )
+          || !_Thread_Owns_resources( the_thread )
+      ) {
+        the_thread->current_priority = priority;
       }
     }
   }
 
+  node->Base.current_priority = priority;
+  _Scheduler_EDF_Enqueue( context, &node->Base, priority );
+
   /*
    *  If the thread that was unblocked is more important than the heir,
    *  then we have a new heir.  This may or may not result in a
@@ -78,16 +82,8 @@ Scheduler_Void_or_thread _Scheduler_CBS_Unblock(
    *    Even if the thread isn't preemptible, if the new heir is
    *    a pseudo-ISR system task, we need to do a context switch.
    */
-  if (
-    _Scheduler_EDF_Priority_compare(
-      the_thread->current_priority,
-      _Thread_Heir->current_priority
-    ) == 1 
-  ) {
-    _Scheduler_Update_heir(
-      the_thread,
-      the_thread->current_priority == PRIORITY_PSEUDO_ISR
-    );
+  if ( priority < _Thread_Heir->current_priority ) {
+    _Scheduler_Update_heir( the_thread, priority == PRIORITY_PSEUDO_ISR );
   }
 
   SCHEDULER_RETURN_VOID_OR_NULL;
diff --git a/cpukit/score/src/scheduleredf.c b/cpukit/score/src/scheduleredf.c
index 372612819a..21bbe240c8 100644
--- a/cpukit/score/src/scheduleredf.c
+++ b/cpukit/score/src/scheduleredf.c
@@ -20,47 +20,6 @@
 
 #include <rtems/score/scheduleredfimpl.h>
 
-int _Scheduler_EDF_Priority_compare (
-  Priority_Control p1,
-  Priority_Control p2
-)
-{
-  Watchdog_Interval time = _Watchdog_Ticks_since_boot;
-
-  /*
-   * Reorder priorities to separate deadline driven and background tasks.
-   *
-   * The background tasks have p1 or p2 > SCHEDULER_EDF_PRIO_MSB.
-   * The deadline driven tasks need to have subtracted current time in order
-   * to see which deadline is closer wrt. current time.
-   */
-  if (!(p1 & SCHEDULER_EDF_PRIO_MSB))
-    p1 = (p1 - time) & ~SCHEDULER_EDF_PRIO_MSB;
-  if (!(p2 & SCHEDULER_EDF_PRIO_MSB))
-    p2 = (p2 - time) & ~SCHEDULER_EDF_PRIO_MSB;
-
-  return ((p1<p2) - (p1>p2));
-}
-
-RBTree_Compare_result _Scheduler_EDF_Compare(
-  const RBTree_Node* n1,
-  const RBTree_Node* n2
-)
-{
-  Scheduler_EDF_Node *edf1 =
-    RTEMS_CONTAINER_OF( n1, Scheduler_EDF_Node, Node );
-  Scheduler_EDF_Node *edf2 =
-    RTEMS_CONTAINER_OF( n2, Scheduler_EDF_Node, Node );
-  Priority_Control value1 = edf1->thread->current_priority;
-  Priority_Control value2 = edf2->thread->current_priority;
-
-  /*
-   * This function compares only numbers for the red-black tree,
-   * but priorities have an opposite sense.
-   */
-  return (-1)*_Scheduler_EDF_Priority_compare(value1, value2);
-}
-
 void _Scheduler_EDF_Initialize( const Scheduler_Control *scheduler )
 {
   Scheduler_EDF_Context *context =
diff --git a/cpukit/score/src/scheduleredfblock.c b/cpukit/score/src/scheduleredfblock.c
index aaa9c127b2..80cb83d70b 100644
--- a/cpukit/score/src/scheduleredfblock.c
+++ b/cpukit/score/src/scheduleredfblock.c
@@ -29,7 +29,7 @@ void _Scheduler_EDF_Block(
   _Scheduler_Generic_block(
     scheduler,
     the_thread,
-    _Scheduler_EDF_Extract,
+    _Scheduler_EDF_Extract_body,
     _Scheduler_EDF_Schedule_body
   );
 }
diff --git a/cpukit/score/src/scheduleredfchangepriority.c b/cpukit/score/src/scheduleredfchangepriority.c
index 6efbf87ad3..a0f5ec7061 100644
--- a/cpukit/score/src/scheduleredfchangepriority.c
+++ b/cpukit/score/src/scheduleredfchangepriority.c
@@ -43,17 +43,25 @@ Scheduler_Void_or_thread _Scheduler_EDF_Change_priority(
   bool                     prepend_it
 )
 {
-  Scheduler_EDF_Context *context =
-    _Scheduler_EDF_Get_context( scheduler );
-  Scheduler_EDF_Node *node = _Scheduler_EDF_Thread_get_node( the_thread );
-
-  _RBTree_Extract( &context->Ready, &node->Node );
-  _RBTree_Insert(
-    &context->Ready,
-    &node->Node,
-    _Scheduler_EDF_Compare,
-    false
-  );
+  Scheduler_EDF_Context *context;
+  Scheduler_EDF_Node    *node;
+
+  context = _Scheduler_EDF_Get_context( scheduler );
+  node = _Scheduler_EDF_Thread_get_node( the_thread );
+
+  if ( ( new_priority & SCHEDULER_EDF_PRIO_MSB ) != 0 ) {
+    node->background_priority = new_priority;
+  }
+
+  node->current_priority = new_priority;
+
+  _Scheduler_EDF_Extract( context, node );
+
+  if ( prepend_it ) {
+    _Scheduler_EDF_Enqueue_first( context, node, new_priority );
+  } else {
+    _Scheduler_EDF_Enqueue( context, node, new_priority );
+  }
 
   _Scheduler_EDF_Schedule_body( scheduler, the_thread, false );
 
diff --git a/cpukit/score/src/scheduleredfnodeinit.c b/cpukit/score/src/scheduleredfnodeinit.c
index e7f8af70a5..ec6d9bae7c 100644
--- a/cpukit/score/src/scheduleredfnodeinit.c
+++ b/cpukit/score/src/scheduleredfnodeinit.c
@@ -32,5 +32,4 @@ void _Scheduler_EDF_Node_initialize(
   _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/scheduleredfreleasejob.c b/cpukit/score/src/scheduleredfreleasejob.c
index b7c83a55ad..b35fb9d80e 100644
--- a/cpukit/score/src/scheduleredfreleasejob.c
+++ b/cpukit/score/src/scheduleredfreleasejob.c
@@ -18,29 +18,45 @@
 #include "config.h"
 #endif
 
-#include <rtems/score/scheduleredf.h>
-#include <rtems/score/threadimpl.h>
-#include <rtems/score/watchdogimpl.h>
+#include <rtems/score/scheduleredfimpl.h>
 
-void _Scheduler_EDF_Release_job(
-  const Scheduler_Control *scheduler,
-  Thread_Control          *the_thread,
-  uint64_t                 deadline
+static bool _Scheduler_EDF_Priority_filter(
+  Thread_Control   *the_thread,
+  Priority_Control *new_priority_p,
+  void             *arg
 )
 {
-  Priority_Control new_priority;
-  Priority_Control unused;
+  Scheduler_EDF_Node *node;
+  Priority_Control    current_priority;
+  Priority_Control    new_priority;
 
-  (void) scheduler;
+  node = _Scheduler_EDF_Thread_get_node( the_thread );
 
-  if (deadline) {
-    /* Initializing or shifting deadline. */
-    new_priority = (uint32_t) deadline & ~SCHEDULER_EDF_PRIO_MSB;
-  }
-  else {
-    /* Switch back to background priority. */
-    new_priority = the_thread->Start.initial_priority;
+  current_priority = the_thread->current_priority;
+  new_priority = *new_priority_p;
+
+  if ( new_priority == 0 ) {
+    new_priority = node->background_priority;
   }
 
-  _Thread_Set_priority( the_thread, new_priority, &unused, true );
+  node->current_priority = new_priority;
+  the_thread->real_priority = new_priority;
+
+  return _Thread_Priority_less_than( current_priority, new_priority )
+    || !_Thread_Owns_resources( the_thread );
+}
+
+void _Scheduler_EDF_Release_job(
+  const Scheduler_Control *scheduler,
+  Thread_Control          *the_thread,
+  uint64_t                 deadline
+)
+{
+  _Thread_Change_priority(
+    the_thread,
+    (Priority_Control) deadline,
+    NULL,
+    _Scheduler_EDF_Priority_filter,
+    true
+  );
 }
diff --git a/cpukit/score/src/scheduleredfunblock.c b/cpukit/score/src/scheduleredfunblock.c
index 43d9753d34..b3acbcd4c2 100644
--- a/cpukit/score/src/scheduleredfunblock.c
+++ b/cpukit/score/src/scheduleredfunblock.c
@@ -27,8 +27,13 @@ Scheduler_Void_or_thread _Scheduler_EDF_Unblock(
   Thread_Control          *the_thread
 )
 {
-  _Scheduler_EDF_Enqueue( scheduler, the_thread );
-  /* TODO: flash critical section? */
+  Scheduler_EDF_Context *context;
+  Scheduler_EDF_Node    *node;
+
+  context = _Scheduler_EDF_Get_context( scheduler );
+  node = _Scheduler_EDF_Thread_get_node( the_thread );
+
+  _Scheduler_EDF_Enqueue( context, node, node->current_priority );
 
   /*
    *  If the thread that was unblocked is more important than the heir,
@@ -42,12 +47,7 @@ Scheduler_Void_or_thread _Scheduler_EDF_Unblock(
    *    Even if the thread isn't preemptible, if the new heir is
    *    a pseudo-ISR system task, we need to do a context switch.
    */
-  if (
-    _Scheduler_EDF_Priority_compare(
-      the_thread->current_priority,
-      _Thread_Heir->current_priority
-    ) == 1 
-  ) {
+  if ( the_thread->current_priority < _Thread_Heir->current_priority ) {
     _Scheduler_Update_heir(
       the_thread,
       the_thread->current_priority == PRIORITY_PSEUDO_ISR
diff --git a/cpukit/score/src/scheduleredfupdate.c b/cpukit/score/src/scheduleredfupdate.c
index dfa2e81281..5d475fe5f1 100644
--- a/cpukit/score/src/scheduleredfupdate.c
+++ b/cpukit/score/src/scheduleredfupdate.c
@@ -26,16 +26,13 @@ void _Scheduler_EDF_Update_priority(
   Priority_Control         new_priority
 )
 {
-  Scheduler_EDF_Node *node = _Scheduler_EDF_Thread_get_node( the_thread );
+  Scheduler_EDF_Node *node;
 
-  (void) scheduler;
-  (void) new_priority;
+  node = _Scheduler_EDF_Thread_get_node( the_thread );
 
-  if (node->queue_state == SCHEDULER_EDF_QUEUE_STATE_NEVER_HAS_BEEN) {
-    /* Shifts the priority to the region of background tasks. */
-    the_thread->Start.initial_priority |= (SCHEDULER_EDF_PRIO_MSB);
-    the_thread->real_priority    = the_thread->Start.initial_priority;
-    the_thread->current_priority = the_thread->Start.initial_priority;
-    node->queue_state = SCHEDULER_EDF_QUEUE_STATE_NOT_PRESENTLY;
+  if ( ( new_priority & SCHEDULER_EDF_PRIO_MSB ) != 0 ) {
+    node->background_priority = new_priority;
   }
+
+  node->current_priority = new_priority;
 }
diff --git a/cpukit/score/src/scheduleredfyield.c b/cpukit/score/src/scheduleredfyield.c
index 0ad5c32c7c..dae023a0c5 100644
--- a/cpukit/score/src/scheduleredfyield.c
+++ b/cpukit/score/src/scheduleredfyield.c
@@ -26,22 +26,14 @@ Scheduler_Void_or_thread _Scheduler_EDF_Yield(
   Thread_Control          *the_thread
 )
 {
-  Scheduler_EDF_Context *context =
-    _Scheduler_EDF_Get_context( scheduler );
-  Scheduler_EDF_Node    *node = _Scheduler_EDF_Thread_get_node( the_thread );
+  Scheduler_EDF_Context *context;
+  Scheduler_EDF_Node    *node;
 
-  /*
-   * The RBTree has more than one node, enqueue behind the tasks
-   * with the same priority in case there are such ones.
-   */
-  _RBTree_Extract( &context->Ready, &node->Node );
-  _RBTree_Insert(
-    &context->Ready,
-    &node->Node,
-    _Scheduler_EDF_Compare,
-    false
-  );
+  context = _Scheduler_EDF_Get_context( scheduler );
+  node = _Scheduler_EDF_Thread_get_node( the_thread );
 
+  _Scheduler_EDF_Extract( context, node );
+  _Scheduler_EDF_Enqueue( context, node, node->current_priority );
   _Scheduler_EDF_Schedule_body( scheduler, the_thread, false );
 
   SCHEDULER_RETURN_VOID_OR_NULL;