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

9 files changed, 579 insertions, 0 deletions

diff --git a/UefiCpuPkg/CpuMpPei/CpuMpPei.h b/UefiCpuPkg/CpuMpPei/CpuMpPei.h
index ed6cf05986..d8ee2ee5fb 100644
--- a/UefiCpuPkg/CpuMpPei/CpuMpPei.h
+++ b/UefiCpuPkg/CpuMpPei/CpuMpPei.h
@@ -58,6 +58,16 @@ typedef struct {
   UINTN             RendezvousFunnelSize;
 } MP_ASSEMBLY_ADDRESS_MAP;
 
+//
+// CPU exchange information for switch BSP
+//
+typedef struct {
+  UINT8             State;        // offset 0
+  UINTN             StackPointer; // offset 4 / 8
+  IA32_DESCRIPTOR   Gdtr;         // offset 8 / 16
+  IA32_DESCRIPTOR   Idtr;         // offset 14 / 26
+} CPU_EXCHANGE_ROLE_INFO;
+
 typedef struct _PEI_CPU_MP_DATA  PEI_CPU_MP_DATA;
 
 #pragma pack()
@@ -124,6 +134,8 @@ struct _PEI_CPU_MP_DATA {
   UINTN                          ApFunctionArgument;
   volatile UINT32                FinishedCount;
   BOOLEAN                        InitFlag;
+  CPU_EXCHANGE_ROLE_INFO         BSPInfo;
+  CPU_EXCHANGE_ROLE_INFO         APInfo;
   MTRR_SETTINGS                  MtrrTable;
   PEI_CPU_DATA                   *CpuData;
   volatile MP_CPU_EXCHANGE_INFO  *MpCpuExchangeInfo;
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
diff --git a/UefiCpuPkg/CpuMpPei/PeiMpServices.c b/UefiCpuPkg/CpuMpPei/PeiMpServices.c
index 2f82130efd..8c14620a6a 100644
--- a/UefiCpuPkg/CpuMpPei/PeiMpServices.c
+++ b/UefiCpuPkg/CpuMpPei/PeiMpServices.c
@@ -154,6 +154,25 @@ GetProcessorNumber (
 }
 
 /**
+  Worker function for SwitchBSP().
+
+  Worker function for SwitchBSP(), assigned to the AP which is intended to become BSP.
+
+  @param Buffer        Pointer to CPU MP Data
+**/
+VOID
+EFIAPI
+FutureBSPProc (
+  IN  VOID                *Buffer
+  )
+{
+  PEI_CPU_MP_DATA         *DataInHob;
+
+  DataInHob = (PEI_CPU_MP_DATA *) Buffer;
+  AsmExchangeRole (&DataInHob->APInfo, &DataInHob->BSPInfo);
+}
+
+/**
   This service retrieves the number of logical processor in the platform
   and the number of those logical processors that are enabled on this boot.
   This service may only be called from the BSP.
@@ -617,6 +636,118 @@ PeiStartupThisAP (
   return Status;
 }
 
+/**
+  This service switches the requested AP to be the BSP from that point onward.
+  This service changes the BSP for all purposes.   This call can only be performed
+  by the current BSP.
+
+  This service switches the requested AP to be the BSP from that point onward.
+  This service changes the BSP for all purposes. The new BSP can take over the
+  execution of the old BSP and continue seamlessly from where the old one left
+  off.
+
+  If the BSP cannot be switched prior to the return from this service, then
+  EFI_UNSUPPORTED must be returned.
+
+  @param[in] PeiServices          An indirect pointer to the PEI Services Table
+                                  published by the PEI Foundation.
+  @param[in] This                 A pointer to the EFI_PEI_MP_SERVICES_PPI instance.
+  @param[in] ProcessorNumber      The handle number of the AP. The range is from 0 to the
+                                  total number of logical processors minus 1. The total
+                                  number of logical processors can be retrieved by
+                                  EFI_PEI_MP_SERVICES_PPI.GetNumberOfProcessors().
+  @param[in] EnableOldBSP         If TRUE, then the old BSP will be listed as an enabled
+                                  AP. Otherwise, it will be disabled.
+
+  @retval EFI_SUCCESS             BSP successfully switched.
+  @retval EFI_UNSUPPORTED         Switching the BSP cannot be completed prior to this
+                                  service returning.
+  @retval EFI_UNSUPPORTED         Switching the BSP is not supported.
+  @retval EFI_SUCCESS             The calling processor is an AP.
+  @retval EFI_NOT_FOUND           The processor with the handle specified by
+                                  ProcessorNumber does not exist.
+  @retval EFI_INVALID_PARAMETER   ProcessorNumber specifies the current BSP or a disabled
+                                  AP.
+  @retval EFI_NOT_READY           The specified AP is busy.
+**/
+EFI_STATUS
+EFIAPI
+PeiSwitchBSP (
+  IN  CONST EFI_PEI_SERVICES   **PeiServices,
+  IN  EFI_PEI_MP_SERVICES_PPI  *This,
+  IN  UINTN                    ProcessorNumber,
+  IN  BOOLEAN                  EnableOldBSP
+  )
+{
+  PEI_CPU_MP_DATA         *PeiCpuMpData;
+  UINTN                   CallerNumber;
+  MSR_IA32_APIC_BASE      ApicBaseMsr;
+
+  PeiCpuMpData = GetMpHobData ();
+  if (PeiCpuMpData == NULL) {
+    return EFI_NOT_FOUND;
+  }
+
+  //
+  // Check whether caller processor is BSP
+  //
+  PeiWhoAmI (PeiServices, This, &CallerNumber);
+  if (CallerNumber != PeiCpuMpData->BspNumber) {
+    return EFI_SUCCESS;
+  }
+
+  if (ProcessorNumber >= PeiCpuMpData->CpuCount) {
+    return EFI_NOT_FOUND;
+  }
+
+  //
+  // Check whether specified AP is disabled
+  //
+  if (PeiCpuMpData->CpuData[ProcessorNumber].State == CpuStateDisabled) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Check whether ProcessorNumber specifies the current BSP
+  //
+  if (ProcessorNumber == PeiCpuMpData->BspNumber) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  //
+  // Check whether specified AP is busy
+  //
+  if (PeiCpuMpData->CpuData[ProcessorNumber].State == CpuStateBusy) {
+    return EFI_NOT_READY;
+  }
+
+  //
+  // Clear the BSP bit of MSR_IA32_APIC_BASE
+  //
+  ApicBaseMsr.Uint64 = AsmReadMsr64 (MSR_IA32_APIC_BASE_ADDRESS);
+  ApicBaseMsr.Bits.Bsp = 0;
+  AsmWriteMsr64 (MSR_IA32_APIC_BASE_ADDRESS, ApicBaseMsr.Uint64);
+
+  PeiCpuMpData->BSPInfo.State = CPU_SWITCH_STATE_IDLE;
+  PeiCpuMpData->APInfo.State  = CPU_SWITCH_STATE_IDLE;
+
+  //
+  // Need to wakeUp AP (future BSP).
+  //
+  WakeUpAP (PeiCpuMpData, FALSE, PeiCpuMpData->CpuData[ProcessorNumber].ApicId, FutureBSPProc, PeiCpuMpData);
+
+  AsmExchangeRole (&PeiCpuMpData->BSPInfo, &PeiCpuMpData->APInfo);
+
+  //
+  // Set the BSP bit of MSR_IA32_APIC_BASE on new BSP
+  //
+  ApicBaseMsr.Uint64 = AsmReadMsr64 (MSR_IA32_APIC_BASE_ADDRESS);
+  ApicBaseMsr.Bits.Bsp = 1;
+  AsmWriteMsr64 (MSR_IA32_APIC_BASE_ADDRESS, ApicBaseMsr.Uint64);
+
+  return EFI_SUCCESS;
+}
+
 
 /**
   This return the handle number for the calling processor.  This service may be
diff --git a/UefiCpuPkg/CpuMpPei/PeiMpServices.h b/UefiCpuPkg/CpuMpPei/PeiMpServices.h
index 6264abd7c6..1461eec8cc 100644
--- a/UefiCpuPkg/CpuMpPei/PeiMpServices.h
+++ b/UefiCpuPkg/CpuMpPei/PeiMpServices.h
@@ -17,10 +17,31 @@
 
 #include "CpuMpPei.h"
 
+//
+//  The MP data for switch BSP
+//
+#define CPU_SWITCH_STATE_IDLE   0
+#define CPU_SWITCH_STATE_STORED 1
+#define CPU_SWITCH_STATE_LOADED 2
 
 #define CPU_CHECK_AP_INTERVAL             0x100     // 100 microseconds
 
 /**
+  This function is called by both the BSP and the AP which is to become the BSP to
+  Exchange execution context including stack between them. After return from this
+  function, the BSP becomes AP and the AP becomes the BSP.
+
+  @param MyInfo      Pointer to buffer holding the exchanging information for the executing processor.
+  @param OthersInfo  Pointer to buffer holding the exchanging information for the peer.
+**/
+VOID
+EFIAPI
+AsmExchangeRole (
+  IN   CPU_EXCHANGE_ROLE_INFO    *MyInfo,
+  IN   CPU_EXCHANGE_ROLE_INFO    *OthersInfo
+  );
+
+/**
   This service retrieves the number of logical processor in the platform
   and the number of those logical processors that are enabled on this boot.
   This service may only be called from the BSP.
@@ -229,6 +250,49 @@ PeiStartupThisAP (
   IN  VOID                      *ProcedureArgument      OPTIONAL
   );
 
+/**
+  This service switches the requested AP to be the BSP from that point onward.
+  This service changes the BSP for all purposes.   This call can only be performed
+  by the current BSP.
+
+  This service switches the requested AP to be the BSP from that point onward.
+  This service changes the BSP for all purposes. The new BSP can take over the
+  execution of the old BSP and continue seamlessly from where the old one left
+  off.
+
+  If the BSP cannot be switched prior to the return from this service, then
+  EFI_UNSUPPORTED must be returned.
+
+  @param[in] PeiServices          An indirect pointer to the PEI Services Table
+                                  published by the PEI Foundation.
+  @param[in] This                 A pointer to the EFI_PEI_MP_SERVICES_PPI instance.
+  @param[in] ProcessorNumber      The handle number of the AP. The range is from 0 to the
+                                  total number of logical processors minus 1. The total
+                                  number of logical processors can be retrieved by
+                                  EFI_PEI_MP_SERVICES_PPI.GetNumberOfProcessors().
+  @param[in] EnableOldBSP         If TRUE, then the old BSP will be listed as an enabled
+                                  AP. Otherwise, it will be disabled.
+
+  @retval EFI_SUCCESS             BSP successfully switched.
+  @retval EFI_UNSUPPORTED         Switching the BSP cannot be completed prior to this
+                                  service returning.
+  @retval EFI_UNSUPPORTED         Switching the BSP is not supported.
+  @retval EFI_SUCCESS             The calling processor is an AP.
+  @retval EFI_NOT_FOUND           The processor with the handle specified by
+                                  ProcessorNumber does not exist.
+  @retval EFI_INVALID_PARAMETER   ProcessorNumber specifies the current BSP or a disabled
+                                  AP.
+  @retval EFI_NOT_READY           The specified AP is busy.
+**/
+EFI_STATUS
+EFIAPI
+PeiSwitchBSP (
+  IN  CONST EFI_PEI_SERVICES   **PeiServices,
+  IN  EFI_PEI_MP_SERVICES_PPI  *This,
+  IN  UINTN                    ProcessorNumber,
+  IN  BOOLEAN                  EnableOldBSP
+  );
+
 
 /**
   This return the handle number for the calling processor.  This service may be
diff --git a/UefiCpuPkg/CpuMpPei/X64/MpEqu.inc b/UefiCpuPkg/CpuMpPei/X64/MpEqu.inc
index 28a826c643..946fe506d0 100644
--- a/UefiCpuPkg/CpuMpPei/X64/MpEqu.inc
+++ b/UefiCpuPkg/CpuMpPei/X64/MpEqu.inc
@@ -26,6 +26,9 @@ LONG_MODE_DS                  equ        30h
 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 + 08h
diff --git a/UefiCpuPkg/CpuMpPei/X64/MpFuncs.asm b/UefiCpuPkg/CpuMpPei/X64/MpFuncs.asm
index 9e85fac435..6622c43fcf 100644
--- a/UefiCpuPkg/CpuMpPei/X64/MpFuncs.asm
+++ b/UefiCpuPkg/CpuMpPei/X64/MpFuncs.asm
@@ -193,6 +193,112 @@ AsmGetAddressMap   PROC
     ret
 AsmGetAddressMap   ENDP
 
+;-------------------------------------------------------------------------------------
+;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
+    ; 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.
+
+    push       rax
+    push       rbx
+    push       rcx
+    push       rdx
+    push       rsi
+    push       rdi
+    push       rbp
+    push       r8
+    push       r9
+    push       r10
+    push       r11
+    push       r12
+    push       r13
+    push       r14
+    push       r15
+
+    mov        rax, cr0
+    push       rax
+
+    mov        rax, cr4
+    push       rax
+
+    ; rsi contains MyInfo pointer
+    mov        rsi, rcx
+
+    ; rdi contains OthersInfo pointer
+    mov        rdi, rdx
+
+    ;Store EFLAGS, GDTR and IDTR regiter to stack
+    pushfq
+    sgdt       fword ptr [rsi + 16]
+    sidt       fword ptr [rsi + 26]
+
+    ; Store the its StackPointer
+    mov        qword ptr [rsi + 8], rsp
+
+    ; update its switch state to STORED
+    mov        byte ptr [rsi], CPU_SWITCH_STATE_STORED
+
+WaitForOtherStored:
+    ; wait until the other CPU finish storing its state
+    cmp        byte ptr [rdi], CPU_SWITCH_STATE_STORED
+    jz         OtherStored
+    pause
+    jmp        WaitForOtherStored
+
+OtherStored:
+    ; Since another CPU already stored its state, load them
+    ; load GDTR value
+    lgdt       fword ptr [rdi + 16]
+
+    ; load IDTR value
+    lidt       fword ptr [rdi + 26]
+
+    ; load its future StackPointer
+    mov        rsp, qword ptr [rdi + 8]
+
+    ; update the other CPU's switch state to LOADED
+    mov        byte ptr [rdi], CPU_SWITCH_STATE_LOADED
+
+WaitForOtherLoaded:
+    ; wait until the other CPU finish loading new state,
+    ; otherwise the data in stack may corrupt
+    cmp        byte ptr [rsi], CPU_SWITCH_STATE_LOADED
+    jz         OtherLoaded
+    pause
+    jmp        WaitForOtherLoaded
+
+OtherLoaded:
+    ; since the other CPU already get the data it want, leave this procedure
+    popfq
+
+    pop        rax
+    mov        cr4, rax
+
+    pop        rax
+    mov        cr0, rax
+
+    pop        r15
+    pop        r14
+    pop        r13
+    pop        r12
+    pop        r11
+    pop        r10
+    pop        r9
+    pop        r8
+    pop        rbp
+    pop        rdi
+    pop        rsi
+    pop        rdx
+    pop        rcx
+    pop        rbx
+    pop        rax
+
+    ret
+AsmExchangeRole   ENDP
+
 AsmInitializeGdt   PROC
     push       rbp
     mov        rbp, rsp
diff --git a/UefiCpuPkg/CpuMpPei/X64/MpFuncs.nasm b/UefiCpuPkg/CpuMpPei/X64/MpFuncs.nasm
index 09c2fbc15e..8b93c0d1f7 100644
--- a/UefiCpuPkg/CpuMpPei/X64/MpFuncs.nasm
+++ b/UefiCpuPkg/CpuMpPei/X64/MpFuncs.nasm
@@ -187,6 +187,112 @@ ASM_PFX(AsmGetAddressMap):
     mov        qword [rcx + 18h], RendezvousFunnelProcEnd - RendezvousFunnelProcStart
     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.
+
+    push       rax
+    push       rbx
+    push       rcx
+    push       rdx
+    push       rsi
+    push       rdi
+    push       rbp
+    push       r8
+    push       r9
+    push       r10
+    push       r11
+    push       r12
+    push       r13
+    push       r14
+    push       r15
+
+    mov        rax, cr0
+    push       rax
+
+    mov        rax, cr4
+    push       rax
+
+    ; rsi contains MyInfo pointer
+    mov        rsi, rcx
+
+    ; rdi contains OthersInfo pointer
+    mov        rdi, rdx
+
+    ;Store EFLAGS, GDTR and IDTR regiter to stack
+    pushfq
+    sgdt       [rsi + 16]
+    sidt       [rsi + 26]
+
+    ; Store the its StackPointer
+    mov        [rsi + 8], rsp
+
+    ; update its switch state to STORED
+    mov        byte [rsi], CPU_SWITCH_STATE_STORED
+
+WaitForOtherStored:
+    ; wait until the other CPU finish storing its state
+    cmp        byte [rdi], CPU_SWITCH_STATE_STORED
+    jz         OtherStored
+    pause
+    jmp        WaitForOtherStored
+
+OtherStored:
+    ; Since another CPU already stored its state, load them
+    ; load GDTR value
+    lgdt       [rdi + 16]
+
+    ; load IDTR value
+    lidt       [rdi + 26]
+
+    ; load its future StackPointer
+    mov        rsp, [rdi + 8]
+
+    ; update the other CPU's switch state to LOADED
+    mov        byte [rdi], CPU_SWITCH_STATE_LOADED
+
+WaitForOtherLoaded:
+    ; wait until the other CPU finish loading new state,
+    ; otherwise the data in stack may corrupt
+    cmp        byte [rsi], CPU_SWITCH_STATE_LOADED
+    jz         OtherLoaded
+    pause
+    jmp        WaitForOtherLoaded
+
+OtherLoaded:
+    ; since the other CPU already get the data it want, leave this procedure
+    popfq
+
+    pop        rax
+    mov        cr4, rax
+
+    pop        rax
+    mov        cr0, rax
+
+    pop        r15
+    pop        r14
+    pop        r13
+    pop        r12
+    pop        r11
+    pop        r10
+    pop        r9
+    pop        r8
+    pop        rbp
+    pop        rdi
+    pop        rsi
+    pop        rdx
+    pop        rcx
+    pop        rbx
+    pop        rax
+
+    ret
+
 global ASM_PFX(AsmInitializeGdt)
 ASM_PFX(AsmInitializeGdt):
     push       rbp