score: SMP initialization changes

Add and use _CPU_SMP_Start_processor().  Add and use
_CPU_SMP_Finalize_initialization().  This makes most
_CPU_SMP_Initialize() functions a bit simpler since we can calculate the
minimum value of the count of processors requested by the application
configuration and the count of physically or virtually available
processors in the high-level code.

The CPU port has now the ability to signal a processor start failure.
With the support for clustered/partitioned scheduling the presence of
particular processors can be configured to be optional or mandatory.
There will be a fatal error only in case mandatory processors are not
present.

The CPU port may use a timeout to monitor the start of a processor.

c/src/lib/libbsp/arm/shared/arm-a9mpcore-smp.c

@@ -25,28 +25,39 @@ static void bsp_inter_processor_interrupt(void *arg)
   _SMP_Inter_processor_interrupt_handler();
 }
 
-uint32_t _CPU_SMP_Initialize(uint32_t configured_cpu_count)
+uint32_t _CPU_SMP_Initialize(void)
 {
-  rtems_status_code sc;
+  return arm_gic_irq_processor_count();
-  uint32_t max_cpu_count = arm_gic_irq_processor_count();
+}
-  uint32_t used_cpu_count = configured_cpu_count < max_cpu_count ?
-    configured_cpu_count : max_cpu_count;
 
-  sc = rtems_interrupt_handler_install(
+bool _CPU_SMP_Start_processor(uint32_t cpu_index)
-    ARM_GIC_IRQ_SGI_0,
+{
-    "IPI",
+  (void) cpu_index;
-    RTEMS_INTERRUPT_UNIQUE,
-    bsp_inter_processor_interrupt,
-    NULL
-  );
-  assert(sc == RTEMS_SUCCESSFUL);
 
-  return used_cpu_count;
+  /* Nothing to do */
+
+  return true;
+}
+
+void _CPU_SMP_Finalize_initialization(uint32_t cpu_count)
+{
+  if (cpu_count > 0) {
+    rtems_status_code sc;
+
+    sc = rtems_interrupt_handler_install(
+      ARM_GIC_IRQ_SGI_0,
+      "IPI",
+      RTEMS_INTERRUPT_UNIQUE,
+      bsp_inter_processor_interrupt,
+      NULL
+    );
+    assert(sc == RTEMS_SUCCESSFUL);
+  }
 }
 
 void _CPU_SMP_Send_interrupt( uint32_t target_processor_index )
 {
-  rtems_status_code sc = arm_gic_irq_generate_software_irq(
+  arm_gic_irq_generate_software_irq(
     ARM_GIC_IRQ_SGI_0,
     ARM_GIC_IRQ_SOFTWARE_IRQ_TO_ALL_IN_LIST,
     (uint8_t) (1U << target_processor_index)

c/src/lib/libbsp/i386/shared/smp/smp-imps.c

@@ -786,16 +786,24 @@ static void secondary_cpu_initialize(void)
   _SMP_Start_multitasking_on_secondary_processor();
 }
 
-uint32_t _CPU_SMP_Initialize( uint32_t configured_cpu_count )
+uint32_t _CPU_SMP_Initialize( void )
 {
-  int cores;
   /* XXX need to deal with finding too many cores */
 
-  cores = imps_probe();
+  return (uint32_t) imps_probe();
+}
 
-  if ( cores > 1 )
+bool _CPU_SMP_Start_processor( uint32_t cpu_index )
+{
+  (void) cpu_index;
+
+  return true;
+}
+
+void _CPU_SMP_Finalize_initialization( uint32_t cpu_count )
+{
+  if ( cpu_count > 1 )
     ipi_install_irq();
-  return cores;
 }
 
 void _CPU_SMP_Send_interrupt( uint32_t target_processor_index )

c/src/lib/libbsp/powerpc/qoriq/startup/smp.c

@@ -141,26 +141,34 @@ static void bsp_inter_processor_interrupt(void *arg)
   _SMP_Inter_processor_interrupt_handler();
 }
 
-uint32_t _CPU_SMP_Initialize(uint32_t configured_cpu_count)
+uint32_t _CPU_SMP_Initialize(void)
 {
-  rtems_status_code sc;
+  return CORE_COUNT;
-  uint32_t cores = configured_cpu_count < CORE_COUNT ?
+}
-    configured_cpu_count : CORE_COUNT;
 
-  sc = rtems_interrupt_handler_install(
+bool _CPU_SMP_Start_processor(uint32_t cpu_index)
-    QORIQ_IRQ_IPI_0,
+{
-    "IPI",
+  (void) cpu_index;
-    RTEMS_INTERRUPT_UNIQUE,
-    bsp_inter_processor_interrupt,
-    NULL
-  );
-  assert(sc == RTEMS_SUCCESSFUL);
 
-  if (cores > 1) {
+  release_core_1();
-    release_core_1();
+
+  return true;
+}
+
+void _CPU_SMP_Finalize_initialization(uint32_t cpu_count)
+{
+  if (cpu_count > 1) {
+    rtems_status_code sc;
+
+    sc = rtems_interrupt_handler_install(
+      QORIQ_IRQ_IPI_0,
+      "IPI",
+      RTEMS_INTERRUPT_UNIQUE,
+      bsp_inter_processor_interrupt,
+      NULL
+    );
+    assert(sc == RTEMS_SUCCESSFUL);
   }
-
-  return cores;
 }
 
 void _CPU_SMP_Send_interrupt(uint32_t target_processor_index)

c/src/lib/libbsp/shared/smp/smp_stub.c

@@ -11,12 +11,21 @@
 
 #include <rtems/score/cpu.h>
 
-uint32_t _CPU_SMP_Initialize( uint32_t configured_cpu_count )
+uint32_t _CPU_SMP_Initialize( void )
 {
   /* return the number of CPUs */
   return 1;
 }
 
+bool _CPU_SMP_Start_processor( uint32_t cpu_index )
+{
+  return true;
+}
+
+void _CPU_SMP_Finalize_initialization( uint32_t cpu_count )
+{
+}
+
 void _CPU_SMP_Send_interrupt( uint32_t target_processor_index )
 {
 }

c/src/lib/libbsp/sparc/leon3/smp/smp_leon3.c

@@ -26,53 +26,43 @@ static rtems_isr bsp_inter_processor_interrupt(
   _SMP_Inter_processor_interrupt_handler();
 }
 
-void leon3_secondary_cpu_initialize(uint32_t cpu)
+void leon3_secondary_cpu_initialize(uint32_t cpu_index)
 {
   leon3_set_cache_control_register(0x80000F);
   /* Unmask IPI interrupts at Interrupt controller for this CPU */
-  LEON3_IrqCtrl_Regs->mask[cpu] |= 1 << LEON3_MP_IRQ;
+  LEON3_IrqCtrl_Regs->mask[cpu_index] |= 1U << LEON3_MP_IRQ;
 
   _SMP_Start_multitasking_on_secondary_processor();
 }
 
-uint32_t _CPU_SMP_Initialize( uint32_t configured_cpu_count )
+uint32_t _CPU_SMP_Initialize( void )
 {
-  uint32_t max_cpu_count;
-  uint32_t used_cpu_count;
-  uint32_t cpu;
-
   leon3_set_cache_control_register(0x80000F);
 
-  max_cpu_count = leon3_get_cpu_count(LEON3_IrqCtrl_Regs);
+  if ( rtems_configuration_get_maximum_processors() > 1 ) {
-  used_cpu_count = configured_cpu_count < max_cpu_count ?
-    configured_cpu_count : max_cpu_count;
-
-  #if defined(RTEMS_DEBUG)
-    printk( "Found %d CPUs\n", max_cpu_count );
-
-    if ( max_cpu_count > configured_cpu_count ) {
-      printk(
-        "%d CPUs IS MORE THAN CONFIGURED -- ONLY USING %d\n",
-        max_cpu_count,
-        configured_cpu_count
-      );
-    }
-  #endif
-
-  if ( used_cpu_count > 1 ) {
     LEON_Unmask_interrupt(LEON3_MP_IRQ);
     set_vector(bsp_inter_processor_interrupt, LEON_TRAP_TYPE(LEON3_MP_IRQ), 1);
   }
 
-  for ( cpu = 1 ; cpu < used_cpu_count ; ++cpu ) {
+  return leon3_get_cpu_count(LEON3_IrqCtrl_Regs);
-    #if defined(RTEMS_DEBUG)
+}
-      printk( "Waking CPU %d\n", cpu );
-    #endif
 
-    LEON3_IrqCtrl_Regs->mpstat = 1 << cpu;
+bool _CPU_SMP_Start_processor( uint32_t cpu_index )
-  }
+{
+  #if defined(RTEMS_DEBUG)
+    printk( "Waking CPU %d\n", cpu_index );
+  #endif
 
-  return used_cpu_count;
+  LEON3_IrqCtrl_Regs->mpstat = 1U << cpu_index;
+
+  return true;
+}
+
+void _CPU_SMP_Finalize_initialization( uint32_t cpu_count )
+{
+  (void) cpu_count;
+
+  /* Nothing to do */
 }
 
 void _CPU_SMP_Send_interrupt(uint32_t target_processor_index)

cpukit/score/cpu/arm/rtems/score/cpu.h

@@ -467,7 +467,11 @@ void _CPU_Context_volatile_clobber( uintptr_t pattern );
 void _CPU_Context_validate( uintptr_t pattern );
 
 #ifdef RTEMS_SMP
-  uint32_t _CPU_SMP_Initialize( uint32_t configured_cpu_count );
+  uint32_t _CPU_SMP_Initialize( void );
+
+  bool _CPU_SMP_Start_processor( uint32_t cpu_index );
+
+  void _CPU_SMP_Finalize_initialization( uint32_t cpu_count );
 
   static inline uint32_t _CPU_SMP_Get_current_processor( void )
   {

cpukit/score/cpu/i386/rtems/score/cpu.h

@@ -462,7 +462,11 @@ uint32_t   _CPU_ISR_Get_level( void );
    _CPU_Context_restore( (_the_context) );
 
 #if defined(RTEMS_SMP)
-  uint32_t _CPU_SMP_Initialize( uint32_t configured_cpu_count );
+  uint32_t _CPU_SMP_Initialize( void );
+
+  bool _CPU_SMP_Start_processor( uint32_t cpu_index );
+
+  void _CPU_SMP_Finalize_initialization( uint32_t cpu_count );
 
   uint32_t _CPU_SMP_Get_current_processor( void );
 

cpukit/score/cpu/no_cpu/rtems/score/cpu.h

@@ -1467,19 +1467,47 @@ CPU_Counter_ticks _CPU_Counter_difference(
    * @brief Performs CPU specific SMP initialization in the context of the boot
    * processor.
    *
-   * This function is invoked on the boot processor by RTEMS during
+   * This function is invoked on the boot processor during system
    * initialization.  All interrupt stacks are allocated at this point in case
-   * the CPU port allocates the interrupt stacks.
+   * the CPU port allocates the interrupt stacks.  This function is called
+   * before _CPU_SMP_Start_processor() or _CPU_SMP_Finalize_initialization() is
+   * used.
    *
-   * The CPU port should start secondary processors now.
+   * @return The count of physically or virtually available processors.
-   *
+   * Depending on the configuration the application may use not all processors.
-   * @param[in] configured_cpu_count The count of processors requested by the
-   * application configuration.
-   *
-   * @return The count of processors available for the application in the system.
-   * This value is less than or equal to the configured count of processors.
    */
-  uint32_t _CPU_SMP_Initialize( uint32_t configured_cpu_count );
+  uint32_t _CPU_SMP_Initialize( void );
+
+  /**
+   * @brief Starts a processor specified by its index.
+   *
+   * This function is invoked on the boot processor during system
+   * initialization.
+   *
+   * This function will be called after _CPU_SMP_Initialize().
+   *
+   * @param[in] cpu_index The processor index.
+   *
+   * @retval true Successful operation.
+   * @retval false Unable to start this processor.
+   */
+  bool _CPU_SMP_Start_processor( uint32_t cpu_index );
+
+  /**
+   * @brief Performs final steps of CPU specific SMP initialization in the
+   * context of the boot processor.
+   *
+   * This function is invoked on the boot processor during system
+   * initialization.
+   *
+   * This function will be called after all processors requested by the
+   * application have been started.
+   *
+   * @param[in] cpu_count The minimum value of the count of processors
+   * requested by the application configuration and the count of physically or
+   * virtually available processors.
+   */
+  void _CPU_SMP_Finalize_initialization( uint32_t cpu_count );
 
   /**
    * @brief Returns the index of the current processor.

cpukit/score/cpu/powerpc/rtems/score/cpu.h

@@ -1034,7 +1034,11 @@ void _CPU_Context_volatile_clobber( uintptr_t pattern );
 void _CPU_Context_validate( uintptr_t pattern );
 
 #ifdef RTEMS_SMP
-  uint32_t _CPU_SMP_Initialize( uint32_t configured_cpu_count );
+  uint32_t _CPU_SMP_Initialize( void );
+
+  bool _CPU_SMP_Start_processor( uint32_t cpu_index );
+
+  void _CPU_SMP_Finalize_initialization( uint32_t cpu_count );
 
   static inline uint32_t _CPU_SMP_Get_current_processor( void )
   {

cpukit/score/cpu/sparc/rtems/score/cpu.h

@@ -1161,7 +1161,11 @@ void _CPU_Context_restore(
 ) RTEMS_COMPILER_NO_RETURN_ATTRIBUTE;
 
 #if defined(RTEMS_SMP)
-  uint32_t _CPU_SMP_Initialize( uint32_t configured_cpu_count );
+  uint32_t _CPU_SMP_Initialize( void );
+
+  bool _CPU_SMP_Start_processor( uint32_t cpu_index );
+
+  void _CPU_SMP_Finalize_initialization( uint32_t cpu_count );
 
   #if defined(__leon__)
     static inline uint32_t _CPU_SMP_Get_current_processor( void )

cpukit/score/include/rtems/score/smpimpl.h

@@ -48,7 +48,8 @@ extern "C" {
  */
 typedef enum {
   SMP_FATAL_SHUTDOWN,
-  SMP_FATAL_SHUTDOWN_EARLY
+  SMP_FATAL_SHUTDOWN_EARLY,
+  SMP_FATAL_START_OF_MANDATORY_PROCESSOR_FAILED
 } SMP_Fatal_code;
 
 /**

cpukit/score/src/smp.c

@@ -26,11 +26,13 @@
 
 void _SMP_Handler_initialize( void )
 {
-  uint32_t max_cpus = rtems_configuration_get_maximum_processors();
+  uint32_t cpu_max = rtems_configuration_get_maximum_processors();
-  uint32_t cpu;
+  uint32_t cpu_self;
+  uint32_t cpu_count;
+  uint32_t cpu_index;
 
-  for ( cpu = 0 ; cpu < max_cpus; ++cpu ) {
+  for ( cpu_index = 0 ; cpu_index < cpu_max; ++cpu_index ) {
-    Per_CPU_Control *per_cpu = _Per_CPU_Get_by_index( cpu );
+    Per_CPU_Control *per_cpu = _Per_CPU_Get_by_index( cpu_index );
 
     _SMP_ticket_lock_Initialize( &per_cpu->Lock, "per-CPU" );
   }
@@ -38,9 +40,28 @@ void _SMP_Handler_initialize( void )
   /*
    * Discover and initialize the secondary cores in an SMP system.
    */
-  max_cpus = _CPU_SMP_Initialize( max_cpus );
 
-  _SMP_Processor_count = max_cpus;
+  cpu_count = _CPU_SMP_Initialize();
+  cpu_count = cpu_count < cpu_max ? cpu_count : cpu_max;
+  _SMP_Processor_count = cpu_count;
+
+  cpu_self = _SMP_Get_current_processor();
+
+  for ( cpu_index = 0 ; cpu_index < cpu_count; ++cpu_index ) {
+    if ( cpu_index != cpu_self ) {
+      bool ok = _CPU_SMP_Start_processor( cpu_index );
+
+      if ( !ok ) {
+        _Terminate(
+          RTEMS_FATAL_SOURCE_SMP,
+          false,
+          SMP_FATAL_START_OF_MANDATORY_PROCESSOR_FAILED
+        );
+      }
+    }
+  }
+
+  _CPU_SMP_Finalize_initialization( cpu_count );
 }
 
 void _SMP_Request_start_multitasking( void )