| #=============================================================================== | |
| # Copyright (c) 2022 Qualcomm Innovation Center, Inc. All rights reserved. | |
| # | |
| # SPDX-License-Identifier: BSD-2-Clause-Patent | |
| #=============================================================================== | |
| .text | |
| .align 3 | |
| #include <AsmMacroLib.h> | |
| #include <IndustryStandard/ArmStdSmc.h> | |
| #include <Library/ArmLib.h> | |
| #include "MpServicesInternal.h" | |
| GCC_ASM_IMPORT (gApStacksBase) | |
| GCC_ASM_IMPORT (gProcessorIDs) | |
| GCC_ASM_IMPORT (ApProcedure) | |
| GCC_ASM_IMPORT (gApStackSize) | |
| GCC_ASM_IMPORT (gTcr) | |
| GCC_ASM_IMPORT (gTtbr0) | |
| GCC_ASM_IMPORT (gMair) | |
| GCC_ASM_IMPORT (ArmEnableVFP) | |
| GCC_ASM_EXPORT (ApEntryPoint) | |
| // Entry-point for the AP | |
| // VOID | |
| // ApEntryPoint ( | |
| // VOID | |
| // ); | |
| ASM_PFX(ApEntryPoint): | |
| // Configure the MMU and caches | |
| LDR_LIT(x0, gTcr) | |
| bl ArmSetTCR | |
| LDR_LIT(x0, gTtbr0) | |
| bl ArmSetTTBR0 | |
| LDR_LIT(x0, gMair) | |
| bl ArmSetMAIR | |
| bl ArmDisableAlignmentCheck | |
| bl ArmEnableStackAlignmentCheck | |
| bl ArmEnableInstructionCache | |
| bl ArmEnableDataCache | |
| bl ArmEnableMmu | |
| mrs x0, mpidr_el1 | |
| // Mask the non-affinity bits | |
| bic x0, x0, 0x00ff000000 | |
| and x0, x0, 0xffffffffff | |
| LDR_LIT(x1, gProcessorIDs) | |
| mov x2, 0 // x2 = processor index | |
| // Find index in gProcessorIDs for current processor | |
| 1: | |
| ldr x3, [x1, x2, lsl #3] // x4 = gProcessorIDs + x2 * 8 | |
| cmp x3, #-1 // check if we've reached the end of gProcessorIDs | |
| beq ProcessorNotFound | |
| add x2, x2, 1 // x2++ | |
| cmp x0, x3 // if mpidr_el1 != gProcessorIDs[x] then loop | |
| bne 1b | |
| // Calculate stack address | |
| // x2 contains the index for the current processor plus 1 | |
| LDR_LIT(x0, gApStacksBase) | |
| LDR_LIT(x1, gApStackSize) | |
| mul x3, x2, x1 // x3 = (ProcessorIndex + 1) * gApStackSize | |
| add sp, x0, x3 // sp = gApStacksBase + x3 | |
| mov x29, xzr | |
| bl ArmEnableVFP // Do not trap SIMD instructions | |
| bl ApProcedure // doesn't return | |
| ProcessorNotFound: | |
| // Turn off the processor | |
| MOV32 (w0, ARM_SMC_ID_PSCI_CPU_OFF) | |
| smc #0 | |
| b . |