[PATCH 4/8] mpci: Fix thread proxies

[Sebastian Huber]

---
 cpukit/sapi/include/confdefs.h    |  4 +++-
 cpukit/score/src/threadmp.c       | 40 +++++++++++++++++++++++++++++++++------
 cpukit/score/src/threadqenqueue.c | 11 ++++++-----
 3 files changed, 43 insertions(+), 12 deletions(-)

diff --git a/cpukit/sapi/include/confdefs.h b/cpukit/sapi/include/confdefs.h
index 2366c7a..6d67a02 100644
--- a/cpukit/sapi/include/confdefs.h
+++ b/cpukit/sapi/include/confdefs.h
@@ -1899,7 +1899,9 @@ extern rtems_initialization_tasks_table Initialization_tasks[];
         #define CONFIGURE_MP_MAXIMUM_PROXIES            32
       #endif
       #define CONFIGURE_MEMORY_FOR_PROXIES(_proxies) \
-        _Configure_Object_RAM((_proxies) + 1, sizeof(Thread_Proxy_control) )
+        (_Configure_Object_RAM((_proxies) + 1, sizeof(Thread_Proxy_control)) \
+          + _Configure_From_workspace((_proxies) \
+            * THREAD_QUEUE_HEADS_SIZE(CONFIGURE_SCHEDULER_COUNT)))
 
       #ifndef CONFIGURE_MP_MPCI_TABLE_POINTER
         #include <mpci.h>
diff --git a/cpukit/score/src/threadmp.c b/cpukit/score/src/threadmp.c
index a084624..993271b 100644
--- a/cpukit/score/src/threadmp.c
+++ b/cpukit/score/src/threadmp.c
@@ -30,6 +30,10 @@ void _Thread_MP_Handler_initialization (
   uint32_t    maximum_proxies
 )
 {
+  Per_CPU_Control      *cpu = _Per_CPU_Get_by_index( 0 );
+  Thread_Proxy_control *proxies;
+  char                 *thread_queue_heads;
+  uint32_t              i;
 
   _Chain_Initialize_empty( &_Thread_MP_Active_proxies );
 
@@ -38,16 +42,32 @@ void _Thread_MP_Handler_initialization (
     return;
   }
 
+  proxies = _Workspace_Allocate_or_fatal_error(
+    maximum_proxies * sizeof( *proxies )
+  );
+
+  thread_queue_heads = _Workspace_Allocate_or_fatal_error(
+    maximum_proxies * THREAD_QUEUE_HEADS_SIZE( _Scheduler_Count )
+  );
 
   _Chain_Initialize(
     &_Thread_MP_Inactive_proxies,
-    _Workspace_Allocate_or_fatal_error(
-      maximum_proxies * sizeof( Thread_Proxy_control )
-    ),
+    proxies,
     maximum_proxies,
-    sizeof( Thread_Proxy_control )
+    sizeof( *proxies )
   );
 
+  for ( i = 0 ; i < maximum_proxies ; ++i ) {
+    Thread_Control *the_thread = (Thread_Control *) &proxies[ i ];
+
+    _ISR_lock_Initialize( &the_thread->Timer.Lock, "Thread Timer" );
+    the_thread->Timer.header = &cpu->Watchdog.Header[ PER_CPU_WATCHDOG_RELATIVE ];
+    _Watchdog_Preinitialize( &the_thread->Timer.Watchdog, cpu );
+
+    the_thread->Wait.spare_heads =
+      thread_queue_heads + i * THREAD_QUEUE_HEADS_SIZE( _Scheduler_Count );
+    _Thread_queue_Heads_initialize( the_thread->Wait.spare_heads );
+  }
 }
 
 Thread_Control *_Thread_MP_Allocate_proxy (
@@ -60,10 +80,12 @@ Thread_Control *_Thread_MP_Allocate_proxy (
   the_thread = (Thread_Control *)_Chain_Get( &_Thread_MP_Inactive_proxies );
 
   if ( !_Thread_Is_null( the_thread ) ) {
+    Thread_Control *executing;
 
+    executing = _Thread_Executing;
     the_proxy = (Thread_Proxy_control *) the_thread;
 
-    _Thread_Executing->Wait.return_code = THREAD_STATUS_PROXY_BLOCKING;
+    executing->Wait.return_code = THREAD_STATUS_PROXY_BLOCKING;
 
     the_proxy->receive_packet = _MPCI_Receive_server_tcb->receive_packet;
 
@@ -74,7 +96,13 @@ Thread_Control *_Thread_MP_Allocate_proxy (
 
     the_proxy->current_state = _States_Set( STATES_DORMANT, the_state );
 
-    the_proxy->Wait = _Thread_Executing->Wait;
+    the_proxy->Wait.id                      = executing->Wait.id;
+    the_proxy->Wait.count                   = executing->Wait.count;
+    the_proxy->Wait.return_argument         = executing->Wait.return_argument;
+    the_proxy->Wait.return_argument_second  = executing->Wait.return_argument_second;
+    the_proxy->Wait.option                  = executing->Wait.option;
+    the_proxy->Wait.return_code             = executing->Wait.return_code;
+    the_proxy->Wait.timeout_code            = executing->Wait.timeout_code;
 
     _Chain_Append( &_Thread_MP_Active_proxies, &the_proxy->Active );
 
diff --git a/cpukit/score/src/threadqenqueue.c b/cpukit/score/src/threadqenqueue.c
index e775135..803b556 100644
--- a/cpukit/score/src/threadqenqueue.c
+++ b/cpukit/score/src/threadqenqueue.c
@@ -58,6 +58,12 @@ void _Thread_queue_Enqueue_critical(
   Per_CPU_Control *cpu_self;
   bool             success;
 
+#if defined(RTEMS_MULTIPROCESSING)
+  if ( _Thread_MP_Is_receive( the_thread ) && the_thread->receive_packet ) {
+    the_thread = _Thread_MP_Allocate_proxy( state );
+  }
+#endif
+
   _Thread_Lock_set( the_thread, &queue->Lock );
 
   _Thread_Wait_set_queue( the_thread, queue );
@@ -69,11 +75,6 @@ void _Thread_queue_Enqueue_critical(
   cpu_self = _Thread_Dispatch_disable_critical( lock_context );
   _Thread_queue_Queue_release( queue, lock_context );
 
-#if defined(RTEMS_MULTIPROCESSING)
-  if ( _Thread_MP_Is_receive( the_thread ) && the_thread->receive_packet )
-    the_thread = _Thread_MP_Allocate_proxy( state );
-  else
-#endif
   /*
    *  Set the blocking state for this thread queue in the thread.
    */
-- 
1.8.4.5