[PATCH edk2-platforms 1/1] SbsaQemu: AcpiDxe: Read MPIDR from device tree

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

The Qemu device tree for Sbsa platform now contains MPIDR value
for every CPU in the form of "reg" property under every CPU's
node. Hence, add a function that provides support to read this
value from the device tree.

Signed-off-by: Tanmay Jagdale <tanmay.jagdale@linaro.org>
---
 .../Drivers/SbsaQemuAcpiDxe/SbsaQemuAcpiDxe.c | 35 ++++++++++++++++++-
 1 file changed, 34 insertions(+), 1 deletion(-)

diff --git a/Silicon/Qemu/SbsaQemu/Drivers/SbsaQemuAcpiDxe/SbsaQemuAcpiDxe.c b/Silicon/Qemu/SbsaQemu/Drivers/SbsaQemuAcpiDxe/SbsaQemuAcpiDxe.c
index 47a9bd1d423a..fb7c1835c3d7 100644
--- a/Silicon/Qemu/SbsaQemu/Drivers/SbsaQemuAcpiDxe/SbsaQemuAcpiDxe.c
+++ b/Silicon/Qemu/SbsaQemu/Drivers/SbsaQemuAcpiDxe/SbsaQemuAcpiDxe.c
@@ -22,6 +22,9 @@
 #include <Protocol/FdtClient.h>
 #include <libfdt.h>
 
+STATIC INT32 FdtFirstCpuOffset;
+STATIC INT32 FdtCpuNodeSize;
+
 /*
  * A function that walks through the Device Tree created
  * by Qemu and counts the number of CPUs present in it.
@@ -56,12 +59,14 @@ CountCpusFromFdt (
   // The count of these subnodes corresponds to the number of
   // CPUs created by Qemu.
   Prev = fdt_first_subnode (DeviceTreeBase, CpuNode);
+  FdtFirstCpuOffset = Prev;
   while (1) {
     CpuCount++;
     Node = fdt_next_subnode (DeviceTreeBase, Prev);
     if (Node < 0) {
       break;
     }
+    FdtCpuNodeSize = Node - Prev;
     Prev = Node;
   }
 
@@ -69,6 +74,34 @@ CountCpusFromFdt (
   ASSERT_RETURN_ERROR (PcdStatus);
 }
 
+/*
+ * Get MPIDR from device tree passed by Qemu
+ */
+STATIC
+UINT64
+GetMpidr (
+  IN UINTN   CpuId
+  )
+{
+  VOID           *DeviceTreeBase;
+  CONST UINT64   *RegVal;
+  INT32          Len;
+
+  DeviceTreeBase = (VOID *)(UINTN)PcdGet64 (PcdDeviceTreeBaseAddress);
+  ASSERT (DeviceTreeBase != NULL);
+
+  RegVal = fdt_getprop (DeviceTreeBase,
+             FdtFirstCpuOffset + (CpuId * FdtCpuNodeSize),
+             "reg",
+             &Len);
+  if (!RegVal) {
+    DEBUG ((DEBUG_ERROR, "Couldn't find reg property for CPU:%d\n", CpuId));
+    return 0;
+  }
+
+  return (fdt64_to_cpu (ReadUnaligned64 (RegVal)));
+}
+
 /*
  * A Function to Compute the ACPI Table Checksum
  */
@@ -173,7 +206,7 @@ AddMadtTable (
     CopyMem (New, &Gicc, sizeof (EFI_ACPI_6_0_GIC_STRUCTURE));
     GiccPtr = (EFI_ACPI_6_0_GIC_STRUCTURE *) New;
     GiccPtr->AcpiProcessorUid = NumCores;
-    GiccPtr->MPIDR = NumCores;
+    GiccPtr->MPIDR = GetMpidr (NumCores);
     New += sizeof (EFI_ACPI_6_0_GIC_STRUCTURE);
   }
 
-- 
2.28.0