From 518eb4076ed4ccf46eaf7f79a38b862b30eae74b Mon Sep 17 00:00:00 2001 From: qinwei1 Date: Tue, 28 Feb 2023 21:31:23 +0800 Subject: [PATCH] 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 --- 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(-) delete mode 100644 arch/arm64/src/common/arm64_cpuindex.c 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 - -#include - -#include - -#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 *