Ticket #1796: InterprocessorPatch.txt

Index: cpukit/sapi/src/exshutdown.c
===================================================================
RCS file: /usr1/CVS/rtems/cpukit/sapi/src/exshutdown.c,v
retrieving revision 1.4
diff -u -r1.4 exshutdown.c
--- cpukit/sapi/src/exshutdown.c        10 Aug 2010 22:56:35 -0000      1.4
+++ cpukit/sapi/src/exshutdown.c        12 May 2011 19:13:16 -0000
@@ -1,7 +1,7 @@
 /*
  *  Initialization Manager
  *
- *  COPYRIGHT (c) 1989-1999.
+ *  COPYRIGHT (c) 1989-2011.
  *  On-Line Applications Research Corporation (OAR).
  *
  *  The license and distribution terms for this file may be
@@ -20,6 +20,10 @@
 #include <rtems/score/thread.h>
 #include <rtems/score/interr.h>
 
+#if defined(RTEMS_SMP)
+  #include <rtems/score/smp.h>
+#endif
+
 /*
  *  rtems_shutdown_executive
  *
@@ -37,6 +41,9 @@
 )
 {
   if ( _System_state_Is_up( _System_state_Get() ) ) {
+    #if defined(RTEMS_SMP)
+      _SMP_Request_other_cores_to_shutdown();
+    #endif
     _System_state_Set( SYSTEM_STATE_SHUTDOWN );
     _Thread_Stop_multitasking();
   }
Index: cpukit/score/include/rtems/score/percpu.h
===================================================================
RCS file: /usr1/CVS/rtems/cpukit/score/include/rtems/score/percpu.h,v
retrieving revision 1.6
diff -u -r1.6 percpu.h
--- cpukit/score/include/rtems/score/percpu.h   11 May 2011 14:22:35 -0000      1.6
+++ cpukit/score/include/rtems/score/percpu.h   12 May 2011 19:13:16 -0000
@@ -76,9 +76,15 @@
 
   /**
    *  This defines the constant used to indicate that the cpu code has
+   *  completed basic initialization and awaits further commands.
+   */
+  RTEMS_BSP_SMP_CPU_UP = 3,
+
+  /**
+   *  This defines the constant used to indicate that the cpu code has
    *  shut itself down.
    */
-  RTEMS_BSP_SMP_CPU_SHUTDOWN = 3
+  RTEMS_BSP_SMP_CPU_SHUTDOWN = 4
 } bsp_smp_cpu_state;
 
 /**
Index: cpukit/score/include/rtems/score/smp.h
===================================================================
RCS file: /usr1/CVS/rtems/cpukit/score/include/rtems/score/smp.h,v
retrieving revision 1.1
diff -u -r1.1 smp.h
--- cpukit/score/include/rtems/score/smp.h      27 Apr 2011 17:18:59 -0000      1.1
+++ cpukit/score/include/rtems/score/smp.h      12 May 2011 19:13:16 -0000
@@ -87,6 +87,14 @@
 );
 
 /**
+ *  @brief Request Other Cores to Perform First Context Switch
+ *
+ *  Send message to other cores requesting them to perform
+ *  their first context switch operation.
+ */
+void _SMP_Request_other_cores_to_perform_first_context_switch(void);
+
+/**
  *  @brief Request Dispatch on Other Cores
  *
  *  Send message to other cores requesting them to perform
Index: cpukit/score/src/smp.c
===================================================================
RCS file: /usr1/CVS/rtems/cpukit/score/src/smp.c,v
retrieving revision 1.3
diff -u -r1.3 smp.c
--- cpukit/score/src/smp.c      27 Apr 2011 17:18:59 -0000      1.3
+++ cpukit/score/src/smp.c      12 May 2011 19:13:16 -0000
@@ -19,30 +19,35 @@
 #include <rtems/score/thread.h>
 
 #if defined(RTEMS_SMP)
-#define SMP_DEBUG
+  #define RTEMS_DEBUG
+#endif
 
-#if defined(SMP_DEBUG)
+#if defined(RTEMS_DEBUG)
   #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)
 {
   Thread_Control *heir;
 
   /*
-   *  This CPU has an heir thread so we need to dispatch it.
+   *  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 
+   *  THIS core.
    */
-  heir = _Thread_Heir;
-
-  /*
-   *  This is definitely a hack until we have SMP scheduling.  Since there
-   *  is only one executing and heir right now, we have to fake this out.
-   */
-  _Thread_Dispatch_set_disable_level(1);
+  heir              = _Thread_Heir;
   _Thread_Executing = heir;
+
   _CPU_Context_switch_to_first_task_smp( &heir->Registers );
 }
 
+/*
+ *  Process request to initialize this secondary core.
+ */
 void rtems_smp_secondary_cpu_initialize(void)
 {
   int cpu;
@@ -63,15 +68,26 @@
   _Per_CPU_Information[cpu].state = RTEMS_BSP_SMP_CPU_INITIALIZED;
 
   /*
-   *  HACK: Should not have to enable interrupts in real system here.
-   *        It should happen as part of switching to the first task.
+   *  With this secondary core out of reset, we can wait for the
+   *  request to switch to the first task.
+   *
+   *  XXX When SMP ISR code is complete, do we want interrupts on
+   *  XXX or off at this point?
    */
-   
-  _Per_CPU_Information[cpu].isr_nest_level = 1;
   _ISR_Set_level( 0 );
-  while(1) ;
+  while(1) {
+    bsp_smp_wait_for(
+      (volatile unsigned int *)&_Per_CPU_Information[cpu].message,
+      RTEMS_BSP_SMP_FIRST_TASK,
+      10000
+    );
+  }
 }
 
+/*
+ *  Process an interrupt processor interrupt which indicates a request
+ *  from another core.
+ */
 void rtems_smp_process_interrupt(void)
 {
   int        cpu;
@@ -81,30 +97,52 @@
   cpu = bsp_smp_processor_id();
 
   level = _SMP_lock_Spinlock_Obtain( &_Per_CPU_Information[cpu].lock );
-    message = _Per_CPU_Information[cpu].message;
-    _Per_CPU_Information[cpu].message &= ~message;
-  _SMP_lock_Spinlock_Release( &_Per_CPU_Information[cpu].lock, level );
+  message = _Per_CPU_Information[cpu].message;
 
   #if defined(SMP_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( "Dispatch level %d\n", _Thread_Dispatch_disable_level );
+      if ( !(message & RTEMS_BSP_SMP_SHUTDOWN) ) {
+        printk( "ISR on CPU %d -- (0x%02x) (0x%p)\n", cpu, 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 ) {
+    /*
+     * XXX Thread dispatch disable level at this point will have to be
+     * XXX revisited when Interrupts on SMP is addressed.
+     */
+    _Thread_Dispatch_disable_level--; /* undo ISR code */
     _Per_CPU_Information[cpu].isr_nest_level = 0;
-    _Per_CPU_Information[cpu].message = 0;
-    _Per_CPU_Information[cpu].state = RTEMS_BSP_SMP_CPU_INITIALIZED;
+    _Per_CPU_Information[cpu].message &= ~message;
+    _Per_CPU_Information[cpu].state = RTEMS_BSP_SMP_CPU_UP;
+
+    _SMP_lock_Spinlock_Release( &_Per_CPU_Information[cpu].lock, level );
+    _Thread_Disable_dispatch();
     rtems_smp_run_first_task(cpu);
     /* does not return */
   }
 
   if ( message & RTEMS_BSP_SMP_SHUTDOWN ) {
-    ISR_Level level;
-    _Thread_Dispatch_set_disable_level(0);
+    /*
+     * XXX Thread dispatch disable level at this point will have to be
+     * XXX revisited when Interrupts on SMP is addressed.
+     */
+    _Per_CPU_Information[cpu].message &= ~message;
+    _SMP_lock_Spinlock_Release( &_Per_CPU_Information[cpu].lock, level );
+
+    _Thread_Dispatch_disable_level--; /* undo ISR code */
     _Per_CPU_Information[cpu].isr_nest_level = 0;
     _Per_CPU_Information[cpu].state = RTEMS_BSP_SMP_CPU_SHUTDOWN;
     _ISR_Disable( level );
@@ -114,12 +152,22 @@
   }
 
   if ( message & RTEMS_BSP_SMP_CONTEXT_SWITCH_NECESSARY ) {
-    printk( "switch needed\n" );
-    _Per_CPU_Information[cpu].dispatch_necessary = true;
+    #if defined(SMP_DEBUG)
+      printk( "switch needed\n" );
+    #endif
+    /*
+     * XXX Thread dispatch disable level at this point will have to be
+     * XXX revisited when Interrupts on SMP is addressed.
+     */
+    _Per_CPU_Information[cpu].message &= ~message;
+    _SMP_lock_Spinlock_Release( &_Per_CPU_Information[cpu].lock, level );
   }
 }
 
-void rtems_smp_send_message( 
+/*
+ *  Send an interrupt processor request to another cpu.
+ */
+void _SMP_send_message(
   int       cpu,
   uint32_t  message
 )
@@ -132,7 +180,10 @@
   bsp_smp_interrupt_cpu( cpu );
 }
 
-void rtems_smp_broadcast_message(
+/*
+ *  Send interrupt processor request to all other nodes
+ */
+void _SMP_Broadcast_message(
   uint32_t  message
 )
 {
@@ -151,4 +202,80 @@
   }
   bsp_smp_broadcast_interrupt();
 }
-#endif
+
+/*
+ *  Send interrupt processor requests to perform first context switch 
+ */
+void _SMP_Request_other_cores_to_perform_first_context_switch(void)
+{
+  int    cpu;
+
+  for (cpu=1 ; cpu < _SMP_Processor_count ; cpu++ ) {
+    _SMP_send_message( cpu, RTEMS_BSP_SMP_FIRST_TASK );
+    while (_Per_CPU_Information[cpu].state != RTEMS_BSP_SMP_CPU_UP ) {
+      bsp_smp_wait_for(
+        (volatile unsigned int *)&_Per_CPU_Information[cpu].state,
+        RTEMS_BSP_SMP_CPU_UP,
+        10000
+      );
+    }
+  }
+}
+
+/*
+ *  Send message to other cores requesting them to perform
+ *  a thread dispatch operation.
+ */
+void _SMP_Request_other_cores_to_dispatch(void)
+{
+  int i;
+  int cpu_num;
+
+  cpu_num = bsp_smp_processor_id();
+
+  if ( !_System_state_Is_up (_System_state_Current) )
+    return;
+  for (i=1 ; i < _SMP_Processor_count ; i++ ) {
+    if ( cpu_num == 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 );
+    bsp_smp_wait_for(
+      (volatile unsigned int *)&_Per_CPU_Information[i].message,
+      0,
+      10000
+    );
+  }
+}
+
+/*
+ *  Send message to other cores requesting them to shutdown.
+ */
+void _SMP_Request_other_cores_to_shutdown(void)
+{
+  bool allDown;
+  int  ncpus;
+  int  n;
+
+  ncpus = _SMP_Processor_count;
+
+  _SMP_Broadcast_message( RTEMS_BSP_SMP_SHUTDOWN );
+
+  allDown = true;
+  for (n=1 ; n<ncpus ; n++ ) {
+     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;
+  }
+  if ( !allDown )
+    printk( "not all down -- timed out\n" );
+  else
+    printk( "All CPUs shutdown successfully\n" );
+}
Index: cpukit/score/src/threadhandler.c
===================================================================
RCS file: /usr1/CVS/rtems/cpukit/score/src/threadhandler.c,v
retrieving revision 1.32
diff -u -r1.32 threadhandler.c
--- cpukit/score/src/threadhandler.c    28 Jan 2011 20:24:53 -0000      1.32
+++ cpukit/score/src/threadhandler.c    12 May 2011 19:13:16 -0000
@@ -2,7 +2,7 @@
  *  Thread Handler
  *
  *
- *  COPYRIGHT (c) 1989-2009.
+ *  COPYRIGHT (c) 1989-2011.
  *  On-Line Applications Research Corporation (OAR).
  *
  *  The license and distribution terms for this file may be
@@ -51,6 +51,11 @@
   #define EXECUTE_GLOBAL_CONSTRUCTORS
 #endif
 
+#if defined(RTEMS_SMP)
+  #include <rtems/score/smp.h>
+#endif
+
+
 /*PAGE
  *
  *  _Thread_Handler
@@ -138,8 +143,15 @@
      */
     if (!doneCons) /* && (volatile void *)_init) */ {
       INIT_NAME ();
+   
+      #if defined(RTEMS_SMP)
+        _Thread_Disable_dispatch();
+          _SMP_Request_other_cores_to_perform_first_context_switch();
+        _Thread_Enable_dispatch();
+      #endif
+
     }
-  #endif
+ #endif
 
   if ( executing->Start.prototype == THREAD_START_NUMERIC ) {
     executing->Wait.return_argument =