init. 1AQQW051HEADmaster

1 files changed, 618 insertions, 0 deletions

diff --git a/ReferenceCode/Haswell/CpuInit/Dxe/x64/MpFuncs.asm b/ReferenceCode/Haswell/CpuInit/Dxe/x64/MpFuncs.asm
new file mode 100644
index 0000000..58714f8
--- /dev/null
+++ b/ReferenceCode/Haswell/CpuInit/Dxe/x64/MpFuncs.asm
@@ -0,0 +1,618 @@
+;
+; This file contains an 'Intel Peripheral Driver' and is      
+; licensed for Intel CPUs and chipsets under the terms of your
+; license agreement with Intel or your vendor.  This file may 
+; be modified by the user, subject to additional terms of the 
+; license agreement                                           
+;
+;-------------------------------------------------------------------------------
+;
+; Copyright (c) 1999 - 2012 Intel Corporation. All rights reserved
+; This software and associated documentation (if any) is furnished
+; under a license and may only be used or copied in accordance
+; with the terms of the license. Except as permitted by such
+; license, no part of this software or documentation may be
+; reproduced, stored in a retrieval system, or transmitted in any
+; form or by any means without the express written consent of
+; Intel Corporation.
+;
+;
+; Module Name:
+;
+;   MpFuncs.asm
+; 
+; Abstract:
+; 
+;   This is the assembly code for EM64T MP support
+;
+;-------------------------------------------------------------------------------
+
+
+include  MpEqu.inc 
+CpuInitFloatPointUnit PROTO C
+
+;-------------------------------------------------------------------------------------
+
+;-------------------------------------------------------------------------------------
+;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 (&WakeUpBuffer,MemAddress);
+
+text      SEGMENT
+
+RendezvousFunnelProc   PROC  PUBLIC
+RendezvousFunnelProcStart::
+
+; At this point CS = 0x(vv00) and ip= 0x0.
+        db 66h,  08bh, 0e8h           ; mov        ebp, eax
+
+        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
+
+; Get APIC ID
+;
+        db 66h,  0B8h
+        dd 00000001h                  ; mov        eax, 1
+        db 0Fh,  0A2h                 ; cpuid
+        db 66h,  0C1h, 0EBh, 18h      ; shr        ebx, 24
+        db 66h,  81h,  0E3h
+        dd 000000FFh                  ; and        ebx, 0ffh                   ; EBX is APIC ID
+
+; If it is the first time AP wakes up, just record AP's BIST
+; Otherwise, switch to protected mode.
+
+        db 0BEh                       ; opcode of mov si, imm16
+        dw InitFlagLocation           ; mov        si,  InitFlag
+        db 66h,  83h, 3Ch, 00h        ; cmp        dword ptr [si], 0
+        db 74h                        ; opcode of jz
+        db flat32Start - ($ + 1)      ; jz         flat32Start
+
+; Record BIST information
+;        
+        db 0B0h, 08h                  ; mov        al,  8
+        db 0F6h, 0E3h                 ; mul        bl
+
+        db 0BEh                       ; opcode of mov si, imm16
+        dw BistBuffer                 ; mov        si,  BistBuffer
+        db 03h,  0F0h                 ; add        si,  ax
+
+        db 66h,  0C7h, 04h
+        dd 00000001h                  ; mov        dword ptr [si], 1           ; Set Valid Flag
+        db 66h,  89h,  6Ch,  04h      ; mov        dword ptr [si + 4], ebp     ; Store BIST value        
+
+;
+; Switch to flat mode.
+;
+flat32Start::
+
+        db 0BFh                       ; opcode of mov di, imm16
+        dw BufferStartLocation        ; mov        di, BufferStartLocation
+        db 66h,  8Bh, 35h             ; mov        esi,dword ptr [di]          ; ESI is keeping the start address of wakeup buffer
+
+        db 0BFh                       ; opcode of mov di, imm16
+        dw Cr3OffsetLocation          ; mov        di, Cr3Location
+        db 66h,  8Bh, 0Dh             ; mov        ecx,dword ptr [di]          ; ECX is keeping the value of CR3
+
+        db 0BFh                       ; opcode of mov di, imm16
+        dw GdtrLocation               ; mov        di, GdtrProfile
+        db 66h                        ; db         66h
+        db 2Eh,  0Fh, 01h, 15h        ; lgdt       fword ptr cs:[di]
+
+        db 0BFh                       ; opcode of mov di, imm16
+        dw IdtrLocation               ; mov        di, IdtrProfile
+        db 66h                        ; db         66h
+        db 2Eh,  0Fh, 01h, 1Dh        ; lidt       fword ptr cs:[di]
+
+        db 0BFh                       ; opcode of mov di, imm16
+        dw LongModeStartJump - RendezvousFunnelProcStart                       ; Get offset of LongModeStartJump
+        db 66h,  8Bh, 3Dh             ; mov        edi,dword ptr [di]          ; EDI is keeping the LongModeStart Jump Address 
+
+        db 31h,  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) and MP
+        db 0Fh,  22h, 0C0h            ; mov        cr0, eax
+
+FLAT32_JUMP::
+
+        db 66h,  67h, 0EAh            ; far jump
+        dd 0h                         ; 32-bit offset
+        dw 20h                        ; 16-bit selector
+
+NemInit::                             ; 32-bits protected mode entry point
+
+        db 66h,  0B8h, 18h,  00h      ; mov        ax,  18h
+        db 66h,  8Eh,  0D8h           ; mov        ds,  ax
+        db 66h,  8Eh,  0C0h           ; mov        es,  ax
+        db 66h,  8Eh,  0E0h           ; mov        fs,  ax
+        db 66h,  8Eh,  0E8h           ; mov        gs,  ax
+        db 66h,  8Eh,  0D0h           ; mov        ss,  ax                     ; Flat mode setup.
+
+
+PrepareToGoLongMode64::
+
+        db 0Fh,  20h,  0E0h           ; mov        eax, cr4
+        db 66h,  0Dh,  020h, 06h      ; or         ax,  0620h                  ; Set PAE=1, OSFXSR=1, OSXMMEXCPT=1.
+        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
+
+LongModeStartJump:
+
+        dd 0h                         ; 32-bit offset
+        dw 38h                        ; 16-bit selector
+
+
+LongModeStart::
+
+        mov         ax,  30h
+        mov         ds,  ax
+        mov         es,  ax
+        mov         ss,  ax
+
+WaitFirstApTaskAssigned::
+;
+; First INIT-SIPI-SIPI will loop here until DetailedMpInitialization function assigned for each AP
+;
+        pause
+        cmp         qword ptr [esi+CProcedureLocation], 0
+        jz          WaitFirstApTaskAssigned
+
+;
+; Patch Addresses for jumping between RUN and MONITOR MWAIT loops 32-bits and Long Monde Procedure 64-bits
+; Based on the running address of LongModeStart in physic memory which was actually copied by CPU DXE INIT
+;
+        xor         rdx, rdx
+        mov         eax, edi
+        add         eax, RunLoopAndMwaitLoop32 - LongModeStart
+        mov         edx, edi
+        add         edx, RunLoopAndMwaitLoop32Jump - LongModeStart
+        mov         dword ptr [rdx], eax
+
+        mov         rbp, rdx                          ; RBP = 32-bits compatibility mode FAR JUMP m16:32 operand pointer
+
+        mov         eax, edi
+        add         eax, RunLoopAndMwaitLoop64 - LongModeStart
+        mov         edx, edi
+        add         edx, RunLoopAndMwaitLoop64Jump - LongModeStart
+        mov         dword ptr [rdx], eax
+
+; 
+; ProgramStack
+;
+        xor         rcx, rcx
+        mov         edi, esi
+        add         edi, BistBuffer
+        mov         ecx, dword ptr [edi + 8 * ebx]    ; RCX = CpuNumber
+
+        mov         edi, esi
+        add         edi, StackSizeLocation
+        mov         rax, qword ptr [edi]
+        inc         rcx
+        mul         rcx                               ; RAX = StackSize * (CpuNumber + 1)
+
+        mov         edi, esi
+        add         edi, StackStartAddressLocation
+        mov         rdx, qword ptr [edi]
+        add         rax, rdx                          ; RAX = StackStart + StackSize * (CpuNumber + 1)
+
+        mov         rsp, rax
+        sub         rsp, MonitorFilterSize            ; Reserved Monitor data space
+        or          ebx, BreakToRunApSignal           ; ebx = #Cpu run signature
+
+;
+; Call assembly function to initialize FPU.
+;
+        mov         rax, CpuInitFloatPointUnit
+        sub         rsp, 20h
+        call        rax
+        add         rsp, 20h
+        
+;
+; Load C Function pointer and wakeup manner location
+;
+        mov         edi, esi
+        add         edi, CProcedureLocation
+        add         esi, WakeUpApManner               ; esi = WakeUpApManner Address Location
+
+WakeUpThisAp64::
+
+        mov         rax, qword ptr [edi]
+
+        test        rax, rax
+        jz          CheckWakeUpCounterInit64
+
+        push        rbp
+        push        rbx
+        push        rsi
+        push        rdi
+
+        sub         rsp, 20h
+        call        rax
+        add         rsp, 20h
+
+        pop         rdi
+        pop         rsi
+        pop         rbx
+        pop         rbp
+
+CheckWakeUpCounterInit64::
+
+        cmp         dword ptr [esi], WakeUpApCounterInit
+        jnz         CheckWakeUpManner64
+
+;
+; Initialize MONITOR_MWAIT_DATA data structure per thread
+;
+        xor         rcx, rcx
+        mov         qword ptr [rsp +  0], rcx         ; BreakToRunApSignal
+        mov         qword ptr [rsp +  8], rcx         ; HltLoopBreakCounter
+        mov         qword ptr [rsp + 16], rcx         ; MwaitLoopBreakCounter
+        mov         qword ptr [rsp + 24], rcx         ; RunLoopBreakCounter
+        mov         qword ptr [rsp + 32], rcx         ; MwaitLoopBreakCounter32
+        mov         qword ptr [rsp + 40], rcx         ; RunLoopBreakCounter32
+        mov         qword ptr [rsp + 48], rcx         ; WakeUpApVectorChangeFlag
+        mov         qword ptr [rsp + 56], rcx         ; MwaitTargetCstate
+
+WaitWakeUpMannerAssigned::
+
+        pause
+        cmp         dword ptr [esi], WakeUpApCounterInit
+        jz          WaitWakeUpMannerAssigned
+
+CheckWakeUpManner64::
+
+        pause
+        mov         edx, dword ptr [esi]
+        cmp         edx, WakeUpApPerHltLoop
+        jz          HltApLoop64
+
+        cmp         edx, WakeUpApPerMwaitLoop
+        jz          ApMwaitLoop64
+
+        cmp         edx, WakeUpApPerRunLoop
+        jz          CheckRunSignal64
+
+        jmp         JumpToCompatibility32Mode
+
+ApMwaitLoop64::
+
+        cli
+        mov         rax, rsp                          ; Set Monitor Address
+        xor         rcx, rcx
+        xor         rdx, rdx
+        DB          0fh, 1, 0c8h                      ; MONITOR
+        mov         rax, qword ptr [rsp + 56]         ; Mwait Target C-State per rax[7:4]
+        DB          0fh, 1, 0c9h                      ; MWAIT
+
+CheckRunSignal64::
+
+        cmp         qword ptr [rsp], rbx              ; Check if run signal correct?
+        jnz         CheckWakeUpManner64               ; Unknown break, go checking run manner
+
+        jmp         WakeUpThisAp64                    ; Jmp to execute AP task
+
+HltApLoop64::
+
+        cli
+        hlt
+        jmp         HltApLoop64                       ; Jump to halt loop
+
+
+JumpToCompatibility32Mode::
+
+        db 0FFh, 6Dh, 0     ; jmp pword ptr [rbp+0]   ; Far jump to m16:32 for 32-bits compatibility mode
+
+RunLoopAndMwaitLoop32Jump:
+
+        dd 0h                                         ; m32 part of m16:32
+        dw 20h                                        ; m16 part of m16:32
+
+RunLoopAndMwaitLoop32::
+
+        db 66h,  0B8h, 18h,  00h      ; mov        ax,  18h
+        db 66h,  8Eh,  0D8h           ; mov        ds,  ax
+        db 8eh,  0d0h                 ; mov        ss,  ax 
+
+        db 0Fh,  20h,  0C0h           ; mov        eax, cr0                    ; Read CR0.
+        db 0Fh,  0BAh, 0F0h, 1Fh      ; btr        eax, 31                     ; Reset PG=0.
+        db 0Fh,  22h,  0C0h           ; mov        cr0, eax                    ; Write CR0.
+
+        db 0B9h
+        dd 0C0000080h                 ; mov        ecx, 0c0000080h             ; EFER MSR number.
+        db 0Fh,  32h                  ; rdmsr                                  ; Read EFER.
+        db 0Fh,  0BAh, 0F0h, 08h      ; btr        eax, 8                      ; Reset LME=0.
+        db 0Fh,  30h                  ; wrmsr                                  ; Write EFER.
+
+        db 0Fh,  20h,  0E0h           ; mov        eax, cr4
+        db 24h,  0DFh                 ; and        al,  0DFh                   ; Reset PAE=0 in CR4 bit 5
+        db 0Fh,  22h,  0E0h           ; mov        cr4, eax
+
+CheckWakeUpManner32::
+
+        pause
+        cmp         dword ptr [rsi], WakeUpApPerMwaitLoop32                    ; Use rsi for esi per compling in 64-bits mode
+        jnz         CheckRunSignal32
+
+        cli
+        mov         eax, esp                          ; Set Monitor Address
+        xor         ecx, ecx
+        xor         edx, edx
+        DB          0fh, 1, 0c8h                      ; MONITOR
+        mov         eax, dword ptr [rsp + 56]         ; Mwait Target C-State per eax[7:4]
+        DB          0fh, 1, 0c9h                      ; MWAIT
+
+
+CheckRunSignal32::
+
+        cmp         dword ptr [rsp], ebx              ; Check if run signal correct?
+        jnz         CheckWakeUpManner32               ; Unknown break, go checking run manner
+
+        db 0Fh,  20h,  0E0h           ; mov        eax, cr4
+        db 0Ch,  20h                  ; or         al,  20h                    ; Set PAE=1 in CR4 bit 5
+        db 0Fh,  22h,  0E0h           ; mov        cr4, eax
+
+        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.
+
+        db 67h,  0EAh                 ; far jump back to 64-bits long mode
+
+RunLoopAndMwaitLoop64Jump:
+
+        dd 0h                         ; 32-bit offset
+        dw 38h                        ; 16-bit selector
+
+RunLoopAndMwaitLoop64::
+
+        mov         ax,  30h
+        mov         ds,  ax
+        mov         ss,  ax
+
+        jmp         WakeUpThisAp64
+
+RendezvousFunnelProc   ENDP
+RendezvousFunnelProcEnd::
+
+
+;-------------------------------------------------------------------------------------
+;  AsmGetAddressMap (&AddressMap);
+;-------------------------------------------------------------------------------------
+AsmGetAddressMap   PROC   PUBLIC
+
+        mov         rax, offset RendezvousFunnelProcStart
+        mov         qword ptr [rcx], rax
+        mov         qword ptr [rcx+8h], NemInit - RendezvousFunnelProcStart
+        mov         qword ptr [rcx+10h], FLAT32_JUMP - RendezvousFunnelProcStart
+        mov         qword ptr [rcx+18h], LongModeStart - RendezvousFunnelProcStart
+        mov         qword ptr [rcx+20h], LONG_JUMP - RendezvousFunnelProcStart
+        mov         qword ptr [rcx+28h], RendezvousFunnelProcEnd - RendezvousFunnelProcStart
+
+        ret
+
+AsmGetAddressMap   ENDP
+
+AsmGetGdtrIdtr   PROC    PUBLIC
+
+        sgdt        GdtDesc
+        lea         rax, GdtDesc
+        mov         [rcx], rax
+
+        sidt        IdtDesc
+        lea         rax, IdtDesc
+        mov         [rdx], rax
+
+        ret
+
+AsmGetGdtrIdtr   ENDP
+
+AsmAcquireMPLock   PROC    PUBLIC
+
+        mov         al, NotVacantFlag
+TryGetLock:
+        xchg        al, byte ptr [rcx]
+        cmp         al, VacantFlag
+        jz          LockObtained
+
+        pause
+        jmp         TryGetLock       
+
+LockObtained:
+        ret
+
+AsmAcquireMPLock   ENDP
+
+AsmReleaseMPLock   PROC    PUBLIC
+
+        mov         al, VacantFlag
+        xchg        al, byte ptr [rcx]
+
+        ret
+
+AsmReleaseMPLock   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);
+;-------------------------------------------------------------------------------------
+CPU_SWITCH_STATE_IDLE          equ        0
+CPU_SWITCH_STATE_STORED        equ        1
+CPU_SWITCH_STATE_LOADED        equ        2
+
+AsmExchangeRole   PROC    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 functiosn, 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         al, NotVacantFlag
+TryLock1:
+        lock xchg   al, byte ptr [rsi]
+        cmp         al, VacantFlag
+        jz          LockObtained1
+        pause
+        jmp         TryLock1
+
+LockObtained1:
+        mov         byte ptr [rsi + 1], CPU_SWITCH_STATE_STORED
+        lock xchg   al, byte ptr [rsi]
+
+WaitForOtherStored::        
+        ; wait until the other CPU finish storing its state
+        mov         al, NotVacantFlag
+TryLock2:
+        lock xchg   al, byte ptr [rdi]
+        cmp         al, VacantFlag
+        jz          LockObtained2
+        PAUSE32
+        jmp         TryLock2
+
+LockObtained2:
+        mov         bl, byte ptr [rdi + 1]
+        lock xchg   al, byte ptr [rdi]
+        cmp         bl, CPU_SWITCH_STATE_STORED
+        jb          WaitForOtherStored
+
+        ; 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 its switch state to LOADED
+        mov         al, NotVacantFlag
+TryLock3:
+        lock xchg   al, byte ptr [rsi]
+        cmp         al, VacantFlag
+        jz          LockObtained3
+        PAUSE32
+        jmp         TryLock3
+
+LockObtained3:
+        mov         byte ptr [rsi+1], CPU_SWITCH_STATE_LOADED
+        lock xchg   al, byte ptr [rsi]
+
+WaitForOtherLoaded::
+        ; wait until the other CPU finish loading new state,
+        ; otherwise the data in stack may corrupt
+        mov         al, NotVacantFlag
+TryLock4:
+        lock xchg   al, byte ptr [rdi]
+        cmp         al, VacantFlag
+        jz          LockObtained4
+        PAUSE32
+        jmp         TryLock4
+
+LockObtained4:
+        mov         bl, byte ptr [rdi+1]
+        lock xchg   al, byte ptr [rdi]
+        cmp         bl, CPU_SWITCH_STATE_LOADED
+        jb          WaitForOtherLoaded
+
+        ; 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
+
+GdtDesc             QWORD    0
+                    WORD     0
+
+IdtDesc             QWORD    0
+                    WORD     0
+
+text    ENDS
+
+END