[PATCH v2 3/3] UefiCpuPkg/MpInitLib: Use XADD to avoid lock acquire/release

["Ni, Ray" <ray.ni@intel.com>]

When AP firstly wakes up, MpFuncs.nasm contains below logic to assign
an unique ApIndex to each AP according to who comes first:
---NASM---
    mov        edi, esi
    add        edi, MP_CPU_EXCHANGE_INFO_FIELD (Lock)
    mov        eax, NotVacantFlag

TestLock:
    xchg       [edi], eax
    cmp        eax, NotVacantFlag
    jz         TestLock

    mov        ecx, esi
    add        ecx, MP_CPU_EXCHANGE_INFO_FIELD (ApIndex)
    inc        dword [ecx]
    mov        ebx, [ecx]

Releaselock:
    mov        eax, VacantFlag
    xchg       [edi], eax
---NASM END---

"LOCK INC" cannot be used to increase MP_CPU_EXCHANGE_INFO.ApIndex
because not only the MP_CPU_EXCHANGE_INFO.ApIndex should be
increased, but also the result should be stored to a thread local
general purpose register EBX.

This patch learns from the NASM implementation of
InternalSyncIncrement() to use "XADD" instruction which can increase
the global ApIndex and store the original ApIndex to EBX in one
instruction.

With this patch, OVMF when running in a 255 threads QEMU spends about
one second to wakeup all APs. Original implementation needs more than
10 seconds.

Signed-off-by: Ray Ni <ray.ni@intel.com>
Cc: Laszlo Ersek <lersek@redhat.com>
Cc: Eric Dong <eric.dong@intel.com>
Cc: Rahul1 Kumar <rahul1.kumar@intel.com>
---
 .../Library/MpInitLib/Ia32/MpFuncs.nasm       | 20 ++++---------------
 UefiCpuPkg/Library/MpInitLib/MpEqu.inc        |  4 ----
 UefiCpuPkg/Library/MpInitLib/MpLib.c          |  1 -
 UefiCpuPkg/Library/MpInitLib/MpLib.h          |  3 +--
 UefiCpuPkg/Library/MpInitLib/X64/MpFuncs.nasm | 18 ++++-------------
 5 files changed, 9 insertions(+), 37 deletions(-)

diff --git a/UefiCpuPkg/Library/MpInitLib/Ia32/MpFuncs.nasm b/UefiCpuPkg/Library/MpInitLib/Ia32/MpFuncs.nasm
index 2f1b102717..7bd2415670 100644
--- a/UefiCpuPkg/Library/MpInitLib/Ia32/MpFuncs.nasm
+++ b/UefiCpuPkg/Library/MpInitLib/Ia32/MpFuncs.nasm
@@ -122,22 +122,10 @@ SkipEnableExecuteDisable:
 
     ; AP init
     mov        edi, esi
-    add        edi, MP_CPU_EXCHANGE_INFO_FIELD (Lock)
-    mov        eax, NotVacantFlag
-
-TestLock:
-    xchg       [edi], eax
-    cmp        eax, NotVacantFlag
-    jz         TestLock
-
-    mov        ecx, esi
-    add        ecx, MP_CPU_EXCHANGE_INFO_FIELD (ApIndex)
-    inc        dword [ecx]
-    mov        ebx, [ecx]
-
-Releaselock:
-    mov        eax, VacantFlag
-    xchg       [edi], eax
+    add        edi, MP_CPU_EXCHANGE_INFO_FIELD (ApIndex)
+    mov        ebx, 1
+    lock xadd  dword [edi], ebx                 ; EBX = ApIndex++
+    inc        ebx                              ; EBX is CpuNumber
 
     mov        edi, esi
     add        edi, MP_CPU_EXCHANGE_INFO_FIELD (StackSize)
diff --git a/UefiCpuPkg/Library/MpInitLib/MpEqu.inc b/UefiCpuPkg/Library/MpInitLib/MpEqu.inc
index 46c2b5c116..2e9368a374 100644
--- a/UefiCpuPkg/Library/MpInitLib/MpEqu.inc
+++ b/UefiCpuPkg/Library/MpInitLib/MpEqu.inc
@@ -13,9 +13,6 @@
 ;-------------------------------------------------------------------------------
 %include "Nasm.inc"
 
-VacantFlag                    equ        00h
-NotVacantFlag                 equ        0ffh
-
 CPU_SWITCH_STATE_IDLE         equ        0
 CPU_SWITCH_STATE_STORED       equ        1
 CPU_SWITCH_STATE_LOADED       equ        2
@@ -72,7 +69,6 @@ endstruc
 ; Equivalent NASM structure of MP_CPU_EXCHANGE_INFO
 ;
 struc MP_CPU_EXCHANGE_INFO
-  .Lock:                         CTYPE_UINTN 1
   .StackStart:                   CTYPE_UINTN 1
   .StackSize:                    CTYPE_UINTN 1
   .CFunction:                    CTYPE_UINTN 1
diff --git a/UefiCpuPkg/Library/MpInitLib/MpLib.c b/UefiCpuPkg/Library/MpInitLib/MpLib.c
index 2568986d8c..5040053dad 100644
--- a/UefiCpuPkg/Library/MpInitLib/MpLib.c
+++ b/UefiCpuPkg/Library/MpInitLib/MpLib.c
@@ -1006,7 +1006,6 @@ FillExchangeInfoData (
   IA32_CR4                         Cr4;
 
   ExchangeInfo                  = CpuMpData->MpCpuExchangeInfo;
-  ExchangeInfo->Lock            = 0;
   ExchangeInfo->StackStart      = CpuMpData->Buffer;
   ExchangeInfo->StackSize       = CpuMpData->CpuApStackSize;
   ExchangeInfo->BufferStart     = CpuMpData->WakeupBuffer;
diff --git a/UefiCpuPkg/Library/MpInitLib/MpLib.h b/UefiCpuPkg/Library/MpInitLib/MpLib.h
index 02652eaae1..0bd60388b1 100644
--- a/UefiCpuPkg/Library/MpInitLib/MpLib.h
+++ b/UefiCpuPkg/Library/MpInitLib/MpLib.h
@@ -1,7 +1,7 @@
 /** @file
   Common header file for MP Initialize Library.
 
-  Copyright (c) 2016 - 2020, Intel Corporation. All rights reserved.<BR>
+  Copyright (c) 2016 - 2021, Intel Corporation. All rights reserved.<BR>
   Copyright (c) 2020, AMD Inc. All rights reserved.<BR>
 
   SPDX-License-Identifier: BSD-2-Clause-Patent
@@ -190,7 +190,6 @@ typedef struct _CPU_MP_DATA  CPU_MP_DATA;
 // into this structure are used in assembly code in this module
 //
 typedef struct {
-  UINTN                 Lock;
   UINTN                 StackStart;
   UINTN                 StackSize;
   UINTN                 CFunction;
diff --git a/UefiCpuPkg/Library/MpInitLib/X64/MpFuncs.nasm b/UefiCpuPkg/Library/MpInitLib/X64/MpFuncs.nasm
index bf7faaf60b..50df802d1f 100644
--- a/UefiCpuPkg/Library/MpInitLib/X64/MpFuncs.nasm
+++ b/UefiCpuPkg/Library/MpInitLib/X64/MpFuncs.nasm
@@ -158,21 +158,11 @@ LongModeStart:
 
     ; AP init
     mov        edi, esi
-    add        edi, MP_CPU_EXCHANGE_INFO_FIELD (Lock)
-    mov        rax, NotVacantFlag
+    add        edi, MP_CPU_EXCHANGE_INFO_FIELD (ApIndex)
+    mov        ebx, 1
+    lock xadd  dword [edi], ebx                 ; EBX = ApIndex++
+    inc        ebx                              ; EBX is CpuNumber
 
-TestLock:
-    xchg       qword [edi], rax
-    cmp        rax, NotVacantFlag
-    jz         TestLock
-
-    lea        ecx, [esi + MP_CPU_EXCHANGE_INFO_FIELD (ApIndex)]
-    inc        dword [ecx]
-    mov        ebx, [ecx]
-
-Releaselock:
-    mov        rax, VacantFlag
-    xchg       qword [edi], rax
     ; program stack
     mov        edi, esi
     add        edi, MP_CPU_EXCHANGE_INFO_FIELD (StackSize)
-- 
2.27.0.windows.1