score: Improve _SMP_Multicast_action()

Let it work during system initialization.

5 files changed, 151 insertions, 81 deletions

diff --git a/cpukit/include/rtems/score/smpimpl.h b/cpukit/include/rtems/score/smpimpl.h
index d6fdef5ca1..9d76eb5a23 100644
--- a/cpukit/include/rtems/score/smpimpl.h
+++ b/cpukit/include/rtems/score/smpimpl.h
@@ -78,7 +78,8 @@ typedef enum {
   SMP_FATAL_SHUTDOWN,
   SMP_FATAL_SHUTDOWN_RESPONSE,
   SMP_FATAL_START_OF_MANDATORY_PROCESSOR_FAILED,
-  SMP_FATAL_SCHEDULER_PIN_OR_UNPIN_NOT_SUPPORTED
+  SMP_FATAL_SCHEDULER_PIN_OR_UNPIN_NOT_SUPPORTED,
+  SMP_FATAL_WRONG_CPU_STATE_TO_PERFORM_JOBS
 } SMP_Fatal_code;
 
 static inline void _SMP_Fatal( SMP_Fatal_code code )
diff --git a/cpukit/score/src/percpu.c b/cpukit/score/src/percpu.c
index 0e4c0678e7..79e940005c 100644
--- a/cpukit/score/src/percpu.c
+++ b/cpukit/score/src/percpu.c
@@ -89,6 +89,7 @@ static void _Per_CPU_State_busy_wait(
         state != PER_CPU_STATE_READY_TO_START_MULTITASKING
           && state != PER_CPU_STATE_SHUTDOWN
       ) {
+        _Per_CPU_Perform_jobs( cpu );
         _CPU_SMP_Processor_event_receive();
         state = cpu->state;
       }
@@ -99,6 +100,7 @@ static void _Per_CPU_State_busy_wait(
           && state != PER_CPU_STATE_SHUTDOWN
       ) {
         _Per_CPU_State_before_multitasking_action( cpu );
+        _Per_CPU_Perform_jobs( cpu );
         _CPU_SMP_Processor_event_receive();
         state = cpu->state;
       }
diff --git a/cpukit/score/src/smpmulticastaction.c b/cpukit/score/src/smpmulticastaction.c
index a1df0269e1..966a119217 100644
--- a/cpukit/score/src/smpmulticastaction.c
+++ b/cpukit/score/src/smpmulticastaction.c
@@ -32,7 +32,6 @@
 #include <rtems/score/smpimpl.h>
 #include <rtems/score/assert.h>
 #include <rtems/score/threaddispatch.h>
-#include <rtems/score/sysstate.h>
 
 typedef struct Per_CPU_Job Per_CPU_Job;
 
@@ -181,11 +180,30 @@ static void _SMP_Wait_for_action_jobs(
   for ( cpu_index = 0; cpu_index < cpu_max; ++cpu_index ) {
     if ( _Processor_mask_Is_set( targets, cpu_index ) ) {
       const Per_CPU_Job *job;
+      Per_CPU_Control   *cpu;
 
       job = &jobs->Jobs[ cpu_index ];
+      cpu = _Per_CPU_Get_by_index( cpu_index );
 
       while ( _Atomic_Load_ulong( &job->done, ATOMIC_ORDER_ACQUIRE ) == 0 ) {
-        _Per_CPU_Try_perform_jobs( cpu_self );
+        switch ( cpu->state ) {
+          case PER_CPU_STATE_INITIAL:
+          case PER_CPU_STATE_READY_TO_START_MULTITASKING:
+          case PER_CPU_STATE_REQUEST_START_MULTITASKING:
+            _CPU_SMP_Processor_event_broadcast();
+            /* Fall through */
+          case PER_CPU_STATE_UP:
+            /*
+             * Calling this function with the current processor is intentional.
+             * We have to perform our own jobs here in case inter-processor
+             * interrupts are not working.
+             */
+            _Per_CPU_Try_perform_jobs( cpu_self );
+            break;
+          default:
+            _SMP_Fatal( SMP_FATAL_WRONG_CPU_STATE_TO_PERFORM_JOBS );
+            break;
+        }
       }
     }
   }
@@ -204,11 +222,6 @@ void _SMP_Multicast_action(
   cpu_max = _SMP_Get_processor_maximum();
   _Assert( cpu_max <= CPU_MAXIMUM_PROCESSORS );
 
-  if ( ! _System_state_Is_up( _System_state_Get() ) ) {
-    ( *handler )( arg );
-    return;
-  }
-
   if ( targets == NULL ) {
     targets = _SMP_Get_online_processors();
   }
diff --git a/testsuites/smptests/smpmulticast01/init.c b/testsuites/smptests/smpmulticast01/init.c
index e599a78bde..3f3f7dc3fc 100644
--- a/testsuites/smptests/smpmulticast01/init.c
+++ b/testsuites/smptests/smpmulticast01/init.c
@@ -104,18 +104,15 @@ static void action(void *arg)
 
 static void test_unicast(
   test_context *ctx,
-  void (*multicast_action)(const Processor_mask *, SMP_Action_handler, void *),
-  bool before_multitasking
+  void (*multicast_action)(const Processor_mask *, SMP_Action_handler, void *)
 )
 {
   uint32_t step;
   uint32_t i;
   uint32_t n;
-  uint32_t self;
 
   T_plan(1);
   step = 0;
-  self = rtems_scheduler_get_processor();
   n = rtems_scheduler_get_processor_maximum();
 
   for (i = 0; i < n; ++i) {
@@ -134,18 +131,10 @@ static void test_unicast(
       ++step;
       id = _Atomic_Load_uint(&ctx->id[j], ATOMIC_ORDER_RELAXED);
 
-      if (before_multitasking) {
-        if (j == self) {
-          T_quiet_eq_uint(j + 1, id);
-        } else {
-          T_quiet_eq_uint(0, id);
-        }
+      if (j == i) {
+        T_quiet_eq_uint(j + 1, id);
       } else {
-        if (j == i) {
-          T_quiet_eq_uint(j + 1, id);
-        } else {
-          T_quiet_eq_uint(0, id);
-        }
+        T_quiet_eq_uint(0, id);
       }
     }
   }
@@ -155,18 +144,15 @@ static void test_unicast(
 
 static void test_broadcast(
   test_context *ctx,
-  void (*multicast_action)(const Processor_mask *, SMP_Action_handler, void *),
-  bool before_multitasking
+  void (*multicast_action)(const Processor_mask *, SMP_Action_handler, void *)
 )
 {
   uint32_t step;
   uint32_t i;
   uint32_t n;
-  uint32_t self;
 
   T_plan(1);
   step = 0;
-  self = rtems_scheduler_get_processor();
   n = rtems_scheduler_get_processor_maximum();
 
   for (i = 0; i < n; ++i) {
@@ -181,16 +167,7 @@ static void test_broadcast(
 
       ++step;
       id = _Atomic_Load_uint(&ctx->id[j], ATOMIC_ORDER_RELAXED);
-
-      if (before_multitasking) {
-        if (j == self) {
-          T_quiet_eq_uint(j + 1, id);
-        } else {
-          T_quiet_eq_uint(0, id);
-        }
-      } else {
-        T_quiet_eq_uint(j + 1, id);
-      }
+      T_quiet_eq_uint(j + 1, id);
     }
   }
 
@@ -204,27 +181,27 @@ static void test_before_multitasking(void)
   ctx = &test_instance;
 
   T_case_begin("UnicastBeforeMultitasking", NULL);
-  test_unicast(ctx, _SMP_Multicast_action, true);
+  test_unicast(ctx, _SMP_Multicast_action);
   T_case_end();
 
   T_case_begin("UnicastBeforeMultitaskingIRQDisabled", NULL);
-  test_unicast(ctx, multicast_action_irq_disabled, true);
+  test_unicast(ctx, multicast_action_irq_disabled);
   T_case_end();
 
   T_case_begin("UnicastBeforeMultitaskingDispatchDisabled", NULL);
-  test_unicast(ctx, multicast_action_dispatch_disabled, true);
+  test_unicast(ctx, multicast_action_dispatch_disabled);
   T_case_end();
 
   T_case_begin("BroadcastBeforeMultitasking", NULL);
-  test_broadcast(ctx, _SMP_Multicast_action, true);
+  test_broadcast(ctx, _SMP_Multicast_action);
   T_case_end();
 
   T_case_begin("BroadcastBeforeMultitaskingIRQDisabled", NULL);
-  test_broadcast(ctx, multicast_action_irq_disabled, true);
+  test_broadcast(ctx, multicast_action_irq_disabled);
   T_case_end();
 
   T_case_begin("BroadcastBeforeMultitaskingDispatchDisabled", NULL);
-  test_broadcast(ctx, multicast_action_dispatch_disabled, true);
+  test_broadcast(ctx, multicast_action_dispatch_disabled);
   T_case_end();
 }
 
@@ -241,41 +218,108 @@ RTEMS_SYSINIT_ITEM(
   RTEMS_SYSINIT_ORDER_LAST
 );
 
+static void set_wrong_cpu_state(void *arg)
+{
+  Per_CPU_Control *cpu_self;
+
+  cpu_self = arg;
+  T_step_eq_ptr(0, cpu_self, _Per_CPU_Get());
+  cpu_self->state = 123;
+
+  while (true) {
+    /* Do nothing */
+  }
+}
+
+static void test_wrong_cpu_state_to_perform_jobs(void)
+{
+  Per_CPU_Control *cpu_self;
+  rtems_interrupt_level level;
+  Processor_mask targets;
+  uint32_t cpu_index;
+
+  T_case_begin("WrongCPUStateToPerformJobs", NULL);
+  T_plan(4);
+  cpu_self = _Thread_Dispatch_disable();
+
+  cpu_index = _Per_CPU_Get_index(cpu_self);
+  cpu_index = (cpu_index + 1) % rtems_scheduler_get_processor_maximum();
+  _Processor_mask_Zero(&targets);
+  _Processor_mask_Set(&targets, cpu_index);
+
+  rtems_interrupt_local_disable(level);
+
+  _SMP_Multicast_action(
+    &targets,
+    set_wrong_cpu_state,
+    _Per_CPU_Get_by_index(cpu_index)
+  );
+
+  /* If everything is all right, we don't end up here */
+  rtems_interrupt_local_enable(level);
+  _Thread_Dispatch_enable(cpu_self);
+  rtems_fatal(RTEMS_FATAL_SOURCE_APPLICATION, 0);
+}
+
 static void Init(rtems_task_argument arg)
 {
   test_context *ctx;
-  bool ok;
 
   ctx = &test_instance;
 
   T_case_begin("UnicastDuringMultitasking", NULL);
-  test_unicast(ctx, _SMP_Multicast_action, false);
+  test_unicast(ctx, _SMP_Multicast_action);
   T_case_end();
 
   T_case_begin("UnicastDuringMultitaskingIRQDisabled", NULL);
-  test_unicast(ctx, multicast_action_irq_disabled, false);
+  test_unicast(ctx, multicast_action_irq_disabled);
   T_case_end();
 
   T_case_begin("UnicastDuringMultitaskingDispatchDisabled", NULL);
-  test_unicast(ctx, multicast_action_dispatch_disabled, false);
+  test_unicast(ctx, multicast_action_dispatch_disabled);
   T_case_end();
 
   T_case_begin("BroadcastDuringMultitasking", NULL);
-  test_broadcast(ctx, _SMP_Multicast_action, false);
+  test_broadcast(ctx, _SMP_Multicast_action);
   T_case_end();
 
   T_case_begin("BroadcastDuringMultitaskingIRQDisabled", NULL);
-  test_broadcast(ctx, multicast_action_irq_disabled, false);
+  test_broadcast(ctx, multicast_action_irq_disabled);
   T_case_end();
 
   T_case_begin("BroadcastDuringMultitaskingDispatchDisabled", NULL);
-  test_broadcast(ctx, multicast_action_dispatch_disabled, false);
+  test_broadcast(ctx, multicast_action_dispatch_disabled);
   T_case_end();
 
-  ok = T_run_finalize();
-  rtems_test_assert(ok);
-  TEST_END();
-  rtems_test_exit(0);
+  if (rtems_scheduler_get_processor_maximum() > 1) {
+    test_wrong_cpu_state_to_perform_jobs();
+  } else {
+    rtems_fatal(RTEMS_FATAL_SOURCE_APPLICATION, 0);
+  }
+}
+
+static void fatal_extension(
+  rtems_fatal_source source,
+  bool always_set_to_false,
+  rtems_fatal_code code
+)
+{
+  bool ok;
+
+  if (source == RTEMS_FATAL_SOURCE_SMP) {
+    T_step_eq_int(1, source, RTEMS_FATAL_SOURCE_SMP);
+    T_step_false(2, always_set_to_false, "unexpected argument value");
+    T_step_eq_int(3, code, SMP_FATAL_WRONG_CPU_STATE_TO_PERFORM_JOBS);
+    T_case_end();
+
+    ok = T_run_finalize();
+    rtems_test_assert(ok);
+    TEST_END();
+  } else if (source == RTEMS_FATAL_SOURCE_APPLICATION) {
+    ok = T_run_finalize();
+    rtems_test_assert(ok);
+    TEST_END();
+  }
 }
 
 #define CONFIGURE_APPLICATION_NEEDS_CLOCK_DRIVER
@@ -284,6 +328,10 @@ static void Init(rtems_task_argument arg)
 
 #define CONFIGURE_MAXIMUM_PROCESSORS CPU_COUNT
 
+#define CONFIGURE_INITIAL_EXTENSIONS \
+  { .fatal = fatal_extension }, \
+  RTEMS_TEST_INITIAL_EXTENSION
+
 #define CONFIGURE_RTEMS_INIT_TASKS_TABLE
 
 #define CONFIGURE_INIT
diff --git a/testsuites/smptests/smpmulticast01/smpmulticast01.scn b/testsuites/smptests/smpmulticast01/smpmulticast01.scn
index ea532fdead..9433775dba 100644
--- a/testsuites/smptests/smpmulticast01/smpmulticast01.scn
+++ b/testsuites/smptests/smpmulticast01/smpmulticast01.scn
@@ -1,12 +1,12 @@
 *** BEGIN OF TEST SMPMULTICAST 1 ***
-*** TEST VERSION: 5.0.0.c44199ccea624f31b1116fa2e47d547944e90909-modified
+*** TEST VERSION: 5.0.0.d9c9d1af7a885bc402c57e88919635b27b363111
 *** TEST STATE: EXPECTED-PASS
 *** TEST BUILD: RTEMS_POSIX_API RTEMS_SMP
 *** TEST TOOLS: 7.4.0 20181206 (RTEMS 5, RSB e0aec65182449a4e22b820e773087636edaf5b32, Newlib 1d35a003f)
 A:SMPMultiCast
 S:Platform:RTEMS
 S:Compiler:7.4.0 20181206 (RTEMS 5, RSB e0aec65182449a4e22b820e773087636edaf5b32, Newlib 1d35a003f)
-S:Version:5.0.0.c44199ccea624f31b1116fa2e47d547944e90909
+S:Version:5.0.0.d9c9d1af7a885bc402c57e88919635b27b363111
 S:BSP:qoriq_e6500_32
 S:RTEMS_DEBUG:0
 S:RTEMS_MULTIPROCESSING:0
@@ -14,41 +14,47 @@ S:RTEMS_POSIX_API:1
 S:RTEMS_PROFILING:0
 S:RTEMS_SMP:1
 B:UnicastBeforeMultitasking
-P:0:0:IDLE:init.c:150
-E:UnicastBeforeMultitasking:N:1:F:0:D:0.001999
+P:0:0:IDLE:init.c:142
+E:UnicastBeforeMultitasking:N:1:F:0:D:0.002206
 B:UnicastBeforeMultitaskingIRQDisabled
-P:0:0:IDLE:init.c:150
-E:UnicastBeforeMultitaskingIRQDisabled:N:1:F:0:D:0.001999
+P:0:0:IDLE:init.c:142
+E:UnicastBeforeMultitaskingIRQDisabled:N:1:F:0:D:0.002184
 B:UnicastBeforeMultitaskingDispatchDisabled
-P:0:0:IDLE:init.c:150
-E:UnicastBeforeMultitaskingDispatchDisabled:N:1:F:0:D:0.001999
+P:0:0:IDLE:init.c:142
+E:UnicastBeforeMultitaskingDispatchDisabled:N:1:F:0:D:0.002198
 B:BroadcastBeforeMultitasking
-P:0:0:IDLE:init.c:194
-E:BroadcastBeforeMultitasking:N:1:F:0:D:0.001999
+P:0:0:IDLE:init.c:174
+E:BroadcastBeforeMultitasking:N:1:F:0:D:0.004153
 B:BroadcastBeforeMultitaskingIRQDisabled
-P:0:0:IDLE:init.c:194
-E:BroadcastBeforeMultitaskingIRQDisabled:N:1:F:0:D:0.001999
+P:0:0:IDLE:init.c:174
+E:BroadcastBeforeMultitaskingIRQDisabled:N:1:F:0:D:0.004135
 B:BroadcastBeforeMultitaskingDispatchDisabled
-P:0:0:IDLE:init.c:194
-E:BroadcastBeforeMultitaskingDispatchDisabled:N:1:F:0:D:0.001999
+P:0:0:IDLE:init.c:174
+E:BroadcastBeforeMultitaskingDispatchDisabled:N:1:F:0:D:0.004123
 B:UnicastDuringMultitasking
-P:0:23:UI1:init.c:150
-E:UnicastDuringMultitasking:N:1:F:0:D:0.001999
+P:0:23:UI1:init.c:142
+E:UnicastDuringMultitasking:N:1:F:0:D:0.002270
 B:UnicastDuringMultitaskingIRQDisabled
-P:0:23:UI1:init.c:150
-E:UnicastDuringMultitaskingIRQDisabled:N:1:F:0:D:0.002000
+P:0:23:UI1:init.c:142
+E:UnicastDuringMultitaskingIRQDisabled:N:1:F:0:D:0.002272
 B:UnicastDuringMultitaskingDispatchDisabled
-P:0:23:UI1:init.c:150
-E:UnicastDuringMultitaskingDispatchDisabled:N:1:F:0:D:0.002000
+P:0:23:UI1:init.c:142
+E:UnicastDuringMultitaskingDispatchDisabled:N:1:F:0:D:0.002271
 B:BroadcastDuringMultitasking
-P:0:23:UI1:init.c:194
-E:BroadcastDuringMultitasking:N:1:F:0:D:0.002134
+P:0:23:UI1:init.c:174
+E:BroadcastDuringMultitasking:N:1:F:0:D:0.003904
 B:BroadcastDuringMultitaskingIRQDisabled
-P:0:23:UI1:init.c:194
-E:BroadcastDuringMultitaskingIRQDisabled:N:1:F:0:D:0.002165
+P:0:23:UI1:init.c:174
+E:BroadcastDuringMultitaskingIRQDisabled:N:1:F:0:D:0.003949
 B:BroadcastDuringMultitaskingDispatchDisabled
-P:0:23:UI1:init.c:194
-E:BroadcastDuringMultitaskingDispatchDisabled:N:1:F:0:D:0.002130
-Z:SMPMultiCast:C:12:N:12:F:0:D:0.125615
+P:0:23:UI1:init.c:174
+E:BroadcastDuringMultitaskingDispatchDisabled:N:1:F:0:D:0.003914
+B:WrongCPUStateToPerformJobs
+P:0:0:ISR:init.c:226
+P:1:23:UI1:init.c:310
+P:2:23:UI1:init.c:311
+P:3:23:UI1:init.c:312
+E:WrongCPUStateToPerformJobs:N:4:F:0:D:0.007911
+Z:SMPMultiCast:C:13:N:16:F:0:D:0.153651
 
 *** END OF TEST SMPMULTICAST 1 ***