[mirror_edk2.git] / UefiCpuPkg / Library / MpInitLib / X64 / MpFuncs.nasm

;------------------------------------------------------------------------------ ;\r
; Copyright (c) 2015 - 2023, Intel Corporation. All rights reserved.<BR>\r
; SPDX-License-Identifier: BSD-2-Clause-Patent\r
;\r
; Module Name:\r
;\r
;   MpFuncs.nasm\r
;\r
; Abstract:\r
;\r
;   This is the assembly code for MP support\r
;\r
;-------------------------------------------------------------------------------\r
\r
%include "MpEqu.inc"\r
extern ASM_PFX(InitializeFloatingPointUnits)\r
\r
%macro  OneTimeCall 1\r
    jmp     %1\r
%1 %+ OneTimerCallReturn:\r
%endmacro\r
\r
%macro  OneTimeCallRet 1\r
    jmp     %1 %+ OneTimerCallReturn\r
%endmacro\r
\r
DEFAULT REL\r
\r
SECTION .text\r
\r
;-------------------------------------------------------------------------------------\r
;RendezvousFunnelProc  procedure follows. All APs execute their procedure. This\r
;procedure serializes all the AP processors through an Init sequence. It must be\r
;noted that APs arrive here very raw...ie: real mode, no stack.\r
;ALSO THIS PROCEDURE IS EXECUTED BY APs ONLY ON 16 BIT MODE. HENCE THIS PROC\r
;IS IN MACHINE CODE.\r
;-------------------------------------------------------------------------------------\r
RendezvousFunnelProcStart:\r
; At this point CS = 0x(vv00) and ip= 0x0.\r
; Save BIST information to ebp firstly\r
\r
BITS 16\r
    mov        ebp, eax                        ; Save BIST information\r
\r
    mov        ax, cs\r
    mov        ds, ax\r
    mov        es, ax\r
    mov        ss, ax\r
    xor        ax, ax\r
    mov        fs, ax\r
    mov        gs, ax\r
\r
    mov        si,  MP_CPU_EXCHANGE_INFO_FIELD (BufferStart)\r
    mov        ebx, [si]\r
\r
    mov        si,  MP_CPU_EXCHANGE_INFO_FIELD (DataSegment)\r
    mov        edx, [si]\r
\r
    ;\r
    ; Get start address of 32-bit code in low memory (<1MB)\r
    ;\r
    mov        edi, MP_CPU_EXCHANGE_INFO_FIELD (ModeTransitionMemory)\r
\r
    mov        si, MP_CPU_EXCHANGE_INFO_FIELD (GdtrProfile)\r
o32 lgdt       [cs:si]\r
\r
    ;\r
    ; Switch to protected mode\r
    ;\r
    mov        eax, cr0                    ; Get control register 0\r
    or         eax, 000000003h             ; Set PE bit (bit #0) & MP\r
    mov        cr0, eax\r
\r
    ; Switch to 32-bit code (>1MB)\r
o32 jmp far    [cs:di]\r
\r
;\r
; Following code must be copied to memory with type of EfiBootServicesCode.\r
; This is required if NX is enabled for EfiBootServicesCode of memory.\r
;\r
BITS 32\r
Flat32Start:                                   ; protected mode entry point\r
    mov        ds, dx\r
    mov        es, dx\r
    mov        fs, dx\r
    mov        gs, dx\r
    mov        ss, dx\r
\r
    ;\r
    ; Enable execute disable bit\r
    ;\r
    mov        esi, MP_CPU_EXCHANGE_INFO_FIELD (EnableExecuteDisable)\r
    cmp        byte [ebx + esi], 0\r
    jz         SkipEnableExecuteDisableBit\r
\r
    mov        ecx, 0c0000080h             ; EFER MSR number\r
    rdmsr                                  ; Read EFER\r
    bts        eax, 11                     ; Enable Execute Disable Bit\r
    wrmsr                                  ; Write EFER\r
\r
SkipEnableExecuteDisableBit:\r
    ;\r
    ; Enable PAE\r
    ;\r
    mov        eax, cr4\r
    bts        eax, 5\r
\r
    mov        esi, MP_CPU_EXCHANGE_INFO_FIELD (Enable5LevelPaging)\r
    cmp        byte [ebx + esi], 0\r
    jz         SkipEnable5LevelPaging\r
\r
    ;\r
    ; Enable 5 Level Paging\r
    ;\r
    bts        eax, 12                     ; Set LA57=1.\r
\r
SkipEnable5LevelPaging:\r
\r
    mov        cr4, eax\r
\r
    ;\r
    ; Load page table\r
    ;\r
    mov        esi, MP_CPU_EXCHANGE_INFO_FIELD (Cr3)             ; Save CR3 in ecx\r
    mov        ecx, [ebx + esi]\r
    mov        cr3, ecx                    ; Load CR3\r
\r
    ;\r
    ; Enable long mode\r
    ;\r
    mov        ecx, 0c0000080h             ; EFER MSR number\r
    rdmsr                                  ; Read EFER\r
    bts        eax, 8                      ; Set LME=1\r
    wrmsr                                  ; Write EFER\r
\r
    ;\r
    ; Enable paging\r
    ;\r
    mov        eax, cr0                    ; Read CR0\r
    bts        eax, 31                     ; Set PG=1\r
    mov        cr0, eax                    ; Write CR0\r
\r
    ;\r
    ; Far jump to 64-bit code\r
    ;\r
    mov        edi, MP_CPU_EXCHANGE_INFO_FIELD (ModeHighMemory)\r
    add        edi, ebx\r
    jmp far    [edi]\r
\r
BITS 64\r
\r
LongModeStart:\r
    mov        esi, ebx\r
\r
    ; Set IDT table at the start of 64 bit code\r
    lea        edi, [esi + MP_CPU_EXCHANGE_INFO_FIELD (IdtrProfile)]\r
    lidt       [edi]\r
\r
    lea        edi, [esi + MP_CPU_EXCHANGE_INFO_FIELD (InitFlag)]\r
    cmp        qword [edi], 1       ; ApInitConfig\r
    jnz        GetApicId\r
\r
    ; Increment the number of APs executing here as early as possible\r
    ; This is decremented in C code when AP is finished executing\r
    mov        edi, esi\r
    add        edi, MP_CPU_EXCHANGE_INFO_FIELD (NumApsExecuting)\r
    lock inc   dword [edi]\r
\r
    ; AP init\r
    mov        edi, esi\r
    add        edi, MP_CPU_EXCHANGE_INFO_FIELD (ApIndex)\r
    mov        ebx, 1\r
    lock xadd  dword [edi], ebx                 ; EBX = ApIndex++\r
    inc        ebx                              ; EBX is CpuNumber\r
\r
    ; program stack\r
    mov        edi, esi\r
    add        edi, MP_CPU_EXCHANGE_INFO_FIELD (StackSize)\r
    mov        eax, dword [edi]\r
    mov        ecx, ebx\r
    inc        ecx\r
    mul        ecx                               ; EAX = StackSize * (CpuNumber + 1)\r
    mov        edi, esi\r
    add        edi, MP_CPU_EXCHANGE_INFO_FIELD (StackStart)\r
    add        rax, qword [edi]\r
    mov        rsp, rax\r
\r
    ;\r
    ;  Setup the GHCB when AMD SEV-ES active.\r
    ;\r
    OneTimeCall SevEsSetupGhcb\r
    jmp        CProcedureInvoke\r
\r
GetApicId:\r
    ;\r
    ; Use the GHCB protocol to get the ApicId when SEV-ES is active.\r
    ;\r
    OneTimeCall SevEsGetApicId\r
\r
DoCpuid:\r
    mov        eax, 0\r
    cpuid\r
    cmp        eax, 0bh\r
    jb         NoX2Apic             ; CPUID level below CPUID_EXTENDED_TOPOLOGY\r
\r
    mov        eax, 0bh\r
    xor        ecx, ecx\r
    cpuid\r
    test       ebx, 0ffffh\r
    jz         NoX2Apic             ; CPUID.0BH:EBX[15:0] is zero\r
\r
    ; Processor is x2APIC capable; 32-bit x2APIC ID is already in EDX\r
    jmp        GetProcessorNumber\r
\r
NoX2Apic:\r
    ; Processor is not x2APIC capable, so get 8-bit APIC ID\r
    mov        eax, 1\r
    cpuid\r
    shr        ebx, 24\r
    mov        edx, ebx\r
\r
GetProcessorNumber:\r
    ;\r
    ; Get processor number for this AP\r
    ; Note that BSP may become an AP due to SwitchBsp()\r
    ;\r
    xor         ebx, ebx\r
    lea         eax, [esi + MP_CPU_EXCHANGE_INFO_FIELD (CpuInfo)]\r
    mov         rdi, [eax]\r
\r
GetNextProcNumber:\r
    cmp         dword [rdi + CPU_INFO_IN_HOB.InitialApicId], edx                      ; APIC ID match?\r
    jz          ProgramStack\r
    add         rdi, CPU_INFO_IN_HOB_size\r
    inc         ebx\r
    jmp         GetNextProcNumber\r
\r
ProgramStack:\r
    mov         rsp, qword [rdi + CPU_INFO_IN_HOB.ApTopOfStack]\r
\r
CProcedureInvoke:\r
    ;\r
    ; Reserve 8 bytes for CpuMpData.\r
    ; When the AP wakes up again via INIT-SIPI-SIPI, push 0 will cause the existing CpuMpData to be overwritten with 0.\r
    ; CpuMpData is filled in via InitializeApData() during the first time INIT-SIPI-SIPI,\r
    ; while overwirrten may occurs when under ApInHltLoop but InitFlag is not set to ApInitConfig.\r
    ; Therefore reservation is implemented by sub rsp instead of push 0.\r
    ;\r
    sub        rsp, 8\r
    push       rbp               ; Push BIST data at top of AP stack\r
    xor        rbp, rbp          ; Clear ebp for call stack trace\r
    push       rbp\r
    mov        rbp, rsp\r
\r
    push       qword 0          ; Push 8 bytes for alignment\r
    mov        rax, qword [esi + MP_CPU_EXCHANGE_INFO_FIELD (InitializeFloatingPointUnits)]\r
    sub        rsp, 20h\r
    call       rax               ; Call assembly function to initialize FPU per UEFI spec\r
    add        rsp, 20h\r
\r
    mov        edx, ebx          ; edx is ApIndex\r
    mov        ecx, esi\r
    add        ecx, MP_CPU_EXCHANGE_INFO_OFFSET ; rcx is address of exchange info data buffer\r
\r
    mov        edi, esi\r
    add        edi, MP_CPU_EXCHANGE_INFO_FIELD (CFunction)\r
    mov        rax, qword [edi]\r
\r
    sub        rsp, 20h\r
    call       rax               ; Invoke C function\r
    add        rsp, 20h\r
    jmp        $                 ; Should never reach here\r
\r
;\r
; Required for the AMD SEV helper functions\r
;\r
%include "AmdSev.nasm"\r
\r
RendezvousFunnelProcEnd:\r
\r
;-------------------------------------------------------------------------------------\r
;  AsmRelocateApLoop (MwaitSupport, ApTargetCState, TopOfApStack, CountTofinish, Cr3);\r
;  This function is called during the finalizaiton of Mp initialization before booting\r
;  to OS, and aim to put Aps either in Mwait or HLT.\r
;-------------------------------------------------------------------------------------\r
; +----------------+\r
; | Cr3            |  rsp+40\r
; +----------------+\r
; | CountTofinish  |  r9\r
; +----------------+\r
; | TopOfApStack   |  r8\r
; +----------------+\r
; | ApTargetCState |  rdx\r
; +----------------+\r
; | MwaitSupport   |  rcx\r
; +----------------+\r
; | the return     |\r
; +----------------+ low address\r
\r
AsmRelocateApLoopGenericStart:\r
    mov        rax, r9           ; CountTofinish\r
    lock dec   dword [rax]       ; (*CountTofinish)--\r
\r
    mov        rax, [rsp + 40]    ; Cr3\r
    ; Do not push on old stack, since old stack is not mapped\r
    ; in the page table pointed by cr3\r
    mov        cr3, rax\r
    mov        rsp, r8            ; TopOfApStack\r
\r
MwaitCheckGeneric:\r
    cmp        cl, 1              ; Check mwait-monitor support\r
    jnz        HltLoopGeneric\r
    mov        rbx, rdx           ; Save C-State to ebx\r
\r
MwaitLoopGeneric:\r
    cli\r
    mov        rax, rsp           ; Set Monitor Address\r
    xor        ecx, ecx           ; ecx = 0\r
    xor        edx, edx           ; edx = 0\r
    monitor\r
    mov        rax, rbx           ; Mwait Cx, Target C-State per eax[7:4]\r
    shl        eax, 4\r
    mwait\r
    jmp        MwaitLoopGeneric\r
\r
HltLoopGeneric:\r
    cli\r
    hlt\r
    jmp        HltLoopGeneric\r
\r
AsmRelocateApLoopGenericEnd:\r
\r
;-------------------------------------------------------------------------------------\r
;  AsmGetAddressMap (&AddressMap);\r
;-------------------------------------------------------------------------------------\r
global ASM_PFX(AsmGetAddressMap)\r
ASM_PFX(AsmGetAddressMap):\r
    lea        rax, [RendezvousFunnelProcStart]\r
    mov        qword [rcx + MP_ASSEMBLY_ADDRESS_MAP.RendezvousFunnelAddress], rax\r
    mov        qword [rcx + MP_ASSEMBLY_ADDRESS_MAP.ModeEntryOffset], LongModeStart - RendezvousFunnelProcStart\r
    mov        qword [rcx + MP_ASSEMBLY_ADDRESS_MAP.RendezvousFunnelSize], RendezvousFunnelProcEnd - RendezvousFunnelProcStart\r
lea        rax, [AsmRelocateApLoopGenericStart]\r
    mov        qword [rcx + MP_ASSEMBLY_ADDRESS_MAP.RelocateApLoopFuncAddressGeneric], rax\r
    mov        qword [rcx + MP_ASSEMBLY_ADDRESS_MAP.RelocateApLoopFuncSizeGeneric], AsmRelocateApLoopGenericEnd - AsmRelocateApLoopGenericStart\r
    lea        rax, [AsmRelocateApLoopAmdSevStart]\r
    mov        qword [rcx + MP_ASSEMBLY_ADDRESS_MAP.RelocateApLoopFuncAddressAmdSev], rax\r
    mov        qword [rcx + MP_ASSEMBLY_ADDRESS_MAP.RelocateApLoopFuncSizeAmdSev], AsmRelocateApLoopAmdSevEnd - AsmRelocateApLoopAmdSevStart\r
    mov        qword [rcx + MP_ASSEMBLY_ADDRESS_MAP.ModeTransitionOffset], Flat32Start - RendezvousFunnelProcStart\r
    mov        qword [rcx + MP_ASSEMBLY_ADDRESS_MAP.SwitchToRealNoNxOffset], SwitchToRealProcStart - Flat32Start\r
    mov        qword [rcx + MP_ASSEMBLY_ADDRESS_MAP.SwitchToRealPM16ModeOffset], PM16Mode - RendezvousFunnelProcStart\r
    mov        qword [rcx + MP_ASSEMBLY_ADDRESS_MAP.SwitchToRealPM16ModeSize], SwitchToRealProcEnd - PM16Mode\r
    ret\r
\r
;-------------------------------------------------------------------------------------\r
;AsmExchangeRole procedure follows. This procedure executed by current BSP, that is\r
;about to become an AP. It switches its stack with the current AP.\r
;AsmExchangeRole (IN   CPU_EXCHANGE_INFO    *MyInfo, IN   CPU_EXCHANGE_INFO    *OthersInfo);\r
;-------------------------------------------------------------------------------------\r
global ASM_PFX(AsmExchangeRole)\r
ASM_PFX(AsmExchangeRole):\r
    ; DO NOT call other functions in this function, since 2 CPU may use 1 stack\r
    ; at the same time. If 1 CPU try to call a function, stack will be corrupted.\r
\r
    push       rax\r
    push       rbx\r
    push       rcx\r
    push       rdx\r
    push       rsi\r
    push       rdi\r
    push       rbp\r
    push       r8\r
    push       r9\r
    push       r10\r
    push       r11\r
    push       r12\r
    push       r13\r
    push       r14\r
    push       r15\r
\r
    ; rsi contains MyInfo pointer\r
    mov        rsi, rcx\r
\r
    ; rdi contains OthersInfo pointer\r
    mov        rdi, rdx\r
\r
    pushfq\r
\r
    ; Store the its StackPointer\r
    mov        [rsi + CPU_EXCHANGE_ROLE_INFO.StackPointer], rsp\r
\r
    ; update its switch state to STORED\r
    mov        byte [rsi + CPU_EXCHANGE_ROLE_INFO.State], CPU_SWITCH_STATE_STORED\r
\r
WaitForOtherStored:\r
    ; wait until the other CPU finish storing its state\r
    cmp        byte [rdi + CPU_EXCHANGE_ROLE_INFO.State], CPU_SWITCH_STATE_STORED\r
    jz         OtherStored\r
    pause\r
    jmp        WaitForOtherStored\r
\r
OtherStored:\r
\r
    ; load its future StackPointer\r
    mov        rsp, [rdi + CPU_EXCHANGE_ROLE_INFO.StackPointer]\r
\r
    ; update the other CPU's switch state to LOADED\r
    mov        byte [rdi + CPU_EXCHANGE_ROLE_INFO.State], CPU_SWITCH_STATE_LOADED\r
\r
WaitForOtherLoaded:\r
    ; wait until the other CPU finish loading new state,\r
    ; otherwise the data in stack may corrupt\r
    cmp        byte [rsi + CPU_EXCHANGE_ROLE_INFO.State], CPU_SWITCH_STATE_LOADED\r
    jz         OtherLoaded\r
    pause\r
    jmp        WaitForOtherLoaded\r
\r
OtherLoaded:\r
    ; since the other CPU already get the data it want, leave this procedure\r
    popfq\r
\r
    pop        r15\r
    pop        r14\r
    pop        r13\r
    pop        r12\r
    pop        r11\r
    pop        r10\r
    pop        r9\r
    pop        r8\r
    pop        rbp\r
    pop        rdi\r
    pop        rsi\r
    pop        rdx\r
    pop        rcx\r
    pop        rbx\r
    pop        rax\r
\r
    ret\r
Commit	Line	Data
	1	;------------------------------------------------------------------------------ ;\r
	2	; Copyright (c) 2015 - 2023, Intel Corporation. All rights reserved.<BR>\r
	3	; SPDX-License-Identifier: BSD-2-Clause-Patent\r
	4	;\r
	5	; Module Name:\r
	6	;\r
	7	; MpFuncs.nasm\r
	8	;\r
	9	; Abstract:\r
	10	;\r
	11	; This is the assembly code for MP support\r
	12	;\r
	13	;-------------------------------------------------------------------------------\r
	14	\r
	15	%include "MpEqu.inc"\r
	16	extern ASM_PFX(InitializeFloatingPointUnits)\r
	17	\r
	18	%macro OneTimeCall 1\r
	19	jmp %1\r
	20	%1 %+ OneTimerCallReturn:\r
	21	%endmacro\r
	22	\r
	23	%macro OneTimeCallRet 1\r
	24	jmp %1 %+ OneTimerCallReturn\r
	25	%endmacro\r
	26	\r
	27	DEFAULT REL\r
	28	\r
	29	SECTION .text\r
	30	\r
	31	;-------------------------------------------------------------------------------------\r
	32	;RendezvousFunnelProc procedure follows. All APs execute their procedure. This\r
	33	;procedure serializes all the AP processors through an Init sequence. It must be\r
	34	;noted that APs arrive here very raw...ie: real mode, no stack.\r
	35	;ALSO THIS PROCEDURE IS EXECUTED BY APs ONLY ON 16 BIT MODE. HENCE THIS PROC\r
	36	;IS IN MACHINE CODE.\r
	37	;-------------------------------------------------------------------------------------\r
	38	RendezvousFunnelProcStart:\r
	39	; At this point CS = 0x(vv00) and ip= 0x0.\r
	40	; Save BIST information to ebp firstly\r
	41	\r
	42	BITS 16\r
	43	mov ebp, eax ; Save BIST information\r
	44	\r
	45	mov ax, cs\r
	46	mov ds, ax\r
	47	mov es, ax\r
	48	mov ss, ax\r
	49	xor ax, ax\r
	50	mov fs, ax\r
	51	mov gs, ax\r
	52	\r
	53	mov si, MP_CPU_EXCHANGE_INFO_FIELD (BufferStart)\r
	54	mov ebx, [si]\r
	55	\r
	56	mov si, MP_CPU_EXCHANGE_INFO_FIELD (DataSegment)\r
	57	mov edx, [si]\r
	58	\r
	59	;\r
	60	; Get start address of 32-bit code in low memory (<1MB)\r
	61	;\r
	62	mov edi, MP_CPU_EXCHANGE_INFO_FIELD (ModeTransitionMemory)\r
	63	\r
	64	mov si, MP_CPU_EXCHANGE_INFO_FIELD (GdtrProfile)\r
	65	o32 lgdt [cs:si]\r
	66	\r
	67	;\r
	68	; Switch to protected mode\r
	69	;\r
	70	mov eax, cr0 ; Get control register 0\r
	71	or eax, 000000003h ; Set PE bit (bit #0) & MP\r
	72	mov cr0, eax\r
	73	\r
	74	; Switch to 32-bit code (>1MB)\r
	75	o32 jmp far [cs:di]\r
	76	\r
	77	;\r
	78	; Following code must be copied to memory with type of EfiBootServicesCode.\r
	79	; This is required if NX is enabled for EfiBootServicesCode of memory.\r
	80	;\r
	81	BITS 32\r
	82	Flat32Start: ; protected mode entry point\r
	83	mov ds, dx\r
	84	mov es, dx\r
	85	mov fs, dx\r
	86	mov gs, dx\r
	87	mov ss, dx\r
	88	\r
	89	;\r
	90	; Enable execute disable bit\r
	91	;\r
	92	mov esi, MP_CPU_EXCHANGE_INFO_FIELD (EnableExecuteDisable)\r
	93	cmp byte [ebx + esi], 0\r
	94	jz SkipEnableExecuteDisableBit\r
	95	\r
	96	mov ecx, 0c0000080h ; EFER MSR number\r
	97	rdmsr ; Read EFER\r
	98	bts eax, 11 ; Enable Execute Disable Bit\r
	99	wrmsr ; Write EFER\r
	100	\r
	101	SkipEnableExecuteDisableBit:\r
	102	;\r
	103	; Enable PAE\r
	104	;\r
	105	mov eax, cr4\r
	106	bts eax, 5\r
	107	\r
	108	mov esi, MP_CPU_EXCHANGE_INFO_FIELD (Enable5LevelPaging)\r
	109	cmp byte [ebx + esi], 0\r
	110	jz SkipEnable5LevelPaging\r
	111	\r
	112	;\r
	113	; Enable 5 Level Paging\r
	114	;\r
	115	bts eax, 12 ; Set LA57=1.\r
	116	\r
	117	SkipEnable5LevelPaging:\r
	118	\r
	119	mov cr4, eax\r
	120	\r
	121	;\r
	122	; Load page table\r
	123	;\r
	124	mov esi, MP_CPU_EXCHANGE_INFO_FIELD (Cr3) ; Save CR3 in ecx\r
	125	mov ecx, [ebx + esi]\r
	126	mov cr3, ecx ; Load CR3\r
	127	\r
	128	;\r
	129	; Enable long mode\r
	130	;\r
	131	mov ecx, 0c0000080h ; EFER MSR number\r
	132	rdmsr ; Read EFER\r
	133	bts eax, 8 ; Set LME=1\r
	134	wrmsr ; Write EFER\r
	135	\r
	136	;\r
	137	; Enable paging\r
	138	;\r
	139	mov eax, cr0 ; Read CR0\r
	140	bts eax, 31 ; Set PG=1\r
	141	mov cr0, eax ; Write CR0\r
	142	\r
	143	;\r
	144	; Far jump to 64-bit code\r
	145	;\r
	146	mov edi, MP_CPU_EXCHANGE_INFO_FIELD (ModeHighMemory)\r
	147	add edi, ebx\r
	148	jmp far [edi]\r
	149	\r
	150	BITS 64\r
	151	\r
	152	LongModeStart:\r
	153	mov esi, ebx\r
	154	\r
	155	; Set IDT table at the start of 64 bit code\r
	156	lea edi, [esi + MP_CPU_EXCHANGE_INFO_FIELD (IdtrProfile)]\r
	157	lidt [edi]\r
	158	\r
	159	lea edi, [esi + MP_CPU_EXCHANGE_INFO_FIELD (InitFlag)]\r
	160	cmp qword [edi], 1 ; ApInitConfig\r
	161	jnz GetApicId\r
	162	\r
	163	; Increment the number of APs executing here as early as possible\r
	164	; This is decremented in C code when AP is finished executing\r
	165	mov edi, esi\r
	166	add edi, MP_CPU_EXCHANGE_INFO_FIELD (NumApsExecuting)\r
	167	lock inc dword [edi]\r
	168	\r
	169	; AP init\r
	170	mov edi, esi\r
	171	add edi, MP_CPU_EXCHANGE_INFO_FIELD (ApIndex)\r
	172	mov ebx, 1\r
	173	lock xadd dword [edi], ebx ; EBX = ApIndex++\r
	174	inc ebx ; EBX is CpuNumber\r
	175	\r
	176	; program stack\r
	177	mov edi, esi\r
	178	add edi, MP_CPU_EXCHANGE_INFO_FIELD (StackSize)\r
	179	mov eax, dword [edi]\r
	180	mov ecx, ebx\r
	181	inc ecx\r
	182	mul ecx ; EAX = StackSize * (CpuNumber + 1)\r
	183	mov edi, esi\r
	184	add edi, MP_CPU_EXCHANGE_INFO_FIELD (StackStart)\r
	185	add rax, qword [edi]\r
	186	mov rsp, rax\r
	187	\r
	188	;\r
	189	; Setup the GHCB when AMD SEV-ES active.\r
	190	;\r
	191	OneTimeCall SevEsSetupGhcb\r
	192	jmp CProcedureInvoke\r
	193	\r
	194	GetApicId:\r
	195	;\r
	196	; Use the GHCB protocol to get the ApicId when SEV-ES is active.\r
	197	;\r
	198	OneTimeCall SevEsGetApicId\r
	199	\r
	200	DoCpuid:\r
	201	mov eax, 0\r
	202	cpuid\r
	203	cmp eax, 0bh\r
	204	jb NoX2Apic ; CPUID level below CPUID_EXTENDED_TOPOLOGY\r
	205	\r
	206	mov eax, 0bh\r
	207	xor ecx, ecx\r
	208	cpuid\r
	209	test ebx, 0ffffh\r
	210	jz NoX2Apic ; CPUID.0BH:EBX[15:0] is zero\r
	211	\r
	212	; Processor is x2APIC capable; 32-bit x2APIC ID is already in EDX\r
	213	jmp GetProcessorNumber\r
	214	\r
	215	NoX2Apic:\r
	216	; Processor is not x2APIC capable, so get 8-bit APIC ID\r
	217	mov eax, 1\r
	218	cpuid\r
	219	shr ebx, 24\r
	220	mov edx, ebx\r
	221	\r
	222	GetProcessorNumber:\r
	223	;\r
	224	; Get processor number for this AP\r
	225	; Note that BSP may become an AP due to SwitchBsp()\r
	226	;\r
	227	xor ebx, ebx\r
	228	lea eax, [esi + MP_CPU_EXCHANGE_INFO_FIELD (CpuInfo)]\r
	229	mov rdi, [eax]\r
	230	\r
	231	GetNextProcNumber:\r
	232	cmp dword [rdi + CPU_INFO_IN_HOB.InitialApicId], edx ; APIC ID match?\r
	233	jz ProgramStack\r
	234	add rdi, CPU_INFO_IN_HOB_size\r
	235	inc ebx\r
	236	jmp GetNextProcNumber\r
	237	\r
	238	ProgramStack:\r
	239	mov rsp, qword [rdi + CPU_INFO_IN_HOB.ApTopOfStack]\r
	240	\r
	241	CProcedureInvoke:\r
	242	;\r
	243	; Reserve 8 bytes for CpuMpData.\r
	244	; When the AP wakes up again via INIT-SIPI-SIPI, push 0 will cause the existing CpuMpData to be overwritten with 0.\r
	245	; CpuMpData is filled in via InitializeApData() during the first time INIT-SIPI-SIPI,\r
	246	; while overwirrten may occurs when under ApInHltLoop but InitFlag is not set to ApInitConfig.\r
	247	; Therefore reservation is implemented by sub rsp instead of push 0.\r
	248	;\r
	249	sub rsp, 8\r
	250	push rbp ; Push BIST data at top of AP stack\r
	251	xor rbp, rbp ; Clear ebp for call stack trace\r
	252	push rbp\r
	253	mov rbp, rsp\r
	254	\r
	255	push qword 0 ; Push 8 bytes for alignment\r
	256	mov rax, qword [esi + MP_CPU_EXCHANGE_INFO_FIELD (InitializeFloatingPointUnits)]\r
	257	sub rsp, 20h\r
	258	call rax ; Call assembly function to initialize FPU per UEFI spec\r
	259	add rsp, 20h\r
	260	\r
	261	mov edx, ebx ; edx is ApIndex\r
	262	mov ecx, esi\r
	263	add ecx, MP_CPU_EXCHANGE_INFO_OFFSET ; rcx is address of exchange info data buffer\r
	264	\r
	265	mov edi, esi\r
	266	add edi, MP_CPU_EXCHANGE_INFO_FIELD (CFunction)\r
	267	mov rax, qword [edi]\r
	268	\r
	269	sub rsp, 20h\r
	270	call rax ; Invoke C function\r
	271	add rsp, 20h\r
	272	jmp $ ; Should never reach here\r
	273	\r
	274	;\r
	275	; Required for the AMD SEV helper functions\r
	276	;\r
	277	%include "AmdSev.nasm"\r
	278	\r
	279	RendezvousFunnelProcEnd:\r
	280	\r
	281	;-------------------------------------------------------------------------------------\r
	282	; AsmRelocateApLoop (MwaitSupport, ApTargetCState, TopOfApStack, CountTofinish, Cr3);\r
	283	; This function is called during the finalizaiton of Mp initialization before booting\r
	284	; to OS, and aim to put Aps either in Mwait or HLT.\r
	285	;-------------------------------------------------------------------------------------\r
	286	; +----------------+\r
	287	; \| Cr3 \| rsp+40\r
	288	; +----------------+\r
	289	; \| CountTofinish \| r9\r
	290	; +----------------+\r
	291	; \| TopOfApStack \| r8\r
	292	; +----------------+\r
	293	; \| ApTargetCState \| rdx\r
	294	; +----------------+\r
	295	; \| MwaitSupport \| rcx\r
	296	; +----------------+\r
	297	; \| the return \|\r
	298	; +----------------+ low address\r
	299	\r
	300	AsmRelocateApLoopGenericStart:\r
	301	mov rax, r9 ; CountTofinish\r
	302	lock dec dword [rax] ; (*CountTofinish)--\r
	303	\r
	304	mov rax, [rsp + 40] ; Cr3\r
	305	; Do not push on old stack, since old stack is not mapped\r
	306	; in the page table pointed by cr3\r
	307	mov cr3, rax\r
	308	mov rsp, r8 ; TopOfApStack\r
	309	\r
	310	MwaitCheckGeneric:\r
	311	cmp cl, 1 ; Check mwait-monitor support\r
	312	jnz HltLoopGeneric\r
	313	mov rbx, rdx ; Save C-State to ebx\r
	314	\r
	315	MwaitLoopGeneric:\r
	316	cli\r
	317	mov rax, rsp ; Set Monitor Address\r
	318	xor ecx, ecx ; ecx = 0\r
	319	xor edx, edx ; edx = 0\r
	320	monitor\r
	321	mov rax, rbx ; Mwait Cx, Target C-State per eax[7:4]\r
	322	shl eax, 4\r
	323	mwait\r
	324	jmp MwaitLoopGeneric\r
	325	\r
	326	HltLoopGeneric:\r
	327	cli\r
	328	hlt\r
	329	jmp HltLoopGeneric\r
	330	\r
	331	AsmRelocateApLoopGenericEnd:\r
	332	\r
	333	;-------------------------------------------------------------------------------------\r
	334	; AsmGetAddressMap (&AddressMap);\r
	335	;-------------------------------------------------------------------------------------\r
	336	global ASM_PFX(AsmGetAddressMap)\r
	337	ASM_PFX(AsmGetAddressMap):\r
	338	lea rax, [RendezvousFunnelProcStart]\r
	339	mov qword [rcx + MP_ASSEMBLY_ADDRESS_MAP.RendezvousFunnelAddress], rax\r
	340	mov qword [rcx + MP_ASSEMBLY_ADDRESS_MAP.ModeEntryOffset], LongModeStart - RendezvousFunnelProcStart\r
	341	mov qword [rcx + MP_ASSEMBLY_ADDRESS_MAP.RendezvousFunnelSize], RendezvousFunnelProcEnd - RendezvousFunnelProcStart\r
	342	lea rax, [AsmRelocateApLoopGenericStart]\r
	343	mov qword [rcx + MP_ASSEMBLY_ADDRESS_MAP.RelocateApLoopFuncAddressGeneric], rax\r
	344	mov qword [rcx + MP_ASSEMBLY_ADDRESS_MAP.RelocateApLoopFuncSizeGeneric], AsmRelocateApLoopGenericEnd - AsmRelocateApLoopGenericStart\r
	345	lea rax, [AsmRelocateApLoopAmdSevStart]\r
	346	mov qword [rcx + MP_ASSEMBLY_ADDRESS_MAP.RelocateApLoopFuncAddressAmdSev], rax\r
	347	mov qword [rcx + MP_ASSEMBLY_ADDRESS_MAP.RelocateApLoopFuncSizeAmdSev], AsmRelocateApLoopAmdSevEnd - AsmRelocateApLoopAmdSevStart\r
	348	mov qword [rcx + MP_ASSEMBLY_ADDRESS_MAP.ModeTransitionOffset], Flat32Start - RendezvousFunnelProcStart\r
	349	mov qword [rcx + MP_ASSEMBLY_ADDRESS_MAP.SwitchToRealNoNxOffset], SwitchToRealProcStart - Flat32Start\r
	350	mov qword [rcx + MP_ASSEMBLY_ADDRESS_MAP.SwitchToRealPM16ModeOffset], PM16Mode - RendezvousFunnelProcStart\r
	351	mov qword [rcx + MP_ASSEMBLY_ADDRESS_MAP.SwitchToRealPM16ModeSize], SwitchToRealProcEnd - PM16Mode\r
	352	ret\r
	353	\r
	354	;-------------------------------------------------------------------------------------\r
	355	;AsmExchangeRole procedure follows. This procedure executed by current BSP, that is\r
	356	;about to become an AP. It switches its stack with the current AP.\r
	357	;AsmExchangeRole (IN CPU_EXCHANGE_INFO MyInfo, IN CPU_EXCHANGE_INFO OthersInfo);\r
	358	;-------------------------------------------------------------------------------------\r
	359	global ASM_PFX(AsmExchangeRole)\r
	360	ASM_PFX(AsmExchangeRole):\r
	361	; DO NOT call other functions in this function, since 2 CPU may use 1 stack\r
	362	; at the same time. If 1 CPU try to call a function, stack will be corrupted.\r
	363	\r
	364	push rax\r
	365	push rbx\r
	366	push rcx\r
	367	push rdx\r
	368	push rsi\r
	369	push rdi\r
	370	push rbp\r
	371	push r8\r
	372	push r9\r
	373	push r10\r
	374	push r11\r
	375	push r12\r
	376	push r13\r
	377	push r14\r
	378	push r15\r
	379	\r
	380	; rsi contains MyInfo pointer\r
	381	mov rsi, rcx\r
	382	\r
	383	; rdi contains OthersInfo pointer\r
	384	mov rdi, rdx\r
	385	\r
	386	pushfq\r
	387	\r
	388	; Store the its StackPointer\r
	389	mov [rsi + CPU_EXCHANGE_ROLE_INFO.StackPointer], rsp\r
	390	\r
	391	; update its switch state to STORED\r
	392	mov byte [rsi + CPU_EXCHANGE_ROLE_INFO.State], CPU_SWITCH_STATE_STORED\r
	393	\r
	394	WaitForOtherStored:\r
	395	; wait until the other CPU finish storing its state\r
	396	cmp byte [rdi + CPU_EXCHANGE_ROLE_INFO.State], CPU_SWITCH_STATE_STORED\r
	397	jz OtherStored\r
	398	pause\r
	399	jmp WaitForOtherStored\r
	400	\r
	401	OtherStored:\r
	402	\r
	403	; load its future StackPointer\r
	404	mov rsp, [rdi + CPU_EXCHANGE_ROLE_INFO.StackPointer]\r
	405	\r
	406	; update the other CPU's switch state to LOADED\r
	407	mov byte [rdi + CPU_EXCHANGE_ROLE_INFO.State], CPU_SWITCH_STATE_LOADED\r
	408	\r
	409	WaitForOtherLoaded:\r
	410	; wait until the other CPU finish loading new state,\r
	411	; otherwise the data in stack may corrupt\r
	412	cmp byte [rsi + CPU_EXCHANGE_ROLE_INFO.State], CPU_SWITCH_STATE_LOADED\r
	413	jz OtherLoaded\r
	414	pause\r
	415	jmp WaitForOtherLoaded\r
	416	\r
	417	OtherLoaded:\r
	418	; since the other CPU already get the data it want, leave this procedure\r
	419	popfq\r
	420	\r
	421	pop r15\r
	422	pop r14\r
	423	pop r13\r
	424	pop r12\r
	425	pop r11\r
	426	pop r10\r
	427	pop r9\r
	428	pop r8\r
	429	pop rbp\r
	430	pop rdi\r
	431	pop rsi\r
	432	pop rdx\r
	433	pop rcx\r
	434	pop rbx\r
	435	pop rax\r
	436	\r
	437	ret\r