Fix the risk of AP stack conflict.

[mirror_edk2.git] / EdkCompatibilityPkg / Compatibility / MpServicesOnFrameworkMpServicesThunk / X64 / MpFuncs.S

diff --git a/EdkCompatibilityPkg/Compatibility/MpServicesOnFrameworkMpServicesThunk/X64/MpFuncs.S b/EdkCompatibilityPkg/Compatibility/MpServicesOnFrameworkMpServicesThunk/X64/MpFuncs.S
index 8efd2d183ed73fd0a76828ed466f8201fe569849..5ec90bdaac54db7f49576cbed8374c120b09f278 100644 (file)
--- a/EdkCompatibilityPkg/Compatibility/MpServicesOnFrameworkMpServicesThunk/X64/MpFuncs.S
+++ b/EdkCompatibilityPkg/Compatibility/MpServicesOnFrameworkMpServicesThunk/X64/MpFuncs.S
@@ -24,6 +24,7 @@
 .equ                   IdtrLocation,              RendezvousFunnelProcEnd - RendezvousFunnelProcStart + 0x2A\r
 .equ                   BufferStartLocation,       RendezvousFunnelProcEnd - RendezvousFunnelProcStart + 0x34\r
 .equ                   Cr3OffsetLocation,         RendezvousFunnelProcEnd - RendezvousFunnelProcStart + 0x38\r
+.equ                   ProcessorNumberLocation,   RendezvousFunnelProcEnd - RendezvousFunnelProcStart + 0x38\r
 \r
 #-------------------------------------------------------------------------------------\r
 \r
@@ -128,31 +129,33 @@ LongModeStart:
         .byte       0x66\r
         movw        %ax,%ss\r
 \r
-        movl        %esi, %edi\r
-        addl        $LockLocation, %edi\r
-        movb        $NotVacantFlag, %al\r
-TestLock:\r
-        xchgb       (%edi), %al\r
-        cmpb        $NotVacantFlag, %al\r
-        jz          TestLock\r
-\r
-ProgramStack:\r
+        #\r
+        # ProgramStack\r
+        #\r
+        movl        $0x1b, %ecx\r
+        rdmsr\r
+        andl        $0xfffff000, %eax\r
+        addl        $0x20, %eax\r
+        movl        (%eax), %ebx\r
+        shrl        $24, %ebx\r
+        \r
+        xorq        %rcx, %rcx\r
+        movl        %esi,%edi\r
+        addl        $ProcessorNumberLocation, %edi\r
+        movl        (%edi, %ebx, 4), %ecx\r
 \r
-        movl        %esi, %edi\r
-        addl        $StackSizeLocation, %edi \r
+        movl        %esi,%edi\r
+        addl        $StackSizeLocation, %edi\r
         movq        (%edi), %rax\r
-        movl        %esi, %edi\r
-        addl        $StackStartAddressLocation, %edi\r
-        addq        (%edi), %rax\r
-        movq        %rax, %rsp\r
-        movq        %rax, (%edi) \r
+        incq        %rcx\r
+        mulq        %rcx\r
 \r
-Releaselock:\r
+        movl        %esi,%edi\r
+        addl        $StackStartAddressLocation, %edi\r
+        movq        (%edi), %rbx\r
+        addq        %rbx, %rax\r
 \r
-        movb        $VacantFlag, %al\r
-        movl        %esi, %edi\r
-        addl        $LockLocation, %edi\r
-        xchgb       (%edi), %al\r
+        movq        %rax, %rsp\r
 \r
         #\r
         # Call C Function\r