[PATCH v3 edk2-platforms 5/8] SbsaQemu: AcpiDxe: Create MADT table at runtime

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

- Add support to create MADT table at runtime.
- Included a macro for GIC Redistributor structure initialisation.

Signed-off-by: Tanmay Jagdale <tanmay.jagdale@linaro.org>
---
 Silicon/Qemu/SbsaQemu/Drivers/SbsaQemuAcpiDxe/SbsaQemuAcpiDxe.inf |  20 ++-
 Silicon/Qemu/SbsaQemu/Include/IndustryStandard/SbsaQemuAcpi.h     |  15 ++
 Silicon/Qemu/SbsaQemu/Drivers/SbsaQemuAcpiDxe/SbsaQemuAcpiDxe.c   | 156 ++++++++++++++++++++
 3 files changed, 190 insertions(+), 1 deletion(-)

diff --git a/Silicon/Qemu/SbsaQemu/Drivers/SbsaQemuAcpiDxe/SbsaQemuAcpiDxe.inf b/Silicon/Qemu/SbsaQemu/Drivers/SbsaQemuAcpiDxe/SbsaQemuAcpiDxe.inf
index 3795a7e11639..8125e8ba7553 100644
--- a/Silicon/Qemu/SbsaQemu/Drivers/SbsaQemuAcpiDxe/SbsaQemuAcpiDxe.inf
+++ b/Silicon/Qemu/SbsaQemu/Drivers/SbsaQemuAcpiDxe/SbsaQemuAcpiDxe.inf
@@ -41,9 +41,27 @@ [LibraryClasses]
   UefiRuntimeServicesTableLib
 
 [Pcd]
+  gEfiMdeModulePkgTokenSpaceGuid.PcdAcpiTableStorageFile
   gArmVirtSbsaQemuPlatformTokenSpaceGuid.PcdCoreCount
   gArmVirtSbsaQemuPlatformTokenSpaceGuid.PcdClusterCount
   gArmVirtSbsaQemuPlatformTokenSpaceGuid.PcdDeviceTreeBaseAddress
 
 [Depex]
-  TRUE
+  gEfiAcpiTableProtocolGuid                       ## CONSUMES
+
+[Guids]
+  gEdkiiPlatformHasAcpiGuid
+
+[Protocols]
+  gEfiAcpiTableProtocolGuid                       ## CONSUMES
+
+[FixedPcd]
+  gEfiMdeModulePkgTokenSpaceGuid.PcdAcpiDefaultOemRevision
+  gArmTokenSpaceGuid.PcdGicDistributorBase
+  gArmTokenSpaceGuid.PcdGicRedistributorsBase
+
+  gEfiMdeModulePkgTokenSpaceGuid.PcdAcpiDefaultCreatorId
+  gEfiMdeModulePkgTokenSpaceGuid.PcdAcpiDefaultCreatorRevision
+  gEfiMdeModulePkgTokenSpaceGuid.PcdAcpiDefaultOemId
+  gEfiMdeModulePkgTokenSpaceGuid.PcdAcpiDefaultOemTableId
+  gEfiMdeModulePkgTokenSpaceGuid.PcdAcpiDefaultOemRevision
diff --git a/Silicon/Qemu/SbsaQemu/Include/IndustryStandard/SbsaQemuAcpi.h b/Silicon/Qemu/SbsaQemu/Include/IndustryStandard/SbsaQemuAcpi.h
index eac195b0585c..7a9a0061675f 100644
--- a/Silicon/Qemu/SbsaQemu/Include/IndustryStandard/SbsaQemuAcpi.h
+++ b/Silicon/Qemu/SbsaQemu/Include/IndustryStandard/SbsaQemuAcpi.h
@@ -22,6 +22,21 @@
     FixedPcdGet32 (PcdAcpiDefaultCreatorRevision)/* UINT32  CreatorRevision */ \
   }
 
+// Defines for MADT
+#define SBSAQEMU_MADT_GIC_VBASE          0x2c020000
+#define SBSAQEMU_MADT_GIC_HBASE          0x2c010000
+#define SBSAQEMU_MADT_GIC_PMU_IRQ        23
+#define SBSAQEMU_MADT_GICR_SIZE          0x4000000
+
+// Macro for MADT GIC Redistributor Structure
+#define SBSAQEMU_MADT_GICR_INIT() {                                            \
+   EFI_ACPI_6_0_GICR,                        /* Type */                        \
+   sizeof (EFI_ACPI_6_0_GICR_STRUCTURE),     /* Length */                      \
+   EFI_ACPI_RESERVED_WORD,                   /* Reserved */                    \
+   FixedPcdGet32 (PcdGicRedistributorsBase), /* DiscoveryRangeBaseAddress */   \
+   SBSAQEMU_MADT_GICR_SIZE                   /* DiscoveryRangeLength */        \
+   }
+
 #define SBSAQEMU_UART0_BASE              0x60000000
 
 #define SBSAQEMU_PCI_SEG0_CONFIG_BASE    0xf0000000
diff --git a/Silicon/Qemu/SbsaQemu/Drivers/SbsaQemuAcpiDxe/SbsaQemuAcpiDxe.c b/Silicon/Qemu/SbsaQemu/Drivers/SbsaQemuAcpiDxe/SbsaQemuAcpiDxe.c
index 75abdae3b8ce..16cb4e904e6f 100644
--- a/Silicon/Qemu/SbsaQemu/Drivers/SbsaQemuAcpiDxe/SbsaQemuAcpiDxe.c
+++ b/Silicon/Qemu/SbsaQemu/Drivers/SbsaQemuAcpiDxe/SbsaQemuAcpiDxe.c
@@ -6,11 +6,17 @@
 *  SPDX-License-Identifier: BSD-2-Clause-Patent
 *
 **/
+#include <IndustryStandard/Acpi.h>
+#include <IndustryStandard/SbsaQemuAcpi.h>
+#include <Library/AcpiLib.h>
+#include <Library/BaseMemoryLib.h>
 #include <Library/DebugLib.h>
+#include <Library/MemoryAllocationLib.h>
 #include <Library/PcdLib.h>
 #include <Library/UefiBootServicesTableLib.h>
 #include <Library/UefiDriverEntryPoint.h>
 #include <Library/UefiLib.h>
+#include <Protocol/AcpiTable.h>
 #include <Protocol/FdtClient.h>
 #include <libfdt.h>
 
@@ -61,6 +67,137 @@ CountCpusFromFdt (
   ASSERT_RETURN_ERROR (PcdStatus);
 }
 
+/*
+ * A Function to Compute the ACPI Table Checksum
+ */
+VOID
+AcpiPlatformChecksum (
+  IN UINT8      *Buffer,
+  IN UINTN      Size
+  )
+{
+  UINTN ChecksumOffset;
+
+  ChecksumOffset = OFFSET_OF (EFI_ACPI_DESCRIPTION_HEADER, Checksum);
+
+  // Set checksum field to 0 since it is used as part of the calculation
+  Buffer[ChecksumOffset] = 0;
+
+  Buffer[ChecksumOffset] = CalculateCheckSum8(Buffer, Size);
+}
+
+/*
+ * A function that add the MADT ACPI table.
+  IN EFI_ACPI_COMMON_HEADER    *CurrentTable
+ */
+EFI_STATUS
+AddMadtTable (
+  IN EFI_ACPI_TABLE_PROTOCOL   *AcpiTable
+  )
+{
+  EFI_STATUS            Status;
+  UINTN                 TableHandle;
+  UINT32                TableSize;
+  EFI_PHYSICAL_ADDRESS  PageAddress;
+  UINT8                 *New;
+  UINT32                NumCores;
+
+  // Initialize MADT ACPI Header
+  EFI_ACPI_6_0_MULTIPLE_APIC_DESCRIPTION_TABLE_HEADER Header = {
+     SBSAQEMU_ACPI_HEADER (EFI_ACPI_6_0_MULTIPLE_APIC_DESCRIPTION_TABLE_SIGNATURE,
+                           EFI_ACPI_6_0_MULTIPLE_APIC_DESCRIPTION_TABLE_HEADER,
+                           EFI_ACPI_6_0_MULTIPLE_APIC_DESCRIPTION_TABLE_REVISION),
+      0, 0 };
+
+  // Initialize GICC Structure
+  EFI_ACPI_6_0_GIC_STRUCTURE Gicc = EFI_ACPI_6_0_GICC_STRUCTURE_INIT (
+    0,                                     /* GicID */
+    0,                                     /* AcpiCpuUid */
+    0,                                     /* Mpidr */
+    EFI_ACPI_6_0_GIC_ENABLED,              /* Flags */
+    SBSAQEMU_MADT_GIC_PMU_IRQ,             /* PMU Irq */
+    FixedPcdGet32 (PcdGicDistributorBase), /* PhysicalBaseAddress */
+    SBSAQEMU_MADT_GIC_VBASE,               /* GicVBase */
+    SBSAQEMU_MADT_GIC_HBASE,               /* GicHBase */
+    25,                                    /* GsivId */
+    0,                                     /* GicRBase */
+    0                                      /* Efficiency */
+    );
+
+  // Initialize GIC Distributor Structure
+  EFI_ACPI_6_0_GIC_DISTRIBUTOR_STRUCTURE Gicd =
+    EFI_ACPI_6_0_GIC_DISTRIBUTOR_INIT (
+      0,
+      FixedPcdGet32 (PcdGicDistributorBase),
+      0,
+      3 /* GicVersion */
+    );
+
+ // Initialize GIC Redistributor Structure
+  EFI_ACPI_6_0_GICR_STRUCTURE Gicr = SBSAQEMU_MADT_GICR_INIT();
+
+  // Get CoreCount which was determined eariler after parsing device tree
+  NumCores = PcdGet32 (PcdCoreCount);
+
+  // Calculate the new table size based on the number of cores
+  TableSize = sizeof (EFI_ACPI_6_0_MULTIPLE_APIC_DESCRIPTION_TABLE_HEADER) +
+               (sizeof (EFI_ACPI_6_0_GIC_STRUCTURE) * NumCores) +
+               sizeof (EFI_ACPI_6_0_GIC_DISTRIBUTOR_STRUCTURE) +
+               sizeof (EFI_ACPI_6_0_GICR_STRUCTURE);
+
+  Status = gBS->AllocatePages (
+                  AllocateAnyPages,
+                  EfiACPIReclaimMemory,
+                  EFI_SIZE_TO_PAGES (TableSize),
+                  &PageAddress
+                  );
+  if (EFI_ERROR(Status)) {
+    DEBUG ((DEBUG_ERROR, "Failed to allocate pages for MADT table\n"));
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  New = (UINT8 *)(UINTN) PageAddress;
+  ZeroMem (New, TableSize);
+
+  // Add the  ACPI Description table header
+  CopyMem (New, &Header, sizeof (EFI_ACPI_6_0_MULTIPLE_APIC_DESCRIPTION_TABLE_HEADER));
+  ((EFI_ACPI_DESCRIPTION_HEADER*) New)->Length = TableSize;
+  New += sizeof (EFI_ACPI_6_0_MULTIPLE_APIC_DESCRIPTION_TABLE_HEADER);
+
+  // Add new GICC structures for the Cores
+  for (NumCores = 0; NumCores < PcdGet32 (PcdCoreCount); NumCores++) {
+    EFI_ACPI_6_0_GIC_STRUCTURE *GiccPtr;
+
+    CopyMem (New, &Gicc, sizeof (EFI_ACPI_6_0_GIC_STRUCTURE));
+    GiccPtr = (EFI_ACPI_6_0_GIC_STRUCTURE *) New;
+    GiccPtr->AcpiProcessorUid = NumCores;
+    GiccPtr->MPIDR = NumCores;
+    New += sizeof (EFI_ACPI_6_0_GIC_STRUCTURE);
+  }
+
+  // GIC Distributor Structure
+  CopyMem (New, &Gicd, sizeof (EFI_ACPI_6_0_GIC_DISTRIBUTOR_STRUCTURE));
+  New += sizeof (EFI_ACPI_6_0_GIC_DISTRIBUTOR_STRUCTURE);
+
+  // GIC ReDistributor Structure
+  CopyMem (New, &Gicr, sizeof (EFI_ACPI_6_0_GICR_STRUCTURE));
+  New += sizeof (EFI_ACPI_6_0_GICR_STRUCTURE);
+
+  AcpiPlatformChecksum ((UINT8*) PageAddress, TableSize);
+
+  Status = AcpiTable->InstallAcpiTable (
+                        AcpiTable,
+                        (EFI_ACPI_COMMON_HEADER *)PageAddress,
+                        TableSize,
+                        &TableHandle
+                        );
+  if (EFI_ERROR(Status)) {
+    DEBUG ((DEBUG_ERROR, "Failed to install MADT table\n"));
+  }
+
+  return Status;
+}
+
 EFI_STATUS
 EFIAPI
 InitializeSbsaQemuAcpiDxe (
@@ -68,8 +205,27 @@ InitializeSbsaQemuAcpiDxe (
   IN EFI_SYSTEM_TABLE     *SystemTable
   )
 {
+  EFI_STATUS                     Status;
+  EFI_ACPI_TABLE_PROTOCOL        *AcpiTable;
+
   // Parse the device tree and get the number of CPUs
   CountCpusFromFdt ();
 
+  // Check if ACPI Table Protocol has been installed
+  Status = gBS->LocateProtocol (
+                  &gEfiAcpiTableProtocolGuid,
+                  NULL,
+                  (VOID **)&AcpiTable
+                  );
+  if (EFI_ERROR (Status)) {
+    DEBUG ((DEBUG_ERROR, "Failed to locate ACPI Table Protocol\n"));
+    return Status;
+  }
+
+  Status = AddMadtTable (AcpiTable);
+  if (EFI_ERROR(Status)) {
+     DEBUG ((DEBUG_ERROR, "Failed to add MADT table\n"));
+  }
+
   return EFI_SUCCESS;
 }
-- 
2.28.0