[edk2-platforms][PATCH v2 10/14] RISC-V/PlatformPkg: Fixup FDT from HOB and install into config table

["Daniel Schaefer" <daniel.schaefer@hpe.com>]

The Linux EFISTUB reads the FDT from the EFI system configuration
table. Before installing the FDT needs to be patched with the booting
hartid, because the kernel in S-Mode cannot determine it.

Cc: Daniel Schaefer <daniel.schaefer@hpe.com>
Cc: Abner Chang <abner.chang@hpe.com>
Cc: Sunil V L <sunilvl@ventanamicro.com>

Signed-off-by: Daniel Schaefer <daniel.schaefer@hpe.com>
---
 Platform/SiFive/U5SeriesPkg/FreedomU540HiFiveUnleashedBoard/U540.dsc |   2 +
 Platform/SiFive/U5SeriesPkg/FreedomU540HiFiveUnleashedBoard/U540.fdf |   1 +
 Silicon/RISC-V/ProcessorPkg/RiscVProcessorPkg.dsc                    |   3 +-
 Silicon/RISC-V/ProcessorPkg/Universal/FdtDxe/FdtDxe.c                | 116 ++++++++++++++++++++
 Silicon/RISC-V/ProcessorPkg/Universal/FdtDxe/FdtDxe.inf              |  53 +++++++++
 5 files changed, 174 insertions(+), 1 deletion(-)

diff --git a/Platform/SiFive/U5SeriesPkg/FreedomU540HiFiveUnleashedBoard/U540.dsc b/Platform/SiFive/U5SeriesPkg/FreedomU540HiFiveUnleashedBoard/U540.dsc
index 4fab3c4bdc..cc62ad0521 100644
--- a/Platform/SiFive/U5SeriesPkg/FreedomU540HiFiveUnleashedBoard/U540.dsc
+++ b/Platform/SiFive/U5SeriesPkg/FreedomU540HiFiveUnleashedBoard/U540.dsc
@@ -516,6 +516,8 @@
   MdeModulePkg/Bus/Usb/UsbKbDxe/UsbKbDxe.inf
   MdeModulePkg/Bus/Usb/UsbMassStorageDxe/UsbMassStorageDxe.inf
 
+  Silicon/RISC-V/ProcessorPkg/Universal/FdtDxe/FdtDxe.inf
+
   #
   # FAT filesystem + GPT/MBR partitioning + UDF filesystem
   #
diff --git a/Platform/SiFive/U5SeriesPkg/FreedomU540HiFiveUnleashedBoard/U540.fdf b/Platform/SiFive/U5SeriesPkg/FreedomU540HiFiveUnleashedBoard/U540.fdf
index f0bd65fad6..c58fa63574 100644
--- a/Platform/SiFive/U5SeriesPkg/FreedomU540HiFiveUnleashedBoard/U540.fdf
+++ b/Platform/SiFive/U5SeriesPkg/FreedomU540HiFiveUnleashedBoard/U540.fdf
@@ -182,6 +182,7 @@ INF  MdeModulePkg/Universal/DisplayEngineDxe/DisplayEngineDxe.inf
 INF  MdeModulePkg/Universal/MemoryTest/NullMemoryTestDxe/NullMemoryTestDxe.inf
 INF  FatPkg/EnhancedFatDxe/Fat.inf
 INF  MdeModulePkg/Universal/Disk/UdfDxe/UdfDxe.inf
+INF  Silicon/RISC-V/ProcessorPkg/Universal/FdtDxe/FdtDxe.inf
 
 !ifndef $(SOURCE_DEBUG_ENABLE)
 INF MdeModulePkg/Universal/SerialDxe/SerialDxe.inf
diff --git a/Silicon/RISC-V/ProcessorPkg/RiscVProcessorPkg.dsc b/Silicon/RISC-V/ProcessorPkg/RiscVProcessorPkg.dsc
index 1292ba1bea..531319322c 100644
--- a/Silicon/RISC-V/ProcessorPkg/RiscVProcessorPkg.dsc
+++ b/Silicon/RISC-V/ProcessorPkg/RiscVProcessorPkg.dsc
@@ -1,7 +1,7 @@
 #/** @file
 # RISC-V processor package.
 #
-# Copyright (c) 2020, Hewlett Packard Enterprise Development LP. All rights reserved.<BR>
+# Copyright (c) 2021, Hewlett Packard Enterprise Development LP. All rights reserved.<BR>
 #
 #  SPDX-License-Identifier: BSD-2-Clause-Patent
 #
@@ -102,3 +102,4 @@
 
   Silicon/RISC-V/ProcessorPkg/Universal/CpuDxe/CpuDxe.inf
   Silicon/RISC-V/ProcessorPkg/Universal/SmbiosDxe/RiscVSmbiosDxe.inf
+  Silicon/RISC-V/ProcessorPkg/Universal/FdtDxe/FdtDxe.inf
diff --git a/Silicon/RISC-V/ProcessorPkg/Universal/FdtDxe/FdtDxe.c b/Silicon/RISC-V/ProcessorPkg/Universal/FdtDxe/FdtDxe.c
new file mode 100644
index 0000000000..22b12027d3
--- /dev/null
+++ b/Silicon/RISC-V/ProcessorPkg/Universal/FdtDxe/FdtDxe.c
@@ -0,0 +1,116 @@
+/** @file
+  RISC-V Flattened Device Tree DXE module
+
+  The Linux booting protocol on RISC-V requires the id of the booting hart to
+  be passed as a0. Therefore the EFISTUB needs to get this information. Because
+  it runs in S-Mode, it cannot get this information from mhartid. Instead we
+  insert the id into the device tree, that the EFIFSTUB can read from the config table.
+
+  Copyright (c) 2021, Hewlett Packard Enterprise Development LP. All rights reserved.<BR>
+
+  SPDX-License-Identifier: BSD-2-Clause-Patent
+**/
+
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/DebugLib.h>
+#include <Library/HobLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/PcdLib.h>
+#include <libfdt.h>
+
+/**
+  Fix up the device tree with booting hartid for the kernel
+
+  @param DtbBlob The device tree. Is extended to fit the hart id.
+
+  @retval EFI_SUCCESS           The device tree was success fixed up with the hart id.
+  @retval EFI_OUT_OF_RESOURCES  There is not enough memory available to complete the operation.
+**/
+EFI_STATUS
+EFIAPI
+FixDtb (
+  IN OUT VOID  *DtbBlob,
+  IN     UINTN  BootingHartId
+  )
+{
+  fdt32_t Size;
+  UINT32 ChosenOffset, Err;
+
+  DEBUG ((DEBUG_INFO, "Fixing up device tree with boot hart id: %d\n",
+    BootingHartId));
+
+  Size = fdt_totalsize(DtbBlob);
+  Err  = fdt_open_into(DtbBlob, DtbBlob, Size + 32);
+  if (Err < 0) {
+    DEBUG ((DEBUG_ERROR,
+      "Device Tree can't be expanded to accommodate new node\n", __FUNCTION__));
+    return EFI_OUT_OF_RESOURCES;
+  }
+  ChosenOffset = fdt_path_offset(DtbBlob, "/chosen");
+  fdt_setprop_u32(DtbBlob, ChosenOffset, "boot-hartid", BootingHartId);
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Install the FDT passed in HOB into EFI system configuration table.
+
+  @retval EFI_SUCCESS          Successfully installed fixed up FDT in config table.
+  @retval EFI_NOT_FOUND        Did not find FDT HOB.
+  @retval EFI_OUT_OF_RESOURCES There is not enough memory available to complete the operation.
+**/
+EFI_STATUS
+EFIAPI
+InstallFdtFromHob (VOID)
+{
+  EFI_STATUS         Status;
+  EFI_HOB_GUID_TYPE *GuidHob;
+  VOID              *DataInHob;
+  UINTN              DataSize;
+
+  GuidHob = GetFirstGuidHob (&gFdtHobGuid);
+  if (GuidHob == NULL) {
+    DEBUG ((DEBUG_ERROR, "Failed to find RISC-V DTB Hob\n",
+      __FUNCTION__));
+    return EFI_NOT_FOUND;
+  }
+  DataInHob = (VOID *) *((UINTN *) GET_GUID_HOB_DATA (GuidHob));
+  DataSize  = GET_GUID_HOB_DATA_SIZE (GuidHob);
+
+  Status = FixDtb (DataInHob, PcdGet32(PcdBootHartId));
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  Status = gBS->InstallConfigurationTable (&gFdtTableGuid, DataInHob);
+  if (EFI_ERROR (Status)) {
+    DEBUG ((DEBUG_ERROR, "%a: failed to install FDT configuration table\n",
+      __FUNCTION__));
+  }
+  return Status;
+}
+
+/**
+  Install the FDT from the HOB into the EFI system configuration table.
+
+  @param ImageHandle     Image handle of this driver.
+  @param SystemTable     Pointer to the System Table.
+
+  @retval EFI_SUCCESS    FDT successfully installed into config table.
+  @retval EFI_NOT_FOUND  Did not find FDT HOB.
+  @retval EFI_OUT_OF_RESOURCES There is not enough memory available to complete the operation.
+
+**/
+EFI_STATUS
+EFIAPI
+InstallFdt (
+  IN EFI_HANDLE                            ImageHandle,
+  IN EFI_SYSTEM_TABLE                      *SystemTable
+  )
+{
+  EFI_STATUS  Status;
+
+  Status = InstallFdtFromHob ();
+
+  return Status;
+}
diff --git a/Silicon/RISC-V/ProcessorPkg/Universal/FdtDxe/FdtDxe.inf b/Silicon/RISC-V/ProcessorPkg/Universal/FdtDxe/FdtDxe.inf
new file mode 100644
index 0000000000..ae6468f9f5
--- /dev/null
+++ b/Silicon/RISC-V/ProcessorPkg/Universal/FdtDxe/FdtDxe.inf
@@ -0,0 +1,53 @@
+## @file
+#  RISC-V Flattened Device Tree DXE module.
+#
+#  Copyright (c) 2021, Hewlett Packard Enterprise Development LP. All rights reserved.<BR>
+#  SPDX-License-Identifier: BSD-2-Clause-Patent
+#
+##
+
+[Defines]
+  INF_VERSION               = 0x0001001b
+  BASE_NAME                 = FdtDxe
+  FILE_GUID                 = a7d8f3f7-d8a7-47df-b3ec-9E5A693C380C
+  MODULE_TYPE               = DXE_DRIVER
+  VERSION_STRING            = 1.0
+  ENTRY_POINT               = InstallFdt
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES      = RISCV64
+#
+
+
+[Packages]
+  EmbeddedPkg/EmbeddedPkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+  MdePkg/MdePkg.dec
+  Platform/RISC-V/PlatformPkg/RiscVPlatformPkg.dec
+  EmbeddedPkg/EmbeddedPkg.dec
+
+[LibraryClasses]
+  BaseLib
+  BaseMemoryLib
+  DebugLib
+  FdtLib
+  HobLib
+  MemoryAllocationLib
+  RiscVCpuLib
+  UefiBootServicesTableLib
+  UefiDriverEntryPoint
+
+[Sources]
+  FdtDxe.c
+
+[Guids]
+  gFdtHobGuid
+  gFdtTableGuid
+
+[Pcd]
+  gUefiRiscVPlatformPkgTokenSpaceGuid.PcdBootHartId  ## CONSUMES
+
+[Depex]
+  TRUE
-- 
2.33.0