From 5f652cb27e0134362e0160135124352539315845 Mon Sep 17 00:00:00 2001 From: Kinsey Moore Date: Mon, 26 Jul 2021 15:43:00 -0500 Subject: cpukit: Add AArch64 SMP Support This adds SMP support for AArch64 in cpukit and for the ZynqMP BSPs. --- bsps/aarch64/include/bsp/aarch64-mmu.h | 13 ++-- bsps/aarch64/shared/start/aarch64-smp.c | 85 ++++++++++++++++++++++++ bsps/aarch64/shared/start/start.S | 12 ++-- bsps/aarch64/xilinx-versal/start/bspstartmmu.c | 4 +- bsps/aarch64/xilinx-zynqmp/include/bsp.h | 9 +++ bsps/aarch64/xilinx-zynqmp/start/bspstarthooks.c | 40 ++++++++++- bsps/aarch64/xilinx-zynqmp/start/bspstartmmu.c | 28 +++++--- 7 files changed, 165 insertions(+), 26 deletions(-) create mode 100644 bsps/aarch64/shared/start/aarch64-smp.c (limited to 'bsps') 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 + * + * 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 + +#include + +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 #include -BSP_START_TEXT_SECTION void bsp_start_hook_0(void) +#ifdef RTEMS_SMP +#include +#include + +#include +#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(); } -- cgit v1.2.3