This is an automated email from the ASF dual-hosted git repository.
xiaoxiang pushed a commit to branch master
in repository https://gitbox.apache.org/repos/asf/nuttx.git
commit 518eb4076ed4ccf46eaf7f79a38b862b30eae74b
Author: qinwei1 <qinw...@xiaomi.com>
AuthorDate: Tue Feb 28 21:31:23 2023 +0800
arm64: ARMv8-r(Cortex-R82) support(mpid fix)
Summary
Different ARM64 Core will use different Affn define, the mpidr_el1
value is not CPU number, So we need to change CPU number to mpid
and vice versa, the patch change the mpid define into platform
Signed-off-by: qinwei1 <qinw...@xiaomi.com>
---
arch/arm64/include/qemu/chip.h | 2 +-
arch/arm64/src/common/Make.defs | 2 +-
arch/arm64/src/common/arm64_arch.h | 40 ++++++++++++++++++--
arch/arm64/src/common/arm64_cpuindex.c | 68 ----------------------------------
arch/arm64/src/common/arm64_cpustart.c | 6 ++-
arch/arm64/src/common/arm64_gicv3.c | 12 ++++--
arch/arm64/src/common/arm64_head.S | 60 ++++++++++++++++++++++--------
arch/arm64/src/qemu/qemu_boot.c | 52 ++++++++++++++++++++++++++
8 files changed, 148 insertions(+), 94 deletions(-)
diff --git a/arch/arm64/include/qemu/chip.h b/arch/arm64/include/qemu/chip.h
index 6b6cae7521..f854f888bc 100644
--- a/arch/arm64/include/qemu/chip.h
+++ b/arch/arm64/include/qemu/chip.h
@@ -44,7 +44,7 @@
#define CONFIG_GICD_BASE 0x8000000
#define CONFIG_GICR_BASE 0x80a0000
-
+#define CONFIG_GICR_OFFSET 0x20000
#else
#error CONFIG_ARM_GIC_VERSION should be 2, 3 or 4
diff --git a/arch/arm64/src/common/Make.defs b/arch/arm64/src/common/Make.defs
index eea3c1d3de..afae287cf6 100644
--- a/arch/arm64/src/common/Make.defs
+++ b/arch/arm64/src/common/Make.defs
@@ -78,7 +78,7 @@ CMN_CSRCS += arm64_cpu_psci.c arm64_systemreset.c
endif
ifeq ($(CONFIG_SMP),y)
-CMN_CSRCS += arm64_cpuidlestack.c arm64_cpustart.c arm64_cpuindex.c
+CMN_CSRCS += arm64_cpuidlestack.c arm64_cpustart.c
CMN_CSRCS += arm64_cpupause.c
endif
diff --git a/arch/arm64/src/common/arm64_arch.h
b/arch/arm64/src/common/arm64_arch.h
index 61a61e6e78..dd30b3f185 100644
--- a/arch/arm64/src/common/arm64_arch.h
+++ b/arch/arm64/src/common/arm64_arch.h
@@ -156,12 +156,31 @@
#define MPIDR_AFF2_SHIFT (16)
#define MPIDR_AFF3_SHIFT (32)
+/* mpidr_el1 register, the register is define:
+ * - bit 0~7: Aff0
+ * - bit 8~15: Aff1
+ * - bit 16~23: Aff2
+ * - bit 24: MT, multithreading
+ * - bit 25~29: RES0
+ * - bit 30: U, multiprocessor/Uniprocessor
+ * - bit 31: RES1
+ * - bit 32~39: Aff3
+ * - bit 40~63: RES0
+ * Different ARM64 Core will use different Affn define, the mpidr_el1
+ * value is not CPU number, So we need to change CPU number to mpid
+ * and vice versa
+ */
+
+#define GET_MPIDR() read_sysreg(mpidr_el1)
+
#define MPIDR_AFFLVL(mpidr, aff_level) \
(((mpidr) >> MPIDR_AFF ## aff_level ## _SHIFT) & MPIDR_AFFLVL_MASK)
-#define GET_MPIDR() read_sysreg(mpidr_el1)
-#define MPIDR_TO_CORE(mpidr) MPIDR_AFFLVL((mpidr), 0)
-#define IS_PRIMARY_CORE() (!MPIDR_TO_CORE(GET_MPIDR()))
+#define MPID_TO_CORE(mpid, aff_level) \
+ (((mpid) >> MPIDR_AFF ## aff_level ## _SHIFT) & MPIDR_AFFLVL_MASK)
+
+#define CORE_TO_MPID(core, aff_level) \
+ (((core) << MPIDR_AFF ## aff_level ## _SHIFT))
/* System register interface to GICv3 */
@@ -529,6 +548,21 @@ void arm64_cpu_enable(void);
# define arm64_cpu_enable()
#endif
+/****************************************************************************
+ * Name: arm64_get_mpid
+ *
+ * Description:
+ * The function from cpu index to get cpu mpid which is reading
+ * from mpidr_el1 register. Different ARM64 Core will use different
+ * Affn define, the mpidr_el1 value is not CPU number, So we need
+ * to change CPU number to mpid and vice versa
+ *
+ ****************************************************************************/
+
+#ifdef CONFIG_SMP
+uint64_t arm64_get_mpid(int cpu);
+#endif /* CONFIG_SMP */
+
#endif /* __ASSEMBLY__ */
#endif /* ___ARCH_ARM64_SRC_COMMON_ARM64_ARCH_H */
diff --git a/arch/arm64/src/common/arm64_cpuindex.c
b/arch/arm64/src/common/arm64_cpuindex.c
deleted file mode 100644
index a0249d8273..0000000000
--- a/arch/arm64/src/common/arm64_cpuindex.c
+++ /dev/null
@@ -1,68 +0,0 @@
-/****************************************************************************
- * arch/arm64/src/common/arm64_cpuindex.c
- *
- * Licensed to the Apache Software Foundation (ASF) under one or more
- * contributor license agreements. See the NOTICE file distributed with
- * this work for additional information regarding copyright ownership. The
- * ASF licenses this file to you under the Apache License, Version 2.0 (the
- * "License"); you may not use this file except in compliance with the
- * License. You may obtain a copy of the License at
- *
- * http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
- * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
- * License for the specific language governing permissions and limitations
- * under the License.
- *
- ****************************************************************************/
-
-/****************************************************************************
- * Included Files
- ****************************************************************************/
-
-#include <nuttx/config.h>
-
-#include <stdint.h>
-
-#include <nuttx/arch.h>
-
-#include "arm64_arch.h"
-
-#ifdef CONFIG_SMP
-
-/****************************************************************************
- * Public Functions
- ****************************************************************************/
-
-/****************************************************************************
- * Name: up_cpu_index
- *
- * Description:
- * Return an index in the range of 0 through (CONFIG_SMP_NCPUS-1) that
- * corresponds to the currently executing CPU.
- *
- * If TLS is enabled, then the RTOS can get this information from the TLS
- * info structure. Otherwise, the MCU-specific logic must provide some
- * mechanism to provide the CPU index.
- *
- * Input Parameters:
- * None
- *
- * Returned Value:
- * An integer index in the range of 0 through (CONFIG_SMP_NCPUS-1) that
- * corresponds to the currently executing CPU.
- *
- ****************************************************************************/
-
-int up_cpu_index(void)
-{
- /* Read the Multiprocessor Affinity Register (MPIDR)
- * And return the CPU ID field
- */
-
- return MPIDR_TO_CORE(GET_MPIDR());
-}
-
-#endif /* CONFIG_SMP */
diff --git a/arch/arm64/src/common/arm64_cpustart.c
b/arch/arm64/src/common/arm64_cpustart.c
index 808799c8b2..926854a7f0 100644
--- a/arch/arm64/src/common/arm64_cpustart.c
+++ b/arch/arm64/src/common/arm64_cpustart.c
@@ -147,7 +147,9 @@ static void arm64_smp_init_top(void *arg)
static void arm64_start_cpu(int cpu_num, char *stack, int stack_sz,
arm64_cpustart_t fn)
{
- uint64_t cpu_mpid = cpu_num;
+ uint64_t cpu_mpid;
+
+ cpu_mpid = arm64_get_mpid(cpu_num);
#ifdef CONFIG_SCHED_INSTRUMENTATION
@@ -167,7 +169,7 @@ static void arm64_start_cpu(int cpu_num, char *stack, int
stack_sz,
/* store mpid last as this is our synchronization point */
- cpu_boot_params.mpid = cpu_num;
+ cpu_boot_params.mpid = cpu_mpid;
flush_boot_params();
diff --git a/arch/arm64/src/common/arm64_gicv3.c
b/arch/arm64/src/common/arm64_gicv3.c
index 3c23d0aa53..7b714b2383 100644
--- a/arch/arm64/src/common/arm64_gicv3.c
+++ b/arch/arm64/src/common/arm64_gicv3.c
@@ -145,6 +145,13 @@ static int gic_wait_rwp(uint32_t intid)
return 0;
}
+static inline void arm64_gic_write_irouter(uint64_t val, unsigned int intid)
+{
+ unsigned long addr = IROUTER(GET_DIST_BASE(intid), intid);
+
+ putreg64(val, addr);
+}
+
void arm64_gic_irq_set_priority(unsigned int intid, unsigned int prio,
uint32_t flags)
{
@@ -195,8 +202,7 @@ void arm64_gic_irq_enable(unsigned int intid)
if (GIC_IS_SPI(intid))
{
- putreg64(MPIDR_TO_CORE(GET_MPIDR()),
- IROUTER(GET_DIST_BASE(intid), intid));
+ arm64_gic_write_irouter(up_cpu_index(), intid);
}
}
@@ -608,7 +614,7 @@ static void arm64_gic_init(void)
cpu = this_cpu();
gic_rdists[cpu] = CONFIG_GICR_BASE +
- MPIDR_TO_CORE(GET_MPIDR()) * 0x20000;
+ up_cpu_index() * CONFIG_GICR_OFFSET;
err = gic_validate_redist_version();
if (err)
diff --git a/arch/arm64/src/common/arm64_head.S
b/arch/arm64/src/common/arm64_head.S
index 40ded82937..47f052e3e2 100644
--- a/arch/arm64/src/common/arm64_head.S
+++ b/arch/arm64/src/common/arm64_head.S
@@ -122,43 +122,65 @@ real_start:
ldr x0, =cpu_boot_params
get_cpu_id x1
- /* If the cores start up at the same time, we should atomically load and
- * store the mpid into arm64_cpu_boot_params.
+ /* The global variable cpu_boot_params is not safety to
+ * access in some case. eg. Some debugger will reboot
+ * the NuttX but not reload the whole image, so it will
+ * be not predicable for the initial value of the global
+ * value in that case
+ *
+ * get_cpu_id is safety because the CPU identification is
+ * not change in any case, so the code will judge in a
+ * very simple method:
+ * -- Primary core will go on until it want to boot the
+ * other core. For NuttX OS usage case, we can consider
+ * the CPU id (the affinity of mpidr_el1) of primary
+ * core is always 0.
+ * -- The other cores will waiting until the primary
+ * core write mpid and notify it to go on
*/
- ldaxr x2, [x0, #BOOT_PARAM_MPID]
- cmp x2, #-1
- bne 1f
-
- /* try to store x1 (mpid) */
-
- stlxr w3, x1, [x0]
-
- /* If succeed, go to primary_core */
-
- cbz w3, primary_core
+ cmp x1, #0
+ beq primary_core
/* loop until our turn comes */
1: dmb ld
- ldr x2, [x0, #BOOT_PARAM_MPID]
+ wfe
+ ldr x2, [x0, #BOOT_PARAM_MPID]
cmp x1, x2
bne 1b
/* we can now load our stack pointer value and move on */
+
ldr x24, [x0, #BOOT_PARAM_SP]
add x24, x24, #(CONFIG_IDLETHREAD_STACKSIZE)
ldr x25, =arm64_boot_secondary_c_routine
bl __reset_prep_c
+
+ PRINT(second_boot, "- Ready to Boot Second CPU\r\n")
+
b cpu_boot
primary_core:
+ /* set primary core id */
+
+ str x1, [x0, #BOOT_PARAM_MPID]
+
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,
+ * To avoid the primary core disturbed by the other cores,
+ * we need keep the other cores into WFE loop
+ */
+ get_cpu_id x1
+ cmp x1, #0
+ bne fail
+
/* load stack and entry point */
+
ldr x24, =(g_idle_stack + CONFIG_IDLETHREAD_STACKSIZE)
-#endif
+#endif /* CONFIG_SMP */
ldr x25, =arm64_boot_primary_c_routine
@@ -174,8 +196,14 @@ primary_core:
bl up_earlyserialinit
#endif
-cpu_boot:
+#ifdef CONFIG_SMP
+ PRINT(primary_boot, "- Ready to Boot Primary CPU\r\n")
+#else
PRINT(cpu_boot, "- Ready to Boot CPU\r\n")
+#endif
+
+cpu_boot:
+
/* Platform hook for highest EL */
bl arm64_el_init
diff --git a/arch/arm64/src/qemu/qemu_boot.c b/arch/arm64/src/qemu/qemu_boot.c
index aff6461b2f..f2b0873988 100644
--- a/arch/arm64/src/qemu/qemu_boot.c
+++ b/arch/arm64/src/qemu/qemu_boot.c
@@ -70,6 +70,58 @@ const struct arm_mmu_config mmu_config =
* Public Functions
****************************************************************************/
+#ifdef CONFIG_SMP
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: up_cpu_index
+ *
+ * Description:
+ * Return an index in the range of 0 through (CONFIG_SMP_NCPUS-1) that
+ * corresponds to the currently executing CPU.
+ *
+ * If TLS is enabled, then the RTOS can get this information from the TLS
+ * info structure. Otherwise, the MCU-specific logic must provide some
+ * mechanism to provide the CPU index.
+ *
+ * Input Parameters:
+ * None
+ *
+ * Returned Value:
+ * An integer index in the range of 0 through (CONFIG_SMP_NCPUS-1) that
+ * corresponds to the currently executing CPU.
+ *
+ ****************************************************************************/
+
+int up_cpu_index(void)
+{
+ /* Read the Multiprocessor Affinity Register (MPIDR)
+ * And return the CPU ID field
+ */
+
+ return MPID_TO_CORE(GET_MPIDR(), 0);
+}
+
+/****************************************************************************
+ * Name: arm64_get_mpid
+ *
+ * Description:
+ * The function from cpu index to get cpu mpid which is reading
+ * from mpidr_el1 register. Different ARM64 Core will use different
+ * Affn define, the mpidr_el1 value is not CPU number, So we need
+ * to change CPU number to mpid and vice versa
+ *
+ ****************************************************************************/
+
+uint64_t arm64_get_mpid(int cpu)
+{
+ return CORE_TO_MPID(cpu, 0);
+}
+
+#endif /* CONFIG_SMP */
/****************************************************************************
* Name: arm64_el_init
*
