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.

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

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/isrlevel.h>
-#include <rtems/score/wkspace.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
 )
 {
+  Chain_Control *pending_messages;
   ISR_Level      level;
-  #if defined(RTEMS_SCORE_COREMSG_ENABLE_NOTIFICATION)
+#if defined(RTEMS_SCORE_COREMSG_ENABLE_NOTIFICATION)
-    bool    notify = false;
+  bool           notify;
-    #define SET_NOTIFY() \
+#endif
-      do { \
-        if ( the_message_queue->number_of_pending_messages == 0 ) \
-          notify = true; \
-      } while (0)
-  #else
-    #define SET_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 defined(RTEMS_SCORE_COREMSG_ENABLE_NOTIFICATION)
-      if ( submit_type == CORE_MESSAGE_QUEUE_SEND_REQUEST )
+  notify = ( the_message_queue->number_of_pending_messages == 0 );
-        _CORE_message_queue_Append_unprotected(the_message_queue, the_message);
+#endif
-      else
+  ++the_message_queue->number_of_pending_messages;
-        _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 );
+    _Chain_Append_unprotected( pending_messages, &the_message->Node );
-        SET_NOTIFY();
+#if defined(RTEMS_SCORE_COREMSG_ENABLE_MESSAGE_PRIORITY)
-        the_message_queue->number_of_pending_messages++;
+  } else  if ( submit_type != CORE_MESSAGE_QUEUE_URGENT_REQUEST ) {
-        _CORE_message_queue_Append_unprotected(the_message_queue, the_message);
+    _Chain_Insert_ordered_unprotected(
-      _ISR_Enable( level );
+      pending_messages,
-    } else if ( submit_type == CORE_MESSAGE_QUEUE_URGENT_REQUEST ) {
+      &the_message->Node,
-      _ISR_Disable( level );
+      _CORE_message_queue_Order
-        SET_NOTIFY();
+    );
-        the_message_queue->number_of_pending_messages++;
+#endif
-        _CORE_message_queue_Prepend_unprotected(the_message_queue, the_message);
-      _ISR_Enable( level );
   } else {
-      CORE_message_queue_Buffer_control *this_message;
+    _Chain_Prepend_unprotected( pending_messages, &the_message->Node );
-      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;
-
-        this_message = (CORE_message_queue_Buffer_control *) the_node;
-
-        this_priority = _CORE_message_queue_Get_message_priority(this_message);
-
-        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
 
   #if defined(RTEMS_SCORE_COREMSG_ENABLE_NOTIFICATION)
     /*