[PATCH v2 01/10] OvmfPkg/CpuHotplugSmm: move CPU Hotplug into PlugCpus()

["Ankur Arora" <ankur.a.arora@oracle.com>]

Refactor CpuHotplugMmi() to pull out the CPU hotplug logic into
PlugCpus(). This is in preparation for supporting CPU hot-unplug.

Cc: Laszlo Ersek <lersek@redhat.com>
Cc: Jordan Justen <jordan.l.justen@intel.com>
Cc: Ard Biesheuvel <ard.biesheuvel@arm.com>
Cc: Igor Mammedov <imammedo@redhat.com>
Cc: Boris Ostrovsky <boris.ostrovsky@oracle.com>
Cc: Aaron Young <aaron.young@oracle.com>
Ref: https://bugzilla.tianocore.org/show_bug.cgi?id=3132
Signed-off-by: Ankur Arora <ankur.a.arora@oracle.com>
---
 OvmfPkg/CpuHotplugSmm/CpuHotplug.c | 208 ++++++++++++++++++++++---------------
 1 file changed, 123 insertions(+), 85 deletions(-)

diff --git a/OvmfPkg/CpuHotplugSmm/CpuHotplug.c b/OvmfPkg/CpuHotplugSmm/CpuHotplug.c
index cfe698ed2b5e..0f8f210d0ecf 100644
--- a/OvmfPkg/CpuHotplugSmm/CpuHotplug.c
+++ b/OvmfPkg/CpuHotplugSmm/CpuHotplug.c
@@ -62,6 +62,124 @@ STATIC UINT32 mPostSmmPenAddress;
 //
 STATIC EFI_HANDLE mDispatchHandle;
 
+/**
+  CPU Hotplug handler function.
+
+  @param[in] mPluggedApicIds     List of APIC IDs to be plugged.
+
+  @param[in] PluggedCount        Count of APIC IDs to be plugged.
+
+  @retval EFI_SUCCESS	         Some of the requested APIC IDs were hot-plugged.
+
+  @retval EFI_INTERRUPT_PENDING  Fatal error while hot-plugging.
+
+**/
+STATIC
+EFI_STATUS
+EFIAPI
+PlugCpus(
+  IN APIC_ID                      *mPluggedApicIds,
+  IN UINT32                       PluggedCount
+  )
+{
+  EFI_STATUS Status;
+  UINT32     PluggedIdx;
+  UINT32     NewSlot;
+
+  //
+  // Process hot-added CPUs.
+  //
+  // The Post-SMM Pen need not be reinstalled multiple times within a single
+  // root MMI handling. Even reinstalling once per root MMI is only prudence;
+  // in theory installing the pen in the driver's entry point function should
+  // suffice.
+  //
+  SmbaseReinstallPostSmmPen (mPostSmmPenAddress);
+
+  PluggedIdx = 0;
+  NewSlot = 0;
+  while (PluggedIdx < PluggedCount) {
+    APIC_ID NewApicId;
+    UINT32  CheckSlot;
+    UINTN   NewProcessorNumberByProtocol;
+
+    NewApicId = mPluggedApicIds[PluggedIdx];
+
+    //
+    // Check if the supposedly hot-added CPU is already known to us.
+    //
+    for (CheckSlot = 0;
+         CheckSlot < mCpuHotPlugData->ArrayLength;
+         CheckSlot++) {
+      if (mCpuHotPlugData->ApicId[CheckSlot] == NewApicId) {
+        break;
+      }
+    }
+    if (CheckSlot < mCpuHotPlugData->ArrayLength) {
+      DEBUG ((DEBUG_VERBOSE, "%a: APIC ID " FMT_APIC_ID " was hot-plugged "
+        "before; ignoring it\n", __FUNCTION__, NewApicId));
+      PluggedIdx++;
+      continue;
+    }
+
+    //
+    // Find the first empty slot in CPU_HOT_PLUG_DATA.
+    //
+    while (NewSlot < mCpuHotPlugData->ArrayLength &&
+           mCpuHotPlugData->ApicId[NewSlot] != MAX_UINT64) {
+      NewSlot++;
+    }
+    if (NewSlot == mCpuHotPlugData->ArrayLength) {
+      DEBUG ((DEBUG_ERROR, "%a: no room for APIC ID " FMT_APIC_ID "\n",
+        __FUNCTION__, NewApicId));
+      goto Fatal;
+    }
+
+    //
+    // Store the APIC ID of the new processor to the slot.
+    //
+    mCpuHotPlugData->ApicId[NewSlot] = NewApicId;
+
+    //
+    // Relocate the SMBASE of the new CPU.
+    //
+    Status = SmbaseRelocate (NewApicId, mCpuHotPlugData->SmBase[NewSlot],
+               mPostSmmPenAddress);
+    if (EFI_ERROR (Status)) {
+      goto RevokeNewSlot;
+    }
+
+    //
+    // Add the new CPU with EFI_SMM_CPU_SERVICE_PROTOCOL.
+    //
+    Status = mMmCpuService->AddProcessor (mMmCpuService, NewApicId,
+                              &NewProcessorNumberByProtocol);
+    if (EFI_ERROR (Status)) {
+      DEBUG ((DEBUG_ERROR, "%a: AddProcessor(" FMT_APIC_ID "): %r\n",
+        __FUNCTION__, NewApicId, Status));
+      goto RevokeNewSlot;
+    }
+
+    DEBUG ((DEBUG_INFO, "%a: hot-added APIC ID " FMT_APIC_ID ", SMBASE 0x%Lx, "
+      "EFI_SMM_CPU_SERVICE_PROTOCOL assigned number %Lu\n", __FUNCTION__,
+      NewApicId, (UINT64)mCpuHotPlugData->SmBase[NewSlot],
+      (UINT64)NewProcessorNumberByProtocol));
+
+    NewSlot++;
+    PluggedIdx++;
+  }
+
+  //
+  // We've handled this hotplug.
+  //
+  return EFI_SUCCESS;
+
+RevokeNewSlot:
+  mCpuHotPlugData->ApicId[NewSlot] = MAX_UINT64;
+
+Fatal:
+  return EFI_INTERRUPT_PENDING;
+}
 
 /**
   CPU Hotplug MMI handler function.
@@ -122,8 +240,6 @@ CpuHotplugMmi (
   UINT8      ApmControl;
   UINT32     PluggedCount;
   UINT32     ToUnplugCount;
-  UINT32     PluggedIdx;
-  UINT32     NewSlot;
 
   //
   // Assert that we are entering this function due to our root MMI handler
@@ -179,87 +295,12 @@ CpuHotplugMmi (
     goto Fatal;
   }
 
-  //
-  // Process hot-added CPUs.
-  //
-  // The Post-SMM Pen need not be reinstalled multiple times within a single
-  // root MMI handling. Even reinstalling once per root MMI is only prudence;
-  // in theory installing the pen in the driver's entry point function should
-  // suffice.
-  //
-  SmbaseReinstallPostSmmPen (mPostSmmPenAddress);
+  if (PluggedCount > 0) {
+    Status = PlugCpus(mPluggedApicIds, PluggedCount);
+  }
 
-  PluggedIdx = 0;
-  NewSlot = 0;
-  while (PluggedIdx < PluggedCount) {
-    APIC_ID NewApicId;
-    UINT32  CheckSlot;
-    UINTN   NewProcessorNumberByProtocol;
-
-    NewApicId = mPluggedApicIds[PluggedIdx];
-
-    //
-    // Check if the supposedly hot-added CPU is already known to us.
-    //
-    for (CheckSlot = 0;
-         CheckSlot < mCpuHotPlugData->ArrayLength;
-         CheckSlot++) {
-      if (mCpuHotPlugData->ApicId[CheckSlot] == NewApicId) {
-        break;
-      }
-    }
-    if (CheckSlot < mCpuHotPlugData->ArrayLength) {
-      DEBUG ((DEBUG_VERBOSE, "%a: APIC ID " FMT_APIC_ID " was hot-plugged "
-        "before; ignoring it\n", __FUNCTION__, NewApicId));
-      PluggedIdx++;
-      continue;
-    }
-
-    //
-    // Find the first empty slot in CPU_HOT_PLUG_DATA.
-    //
-    while (NewSlot < mCpuHotPlugData->ArrayLength &&
-           mCpuHotPlugData->ApicId[NewSlot] != MAX_UINT64) {
-      NewSlot++;
-    }
-    if (NewSlot == mCpuHotPlugData->ArrayLength) {
-      DEBUG ((DEBUG_ERROR, "%a: no room for APIC ID " FMT_APIC_ID "\n",
-        __FUNCTION__, NewApicId));
-      goto Fatal;
-    }
-
-    //
-    // Store the APIC ID of the new processor to the slot.
-    //
-    mCpuHotPlugData->ApicId[NewSlot] = NewApicId;
-
-    //
-    // Relocate the SMBASE of the new CPU.
-    //
-    Status = SmbaseRelocate (NewApicId, mCpuHotPlugData->SmBase[NewSlot],
-               mPostSmmPenAddress);
-    if (EFI_ERROR (Status)) {
-      goto RevokeNewSlot;
-    }
-
-    //
-    // Add the new CPU with EFI_SMM_CPU_SERVICE_PROTOCOL.
-    //
-    Status = mMmCpuService->AddProcessor (mMmCpuService, NewApicId,
-                              &NewProcessorNumberByProtocol);
-    if (EFI_ERROR (Status)) {
-      DEBUG ((DEBUG_ERROR, "%a: AddProcessor(" FMT_APIC_ID "): %r\n",
-        __FUNCTION__, NewApicId, Status));
-      goto RevokeNewSlot;
-    }
-
-    DEBUG ((DEBUG_INFO, "%a: hot-added APIC ID " FMT_APIC_ID ", SMBASE 0x%Lx, "
-      "EFI_SMM_CPU_SERVICE_PROTOCOL assigned number %Lu\n", __FUNCTION__,
-      NewApicId, (UINT64)mCpuHotPlugData->SmBase[NewSlot],
-      (UINT64)NewProcessorNumberByProtocol));
-
-    NewSlot++;
-    PluggedIdx++;
+  if (EFI_ERROR(Status)) {
+    goto Fatal;
   }
 
   //
@@ -267,9 +308,6 @@ CpuHotplugMmi (
   //
   return EFI_SUCCESS;
 
-RevokeNewSlot:
-  mCpuHotPlugData->ApicId[NewSlot] = MAX_UINT64;
-
 Fatal:
   ASSERT (FALSE);
   CpuDeadLoop ();
-- 
2.9.3