bsps/aarch64/raspberrypi: Add SMP Support

bsps/aarch64/raspberrypi/include/bsp.h

@@ -54,6 +54,7 @@
 
 /*Raspberry pi MMU initialization */
 BSP_START_TEXT_SECTION void raspberrypi_4_setup_mmu_and_cache(void);
+BSP_START_TEXT_SECTION void rpi_setup_secondary_cpu_mmu_and_cache( void );
 
 #ifdef __cplusplus
 extern "C" {

bsps/aarch64/raspberrypi/include/bsp/raspberrypi.h

@@ -59,7 +59,7 @@
  * @{
  */
 
-#define BCM2711_REG(x)           (*(volatile uint64_t *)(x))
+#define BCM2711_REG(x)           (*(volatile uintptr_t *)(x))
 #define BCM2711_BIT(n)           (1 << (n))
 #define BCM2835_REG(addr)        (*(volatile uint32_t*)(addr))
 

bsps/aarch64/raspberrypi/start/bspsmp.c

@@ -0,0 +1,56 @@
+/* SPDX-License-Identifier: BSD-2-Clause */
+
+/**
+ * @file
+ *
+ * @ingroup RTEMSBSPsAArch64Raspberrypi4
+ *
+ * @brief BSP SMP Support
+ */
+
+/*
+ * Copyright (C) 2023 Mohd Noor Aman
+ *
+ *
+ * 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/score/smpimpl.h>
+#include <bsp/raspberrypi.h>
+
+#include <bsp/irq.h>
+
+static uintptr_t *cpu_addr[] =
+{
+    [0] = (uintptr_t *)0xd8,
+    [1] = (uintptr_t *)0xe0,
+    [2] = (uintptr_t *)0xe8,
+    [3] = (uintptr_t *)0xf0
+};
+
+bool _CPU_SMP_Start_processor( uint32_t cpu_index )
+{
+  BCM2711_REG(cpu_addr[cpu_index]) = (uintptr_t)_start;
+  _AARCH64_Send_event();
+  _AARCH64_Data_synchronization_barrier();
+  return true;
+}

bsps/aarch64/raspberrypi/start/bspstarthooks.c

@@ -39,7 +39,15 @@
 #include <bsp/start.h>
 #include <rtems/score/cpu.h>
 
+#ifdef RTEMS_SMP
+#include <rtems/score/aarch64-system-registers.h>
+#include <rtems/score/smpimpl.h>
+#include <rtems/score/smp.h>
+#include <bsp/irq-generic.h>
+#endif
+
 #ifdef BSP_START_ENABLE_EL3_START_SUPPORT
+
 BSP_START_TEXT_SECTION void bsp_start_hook_0(void)
 {
   /* Do nothing */
@@ -48,6 +56,27 @@ BSP_START_TEXT_SECTION void bsp_start_hook_0(void)
 
 BSP_START_TEXT_SECTION void bsp_start_hook_1(void)
 {
+#ifdef RTEMS_SMP
+  uint32_t cpu_index_self = _SMP_Get_current_processor();
+  if ( cpu_index_self != 0 ) {
+    if (
+      cpu_index_self >= rtems_configuration_get_maximum_processors()
+      || !_SMP_Should_start_processor( cpu_index_self )
+    ) {
+      while ( true ) {
+        _AARCH64_Wait_for_event();
+      }
+    }
+    AArch64_start_set_vector_base();
+    arm_gic_irq_initialize_secondary_cpu();
+    rpi_setup_secondary_cpu_mmu_and_cache();
+    bsp_interrupt_vector_enable( ARM_GIC_IRQ_SGI_0 );
+    _SMP_Start_multitasking_on_secondary_processor(
+      _Per_CPU_Get_by_index( cpu_index_self )
+    );
+  }
+#endif
+
   AArch64_start_set_vector_base();
   bsp_start_copy_sections();
   raspberrypi_4_setup_mmu_and_cache();

bsps/aarch64/raspberrypi/start/bspstartmmu.c

@@ -57,6 +57,12 @@ raspberrypi_4_mmu_config_table[] = {
     .flags = AARCH64_MMU_DEVICE
   },
 
+  { /* RPI firmware-owned addresses including spintables */
+    .begin = (unsigned)0x0,
+    .end = (unsigned)0x1000,
+    .flags = AARCH64_MMU_DEVICE
+  },
+
   { /* RPI GIC Interface address */
     .begin = 0xFF800000U,
     .end = 0xFFA00000U,
@@ -91,3 +97,19 @@ raspberrypi_4_setup_mmu_and_cache( void )
 
   aarch64_mmu_enable( control );
 }
+
+BSP_START_TEXT_SECTION void rpi_setup_secondary_cpu_mmu_and_cache( void )
+__attribute__ ( ( weak ) );
+
+BSP_START_TEXT_SECTION void rpi_setup_secondary_cpu_mmu_and_cache( void )
+{
+  aarch64_mmu_control *control = &aarch64_mmu_instance;
+
+  /* Perform basic MMU setup */
+  aarch64_mmu_setup();
+
+  /* Use the existing root page table already configured by CPU0 */
+  _AArch64_Write_ttbr0_el1( (uintptr_t) bsp_translation_table_base );
+
+  aarch64_mmu_enable( control );
+}

spec/build/bsps/aarch64/raspberrypi/abi.yml

@@ -11,6 +11,7 @@ default:
   value:
   - -mcpu=cortex-a72
   - -march=armv8-a
+  - -mno-outline-atomics
 description: |
   ABI flags
 enabled-by: true

spec/build/bsps/aarch64/raspberrypi/bspraspberrypi4.yml

@@ -25,6 +25,8 @@ links:
   uid: optspiirq
 - role: build-dependency
   uid: objclock
+- role: build-dependency
+  uid: objsmp
 - role: build-dependency
   uid: objsystemtimer
 - role: build-dependency

spec/build/bsps/aarch64/raspberrypi/linkercmds.yml

@@ -29,9 +29,9 @@ content: |
    */
 
   MEMORY {
-  RAM_MMU  : ORIGIN = 0x0, LENGTH = (0x1000 * ${AARCH64_MMU_TRANSLATION_TABLE_PAGES})
   RAM      : ORIGIN = 0x80000, LENGTH = 1024M - 0x80000 - 16M
   RAM_NOCACHE  : ORIGIN = 0x40000000 - 16M, LENGTH = 16M
+  RAM_MMU  : ORIGIN = 0x40000000, LENGTH = (0x1000 * ${AARCH64_MMU_TRANSLATION_TABLE_PAGES})
   }
 
   REGION_ALIAS ("REGION_START",          RAM);

spec/build/bsps/aarch64/raspberrypi/objsmp.yml

@@ -0,0 +1,16 @@
+SPDX-License-Identifier: CC-BY-SA-4.0 OR BSD-2-Clause
+build-type: objects
+cflags: []
+copyrights:
+- Copyright (C) 2023 Mohd Noor Aman
+cppflags: []
+cxxflags: []
+enabled-by:
+- RTEMS_SMP
+includes: []
+install: []
+links: []
+source:
+- bsps/aarch64/raspberrypi/start/bspsmp.c
+- bsps/aarch64/shared/start/aarch64-smp.c
+type: build

spec/build/cpukit/optsmp.yml

@@ -13,6 +13,7 @@ default:
 description: |
   Enable the Symmetric Multiprocessing (SMP) support
 enabled-by:
+- aarch64/raspberrypi4b
 - arm/altcycv_devkit
 - arm/fvp_cortex_r52
 - arm/imx7