1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
|
#===============================================================================
# Copyright (c) 2022 Qualcomm Innovation Center, Inc. All rights reserved.
#
# SPDX-License-Identifier: BSD-2-Clause-Patent
#===============================================================================
.text
.align 3
#include <AsmMacroIoLibV8.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_EXPORT (ApEntryPoint)
// Entry-point for the AP
// VOID
// ApEntryPoint (
// VOID
// );
ASM_PFX(ApEntryPoint):
// Configure the MMU and caches
ldr x0, gTcr
bl ArmSetTCR
ldr x0, gTtbr0
bl ArmSetTTBR0
ldr 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 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 x0, gApStacksBase
ldr x1, gApStackSize
mul x3, x2, x1 // x3 = (ProcessorIndex + 1) * gApStackSize
add sp, x0, x3 // sp = gApStacksBase + x3
mov x29, xzr
bl ApProcedure // doesn't return
ProcessorNotFound:
// Turn off the processor
MOV32 (w0, ARM_SMC_ID_PSCI_CPU_OFF)
smc #0
b .
|
