arm64: changing the startup of arm64 SMP from serial to parallel

arch/arm64/Kconfig

@@ -136,6 +136,17 @@ config ARCH_SINGLE_SECURITY_STATE
 		GIC or other ARM architecture feature will with different
 		configure
 
+config ARM64_SMP_BUSY_WAIT
+	bool "Busy wait when SMP boot"
+	default n
+	depends on SMP
+	---help---
+		Enables busy wait when SMP boot
+
+config ARM64_SMP_BUSY_WAIT_FLAG_ADDR
+	hex "Busy wait flag address"
+	depends on ARM64_SMP_BUSY_WAIT
+
 config ARCH_HAVE_EL3
 	bool
 	default n

arch/arm64/src/common/arm64_arch.h

@@ -133,13 +133,6 @@
 #define MODE_EL1            (0x1)
 #define MODE_EL0            (0x0)
 
-/* struct arm64_boot_params member offset for assembly code
- * struct is defined at arm64_cpustart.c
- */
-
-#define BOOT_PARAM_MPID     0
-#define BOOT_PARAM_SP       8
-
 #ifndef __ASSEMBLY__
 
 /****************************************************************************

arch/arm64/src/common/arm64_cpustart.c

@@ -54,34 +54,45 @@
  * Public data
  ****************************************************************************/
 
-typedef void (*arm64_cpustart_t)(void *data);
-
-struct arm64_boot_params
-{
-  uint64_t cpuid;
-  char *boot_sp;
-  arm64_cpustart_t func;
-  void *arg;
-  int cpu_num;
-  volatile long cpu_ready_flag;
-};
-
-volatile struct arm64_boot_params aligned_data(L1_CACHE_BYTES)
-cpu_boot_params =
-{
-  .cpuid   = -1,
-  .boot_sp = (char *)g_cpu_idlestackalloc[0],
-};
-
-volatile uint64_t *g_cpu_int_stacktop[CONFIG_SMP_NCPUS] =
+uint64_t *const g_cpu_int_stacktop[CONFIG_SMP_NCPUS] =
 {
   (uint64_t *)(g_interrupt_stacks[0] + INTSTACK_SIZE),
+#if CONFIG_SMP_NCPUS > 1
+  (uint64_t *)(g_interrupt_stacks[1] + INTSTACK_SIZE),
+#if CONFIG_SMP_NCPUS > 2
+  (uint64_t *)(g_interrupt_stacks[2] + INTSTACK_SIZE),
+#if CONFIG_SMP_NCPUS > 3
+  (uint64_t *)(g_interrupt_stacks[3] + INTSTACK_SIZE),
+#if CONFIG_SMP_NCPUS > 4
+  (uint64_t *)(g_interrupt_stacks[4] + INTSTACK_SIZE),
+#if CONFIG_SMP_NCPUS > 5
+#  error This logic needs to extended for CONFIG_SMP_NCPUS > 5
+#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 */
 };
 
 #ifdef CONFIG_ARM64_DECODEFIQ
-volatile uint64_t *g_cpu_int_fiq_stacktop[CONFIG_SMP_NCPUS] =
+uint64_t *const g_cpu_int_fiq_stacktop[CONFIG_SMP_NCPUS] =
 {
   (uint64_t *)(g_interrupt_fiq_stacks[0] + INTSTACK_SIZE),
+#if CONFIG_SMP_NCPUS > 1
+  (uint64_t *)(g_interrupt_fiq_stacks[1] + INTSTACK_SIZE),
+#if CONFIG_SMP_NCPUS > 2
+  (uint64_t *)(g_interrupt_fiq_stacks[2] + INTSTACK_SIZE),
+#if CONFIG_SMP_NCPUS > 3
+  (uint64_t *)(g_interrupt_fiq_stacks[3] + INTSTACK_SIZE),
+#if CONFIG_SMP_NCPUS > 4
+  (uint64_t *)(g_interrupt_fiq_stacks[4] + INTSTACK_SIZE),
+#if CONFIG_SMP_NCPUS > 5
+#  error This logic needs to extended for CONFIG_SMP_NCPUS > 5
+#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
 
@@ -100,20 +111,7 @@ static inline void local_delay(void)
     }
 }
 
-#if defined (CONFIG_ARCH_HAVE_MMU) || defined (CONFIG_ARCH_HAVE_MPU)
-static void flush_boot_params(void)
-{
-  uintptr_t flush_start;
-  uintptr_t flush_end;
-
-  flush_start   = (uintptr_t)&cpu_boot_params;
-  flush_end     = flush_start + sizeof(cpu_boot_params);
-
-  up_flush_dcache(flush_start, flush_end);
-}
-#endif
-
-static void arm64_smp_init_top(void *arg)
+static void arm64_smp_init_top(void)
 {
   struct tcb_s *tcb = this_task();
 
@@ -139,16 +137,14 @@ static void arm64_smp_init_top(void *arg)
   write_sysreg(0, tpidrro_el0);
   UNUSED(tcb);
 
-  cpu_boot_params.cpu_ready_flag = 1;
-  SP_SEV();
-
   nx_idle_trampoline();
 }
 
-static void arm64_start_cpu(int cpu_num, char *stack, int stack_sz,
-                            arm64_cpustart_t fn)
+static void arm64_start_cpu(int cpu_num)
 {
+#ifdef CONFIG_ARM64_PSCI
   uint64_t cpu_mpid = arm64_get_mpid(cpu_num);
+#endif
 
 #ifdef CONFIG_SCHED_INSTRUMENTATION
 
@@ -157,26 +153,6 @@ static void arm64_start_cpu(int cpu_num, char *stack, int stack_sz,
   sched_note_cpu_start(this_task(), cpu_num);
 #endif
 
-  cpu_boot_params.boot_sp   = stack;
-  cpu_boot_params.func      = fn;
-  cpu_boot_params.arg       = 0;
-  cpu_boot_params.cpu_num   = cpu_num;
-  g_cpu_int_stacktop[cpu_num] =
-            (uint64_t *)(g_interrupt_stacks[cpu_num] + INTSTACK_SIZE);
-
-#ifdef CONFIG_ARM64_DECODEFIQ
-  g_cpu_int_fiq_stacktop[cpu_num] =
-            (uint64_t *)(g_interrupt_fiq_stacks[cpu_num] + INTSTACK_SIZE);
-#endif
-
-  ARM64_DSB();
-
-  /* store mpid last as this is our synchronization point */
-
-  cpu_boot_params.cpuid = arm64_get_cpuid(cpu_mpid);
-
-  flush_boot_params();
-
 #ifdef CONFIG_ARM64_PSCI
   if (psci_cpu_on(cpu_mpid, (uint64_t)__start))
     {
@@ -231,17 +207,13 @@ int up_cpu_start(int cpu)
   sched_note_cpu_start(this_task(), cpu);
 #endif
 
-  cpu_boot_params.cpu_ready_flag = 0;
-  arm64_start_cpu(cpu, (char *)g_cpu_idlestackalloc[cpu], SMP_STACK_SIZE,
-                  arm64_smp_init_top);
+#ifdef CONFIG_ARM64_SMP_BUSY_WAIT
+  uint32_t *address = (uint32_t *)CONFIG_ARM64_SMP_BUSY_WAIT_FLAG_ADDR;
+  *address = 1;
+  up_flush_dcache((uintptr_t)address, sizeof(address));
+#endif
 
-  /* Waiting for this CPU to be boot complete */
-
-  while (!cpu_boot_params.cpu_ready_flag)
-    {
-      SP_WFE();
-      flush_boot_params();
-    }
+  arm64_start_cpu(cpu);
 
   return 0;
 }
@@ -250,9 +222,6 @@ int up_cpu_start(int cpu)
 
 void arm64_boot_secondary_c_routine(void)
 {
-  arm64_cpustart_t  func;
-  void              *arg;
-
 #ifdef CONFIG_ARCH_HAVE_MPU
   arm64_mpu_init(false);
 #endif
@@ -267,20 +236,6 @@ void arm64_boot_secondary_c_routine(void)
 
   up_perf_init(NULL);
 
-  func  = cpu_boot_params.func;
-  arg   = cpu_boot_params.arg;
-  ARM64_DSB();
-
-  /* Secondary core clears .func to announce its presence.
-   * Primary core is polling for this. We no longer own
-   * arm64_cpu_boot_params afterwards.
-   */
-
-  cpu_boot_params.func = NULL;
-
-  ARM64_DSB();
-  SP_SEV();
-
-  func(arg);
+  arm64_smp_init_top();
 }
 

arch/arm64/src/common/arm64_gicv3.c

@@ -483,10 +483,6 @@ static void gicv3_cpuif_init(void)
 
   gic_wait_rwp(0);
 
-  /* Clear pending */
-
-  putreg32(BIT64_MASK(GIC_NUM_INTR_PER_REG), ICPENDR(base, 0));
-
   /* Configure all SGIs/PPIs as G1S or G1NS depending on Zephyr
    * is run in EL1S or EL1NS respectively.
    * All interrupts will be delivered as irq

arch/arm64/src/common/arm64_head.S

@@ -122,8 +122,6 @@ real_start:
 
     msr    DAIFSet, 0xf
 #ifdef CONFIG_SMP
-
-    ldr    x0, =cpu_boot_params
     get_cpu_id x1
 
     /* The global variable cpu_boot_params is not safety to
@@ -146,17 +144,30 @@ real_start:
     cmp    x1, #0
     beq    primary_core
 
-    /* loop until our turn comes */
+    /* Wait until cpu0 notify */
 
-1:  dmb    ld
-    wfe
-    ldr    x2,  [x0, #BOOT_PARAM_MPID]
-    cmp    x1, x2
-    bne    1b
+#ifdef CONFIG_ARM64_SMP_BUSY_WAIT
+    ldr    x2, =CONFIG_ARM64_SMP_BUSY_WAIT_FLAG_ADDR
+1:
+    ldr    x3, [x2, #0]
+    cmp    x3, #0
+    beq    1b
+#endif
 
     /* we can now load our stack pointer value and move on */
 
-    ldr    x24, [x0, #BOOT_PARAM_SP]
+    ldr    x24, =g_cpu_idlestackalloc
+
+    /* g_cpu_idlestackalloc represents a continuous
+     * stack space allocated for CPUs from 0 to n.
+     * the stack top address for each CPU based on
+     * its index,x24 is the top of the stack for CPUs 0 to n.
+     */
+1:
+    sub    x1, x1, #1
+    add    x24, x24, #(SMP_STACK_SIZE)
+    cmp    x1, #0
+    bne    1b
 
 #  ifdef CONFIG_STACK_COLORATION
     /* Write a known value to the IDLE thread stack to support stack
@@ -184,11 +195,8 @@ real_start:
     b      cpu_boot
 
 primary_core:
-    /* set primary core id */
-
-    str    x1,  [x0, #BOOT_PARAM_MPID]
+    ldr    x24, =g_cpu_idlestackalloc
 
-    ldr    x24, [x0, #BOOT_PARAM_SP]
     add    x24, x24, #(CONFIG_IDLETHREAD_STACKSIZE)
 #else
     /* In some case, we need to boot one core in a SMP system,