smp: Simplify SMP initialization sequence

Delete bsp_smp_wait_for().  Other parts of the system work without
timeout, e.g. the spinlocks.  Using a timeout here does not make the
system more robust.

Delete bsp_smp_cpu_state and replace it with Per_CPU_State.  The
Per_CPU_State follows the Score naming conventions.  Add
_Per_CPU_Change_state() and _Per_CPU_Wait_for_state() functions to
change and observe states.

Use Per_CPU_State in Per_CPU_Control instead of the anonymous integer.

Add _CPU_Processor_event_broadcast() and _CPU_Processor_event_receive()
functions provided by the CPU port.  Use these functions in
_Per_CPU_Change_state() and _Per_CPU_Wait_for_state().

Add prototype for _SMP_Send_message().

Delete RTEMS_BSP_SMP_FIRST_TASK message.  The first context switch is
now performed in rtems_smp_secondary_cpu_initialize().  Issuing the
first context switch in the context of the inter-processor interrupt is
not possible on systems with a modern interrupt controller.  Such an
interrupt controler usually requires a handshake protocol with interrupt
acknowledge and end of interrupt signals.  A direct context switch in an
interrupt handler circumvents the interrupt processing epilogue and may
leave the system in an inconsistent state.

Release lock in rtems_smp_process_interrupt() even if no message was
delivered.  This prevents deadlock of the system.

Simplify and format _SMP_Send_message(),
_SMP_Request_other_cores_to_perform_first_context_switch(),
_SMP_Request_other_cores_to_dispatch() and
_SMP_Request_other_cores_to_shutdown().

13 files changed, 260 insertions, 293 deletions

diff --git a/c/src/lib/libbsp/i386/shared/smp/smp-imps.c b/c/src/lib/libbsp/i386/shared/smp/smp-imps.c
index 62ce8aca27..916379ed02 100644
--- a/c/src/lib/libbsp/i386/shared/smp/smp-imps.c
+++ b/c/src/lib/libbsp/i386/shared/smp/smp-imps.c
@@ -71,7 +71,6 @@
 #define PHYS_TO_VIRTUAL(x)    /* convert physical address "x" to virtual */
 #define VIRTUAL_TO_PHYS(x)    /* convert virtual address "x" to physical */
 #define UDELAY(x)             /* delay roughly at least "x" microsecs */
-#define TEST_BOOTED(x)        /* test bootaddr x to see if CPU started */
 #define READ_MSR_LO(x)        /* Read MSR low function */
 #else
 #include <string.h>
@@ -125,9 +124,6 @@ static void UDELAY(int x)
 #define READ_MSR_LO(_x) \
   (unsigned int)(read_msr(_x) & 0xffffffff)
 
-#define TEST_BOOTED(_cpu) \
-  (_Per_CPU_Information[_cpu].state == RTEMS_BSP_SMP_CPU_INITIALIZED)
-
 static inline unsigned long long read_msr(unsigned int msr)
 {
   unsigned long long value;
@@ -311,22 +307,6 @@ boot_cpu(imps_processor *proc)
   }
 
   /*
-   *  Check to see if other processor has started.
-   */
-  bsp_smp_wait_for(
-    (volatile unsigned int *)&_Per_CPU_Information[imps_num_cpus].state,
-    RTEMS_BSP_SMP_CPU_INITIALIZED,
-    1600
-  );
-  if ( _Per_CPU_Information[imps_num_cpus].state ==
-        RTEMS_BSP_SMP_CPU_INITIALIZED )
-    printk("#%d  Application Processor (AP)", imps_num_cpus);
-  else {
-    printk("CPU Not Responding, DISABLED");
-    success = 0;
-  }
-
-  /*
    *  Generic CPU startup sequence ends here, the rest is cleanup.
    */
 
@@ -835,32 +815,3 @@ void bsp_smp_broadcast_interrupt(void)
   /* Single broadcast interrupt */
   send_ipi( 0, LAPIC_ICR_DS_ALLEX | 0x30 );
 }
-
-void bsp_smp_wait_for(
-  volatile unsigned int *address,
-  unsigned int           desired,
-  int                    maximum_usecs
-)
-{
-  int iterations;
-  volatile int i;
-  volatile unsigned int *p = (volatile unsigned int *)address;
-
-  for (iterations=0 ;  iterations < maximum_usecs ; iterations++ ) {
-    if ( *p == desired )
-      break;
-    #ifdef __SSE3__
-      __builtin_ia32_monitor( (const void *)address, 0, 0 );
-      if ( *p == desired )
-        break;
-      __builtin_ia32_mwait( 0, 0 );
-    #endif
-
-    /*
-     *  Until i386 ms delay does not depend upon the clock we
-     *  will use this less sophisticated delay. 
-     */
-    for(i=5000; i>0; i--)
-      ;
-  }
-}
diff --git a/c/src/lib/libbsp/shared/smp/bspsmp_wait_for.c b/c/src/lib/libbsp/shared/smp/bspsmp_wait_for.c
deleted file mode 100644
index 8c4dcf75c1..0000000000
--- a/c/src/lib/libbsp/shared/smp/bspsmp_wait_for.c
+++ /dev/null
@@ -1,28 +0,0 @@
-/*
- *
- *  COPYRIGHT (c) 1989-2011.
- *  On-Line Applications Research Corporation (OAR).
- *
- *  The license and distribution terms for this file may be
- *  found in the file LICENSE in this distribution or at
- *  http://www.rtems.com/license/LICENSE.
- */
-
-void rtems_bsp_delay( int usec );
-
-void bsp_smp_wait_for(
-  volatile unsigned int *address,
-  unsigned int           desired,
-  int                    maximum_usecs
-)
-{
-  int iterations;
-  volatile unsigned int *p = address;
-
-  for (iterations=0 ;  iterations < maximum_usecs ; iterations++ ) {
-    if ( *p == desired )
-      break;
-
-    rtems_bsp_delay( 1 );
-  }
-}
diff --git a/c/src/lib/libbsp/sparc/erc32/Makefile.am b/c/src/lib/libbsp/sparc/erc32/Makefile.am
index ddb8d94c3b..988f3ec88a 100644
--- a/c/src/lib/libbsp/sparc/erc32/Makefile.am
+++ b/c/src/lib/libbsp/sparc/erc32/Makefile.am
@@ -66,8 +66,7 @@ libbsp_a_SOURCES += \
     ../../shared/src/irq-shell.c
 
 if HAS_SMP
-libbsp_a_SOURCES += ../../shared/smp/getcpuid.c ../../shared/smp/smp_stub.c \
-    ../../shared/smp/bspsmp_wait_for.c
+libbsp_a_SOURCES += ../../shared/smp/getcpuid.c ../../shared/smp/smp_stub.c
 endif
 
 if HAS_NETWORKING
diff --git a/c/src/lib/libbsp/sparc/leon2/Makefile.am b/c/src/lib/libbsp/sparc/leon2/Makefile.am
index 9699bf1a32..ae6dfb0155 100644
--- a/c/src/lib/libbsp/sparc/leon2/Makefile.am
+++ b/c/src/lib/libbsp/sparc/leon2/Makefile.am
@@ -119,8 +119,7 @@ libbsp_a_SOURCES += ../../sparc/shared/i2c/i2cmst.c
 libbsp_a_SOURCES += timer/timer.c
 
 if HAS_SMP
-libbsp_a_SOURCES += ../../shared/smp/getcpuid.c ../../shared/smp/smp_stub.c \
-    ../../shared/smp/bspsmp_wait_for.c
+libbsp_a_SOURCES += ../../shared/smp/getcpuid.c ../../shared/smp/smp_stub.c
 endif
 
 if HAS_NETWORKING
diff --git a/c/src/lib/libbsp/sparc/leon3/smp/smp_leon3.c b/c/src/lib/libbsp/sparc/leon3/smp/smp_leon3.c
index 6a9c189ec2..90818194cd 100644
--- a/c/src/lib/libbsp/sparc/leon3/smp/smp_leon3.c
+++ b/c/src/lib/libbsp/sparc/leon3/smp/smp_leon3.c
@@ -33,7 +33,7 @@ static inline unsigned int sparc_leon3_get_cctrl( void )
   return v;
 }
 
-rtems_isr bsp_ap_ipi_isr(
+static rtems_isr bsp_ap_ipi_isr(
   rtems_vector_number vector
 )
 {
@@ -99,10 +99,11 @@ uint32_t bsp_smp_initialize( uint32_t configured_cpu_count )
     bsp_smp_delay( 1000000 );
     #if defined(RTEMS_DEBUG)
       printk(
-	"CPU %d is %s\n",
-	cpu,
-	((_Per_CPU_Information[cpu].state == RTEMS_BSP_SMP_CPU_INITIALIZED) ?
-	   "online" : "offline")
+        "CPU %d is %s\n",
+        cpu,
+        _Per_CPU_Information[cpu].state
+          == PER_CPU_STATE_READY_TO_BEGIN_MULTITASKING ?
+            "online" : "offline"
       );
     #endif
   }
@@ -160,21 +161,3 @@ void bsp_smp_delay( int max )
 {
    __delay( max );
 }
-
-void bsp_smp_wait_for(
-  volatile unsigned int *address,
-  unsigned int           desired,
-  int                    maximum_usecs
-)
-{
-  int iterations;
-  volatile unsigned int *p = address;
-
-  for (iterations=0 ;  iterations < maximum_usecs ; iterations++ ) {
-    if ( *p == desired )
-      break;
-    bsp_smp_delay( 5000 );
-  }
-}
-
-
diff --git a/cpukit/score/cpu/i386/rtems/score/cpu.h b/cpukit/score/cpu/i386/rtems/score/cpu.h
index 6a19fb128c..c262e3cf45 100644
--- a/cpukit/score/cpu/i386/rtems/score/cpu.h
+++ b/cpukit/score/cpu/i386/rtems/score/cpu.h
@@ -463,6 +463,16 @@ uint32_t   _CPU_ISR_Get_level( void );
         "1" (_value) : \
         "cc"); \
     } while (0)
+
+  static inline void _CPU_Processor_event_broadcast( void )
+  {
+    __asm__ volatile ( "" : : : "memory" );
+  }
+
+  static inline void _CPU_Processor_event_receive( void )
+  {
+    __asm__ volatile ( "" : : : "memory" );
+  }
 #endif
 
 #define _CPU_Context_Fp_start( _base, _offset ) \
diff --git a/cpukit/score/cpu/no_cpu/rtems/score/cpu.h b/cpukit/score/cpu/no_cpu/rtems/score/cpu.h
index 6d72976d07..e2c6d94c10 100644
--- a/cpukit/score/cpu/no_cpu/rtems/score/cpu.h
+++ b/cpukit/score/cpu/no_cpu/rtems/score/cpu.h
@@ -1402,6 +1402,37 @@ static inline uint32_t CPU_swap_u32(
 #define CPU_swap_u16( value ) \
   (((value&0xff) << 8) | ((value >> 8)&0xff))
 
+#ifdef RTEMS_SMP
+  /**
+   * @brief Broadcasts a processor event.
+   *
+   * Some architectures provide a low-level synchronization primitive for
+   * processors in a multi-processor environment.  Processors waiting for this
+   * event may go into a low-power state and stop generating system bus
+   * transactions.  This function must ensure that preceding store operations
+   * can be observed by other processors.
+   *
+   * @see _CPU_Processor_event_receive().
+   */
+  static inline void _CPU_Processor_event_broadcast( void )
+  {
+    __asm__ volatile ( "" : : : "memory" );
+  }
+
+  /**
+   * @brief Receives a processor event.
+   *
+   * This function will wait for the processor event and may wait forever if no
+   * such event arrives.
+   *
+   * @see _CPU_Processor_event_broadcast().
+   */
+  static inline void _CPU_Processor_event_receive( void )
+  {
+    __asm__ volatile ( "" : : : "memory" );
+  }
+#endif
+
 #ifdef __cplusplus
 }
 #endif
diff --git a/cpukit/score/cpu/sparc/rtems/score/cpu.h b/cpukit/score/cpu/sparc/rtems/score/cpu.h
index 2209c8972b..9654c294f5 100644
--- a/cpukit/score/cpu/sparc/rtems/score/cpu.h
+++ b/cpukit/score/cpu/sparc/rtems/score/cpu.h
@@ -1203,6 +1203,16 @@ void _CPU_Context_restore(
       ); \
       _previous = _val; \
     } while (0)
+
+  static inline void _CPU_Processor_event_broadcast( void )
+  {
+    __asm__ volatile ( "" : : : "memory" );
+  }
+
+  static inline void _CPU_Processor_event_receive( void )
+  {
+    __asm__ volatile ( "" : : : "memory" );
+  }
 #endif
 
 /**
diff --git a/cpukit/score/include/rtems/bspsmp.h b/cpukit/score/include/rtems/bspsmp.h
index 240f8200b2..4806c9076b 100644
--- a/cpukit/score/include/rtems/bspsmp.h
+++ b/cpukit/score/include/rtems/bspsmp.h
@@ -148,12 +148,6 @@ void rtems_smp_secondary_cpu_initialize( void );
  */
 void rtems_smp_process_interrupt(void);
 
-void bsp_smp_wait_for(
-  volatile unsigned int *address,
-  unsigned int           desired,
-  int                    maximum_usecs
-);
-
 #endif
 
 #ifdef __cplusplus
diff --git a/cpukit/score/include/rtems/score/percpu.h b/cpukit/score/include/rtems/score/percpu.h
index 5469d25c5d..735b422c54 100644
--- a/cpukit/score/include/rtems/score/percpu.h
+++ b/cpukit/score/include/rtems/score/percpu.h
@@ -60,32 +60,71 @@ extern "C" {
 typedef struct Thread_Control_struct Thread_Control;
 #endif
 
+#ifdef RTEMS_SMP
+
 typedef enum {
+  /**
+   * @brief The per CPU controls are initialized to zero.
+   *
+   * In this state the only valid field of the per CPU controls for secondary
+   * processors is the per CPU state.  The secondary processors should perform
+   * their basic initialization now and change into the
+   * PER_CPU_STATE_READY_TO_BEGIN_MULTITASKING state once this is complete.
+   *
+   * The owner of the per CPU state field is the secondary processor in this
+   * state.
+   */
+  PER_CPU_STATE_BEFORE_INITIALIZATION,
 
   /**
-   *  This defines the constant used to indicate that the cpu code is in
-   *  its initial powered up start.
+   * @brief Secondary processor is ready to begin multitasking.
+   *
+   * The secondary processor performed its basic initialization and is ready to
+   * receive inter-processor interrupts.  Interrupt delivery must be disabled
+   * in this state, but requested inter-processor interrupts must be recorded
+   * and must be delivered once the secondary processor enables interrupts for
+   * the first time.  The main processor will wait for all secondary processors
+   * to change into this state.  In case a secondary processor does not reach
+   * this state the system will not start.  The secondary processors wait now
+   * for a change into the PER_CPU_STATE_BEGIN_MULTITASKING state set by the
+   * main processor once all secondary processors reached the
+   * PER_CPU_STATE_READY_TO_BEGIN_MULTITASKING state.
+   *
+   * The owner of the per CPU state field is the main processor in this state.
    */
-   RTEMS_BSP_SMP_CPU_INITIAL_STATE = 1,
+  PER_CPU_STATE_READY_TO_BEGIN_MULTITASKING,
 
   /**
-   *  This defines the constant used to indicate that the cpu code has
-   *  completed basic initialization and awaits further commands.
+   * @brief Multitasking begin of secondary processor is requested.
+   *
+   * The main processor completed system initialization and is about to perform
+   * a context switch to its heir thread.  Secondary processors should now
+   * issue a context switch to the heir thread.  This normally enables
+   * interrupts on the processor for the first time.
+   *
+   * The owner of the per CPU state field is the secondary processor in this
+   * state.
    */
-   RTEMS_BSP_SMP_CPU_INITIALIZED = 2,
+  PER_CPU_STATE_BEGIN_MULTITASKING,
 
   /**
-   *  This defines the constant used to indicate that the cpu code has
-   *  completed basic initialization and awaits further commands.
+   * @brief Normal multitasking state.
+   *
+   * The owner of the per CPU state field is the secondary processor in this
+   * state.
    */
-  RTEMS_BSP_SMP_CPU_UP = 3,
+  PER_CPU_STATE_UP,
 
   /**
-   *  This defines the constant used to indicate that the cpu code has
-   *  shut itself down.
+   * @brief This is the terminal state.
+   *
+   * The owner of the per CPU state field is the secondary processor in this
+   * state.
    */
-  RTEMS_BSP_SMP_CPU_SHUTDOWN = 4
-} bsp_smp_cpu_state;
+  PER_CPU_STATE_SHUTDOWN
+} Per_CPU_State;
+
+#endif /* RTEMS_SMP */
 
 /**
  *  @brief Per CPU Core Structure
@@ -133,15 +172,21 @@ typedef struct {
     /** This element is used to lock this structure */
     SMP_lock_spinlock_simple_Control  lock;
 
-    /** This indicates that the CPU is online. */
-    uint32_t                          state;
-
     /**
      *  This is the request for the interrupt.
      *
      *  @note This may become a chain protected by atomic instructions.
      */
-    uint32_t                          message;
+    uint32_t message;
+
+    /**
+     * @brief Indicates the current state of the CPU.
+     *
+     * This field is not protected by a lock.
+     *
+     * @see _Per_CPU_Change_state() and _Per_CPU_Wait_for_state().
+     */
+    Per_CPU_State state;
   #endif
 } Per_CPU_Control;
 #endif
@@ -218,6 +263,16 @@ void _SMP_Handler_initialize(void);
  */
 void _Per_CPU_Initialize(void);
 
+void _Per_CPU_Change_state(
+  Per_CPU_Control *per_cpu,
+  Per_CPU_State new_state
+);
+
+void _Per_CPU_Wait_for_state(
+  const Per_CPU_Control *per_cpu,
+  Per_CPU_State desired_state
+);
+
 #endif
 
 /*
diff --git a/cpukit/score/include/rtems/score/smp.h b/cpukit/score/include/rtems/score/smp.h
index f4bf72edd9..c7de6d64a9 100644
--- a/cpukit/score/include/rtems/score/smp.h
+++ b/cpukit/score/include/rtems/score/smp.h
@@ -58,13 +58,6 @@ extern "C" {
  */
 #define RTEMS_BSP_SMP_SHUTDOWN                  0x04
 
-/**
- *  This defines the bit which indicates the interprocessor interrupt
- *  has been requested that the receiving CPU needs to perform a context
- *  switch to the first task.
- */
-#define RTEMS_BSP_SMP_FIRST_TASK                0x08
-
 #ifndef ASM
 /**
  *  @brief Number of CPUs in a SMP system.
@@ -75,6 +68,16 @@ extern "C" {
 SCORE_EXTERN uint32_t _SMP_Processor_count;
 
 /**
+ *  @brief Sends a SMP message to a processor.
+ *
+ *  The target processor may be the sending processor.
+ *
+ *  @param[in] cpu The target processor of the message.
+ *  @param[in] message The message.
+ */
+void _SMP_Send_message( int cpu, uint32_t message );
+
+/**
  *  @brief Request of others CPUs.
  *
  *  This method is invoked by RTEMS when it needs to make a request
diff --git a/cpukit/score/src/percpu.c b/cpukit/score/src/percpu.c
index f01d933cfe..a957053e18 100644
--- a/cpukit/score/src/percpu.c
+++ b/cpukit/score/src/percpu.c
@@ -56,9 +56,6 @@
         p->interrupt_stack_high = (void *)ptr;
       }
 #endif
-
-      p->state = RTEMS_BSP_SMP_CPU_INITIAL_STATE;
-      RTEMS_COMPILER_MEMORY_BARRIER();
     }
 
     /*
@@ -67,6 +64,32 @@
     max_cpus = bsp_smp_initialize( max_cpus );
 
     _SMP_Processor_count = max_cpus;
+
+    for ( cpu = 1 ; cpu < max_cpus; ++cpu ) {
+      _Per_CPU_Wait_for_state(
+        &_Per_CPU_Information[ cpu ],
+        PER_CPU_STATE_READY_TO_BEGIN_MULTITASKING
+      );
+    }
+  }
+
+  void _Per_CPU_Change_state(
+    Per_CPU_Control *per_cpu,
+    Per_CPU_State new_state
+  )
+  {
+    per_cpu->state = new_state;
+    _CPU_Processor_event_broadcast();
+  }
+
+  void _Per_CPU_Wait_for_state(
+    const Per_CPU_Control *per_cpu,
+    Per_CPU_State desired_state
+  )
+  {
+    while ( per_cpu->state != desired_state ) {
+      _CPU_Processor_event_receive();
+    }
   }
 #else
   /*
diff --git a/cpukit/score/src/smp.c b/cpukit/score/src/smp.c
index a06db5ef84..aee1c4584f 100644
--- a/cpukit/score/src/smp.c
+++ b/cpukit/score/src/smp.c
@@ -27,142 +27,87 @@
   #include <rtems/bspIo.h>
 #endif
 
-/*
- *  Process request to switch to the first task on a secondary core.
- */
-void rtems_smp_run_first_task(int cpu)
+void rtems_smp_secondary_cpu_initialize( void )
 {
-  Thread_Control *heir;
-  ISR_Level       level;
+  int              self = bsp_smp_processor_id();
+  Per_CPU_Control *per_cpu = &_Per_CPU_Information[ self ];
+  Thread_Control  *heir;
+
+  #if defined(RTEMS_DEBUG)
+    printk( "Made it to %d -- ", self );
+  #endif
+
+  _Per_CPU_Change_state( per_cpu, PER_CPU_STATE_READY_TO_BEGIN_MULTITASKING );
+
+  _Per_CPU_Wait_for_state( per_cpu, PER_CPU_STATE_BEGIN_MULTITASKING );
 
-  _ISR_Disable_on_this_core( level );
+  _Per_CPU_Change_state( per_cpu, PER_CPU_STATE_UP );
 
   /*
    *  The Scheduler will have selected the heir thread for each CPU core.
    *  Now we have been requested to perform the first context switch.  So
-   *  force a switch to the designated heir and make it executing on 
+   *  force a switch to the designated heir and make it executing on
    *  THIS core.
    */
-  heir              = _Thread_Heir;
-  _Thread_Executing = heir;
-
-  _CPU_Context_switch_to_first_task_smp( &heir->Registers );
-}
-
-void rtems_smp_secondary_cpu_initialize(void)
-{
-  int       cpu;
-  ISR_Level level;
-
-  cpu = bsp_smp_processor_id();
+  heir = per_cpu->heir;
+  per_cpu->executing = heir;
 
   /*
-   *  Inform the primary CPU that this secondary CPU is initialized
-   *  and ready to dispatch to the first thread it is supposed to
-   *  execute when the primary CPU is ready.
+   * Threads begin execution in the _Thread_Handler() function.   This function
+   * will call _Thread_Enable_dispatch().
    */
-  _Per_CPU_Information[cpu].state = RTEMS_BSP_SMP_CPU_INITIALIZED;
-
-  #if defined(RTEMS_DEBUG)
-    printk( "Made it to %d -- ", cpu );
-  #endif
+  _Thread_Disable_dispatch();
 
-  /*
-   *  With this secondary core out of reset, we can wait for the
-   *  request to switch to the first task.
-   */
-  while(1) {
-    uint32_t   message;
-
-    bsp_smp_wait_for(
-      (volatile unsigned int *)&_Per_CPU_Information[cpu].message,
-      RTEMS_BSP_SMP_FIRST_TASK,
-      10000
-    );
-
-    level = _SMP_lock_spinlock_simple_Obtain( &_Per_CPU_Information[cpu].lock );
-      message = _Per_CPU_Information[cpu].message;
-      if ( message & RTEMS_BSP_SMP_FIRST_TASK ) {
-	_SMP_lock_spinlock_simple_Release( &_Per_CPU_Information[cpu].lock, level );
-        _ISR_Set_level( 0 );
-      }
-     
-    _SMP_lock_spinlock_simple_Release( &_Per_CPU_Information[cpu].lock, level );
-  }
+  _CPU_Context_switch_to_first_task_smp( &heir->Registers );
 }
 
-void rtems_smp_process_interrupt(void)
+void rtems_smp_process_interrupt( void )
 {
-  int        cpu;
-  uint32_t   message;
-  ISR_Level  level;
+  int              self = bsp_smp_processor_id();
+  Per_CPU_Control *per_cpu = &_Per_CPU_Information[ self ];
+  uint32_t         message;
+  ISR_Level        level;
 
-  cpu = bsp_smp_processor_id();
 
-  level = _SMP_lock_spinlock_simple_Obtain( &_Per_CPU_Information[cpu].lock );
-  message = _Per_CPU_Information[cpu].message;
+  level = _SMP_lock_spinlock_simple_Obtain( &per_cpu->lock );
+  message = per_cpu->message;
+  per_cpu->message = 0;
+  _SMP_lock_spinlock_simple_Release( &per_cpu->lock, level );
 
   #if defined(RTEMS_DEBUG)
     {
       void *sp = __builtin_frame_address(0);
       if ( !(message & RTEMS_BSP_SMP_SHUTDOWN) ) {
-        printk( "ISR on CPU %d -- (0x%02x) (0x%p)\n", cpu, message, sp );
+        printk( "ISR on CPU %d -- (0x%02x) (0x%p)\n", self, message, sp );
 	if ( message & RTEMS_BSP_SMP_CONTEXT_SWITCH_NECESSARY )
 	  printk( "context switch necessary\n" );
 	if ( message & RTEMS_BSP_SMP_SIGNAL_TO_SELF )
 	  printk( "signal to self\n" );
 	if ( message & RTEMS_BSP_SMP_SHUTDOWN )
 	  printk( "shutdown\n" );
-	if ( message & RTEMS_BSP_SMP_FIRST_TASK )
-	  printk( "switch to first task\n" );
       }
  
       printk( "Dispatch level %d\n", _Thread_Dispatch_get_disable_level() );
     }
   #endif
 
-  if ( message & RTEMS_BSP_SMP_FIRST_TASK ) {
-    _Per_CPU_Information[cpu].isr_nest_level = 0;
-    _Per_CPU_Information[cpu].message &= ~message;
-    _Per_CPU_Information[cpu].state = RTEMS_BSP_SMP_CPU_UP;
-
-    _SMP_lock_spinlock_simple_Release( &_Per_CPU_Information[cpu].lock, level );
-
-    rtems_smp_run_first_task(cpu);
-    /* does not return */
-  }
-
   if ( message & RTEMS_BSP_SMP_SHUTDOWN ) {
-    _Per_CPU_Information[cpu].message &= ~message;
+    _ISR_Disable_on_this_core( level );
 
-    _Per_CPU_Information[cpu].isr_nest_level = 0;
-    _Per_CPU_Information[cpu].state = RTEMS_BSP_SMP_CPU_SHUTDOWN;
-    _SMP_lock_spinlock_simple_Release( &_Per_CPU_Information[cpu].lock, level );
+    while ( _Thread_Dispatch_decrement_disable_level() != 0 ) {
+      /* Release completely */
+    }
 
-    _Thread_Enable_dispatch();       /* undo ISR code */
-    _ISR_Disable_on_this_core( level );
+    _Per_CPU_Change_state( per_cpu, PER_CPU_STATE_SHUTDOWN );
     while(1)
       ;
     /* does not continue past here */
   }
-
-  if ( message & RTEMS_BSP_SMP_CONTEXT_SWITCH_NECESSARY ) {
-    #if defined(RTEMS_DEBUG)
-      printk( "switch needed\n" );
-    #endif
-    _Per_CPU_Information[cpu].message &= ~message;
-    _SMP_lock_spinlock_simple_Release( &_Per_CPU_Information[cpu].lock, level );
-  }
 }
 
-/*
- *  Send an interrupt processor request to another cpu.
- */
-void _SMP_Send_message(
-  int       cpu,
-  uint32_t  message
-)
+void _SMP_Send_message( int cpu, uint32_t message )
 {
+  Per_CPU_Control *per_cpu = &_Per_CPU_Information[ cpu ];
   ISR_Level level;
 
   #if defined(RTEMS_DEBUG)
@@ -170,90 +115,82 @@ void _SMP_Send_message(
       printk( "Send 0x%x to %d\n", message, cpu );
   #endif
 
-  level = _SMP_lock_spinlock_simple_Obtain( &_Per_CPU_Information[cpu].lock );
-    _Per_CPU_Information[cpu].message |= message;
-  _SMP_lock_spinlock_simple_Release( &_Per_CPU_Information[cpu].lock, level );
+  level = _SMP_lock_spinlock_simple_Obtain( &per_cpu->lock );
+  per_cpu->message |= message;
+  _SMP_lock_spinlock_simple_Release( &per_cpu->lock, level );
+
   bsp_smp_interrupt_cpu( cpu );
 }
 
-void _SMP_Broadcast_message(
-  uint32_t  message
-)
+void _SMP_Broadcast_message( uint32_t message )
 {
-  int        dest_cpu;
-  int        cpu;
-  ISR_Level  level;
-
-  cpu = bsp_smp_processor_id();
-
-  for ( dest_cpu=0 ; dest_cpu <  _SMP_Processor_count; dest_cpu++ ) {
-    if ( cpu == dest_cpu )
-      continue;
-    level = _SMP_lock_spinlock_simple_Obtain( &_Per_CPU_Information[cpu].lock );
-      _Per_CPU_Information[dest_cpu].message |= message;
-    _SMP_lock_spinlock_simple_Release( &_Per_CPU_Information[cpu].lock, level );
+  int self = bsp_smp_processor_id();
+  int ncpus = _SMP_Processor_count;
+  int cpu;
+
+  for ( cpu = 0 ; cpu < ncpus ; ++cpu ) {
+    if ( cpu != self ) {
+      Per_CPU_Control *per_cpu = &_Per_CPU_Information[ cpu ];
+      ISR_Level level = _SMP_lock_spinlock_simple_Obtain( &per_cpu->lock );
+      per_cpu->message |= message;
+      _SMP_lock_spinlock_simple_Release( &per_cpu->lock, level );
+    }
   }
+
   bsp_smp_broadcast_interrupt();
 }
 
-void _SMP_Request_other_cores_to_perform_first_context_switch(void)
+void _SMP_Request_other_cores_to_perform_first_context_switch( void )
 {
-  int    cpu;
+  int self = bsp_smp_processor_id();
+  int ncpus = _SMP_Processor_count;
+  int cpu;
 
-  _Per_CPU_Information[cpu].state = RTEMS_BSP_SMP_CPU_UP; 
-  for (cpu=1 ; cpu < _SMP_Processor_count ; cpu++ ) {
-    _SMP_Send_message( cpu, RTEMS_BSP_SMP_FIRST_TASK );
+  for ( cpu = 0 ; cpu < ncpus ; ++cpu ) {
+    if ( cpu != self ) {
+      _Per_CPU_Change_state(
+        &_Per_CPU_Information[ cpu ],
+        PER_CPU_STATE_BEGIN_MULTITASKING
+      );
+    }
   }
 }
 
-void _SMP_Request_other_cores_to_dispatch(void)
+void _SMP_Request_other_cores_to_dispatch( void )
 {
-  int i;
-  int cpu;
-
-  cpu = bsp_smp_processor_id();
-
-  if ( !_System_state_Is_up (_System_state_Current) )
-    return;
-  for (i=1 ; i < _SMP_Processor_count ; i++ ) {
-    if ( cpu == i )
-      continue;
-    if ( _Per_CPU_Information[i].state != RTEMS_BSP_SMP_CPU_UP )
-      continue;
-    if ( !_Per_CPU_Information[i].dispatch_necessary )
-      continue;
-    _SMP_Send_message( i, RTEMS_BSP_SMP_CONTEXT_SWITCH_NECESSARY );
+  if ( _System_state_Is_up( _System_state_Get() ) ) {
+    int self = bsp_smp_processor_id();
+    int ncpus = _SMP_Processor_count;
+    int cpu;
+
+    for ( cpu = 0 ; cpu < ncpus ; ++cpu ) {
+      const Per_CPU_Control *per_cpu = &_Per_CPU_Information[ cpu ];
+
+      if (
+        cpu != self
+          && per_cpu->state == PER_CPU_STATE_UP
+          && per_cpu->dispatch_necessary
+      ) {
+        _SMP_Send_message( cpu, RTEMS_BSP_SMP_CONTEXT_SWITCH_NECESSARY );
+      }
+    }
   }
 }
 
-void _SMP_Request_other_cores_to_shutdown(void)
+void _SMP_Request_other_cores_to_shutdown( void )
 {
-  bool   allDown;
-  int    ncpus;
-  int    n;
-  int    cpu;
-
-  cpu   = bsp_smp_processor_id();
-  ncpus = _SMP_Processor_count;
+  int self = bsp_smp_processor_id();
+  int ncpus = _SMP_Processor_count;
+  int cpu;
 
   _SMP_Broadcast_message( RTEMS_BSP_SMP_SHUTDOWN );
 
-  allDown = true;
-  for (n=0 ; n<ncpus ; n++ ) {
-     if ( n == cpu ) 
-       continue;
-     bsp_smp_wait_for(
-       (unsigned int *)&_Per_CPU_Information[n].state,
-       RTEMS_BSP_SMP_CPU_SHUTDOWN,
-       10000
-    );
-    if ( _Per_CPU_Information[n].state != RTEMS_BSP_SMP_CPU_SHUTDOWN )
-      allDown = false;
+  for ( cpu = 0 ; cpu < ncpus ; ++cpu ) {
+    if ( cpu != self ) {
+      _Per_CPU_Wait_for_state(
+        &_Per_CPU_Information[ cpu ],
+        PER_CPU_STATE_SHUTDOWN
+      );
+    }
   }
-  if ( !allDown )
-    printk( "not all down -- timed out\n" );
-  #if defined(RTEMS_DEBUG)
-    else
-      printk( "All CPUs shutdown successfully\n" );
-  #endif
 }