score: Add _SMP_Before_multitasking_action()

The use case for this is the Cortex-A9 MPCore which has per-processor
registers (only accessible by a particular processor) for the global
timer used by the clock driver.  This might be useful for other drivers
as well.

Update #2554.

5 files changed, 164 insertions, 11 deletions

diff --git a/cpukit/score/include/rtems/score/percpu.h b/cpukit/score/include/rtems/score/percpu.h
index 19f46d2725..39be1e3367 100644
--- a/cpukit/score/include/rtems/score/percpu.h
+++ b/cpukit/score/include/rtems/score/percpu.h
@@ -361,6 +361,14 @@ typedef struct Per_CPU_Control {
     Per_CPU_State state;
 
     /**
+     * @brief Action to be executed by this processor in the
+     * SYSTEM_STATE_BEFORE_MULTITASKING state on behalf of the boot processor.
+     *
+     * @see _SMP_Before_multitasking_action().
+     */
+    Atomic_Uintptr before_multitasking_action;
+
+    /**
      * @brief Indicates if the processor has been successfully started via
      * _CPU_SMP_Start_processor().
      */
diff --git a/cpukit/score/include/rtems/score/smpimpl.h b/cpukit/score/include/rtems/score/smpimpl.h
index 386216fdfc..59a99ec6b3 100644
--- a/cpukit/score/include/rtems/score/smpimpl.h
+++ b/cpukit/score/include/rtems/score/smpimpl.h
@@ -235,7 +235,7 @@ void _SMP_Send_message_multicast(
   unsigned long message
 );
 
-typedef void ( *SMP_Multicast_action_handler )( void *arg );
+typedef void ( *SMP_Action_handler )( void *arg );
 
 /**
  *  @brief Initiates a SMP multicast action to a set of processors.
@@ -250,10 +250,54 @@ typedef void ( *SMP_Multicast_action_handler )( void *arg );
 void _SMP_Multicast_action(
   const size_t setsize,
   const cpu_set_t *cpus,
-  SMP_Multicast_action_handler handler,
+  SMP_Action_handler handler,
   void *arg
 );
 
+/**
+ * @brief Executes a handler with argument on the specified processor on behalf
+ * of the boot processor.
+ *
+ * The calling processor must be the boot processor.  In case the specified
+ * processor is not online or not in the
+ * PER_CPU_STATE_READY_TO_START_MULTITASKING state, then no action is
+ * performed.
+ *
+ * @param cpu The processor to execute the action.
+ * @param handler The handler of the action.
+ * @param arg The argument of the action.
+ *
+ * @retval true The handler executed on the specified processor.
+ * @retval false Otherwise.
+ *
+ * @see _SMP_Before_multitasking_action_broadcast().
+ */
+bool _SMP_Before_multitasking_action(
+  Per_CPU_Control    *cpu,
+  SMP_Action_handler  handler,
+  void               *arg
+);
+
+/**
+ * @brief Executes a handler with argument on all online processors except the
+ * boot processor on behalf of the boot processor.
+ *
+ * The calling processor must be the boot processor.
+ *
+ * @param handler The handler of the action.
+ * @param arg The argument of the action.
+ *
+ * @retval true The handler executed on all online processors except the boot
+ * processor.
+ * @retval false Otherwise.
+ *
+ * @see _SMP_Before_multitasking_action().
+ */
+bool _SMP_Before_multitasking_action_broadcast(
+  SMP_Action_handler  handler,
+  void               *arg
+);
+
 #endif /* defined( RTEMS_SMP ) */
 
 /**
diff --git a/cpukit/score/src/percpu.c b/cpukit/score/src/percpu.c
index 730528ae4c..0e4c0678e7 100644
--- a/cpukit/score/src/percpu.c
+++ b/cpukit/score/src/percpu.c
@@ -20,6 +20,7 @@
 
 #include <rtems/score/percpu.h>
 #include <rtems/score/assert.h>
+#include <rtems/score/isrlock.h>
 #include <rtems/score/smpimpl.h>
 #include <rtems/config.h>
 
@@ -35,11 +36,48 @@ RTEMS_STATIC_ASSERT(
 
 #if defined(RTEMS_SMP)
 
-static SMP_lock_Control _Per_CPU_State_lock =
-  SMP_LOCK_INITIALIZER("per-CPU state");
+typedef struct {
+  SMP_Action_handler handler;
+  void *arg;
+} SMP_Before_multicast_action;
+
+ISR_LOCK_DEFINE( static, _Per_CPU_State_lock, "Per-CPU State" )
+
+static void _Per_CPU_State_acquire( ISR_lock_Context *lock_context )
+{
+  _ISR_lock_ISR_disable_and_acquire( &_Per_CPU_State_lock, lock_context );
+}
+
+static void _Per_CPU_State_release( ISR_lock_Context *lock_context )
+{
+  _ISR_lock_Release_and_ISR_enable( &_Per_CPU_State_lock, lock_context );
+}
+
+static void _Per_CPU_State_before_multitasking_action( Per_CPU_Control *cpu )
+{
+  uintptr_t action_value;
+
+  action_value = _Atomic_Load_uintptr(
+    &cpu->before_multitasking_action,
+    ATOMIC_ORDER_ACQUIRE
+  );
+
+  if ( action_value != 0 ) {
+    SMP_Before_multicast_action *action =
+      (SMP_Before_multicast_action *) action_value;
+
+    ( *action->handler )( action->arg );
+
+    _Atomic_Store_uintptr(
+      &cpu->before_multitasking_action,
+      0,
+      ATOMIC_ORDER_RELEASE
+    );
+  }
+}
 
 static void _Per_CPU_State_busy_wait(
-  const Per_CPU_Control *cpu,
+  Per_CPU_Control *cpu,
   Per_CPU_State new_state
 )
 {
@@ -60,6 +98,7 @@ static void _Per_CPU_State_busy_wait(
         state != PER_CPU_STATE_REQUEST_START_MULTITASKING
           && state != PER_CPU_STATE_SHUTDOWN
       ) {
+        _Per_CPU_State_before_multitasking_action( cpu );
         _CPU_SMP_Processor_event_receive();
         state = cpu->state;
       }
@@ -122,13 +161,12 @@ void _Per_CPU_State_change(
   Per_CPU_State new_state
 )
 {
-  SMP_lock_Control *lock = &_Per_CPU_State_lock;
-  SMP_lock_Context lock_context;
+  ISR_lock_Context lock_context;
   Per_CPU_State next_state;
 
   _Per_CPU_State_busy_wait( cpu, new_state );
 
-  _SMP_lock_ISR_disable_and_acquire( lock, &lock_context );
+  _Per_CPU_State_acquire( &lock_context );
 
   next_state = _Per_CPU_State_get_next( cpu->state, new_state );
   cpu->state = next_state;
@@ -157,7 +195,7 @@ void _Per_CPU_State_change(
 
   _CPU_SMP_Processor_event_broadcast();
 
-  _SMP_lock_Release_and_ISR_enable( lock, &lock_context );
+  _Per_CPU_State_release( &lock_context );
 
   if (
     next_state == PER_CPU_STATE_SHUTDOWN
@@ -167,6 +205,46 @@ void _Per_CPU_State_change(
   }
 }
 
+bool _SMP_Before_multitasking_action(
+  Per_CPU_Control    *cpu,
+  SMP_Action_handler  handler,
+  void               *arg
+)
+{
+  bool done;
+
+  _Assert( _Per_CPU_Is_boot_processor( _Per_CPU_Get() ) );
+
+  if ( _Per_CPU_Is_processor_online( cpu ) ) {
+    SMP_Before_multicast_action action = {
+      .handler = handler,
+      .arg = arg
+    };
+    Per_CPU_State expected_state = PER_CPU_STATE_READY_TO_START_MULTITASKING;
+
+    _Atomic_Store_uintptr(
+      &cpu->before_multitasking_action,
+      (uintptr_t) &action,
+      ATOMIC_ORDER_RELEASE
+    );
+
+    _CPU_SMP_Processor_event_broadcast();
+
+    _Per_CPU_State_busy_wait( cpu, expected_state );
+
+    do {
+      done = _Atomic_Load_uintptr(
+        &cpu->before_multitasking_action,
+        ATOMIC_ORDER_ACQUIRE
+      ) == 0;
+    } while ( !done && cpu->state == expected_state );
+  } else {
+    done = false;
+  }
+
+  return done;
+}
+
 #else
   /*
    * On single core systems, we can efficiently directly access a single
diff --git a/cpukit/score/src/smp.c b/cpukit/score/src/smp.c
index 8049643a95..4dacd4ed5c 100644
--- a/cpukit/score/src/smp.c
+++ b/cpukit/score/src/smp.c
@@ -211,4 +211,27 @@ void _SMP_Send_message_multicast(
   }
 }
 
+bool _SMP_Before_multitasking_action_broadcast(
+  SMP_Action_handler  handler,
+  void               *arg
+)
+{
+  bool done = true;
+  uint32_t cpu_count = _SMP_Get_processor_count();
+  uint32_t cpu_index;
+
+  for ( cpu_index = 0 ; done && cpu_index < cpu_count ; ++cpu_index ) {
+    Per_CPU_Control *cpu = _Per_CPU_Get_by_index( cpu_index );
+
+    if (
+      !_Per_CPU_Is_boot_processor( cpu )
+        && _Per_CPU_Is_processor_online( cpu )
+    ) {
+      done = _SMP_Before_multitasking_action( cpu, handler, arg );
+    }
+  }
+
+  return done;
+}
+
 SMP_Test_message_handler _SMP_Test_message_handler;
diff --git a/cpukit/score/src/smpmulticastaction.c b/cpukit/score/src/smpmulticastaction.c
index 2e592627d7..d5d00641e8 100644
--- a/cpukit/score/src/smpmulticastaction.c
+++ b/cpukit/score/src/smpmulticastaction.c
@@ -17,7 +17,7 @@
 
 typedef struct {
   Chain_Node Node;
-  SMP_Multicast_action_handler handler;
+  SMP_Action_handler handler;
   void *arg;
   cpu_set_t *recipients;
   size_t setsize;
@@ -94,7 +94,7 @@ _SMP_Multicast_actions_try_process( void )
 void _SMP_Multicast_action(
   const size_t setsize,
   const cpu_set_t *cpus,
-  SMP_Multicast_action_handler handler,
+  SMP_Action_handler handler,
   void *arg
 )
 {