3 files changed, 50 insertions, 43 deletions

diff --git a/cpukit/score/inline/rtems/score/schedulersimple.inl b/cpukit/score/inline/rtems/score/schedulersimple.inl
index e67fc3cd84..8b2480ea0a 100644
--- a/cpukit/score/inline/rtems/score/schedulersimple.inl
+++ b/cpukit/score/inline/rtems/score/schedulersimple.inl
@@ -48,6 +48,52 @@ RTEMS_INLINE_ROUTINE void _Scheduler_simple_Ready_queue_requeue(
   _Scheduler_simple_Ready_queue_enqueue( the_thread );
 }
 
+RTEMS_INLINE_ROUTINE bool _Scheduler_simple_Insert_priority_lifo_order(
+  const Chain_Node *to_insert,
+  const Chain_Node *next
+)
+{
+  const Thread_Control *thread_to_insert = (const Thread_Control *) to_insert;
+  const Thread_Control *thread_next = (const Thread_Control *) next;
+
+  return thread_to_insert->current_priority <= thread_next->current_priority;
+}
+
+RTEMS_INLINE_ROUTINE bool _Scheduler_simple_Insert_priority_fifo_order(
+  const Chain_Node *to_insert,
+  const Chain_Node *next
+)
+{
+  const Thread_Control *thread_to_insert = (const Thread_Control *) to_insert;
+  const Thread_Control *thread_next = (const Thread_Control *) next;
+
+  return thread_to_insert->current_priority < thread_next->current_priority;
+}
+
+RTEMS_INLINE_ROUTINE void _Scheduler_simple_Insert_priority_lifo(
+  Chain_Control *chain,
+  Thread_Control *to_insert
+)
+{
+  _Chain_Insert_ordered_unprotected(
+    chain,
+    &to_insert->Object.Node,
+    _Scheduler_simple_Insert_priority_lifo_order
+  );
+}
+
+RTEMS_INLINE_ROUTINE void _Scheduler_simple_Insert_priority_fifo(
+  Chain_Control *chain,
+  Thread_Control *to_insert
+)
+{
+  _Chain_Insert_ordered_unprotected(
+    chain,
+    &to_insert->Object.Node,
+    _Scheduler_simple_Insert_priority_fifo_order
+  );
+}
+
 /** @} */
 
 #endif
diff --git a/cpukit/score/src/schedulersimplereadyqueueenqueue.c b/cpukit/score/src/schedulersimplereadyqueueenqueue.c
index ea3c2c45ec..b33512f902 100644
--- a/cpukit/score/src/schedulersimplereadyqueueenqueue.c
+++ b/cpukit/score/src/schedulersimplereadyqueueenqueue.c
@@ -28,24 +28,7 @@ void _Scheduler_simple_Ready_queue_enqueue(
   Thread_Control    *the_thread
 )
 {
-  Chain_Control    *ready;
-  Chain_Node       *the_node;
-  Thread_Control   *current;
+  Chain_Control *ready = (Chain_Control *) _Scheduler.information;
 
-  ready    = (Chain_Control *)_Scheduler.information;
-  the_node = _Chain_First( ready );
-  current  = (Thread_Control *)ready;
-
-  for ( ; !_Chain_Is_tail( ready, the_node ) ; the_node = the_node->next ) {
-    current = (Thread_Control *) the_node;
-
-    /* break when AT END OR PAST our priority */
-    if ( the_thread->current_priority < current->current_priority ) {
-      current = (Thread_Control *)current->Object.Node.previous;
-      break;
-    }
-  }
-
-  /* enqueue */
-  _Chain_Insert_unprotected( (Chain_Node *)current, &the_thread->Object.Node );
+  _Scheduler_simple_Insert_priority_fifo( ready, the_thread );
 }
diff --git a/cpukit/score/src/schedulersimplereadyqueueenqueuefirst.c b/cpukit/score/src/schedulersimplereadyqueueenqueuefirst.c
index 47a4a3be64..c5ee8ebe12 100644
--- a/cpukit/score/src/schedulersimplereadyqueueenqueuefirst.c
+++ b/cpukit/score/src/schedulersimplereadyqueueenqueuefirst.c
@@ -27,29 +27,7 @@ void _Scheduler_simple_Ready_queue_enqueue_first(
   Thread_Control    *the_thread
 )
 {
-  Chain_Control    *ready;
-  Chain_Node       *the_node;
-  Thread_Control   *current;
+  Chain_Control *ready = (Chain_Control *) _Scheduler.information;
 
-  ready    = (Chain_Control *)_Scheduler.information;
-  current  = (Thread_Control *)ready;
-
-  /*
-   * Do NOT need to check for end of chain because there is always
-   * at least one task on the ready chain -- the IDLE task.  It can
-   * never block, should never attempt to obtain a semaphore or mutex,
-   * and thus will always be there.
-   */
-  for ( the_node = _Chain_First(ready) ; ; the_node = the_node->next ) {
-    current = (Thread_Control *) the_node;
-
-    /* break when AT HEAD OF (or PAST) our priority */
-    if ( the_thread->current_priority <= current->current_priority ) {
-      current = (Thread_Control *)current->Object.Node.previous;
-      break;
-    }
-  }
-
-  /* enqueue */
-  _Chain_Insert_unprotected( (Chain_Node *)current, &the_thread->Object.Node );
+  _Scheduler_simple_Insert_priority_lifo( ready, the_thread );
 }