1 files changed, 39 insertions, 27 deletions

diff --git a/cpukit/score/src/mpci.c b/cpukit/score/src/mpci.c
index 451592c8d5..533050085e 100644
--- a/cpukit/score/src/mpci.c
+++ b/cpukit/score/src/mpci.c
@@ -226,47 +226,59 @@ void _MPCI_Send_process_packet (
   (*_MPCI_table->send_packet)( destination, the_packet );
 }
 
+static void _MPCI_Enqueue_callout(
+  Thread_queue_Queue   *queue,
+  Thread_Control       *the_thread,
+  Thread_queue_Context *queue_context
+)
+{
+  _Thread_Dispatch_unnest( _Per_CPU_Get() );
+}
+
 Status_Control _MPCI_Send_request_packet(
   uint32_t          destination,
   MP_packet_Prefix *the_packet,
   States_Control    extra_state
 )
 {
-  Per_CPU_Control *cpu_self;
-  Thread_Control  *executing;
-
-  cpu_self = _Thread_Dispatch_disable();
-
-    executing = _Per_CPU_Get_executing( cpu_self );
+  Per_CPU_Control      *cpu_self;
+  Thread_queue_Context  queue_context;
+  Thread_Control       *executing;
 
-    the_packet->source_tid      = executing->Object.id;
-    the_packet->source_priority = _Thread_Get_priority( executing );
-    the_packet->to_convert =
-       ( the_packet->to_convert - sizeof(MP_packet_Prefix) ) / sizeof(uint32_t);
+  /*
+   *  See if we need a default timeout
+   */
 
-    executing->Wait.remote_id = the_packet->id;
+  if (the_packet->timeout == MPCI_DEFAULT_TIMEOUT)
+      the_packet->timeout = _MPCI_table->default_timeout;
 
-    (*_MPCI_table->send_packet)( destination, the_packet );
+  _Thread_queue_Context_initialize( &queue_context );
+  _Thread_queue_Context_set_enqueue_callout(
+    &queue_context,
+    _MPCI_Enqueue_callout
+  );
+  _Thread_queue_Context_set_relative_timeout( &queue_context, the_packet->timeout );
 
-    /*
-     *  See if we need a default timeout
-     */
+  cpu_self = _Thread_Dispatch_disable();
 
-    if (the_packet->timeout == MPCI_DEFAULT_TIMEOUT)
-        the_packet->timeout = _MPCI_table->default_timeout;
+  executing = _Per_CPU_Get_executing( cpu_self );
+  executing->Wait.remote_id = the_packet->id;
 
-    _Thread_queue_Enqueue(
-      &_MPCI_Remote_blocked_threads,
-      &_Thread_queue_Operations_FIFO,
-      executing,
-      STATES_WAITING_FOR_RPC_REPLY | extra_state,
-      the_packet->timeout,
-      WATCHDOG_RELATIVE,
-      2
-    );
+  the_packet->source_tid      = executing->Object.id;
+  the_packet->source_priority = _Thread_Get_priority( executing );
+  the_packet->to_convert =
+     ( the_packet->to_convert - sizeof(MP_packet_Prefix) ) / sizeof(uint32_t);
 
-  _Thread_Dispatch_enable( cpu_self );
+  (*_MPCI_table->send_packet)( destination, the_packet );
 
+  _Thread_queue_Acquire( &_MPCI_Remote_blocked_threads, &queue_context );
+  _Thread_queue_Enqueue_critical(
+    &_MPCI_Remote_blocked_threads.Queue,
+    &_Thread_queue_Operations_FIFO,
+    executing,
+    STATES_WAITING_FOR_RPC_REPLY | extra_state,
+    &queue_context
+  );
   return _Thread_Wait_get_status( executing );
 }