score: Fix priority message queue insert

Move the linear search into a critical section to avoid corruption due
to higher priority interrupts.  The interrupt disable time depends now
on the count of pending messages.

Close #2328.

2 files changed, 44 insertions, 95 deletions

diff --git a/cpukit/score/include/rtems/score/coremsgimpl.h b/cpukit/score/include/rtems/score/coremsgimpl.h
index 52796ad885..cedf2760e8 100644
--- a/cpukit/score/include/rtems/score/coremsgimpl.h
+++ b/cpukit/score/include/rtems/score/coremsgimpl.h
@@ -443,7 +443,7 @@ RTEMS_INLINE_ROUTINE void _CORE_message_queue_Free_message_buffer (
  *       disabled if no API requires it.
  */
 RTEMS_INLINE_ROUTINE int _CORE_message_queue_Get_message_priority (
-  CORE_message_queue_Buffer_control *the_message
+  const CORE_message_queue_Buffer_control *the_message
 )
 {
   #if defined(RTEMS_SCORE_COREMSG_ENABLE_MESSAGE_PRIORITY)
@@ -494,36 +494,6 @@ RTEMS_INLINE_ROUTINE bool _CORE_message_queue_Is_priority(
     (the_attribute->discipline == CORE_MESSAGE_QUEUE_DISCIPLINES_PRIORITY);
 }
 
-/**
- * This routine places the_message at the rear of the outstanding
- * messages on the_message_queue.
- */
-RTEMS_INLINE_ROUTINE void _CORE_message_queue_Append_unprotected (
-  CORE_message_queue_Control        *the_message_queue,
-  CORE_message_queue_Buffer_control *the_message
-)
-{
-  _Chain_Append_unprotected(
-    &the_message_queue->Pending_messages,
-    &the_message->Node
-  );
-}
-
-/**
- * This routine places the_message at the front of the outstanding
- * messages on the_message_queue.
- */
-RTEMS_INLINE_ROUTINE void _CORE_message_queue_Prepend_unprotected (
-  CORE_message_queue_Control        *the_message_queue,
-  CORE_message_queue_Buffer_control *the_message
-)
-{
-  _Chain_Prepend_unprotected(
-    &the_message_queue->Pending_messages,
-    &the_message->Node
-  );
-}
-
 #if defined(RTEMS_SCORE_COREMSG_ENABLE_NOTIFICATION)
   /**
    * This function returns true if notification is enabled on this message
diff --git a/cpukit/score/src/coremsginsert.c b/cpukit/score/src/coremsginsert.c
index 2e42349b59..28407bae98 100644
--- a/cpukit/score/src/coremsginsert.c
+++ b/cpukit/score/src/coremsginsert.c
@@ -18,12 +18,25 @@
 #include "config.h"
 #endif
 
-#include <rtems/system.h>
-#include <rtems/score/chain.h>
-#include <rtems/score/isr.h>
 #include <rtems/score/coremsgimpl.h>
-#include <rtems/score/thread.h>
-#include <rtems/score/wkspace.h>
+#include <rtems/score/isrlevel.h>
+
+#if defined(RTEMS_SCORE_COREMSG_ENABLE_MESSAGE_PRIORITY)
+static bool _CORE_message_queue_Order(
+  const Chain_Node *left,
+  const Chain_Node *right
+)
+{
+   const CORE_message_queue_Buffer_control *left_message;
+   const CORE_message_queue_Buffer_control *right_message;
+
+   left_message = (const CORE_message_queue_Buffer_control *) left;
+   right_message = (const CORE_message_queue_Buffer_control *) right;
+
+   return _CORE_message_queue_Get_message_priority( left_message ) <
+     _CORE_message_queue_Get_message_priority( right_message );
+}
+#endif
 
 void _CORE_message_queue_Insert_message(
   CORE_message_queue_Control        *the_message_queue,
@@ -31,71 +44,37 @@ void _CORE_message_queue_Insert_message(
   CORE_message_queue_Submit_types    submit_type
 )
 {
-  ISR_Level  level;
-  #if defined(RTEMS_SCORE_COREMSG_ENABLE_NOTIFICATION)
-    bool    notify = false;
-    #define SET_NOTIFY() \
-      do { \
-        if ( the_message_queue->number_of_pending_messages == 0 ) \
-          notify = true; \
-      } while (0)
-  #else
-    #define SET_NOTIFY()
-  #endif
+  Chain_Control *pending_messages;
+  ISR_Level      level;
+#if defined(RTEMS_SCORE_COREMSG_ENABLE_NOTIFICATION)
+  bool           notify;
+#endif
 
   _CORE_message_queue_Set_message_priority( the_message, submit_type );
+  pending_messages = &the_message_queue->Pending_messages;
 
-  #if !defined(RTEMS_SCORE_COREMSG_ENABLE_MESSAGE_PRIORITY)
-    _ISR_Disable( level );
-      SET_NOTIFY();
-      the_message_queue->number_of_pending_messages++;
-      if ( submit_type == CORE_MESSAGE_QUEUE_SEND_REQUEST )
-        _CORE_message_queue_Append_unprotected(the_message_queue, the_message);
-      else
-        _CORE_message_queue_Prepend_unprotected(the_message_queue, the_message);
-    _ISR_Enable( level );
-  #else
-    if ( submit_type == CORE_MESSAGE_QUEUE_SEND_REQUEST ) {
-      _ISR_Disable( level );
-        SET_NOTIFY();
-        the_message_queue->number_of_pending_messages++;
-        _CORE_message_queue_Append_unprotected(the_message_queue, the_message);
-      _ISR_Enable( level );
-    } else if ( submit_type == CORE_MESSAGE_QUEUE_URGENT_REQUEST ) {
-      _ISR_Disable( level );
-        SET_NOTIFY();
-        the_message_queue->number_of_pending_messages++;
-        _CORE_message_queue_Prepend_unprotected(the_message_queue, the_message);
-      _ISR_Enable( level );
-    } else {
-      CORE_message_queue_Buffer_control *this_message;
-      Chain_Node                        *the_node;
-      Chain_Control                     *the_header;
-      int                                the_priority;
-
-      the_priority = _CORE_message_queue_Get_message_priority(the_message);
-      the_header = &the_message_queue->Pending_messages;
-      the_node = _Chain_First( the_header );
-      while ( !_Chain_Is_tail( the_header, the_node ) ) {
-        int this_priority;
+  _ISR_Disable( level );
 
-        this_message = (CORE_message_queue_Buffer_control *) the_node;
+#if defined(RTEMS_SCORE_COREMSG_ENABLE_NOTIFICATION)
+  notify = ( the_message_queue->number_of_pending_messages == 0 );
+#endif
+  ++the_message_queue->number_of_pending_messages;
 
-        this_priority = _CORE_message_queue_Get_message_priority(this_message);
+  if ( submit_type == CORE_MESSAGE_QUEUE_SEND_REQUEST ) {
+    _Chain_Append_unprotected( pending_messages, &the_message->Node );
+#if defined(RTEMS_SCORE_COREMSG_ENABLE_MESSAGE_PRIORITY)
+  } else  if ( submit_type != CORE_MESSAGE_QUEUE_URGENT_REQUEST ) {
+    _Chain_Insert_ordered_unprotected(
+      pending_messages,
+      &the_message->Node,
+      _CORE_message_queue_Order
+    );
+#endif
+  } else {
+    _Chain_Prepend_unprotected( pending_messages, &the_message->Node );
+  }
 
-        if ( this_priority <= the_priority ) {
-          the_node = the_node->next;
-          continue;
-        }
-        break;
-      }
-      _ISR_Disable( level );
-        SET_NOTIFY();
-        the_message_queue->number_of_pending_messages++;
-        _Chain_Insert_unprotected( the_node->previous, &the_message->Node );
-      _ISR_Enable( level );
-    }
-  #endif
+  _ISR_Enable( level );
 
   #if defined(RTEMS_SCORE_COREMSG_ENABLE_NOTIFICATION)
     /*