| 1 | #===============================================================================
|
|---|
| 2 | # Copyright (c) 2022 Qualcomm Innovation Center, Inc. All rights reserved.
|
|---|
| 3 | #
|
|---|
| 4 | # SPDX-License-Identifier: BSD-2-Clause-Patent
|
|---|
| 5 | #===============================================================================
|
|---|
| 6 |
|
|---|
| 7 | .text
|
|---|
| 8 | .align 3
|
|---|
| 9 |
|
|---|
| 10 | #include <AsmMacroIoLibV8.h>
|
|---|
| 11 | #include <IndustryStandard/ArmStdSmc.h>
|
|---|
| 12 | #include <Library/ArmLib.h>
|
|---|
| 13 |
|
|---|
| 14 | #include "MpServicesInternal.h"
|
|---|
| 15 |
|
|---|
| 16 | GCC_ASM_IMPORT (gApStacksBase)
|
|---|
| 17 | GCC_ASM_IMPORT (gProcessorIDs)
|
|---|
| 18 | GCC_ASM_IMPORT (ApProcedure)
|
|---|
| 19 | GCC_ASM_IMPORT (gApStackSize)
|
|---|
| 20 | GCC_ASM_IMPORT (gTcr)
|
|---|
| 21 | GCC_ASM_IMPORT (gTtbr0)
|
|---|
| 22 | GCC_ASM_IMPORT (gMair)
|
|---|
| 23 |
|
|---|
| 24 | GCC_ASM_EXPORT (ApEntryPoint)
|
|---|
| 25 |
|
|---|
| 26 | // Entry-point for the AP
|
|---|
| 27 | // VOID
|
|---|
| 28 | // ApEntryPoint (
|
|---|
| 29 | // VOID
|
|---|
| 30 | // );
|
|---|
| 31 | ASM_PFX(ApEntryPoint):
|
|---|
| 32 | // Configure the MMU and caches
|
|---|
| 33 | ldr x0, gTcr
|
|---|
| 34 | bl ArmSetTCR
|
|---|
| 35 | ldr x0, gTtbr0
|
|---|
| 36 | bl ArmSetTTBR0
|
|---|
| 37 | ldr x0, gMair
|
|---|
| 38 | bl ArmSetMAIR
|
|---|
| 39 | bl ArmDisableAlignmentCheck
|
|---|
| 40 | bl ArmEnableStackAlignmentCheck
|
|---|
| 41 | bl ArmEnableInstructionCache
|
|---|
| 42 | bl ArmEnableDataCache
|
|---|
| 43 | bl ArmEnableMmu
|
|---|
| 44 |
|
|---|
| 45 | mrs x0, mpidr_el1
|
|---|
| 46 | // Mask the non-affinity bits
|
|---|
| 47 | bic x0, x0, 0x00ff000000
|
|---|
| 48 | and x0, x0, 0xffffffffff
|
|---|
| 49 | ldr x1, gProcessorIDs
|
|---|
| 50 | mov x2, 0 // x2 = processor index
|
|---|
| 51 |
|
|---|
| 52 | // Find index in gProcessorIDs for current processor
|
|---|
| 53 | 1:
|
|---|
| 54 | ldr x3, [x1, x2, lsl #3] // x4 = gProcessorIDs + x2 * 8
|
|---|
| 55 | cmp x3, #-1 // check if we've reached the end of gProcessorIDs
|
|---|
| 56 | beq ProcessorNotFound
|
|---|
| 57 | add x2, x2, 1 // x2++
|
|---|
| 58 | cmp x0, x3 // if mpidr_el1 != gProcessorIDs[x] then loop
|
|---|
| 59 | bne 1b
|
|---|
| 60 |
|
|---|
| 61 | // Calculate stack address
|
|---|
| 62 | // x2 contains the index for the current processor plus 1
|
|---|
| 63 | ldr x0, gApStacksBase
|
|---|
| 64 | ldr x1, gApStackSize
|
|---|
| 65 | mul x3, x2, x1 // x3 = (ProcessorIndex + 1) * gApStackSize
|
|---|
| 66 | add sp, x0, x3 // sp = gApStacksBase + x3
|
|---|
| 67 | mov x29, xzr
|
|---|
| 68 | bl ApProcedure // doesn't return
|
|---|
| 69 |
|
|---|
| 70 | ProcessorNotFound:
|
|---|
| 71 | // Turn off the processor
|
|---|
| 72 | MOV32 (w0, ARM_SMC_ID_PSCI_CPU_OFF)
|
|---|
| 73 | smc #0
|
|---|
| 74 | b .
|
|---|
