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

@@ -50,6 +50,8 @@
 
 #include <bspopts.h>
 
+#define BSP_CPU_ON_USES_SMC
+
 #ifndef ASM
 
 #include <bsp/default-initial-extension.h>
@@ -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)

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 <riki10112001@gmail.com>
+ *
+ * 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 <bsp/bspsmp-arm-psci.h>
+
+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;
+}

bsps/aarch64/raspberrypi5/start/bspstarthooks.c

@@ -37,6 +37,13 @@
 #include <bsp.h>
 #include <bsp/start.h>
 
+#ifdef RTEMS_SMP
+#include <rtems/score/aarch64-system-registers.h>
+#include <rtems/score/smpimpl.h>
+
+#include <bsp/irq-generic.h>
+#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();
 }

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 );
+}

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 <riki10112001@gmail.com>
+ *
+ * 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 <rtems/asm.h>
+
+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)

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

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 <riki10112001@gmail.com>
+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

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