UefiCpuPkg/CpuMpPei: Implementation of PeiSwitchBSP ()

Contributed-under: TianoCore Contribution Agreement 1.0
Signed-off-by: Jeff Fan <jeff.fan@intel.com>
Reviewed-by: Feng Tian <feng.tian@intel.com>
Reviewed-by: Jiewen Yao <jiewen.yao@intel.com>

git-svn-id: https://svn.code.sf.net/p/edk2/code/trunk/edk2@18010 6f19259b-4bc3-4df7-8a09-765794883524

3 files changed, 157 insertions, 0 deletions

diff --git a/UefiCpuPkg/CpuMpPei/Ia32/MpEqu.inc b/UefiCpuPkg/CpuMpPei/Ia32/MpEqu.inc
index 1f02dad743..50ec8c9cfc 100644
--- a/UefiCpuPkg/CpuMpPei/Ia32/MpEqu.inc
+++ b/UefiCpuPkg/CpuMpPei/Ia32/MpEqu.inc
@@ -24,6 +24,10 @@ PROTECT_MODE_DS               equ        18h
 VacantFlag                    equ        00h
 NotVacantFlag                 equ        0ffh
 
+CPU_SWITCH_STATE_IDLE         equ        0
+CPU_SWITCH_STATE_STORED       equ        1
+CPU_SWITCH_STATE_LOADED       equ        2
+
 LockLocation                  equ        (RendezvousFunnelProcEnd - RendezvousFunnelProcStart)
 StackStartAddressLocation     equ        LockLocation + 04h
 StackSizeLocation             equ        LockLocation + 08h
diff --git a/UefiCpuPkg/CpuMpPei/Ia32/MpFuncs.asm b/UefiCpuPkg/CpuMpPei/Ia32/MpFuncs.asm
index 9861472509..63c80489f4 100644
--- a/UefiCpuPkg/CpuMpPei/Ia32/MpFuncs.asm
+++ b/UefiCpuPkg/CpuMpPei/Ia32/MpFuncs.asm
@@ -161,6 +161,85 @@ AsmGetAddressMap   PROC  near C  PUBLIC
     ret
 AsmGetAddressMap   ENDP
 
+PAUSE32   MACRO
+    DB      0F3h
+    DB      090h
+    ENDM
+
+;-------------------------------------------------------------------------------------
+;AsmExchangeRole procedure follows. This procedure executed by current BSP, that is
+;about to become an AP. It switches it'stack with the current AP.
+;AsmExchangeRole (IN   CPU_EXCHANGE_INFO    *MyInfo, IN   CPU_EXCHANGE_INFO    *OthersInfo);
+;-------------------------------------------------------------------------------------
+AsmExchangeRole   PROC  near C  PUBLIC
+    ; DO NOT call other functions in this function, since 2 CPU may use 1 stack
+    ; at the same time. If 1 CPU try to call a function, stack will be corrupted.
+    pushad
+    mov        ebp,esp
+
+    ; esi contains MyInfo pointer
+    mov        esi, dword ptr [ebp+24h]
+
+    ; edi contains OthersInfo pointer
+    mov        edi, dword ptr [ebp+28h]
+
+    ;Store EFLAGS, GDTR and IDTR register to stack
+    pushfd
+    mov        eax, cr4
+    push       eax       ; push cr4 firstly
+    mov        eax, cr0
+    push       eax
+
+    sgdt       fword ptr [esi+8]
+    sidt       fword ptr [esi+14]
+
+    ; Store the its StackPointer
+    mov        dword ptr [esi+4],esp
+
+    ; update its switch state to STORED
+    mov        byte ptr [esi], CPU_SWITCH_STATE_STORED
+
+WaitForOtherStored:
+    ; wait until the other CPU finish storing its state
+    cmp        byte ptr [edi], CPU_SWITCH_STATE_STORED
+    jz         OtherStored
+    PAUSE32
+    jmp        WaitForOtherStored
+
+OtherStored:
+    ; Since another CPU already stored its state, load them
+    ; load GDTR value
+    lgdt       fword ptr [edi+8]
+
+    ; load IDTR value
+    lidt       fword ptr [edi+14]
+
+    ; load its future StackPointer
+    mov        esp, dword ptr [edi+4]
+
+    ; update the other CPU's switch state to LOADED
+    mov        byte ptr [edi], CPU_SWITCH_STATE_LOADED
+
+WaitForOtherLoaded:
+    ; wait until the other CPU finish loading new state,
+    ; otherwise the data in stack may corrupt
+    cmp        byte ptr [esi], CPU_SWITCH_STATE_LOADED
+    jz         OtherLoaded
+    PAUSE32
+    jmp        WaitForOtherLoaded
+
+OtherLoaded:
+    ; since the other CPU already get the data it want, leave this procedure
+    pop        eax
+    mov        cr0, eax
+    pop        eax
+    mov        cr4, eax
+    popfd
+
+    popad
+    ret
+AsmExchangeRole   ENDP
+
 AsmInitializeGdt   PROC  near C  PUBLIC
   push         ebp
   mov          ebp, esp
diff --git a/UefiCpuPkg/CpuMpPei/Ia32/MpFuncs.nasm b/UefiCpuPkg/CpuMpPei/Ia32/MpFuncs.nasm
index a3c4ae9e58..fe45cf1f9f 100644
--- a/UefiCpuPkg/CpuMpPei/Ia32/MpFuncs.nasm
+++ b/UefiCpuPkg/CpuMpPei/Ia32/MpFuncs.nasm
@@ -149,6 +149,80 @@ ASM_PFX(AsmGetAddressMap):
     popad
     ret
 
+;-------------------------------------------------------------------------------------
+;AsmExchangeRole procedure follows. This procedure executed by current BSP, that is
+;about to become an AP. It switches it'stack with the current AP.
+;AsmExchangeRole (IN   CPU_EXCHANGE_INFO    *MyInfo, IN   CPU_EXCHANGE_INFO    *OthersInfo);
+;-------------------------------------------------------------------------------------
+global ASM_PFX(AsmExchangeRole)
+ASM_PFX(AsmExchangeRole):
+    ; DO NOT call other functions in this function, since 2 CPU may use 1 stack
+    ; at the same time. If 1 CPU try to call a function, stack will be corrupted.
+    pushad
+    mov        ebp,esp
+
+    ; esi contains MyInfo pointer
+    mov        esi, [ebp + 24h]
+
+    ; edi contains OthersInfo pointer
+    mov        edi, [ebp + 28h]
+
+    ;Store EFLAGS, GDTR and IDTR register to stack
+    pushfd
+    mov        eax, cr4
+    push       eax       ; push cr4 firstly
+    mov        eax, cr0
+    push       eax
+
+    sgdt       [esi + 8]
+    sidt       [esi + 14]
+
+    ; Store the its StackPointer
+    mov        [esi + 4],esp
+
+    ; update its switch state to STORED
+    mov        byte [esi], CPU_SWITCH_STATE_STORED
+
+WaitForOtherStored:
+    ; wait until the other CPU finish storing its state
+    cmp        byte [edi], CPU_SWITCH_STATE_STORED
+    jz         OtherStored
+    pause
+    jmp        WaitForOtherStored
+
+OtherStored:
+    ; Since another CPU already stored its state, load them
+    ; load GDTR value
+    lgdt       [edi + 8]
+
+    ; load IDTR value
+    lidt       [edi + 14]
+
+    ; load its future StackPointer
+    mov        esp, [edi + 4]
+
+    ; update the other CPU's switch state to LOADED
+    mov        byte [edi], CPU_SWITCH_STATE_LOADED
+
+WaitForOtherLoaded:
+    ; wait until the other CPU finish loading new state,
+    ; otherwise the data in stack may corrupt
+    cmp        byte [esi], CPU_SWITCH_STATE_LOADED
+    jz         OtherLoaded
+    pause
+    jmp        WaitForOtherLoaded
+
+OtherLoaded:
+    ; since the other CPU already get the data it want, leave this procedure
+    pop        eax
+    mov        cr0, eax
+    pop        eax
+    mov        cr4, eax
+    popfd
+
+    popad
+    ret
+
 global ASM_PFX(AsmInitializeGdt)
 ASM_PFX(AsmInitializeGdt):
   push         ebp