From e4a02bc216062679e434df095ccc56feef753f50 Mon Sep 17 00:00:00 2001 From: Preetam Das Date: Tue, 24 Feb 2026 05:53:10 +0530 Subject: [PATCH] bsps/raspberrypi5: ADD SMP support for Pi5 BSP This updates the PI5 BSP to have SMP Support using PSCI. --- bsps/aarch64/raspberrypi5/include/bsp.h | 3 ++ .../start/bspsmp-arm-psci-targetcpu.c | 47 +++++++++++++++++ .../raspberrypi5/start/bspstarthooks.c | 26 +++++++++- bsps/aarch64/raspberrypi5/start/bspstartmmu.c | 15 ++++++ .../raspberrypi5/start/start-cpu-mpidr.S | 52 +++++++++++++++++++ .../aarch64/raspberrypi5/bspraspberrypi5.yml | 4 +- .../bsps/aarch64/raspberrypi5/objsmp.yml | 17 ++++++ spec/build/cpukit/optsmp.yml | 1 + 8 files changed, 163 insertions(+), 2 deletions(-) create mode 100644 bsps/aarch64/raspberrypi5/start/bspsmp-arm-psci-targetcpu.c create mode 100644 bsps/aarch64/raspberrypi5/start/start-cpu-mpidr.S create mode 100644 spec/build/bsps/aarch64/raspberrypi5/objsmp.yml diff --git a/bsps/aarch64/raspberrypi5/include/bsp.h b/bsps/aarch64/raspberrypi5/include/bsp.h index a518786576..72357da492 100644 --- a/bsps/aarch64/raspberrypi5/include/bsp.h +++ b/bsps/aarch64/raspberrypi5/include/bsp.h @@ -50,6 +50,8 @@ #include +#define BSP_CPU_ON_USES_SMC + #ifndef ASM #include @@ -65,6 +67,7 @@ extern "C" { /* Raspberry Pi 5 MMU initialization */ BSP_START_TEXT_SECTION void raspberrypi5_setup_mmu_and_cache(void); +BSP_START_TEXT_SECTION void raspberrypi5_setup_secondary_cpu_mmu_and_cache(void); #define BSP_ARM_GIC_CPUIF_BASE (BCM2712_GIC_CPUIF_BASE) #define BSP_ARM_GIC_DIST_BASE (BCM2712_GIC_DIST_BASE) diff --git a/bsps/aarch64/raspberrypi5/start/bspsmp-arm-psci-targetcpu.c b/bsps/aarch64/raspberrypi5/start/bspsmp-arm-psci-targetcpu.c new file mode 100644 index 0000000000..870629bad4 --- /dev/null +++ b/bsps/aarch64/raspberrypi5/start/bspsmp-arm-psci-targetcpu.c @@ -0,0 +1,47 @@ +/* SPDX-License-Identifier: BSD-2-Clause */ + +/** + * @file + * + * @ingroup RTEMSBSPsAArch64RaspberryPi5 + * + * @brief This source file contains the implementation of + * _AArch_Get_PSCI_target_cpu() for the Raspberry Pi 5 BSP. + */ + +/* + * Copyright (C) 2026 Preetam Das + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include + +uintptr_t _AArch_Get_PSCI_target_cpu( uint32_t cpu_index ) +{ + /* The BCM2712 places its core id in AFF1, and + * the rest of the affinity levels are zero. + */ + uintptr_t target_cpu = ( (uintptr_t) cpu_index << 8 ); + + return target_cpu; +} diff --git a/bsps/aarch64/raspberrypi5/start/bspstarthooks.c b/bsps/aarch64/raspberrypi5/start/bspstarthooks.c index 6d29f09924..f9959bef23 100644 --- a/bsps/aarch64/raspberrypi5/start/bspstarthooks.c +++ b/bsps/aarch64/raspberrypi5/start/bspstarthooks.c @@ -37,6 +37,13 @@ #include #include +#ifdef RTEMS_SMP +#include +#include + +#include +#endif + #ifdef BSP_START_ENABLE_EL3_START_SUPPORT BSP_START_TEXT_SECTION void bsp_start_hook_0(void) { @@ -47,8 +54,25 @@ BSP_START_TEXT_SECTION void bsp_start_hook_0(void) BSP_START_TEXT_SECTION void bsp_start_hook_1(void) { AArch64_start_set_vector_base(); - bsp_start_copy_sections(); +#ifdef RTEMS_SMP + uint32_t cpu_index_self; + + cpu_index_self = _SMP_Get_current_processor(); + + if ( cpu_index_self != 0 ) { + raspberrypi5_setup_secondary_cpu_mmu_and_cache(); + arm_gic_irq_initialize_secondary_cpu(); + + bsp_interrupt_vector_enable( ARM_GIC_IRQ_SGI_0 ); + _SMP_Start_multitasking_on_secondary_processor( + _Per_CPU_Get_by_index( cpu_index_self ) + ); + __builtin_unreachable(); + } +#endif /* RTEMS_SMP */ + + bsp_start_copy_sections(); raspberrypi5_setup_mmu_and_cache(); bsp_start_clear_bss(); } diff --git a/bsps/aarch64/raspberrypi5/start/bspstartmmu.c b/bsps/aarch64/raspberrypi5/start/bspstartmmu.c index ecd3de5477..b055fd0310 100644 --- a/bsps/aarch64/raspberrypi5/start/bspstartmmu.c +++ b/bsps/aarch64/raspberrypi5/start/bspstartmmu.c @@ -59,3 +59,18 @@ raspberrypi5_setup_mmu_and_cache( void ) aarch64_mmu_enable( control ); } + +/* + * Make weak and let the user override. + */ +BSP_START_TEXT_SECTION void +raspberrypi5_setup_secondary_cpu_mmu_and_cache( void ) __attribute__ ((weak)); + +BSP_START_TEXT_SECTION void +raspberrypi5_setup_secondary_cpu_mmu_and_cache( void ) +{ + aarch64_mmu_control *control = &aarch64_mmu_instance; + + aarch64_mmu_setup(); + aarch64_mmu_enable( control ); +} diff --git a/bsps/aarch64/raspberrypi5/start/start-cpu-mpidr.S b/bsps/aarch64/raspberrypi5/start/start-cpu-mpidr.S new file mode 100644 index 0000000000..20d2362dc2 --- /dev/null +++ b/bsps/aarch64/raspberrypi5/start/start-cpu-mpidr.S @@ -0,0 +1,52 @@ +/* SPDX-License-Identifier: BSD-2-Clause */ + +/** + * @file + * + * @ingroup RTEMSBSPsAArch64RaspberryPi5 + * + * @brief This source file contains the implementation of + * _AArch64_Get_current_processor_for_system_start() for the + * Raspberry Pi 5 BSP. + */ + +/* + * Copyright (C) 2026 Preetam Das + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include + +FUNCTION_ENTRY(_AArch64_Get_current_processor_for_system_start) + + /* Return the affinity level 1 reported by the MPIDR_EL1. In the PI5 + * (BCM2712), the unique core identifiers are in affinity level 1 + * instead of 0. This function shifts the core IDs to the LSB portion + * to correctly return them. + */ + mrs x0, mpidr_el1 + lsr x0, x0, 8 + and x0, x0, #0xff + ret + +FUNCTION_END(_AArch64_Get_current_processor_for_system_start) diff --git a/spec/build/bsps/aarch64/raspberrypi5/bspraspberrypi5.yml b/spec/build/bsps/aarch64/raspberrypi5/bspraspberrypi5.yml index ba261bf258..b7be1b7461 100644 --- a/spec/build/bsps/aarch64/raspberrypi5/bspraspberrypi5.yml +++ b/spec/build/bsps/aarch64/raspberrypi5/bspraspberrypi5.yml @@ -51,6 +51,8 @@ links: uid: objclock - role: build-dependency uid: objconsole +- role: build-dependency + uid: objsmp source: - bsps/aarch64/raspberrypi5/start/bspstart.c - bsps/aarch64/raspberrypi5/start/bspstarthooks.c @@ -59,7 +61,7 @@ source: - bsps/aarch64/shared/cache/cache.c - bsps/aarch64/shared/mmu/mmu-setup.c - bsps/aarch64/shared/mmu/vmsav8-64.c -- bsps/aarch64/shared/start/start-cpu-mpidr.S +- bsps/aarch64/raspberrypi5/start/start-cpu-mpidr.S - bsps/shared/dev/irq/arm-gicv2-get-attributes.c - bsps/shared/dev/getentropy/getentropy-cpucounter.c - bsps/shared/dev/btimer/btimer-cpucounter.c diff --git a/spec/build/bsps/aarch64/raspberrypi5/objsmp.yml b/spec/build/bsps/aarch64/raspberrypi5/objsmp.yml new file mode 100644 index 0000000000..1244a371af --- /dev/null +++ b/spec/build/bsps/aarch64/raspberrypi5/objsmp.yml @@ -0,0 +1,17 @@ +SPDX-License-Identifier: CC-BY-SA-4.0 OR BSD-2-Clause +build-type: objects +cflags: [] +copyrights: +- Copyright (C) 2026 Preetam Das +cppflags: [] +cxxflags: [] +enabled-by: +- RTEMS_SMP +includes: [] +install: [] +links: [] +source: +- bsps/aarch64/shared/start/aarch64-smp.c +- bsps/shared/start/bspsmp-arm-psci.c +- bsps/aarch64/raspberrypi5/start/bspsmp-arm-psci-targetcpu.c +type: build diff --git a/spec/build/cpukit/optsmp.yml b/spec/build/cpukit/optsmp.yml index 09ff1e4cfa..4653bda0f8 100644 --- a/spec/build/cpukit/optsmp.yml +++ b/spec/build/cpukit/optsmp.yml @@ -14,6 +14,7 @@ description: | Enable the Symmetric Multiprocessing (SMP) support enabled-by: - aarch64/raspberrypi4b +- aarch64/raspberrypi5 - arm/altcycv_devkit - arm/fvp_cortex_r52 - arm/imx7