X-Git-Url: https://git.proxmox.com/?a=blobdiff_plain;f=UefiCpuPkg%2FLibrary%2FMpInitLib%2FIa32%2FMpFuncs.nasm;h=2b6c27d4ec675f6389436ce3862b01b22284a5b1;hb=147fd35c3e389ecd025dbfd243312bf5b22da7c9;hp=0852a5bc848cfe00d4d108666620224d3d8e39c2;hpb=d94e5f672994f67ea7bd16aaf577bf4cde65cfc1;p=mirror_edk2.git

diff --git a/UefiCpuPkg/Library/MpInitLib/Ia32/MpFuncs.nasm b/UefiCpuPkg/Library/MpInitLib/Ia32/MpFuncs.nasm
index 0852a5bc84..2b6c27d4ec 100644
--- a/UefiCpuPkg/Library/MpInitLib/Ia32/MpFuncs.nasm
+++ b/UefiCpuPkg/Library/MpInitLib/Ia32/MpFuncs.nasm
@@ -71,8 +71,8 @@ o32 lidt       [cs:si]
     xor        ax,  ax
     mov        ds,  ax
 
-    mov        eax, cr0                        ;Get control register 0
-    or         eax, 000000003h                 ;Set PE bit (bit #0) & MP
+    mov        eax, cr0                        ; Get control register 0
+    or         eax, 000000003h                 ; Set PE bit (bit #0) & MP
     mov        cr0, eax
 
     jmp        0:strict dword 0                ; far jump to protected mode
@@ -85,6 +85,47 @@ Flat32Start:                                   ; protected mode entry point
     mov        ss, dx
 
     mov        esi, ebx
+
+    ; Increment the number of APs executing here as early as possible
+    ; This is decremented in C code when AP is finished executing
+    mov        edi, esi
+    add        edi, NumApsExecutingLocation
+    lock inc   dword [edi]
+
+    mov         edi, esi
+    add         edi, EnableExecuteDisableLocation
+    cmp         byte [edi], 0
+    jz          SkipEnableExecuteDisable
+
+    ;
+    ; Enable IA32 PAE execute disable
+    ;
+
+    mov         ecx, 0xc0000080
+    rdmsr
+    bts         eax, 11
+    wrmsr
+
+    mov         edi, esi
+    add         edi, Cr3Location
+    mov         eax, dword [edi]
+    mov         cr3, eax
+
+    mov         eax, cr4
+    bts         eax, 5
+    mov         cr4, eax
+
+    mov         eax, cr0
+    bts         eax, 31
+    mov         cr0, eax
+
+SkipEnableExecuteDisable:
+    mov        edi, esi
+    add        edi, InitFlagLocation
+    cmp        dword [edi], 1       ; 1 == ApInitConfig
+    jnz        GetApicId
+
+    ; AP init
     mov        edi, esi
     add        edi, LockLocation
     mov        eax, NotVacantFlag
@@ -94,27 +135,68 @@ TestLock:
     cmp        eax, NotVacantFlag
     jz         TestLock
 
-    mov        edi, esi
-    add        edi, NumApsExecutingLoction
-    inc        dword [edi]
-    mov        ebx, [edi]
+    mov        ecx, esi
+    add        ecx, ApIndexLocation
+    inc        dword [ecx]
+    mov        ebx, [ecx]
+
+Releaselock:
+    mov        eax, VacantFlag
+    xchg       [edi], eax
 
-ProgramStack:
     mov        edi, esi
     add        edi, StackSizeLocation
     mov        eax, [edi]
+    mov        ecx, ebx
+    inc        ecx
+    mul        ecx                               ; EAX = StackSize * (CpuNumber + 1)
     mov        edi, esi
     add        edi, StackStartAddressLocation
     add        eax, [edi]
     mov        esp, eax
-    mov        [edi], eax
-
-Releaselock:
-    mov        eax, VacantFlag
-    mov        edi, esi
-    add        edi, LockLocation
-    xchg       [edi], eax
+    jmp        CProcedureInvoke
+
+GetApicId:
+    mov        eax, 0
+    cpuid
+    cmp        eax, 0bh
+    jb         NoX2Apic             ; CPUID level below CPUID_EXTENDED_TOPOLOGY
+
+    mov        eax, 0bh
+    xor        ecx, ecx
+    cpuid
+    test       ebx, 0ffffh
+    jz         NoX2Apic             ; CPUID.0BH:EBX[15:0] is zero
+
+    ; Processor is x2APIC capable; 32-bit x2APIC ID is already in EDX
+    jmp        GetProcessorNumber
+
+NoX2Apic:
+    ; Processor is not x2APIC capable, so get 8-bit APIC ID
+    mov        eax, 1
+    cpuid
+    shr        ebx, 24
+    mov        edx, ebx
+
+GetProcessorNumber:
+    ;
+    ; Get processor number for this AP
+    ; Note that BSP may become an AP due to SwitchBsp()
+    ;
+    xor         ebx, ebx
+    lea         eax, [esi + CpuInfoLocation]
+    mov         edi, [eax]
+
+GetNextProcNumber:
+    cmp         [edi], edx                       ; APIC ID match?
+    jz          ProgramStack
+    add         edi, 20
+    inc         ebx
+    jmp         GetNextProcNumber    
 
+ProgramStack:
+    mov         esp, [edi + 12]
+   
 CProcedureInvoke:
     push       ebp               ; push BIST data at top of AP stack
     xor        ebp, ebp          ; clear ebp for call stack trace
@@ -124,7 +206,7 @@ CProcedureInvoke:
     mov        eax, ASM_PFX(InitializeFloatingPointUnits)
     call       eax               ; Call assembly function to initialize FPU per UEFI spec
 
-    push       ebx               ; Push NumApsExecuting
+    push       ebx               ; Push ApIndex
     mov        eax, esi
     add        eax, LockLocation
     push       eax               ; push address of exchange info data buffer
@@ -133,11 +215,43 @@ CProcedureInvoke:
     add        edi, ApProcedureLocation
     mov        eax, [edi]
 
-    call       eax               ; invoke C function
+    call       eax               ; Invoke C function
 
-    jmp        $                 ; never reach here
+    jmp        $                 ; Never reach here
 RendezvousFunnelProcEnd:
 
+;-------------------------------------------------------------------------------------
+;  AsmRelocateApLoop (MwaitSupport, ApTargetCState, PmCodeSegment, TopOfApStack, CountTofinish);
+;-------------------------------------------------------------------------------------
+global ASM_PFX(AsmRelocateApLoop)
+ASM_PFX(AsmRelocateApLoop):
+AsmRelocateApLoopStart:
+    mov        eax, esp
+    mov        esp, [eax + 16]     ; TopOfApStack
+    push       dword [eax]         ; push return address for stack trace
+    push       ebp
+    mov        ebp, esp
+    mov        ebx, [eax + 8]      ; ApTargetCState
+    mov        ecx, [eax + 4]      ; MwaitSupport
+    mov        eax, [eax + 20]     ; CountTofinish
+    lock dec   dword [eax]         ; (*CountTofinish)--
+    cmp        cl,  1              ; Check mwait-monitor support
+    jnz        HltLoop
+MwaitLoop:
+    mov        eax, esp
+    xor        ecx, ecx
+    xor        edx, edx
+    monitor
+    mov        eax, ebx            ; Mwait Cx, Target C-State per eax[7:4]
+    shl        eax, 4
+    mwait
+    jmp        MwaitLoop
+HltLoop:
+    cli
+    hlt
+    jmp        HltLoop
+AsmRelocateApLoopEnd:
+
 ;-------------------------------------------------------------------------------------
 ;  AsmGetAddressMap (&AddressMap);
 ;-------------------------------------------------------------------------------------
@@ -150,6 +264,8 @@ ASM_PFX(AsmGetAddressMap):
     mov        dword [ebx], RendezvousFunnelProcStart
     mov        dword [ebx +  4h], Flat32Start - RendezvousFunnelProcStart
     mov        dword [ebx +  8h], RendezvousFunnelProcEnd - RendezvousFunnelProcStart
+    mov        dword [ebx + 0Ch], AsmRelocateApLoopStart
+    mov        dword [ebx + 10h], AsmRelocateApLoopEnd - AsmRelocateApLoopStart
 
     popad
     ret