UefiCpuPkg/CpuMpPei: Add AP reset x64 assembly code

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@17992 6f19259b-4bc3-4df7-8a09-765794883524

3 files changed, 320 insertions, 0 deletions

diff --git a/UefiCpuPkg/CpuMpPei/X64/MpEqu.inc b/UefiCpuPkg/CpuMpPei/X64/MpEqu.inc
index f59b2c4969..28a826c643 100644
--- a/UefiCpuPkg/CpuMpPei/X64/MpEqu.inc
+++ b/UefiCpuPkg/CpuMpPei/X64/MpEqu.inc
@@ -18,8 +18,25 @@
 ;
 ;-------------------------------------------------------------------------------
 
+PROTECT_MODE_CS               equ        10h
+PROTECT_MODE_DS               equ        18h
 LONG_MODE_CS                  equ        38h
 LONG_MODE_DS                  equ        30h
 
+VacantFlag                    equ        00h
+NotVacantFlag                 equ        0ffh
 
 
+LockLocation                  equ        (RendezvousFunnelProcEnd - RendezvousFunnelProcStart)
+StackStartAddressLocation     equ        LockLocation + 08h
+StackSizeLocation             equ        LockLocation + 10h
+ApProcedureLocation           equ        LockLocation + 18h
+GdtrLocation                  equ        LockLocation + 20h
+IdtrLocation                  equ        LockLocation + 2Ah
+BufferStartLocation           equ        LockLocation + 34h
+PmodeOffsetLocation           equ        LockLocation + 3Ch
+NumApsExecutingLoction        equ        LockLocation + 44h
+LmodeOffsetLocation           equ        LockLocation + 4Ch
+Cr3Location                   equ        LockLocation + 54h
+
+;-------------------------------------------------------------------------------
diff --git a/UefiCpuPkg/CpuMpPei/X64/MpFuncs.asm b/UefiCpuPkg/CpuMpPei/X64/MpFuncs.asm
index 13a7b5f430..5425547634 100644
--- a/UefiCpuPkg/CpuMpPei/X64/MpFuncs.asm
+++ b/UefiCpuPkg/CpuMpPei/X64/MpFuncs.asm
@@ -20,6 +20,160 @@
 
 include  MpEqu.inc
 .code
+;-------------------------------------------------------------------------------------
+;RendezvousFunnelProc  procedure follows. All APs execute their procedure. This
+;procedure serializes all the AP processors through an Init sequence. It must be
+;noted that APs arrive here very raw...ie: real mode, no stack.
+;ALSO THIS PROCEDURE IS EXECUTED BY APs ONLY ON 16 BIT MODE. HENCE THIS PROC
+;IS IN MACHINE CODE.
+;-------------------------------------------------------------------------------------
+RendezvousFunnelProc   PROC  PUBLIC
+RendezvousFunnelProcStart::
+; At this point CS = 0x(vv00) and ip= 0x0.
+; Save BIST information to ebp firstly
+    db 66h,  08bh, 0e8h               ; mov        ebp, eax    ; save BIST information
+
+    db 8ch,0c8h                       ; mov        ax,cs
+    db 8eh,0d8h                       ; mov        ds,ax
+    db 8eh,0c0h                       ; mov        es,ax
+    db 8eh,0d0h                       ; mov        ss,ax
+    db 33h,0c0h                       ; xor        ax,ax
+    db 8eh,0e0h                       ; mov        fs,ax
+    db 8eh,0e8h                       ; mov        gs,ax
+
+    db 0BEh                           ; opcode of mov si, mem16
+    dw BufferStartLocation            ; mov        si, BufferStartLocation
+    db 66h,  8Bh, 1Ch                 ; mov        ebx,dword ptr [si]
+
+    db 0BFh                           ; opcode of mov di, mem16
+    dw PmodeOffsetLocation            ; mov        di, PmodeOffsetLocation
+    db 66h,  8Bh, 05h                 ; mov        eax,dword ptr [di]
+    db 8Bh,  0F8h                     ; mov        di, ax
+    db 83h,  0EFh,06h                 ; sub        di, 06h
+    db 66h,  03h, 0C3h                ; add        eax, ebx
+    db 66h,  89h, 05h                 ; mov        dword ptr [di],eax
+
+    db 0BFh                           ; opcode of mov di, mem16
+    dw LmodeOffsetLocation            ; mov        di, LmodeOffsetLocation
+    db 66h,  8Bh, 05h                 ; mov        eax,dword ptr [di]
+    db 8Bh,  0F8h                     ; mov        di, ax
+    db 83h,  0EFh,06h                 ; sub        di, 06h
+    db 66h,  03h, 0C3h                ; add        eax, ebx
+    db 66h,  89h, 05h                 ; mov        dword ptr [di],eax
+
+    db 0BEh
+    dw Cr3Location                    ; mov        si, Cr3Location
+    db 66h,  8Bh, 0Ch                 ; mov        ecx,dword ptr [si]     ; ECX is keeping the value of CR3
+
+    db 0BEh                           ; opcode of mov si, mem16
+    dw GdtrLocation                   ; mov        si, GdtrLocation
+    db 66h                            ; db         66h
+    db 2Eh,  0Fh, 01h, 14h            ; lgdt       fword ptr cs:[si]
+
+    db 0BEh
+    dw IdtrLocation                   ; mov        si, IdtrLocation
+    db 66h                            ; db         66h
+    db 2Eh,0Fh, 01h, 1Ch              ; lidt       fword ptr cs:[si]
+
+    db 33h,  0C0h                     ; xor        ax,  ax
+    db 8Eh,  0D8h                     ; mov        ds,  ax
+
+    db 0Fh,  20h, 0C0h                ; mov        eax, cr0               ;Get control register 0
+    db 66h,  83h, 0C8h, 03h           ; or         eax, 000000003h        ;Set PE bit (bit #0) & MP
+    db 0Fh,  22h, 0C0h                ; mov        cr0, eax
+
+    db 66h,  67h, 0EAh                ; far jump
+    dd 0h                             ; 32-bit offset
+    dw PROTECT_MODE_CS                ; 16-bit selector
+
+Flat32Start::                         ; protected mode entry point
+    mov        ax, PROTECT_MODE_DS
+    mov        ds, ax
+    mov        es, ax
+    mov        fs, ax
+    mov        gs, ax
+    mov        ss, ax
+
+    db 0Fh,  20h,  0E0h           ; mov        eax, cr4
+    db 0Fh,  0BAh, 0E8h, 05h      ; bts        eax, 5
+    db 0Fh,  22h,  0E0h           ; mov        cr4, eax
+
+    db 0Fh,  22h,  0D9h           ; mov        cr3, ecx
+
+    db 0B9h
+    dd 0C0000080h                 ; mov        ecx, 0c0000080h     ; EFER MSR number.
+    db 0Fh,  32h                  ; rdmsr                          ; Read EFER.
+    db 0Fh,  0BAh, 0E8h, 08h      ; bts        eax, 8              ; Set LME=1.
+    db 0Fh,  30h                  ; wrmsr                          ; Write EFER.
+
+    db 0Fh,  20h,  0C0h           ; mov        eax, cr0            ; Read CR0.
+    db 0Fh,  0BAh, 0E8h, 1Fh      ; bts        eax, 31             ; Set PG=1.
+    db 0Fh,  22h,  0C0h           ; mov        cr0, eax            ; Write CR0.
+
+LONG_JUMP:
+    db 67h,  0EAh                 ; far jump
+    dd 0h                         ; 32-bit offset
+    dw LONG_MODE_CS               ; 16-bit selector
+
+LongModeStart::
+    mov        ax,  LONG_MODE_DS
+    mov        ds,  ax
+    mov        es,  ax
+    mov        ss,  ax
+
+    mov        esi, ebx
+    mov        edi, esi
+    add        edi, LockLocation
+    mov        rax, NotVacantFlag
+
+TestLock:
+    xchg       qword ptr [edi], rax
+    cmp        rax, NotVacantFlag
+    jz         TestLock
+
+    mov        edi, esi
+    add        edi, NumApsExecutingLoction
+    inc        dword ptr [edi]
+    mov        ebx, dword ptr [edi]
+
+ProgramStack:
+    mov        edi, esi
+    add        edi, StackSizeLocation
+    mov        rax, qword ptr [edi]
+    mov        edi, esi
+    add        edi, StackStartAddressLocation
+    add        rax, qword ptr [edi]
+    mov        rsp, rax
+    mov        qword ptr [edi], rax
+
+Releaselock:
+    mov        rax, VacantFlag
+    mov        edi, esi
+    add        edi, LockLocation
+    xchg       qword ptr [edi], rax
+
+CProcedureInvoke:
+    push       rbp               ; push BIST data
+    xor        rbp, rbp          ; clear ebp for call stack trace
+    push       rbp
+    mov        rbp, rsp
+
+
+    mov        edx, ebx          ; edx is NumApsExecuting
+    mov        ecx, esi
+    add        ecx, LockLocation ; rcx is address of exchange info data buffer
+
+    mov        edi, esi
+    add        edi, ApProcedureLocation
+    mov        rax, qword ptr [edi]
+
+    sub        rsp, 20h
+    call       rax               ; invoke C function
+    add        rsp, 20h
+    jmp        $
+
+RendezvousFunnelProc   ENDP
+RendezvousFunnelProcEnd::
 
 
 AsmInitializeGdt   PROC
diff --git a/UefiCpuPkg/CpuMpPei/X64/MpFuncs.nasm b/UefiCpuPkg/CpuMpPei/X64/MpFuncs.nasm
index bd595724c2..99669ce95e 100644
--- a/UefiCpuPkg/CpuMpPei/X64/MpFuncs.nasm
+++ b/UefiCpuPkg/CpuMpPei/X64/MpFuncs.nasm
@@ -19,9 +19,158 @@
 ;-------------------------------------------------------------------------------
 
 %include "MpEqu.inc"
+
 DEFAULT REL
+
 SECTION .text
 
+;-------------------------------------------------------------------------------------
+;RendezvousFunnelProc  procedure follows. All APs execute their procedure. This
+;procedure serializes all the AP processors through an Init sequence. It must be
+;noted that APs arrive here very raw...ie: real mode, no stack.
+;ALSO THIS PROCEDURE IS EXECUTED BY APs ONLY ON 16 BIT MODE. HENCE THIS PROC
+;IS IN MACHINE CODE.
+;-------------------------------------------------------------------------------------
+global ASM_PFX(RendezvousFunnelProc)
+ASM_PFX(RendezvousFunnelProc):
+RendezvousFunnelProcStart:
+; At this point CS = 0x(vv00) and ip= 0x0.
+; Save BIST information to ebp firstly
+BITS 16
+
+    mov        eax, 1234h
+    mov        ebp, eax                        ; save BIST information
+
+    mov        ax, cs
+    mov        ds, ax
+    mov        es, ax
+    mov        ss, ax
+    xor        ax, ax
+    mov        fs, ax
+    mov        gs, ax
+
+    mov        si,  BufferStartLocation
+    mov        ebx, [si]
+
+    mov        di,  PmodeOffsetLocation
+    mov        eax, [di]
+    mov        di,  ax
+    sub        di,  06h
+    add        eax, ebx
+    mov        [di],eax
+
+    mov        di, LmodeOffsetLocation
+    mov        eax, [di]
+    mov        di,  ax
+    sub        di,  06h
+    add        eax, ebx
+    mov        [di],eax
+
+
+    mov        si, Cr3Location
+    mov        ecx,[si]                        ; ECX is keeping the value of CR3
+
+    mov        si, GdtrLocation
+o32 lgdt       [cs:si]
+
+    mov        si, IdtrLocation
+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        cr0, eax
+
+    jmp        PROTECT_MODE_CS:strict dword 0  ; far jump to protected mode
+BITS 32
+Flat32Start:                                   ; protected mode entry point
+    mov        ax, PROTECT_MODE_DS
+    mov        ds, ax
+    mov        es, ax
+    mov        fs, ax
+    mov        gs, ax
+    mov        ss, ax
+
+    mov        eax, cr4
+    bts        eax, 5
+    mov        cr4, eax
+
+    mov        cr3, ecx
+
+
+    mov        ecx, 0c0000080h             ; EFER MSR number.
+    rdmsr                                  ; Read EFER.
+    bts        eax, 8                      ; Set LME=1.
+    wrmsr                                  ; Write EFER.
+
+    mov        eax, cr0                    ; Read CR0.
+    bts        eax, 31                     ; Set PG=1.
+    mov        cr0, eax                    ; Write CR0.
+
+    jmp        LONG_MODE_CS:strict dword 0 ; far jump to long mode
+BITS 64
+LongModeStart:
+    mov        ax,  LONG_MODE_DS
+    mov        ds,  ax
+    mov        es,  ax
+    mov        ss,  ax
+
+    mov        esi, ebx
+    mov        edi, esi
+    add        edi, LockLocation
+    mov        rax, NotVacantFlag
+
+TestLock:
+    xchg       qword [edi], rax
+    cmp        rax, NotVacantFlag
+    jz         TestLock
+
+    mov        edi, esi
+    add        edi, NumApsExecutingLoction
+    inc        dword [edi]
+    mov        ebx, [edi]
+
+ProgramStack:
+    mov        edi, esi
+    add        edi, StackSizeLocation
+    mov        rax, qword [edi]
+    mov        edi, esi
+    add        edi, StackStartAddressLocation
+    add        rax, qword [edi]
+    mov        rsp, rax
+    mov        qword [edi], rax
+
+Releaselock:
+    mov        rax, VacantFlag
+    mov        edi, esi
+    add        edi, LockLocation
+    xchg       qword [edi], rax
+
+CProcedureInvoke:
+    push       rbp               ; push BIST data at top of AP stack
+    xor        rbp, rbp          ; clear ebp for call stack trace
+    push       rbp
+    mov        rbp, rsp
+
+
+    mov        edx, ebx          ; edx is NumApsExecuting
+    mov        ecx, esi
+    add        ecx, LockLocation ; rcx is address of exchange info data buffer
+
+    mov        edi, esi
+    add        edi, ApProcedureLocation
+    mov        rax, qword [edi]
+
+    sub        rsp, 20h
+    call       rax               ; invoke C function
+    add        rsp, 20h
+
+RendezvousFunnelProcEnd:
+
+
 global ASM_PFX(AsmInitializeGdt)
 ASM_PFX(AsmInitializeGdt):
     push       rbp