diff options
Diffstat (limited to 'bsps/aarch64/xilinx-zynqmp/start/bspstarthooks.c')
-rw-r--r-- | bsps/aarch64/xilinx-zynqmp/start/bspstarthooks.c | 40 |
1 files changed, 37 insertions, 3 deletions
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(); |