[edk2-platforms][PATCH 04/30] RISC-V: Use RISC-V PeiCoreEntryPoint library

["Abner Chang" <abner.chang@hpe.com>]

1. Use RISC-V PeiCoreEntryPoint library instance for opensbi
   to switch to the next phase with arg0 as HART Id and arg1
   as the SEC to PEI handoff data.
2. Introduce EDK2 opensbi platform operation functions.
   With this, OEM can has its won platform initialization code
   before and/or after opensbi vendor platform functions.

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

Signed-off-by: Abner Chang <abner.chang@hpe.com>
---
 .../FreedomU540HiFiveUnleashedBoard/U540.dsc  |   3 +-
 .../PlatformPkg/Universal/Sec/SecMain.inf     |   2 +
 .../PlatformPkg/Universal/Sec/SecMain.h       |  10 +
 .../Include/IndustryStandard/RiscVOpensbi.h   |   8 +-
 .../Universal/Sec/Edk2OpenSbiPlatform.c       | 426 ++++++++++++++++++
 .../PlatformPkg/Universal/Sec/SecMain.c       | 152 +++++--
 6 files changed, 547 insertions(+), 54 deletions(-)
 create mode 100644 Platform/RISC-V/PlatformPkg/Universal/Sec/Edk2OpenSbiPlatform.c

diff --git a/Platform/SiFive/U5SeriesPkg/FreedomU540HiFiveUnleashedBoard/U540.dsc b/Platform/SiFive/U5SeriesPkg/FreedomU540HiFiveUnleashedBoard/U540.dsc
index 5d2ccafaca..be23fc39fd 100644
--- a/Platform/SiFive/U5SeriesPkg/FreedomU540HiFiveUnleashedBoard/U540.dsc
+++ b/Platform/SiFive/U5SeriesPkg/FreedomU540HiFiveUnleashedBoard/U540.dsc
@@ -181,7 +181,6 @@
   RiscVFirmwareContextLib|Silicon/RISC-V/ProcessorPkg/Library/RiscVFirmwareContextSscratchLib/RiscVFirmwareContextSscratchLib.inf
   PeiServicesLib|MdePkg/Library/PeiServicesLib/PeiServicesLib.inf
   MemoryAllocationLib|MdePkg/Library/PeiMemoryAllocationLib/PeiMemoryAllocationLib.inf
-  PeiCoreEntryPoint|MdePkg/Library/PeiCoreEntryPoint/PeiCoreEntryPoint.inf
   ReportStatusCodeLib|MdeModulePkg/Library/PeiReportStatusCodeLib/PeiReportStatusCodeLib.inf
   OemHookStatusCodeLib|MdeModulePkg/Library/OemHookStatusCodeLibNull/OemHookStatusCodeLibNull.inf
   PeCoffGetEntryPointLib|MdePkg/Library/BasePeCoffGetEntryPointLib/BasePeCoffGetEntryPointLib.inf
@@ -191,6 +190,8 @@
   DebugLib|MdePkg/Library/BaseDebugLibNull/BaseDebugLibNull.inf
 !endif
   PeCoffLib|MdePkg/Library/BasePeCoffLib/BasePeCoffLib.inf
+  # RISC-V platform PEI core entry point.
+  PeiCoreEntryPoint|Platform/RISC-V/PlatformPkg/Library/PeiCoreEntryPoint/PeiCoreEntryPoint.inf
 
 [LibraryClasses.common.PEIM]
   HobLib|MdePkg/Library/PeiHobLib/PeiHobLib.inf
diff --git a/Platform/RISC-V/PlatformPkg/Universal/Sec/SecMain.inf b/Platform/RISC-V/PlatformPkg/Universal/Sec/SecMain.inf
index bcb8b9f908..4207c83413 100644
--- a/Platform/RISC-V/PlatformPkg/Universal/Sec/SecMain.inf
+++ b/Platform/RISC-V/PlatformPkg/Universal/Sec/SecMain.inf
@@ -23,11 +23,13 @@
 
 [Sources]
   SecMain.c
+  Edk2OpenSbiPlatform.c
 
 [Sources.RISCV64]
   Riscv64/SecEntry.S
 
 [Packages]
+  EmbeddedPkg/EmbeddedPkg.dec
   MdeModulePkg/MdeModulePkg.dec
   MdePkg/MdePkg.dec
   Silicon/RISC-V/ProcessorPkg/RiscVProcessorPkg.dec
diff --git a/Platform/RISC-V/PlatformPkg/Universal/Sec/SecMain.h b/Platform/RISC-V/PlatformPkg/Universal/Sec/SecMain.h
index 94ea46263c..c04ddbad7f 100644
--- a/Platform/RISC-V/PlatformPkg/Universal/Sec/SecMain.h
+++ b/Platform/RISC-V/PlatformPkg/Universal/Sec/SecMain.h
@@ -28,6 +28,16 @@
 #include <Ppi/TemporaryRamDone.h>
 #include <Ppi/TemporaryRamSupport.h>
 
+int
+SecPostOpenSbiPlatformEarlylInit(
+  IN BOOLEAN ColdBoot
+  );
+
+int
+SecPostOpenSbiPlatformFinalInit (
+  IN BOOLEAN ColdBoot
+  );
+
 VOID
 SecMachineModeTrapHandler (
   IN VOID
diff --git a/Silicon/RISC-V/ProcessorPkg/Include/IndustryStandard/RiscVOpensbi.h b/Silicon/RISC-V/ProcessorPkg/Include/IndustryStandard/RiscVOpensbi.h
index e7ac6d26ee..d639429306 100644
--- a/Silicon/RISC-V/ProcessorPkg/Include/IndustryStandard/RiscVOpensbi.h
+++ b/Silicon/RISC-V/ProcessorPkg/Include/IndustryStandard/RiscVOpensbi.h
@@ -1,7 +1,7 @@
 /** @file
   SBI inline function calls.
 
-  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
 
@@ -46,8 +46,10 @@ typedef struct {
                                                       // structure. Referred by both C code and assembly code.
 
 typedef struct {
-  VOID            *PeiServiceTable;       // PEI Service table
-  UINT64           FlattenedDeviceTree;   // Pointer to Flattened Device tree
+  UINT64              BootHartId;
+  VOID                *PeiServiceTable;      // PEI Service table
+  UINT64              FlattenedDeviceTree;   // Pointer to Flattened Device tree
+  UINT64              SecPeiHandOffData;     // This is EFI_SEC_PEI_HAND_OFF passed to PEI Core.
   EFI_RISCV_FIRMWARE_CONTEXT_HART_SPECIFIC  *HartSpecific[RISC_V_MAX_HART_SUPPORTED];
 } EFI_RISCV_OPENSBI_FIRMWARE_CONTEXT;
 
diff --git a/Platform/RISC-V/PlatformPkg/Universal/Sec/Edk2OpenSbiPlatform.c b/Platform/RISC-V/PlatformPkg/Universal/Sec/Edk2OpenSbiPlatform.c
new file mode 100644
index 0000000000..79b2f33675
--- /dev/null
+++ b/Platform/RISC-V/PlatformPkg/Universal/Sec/Edk2OpenSbiPlatform.c
@@ -0,0 +1,426 @@
+/*
+  Copyright (c) 2021, Hewlett Packard Enterprise Development LP. All rights reserved.<BR>
+
+  SPDX-License-Identifier: BSD-2-Clause-Patent
+
+ */
+
+#include <Library/DebugAgentLib.h>
+#include <Library/DebugLib.h>
+
+#include <libfdt.h>
+#include <sbi/riscv_asm.h>
+#include <sbi/sbi_domain.h>
+#include <sbi/sbi_hartmask.h>
+#include <sbi/sbi_platform.h>
+#include <sbi/sbi_string.h>
+#include <sbi/sbi_math.h>
+#include <sbi_utils/fdt/fdt_domain.h>
+#include <sbi_utils/fdt/fdt_fixup.h>
+#include <sbi_utils/fdt/fdt_helper.h>
+#include <sbi_utils/irqchip/fdt_irqchip.h>
+#include <sbi_utils/serial/fdt_serial.h>
+#include <sbi_utils/timer/fdt_timer.h>
+#include <sbi_utils/ipi/fdt_ipi.h>
+#include <sbi_utils/reset/fdt_reset.h>
+
+#include "SecMain.h"
+
+extern struct sbi_platform_operations platform_ops;
+
+int Edk2OpensbiPlatformEarlyInit (
+    BOOLEAN ColdBoot
+    )
+{
+    int ReturnCode;
+
+    DEBUG((DEBUG_INFO, "%a: Entry\n", __FUNCTION__));
+
+    if (platform_ops.early_init) {
+        ReturnCode = platform_ops.early_init (ColdBoot);
+        if (ReturnCode) {
+            return ReturnCode;
+        }
+    }
+    if (ColdBoot == TRUE) {
+        return SecPostOpenSbiPlatformEarlylInit(ColdBoot);
+    }
+    return 0;
+}
+
+int Edk2OpensbiPlatformFinalInit (
+    BOOLEAN ColdBoot
+    )
+{
+    int ReturnCode;
+
+    DEBUG((DEBUG_INFO, "%a: Entry\n", __FUNCTION__));
+
+    if (platform_ops.final_init) {
+        ReturnCode = platform_ops.final_init (ColdBoot);
+        if (ReturnCode) {
+            return ReturnCode;
+        }
+    }
+    if (ColdBoot == TRUE) {
+        return SecPostOpenSbiPlatformFinalInit(ColdBoot);
+    }
+    return 0;
+}
+
+VOID Edk2OpensbiPlatformEarlyExit (
+    VOID
+    )
+{
+    DEBUG((DEBUG_INFO, "%a: Entry\n", __FUNCTION__));
+
+    if (platform_ops.early_exit) {
+        return platform_ops.early_exit ();
+    }
+}
+
+/** Platform final exit */
+VOID Edk2OpensbiPlatformFinalExit (
+    VOID
+    )
+{
+    DEBUG((DEBUG_INFO, "%a: Entry\n", __FUNCTION__));
+
+    if (platform_ops.early_exit) {
+        return platform_ops.early_exit ();
+    }
+}
+
+/**
+  For platforms that do not implement misa, non-standard
+  methods are needed to determine cpu extension.
+**/
+int Edk2OpensbiPlatforMMISACheckExtension (
+    CHAR8 Extension
+    )
+{
+    if (platform_ops.misa_check_extension) {
+        return platform_ops.misa_check_extension (Extension);
+    }
+    return 0;
+}
+
+/**
+  For platforms that do not implement misa, non-standard
+  methods are needed to get MXL field of misa.
+**/
+int Edk2OpensbiPlatforMMISAGetXLEN (VOID)
+{
+    if (platform_ops.misa_get_xlen) {
+        return platform_ops.misa_get_xlen ();
+    }
+    return 0;
+}
+
+/** Get platform specific root domain memory regions */
+struct sbi_domain_memregion *
+Edk2OpensbiPlatformGetMemRegions (VOID)
+{
+    DEBUG((DEBUG_INFO, "%a: Entry\n", __FUNCTION__));
+
+    if (platform_ops.domains_root_regions) {
+        return platform_ops.domains_root_regions ();
+    }
+    return 0;
+}
+
+/** Initialize (or populate) domains for the platform */
+int Edk2OpensbiPlatformDomainsInit (VOID)
+{
+    DEBUG((DEBUG_INFO, "%a: Entry\n", __FUNCTION__));
+
+    if (platform_ops.domains_init) {
+        return platform_ops.domains_init ();
+    }
+    return 0;
+}
+
+/** Write a character to the platform console output */
+VOID Edk2OpensbiPlatformSerialPutc (
+    CHAR8 Ch
+    )
+{
+    if (platform_ops.console_putc) {
+        return platform_ops.console_putc (Ch);
+    }
+}
+
+/** Read a character from the platform console input */
+int Edk2OpensbiPlatformSerialGetc (VOID)
+{
+    if (platform_ops.console_getc) {
+        return platform_ops.console_getc ();
+    }
+    return 0;
+}
+
+/** Initialize the platform console */
+int Edk2OpensbiPlatformSerialInit (VOID)
+{
+    DEBUG((DEBUG_INFO, "%a: Entry\n", __FUNCTION__));
+
+    if (platform_ops.console_init) {
+        return platform_ops.console_init ();
+    }
+    return 0;
+}
+
+/** Initialize the platform interrupt controller for current HART */
+int Edk2OpensbiPlatformIrqchipInit (
+    BOOLEAN ColdBoot
+    )
+{
+    DEBUG((DEBUG_INFO, "%a: Entry\n", __FUNCTION__));
+
+    if (platform_ops.irqchip_init) {
+        return platform_ops.irqchip_init (ColdBoot);
+    }
+    return 0;
+}
+
+/** Exit the platform interrupt controller for current HART */
+VOID Edk2OpensbiPlatformIrqchipExit (VOID)
+{
+    DEBUG((DEBUG_INFO, "%a: Entry\n", __FUNCTION__));
+
+    if (platform_ops.irqchip_exit) {
+        return platform_ops.irqchip_exit ();
+    }
+}
+
+/** Send IPI to a target HART */
+VOID Edk2OpensbiPlatformIpiSend (
+    UINT32 TargetHart
+    )
+{
+    DEBUG((DEBUG_INFO, "%a: Entry\n", __FUNCTION__));
+
+    if (platform_ops.ipi_send) {
+        return platform_ops.ipi_send (TargetHart);
+    }
+}
+
+/** Clear IPI for a target HART */
+VOID Edk2OpensbiPlatformIpiClear (
+    UINT32 TargetHart
+    )
+{
+    DEBUG((DEBUG_INFO, "%a: Entry\n", __FUNCTION__));
+
+    if (platform_ops.ipi_clear) {
+        return platform_ops.ipi_clear (TargetHart);
+    }
+}
+
+/** Initialize IPI for current HART */
+int Edk2OpensbiPlatformIpiInit (
+    BOOLEAN ColdBoot
+    )
+{
+    DEBUG((DEBUG_INFO, "%a: Entry\n", __FUNCTION__));
+
+    if (platform_ops.ipi_init) {
+        return platform_ops.ipi_init (ColdBoot);
+    }
+    return 0;
+}
+
+/** Exit IPI for current HART */
+VOID Edk2OpensbiPlatformIpiExit (VOID)
+{
+    DEBUG((DEBUG_INFO, "%a: Entry\n", __FUNCTION__));
+
+    if (platform_ops.ipi_exit) {
+        return platform_ops.ipi_exit ();
+    }
+}
+
+/** Get tlb flush limit value **/
+UINT64 Edk2OpensbiPlatformTlbrFlushLimit (VOID)
+{
+    DEBUG((DEBUG_INFO, "%a: Entry\n", __FUNCTION__));
+
+    if (platform_ops.get_tlbr_flush_limit) {
+        return platform_ops.get_tlbr_flush_limit ();
+    }
+    return 0;
+}
+
+/** Get platform timer value */
+UINT64 Edk2OpensbiPlatformTimerValue (VOID)
+{
+    if (platform_ops.timer_value) {
+        return platform_ops.timer_value ();
+    }
+    return 0;
+}
+
+/** Start platform timer event for current HART */
+VOID Edk2OpensbiPlatformTimerEventStart (
+    UINT64 NextEvent
+    )
+{
+    if (platform_ops.timer_event_start) {
+        return platform_ops.timer_event_start (NextEvent);
+    }
+}
+
+/** Stop platform timer event for current HART */
+VOID Edk2OpensbiPlatformTimerEventStop (VOID)
+{
+    if (platform_ops.timer_event_stop) {
+        return platform_ops.timer_event_stop ();
+    }
+}
+
+/** Initialize platform timer for current HART */
+int Edk2OpensbiPlatformTimerInit (
+    BOOLEAN ColdBoot
+    )
+{
+    DEBUG((DEBUG_INFO, "%a: Entry\n", __FUNCTION__));
+
+    if (platform_ops.timer_init) {
+        return platform_ops.timer_init (ColdBoot);
+    }
+    return 0;
+}
+
+/** Exit platform timer for current HART */
+VOID Edk2OpensbiPlatformTimerExit (VOID)
+{
+    DEBUG((DEBUG_INFO, "%a: Entry\n", __FUNCTION__));
+
+    if (platform_ops.timer_exit) {
+        return platform_ops.timer_exit ();
+    }
+}
+
+/** Bringup the given hart */
+int Edk2OpensbiPlatformHartStart (
+    UINT32 HartId,
+    ulong Saddr
+    )
+{
+    DEBUG((DEBUG_INFO, "%a: Entry\n", __FUNCTION__));
+
+    if (platform_ops.hart_start) {
+        return platform_ops.hart_start (HartId, Saddr);
+    }
+    return 0;
+}
+/**
+  Stop the current hart from running. This call doesn't expect to
+  return if success.
+**/
+int Edk2OpensbiPlatformHartStop (VOID)
+{
+    DEBUG((DEBUG_INFO, "%a: Entry\n", __FUNCTION__));
+
+    if (platform_ops.hart_stop) {
+        return platform_ops.hart_stop ();
+    }
+    return 0;
+}
+
+/**
+ Check whether reset type and reason supported by the platform*
+
+**/
+int Edk2OpensbiPlatformSystemResetCheck (
+    UINT32 ResetType,
+    UINT32 ResetReason
+    )
+{
+    if (platform_ops.system_reset_check) {
+        return platform_ops.system_reset_check (ResetType, ResetReason);
+    }
+    return 0;
+}
+
+/** Reset the platform */
+VOID Edk2OpensbiPlatformSystemReset (
+    UINT32 ResetType,
+    UINT32 ResetReason
+    )
+{
+    DEBUG((DEBUG_INFO, "%a: Entry\n", __FUNCTION__));
+
+    if (platform_ops.system_reset) {
+        return platform_ops.system_reset (ResetType, ResetReason);
+    }
+}
+
+/** platform specific SBI extension implementation probe function */
+int Edk2OpensbiPlatformVendorExtCheck (
+    long ExtId
+    )
+{
+    DEBUG((DEBUG_INFO, "%a: Entry\n", __FUNCTION__));
+
+    if (platform_ops.vendor_ext_check) {
+        return platform_ops.vendor_ext_check (ExtId);
+    }
+    return 0;
+}
+
+
+/** platform specific SBI extension implementation provider */
+int Edk2OpensbiPlatformVendorExtProvider (
+    long ExtId,
+    long FuncId,
+    const struct sbi_trap_regs *Regs,
+    unsigned long *OutValue,
+    struct sbi_trap_info *OutTrap
+    )
+{
+    DEBUG((DEBUG_INFO, "%a: Entry\n", __FUNCTION__));
+
+    if (platform_ops.vendor_ext_provider) {
+        return platform_ops.vendor_ext_provider (
+                                ExtId,
+                                FuncId,
+                                Regs,
+                                OutValue,
+                                OutTrap
+                                );
+    }
+    return 0;
+}
+
+const struct sbi_platform_operations Edk2OpensbiPlatformOps = {
+    .early_init             = Edk2OpensbiPlatformEarlyInit,
+    .final_init             = Edk2OpensbiPlatformFinalInit,
+    .early_exit             = Edk2OpensbiPlatformEarlyExit,
+    .final_exit             = Edk2OpensbiPlatformFinalExit,
+    .misa_check_extension   = Edk2OpensbiPlatforMMISACheckExtension,
+    .misa_get_xlen          = Edk2OpensbiPlatforMMISAGetXLEN,
+    .domains_root_regions   = Edk2OpensbiPlatformGetMemRegions,
+    .domains_init           = Edk2OpensbiPlatformDomainsInit,
+    .console_putc           = Edk2OpensbiPlatformSerialPutc,
+    .console_getc           = Edk2OpensbiPlatformSerialGetc,
+    .console_init           = Edk2OpensbiPlatformSerialInit,
+    .irqchip_init           = Edk2OpensbiPlatformIrqchipInit,
+    .irqchip_exit           = Edk2OpensbiPlatformIrqchipExit,
+    .ipi_send               = Edk2OpensbiPlatformIpiSend,
+    .ipi_clear              = Edk2OpensbiPlatformIpiClear,
+    .ipi_init               = Edk2OpensbiPlatformIpiInit,
+    .ipi_exit               = Edk2OpensbiPlatformIpiExit,
+    .get_tlbr_flush_limit   = Edk2OpensbiPlatformTlbrFlushLimit,
+    .timer_value            = Edk2OpensbiPlatformTimerValue,
+    .timer_event_stop       = Edk2OpensbiPlatformTimerEventStop,
+    .timer_event_start      = Edk2OpensbiPlatformTimerEventStart,
+    .timer_init             = Edk2OpensbiPlatformTimerInit,
+    .timer_exit             = Edk2OpensbiPlatformTimerExit,
+    .hart_start             = Edk2OpensbiPlatformHartStart,
+    .hart_stop              = Edk2OpensbiPlatformHartStop,
+    .system_reset_check     = Edk2OpensbiPlatformSystemResetCheck,
+    .system_reset           = Edk2OpensbiPlatformSystemReset,
+    .vendor_ext_check       = Edk2OpensbiPlatformVendorExtCheck,
+    .vendor_ext_provider    = Edk2OpensbiPlatformVendorExtProvider,
+};
diff --git a/Platform/RISC-V/PlatformPkg/Universal/Sec/SecMain.c b/Platform/RISC-V/PlatformPkg/Universal/Sec/SecMain.c
index e88a7b8e80..44984b0078 100644
--- a/Platform/RISC-V/PlatformPkg/Universal/Sec/SecMain.c
+++ b/Platform/RISC-V/PlatformPkg/Universal/Sec/SecMain.c
@@ -23,6 +23,8 @@
 #include <sbi/sbi_ecall.h>    // Reference to header file in opensbi
 #include <sbi/sbi_trap.h>     // Reference to header file in opensbi
 
+extern struct sbi_platform_operations Edk2OpensbiPlatformOps;
+
 //
 // Indicates the boot hart (PcdBootHartId) OpenSBI initialization is done.
 //
@@ -31,27 +33,6 @@ atomic_t NonBootHartMessageLock = ATOMIC_INITIALIZER(0);
 
 typedef struct sbi_scratch *(*hartid2scratch)(ulong hartid, ulong hartindex);
 
-STATIC EFI_PEI_TEMPORARY_RAM_SUPPORT_PPI mTemporaryRamSupportPpi = {
-  TemporaryRamMigration
-};
-
-STATIC EFI_PEI_TEMPORARY_RAM_DONE_PPI mTemporaryRamDonePpi = {
-  TemporaryRamDone
-};
-
-STATIC EFI_PEI_PPI_DESCRIPTOR mPrivateDispatchTable[] = {
-  {
-    EFI_PEI_PPI_DESCRIPTOR_PPI,
-    &gEfiTemporaryRamSupportPpiGuid,
-    &mTemporaryRamSupportPpi
-  },
-  {
-    (EFI_PEI_PPI_DESCRIPTOR_PPI | EFI_PEI_PPI_DESCRIPTOR_TERMINATE_LIST),
-    &gEfiTemporaryRamDonePpiGuid,
-    &mTemporaryRamDonePpi
-  },
-};
-
 /**
   Locates a section within a series of sections
   with the specified section type.
@@ -491,6 +472,91 @@ RegisterFirmwareSbiExtension (
 
   return EFI_SUCCESS;
 }
+
+/**
+  OpenSBI platform early init hook.
+
+**/
+int
+SecPostOpenSbiPlatformEarlylInit(
+  IN BOOLEAN ColdBoot
+  )
+{
+  //
+  // Boot HART is already in the process of OpenSBI initialization.
+  // We can let other HART to keep booting.
+  //
+  DEBUG ((DEBUG_INFO, "%a: Set boot hart done.\n", __FUNCTION__));
+  atomic_write (&BootHartDone, (UINT64)TRUE);
+  return 0;
+}
+
+/**
+  OpenSBI platform final init hook.
+  We restore the next_arg1 to the pointer of EFI_RISCV_OPENSBI_FIRMWARE_CONTEXT.
+
+**/
+int
+SecPostOpenSbiPlatformFinalInit (
+  IN BOOLEAN ColdBoot
+  )
+{
+  UINT32 HartId;
+  struct sbi_scratch *SbiScratch;
+  struct sbi_scratch *ScratchSpace;
+  struct sbi_platform *SbiPlatform;
+  EFI_RISCV_OPENSBI_FIRMWARE_CONTEXT *FirmwareContext;
+
+  DEBUG((DEBUG_INFO, "%a: Entry, preparing to jump to PEI Core\n\n", __FUNCTION__));
+
+  SbiScratch = sbi_scratch_thishart_ptr();
+  SbiPlatform = (struct sbi_platform *)sbi_platform_ptr(SbiScratch);
+  FirmwareContext = (EFI_RISCV_OPENSBI_FIRMWARE_CONTEXT *)SbiPlatform->firmware_context;
+
+  //
+  // Print out scratch address of each hart
+  //
+  DEBUG ((DEBUG_INFO, "%a: OpenSBI scratch address for each hart:\n", __FUNCTION__));
+  for (HartId = 0; HartId < SBI_HARTMASK_MAX_BITS; HartId ++) {
+    if (sbi_platform_hart_invalid(SbiPlatform, HartId)) {
+      continue;
+    }
+    ScratchSpace = sbi_hartid_to_scratch (HartId);
+    if(ScratchSpace != NULL) {
+      DEBUG((DEBUG_INFO, "          Hart %d: 0x%x\n", HartId, ScratchSpace));
+    } else {
+      DEBUG((DEBUG_INFO, "          Hart %d not initialized yet\n", HartId));
+    }
+  }
+
+  //
+  // Set firmware context Hart-specific pointer
+  //
+  for (HartId = 0; HartId < SBI_HARTMASK_MAX_BITS; HartId ++) {
+    if (sbi_platform_hart_invalid(SbiPlatform, HartId)) {
+      continue;
+    }
+    ScratchSpace = sbi_hartid_to_scratch (HartId);
+    if (ScratchSpace != NULL) {
+      FirmwareContext->HartSpecific[HartId] =
+        (EFI_RISCV_FIRMWARE_CONTEXT_HART_SPECIFIC *)((UINT8 *)ScratchSpace - FIRMWARE_CONTEXT_HART_SPECIFIC_SIZE);
+        DEBUG ((DEBUG_INFO, "%a: OpenSBI Hart %d Firmware Context Hart-specific at address: 0x%x\n",
+                __FUNCTION__,
+                 HartId,
+                 FirmwareContext->HartSpecific [HartId]
+                 ));
+    }
+  }
+
+  DEBUG((DEBUG_INFO, "%a: Jump to PEI Core with \n", __FUNCTION__));
+  DEBUG((DEBUG_INFO, "  sbi_scratch = %x\n", SbiScratch));
+  DEBUG((DEBUG_INFO, "  sbi_platform = %x\n", SbiPlatform));
+  DEBUG((DEBUG_INFO, "  FirmwareContext = %x\n", FirmwareContext));
+  SbiScratch->next_arg1 = (unsigned long)FirmwareContext;
+
+  return 0;
+}
+
 /** Transion from SEC phase to PEI phase.
 
   This function transits to S-mode PEI phase from M-mode SEC phase.
@@ -508,9 +574,7 @@ VOID EFIAPI PeiCore (
   EFI_PEI_CORE_ENTRY_POINT    PeiCoreEntryPoint;
   EFI_FIRMWARE_VOLUME_HEADER *BootFv = (EFI_FIRMWARE_VOLUME_HEADER *)FixedPcdGet32(PcdRiscVPeiFvBase);
   EFI_RISCV_OPENSBI_FIRMWARE_CONTEXT FirmwareContext;
-  struct sbi_scratch         *ScratchSpace;
   struct sbi_platform        *ThisSbiPlatform;
-  UINT32 HartId;
 
   FindAndReportEntryPoints (&BootFv, &PeiCoreEntryPoint);
 
@@ -524,17 +588,6 @@ VOID EFIAPI PeiCore (
   SecCoreData.StackBase              = (UINT8 *)SecCoreData.TemporaryRamBase + (SecCoreData.TemporaryRamSize >> 1);
   SecCoreData.StackSize              = SecCoreData.TemporaryRamSize >> 1;
 
-  //
-  // Print out scratch address of each hart
-  //
-  DEBUG ((DEBUG_INFO, "%a: OpenSBI scratch address for each hart:\n", __FUNCTION__));
-  for (HartId = 0; HartId < SBI_HARTMASK_MAX_BITS; HartId ++) {
-    ScratchSpace = sbi_hartid_to_scratch (HartId);
-    if(ScratchSpace != NULL) {
-      DEBUG((DEBUG_INFO, "          Hart %d: 0x%x\n", HartId, ScratchSpace));
-    }
-  }
-
   //
   // Set up OpepSBI firmware context pointer on boot hart OpenSbi scratch.
   // Firmware context residents in stack and will be switched to memory when
@@ -564,20 +617,10 @@ VOID EFIAPI PeiCore (
   FirmwareContext.FlattenedDeviceTree = Scratch->next_arg1;
 
   //
-  // Set firmware context Hart-specific pointer
+  // Transfer the control to the PEI core
   //
-  for (HartId = 0; HartId < SBI_HARTMASK_MAX_BITS; HartId ++) {
-    ScratchSpace = sbi_hartid_to_scratch (HartId);
-    if (ScratchSpace != NULL) {
-      FirmwareContext.HartSpecific[HartId] =
-        (EFI_RISCV_FIRMWARE_CONTEXT_HART_SPECIFIC *)((UINT8 *)ScratchSpace - FIRMWARE_CONTEXT_HART_SPECIFIC_SIZE);
-        DEBUG ((DEBUG_INFO, "%a: OpenSBI Hart %d Firmware Context Hart-specific at address: 0x%x\n",
-                __FUNCTION__,
-                 HartId,
-                 FirmwareContext.HartSpecific [HartId]
-                 ));
-    }
-  }
+  FirmwareContext.SecPeiHandOffData = (UINT64)&SecCoreData;
+
   //
   // Set supervisor translation mode to Bare mode
   //
@@ -585,13 +628,12 @@ VOID EFIAPI PeiCore (
   RiscVSetSupervisorAddressTranslationRegister ((UINT64)RISCV_SATP_MODE_OFF << RISCV_SATP_MODE_BIT_POSITION);
 
   //
-  // Transfer the control to the PEI core
+  // Scratch->next_arg1 is the device tree.
   //
-  Scratch->next_addr = (UINTN)(*PeiCoreEntryPoint);
+  Scratch->next_addr = (UINTN)(PeiCoreEntryPoint);
   Scratch->next_mode = PRV_S;
   DEBUG ((DEBUG_INFO, "%a: Initializing OpenSBI library for booting hart %d\n", __FUNCTION__, BootHartId));
   sbi_init(Scratch);
-  (*PeiCoreEntryPoint) (&SecCoreData, (EFI_PEI_PPI_DESCRIPTOR *)&mPrivateDispatchTable);
 }
 
 /**
@@ -715,6 +757,7 @@ VOID EFIAPI SecCoreStartUpWithStack(
 {
   UINT64 BootHartDoneSbiInit;
   UINT64 NonBootHartMessageLockValue;
+  struct sbi_platform *ThisSbiPlatform;
   EFI_RISCV_FIRMWARE_CONTEXT_HART_SPECIFIC *HartFirmwareContext;
 
   Scratch->next_arg1 = (unsigned long)GetDeviceTreeAddress ();
@@ -737,6 +780,14 @@ VOID EFIAPI SecCoreStartUpWithStack(
   HartFirmwareContext->MachineImplId.Value64_H = 0;
   HartFirmwareContext->HartSwitchMode = RiscVOpenSbiHartSwitchMode;
 
+  //
+  // Hook platorm_ops with EDK2 one. Thus we can have interface
+  // call out to OEM EDK2 platform code in M-mode before switching
+  // to S-mode in opensbo init.
+  //
+  ThisSbiPlatform = (struct sbi_platform *)sbi_platform_ptr(Scratch);
+  ThisSbiPlatform->platform_ops_addr = (unsigned long)&Edk2OpensbiPlatformOps;
+
   if (HartId == FixedPcdGet32(PcdBootHartId)) {
     LaunchPeiCore (HartId, Scratch);
   }
@@ -768,6 +819,7 @@ VOID EFIAPI SecCoreStartUpWithStack(
   // Non boot hart wiil be halted waiting for SBI_HART_STARTING.
   // Use HSM ecall to start non boot hart (SBI_EXT_HSM_HART_START) later on,
   //
+  Scratch->next_mode = PRV_S;
   sbi_init(Scratch);
 }
 
-- 
2.31.1