2 files changed, 64 insertions, 11 deletions

diff --git a/bsps/aarch64/xilinx-zynqmp/start/bspstarthooks.c b/bsps/aarch64/xilinx-zynqmp/start/bspstarthooks.c
index 7bd787592c..14f2bcc280 100644
--- a/bsps/aarch64/xilinx-zynqmp/start/bspstarthooks.c
+++ b/bsps/aarch64/xilinx-zynqmp/start/bspstarthooks.c
@@ -38,13 +38,49 @@
 #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 */
+  /* do nothing */
 }
 
-BSP_START_TEXT_SECTION void bsp_start_hook_1(void)
+BSP_START_TEXT_SECTION void bsp_start_hook_1( void )
 {
+#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 )
+    );
+    /* Unreached */
+  }
+#endif /* RTEMS_SMP */
+
   AArch64_start_set_vector_base();
   bsp_start_copy_sections();
   zynqmp_setup_mmu_and_cache();
diff --git a/bsps/aarch64/xilinx-zynqmp/start/bspstartmmu.c b/bsps/aarch64/xilinx-zynqmp/start/bspstartmmu.c
index 8d302e97b5..e727f9b1de 100644
--- a/bsps/aarch64/xilinx-zynqmp/start/bspstartmmu.c
+++ b/bsps/aarch64/xilinx-zynqmp/start/bspstartmmu.c
@@ -37,17 +37,11 @@
 #include <bsp.h>
 #include <bsp/start.h>
 #include <bsp/aarch64-mmu.h>
+#include <libcpu/mmu-vmsav8-64.h>
 
 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,
@@ -56,6 +50,10 @@ zynqmp_mmu_config_table[] = {
     .begin = 0xfd000000U,
     .end = 0xffc00000U,
     .flags = AARCH64_MMU_DEVICE
+  }, {
+    .begin = 0x80000000U,
+    .end = 0x80100000U,
+    .flags = 0
   }
 };
 
@@ -70,8 +68,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();
 }