cpukit: Add AArch64 SMP Support

This adds SMP support for AArch64 in cpukit and for the ZynqMP BSPs.

7 files changed, 165 insertions, 26 deletions

diff --git a/bsps/aarch64/include/bsp/aarch64-mmu.h b/bsps/aarch64/include/bsp/aarch64-mmu.h
index e82012576f..a5f6e846f3 100644
--- a/bsps/aarch64/include/bsp/aarch64-mmu.h
+++ b/bsps/aarch64/include/bsp/aarch64-mmu.h
@@ -385,17 +385,14 @@ BSP_START_TEXT_SECTION static inline void aarch64_mmu_setup_translation_table(
 }
 
 BSP_START_TEXT_SECTION static inline void
-aarch64_mmu_setup_translation_table_and_enable(
-  const aarch64_mmu_config_entry *config_table,
-  size_t config_count
-)
+aarch64_mmu_enable( void )
 {
   uint64_t sctlr;
 
-  aarch64_mmu_setup_translation_table(
-    config_table,
-    config_count
-  );
+  /* CPUECTLR_EL1.SMPEN is already set on ZynqMP and is not writable */
+
+  /* Invalidate cache */
+  rtems_cache_invalidate_entire_data();
 
   /* Enable MMU and cache */
   sctlr = _AArch64_Read_sctlr_el1();
diff --git a/bsps/aarch64/shared/start/aarch64-smp.c b/bsps/aarch64/shared/start/aarch64-smp.c
new file mode 100644
index 0000000000..5ec7babce7
--- /dev/null
+++ b/bsps/aarch64/shared/start/aarch64-smp.c
@@ -0,0 +1,85 @@
+/* SPDX-License-Identifier: BSD-2-Clause */
+
+/**
+ * @file
+ *
+ * @ingroup RTEMSBSPsAArch64Shared
+ *
+ * @brief SMP startup and interop code.
+ */
+
+/*
+ * Copyright (C) 2021 On-Line Applications Research Corporation (OAR)
+ * Written by Kinsey Moore <kinsey.moore@oarcorp.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/score/smpimpl.h>
+
+#include <bsp/irq.h>
+
+static void bsp_inter_processor_interrupt( void *arg )
+{
+  _SMP_Inter_processor_interrupt_handler( _Per_CPU_Get() );
+}
+
+uint32_t _CPU_SMP_Initialize( void )
+{
+  return arm_gic_irq_processor_count();
+}
+
+void _CPU_SMP_Finalize_initialization( uint32_t cpu_count )
+{
+  if ( cpu_count > 0 ) {
+    rtems_status_code sc;
+
+    sc = rtems_interrupt_handler_install(
+      ARM_GIC_IRQ_SGI_0,
+      "IPI",
+      RTEMS_INTERRUPT_UNIQUE,
+      bsp_inter_processor_interrupt,
+      NULL
+    );
+    _Assert( sc == RTEMS_SUCCESSFUL );
+    (void) sc;
+
+#if defined( BSP_DATA_CACHE_ENABLED ) || \
+    defined( BSP_INSTRUCTION_CACHE_ENABLED )
+    /* Enable unified L2 cache */
+    rtems_cache_enable_data();
+#endif
+  }
+}
+
+void _CPU_SMP_Prepare_start_multitasking( void )
+{
+  /* Do nothing */
+}
+
+void _CPU_SMP_Send_interrupt( uint32_t target_processor_index )
+{
+  arm_gic_irq_generate_software_irq(
+    ARM_GIC_IRQ_SGI_0,
+    1U << target_processor_index
+  );
+}
diff --git a/bsps/aarch64/shared/start/start.S b/bsps/aarch64/shared/start/start.S
index f4c39dacdf..bc6a855217 100644
--- a/bsps/aarch64/shared/start/start.S
+++ b/bsps/aarch64/shared/start/start.S
@@ -166,21 +166,21 @@ _el1_start:
 #ifdef RTEMS_SMP
   /* Read MPIDR and get current processor index */
   mrs x7, mpidr_el1
-  and x7, #0xff
+  and x7, x7, #0xff
 #endif
 
 #ifdef RTEMS_SMP
   /*
    * Get current per-CPU control and store it in PL1 only Thread ID
-   * Register (TPIDRPRW).
+   * Register (TPIDR_EL1).
    */
 #ifdef AARCH64_MULTILIB_ARCH_V8_ILP32
   ldr w1, =_Per_CPU_Information
 #else
   ldr x1, =_Per_CPU_Information
 #endif
-  add x1, x1, x7, asl #PER_CPU_CONTROL_SIZE_LOG2
-  mcr p15, 0, x1, c13, c0, 4
+  add x1, x1, x7, lsl #PER_CPU_CONTROL_SIZE_LOG2
+  msr TPIDR_EL1, x1
 
 #endif
 
@@ -201,8 +201,8 @@ _el1_start:
 #endif
   add x3, x1, x2
 
-  /* Save original DAIF value */
-  mrs  x4, DAIF
+  /* Disable interrupts */
+  msr DAIFSet, #0x2
 
 #ifdef BSP_START_NEEDS_REGISTER_INITIALIZATION
   mov x8, XZR
diff --git a/bsps/aarch64/xilinx-versal/start/bspstartmmu.c b/bsps/aarch64/xilinx-versal/start/bspstartmmu.c
index 6ab33cc4f1..8b622aec7d 100644
--- a/bsps/aarch64/xilinx-versal/start/bspstartmmu.c
+++ b/bsps/aarch64/xilinx-versal/start/bspstartmmu.c
@@ -70,8 +70,10 @@ versal_setup_mmu_and_cache( void )
 {
   aarch64_mmu_setup();
 
-  aarch64_mmu_setup_translation_table_and_enable(
+  aarch64_mmu_setup_translation_table(
     &versal_mmu_config_table[ 0 ],
     RTEMS_ARRAY_SIZE( versal_mmu_config_table )
   );
+
+  aarch64_mmu_enable();
 }
diff --git a/bsps/aarch64/xilinx-zynqmp/include/bsp.h b/bsps/aarch64/xilinx-zynqmp/include/bsp.h
index 6d49b9ad2a..d937a313f2 100644
--- a/bsps/aarch64/xilinx-zynqmp/include/bsp.h
+++ b/bsps/aarch64/xilinx-zynqmp/include/bsp.h
@@ -60,6 +60,7 @@ extern "C" {
 #define BSP_ARM_GIC_DIST_BASE 0xf9010000
 
 #define BSP_RESET_SMC
+#define BSP_CPU_ON_USES_SMC
 
 /**
  * @brief Zynq UltraScale+ MPSoC specific set up of the MMU.
@@ -68,6 +69,14 @@ extern "C" {
  */
 BSP_START_TEXT_SECTION void zynqmp_setup_mmu_and_cache(void);
 
+/**
+ * @brief Zynq UltraScale+ MPSoC specific set up of the MMU for non-primary
+ * cores.
+ *
+ * Provide in the application to override the defaults in the BSP.
+ */
+BSP_START_TEXT_SECTION void zynqmp_setup_secondary_cpu_mmu_and_cache( void );
+
 void zynqmp_debug_console_flush(void);
 
 uint32_t zynqmp_clock_i2c0(void);
diff --git a/bsps/aarch64/xilinx-zynqmp/start/bspstarthooks.c b/bsps/aarch64/xilinx-zynqmp/start/bspstarthooks.c
index 7bd787592c..ad688088e5 100644
--- a/bsps/aarch64/xilinx-zynqmp/start/bspstarthooks.c
+++ b/bsps/aarch64/xilinx-zynqmp/start/bspstarthooks.c
@@ -38,12 +38,46 @@
 #include <bsp.h>
 #include <bsp/start.h>
 
-BSP_START_TEXT_SECTION void bsp_start_hook_0(void)
+#ifdef RTEMS_SMP
+#include <rtems/score/aarch64-system-registers.h>
+#include <rtems/score/smpimpl.h>
+
+#include <bsp/irq-generic.h>
+#endif
+
+BSP_START_TEXT_SECTION void bsp_start_hook_0( void )
 {
-  /* Do nothing */
+#ifdef RTEMS_SMP
+  uint32_t cpu_index_self;
+
+  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();
+      }
+    }
+
+    /* Change the VBAR from the start to the normal vector table */
+    AArch64_start_set_vector_base();
+
+    zynqmp_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 )
+    );
+  }
+
+#endif
 }
 
-BSP_START_TEXT_SECTION void bsp_start_hook_1(void)
+BSP_START_TEXT_SECTION void bsp_start_hook_1( void )
 {
   AArch64_start_set_vector_base();
   bsp_start_copy_sections();
diff --git a/bsps/aarch64/xilinx-zynqmp/start/bspstartmmu.c b/bsps/aarch64/xilinx-zynqmp/start/bspstartmmu.c
index 8d302e97b5..09012c9db5 100644
--- a/bsps/aarch64/xilinx-zynqmp/start/bspstartmmu.c
+++ b/bsps/aarch64/xilinx-zynqmp/start/bspstartmmu.c
@@ -41,13 +41,6 @@
 BSP_START_DATA_SECTION static const aarch64_mmu_config_entry
 zynqmp_mmu_config_table[] = {
   AARCH64_MMU_DEFAULT_SECTIONS,
-#if defined( RTEMS_SMP )
-  {
-    .begin = 0xffff0000U,
-    .end = 0xffffffffU,
-    .flags = AARCH64_MMU_DEVICE
-  },
-#endif
   {
     .begin = 0xf9000000U,
     .end = 0xf9100000U,
@@ -70,8 +63,27 @@ zynqmp_setup_mmu_and_cache( void )
 {
   aarch64_mmu_setup();
 
-  aarch64_mmu_setup_translation_table_and_enable(
+  aarch64_mmu_setup_translation_table(
     &zynqmp_mmu_config_table[ 0 ],
     RTEMS_ARRAY_SIZE( zynqmp_mmu_config_table )
   );
+
+  aarch64_mmu_enable();
+}
+
+/*
+ * Make weak and let the user override.
+ */
+BSP_START_TEXT_SECTION void zynqmp_setup_secondary_cpu_mmu_and_cache( void )
+__attribute__ ( ( weak ) );
+
+BSP_START_TEXT_SECTION void zynqmp_setup_secondary_cpu_mmu_and_cache( void )
+{
+  /* 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();
 }