UefiCpuPkg/MpInitLib: Program AP stack in fixed address

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

diff --git a/UefiCpuPkg/Library/MpInitLib/X64/MpFuncs.nasm b/UefiCpuPkg/Library/MpInitLib/X64/MpFuncs.nasm
index 848992ca4484293e1e17ecf7d713ec87facd529e..bfc3ff1f5c7af7122a6a2546ff795169936720c1 100644 (file)
--- a/UefiCpuPkg/Library/MpInitLib/X64/MpFuncs.nasm
+++ b/UefiCpuPkg/Library/MpInitLib/X64/MpFuncs.nasm
@@ -69,6 +69,19 @@ o32 lgdt       [cs:si]
     mov        si, IdtrLocation\r
 o32 lidt       [cs:si]\r
 \r
+    mov        si, EnableExecuteDisableLocation\r
+    cmp        byte [si], 0\r
+    jz         SkipEnableExecuteDisableBit\r
+\r
+    ;\r
+    ; Enable execute disable bit\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
     mov        di,  DataSegmentLocation\r
     mov        edi, [di]                   ; Save long mode DS in edi\r
@@ -106,6 +119,12 @@ LongModeStart:
     mov        es,  ax\r
     mov        ss,  ax\r
 \r
+    mov        esi, ebx\r
+    lea        edi, [esi + InitFlagLocation]\r
+    cmp        qword [edi], 1       ; ApInitConfig\r
+    jnz        GetApicId\r
+\r
+    ; AP init\r
     mov        esi, ebx\r
     mov        edi, esi\r
     add        edi, LockLocation\r
@@ -116,26 +135,64 @@ TestLock:
     cmp        rax, NotVacantFlag\r
     jz         TestLock\r
 \r
-    mov        edi, esi\r
-    add        edi, NumApsExecutingLocation\r
-    inc        dword [edi]\r
-    mov        ebx, [edi]\r
+    lea        ecx, [esi + InitFlagLocation]\r
+    inc        dword [ecx]\r
+    mov        ebx, [ecx]\r
 \r
-ProgramStack:\r
+Releaselock:\r
+    mov        rax, VacantFlag\r
+    xchg       qword [edi], rax\r
+    ; program stack\r
     mov        edi, esi\r
     add        edi, StackSizeLocation\r
-    mov        rax, qword [edi]\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, StackStartAddressLocation\r
     add        rax, qword [edi]\r
     mov        rsp, rax\r
-    mov        qword [edi], rax\r
+    jmp        CProcedureInvoke\r
+\r
+GetApicId:\r
+    mov        eax, 0\r
+    cpuid\r
+    cmp        eax, 0bh\r
+    jnb        X2Apic\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
+    jmp        GetProcessorNumber\r
+\r
+X2Apic:\r
+    ; Processor is x2APIC capable, so get 32-bit x2APIC ID\r
+    mov        eax, 0bh\r
+    xor        ecx, ecx\r
+    cpuid                   \r
+    ; edx save x2APIC ID\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 + CpuInfoLocation]\r
+    mov         edi, [eax]\r
+\r
+GetNextProcNumber:\r
+    cmp         dword [edi], edx                      ; APIC ID match?\r
+    jz          ProgramStack\r
+    add         edi, 16\r
+    inc         ebx\r
+    jmp         GetNextProcNumber    \r
 \r
-Releaselock:\r
-    mov        rax, VacantFlag\r
-    mov        edi, esi\r
-    add        edi, LockLocation\r
-    xchg       qword [edi], rax\r
+ProgramStack:\r
+    xor         rsp, rsp\r
+    mov         esp, dword [edi + 12]\r
 \r
 CProcedureInvoke:\r
     push       rbp               ; Push BIST data at top of AP stack\r
@@ -163,6 +220,60 @@ CProcedureInvoke:
 \r
 RendezvousFunnelProcEnd:\r
 \r
+;-------------------------------------------------------------------------------------\r
+;  AsmRelocateApLoop (MwaitSupport, ApTargetCState, PmCodeSegment);\r
+;-------------------------------------------------------------------------------------\r
+global ASM_PFX(AsmRelocateApLoop)\r
+ASM_PFX(AsmRelocateApLoop):\r
+AsmRelocateApLoopStart:\r
+    push       rcx\r
+    push       rdx\r
+\r
+    lea        rsi, [PmEntry]    ; rsi <- The start address of transition code\r
+\r
+    push       r8\r
+    push       rsi\r
+    DB         0x48\r
+    retf\r
+BITS 32\r
+PmEntry:\r
+    mov        eax, cr0\r
+    btr        eax, 31           ; Clear CR0.PG\r
+    mov        cr0, eax          ; Disable paging and caches\r
+\r
+    mov        ebx, edx          ; Save EntryPoint to rbx, for rdmsr will overwrite rdx\r
+    mov        ecx, 0xc0000080\r
+    rdmsr\r
+    and        ah, ~ 1           ; Clear LME\r
+    wrmsr\r
+    mov        eax, cr4\r
+    and        al, ~ (1 << 5)    ; Clear PAE\r
+    mov        cr4, eax\r
+\r
+    pop        edx\r
+    add        esp, 4\r
+    pop        ecx,\r
+    add        esp, 4\r
+    cmp        cl, 1              ; Check mwait-monitor support\r
+    jnz        HltLoop\r
+    mov        ebx, edx           ; Save C-State to ebx\r
+MwaitLoop:\r
+    mov        eax, esp           ; Set Monitor Address\r
+    xor        ecx, ecx           ; ecx = 0\r
+    xor        edx, edx           ; edx = 0\r
+    monitor\r
+    shl        ebx, 4\r
+    mov        eax, ebx           ; Mwait Cx, Target C-State per eax[7:4]\r
+    mwait\r
+    jmp        MwaitLoop\r
+HltLoop:\r
+    cli\r
+    hlt\r
+    jmp        HltLoop\r
+    ret\r
+BITS 64\r
+AsmRelocateApLoopEnd:\r
+\r
 ;-------------------------------------------------------------------------------------\r
 ;  AsmGetAddressMap (&AddressMap);\r
 ;-------------------------------------------------------------------------------------\r
@@ -172,6 +283,9 @@ ASM_PFX(AsmGetAddressMap):
     mov        qword [rcx], rax\r
     mov        qword [rcx +  8h], LongModeStart - RendezvousFunnelProcStart\r
     mov        qword [rcx + 10h], RendezvousFunnelProcEnd - RendezvousFunnelProcStart\r
+    mov        rax, ASM_PFX(AsmRelocateApLoop)\r
+    mov        qword [rcx + 18h], rax\r
+    mov        qword [rcx + 20h], AsmRelocateApLoopEnd - AsmRelocateApLoopStart\r
     ret\r
 \r
 ;-------------------------------------------------------------------------------------\r