Fix the risk of AP stack conflict.

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

diff --git a/EdkCompatibilityPkg/Compatibility/MpServicesOnFrameworkMpServicesThunk/IA32/MpFuncs.S b/EdkCompatibilityPkg/Compatibility/MpServicesOnFrameworkMpServicesThunk/IA32/MpFuncs.S
index 1ddf11518f3787f5ef5651773a2402f4e56c37a1..f1b6bc391ae4852e4254d0cda5a731f16437d386 100644 (file)
--- a/EdkCompatibilityPkg/Compatibility/MpServicesOnFrameworkMpServicesThunk/IA32/MpFuncs.S
+++ b/EdkCompatibilityPkg/Compatibility/MpServicesOnFrameworkMpServicesThunk/IA32/MpFuncs.S
@@ -22,6 +22,7 @@
 #define GdtrProfile      RendezvousFunnelProcEnd - RendezvousFunnelProcStart + 0x10\r
 #define IdtrProfile      RendezvousFunnelProcEnd - RendezvousFunnelProcStart + 0x16\r
 #define BufferStart      RendezvousFunnelProcEnd - RendezvousFunnelProcStart + 0x1C\r
+#define ProcessorNumber, RendezvousFunnelProcEnd - RendezvousFunnelProcStart + 0x20\r
 \r
 #-------------------------------------------------------------------------------------\r
 #RendezvousFunnelProc  procedure follows. All APs execute their procedure. This\r
@@ -94,30 +95,33 @@ ProtectedModeStart:                   # protected mode entry point
         .byte       0x66\r
         movw        %ax,%ss           # Flat mode setup.\r
 \r
-\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
+        xorl        %ecx, %ecx\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
+        addl        $ProcessorNumber, %edi\r
+        movl        (%edi, %ebx, 4), %ecx\r
+\r
         movl        %esi,%edi\r
         addl        $StackSize, %edi\r
         movl        (%edi), %eax\r
+        incl        %ecx\r
+        mull        %ecx\r
+\r
         movl        %esi,%edi\r
         addl        $StackStart, %edi\r
-        addl        (%edi), %eax\r
-        movl        %eax,%esp\r
-        movl        %eax, (%edi)\r
+        movl        (%edi), %ebx\r
+        addl        %ebx, %eax\r
 \r
-Releaselock:\r
-        movb        $VacantFlag, %al\r
-        movl        %esi,%edi\r
-        addl        $LockLocation, %edi\r
-        xchgb       (%edi), %al\r
+        movl        %eax, %esp\r
 \r
         #\r
         # Call C Function\r