_Thread_Dispatch wrapper is now shared between uniprocessor and SMP configurations

3 files changed, 82 insertions, 1 deletions

diff --git a/schedsim/shell/shared/Makefile.am b/schedsim/shell/shared/Makefile.am
index af5b282..c6ba74f 100644
--- a/schedsim/shell/shared/Makefile.am
+++ b/schedsim/shell/shared/Makefile.am
@@ -52,6 +52,7 @@ libschedsim_a_SOURCES += printheir_executing.c
 libschedsim_a_SOURCES += schedsim_disable_dispatch.c
 libschedsim_a_SOURCES += shell_cmdset.c
 libschedsim_a_SOURCES += shell_makeargs.c 
+libschedsim_a_SOURCES += wrap_thread_dispatch.c
 if HAS_SMP
 libschedsim_a_SOURCES += smp_stub.c
 libschedsim_a_SOURCES += main_currentcpu.c
diff --git a/schedsim/shell/shared/smp_stub.c b/schedsim/shell/shared/smp_stub.c
index cd73f4e..263f49e 100644
--- a/schedsim/shell/shared/smp_stub.c
+++ b/schedsim/shell/shared/smp_stub.c
@@ -16,8 +16,13 @@
 #include <rtems/bspIo.h>
 #include <stdlib.h>
 
+/*
+ * Actually owned by the _Thread_Dispatch() wrapper.
+ * This way the variable is available all the time.
+ */
+extern uint32_t Schedsim_Current_cpu;
+
 #if RTEMS_SMP
-  uint32_t Schedsim_Current_cpu;
   extern uint32_t Schedsim_Maximum_CPUs_From_Command_Line;
 #endif
 
diff --git a/schedsim/shell/shared/wrap_thread_dispatch.c b/schedsim/shell/shared/wrap_thread_dispatch.c
new file mode 100644
index 0000000..fae4f9f
--- /dev/null
+++ b/schedsim/shell/shared/wrap_thread_dispatch.c
@@ -0,0 +1,75 @@
+/*
+ *  Thread Dispatch Wrapper Implmentation
+ *
+ *  COPYRIGHT (c) 1989-2013.
+ *  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.
+ */
+
+#include "shell.h"
+#include <schedsim_shell.h>
+
+#include <stdlib.h>
+#include <stdio.h>
+#include <rtems.h>
+
+typedef Thread_Control * Thread_Control_ptr;
+uint32_t Schedsim_Current_cpu = 0;
+
+Thread_Control_ptr *last_heir = NULL;
+Thread_Control_ptr *last_executing = NULL;
+
+extern void __real__Thread_Dispatch(void);
+
+#if RTEMS_SMP
+  #define MAX_CPUS _SMP_Processor_count
+#else
+  #define MAX_CPUS 1
+#endif
+
+void Init__wrap__Thread_Dispatch()
+{
+  last_heir = (Thread_Control_ptr *) calloc(
+    sizeof( Thread_Control_ptr ),
+    MAX_CPUS
+  );
+  last_executing =  (Thread_Control_ptr *) calloc(
+    sizeof( Thread_Control_ptr ),
+    MAX_CPUS
+  );
+}
+
+void check_heir_and_executing(void)
+{
+  uint32_t level;
+
+  _ISR_Disable_without_giant( level );
+    if ( last_heir[Schedsim_Current_cpu] != _Thread_Heir )
+      PRINT_HEIR();
+
+    if ( last_executing[Schedsim_Current_cpu] != _Thread_Executing )
+      PRINT_EXECUTING();
+
+    last_heir[Schedsim_Current_cpu] = _Thread_Heir;
+    last_executing[Schedsim_Current_cpu] = _Thread_Executing;
+  _ISR_Enable_without_giant( level );
+}
+
+void __wrap__Thread_Dispatch(void)
+{
+  uint32_t   cpu;
+  uint32_t   current_cpu;
+
+  current_cpu = Schedsim_Current_cpu;
+  for ( cpu=0 ; cpu < MAX_CPUS ; cpu++ ) {
+    Schedsim_Current_cpu = cpu;
+    check_heir_and_executing();
+    __real__Thread_Dispatch();
+    check_heir_and_executing();
+  }
+
+  Schedsim_Current_cpu = current_cpu;
+}