From 80d0b2736e4b37659b3867cbd2c61ea858a570d2 Mon Sep 17 00:00:00 2001
From: Gregory Nutt <gnutt@nuttx.org>
Date: Sun, 22 May 2016 15:01:49 -0600
Subject: [PATCH] Reorder some logic:  (1) set initial CPU IDLE task regsters
 AFTER allocating stack, (2) invalidate cache in CPU start-up BEFORE handling
 first interrupt.

---
 arch/arm/src/armv7-a/arm_assert.c   |  8 +++++-
 arch/arm/src/armv7-a/arm_cpustart.c | 41 ++++++++++++++++++++++++++---
 arch/arm/src/imx6/imx_cpuboot.c     |  5 ++++
 sched/init/os_smpstart.c            |  8 ++++++
 4 files changed, 58 insertions(+), 4 deletions(-)

diff --git a/arch/arm/src/armv7-a/arm_assert.c b/arch/arm/src/armv7-a/arm_assert.c
index 00c730919c..ab3bd4c908 100644
--- a/arch/arm/src/armv7-a/arm_assert.c
+++ b/arch/arm/src/armv7-a/arm_assert.c
@@ -334,7 +334,13 @@ static void up_dumpstate(void)
     }
 #endif
 
-  /* Then dump the registers (if available) */
+#ifdef CONFIG_SMP
+  /* Show the CPU number */
+
+  lldbg("CPU%d:\n", up_cpu_index());
+#endif
+
+  /* Then dump the CPU registers (if available) */
 
   up_registerdump();
 
diff --git a/arch/arm/src/armv7-a/arm_cpustart.c b/arch/arm/src/armv7-a/arm_cpustart.c
index 00addb818d..1ec95da855 100644
--- a/arch/arm/src/armv7-a/arm_cpustart.c
+++ b/arch/arm/src/armv7-a/arm_cpustart.c
@@ -51,6 +51,37 @@
 
 #ifdef CONFIG_SMP
 
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: arm_registerdump
+ ****************************************************************************/
+
+#if 0 /* Was useful in solving some startup problems */
+static inline void arm_registerdump(FAR struct tcb_s *tcb)
+{
+  int regndx;
+
+  lldbg("CPU%d:\n", up_cpu_index());
+
+  /* Dump the startup registers */
+
+  for (regndx = REG_R0; regndx <= REG_R15; regndx += 8)
+    {
+      uint32_t *ptr = (uint32_t *)&tcb->xcp.regs[regndx];
+      lldbg("R%d: %08x %08x %08x %08x %08x %08x %08x %08x\n",
+             regndx, ptr[0], ptr[1], ptr[2], ptr[3],
+             ptr[4], ptr[5], ptr[6], ptr[7]);
+    }
+
+  lldbg("CPSR: %08x\n", tcb->xcp.regs[REG_CPSR]);
+}
+#else
+# define arm_registerdump(tcb)
+#endif
+
 /****************************************************************************
  * Public Functions
  ****************************************************************************/
@@ -75,15 +106,17 @@ int arm_start_handler(int irq, FAR void *context)
 {
   FAR struct tcb_s *tcb;
 
-  /* Invalidate CPUn L1 so that is will be reloaded from coherent L2. */
-
-  cp15_invalidate_dcache_all();
+  sllvdbg("CPU%d Started\n", up_cpu_index());
 
   /* Reset scheduler parameters */
 
   tcb = this_task();
   sched_resume_scheduler(tcb);
 
+  /* Dump registers so that we can see what is going to happen on return */
+
+  arm_registerdump(tcb);
+
   /* Then switch contexts. This instantiates the exception context of the
    * tcb at the head of the assigned task list.  In this case, this should
    * be the CPUs NULL task.
@@ -122,6 +155,8 @@ int arm_start_handler(int irq, FAR void *context)
 
 int up_cpu_start(int cpu)
 {
+  sllvdbg("Starting CPU%d\n", cpu);
+
   DEBUGASSERT(cpu >= 0 && cpu < CONFIG_SMP_NCPUS && cpu != this_cpu());
 
   /* Make the content of CPU0 L1 cache has been written to coherent L2 */
diff --git a/arch/arm/src/imx6/imx_cpuboot.c b/arch/arm/src/imx6/imx_cpuboot.c
index 5aca8d33bb..2c3c618861 100644
--- a/arch/arm/src/imx6/imx_cpuboot.c
+++ b/arch/arm/src/imx6/imx_cpuboot.c
@@ -53,6 +53,7 @@
 #include "smp.h"
 #include "fpu.h"
 #include "gic.h"
+#include "cp15_cacheops.h"
 
 #ifdef CONFIG_SMP
 
@@ -296,6 +297,10 @@ void arm_cpu_boot(int cpu)
   (void)up_irq_enable();
 #endif
 
+  /* Invalidate CPUn L1 so that is will be reloaded from coherent L2. */
+
+  cp15_invalidate_dcache_all();
+
   /* The next thing that we expect to happen is for logic running on CPU0
    * to call up_cpu_start() which generate an SGI and a context switch to
    * the configured NuttX IDLE task.
diff --git a/sched/init/os_smpstart.c b/sched/init/os_smpstart.c
index 5ed82fb878..da7852317c 100644
--- a/sched/init/os_smpstart.c
+++ b/sched/init/os_smpstart.c
@@ -210,6 +210,14 @@ int os_smp_start(void)
           sdbg("ERROR: Failed to allocate stack for CPU%d\n", cpu);
           return ret;
         }
+
+      /* Reinitialize the processor-specific portion of the TCB.  This is
+       * the second time this has been called for this CPU, but the stack
+       * was not yet initialized on the first call so we need to do it
+       * again.
+       */
+
+      up_initial_state(tcb);
     }
 
   /* Then start all of the other CPUs after we have completed the memory