Parcourir la source

SbsaQemu: AcpiDxe: Read MPIDR from device tree

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>
Tested-by: Graeme Gregory <graeme@nuviainc.com>
Reviewed-by: Leif Lindholm <leif@nuviainc.com>
Tanmay Jagdale il y a 3 ans
Parent
commit
8c5c22e667
1 fichiers modifiés avec 34 ajouts et 1 suppressions
  1. 34 1
      Silicon/Qemu/SbsaQemu/Drivers/SbsaQemuAcpiDxe/SbsaQemuAcpiDxe.c

+ 34 - 1
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);
   }