From 078a0486f5e2cdd9972b375bad052dd53c742e18 Mon Sep 17 00:00:00 2001
From: ligd <liguiding1@xiaomi.com>
Date: Tue, 27 Sep 2022 00:53:28 +0800
Subject: [PATCH] armv7-a: SMP hande all cores start at same time

In SMP mode, if all cores start at same time, all from __start(),
then only primary need do initialize, so others core should wait
primary, use 'sev' let the non-primary continue to __cpuN_start().

Signed-off-by: ligd <liguiding1@xiaomi.com>
---
 arch/arm/src/armv7-a/arm_head.S | 26 ++++++++++++++++++++++++++
 arch/arm/src/armv7-a/arm_scu.c  |  4 ++++
 arch/arm/src/armv7-a/barriers.h |  6 ++++--
 3 files changed, 34 insertions(+), 2 deletions(-)

diff --git a/arch/arm/src/armv7-a/arm_head.S b/arch/arm/src/armv7-a/arm_head.S
index fe41d4eb9d..0ce90ec415 100644
--- a/arch/arm/src/armv7-a/arm_head.S
+++ b/arch/arm/src/armv7-a/arm_head.S
@@ -172,6 +172,32 @@
 	.type	__start, #function
 
 __start:
+#if defined(CONFIG_SMP) && CONFIG_SMP_NCPUS > 1
+	/* Get cpuindex, cpu0 continue boot, others wait event from cpu0 */
+
+	mrc		CP15_MPIDR(r0)
+	and		r0, r0, #0x3
+	cmp		r0, #0
+	beq		__cpu0_start
+	wfe
+	cmp		r0, #1
+	beq		__cpu1_start
+#  if CONFIG_SMP_NCPUS > 2
+	cmp		r0, #2
+	beq		__cpu2_start
+#  endif
+#  if CONFIG_SMP_NCPUS > 3
+	cmp		r0, #3
+	beq		__cpu3_start
+#  endif
+#  if CONFIG_SMP_NCPUS > 4
+	cmp		r0, #4
+	beq		__cpu4_start
+#  endif
+
+__cpu0_start:
+#endif
+
 	/* Make sure that we are in SYS mode with IRQs and FIQs disabled */
 
 	cpsid		if, #PSR_MODE_SYS
diff --git a/arch/arm/src/armv7-a/arm_scu.c b/arch/arm/src/armv7-a/arm_scu.c
index 3d717d9bc8..6c5b0198a4 100644
--- a/arch/arm/src/armv7-a/arm_scu.c
+++ b/arch/arm/src/armv7-a/arm_scu.c
@@ -80,6 +80,10 @@ void arm_enable_smp(int cpu)
       regval  = getreg32(SCU_CTRL);
       regval |= SCU_CTRL_ENABLE;
       putreg32(regval, SCU_CTRL);
+
+      /* Initialize done, kick other cpus which waiting on __start */
+
+      ARM_SEV();
     }
 
   /* Actions for other CPUs */
diff --git a/arch/arm/src/armv7-a/barriers.h b/arch/arm/src/armv7-a/barriers.h
index b3a242c52f..e8b5d94f93 100644
--- a/arch/arm/src/armv7-a/barriers.h
+++ b/arch/arm/src/armv7-a/barriers.h
@@ -34,11 +34,13 @@
 #define arm_isb(n) __asm__ __volatile__ ("isb " #n : : : "memory")
 #define arm_dsb(n) __asm__ __volatile__ ("dsb " #n : : : "memory")
 #define arm_dmb(n) __asm__ __volatile__ ("dmb " #n : : : "memory")
-#define arm_nop(n) __asm__ __volatile__ ("nop\n")
+#define arm_nop()  __asm__ __volatile__ ("nop\n")
+#define arm_sev()  __asm__ __volatile__ ("sev\n")
 
 #define ARM_DSB()  arm_dsb(15)
 #define ARM_ISB()  arm_isb(15)
 #define ARM_DMB()  arm_dmb(15)
-#define ARM_NOP()  arm_nop(15)
+#define ARM_NOP()  arm_nop()
+#define ARM_SEV()  arm_sev()
 
 #endif /* __ARCH_ARM_SRC_ARMV7_A_BARRIERS_H */