[PATCH v3 edk2-platforms 4/8] SbsaQemu: Add new ACPI driver and FDT parser to count CPUs

["Tanmay Jagdale" <tanmay.jagdale@linaro.org>]

- Add a new ACPI driver for the SbsaQemu platform which would
  handle any modifications needed for the ACPI tables.

- Add a parser function in this driver which parses the FDT created
  by Qemu to determine the number of CPUs and hence update the
  PcdCoreCount variable.

Signed-off-by: Tanmay Jagdale <tanmay.jagdale@linaro.org>
---
 Silicon/Qemu/SbsaQemu/Acpi.dsc.inc                                |  1 +
 Platform/Qemu/SbsaQemu/SbsaQemu.fdf                               |  1 +
 Silicon/Qemu/SbsaQemu/Drivers/SbsaQemuAcpiDxe/SbsaQemuAcpiDxe.inf | 49 +++++++++++++
 Silicon/Qemu/SbsaQemu/Drivers/SbsaQemuAcpiDxe/SbsaQemuAcpiDxe.c   | 75 ++++++++++++++++++++
 4 files changed, 126 insertions(+)

diff --git a/Silicon/Qemu/SbsaQemu/Acpi.dsc.inc b/Silicon/Qemu/SbsaQemu/Acpi.dsc.inc
index c4a8d7a27b78..593670383750 100644
--- a/Silicon/Qemu/SbsaQemu/Acpi.dsc.inc
+++ b/Silicon/Qemu/SbsaQemu/Acpi.dsc.inc
@@ -33,3 +33,4 @@ [Components.common]
   MdeModulePkg/Universal/Acpi/AcpiTableDxe/AcpiTableDxe.inf
   MdeModulePkg/Universal/Acpi/AcpiPlatformDxe/AcpiPlatformDxe.inf
   Silicon/Qemu/SbsaQemu/AcpiTables/AcpiTables.inf
+  Silicon/Qemu/SbsaQemu/Drivers/SbsaQemuAcpiDxe/SbsaQemuAcpiDxe.inf
diff --git a/Platform/Qemu/SbsaQemu/SbsaQemu.fdf b/Platform/Qemu/SbsaQemu/SbsaQemu.fdf
index 4526eaaa02c5..3bcf0bf0040a 100644
--- a/Platform/Qemu/SbsaQemu/SbsaQemu.fdf
+++ b/Platform/Qemu/SbsaQemu/SbsaQemu.fdf
@@ -232,6 +232,7 @@ [FV.FvMain]
   #
   INF MdeModulePkg/Universal/Acpi/AcpiTableDxe/AcpiTableDxe.inf
   INF MdeModulePkg/Universal/Acpi/AcpiPlatformDxe/AcpiPlatformDxe.inf
+  INF Silicon/Qemu/SbsaQemu/Drivers/SbsaQemuAcpiDxe/SbsaQemuAcpiDxe.inf
   INF RuleOverride = ACPITABLE Silicon/Qemu/SbsaQemu/AcpiTables/AcpiTables.inf
   INF MdeModulePkg/Universal/Acpi/BootGraphicsResourceTableDxe/BootGraphicsResourceTableDxe.inf
 
diff --git a/Silicon/Qemu/SbsaQemu/Drivers/SbsaQemuAcpiDxe/SbsaQemuAcpiDxe.inf b/Silicon/Qemu/SbsaQemu/Drivers/SbsaQemuAcpiDxe/SbsaQemuAcpiDxe.inf
new file mode 100644
index 000000000000..3795a7e11639
--- /dev/null
+++ b/Silicon/Qemu/SbsaQemu/Drivers/SbsaQemuAcpiDxe/SbsaQemuAcpiDxe.inf
@@ -0,0 +1,49 @@
+## @file
+#  This driver modifies ACPI tables for the Qemu SBSA platform
+#
+#  Copyright (c) 2020, Linaro Ltd. All rights reserved.
+#
+#  SPDX-License-Identifier: BSD-2-Clause-Patent
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x0001001d
+  BASE_NAME                      = SbsaQemuAcpiDxe
+  FILE_GUID                      = 6c592dc9-76c8-474f-93b2-bf1e8f15ae35
+  MODULE_TYPE                    = DXE_DRIVER
+  VERSION_STRING                 = 1.0
+
+  ENTRY_POINT                    = InitializeSbsaQemuAcpiDxe
+
+[Sources]
+  SbsaQemuAcpiDxe.c
+
+[Packages]
+  ArmPkg/ArmPkg.dec
+  ArmPlatformPkg/ArmPlatformPkg.dec
+  ArmVirtPkg/ArmVirtPkg.dec
+  EmbeddedPkg/EmbeddedPkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+  MdePkg/MdePkg.dec
+  Silicon/Qemu/SbsaQemu/SbsaQemu.dec
+
+[LibraryClasses]
+  ArmLib
+  BaseMemoryLib
+  BaseLib
+  DebugLib
+  DxeServicesLib
+  FdtLib
+  PcdLib
+  UefiDriverEntryPoint
+  UefiLib
+  UefiRuntimeServicesTableLib
+
+[Pcd]
+  gArmVirtSbsaQemuPlatformTokenSpaceGuid.PcdCoreCount
+  gArmVirtSbsaQemuPlatformTokenSpaceGuid.PcdClusterCount
+  gArmVirtSbsaQemuPlatformTokenSpaceGuid.PcdDeviceTreeBaseAddress
+
+[Depex]
+  TRUE
diff --git a/Silicon/Qemu/SbsaQemu/Drivers/SbsaQemuAcpiDxe/SbsaQemuAcpiDxe.c b/Silicon/Qemu/SbsaQemu/Drivers/SbsaQemuAcpiDxe/SbsaQemuAcpiDxe.c
new file mode 100644
index 000000000000..75abdae3b8ce
--- /dev/null
+++ b/Silicon/Qemu/SbsaQemu/Drivers/SbsaQemuAcpiDxe/SbsaQemuAcpiDxe.c
@@ -0,0 +1,75 @@
+/** @file
+*  This file is an ACPI driver for the Qemu SBSA platform.
+*
+*  Copyright (c) 2020, Linaro Ltd. All rights reserved.
+*
+*  SPDX-License-Identifier: BSD-2-Clause-Patent
+*
+**/
+#include <Library/DebugLib.h>
+#include <Library/PcdLib.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/UefiDriverEntryPoint.h>
+#include <Library/UefiLib.h>
+#include <Protocol/FdtClient.h>
+#include <libfdt.h>
+
+/*
+ * A function that walks through the Device Tree created
+ * by Qemu and counts the number of CPUs present in it.
+ */
+STATIC
+VOID
+CountCpusFromFdt (
+  VOID
+)
+{
+  VOID           *DeviceTreeBase;
+  INT32          Node, Prev;
+  RETURN_STATUS  PcdStatus;
+  INT32          CpuNode;
+  INT32          CpuCount;
+
+  DeviceTreeBase = (VOID *)(UINTN)PcdGet64 (PcdDeviceTreeBaseAddress);
+  ASSERT (DeviceTreeBase != NULL);
+
+  // Make sure we have a valid device tree blob
+  ASSERT (fdt_check_header (DeviceTreeBase) == 0);
+
+  CpuNode = fdt_path_offset (DeviceTreeBase, "/cpus");
+  if (CpuNode <= 0) {
+    DEBUG ((DEBUG_ERROR, "Unable to locate /cpus in device tree\n"));
+    return;
+  }
+
+  CpuCount = 0;
+
+  // Walk through /cpus node and count the number of subnodes.
+  // The count of these subnodes corresponds to the number of
+  // CPUs created by Qemu.
+  Prev = fdt_first_subnode (DeviceTreeBase, CpuNode);
+  while (1) {
+    CpuCount++;
+    Node = fdt_next_subnode (DeviceTreeBase, Prev);
+    if (Node < 0) {
+      break;
+    }
+    Prev = Node;
+  }
+
+  PcdStatus = PcdSet32S (PcdCoreCount, CpuCount);
+  ASSERT_RETURN_ERROR (PcdStatus);
+}
+
+EFI_STATUS
+EFIAPI
+InitializeSbsaQemuAcpiDxe (
+  IN EFI_HANDLE           ImageHandle,
+  IN EFI_SYSTEM_TABLE     *SystemTable
+  )
+{
+  // Parse the device tree and get the number of CPUs
+  CountCpusFromFdt ();
+
+  return EFI_SUCCESS;
+}
-- 
2.28.0