1 files changed, 39 insertions, 3 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();