From 70782b0f14cc763a25d002084c729a1b7e5c3e14 Mon Sep 17 00:00:00 2001
From: Gregory Nutt <gnutt@nuttx.org>
Date: Thu, 12 May 2016 15:04:46 -0600
Subject: [PATCH] ARMv7-A i.MX6: More SMP logic.  Still untested.

---
 Documentation/NuttxPortingGuide.html          |  36 +--
 .../src/{imx6/imx_cpuinit.c => armv7-a/smp.h} |  74 +++--
 arch/arm/src/imx6/Make.defs                   |   2 +-
 arch/arm/src/imx6/imx_boot.c                  |  15 +
 arch/arm/src/imx6/imx_boot.h                  |  57 +++-
 arch/arm/src/imx6/imx_cpuboot.c               | 265 ++++++++++++++++++
 arch/sim/src/up_smphook.c                     |  23 --
 include/nuttx/arch.h                          |  22 --
 sched/init/os_smpstart.c                      |   6 -
 9 files changed, 381 insertions(+), 119 deletions(-)
 rename arch/arm/src/{imx6/imx_cpuinit.c => armv7-a/smp.h} (59%)
 create mode 100644 arch/arm/src/imx6/imx_cpuboot.c

diff --git a/Documentation/NuttxPortingGuide.html b/Documentation/NuttxPortingGuide.html
index d935ad37a1..e44d380fc0 100644
--- a/Documentation/NuttxPortingGuide.html
+++ b/Documentation/NuttxPortingGuide.html
@@ -154,9 +154,8 @@
       <a href="#uptestset">4.7.1 <code>up_testset()</code></a><br>
       <a href="#upcpuindex">4.7.2 <code>up_cpu_index()</code></a><br>
       <a href="#upcpustart">4.7.3 <code>up_cpu_start()</code></a><br>
-      <a href="#upcpuinitialize">4.7.4 <code>up_cpu_initialize()</code></a><br>
-      <a href="#upcpupause">4.7.5 <code>up_cpu_pause()</code></a><br>
-      <a href="#upcpuresume">4.7.6 <code>up_cpu_resume()</code></a>
+      <a href="#upcpupause">4.7.4 <code>up_cpu_pause()</code></a><br>
+      <a href="#upcpuresume">4.7.5 <code>up_cpu_resume()</code></a>
     </ul>
     <a href="#exports">4.8 APIs Exported by NuttX to Architecture-Specific Logic</a>
     <ul>
@@ -3722,34 +3721,7 @@ int up_cpu_start(int cpu);
   </p>
 </ul>
 
-<h3><a name="upcpuinitialize">4.7.4 <code>up_cpu_initialize()</code></a></h3>
-<p><b>Function Prototype</b>:<p>
-<ul><pre>
-#include &lt;nuttx/arch.h&gt;
-#ifdef CONFIG_SMP
-int up_cpu_initialize(void);
-#endif
-</pre></ul>
-
-<p><b>Description</b>:</p>
-<ul>
-  <p>
-    After the CPU has been started (via <code>up_cpu_start()</code>) the system will call back into the architecture-specific code with this function on the thread of execution of the newly started CPU.
-    This gives the architecture-specific a chance to perform ny initial, CPU-specific initialize on that thread.
-  </p>
-</ul>
-<p><b>Input Parameters</b>:</p>
-<ul>
-  None
-</ul>
-<p><b>Returned Value</b>:</p>
-<ul>
-  <p>
-    Zero (<code>OK</code>) is returned on success; a negated <code>errno</code> value on failure.
-  </p>
-</ul>
-
-<h3><a name="upcpupause">4.7.5 <code>up_cpu_pause()</code></a></h3>
+<h3><a name="upcpupause">4.7.4 <code>up_cpu_pause()</code></a></h3>
 <p><b>Function Prototype</b>:<p>
 <ul><pre>
 #include &lt;nuttx/arch.h&gt;
@@ -3781,7 +3753,7 @@ int up_cpu_pause(int cpu);
   </p>
 </ul>
 
-<h3><a name="upcpuresume">4.7.6 <code>up_cpu_resume()</code></a></h3>
+<h3><a name="upcpuresume">4.7.5 <code>up_cpu_resume()</code></a></h3>
 <p><b>Function Prototype</b>:<p>
 <ul><pre>
 #include &lt;nuttx/arch.h&gt;
diff --git a/arch/arm/src/imx6/imx_cpuinit.c b/arch/arm/src/armv7-a/smp.h
similarity index 59%
rename from arch/arm/src/imx6/imx_cpuinit.c
rename to arch/arm/src/armv7-a/smp.h
index d928781408..42b3eda0ed 100644
--- a/arch/arm/src/imx6/imx_cpuinit.c
+++ b/arch/arm/src/armv7-a/smp.h
@@ -1,5 +1,6 @@
 /****************************************************************************
- * arch/arm/src/imx6/imx_clockconfig.c
+ * arch/arm/src/armv7-a/smp.h
+ * Common ARM support for SMP on multi-core CPUs.
  *
  *   Copyright (C) 2016 Gregory Nutt. All rights reserved.
  *   Author: Gregory Nutt <gnutt@nuttx.org>
@@ -33,46 +34,77 @@
  *
  ****************************************************************************/
 
+#ifndef __ARCH_ARM_SRC_ARMV7_A_SMP_H
+#define __ARCH_ARM_SRC_ARMV7_A_SMP_H
+
 /****************************************************************************
  * Included Files
  ****************************************************************************/
 
 #include <nuttx/config.h>
 
-#include <nuttx/arch.h>
-
-#include "gic.h"
-
 #ifdef CONFIG_SMP
 
 /****************************************************************************
- * Public Functions
+ * Public Function Prototypes
  ****************************************************************************/
 
 /****************************************************************************
- * Name: up_cpu_initialize
+ * Name: __cpu[n]_start
  *
  * Description:
- *   After the CPU has been started (via up_cpu_start()) the system will
- *   call back into the architecture-specific code with this function on the
- *   thread of execution of the newly started CPU.  This gives the
- *   architecture-specific a chance to perform ny initial, CPU-specific
- *   initialize on that thread.
+ *   Boot functions for each CPU (other than CPU0).  These functions set up
+ *   the ARM operating mode, the initial stack, and configure co-processor
+ *   registers.  At the end of the boot, arm_cpu_boot() is called.
  *
- * Input Parameters:
+ *   These functions are provided by the common ARMv7-A logic.
+ *
+ * Input parameters:
  *   None
  *
  * Returned Value:
- *   Zero on success; a negated errno value on failure.
+ *   Do not return.
  *
  ****************************************************************************/
 
-int up_cpu_initialize(void)
-{
-  /* Initialize the Generic Interrupt Controller (GIC) for CPUn (n != 0) */
+#if CONFIG_SMP_NCPUS > 1
+void __cpu1_start(void);
+#endif
 
-  arm_gic_initialize();
-  return OK;
-}
+#if CONFIG_SMP_NCPUS > 2
+void __cpu2_start(void);
+#endif
 
-#endif /* CONFIG_SMP */
+#if CONFIG_SMP_NCPUS > 3
+void __cpu3_start(void);
+#endif
+
+#if CONFIG_SMP_NCPUS > 4
+#  error This logic needs to extended for CONFIG_SMP_NCPUS > 4
+#endif
+
+/****************************************************************************
+ * Name: arm_cpu_boot
+ *
+ * Description:
+ *   Continues the C-level initialization started by the assembly language
+ *   __cpu[n]_start function.  At a minimum, this function needs to initialize
+ *   interrupt handling and, perhaps, wait on WFI for arm_cpu_start() to
+ *   issue an SGI.
+ *
+ *   This function must be provided by the each ARMv7-A MCU and implement
+ *   MCU-specific initialization logic.
+ *
+ * Input parameters:
+ *   cpu - The CPU index.  This is the same value that would be obtained by
+ *      calling up_cpu_index();
+ *
+ * Returned Value:
+ *   Does not return.
+ *
+ ****************************************************************************/
+
+void arm_cpu_boot(int cpu);
+
+#endif  /* CONFIG_SMP */
+#endif  /* __ARCH_ARM_SRC_ARMV7_A_SMP_H */
diff --git a/arch/arm/src/imx6/Make.defs b/arch/arm/src/imx6/Make.defs
index 8ddf77b5e8..c33085a1bc 100644
--- a/arch/arm/src/imx6/Make.defs
+++ b/arch/arm/src/imx6/Make.defs
@@ -145,5 +145,5 @@ CHIP_CSRCS += imx_timerisr.c imx_gpio.c imx_iomuxc.c
 CHIP_CSRCS += imx_serial.c imx_lowputc.c
 
 ifeq ($(CONFIG_SMP),y)
-CHIP_CSRCS += imx_cpuinit.c
+CHIP_CSRCS += imx_cpuboot.c
 endif
diff --git a/arch/arm/src/imx6/imx_boot.c b/arch/arm/src/imx6/imx_boot.c
index 89fe54cac9..8e5c56232f 100644
--- a/arch/arm/src/imx6/imx_boot.c
+++ b/arch/arm/src/imx6/imx_boot.c
@@ -400,6 +400,13 @@ void arm_boot(void)
   imx_setupmappings();
   imx_lowputc('A');
 
+  /* Make sure that all other CPUs are in the disabled state.  This is a
+   * formality because the other CPUs are actually running then we have
+   * probably already crashed.
+   */
+
+  imx_cpu_disable();
+
   /* Provide a special mapping for the OCRAM interrupt vector positioned in
    * high memory.
    */
@@ -498,5 +505,13 @@ void arm_boot(void)
   imx_earlyserialinit();
   imx_lowputc('M');
 #endif
+
+  /* Now we can enable all other CPUs.  The enabled CPUs will start execution
+   * at __cpuN_start and, after very low-level CPU initialzation has been
+   * performed, will branch to arm_cpu_boot() (see arch/arm/src/armv7-a/smp.h)
+   */
+
+  imx_cpu_enable();
+  imx_lowputc('N');
   imx_lowputc('\n');
 }
diff --git a/arch/arm/src/imx6/imx_boot.h b/arch/arm/src/imx6/imx_boot.h
index 8e63cc9e9d..11c1ef56fe 100644
--- a/arch/arm/src/imx6/imx_boot.h
+++ b/arch/arm/src/imx6/imx_boot.h
@@ -51,23 +51,11 @@
 #include "chip.h"
 
 /****************************************************************************
- * Pre-processor Definitions
- ****************************************************************************/
-
-/****************************************************************************
- * Public Types
- ****************************************************************************/
-
-/****************************************************************************
- * Inline Functions
+ * Public Function Prototypes
  ****************************************************************************/
 
 #ifndef __ASSEMBLY__
 
-/****************************************************************************
- * Public Data
- ****************************************************************************/
-
 #undef EXTERN
 #if defined(__cplusplus)
 #define EXTERN extern "C"
@@ -78,9 +66,50 @@ extern "C"
 #endif
 
 /****************************************************************************
- * Public Function Prototypes
+ * Name: imx_cpu_disable
+ *
+ * Description:
+ *   Called from CPU0 to make sure that all other CPUs are in the disabled
+ *   state.  This is a formality because the other CPUs are actually running
+ *   then we have probably already crashed.
+ *
+ * Input Parameters:
+ *   None
+ *
+ * Returned Value:
+ *   None
+ *
  ****************************************************************************/
 
+#ifdef CONFIG_SMP
+void imx_cpu_disable(void);
+#else
+#  define imx_cpu_disable()
+#endif
+
+/****************************************************************************
+ * Name: imx_cpu_enable
+ *
+ * Description:
+ *   Called from CPU0 to enable all other CPUs.  The enabled CPUs will start
+ *   execution at __cpuN_start and, after very low-level CPU initialzation
+ *   has been performed, will branch to arm_cpu_boot()
+ *   (see arch/arm/src/armv7-a/smp.h)
+ *
+ * Input Parameters:
+ *   None
+ *
+ * Returned Value:
+ *   None
+ *
+ ****************************************************************************/
+
+#ifdef CONFIG_SMP
+void imx_cpu_enable(void);
+#else
+#  define imx_cpu_enable()
+#endif
+
 /****************************************************************************
  * Name: imx_board_initialize
  *
diff --git a/arch/arm/src/imx6/imx_cpuboot.c b/arch/arm/src/imx6/imx_cpuboot.c
new file mode 100644
index 0000000000..650b9ddf5e
--- /dev/null
+++ b/arch/arm/src/imx6/imx_cpuboot.c
@@ -0,0 +1,265 @@
+/****************************************************************************
+ * arch/arm/src/imx6/imx_cpuboot.c
+ *
+ *   Copyright (C) 2016 Gregory Nutt. All rights reserved.
+ *   Author: Gregory Nutt <gnutt@nuttx.org>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name NuttX nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <stdint.h>
+#include <assert.h>
+
+#include <nuttx/arch.h>
+
+#include "up_arch.h"
+
+#include "chip/imx_src.h"
+#include "smp.h"
+#include "gic.h"
+
+#ifdef CONFIG_SMP
+
+/****************************************************************************
+ * Private Types
+ ****************************************************************************/
+
+typedef CODE void (*cpu_start_t)(void);
+
+/****************************************************************************
+ * Private Data
+ ****************************************************************************/
+
+#if 0 /* Not used */
+static const uint32_t g_cpu_reset[CONFIG_SMP_NCPUS] =
+{
+  0,
+#if CONFIG_SMP_NCPUS > 1
+  SRC_SCR_CORE1_RST,
+#endif
+#if CONFIG_SMP_NCPUS > 2
+  SRC_SCR_CORE2_RST,
+#endif
+#if CONFIG_SMP_NCPUS > 3
+  SRC_SCR_CORE3_RST
+#endif
+};
+#endif
+
+static const uint32_t g_cpu_ctrl[CONFIG_SMP_NCPUS] =
+{
+  0,
+#if CONFIG_SMP_NCPUS > 1
+  SRC_SCR_CORE1_ENABLE,
+#endif
+#if CONFIG_SMP_NCPUS > 2
+  SRC_SCR_CORE2_ENABLE,
+#endif
+#if CONFIG_SMP_NCPUS > 3
+  SRC_SCR_CORE3_ENABLE
+#endif
+};
+
+static const uintptr_t g_cpu_gpr[CONFIG_SMP_NCPUS] =
+{
+  0,
+#if CONFIG_SMP_NCPUS > 1
+  IMX_SRC_GPR3,
+#endif
+#if CONFIG_SMP_NCPUS > 2
+  IMX_SRC_GPR5,
+#endif
+#if CONFIG_SMP_NCPUS > 3
+  IMX_SRC_GPR7
+#endif
+};
+
+static const cpu_start_t g_cpu_boot[CONFIG_SMP_NCPUS] =
+{
+  0,
+#if CONFIG_SMP_NCPUS > 1
+  __cpu1_start,
+#endif
+#if CONFIG_SMP_NCPUS > 2
+  __cpu2_start,
+#endif
+#if CONFIG_SMP_NCPUS > 3
+  __cpu3_start
+#endif
+};
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: imx_cpu_reset
+ *
+ * Description:
+ *   CPUn software reset
+ *
+ ****************************************************************************/
+
+#if 0 /* Not used */
+static void imx_cpu_reset(int cpu)
+{
+  uint32_t regval;
+
+  regval  = getreg32(IMX_SRC_SCR);
+  regval |= g_cpu_reset[cpu];
+  putreg32(regval, IMX_SRC_SCR);
+}
+#endif
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: imx_cpu_disable
+ *
+ * Description:
+ *   Called from CPU0 to make sure that all other CPUs are in the disabled
+ *   state.  This is a formality because the other CPUs are actually running
+ *   then we have probably already crashed.
+ *
+ * Input Parameters:
+ *   None
+ *
+ * Returned Value:
+ *   None
+ *
+ ****************************************************************************/
+
+void imx_cpu_disable(void)
+{
+  uint32_t regval;
+  uint32_t cpumask;
+
+  cpumask  = 0;
+#if CONFIG_SMP_NCPUS > 1
+  cpumask |= SRC_SCR_CORE1_ENABLE;
+#endif
+#if CONFIG_SMP_NCPUS > 2
+  cpumask |= SRC_SCR_CORE2_ENABLE;
+#endif
+#if CONFIG_SMP_NCPUS > 3
+  cpumask |= SRC_SCR_CORE3_ENABLE;
+#endif
+
+  regval  = getreg32(IMX_SRC_SCR);
+  regval &= ~cpumask;
+  putreg32(regval, IMX_SRC_SCR);
+}
+
+/****************************************************************************
+ * Name: imx_cpu_enable
+ *
+ * Description:
+ *   Called from CPU0 to enable all other CPUs.  The enabled CPUs will start
+ *   execution at __cpuN_start and, after very low-level CPU initialzation
+ *   has been performed, will branch to arm_cpu_boot()
+ *   (see arch/arm/src/armv7-a/smp.h)
+ *
+ * Input Parameters:
+ *   None
+ *
+ * Returned Value:
+ *   None
+ *
+ ****************************************************************************/
+
+void imx_cpu_enable(void)
+{
+  cpu_start_t bootaddr;
+  uintptr_t regaddr;
+  uint32_t regval;
+  int cpu;
+
+  for (cpu = 1; cpu < CONFIG_SMP_NCPUS; cpu++)
+    {
+      /* Set the start up address */
+
+      regaddr  = g_cpu_gpr[cpu];
+      bootaddr = g_cpu_boot[cpu];
+      putreg32((uint32_t)bootaddr, regaddr);
+
+      /* Then enable the CPU */
+
+      regval   = getreg32(IMX_SRC_SCR);
+      regval  |= g_cpu_ctrl[cpu];
+      putreg32(regval, IMX_SRC_SCR);
+    }
+}
+
+/****************************************************************************
+ * Name: arm_cpu_boot
+ *
+ * Description:
+ *   Continues the C-level initialization started by the assembly language
+ *   __cpu[n]_start function.  At a minimum, this function needs to initialize
+ *   interrupt handling and, perhaps, wait on WFI for arm_cpu_start() to
+ *   issue an SGI.
+ *
+ *   This function must be provided by the each ARMv7-A MCU and implement
+ *   MCU-specific initialization logic.
+ *
+ * Input parameters:
+ *   cpu - The CPU index.  This is the same value that would be obtained by
+ *      calling up_cpu_index();
+ *
+ * Returned Value:
+ *   Does not return.
+ *
+ ****************************************************************************/
+
+void arm_cpu_boot(int cpu)
+{
+  /* Initialize the Generic Interrupt Controller (GIC) for CPUn (n != 0) */
+
+  arm_gic_initialize();
+
+  /* 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.
+   */
+
+  for (; ; )
+    {
+      asm("WFI");
+    }
+}
+
+#endif /* CONFIG_SMP */
diff --git a/arch/sim/src/up_smphook.c b/arch/sim/src/up_smphook.c
index 7c008a7d3d..e871c42f0f 100644
--- a/arch/sim/src/up_smphook.c
+++ b/arch/sim/src/up_smphook.c
@@ -51,29 +51,6 @@
  * Public Functions
  ****************************************************************************/
 
-/****************************************************************************
- * Name: up_cpu_initialize
- *
- * Description:
- *   After the CPU has been started (via up_cpu_start()) the system will
- *   call back into the architecture-specific code with this function on the
- *   thread of execution of the newly started CPU.  This gives the
- *   architecture-specific a chance to perform ny initial, CPU-specific
- *   initialize on that thread.
- *
- * Input Parameters:
- *   None
- *
- * Returned Value:
- *   Zero on success; a negated errno value on failure.
- *
- ****************************************************************************/
-
-int up_cpu_initialize(void)
-{
-  return OK;
-}
-
 /****************************************************************************
  * Name: sim_smp_hook
  *
diff --git a/include/nuttx/arch.h b/include/nuttx/arch.h
index da9296af2d..02fb699744 100644
--- a/include/nuttx/arch.h
+++ b/include/nuttx/arch.h
@@ -1753,28 +1753,6 @@ int up_cpu_index(void);
 int up_cpu_start(int cpu);
 #endif
 
-/****************************************************************************
- * Name: up_cpu_initialize
- *
- * Description:
- *   After the CPU has been started (via up_cpu_start()) the system will
- *   call back into the architecture-specific code with this function on the
- *   thread of execution of the newly started CPU.  This gives the
- *   architecture-specific a chance to perform ny initial, CPU-specific
- *   initialize on that thread.
- *
- * Input Parameters:
- *   None
- *
- * Returned Value:
- *   Zero on success; a negated errno value on failure.
- *
- ****************************************************************************/
-
-#ifdef CONFIG_SMP
-int up_cpu_initialize(void);
-#endif
-
 /****************************************************************************
  * Name: up_cpu_pause
  *
diff --git a/sched/init/os_smpstart.c b/sched/init/os_smpstart.c
index f613344b5f..baf91a2e37 100644
--- a/sched/init/os_smpstart.c
+++ b/sched/init/os_smpstart.c
@@ -98,13 +98,7 @@ void os_idle_trampoline(void)
 {
 #ifdef CONFIG_SCHED_INSTRUMENTATION
   FAR struct tcb_s *tcb = this_task();
-#endif
 
-  /* Perform architecture-specific initialization for this CPU */
-
-  up_cpu_initialize();
-
-#ifdef CONFIG_SCHED_INSTRUMENTATION
   /* Announce that the IDLE task has started */
 
   sched_note_start(tcb);