sergiotarxz/nuttx
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

arm64: ARMv8-r(Cortex-R82) support(mpid fix)

Browse Source 
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 <qinwei1@xiaomi.com>
This commit is contained in:
qinwei1 2023-02-28 21:31:23 +08:00 committed by Xiang Xiao
parent d5c6b8a94b
commit 518eb4076e
8 changed files with 148 additions and 94 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
arch/arm64/include/qemu/chip.h
View File

@ -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

2
arch/arm64/src/common/Make.defs
View File

@ -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

40
arch/arm64/src/common/arm64_arch.h
View File

@ -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 */

68
arch/arm64/src/common/arm64_cpuindex.c
View File

@ -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 */

6
arch/arm64/src/common/arm64_cpustart.c
View File

@ -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();

12
arch/arm64/src/common/arm64_gicv3.c
View File

@ -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)

60
arch/arm64/src/common/arm64_head.S
View File

@ -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

52
arch/arm64/src/qemu/qemu_boot.c
View File

@ -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
*