[edk2-platforms PATCH v4 1/4] SbsaQemu: Add FdtHelperLib

["Rebecca Cran" <rebecca@nuviainc.com>]

The CountCpusFromFdt function is now used in two places. Create
FdtHelperLib for this and similar functions.

Signed-off-by: Rebecca Cran <rebecca@nuviainc.com>
Reviewed-by: Leif Lindholm <leif@nuviainc.com>
---
 Platform/Qemu/SbsaQemu/SbsaQemu.dsc                             |  2 +
 Silicon/Qemu/SbsaQemu/Drivers/SbsaQemuAcpiDxe/SbsaQemuAcpiDxe.c |  5 +-
 Silicon/Qemu/SbsaQemu/Include/Library/FdtHelperLib.h            | 24 ++++++++
 Silicon/Qemu/SbsaQemu/Library/FdtHelperLib/FdtHelperLib.c       | 62 ++++++++++++++++++++
 Silicon/Qemu/SbsaQemu/Library/FdtHelperLib/FdtHelperLib.inf     | 28 +++++++++
 5 files changed, 120 insertions(+), 1 deletion(-)

diff --git a/Platform/Qemu/SbsaQemu/SbsaQemu.dsc b/Platform/Qemu/SbsaQemu/SbsaQemu.dsc
index f6af3f9111ee..8faad3eda217 100644
--- a/Platform/Qemu/SbsaQemu/SbsaQemu.dsc
+++ b/Platform/Qemu/SbsaQemu/SbsaQemu.dsc
@@ -121,6 +121,8 @@ DEFINE NETWORK_HTTP_BOOT_ENABLE       = FALSE
   # ARM PL011 UART Driver
   PL011UartLib|ArmPlatformPkg/Library/PL011UartLib/PL011UartLib.inf
 
+  FdtHelperLib|Silicon/Qemu/SbsaQemu/Library/FdtHelperLib/FdtHelperLib.inf
+
   # Debug Support
   PeCoffExtraActionLib|ArmPkg/Library/DebugPeCoffExtraActionLib/DebugPeCoffExtraActionLib.inf
   DebugAgentLib|MdeModulePkg/Library/DebugAgentLibNull/DebugAgentLibNull.inf
diff --git a/Silicon/Qemu/SbsaQemu/Drivers/SbsaQemuAcpiDxe/SbsaQemuAcpiDxe.c b/Silicon/Qemu/SbsaQemu/Drivers/SbsaQemuAcpiDxe/SbsaQemuAcpiDxe.c
index fb7c1835c3d7..7bf60cd2ded1 100644
--- a/Silicon/Qemu/SbsaQemu/Drivers/SbsaQemuAcpiDxe/SbsaQemuAcpiDxe.c
+++ b/Silicon/Qemu/SbsaQemu/Drivers/SbsaQemuAcpiDxe/SbsaQemuAcpiDxe.c
@@ -487,9 +487,12 @@ InitializeSbsaQemuAcpiDxe (
 {
   EFI_STATUS                     Status;
   EFI_ACPI_TABLE_PROTOCOL        *AcpiTable;
+  UINT32                         NumCores;
 
   // Parse the device tree and get the number of CPUs
-  CountCpusFromFdt ();
+  NumCores = FdtHelperCountCpus ();
+  Status = PcdSet32S (PcdCoreCount, NumCores);
+  ASSERT_RETURN_ERROR (Status);
 
   // Check if ACPI Table Protocol has been installed
   Status = gBS->LocateProtocol (
diff --git a/Silicon/Qemu/SbsaQemu/Include/Library/FdtHelperLib.h b/Silicon/Qemu/SbsaQemu/Include/Library/FdtHelperLib.h
new file mode 100644
index 000000000000..e9e7281c1342
--- /dev/null
+++ b/Silicon/Qemu/SbsaQemu/Include/Library/FdtHelperLib.h
@@ -0,0 +1,24 @@
+/** @file
+*  FdtHelperLib.h
+*
+*  Copyright (c) 2021, NUVIA Inc. All rights reserved.
+*
+*  SPDX-License-Identifier: BSD-2-Clause-Patent
+*
+**/
+
+#ifndef FDT_HELPER_LIB_
+#define FDT_HELPER_LIB_
+
+/** Walks through the Device Tree created by Qemu and counts the number
+    of CPUs present in it.
+
+    @return The number of CPUs present.
+**/
+EFIAPI
+UINT32
+FdtHelperCountCpus (
+  VOID
+  );
+
+#endif /* FDT_HELPER_LIB_ */
diff --git a/Silicon/Qemu/SbsaQemu/Library/FdtHelperLib/FdtHelperLib.c b/Silicon/Qemu/SbsaQemu/Library/FdtHelperLib/FdtHelperLib.c
new file mode 100644
index 000000000000..411f035ee7d8
--- /dev/null
+++ b/Silicon/Qemu/SbsaQemu/Library/FdtHelperLib/FdtHelperLib.c
@@ -0,0 +1,62 @@
+/** @file
+*  FdtHelperLib.c
+*
+*  Copyright (c) 2021, NUVIA Inc. All rights reserved.
+*  Copyright (c) 2020, Linaro Ltd. All rights reserved.
+*
+*  SPDX-License-Identifier: BSD-2-Clause-Patent
+*
+**/
+
+#include <Uefi.h>
+#include <Library/DebugLib.h>
+#include <Library/FdtHelperLib.h>
+#include <Library/PcdLib.h>
+#include <libfdt.h>
+
+/** Walks through the Device Tree created by Qemu and counts the number
+    of CPUs present in it.
+
+    @return The number of CPUs present.
+**/
+EFIAPI
+UINT32
+FdtHelperCountCpus (
+  VOID
+  )
+{
+  VOID   *DeviceTreeBase;
+  INT32  Node;
+  INT32  Prev;
+  INT32  CpuNode;
+  UINT32 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 0;
+  }
+
+  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;
+  }
+
+  return CpuCount;
+}
diff --git a/Silicon/Qemu/SbsaQemu/Library/FdtHelperLib/FdtHelperLib.inf b/Silicon/Qemu/SbsaQemu/Library/FdtHelperLib/FdtHelperLib.inf
new file mode 100644
index 000000000000..d84c16f888d1
--- /dev/null
+++ b/Silicon/Qemu/SbsaQemu/Library/FdtHelperLib/FdtHelperLib.inf
@@ -0,0 +1,28 @@
+#/** @file
+#
+#  Component description file for FdtHelperLib module
+#
+#  Copyright (c) 2021, NUVIA Inc. All rights reserved.
+#
+#  SPDX-License-Identifier: BSD-2-Clause-Patent
+#
+#**/
+
+[Defines]
+  INF_VERSION                    = 1.29
+  BASE_NAME                      = FdtHelperLib
+  FILE_GUID                      = 34e4396f-c2fc-4f9e-ad58-0f98e99e3875
+  MODULE_TYPE                    = BASE
+  VERSION_STRING                 = 1.0
+  LIBRARY_CLASS                  = FdtHelperLib
+
+[Sources.common]
+  FdtHelperLib.c
+
+[Packages]
+  EmbeddedPkg/EmbeddedPkg.dec
+  MdePkg/MdePkg.dec
+  Silicon/Qemu/SbsaQemu/SbsaQemu.dec
+
+[FixedPcd]
+  gArmVirtSbsaQemuPlatformTokenSpaceGuid.PcdDeviceTreeBaseAddress
-- 
2.26.2