From 8ad1188fe5abfc7362fab6a5ef40e2b43ab98ee3 Mon Sep 17 00:00:00 2001
From: Gregory Nutt <gnutt@nuttx.org>
Date: Sat, 12 Mar 2016 13:23:49 -0600
Subject: [PATCH] i.MX6: Finish initial cut at all SMP support

---
 arch/arm/src/armv7-a/arm_cpupause.c | 191 ++++++++++++++++++++++++++++
 arch/arm/src/armv7-a/arm_cpustart.c | 127 ++++++++++++++++++
 arch/arm/src/armv7-a/arm_gic.c      |  48 +++++++
 arch/arm/src/armv7-a/gic.h          |  69 ++++++++++
 arch/arm/src/imx6/Make.defs         |   2 +-
 5 files changed, 436 insertions(+), 1 deletion(-)
 create mode 100644 arch/arm/src/armv7-a/arm_cpupause.c
 create mode 100644 arch/arm/src/armv7-a/arm_cpustart.c

diff --git a/arch/arm/src/armv7-a/arm_cpupause.c b/arch/arm/src/armv7-a/arm_cpupause.c
new file mode 100644
index 0000000000..6b9bc96df3
--- /dev/null
+++ b/arch/arm/src/armv7-a/arm_cpupause.c
@@ -0,0 +1,191 @@
+/****************************************************************************
+ * arch/arm/src/armv7-a/arm_cpupause.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 <nuttx/arch.h>
+#include <nuttx/sched.h>
+#include <nuttx/spinlock.h>
+
+#include "up_internal.h"
+#include "gic.h"
+#include "sched/sched.h"
+
+#ifdef CONFIG_SMP
+
+/****************************************************************************
+ * Private Data
+ ****************************************************************************/
+
+static spinlock_t g_pause_spinlock[CONFIG_SMP_NCPUS];
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: arm_pause_handler
+ *
+ * Description:
+ *   This is the handler for SGI2.  It performs the following operations:
+ *
+ *   1. It saves the current task state at the head of the current assigned
+ *      task list.
+ *   2. It waits on a spinlock, then
+ *   3. Returns from interrupt, restoring the state of the new task at the
+ *      head of the ready to run list.
+ *
+ * Input Parameters:
+ *   Standard interrupt handling
+ *
+ * Returned Value:
+ *   Zero on success; a negated errno value on failure.
+ *
+ ****************************************************************************/
+
+int arm_pause_handler(int irq, FAR void *context)
+{
+  FAR struct tcb_s *tcb = this_task();
+  int cpu = up_cpu_index();
+
+  /* Update scheduler parameters */
+
+  sched_suspend_scheduler(tcb);
+
+  /* Save the current context at CURRENT_REGS into the TCB at the head of the
+   * assigned task list for this CPU.
+   */
+
+  up_savestate(tcb->xcp.regs);
+
+  /* Wait for the spinlock to be released */
+
+  spin_lock(&g_pause_spinlock[cpu]);
+
+  /* Restore the exception context of the tcb at the (new) head of the
+   * assigned task list.
+   */
+
+  tcb = this_task();
+
+  /* Reset scheduler parameters */
+
+  sched_resume_scheduler(tcb);
+
+  /* Then switch contexts.  Any necessary address environment changes will
+   * be made when the interrupt returns.
+   */
+
+  up_restorestate(tcb->xcp.regs);
+  spin_unlock(&g_pause_spinlock[cpu]);
+  return OK;
+}
+
+/****************************************************************************
+ * Name: up_cpu_pause
+ *
+ * Description:
+ *   Save the state of the current task at the head of the
+ *   g_assignedtasks[cpu] task list and then pause task execution on the
+ *   CPU.
+ *
+ *   This function is called by the OS when the logic executing on one CPU
+ *   needs to modify the state of the g_assignedtasks[cpu] list for another
+ *   CPU.
+ *
+ * Input Parameters:
+ *   cpu - The index of the CPU to be stopped/
+ *
+ * Returned Value:
+ *   Zero on success; a negated errno value on failure.
+ *
+ ****************************************************************************/
+
+int up_cpu_pause(int cpu)
+{
+  DEBUGASSERT(cpu >= 0 && cpu < CONFIG_SMP_NCPUS && cpu != this_cpu());
+
+  /* Take the spinlock.  The spinlock will cause the SGI2 handler to block
+   * on 'cpu'.
+   */
+
+  DEBUGASSERT(!spin_islocked(&g_pause_spinlock[cpu]));
+  spin_lock(&g_pause_spinlock[cpu]);
+
+  /* Execute SGI2 */
+
+  return arm_cpu_sgi(GIC_IRQ_SGI2, (1 << cpu));
+}
+
+/****************************************************************************
+ * Name: up_cpu_resume
+ *
+ * Description:
+ *   Restart the cpu after it was paused via up_cpu_pause(), restoring the
+ *   state of the task at the head of the g_assignedtasks[cpu] list, and
+ *   resume normal tasking.
+ *
+ *   This function is called after up_cpu_pause in order resume operation of
+ *   the CPU after modifying its g_assignedtasks[cpu] list.
+ *
+ * Input Parameters:
+ *   cpu - The index of the CPU being re-started.
+ *
+ * Returned Value:
+ *   Zero on success; a negated errno value on failure.
+ *
+ ****************************************************************************/
+
+int up_cpu_resume(int cpu)
+{
+  DEBUGASSERT(cpu >= 0 && cpu < CONFIG_SMP_NCPUS && cpu != this_cpu());
+
+  /* Release the spinlock.  Releasing the spinlock will cause the SGI2
+   * handler on 'cpu' to continue and return from interrupt to the newly
+   * established thread.
+   */
+
+  DEBUGASSERT(spin_islocked(&g_pause_spinlock[cpu]));
+  spin_unlock(&g_pause_spinlock[cpu]);
+  return OK;
+}
+
+#endif /* CONFIG_SMP */
diff --git a/arch/arm/src/armv7-a/arm_cpustart.c b/arch/arm/src/armv7-a/arm_cpustart.c
new file mode 100644
index 0000000000..302409982e
--- /dev/null
+++ b/arch/arm/src/armv7-a/arm_cpustart.c
@@ -0,0 +1,127 @@
+/****************************************************************************
+ * arch/arm/src/armv7-a/arm_cpustart.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 <nuttx/arch.h>
+#include <nuttx/sched.h>
+
+#include "up_internal.h"
+#include "gic.h"
+#include "sched/sched.h"
+
+#ifdef CONFIG_SMP
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: arm_start_handler
+ *
+ * Description:
+ *   This is the handler for SGI1.  This handler simply returns from the
+ *   interrupt, restoring the state of the new task at the head of the ready
+ *   to run list.
+ *
+ * Input Parameters:
+ *   Standard interrupt handling
+ *
+ * Returned Value:
+ *   Zero on success; a negated errno value on failure.
+ *
+ ****************************************************************************/
+
+int arm_start_handler(int irq, FAR void *context)
+{
+  FAR struct tcb_s *tcb = this_task();
+
+  /* Reset scheduler parameters */
+
+  sched_resume_scheduler(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.
+   */
+
+  up_restorestate(tcb->xcp.regs);
+  return OK;
+}
+
+/****************************************************************************
+ * Name: up_cpu_start
+ *
+ * Description:
+ *   In an SMP configution, only one CPU is initially active (CPU 0). System
+ *   initialization occurs on that single thread. At the completion of the
+ *   initialization of the OS, just before beginning normal multitasking,
+ *   the additional CPUs would be started by calling this function.
+ *
+ *   Each CPU is provided the entry point to is IDLE task when started.  A
+ *   TCB for each CPU's IDLE task has been initialized and placed in the
+ *   CPU's g_assignedtasks[cpu] list.  Not stack has been alloced or
+ *   initialized.
+ *
+ *   The OS initialization logic calls this function repeatedly until each
+ *   CPU has been started, 1 through (CONFIG_SMP_NCPUS-1).
+ *
+ * Input Parameters:
+ *   cpu - The index of the CPU being started.  This will be a numeric
+ *         value in the range of from one to (CONFIG_SMP_NCPUS-1).  (CPU
+ *         0 is already active)
+ *   idletask - The entry point to the IDLE task.
+ *
+ * Returned Value:
+ *   Zero on success; a negated errno value on failure.
+ *
+ ****************************************************************************/
+
+int up_cpu_start(int cpu, main_t idletask)
+{
+  DEBUGASSERT(cpu >= 0 && cpu < CONFIG_SMP_NCPUS && cpu != this_cpu());
+
+  /* Execute SGI1 */
+
+  return arm_cpu_sgi(GIC_IRQ_SGI1, (1 << cpu));
+}
+
+#endif /* CONFIG_SMP */
diff --git a/arch/arm/src/armv7-a/arm_gic.c b/arch/arm/src/armv7-a/arm_gic.c
index f2febac80d..641c7f6d11 100644
--- a/arch/arm/src/armv7-a/arm_gic.c
+++ b/arch/arm/src/armv7-a/arm_gic.c
@@ -41,8 +41,11 @@
 
 #include <sys/types.h>
 #include <stdint.h>
+#include <stdint.h>
+#include <assert.h>
 #include <errno.h>
 
+#include <nuttx/arch.h>
 #include <arch/irq.h>
 
 #include "up_arch.h"
@@ -126,6 +129,13 @@ void arm_gic_initialize(void)
           putreg32(0x80808080, GIC_ICDIPR(irq));   /* SPI priority */
           putreg32(0x01010101, GIC_ICDIPTR(irq));  /* SPI on CPU0 */
         }
+
+#ifdef CONFIG_SMP
+      /* Attach SGI interrupt handlers */
+
+      DEBUGVERIFY(irq_attach(GIC_IRQ_SGI1, arm_start_handler));
+      DEBUGVERIFY(irq_attach(GIC_IRQ_SGI2, arm_pause_handler));
+#endif
     }
 
   /* The remaining steps need to be done by all CPUs */
@@ -390,4 +400,42 @@ int up_prioritize_irq(int irq, int priority)
   return -EINVAL;
 }
 
+/****************************************************************************
+ * Name: arm_cpu_sgi
+ *
+ * Description:
+ *   Perform a Software Generated Interrupt (SGI).  If CONFIG_SMP is
+ *   selected, then the SGI is sent to all CPUs specified in the CPU set.
+ *   That set may include the current CPU.
+ *
+ *   If CONFIG_SMP is not selected, the cpuset is ignored and SGI is sent
+ *   only to the current CPU.
+ *
+ * Input Paramters
+ *   sgi    - The SGI interrupt ID (0-15)
+ *   cpuset - The set of CPUs to receive the SGI
+ *
+ * Returned Value:
+ *   OK is always retured at present.
+ *
+ ****************************************************************************/
+
+int arm_cpu_sgi(int sgi, unsigned int cpuset)
+{
+  uint32_t regval;
+
+  DEBUGASSERT(cpu >= 0 && cpu < CONFIG_SMP_NCPUS);
+
+#if CONFIG_SMP
+  regval = GIC_ICDSGIR_INTID(sgi) |  GIC_ICDSGIR_CPUTARGET(cpuset) |
+           GIC_ICDSGIR_TGTFILTER_LIST;
+#else
+  regval = GIC_ICDSGIR_INTID(sgi) |  GIC_ICDSGIR_CPUTARGET(0) |
+           GIC_ICDSGIR_TGTFILTER_THIS;
+#endif
+
+  putreg32(regval, GIC_ICDSGIR);
+  return OK;
+}
+
 #endif /* CONFIG_ARMV7A_HAVE_GIC */
diff --git a/arch/arm/src/armv7-a/gic.h b/arch/arm/src/armv7-a/gic.h
index 69c27c2f1b..cfa48aec32 100644
--- a/arch/arm/src/armv7-a/gic.h
+++ b/arch/arm/src/armv7-a/gic.h
@@ -438,6 +438,9 @@
  * CPU ID when it deals with SGIs. The priority of an SGI depends on the
  * value set by the receiving Cortex-A9 processor in the banked SGI priority
  * registers, not the priority set by the sending Cortex-A9 processor.
+ *
+ * NOTE: If CONFIG_SMP is enabled then SGI1 and SGI2 are used for inter-CPU
+ * task management.
  */
 
 #define GIC_IRQ_SGI0              0  /* Sofware Generated Interrupt (SGI) 0 */
@@ -513,6 +516,72 @@ void arm_gic_initialize(void);
 
 uint32_t *arm_decodeirq(uint32_t *regs);
 
+/****************************************************************************
+ * Name: arm_cpu_sgi
+ *
+ * Description:
+ *   Perform a Software Generated Interrupt (SGI).  If CONFIG_SMP is
+ *   selected, then the SGI is sent to all CPUs specified in the CPU set.
+ *   That set may include the current CPU.
+ *
+ *   If CONFIG_SMP is not selected, the cpuset is ignored and SGI is sent
+ *   only to the current CPU.
+ *
+ * Input Paramters
+ *   sgi    - The SGI interrupt ID (0-15)
+ *   cpuset - The set of CPUs to receive the SGI
+ *
+ * Returned Value:
+ *   OK is always retured at present.
+ *
+ ****************************************************************************/
+
+int arm_cpu_sgi(int sgi, unsigned int cpuset);
+
+/****************************************************************************
+ * Name: arm_start_handler
+ *
+ * Description:
+ *   This is the handler for SGI1.  This handler simply returns from the
+ *   interrupt, restoring the state of the new task at the head of the ready
+ *   to run list.
+ *
+ * Input Parameters:
+ *   Standard interrupt handling
+ *
+ * Returned Value:
+ *   Zero on success; a negated errno value on failure.
+ *
+ ****************************************************************************/
+
+#ifdef CONFIG_SMP
+int arm_start_handler(int irq, FAR void *context);
+#endif
+
+/****************************************************************************
+ * Name: arm_pause_handler
+ *
+ * Description:
+ *   This is the handler for SGI2.  It performs the following operations:
+ *
+ *   1. It saves the current task state at the head of the current assigned
+ *      task list.
+ *   2. It waits on a spinlock, then
+ *   3. Returns from interrupt, restoring the state of the new task at the
+ *      head of the ready to run list.
+ *
+ * Input Parameters:
+ *   Standard interrupt handling
+ *
+ * Returned Value:
+ *   Zero on success; a negated errno value on failure.
+ *
+ ****************************************************************************/
+
+#ifdef CONFIG_SMP
+int arm_pause_handler(int irq, FAR void *context);
+#endif
+
 #undef EXTERN
 #ifdef __cplusplus
 }
diff --git a/arch/arm/src/imx6/Make.defs b/arch/arm/src/imx6/Make.defs
index 7089b96079..7b5947fb53 100644
--- a/arch/arm/src/imx6/Make.defs
+++ b/arch/arm/src/imx6/Make.defs
@@ -77,7 +77,7 @@ CMN_CSRCS += arm_schedulesigaction.c arm_sigdeliver.c arm_syscall.c
 CMN_CSRCS += arm_unblocktask.c arm_undefinedinsn.c
 
 ifeq ($(CONFIG_SMP),y)
-CMN_CSRCS += arm_cpuindex.c
+CMN_CSRCS += arm_cpuindex.c arm_cpustart.c arm_cpupause.c
 endif
 
 # Use common heap allocation for now (may need to be customized later)