[edk2-platforms PATCH v3 1/3] 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>
---
 Platform/Qemu/SbsaQemu/SbsaQemu.dsc                         |  2 +
 Silicon/Qemu/SbsaQemu/Include/Library/FdtHelperLib.h        | 24 +++++++
 Silicon/Qemu/SbsaQemu/Library/FdtHelperLib/FdtHelperLib.c   | 69 ++++++++++++++++++++
 Silicon/Qemu/SbsaQemu/Library/FdtHelperLib/FdtHelperLib.inf | 28 ++++++++
 4 files changed, 123 insertions(+)

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/Include/Library/FdtHelperLib.h b/Silicon/Qemu/SbsaQemu/Include/Library/FdtHelperLib.h
new file mode 100644
index 000000000000..eac47349a3d7
--- /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
+UINT16
+CountCpusFromFdt (
+  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..c399fec5f9c7
--- /dev/null
+++ b/Silicon/Qemu/SbsaQemu/Library/FdtHelperLib/FdtHelperLib.c
@@ -0,0 +1,69 @@
+/** @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
+*
+**/
+
+
+/** Walks through the Device Tree created by Qemu and counts the number
+    of CPUs present in it.
+
+    @return The number of CPUs present.
+**/
+
+#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
+UINT16
+CountCpusFromFdt (
+  VOID
+  )
+{
+  VOID   *DeviceTreeBase;
+  INT32  Node;
+  INT32  Prev;
+  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 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