From 2be53a9335e45e33651bcbaa8d099d91860f6d80 Mon Sep 17 00:00:00 2001
From: Masayuki Ishikawa <masayuki.ishikawa@gmail.com>
Date: Tue, 6 Oct 2020 18:10:58 +0900
Subject: [PATCH] arch: cxd56xx: Add interrupt stack for SMP

Summary:
- This commit adds interrupt stack for SMP

Impact:
- Affects SMP only

Testing:
- Tested with spresense:wifi_smp with CONFIG_ARCH_INTERRUPTSTACK=2048

Signed-off-by: Masayuki Ishikawa <Masayuki.Ishikawa@jp.sony.com>
---
 arch/arm/src/cxd56xx/chip.h      | 64 ++++++++++++++++++++++++++++++++
 arch/arm/src/cxd56xx/cxd56_irq.c | 57 ++++++++++++++++++++++++++++
 2 files changed, 121 insertions(+)

diff --git a/arch/arm/src/cxd56xx/chip.h b/arch/arm/src/cxd56xx/chip.h
index 33240c5b08..c25c9bdf65 100644
--- a/arch/arm/src/cxd56xx/chip.h
+++ b/arch/arm/src/cxd56xx/chip.h
@@ -42,6 +42,10 @@
 
 #include <nuttx/config.h>
 
+#ifndef __ASSEMBLY__
+#  include <nuttx/arch.h>
+#endif
+
 /* Include the chip capabilities file */
 
 #include <arch/cxd56xx/chip.h>
@@ -50,4 +54,64 @@
 
 #include "hardware/cxd5602_memorymap.h"
 
+#if defined(CONFIG_SMP) && CONFIG_ARCH_INTERRUPTSTACK > 7
+#  include "cxd56_cpuindex.h"
+#endif
+
+/****************************************************************************
+ * Macro Definitions
+ ****************************************************************************/
+
+#ifdef __ASSEMBLY__
+
+/****************************************************************************
+ * Name: setintstack
+ *
+ * Description:
+ *   Set the current stack pointer to the  "top" the correct interrupt stack
+ *   for the current CPU.
+ *
+ ****************************************************************************/
+
+#if defined(CONFIG_SMP) && CONFIG_ARCH_INTERRUPTSTACK > 7
+  .macro  setintstack, tmp1, tmp2
+#if CONFIG_SMP_NCPUS > 1
+  ldr \tmp1, =CXD56_ADSP_PID
+  ldr \tmp1, [\tmp1, 0]
+  sub \tmp1, 2                   /* tmp1 = getreg32(CXD56_ADSP_PID) - 2 */
+  ldr \tmp2, =g_cpu_intstack_top
+  ldr sp, [\tmp2, \tmp1, lsl #2] /* sp = g_cpu_intstack_top[tmp1] */
+#endif
+  .endm
+#endif /* CONFIG_SMP && CONFIG_ARCH_INTERRUPTSTACK > 7 */
+
+#endif /* __ASSEMBLY__  */
+
+/****************************************************************************
+ * Public Data
+ ****************************************************************************/
+
+#ifndef __ASSEMBLY__
+#if defined(__cplusplus)
+#define EXTERN extern "C"
+extern "C"
+{
+#else
+#define EXTERN extern
+#endif
+
+/****************************************************************************
+ * Public Functions Prototypes
+ ****************************************************************************/
+
+#if defined(CONFIG_SMP) && CONFIG_ARCH_INTERRUPTSTACK > 7
+uintptr_t arm_intstack_base(void);
+#endif
+
+#undef EXTERN
+#if defined(__cplusplus)
+}
+#endif
+
+#endif /* __ASSEMBLY__ */
 #endif /* __ARCH_ARM_SRC_CXD56XX_CHIP_H */
diff --git a/arch/arm/src/cxd56xx/cxd56_irq.c b/arch/arm/src/cxd56xx/cxd56_irq.c
index 8a562f659b..805e5d59fd 100644
--- a/arch/arm/src/cxd56xx/cxd56_irq.c
+++ b/arch/arm/src/cxd56xx/cxd56_irq.c
@@ -72,6 +72,13 @@
 
 #define INTC_EN(n) (CXD56_INTC_BASE + 0x10 + (((n) >> 5) << 2))
 
+/* Interrupt stack definitions for SMP */
+
+#if defined(CONFIG_SMP) && CONFIG_ARCH_INTERRUPTSTACK > 7
+#  define INTSTACK_SIZE  CONFIG_ARCH_INTERRUPTSTACK
+#  define INTSTACK_ALLOC (CONFIG_SMP_NCPUS * INTSTACK_SIZE)
+#endif
+
 /****************************************************************************
  * Public Data
  ****************************************************************************/
@@ -97,6 +104,36 @@ static volatile int8_t g_cpu_for_irq[CXD56_IRQ_NIRQS];
 extern void up_send_irqreq(int idx, int irq, int cpu);
 #endif
 
+#if defined(CONFIG_SMP) && CONFIG_ARCH_INTERRUPTSTACK > 7
+/* In the SMP configuration, we will need custom interrupt stacks.
+ * These definitions provide the aligned stack allocations.
+ */
+
+static uint64_t g_intstack_alloc[INTSTACK_ALLOC >> 3];
+
+/* These definitions provide the "top" of the push-down stacks. */
+
+const uint32_t g_cpu_intstack_top[CONFIG_SMP_NCPUS] =
+{
+  (uint32_t)g_intstack_alloc + INTSTACK_SIZE,
+#if CONFIG_SMP_NCPUS > 1
+  (uint32_t)g_intstack_alloc + (2 * INTSTACK_SIZE),
+#if CONFIG_SMP_NCPUS > 2
+  (uint32_t)g_intstack_alloc + (3 * INTSTACK_SIZE),
+#if CONFIG_SMP_NCPUS > 3
+  (uint32_t)g_intstack_alloc + (4 * INTSTACK_SIZE),
+#if CONFIG_SMP_NCPUS > 4
+  (uint32_t)g_intstack_alloc + (5 * INTSTACK_SIZE),
+#if CONFIG_SMP_NCPUS > 5
+  (uint32_t)g_intstack_alloc + (6 * INTSTACK_SIZE),
+#endif /* CONFIG_SMP_NCPUS > 5 */
+#endif /* CONFIG_SMP_NCPUS > 4 */
+#endif /* CONFIG_SMP_NCPUS > 3 */
+#endif /* CONFIG_SMP_NCPUS > 2 */
+#endif /* CONFIG_SMP_NCPUS > 1 */
+};
+#endif /* defined(CONFIG_SMP) && CONFIG_ARCH_INTERRUPTSTACK > 7 */
+
 /* This is the address of the  exception vector table (determined by the
  * linker script).
  */
@@ -604,3 +641,23 @@ int up_prioritize_irq(int irq, int priority)
   return OK;
 }
 #endif
+
+/****************************************************************************
+ * Name: arm_intstack_base
+ *
+ * Description:
+ *   Return a pointer to the "base" the correct interrupt stack allocation
+ *   for the  current CPU.
+ *
+ ****************************************************************************/
+
+#if defined(CONFIG_SMP) && CONFIG_ARCH_INTERRUPTSTACK > 7
+uintptr_t arm_intstack_base(void)
+{
+  uintptr_t base = (uintptr_t)g_intstack_alloc;
+  uint32_t cpu = up_cpu_index();
+  base += cpu * INTSTACK_SIZE;
+
+  return base;
+}
+#endif