[PATCH 19/79] PlatformPkg/SecMain: RISC-V SecMain module.

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

(This is migrated from edk2-platforms:Platform/RISC-V)
SecMain module for RISC-V platform.
This was cloned from OpenSBI fw_base.S
(RiscVPkg/Library/RiscVOpensbiLib/opensbi/firmware/) and revised to
edk2 framework.

Signed-off-by: Abner Chang <abner.chang@hpe.com>
Co-authored-by: Daniel Schaefer <daniel.schaefer@hpe.com>
Co-authored-by: Gilbert Chen <gilbert.chen@hpe.com>
Reviewed-by: Leif Lindholm <leif.lindholm@linaro.org>

Cc: Leif Lindholm <leif.lindholm@linaro.org>
Cc: Gilbert Chen <gilbert.chen@hpe.com>
---
 .../PlatformPkg/Universal/Sec/SecMain.inf     |  77 ++
 .../PlatformPkg/Universal/Sec/SecMain.h       |  57 ++
 .../PlatformPkg/Universal/Sec/SecMain.c       | 733 ++++++++++++++++++
 .../Universal/Sec/Riscv64/SecEntry.S          | 532 +++++++++++++
 4 files changed, 1399 insertions(+)
 create mode 100644 Platform/RISC-V/PlatformPkg/Universal/Sec/SecMain.inf
 create mode 100644 Platform/RISC-V/PlatformPkg/Universal/Sec/SecMain.h
 create mode 100644 Platform/RISC-V/PlatformPkg/Universal/Sec/SecMain.c
 create mode 100644 Platform/RISC-V/PlatformPkg/Universal/Sec/Riscv64/SecEntry.S

diff --git a/Platform/RISC-V/PlatformPkg/Universal/Sec/SecMain.inf b/Platform/RISC-V/PlatformPkg/Universal/Sec/SecMain.inf
new file mode 100644
index 0000000000..89bcb039a6
--- /dev/null
+++ b/Platform/RISC-V/PlatformPkg/Universal/Sec/SecMain.inf
@@ -0,0 +1,77 @@
+## @file
+#  RISC-V SEC module.
+#
+#  Copyright (c) 2020, Hewlett Packard Enterprise Development LP. All rights reserved.<BR>
+#
+#  SPDX-License-Identifier: BSD-2-Clause-Patent
+#
+##
+
+[Defines]
+  INF_VERSION                    = 0x0001001b
+  BASE_NAME                      = SecMain
+  FILE_GUID                      = 743467B0-849F-4ACE-9BFB-515CE6206388
+  MODULE_TYPE                    = SEC
+  VERSION_STRING                 = 1.0
+  ENTRY_POINT                    = SecMain
+
+#
+# The following information is for reference only and not required by the build tools.
+#
+#  VALID_ARCHITECTURES           = RISCV64
+#
+
+[Sources]
+  SecMain.c
+
+[Sources.RISCV64]
+  Riscv64/SecEntry.S
+
+[Packages]
+  MdeModulePkg/MdeModulePkg.dec
+  MdePkg/MdePkg.dec
+  Silicon/RISC-V/ProcessorPkg/RiscVProcessorPkg.dec
+  Platform/RISC-V/PlatformPkg/RiscVPlatformPkg.dec
+
+[LibraryClasses]
+  BaseLib
+  BaseMemoryLib
+  DebugAgentLib
+  DebugLib
+  ExtractGuidedSectionLib
+  IoLib
+  PcdLib
+  PeCoffLib
+  PeCoffGetEntryPointLib
+  PeCoffExtraActionLib
+  PrintLib
+  RiscVCpuLib
+  RiscVOpensbiLib
+  RiscVOpensbiPlatformLib
+  RiscVEdk2SbiLib
+
+[Ppis]
+  gEfiTemporaryRamSupportPpiGuid # PPI ALWAYS_PRODUCED
+  gEfiTemporaryRamDonePpiGuid    # PPI ALWAYS_PRODUCED
+
+[FixedPcd]
+  gUefiRiscVPlatformPkgTokenSpaceGuid.PcdRiscVPeiFvBase
+  gUefiRiscVPlatformPkgTokenSpaceGuid.PcdRiscVPeiFvSize
+
+[Pcd]
+  gUefiRiscVPlatformPkgTokenSpaceGuid.PcdBootHartId
+  gUefiRiscVPlatformPkgTokenSpaceGuid.PcdHartCount
+  gUefiRiscVPlatformPkgTokenSpaceGuid.PcdBootableHartNumber
+  gUefiRiscVPlatformPkgTokenSpaceGuid.PcdFwStartAddress
+  gUefiRiscVPlatformPkgTokenSpaceGuid.PcdFwEndAddress
+  gUefiRiscVPlatformPkgTokenSpaceGuid.PcdOpenSbiStackSize
+  gUefiRiscVPlatformPkgTokenSpaceGuid.PcdScratchRamBase
+  gUefiRiscVPlatformPkgTokenSpaceGuid.PcdScratchRamSize
+  gUefiRiscVPlatformPkgTokenSpaceGuid.PcdTemporaryRamBase
+  gUefiRiscVPlatformPkgTokenSpaceGuid.PcdTemporaryRamSize
+  gUefiRiscVPlatformPkgTokenSpaceGuid.PcdPeiCorePrivilegeMode
+
+[BuildOptions]
+  GCC:*_*_*_PP_FLAGS = -D__ASSEMBLY__
+
+
diff --git a/Platform/RISC-V/PlatformPkg/Universal/Sec/SecMain.h b/Platform/RISC-V/PlatformPkg/Universal/Sec/SecMain.h
new file mode 100644
index 0000000000..94ea46263c
--- /dev/null
+++ b/Platform/RISC-V/PlatformPkg/Universal/Sec/SecMain.h
@@ -0,0 +1,57 @@
+/** @file
+  RISC-V SEC phase module definitions..
+
+  Copyright (c) 2019, Hewlett Packard Enterprise Development LP. All rights reserved.<BR>
+
+  SPDX-License-Identifier: BSD-2-Clause-Patent
+
+**/
+
+#ifndef SECMAIN_H_
+#define SECMAIN_H_
+
+#include <PiPei.h>
+#include <Library/PeimEntryPoint.h>
+#include <Library/BaseLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/DebugAgentLib.h>
+#include <Library/DebugLib.h>
+#include <Library/ExtractGuidedSectionLib.h>
+#include <Library/IoLib.h>
+#include <Library/HobLib.h>
+#include <Library/PcdLib.h>
+#include <Library/PeCoffExtraActionLib.h>
+#include <Library/PeCoffGetEntryPointLib.h>
+#include <Library/PeCoffLib.h>
+#include <Library/PeiServicesLib.h>
+#include <Library/RiscVCpuLib.h>
+#include <Ppi/TemporaryRamDone.h>
+#include <Ppi/TemporaryRamSupport.h>
+
+VOID
+SecMachineModeTrapHandler (
+  IN VOID
+  );
+
+VOID
+EFIAPI
+SecStartupPhase2 (
+  IN VOID                     *Context
+  );
+
+EFI_STATUS
+EFIAPI
+TemporaryRamMigration (
+  IN CONST EFI_PEI_SERVICES   **PeiServices,
+  IN EFI_PHYSICAL_ADDRESS     TemporaryMemoryBase,
+  IN EFI_PHYSICAL_ADDRESS     PermanentMemoryBase,
+  IN UINTN                    CopySize
+  );
+
+EFI_STATUS
+EFIAPI
+TemporaryRamDone (
+  VOID
+  );
+
+#endif // _SECMAIN_H_
diff --git a/Platform/RISC-V/PlatformPkg/Universal/Sec/SecMain.c b/Platform/RISC-V/PlatformPkg/Universal/Sec/SecMain.c
new file mode 100644
index 0000000000..877777bfa1
--- /dev/null
+++ b/Platform/RISC-V/PlatformPkg/Universal/Sec/SecMain.c
@@ -0,0 +1,733 @@
+/** @file
+  RISC-V SEC phase module.
+
+  Copyright (c) 2020, Hewlett Packard Enterprise Development LP. All rights reserved.<BR>
+
+  SPDX-License-Identifier: BSD-2-Clause-Patent
+
+**/
+
+#include <SecMain.h>
+#include <IndustryStandard/RiscVOpensbi.h>
+#include <Library/DebugPrintErrorLevelLib.h>
+#include <Library/PrintLib.h>
+#include <Library/RiscVEdk2SbiLib.h>
+#include <sbi/riscv_asm.h>
+#include <sbi/riscv_atomic.h>
+#include <sbi/sbi_console.h>  // Reference to header file in opensbi
+#include <sbi/sbi_hart.h>     // Reference to header file in opensbi
+#include <sbi/sbi_hartmask.h>    // Reference to header file in opensbi
+#include <sbi/sbi_scratch.h>  // Reference to header file in opensbi
+#include <sbi/sbi_platform.h> // Reference to header file in opensbi
+#include <sbi/sbi_init.h>     // Reference to header file in opensbi
+#include <sbi/sbi_ecall.h>    // Reference to header file in opensbi
+
+//
+// Indicates the boot hart (PcdBootHartId) OpenSBI initialization is done.
+//
+atomic_t BootHartDone = ATOMIC_INITIALIZER(0);
+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.
+
+  The Instance parameter indicates which instance of the section
+  type to return. (0 is first instance, 1 is second...)
+
+  @param[in]   Sections        The sections to search
+  @param[in]   SizeOfSections  Total size of all sections
+  @param[in]   SectionType     The section type to locate
+  @param[in]   Instance        The section instance number
+  @param[out]  FoundSection    The FFS section if found
+
+  @retval EFI_SUCCESS           The file and section was found
+  @retval EFI_NOT_FOUND         The file and section was not found
+  @retval EFI_VOLUME_CORRUPTED  The firmware volume was corrupted
+
+**/
+EFI_STATUS
+FindFfsSectionInstance (
+  IN  VOID                             *Sections,
+  IN  UINTN                            SizeOfSections,
+  IN  EFI_SECTION_TYPE                 SectionType,
+  IN  UINTN                            Instance,
+  OUT EFI_COMMON_SECTION_HEADER        **FoundSection
+  )
+{
+  EFI_PHYSICAL_ADDRESS        CurrentAddress;
+  UINT32                      Size;
+  EFI_PHYSICAL_ADDRESS        EndOfSections;
+  EFI_COMMON_SECTION_HEADER   *Section;
+  EFI_PHYSICAL_ADDRESS        EndOfSection;
+
+  //
+  // Loop through the FFS file sections within the PEI Core FFS file
+  //
+  EndOfSection = (EFI_PHYSICAL_ADDRESS)(UINTN) Sections;
+  EndOfSections = EndOfSection + SizeOfSections;
+  for (;;) {
+    if (EndOfSection == EndOfSections) {
+      break;
+    }
+    CurrentAddress = (EndOfSection + 3) & ~(3ULL);
+    if (CurrentAddress >= EndOfSections) {
+      return EFI_VOLUME_CORRUPTED;
+    }
+
+    Section = (EFI_COMMON_SECTION_HEADER*)(UINTN) CurrentAddress;
+
+    Size = SECTION_SIZE (Section);
+    if (Size < sizeof (*Section)) {
+      return EFI_VOLUME_CORRUPTED;
+    }
+
+    EndOfSection = CurrentAddress + Size;
+    if (EndOfSection > EndOfSections) {
+      return EFI_VOLUME_CORRUPTED;
+    }
+
+    //
+    // Look for the requested section type
+    //
+    if (Section->Type == SectionType) {
+      if (Instance == 0) {
+        *FoundSection = Section;
+        return EFI_SUCCESS;
+      } else {
+        Instance--;
+      }
+    }
+  }
+
+  return EFI_NOT_FOUND;
+}
+
+/**
+  Locates a section within a series of sections
+  with the specified section type.
+
+  @param[in]   Sections        The sections to search
+  @param[in]   SizeOfSections  Total size of all sections
+  @param[in]   SectionType     The section type to locate
+  @param[out]  FoundSection    The FFS section if found
+
+  @retval EFI_SUCCESS           The file and section was found
+  @retval EFI_NOT_FOUND         The file and section was not found
+  @retval EFI_VOLUME_CORRUPTED  The firmware volume was corrupted
+
+**/
+EFI_STATUS
+FindFfsSectionInSections (
+  IN  VOID                             *Sections,
+  IN  UINTN                            SizeOfSections,
+  IN  EFI_SECTION_TYPE                 SectionType,
+  OUT EFI_COMMON_SECTION_HEADER        **FoundSection
+  )
+{
+  return FindFfsSectionInstance (
+           Sections,
+           SizeOfSections,
+           SectionType,
+           0,
+           FoundSection
+           );
+}
+
+/**
+  Locates a FFS file with the specified file type and a section
+  within that file with the specified section type.
+
+  @param[in]   Fv            The firmware volume to search
+  @param[in]   FileType      The file type to locate
+  @param[in]   SectionType   The section type to locate
+  @param[out]  FoundSection  The FFS section if found
+
+  @retval EFI_SUCCESS           The file and section was found
+  @retval EFI_NOT_FOUND         The file and section was not found
+  @retval EFI_VOLUME_CORRUPTED  The firmware volume was corrupted
+
+**/
+EFI_STATUS
+FindFfsFileAndSection (
+  IN  EFI_FIRMWARE_VOLUME_HEADER       *Fv,
+  IN  EFI_FV_FILETYPE                  FileType,
+  IN  EFI_SECTION_TYPE                 SectionType,
+  OUT EFI_COMMON_SECTION_HEADER        **FoundSection
+  )
+{
+  EFI_STATUS                  Status;
+  EFI_PHYSICAL_ADDRESS        CurrentAddress;
+  EFI_PHYSICAL_ADDRESS        EndOfFirmwareVolume;
+  EFI_FFS_FILE_HEADER         *File;
+  UINT32                      Size;
+  EFI_PHYSICAL_ADDRESS        EndOfFile;
+
+  if (Fv->Signature != EFI_FVH_SIGNATURE) {
+    DEBUG ((DEBUG_ERROR, "%a: FV at %p does not have FV header signature\n", __FUNCTION__, Fv));
+    return EFI_VOLUME_CORRUPTED;
+  }
+
+  CurrentAddress = (EFI_PHYSICAL_ADDRESS)(UINTN) Fv;
+  EndOfFirmwareVolume = CurrentAddress + Fv->FvLength;
+
+  //
+  // Loop through the FFS files in the Boot Firmware Volume
+  //
+  for (EndOfFile = CurrentAddress + Fv->HeaderLength; ; ) {
+
+    CurrentAddress = (EndOfFile + 7) & ~(7ULL);
+    if (CurrentAddress > EndOfFirmwareVolume) {
+      return EFI_VOLUME_CORRUPTED;
+    }
+
+    File = (EFI_FFS_FILE_HEADER*)(UINTN) CurrentAddress;
+    Size = *(UINT32*) File->Size & 0xffffff;
+    if (Size < (sizeof (*File) + sizeof (EFI_COMMON_SECTION_HEADER))) {
+      return EFI_VOLUME_CORRUPTED;
+    }
+
+    EndOfFile = CurrentAddress + Size;
+    if (EndOfFile > EndOfFirmwareVolume) {
+      return EFI_VOLUME_CORRUPTED;
+    }
+
+    //
+    // Look for the request file type
+    //
+    if (File->Type != FileType) {
+      continue;
+    }
+
+    Status = FindFfsSectionInSections (
+               (VOID*) (File + 1),
+               (UINTN) EndOfFile - (UINTN) (File + 1),
+               SectionType,
+               FoundSection
+               );
+    if (!EFI_ERROR (Status) || (Status == EFI_VOLUME_CORRUPTED)) {
+      return Status;
+    }
+  }
+}
+
+/**
+  Locates the PEI Core entry point address
+
+  @param[in]  Fv                 The firmware volume to search
+  @param[out] PeiCoreEntryPoint  The entry point of the PEI Core image
+
+  @retval EFI_SUCCESS           The file and section was found
+  @retval EFI_NOT_FOUND         The file and section was not found
+  @retval EFI_VOLUME_CORRUPTED  The firmware volume was corrupted
+
+**/
+EFI_STATUS
+FindPeiCoreImageBaseInFv (
+  IN  EFI_FIRMWARE_VOLUME_HEADER       *Fv,
+  OUT  EFI_PHYSICAL_ADDRESS             *PeiCoreImageBase
+  )
+{
+  EFI_STATUS                  Status;
+  EFI_COMMON_SECTION_HEADER   *Section;
+
+  Status = FindFfsFileAndSection (
+             Fv,
+             EFI_FV_FILETYPE_PEI_CORE,
+             EFI_SECTION_PE32,
+             &Section
+             );
+  if (EFI_ERROR (Status)) {
+    Status = FindFfsFileAndSection (
+               Fv,
+               EFI_FV_FILETYPE_PEI_CORE,
+               EFI_SECTION_TE,
+               &Section
+               );
+    if (EFI_ERROR (Status)) {
+      DEBUG ((DEBUG_ERROR, "%a: Unable to find PEI Core image\n", __FUNCTION__));
+      return Status;
+    }
+  }
+  DEBUG ((DEBUG_INFO, "%a: PeiCoreImageBase found\n", __FUNCTION__));
+  *PeiCoreImageBase = (EFI_PHYSICAL_ADDRESS)(UINTN)(Section + 1);
+  return EFI_SUCCESS;
+}
+
+/**
+  Locates the PEI Core entry point address
+
+  @param[in,out]  Fv                 The firmware volume to search
+  @param[out]     PeiCoreEntryPoint  The entry point of the PEI Core image
+
+  @retval EFI_SUCCESS           The file and section was found
+  @retval EFI_NOT_FOUND         The file and section was not found
+  @retval EFI_VOLUME_CORRUPTED  The firmware volume was corrupted
+
+**/
+VOID
+FindPeiCoreImageBase (
+  IN OUT  EFI_FIRMWARE_VOLUME_HEADER       **BootFv,
+     OUT  EFI_PHYSICAL_ADDRESS             *PeiCoreImageBase
+  )
+{
+  *PeiCoreImageBase = 0;
+
+  DEBUG ((DEBUG_INFO, "%a: Entry\n", __FUNCTION__));
+  FindPeiCoreImageBaseInFv (*BootFv, PeiCoreImageBase);
+}
+
+/*
+  Find and return Pei Core entry point.
+
+  It also find SEC and PEI Core file debug inforamtion. It will report them if
+  remote debug is enabled.
+
+**/
+VOID
+FindAndReportEntryPoints (
+  IN  EFI_FIRMWARE_VOLUME_HEADER       **BootFirmwareVolumePtr,
+  OUT EFI_PEI_CORE_ENTRY_POINT         *PeiCoreEntryPoint
+  )
+{
+  EFI_STATUS                       Status;
+  EFI_PHYSICAL_ADDRESS             PeiCoreImageBase;
+
+  DEBUG ((DEBUG_INFO, "%a: Entry\n", __FUNCTION__));
+
+  FindPeiCoreImageBase (BootFirmwareVolumePtr, &PeiCoreImageBase);
+  //
+  // Find PEI Core entry point
+  //
+  Status = PeCoffLoaderGetEntryPoint ((VOID *) (UINTN) PeiCoreImageBase, (VOID**) PeiCoreEntryPoint);
+  if (EFI_ERROR(Status)) {
+    *PeiCoreEntryPoint = 0;
+  }
+  DEBUG ((DEBUG_INFO, "%a: PeCoffLoaderGetEntryPoint success: %x\n", __FUNCTION__, *PeiCoreEntryPoint));
+
+  return;
+}
+/*
+  Print out the content of firmware context.
+
+**/
+VOID
+DebutPrintFirmwareContext (
+    EFI_RISCV_OPENSBI_FIRMWARE_CONTEXT *FirmwareContext
+    )
+{
+  DEBUG ((DEBUG_INFO, "%a: OpenSBI Firmware Context at 0x%x\n", __FUNCTION__, FirmwareContext));
+  DEBUG ((DEBUG_INFO, "%a:              PEI Service at 0x%x\n\n", __FUNCTION__, FirmwareContext->PeiServiceTable));
+}
+/** Temporary RAM migration function.
+
+  This function migrates the data from temporary RAM to permanent
+  memory.
+
+  @param[in]  PeiServices           PEI service
+  @param[in]  TemporaryMemoryBase   Temporary memory base address
+  @param[in]  PermanentMemoryBase   Permanent memory base address
+  @param[in]  CopySize              Size to copy
+
+**/
+EFI_STATUS
+EFIAPI
+TemporaryRamMigration (
+  IN CONST EFI_PEI_SERVICES   **PeiServices,
+  IN EFI_PHYSICAL_ADDRESS     TemporaryMemoryBase,
+  IN EFI_PHYSICAL_ADDRESS     PermanentMemoryBase,
+  IN UINTN                    CopySize
+  )
+{
+  VOID      *OldHeap;
+  VOID      *NewHeap;
+  VOID      *OldStack;
+  VOID      *NewStack;
+  EFI_RISCV_OPENSBI_FIRMWARE_CONTEXT *FirmwareContext;
+
+  DEBUG ((DEBUG_INFO,
+    "%a: Temp Mem Base:0x%Lx, Permanent Mem Base:0x%Lx, CopySize:0x%Lx\n",
+    __FUNCTION__,
+    TemporaryMemoryBase,
+    PermanentMemoryBase,
+    (UINT64)CopySize
+    ));
+
+  OldHeap = (VOID*)(UINTN)TemporaryMemoryBase;
+  NewHeap = (VOID*)((UINTN)PermanentMemoryBase + (CopySize >> 1));
+
+  OldStack = (VOID*)((UINTN)TemporaryMemoryBase + (CopySize >> 1));
+  NewStack = (VOID*)(UINTN)PermanentMemoryBase;
+
+  CopyMem (NewHeap, OldHeap, CopySize >> 1);   // Migrate Heap
+  CopyMem (NewStack, OldStack, CopySize >> 1); // Migrate Stack
+
+  //
+  // Reset firmware context pointer
+  //
+  SbiGetFirmwareContext (&FirmwareContext);
+  FirmwareContext = (VOID *)FirmwareContext + (unsigned long)((UINTN)NewStack - (UINTN)OldStack);
+  SbiSetFirmwareContext (FirmwareContext);
+
+  //
+  // Relocate PEI Service **
+  //
+  FirmwareContext->PeiServiceTable += (unsigned long)((UINTN)NewStack - (UINTN)OldStack);
+  DEBUG ((DEBUG_INFO, "%a: OpenSBI Firmware Context is relocated to 0x%x\n", __FUNCTION__, FirmwareContext));
+  DebutPrintFirmwareContext ((EFI_RISCV_OPENSBI_FIRMWARE_CONTEXT *)FirmwareContext);
+
+  register uintptr_t a0 asm ("a0") = (uintptr_t)((UINTN)NewStack - (UINTN)OldStack);
+  asm volatile ("add sp, sp, a0"::"r"(a0):);
+  return EFI_SUCCESS;
+}
+
+/** Temprary RAM done function.
+
+**/
+EFI_STATUS EFIAPI TemporaryRamDone (
+  VOID
+  )
+{
+  DEBUG ((DEBUG_INFO, "%a: 2nd time PEI core, temporary ram done.\n", __FUNCTION__));
+  return EFI_SUCCESS;
+}
+
+/** Handles SBI calls of EDK2's SBI FW extension
+
+  @param[in]  ExtId        The extension ID of the FW extension.
+  @param[in]  FuncId       The called function ID.
+  @param[in]  Args         The args to the function.
+  @param[out] OutVal       The value the function returns to the caller.
+  @param[out] OutTrap      Trap info for trapping further, see OpenSBI code.
+                           Is ignored if return value is not SBI_ETRAP.
+
+  @retval 0                If the handler succeeds.
+  @retval SBI_ENOTSUPP     If there's no function with the given ID.
+  @retval SBI_ETRAP        If the called SBI functions wants to trap further.
+**/
+STATIC int SbiEcallFirmwareHandler (
+  IN  unsigned long         ExtId,
+  IN  unsigned long         FuncId,
+  IN  unsigned long        *Args,
+  OUT unsigned long        *OutVal,
+  OUT struct sbi_trap_info *OutTrap
+  )
+{
+  int Ret = 0;
+
+  switch (FuncId) {
+    case SBI_EXT_FW_MSCRATCH_FUNC:
+      *OutVal = (unsigned long) sbi_scratch_thishart_ptr();
+      break;
+    case SBI_EXT_FW_MSCRATCH_HARTID_FUNC:
+      *OutVal = (unsigned long) sbi_hartid_to_scratch (Args[0]);
+      break;
+    default:
+      Ret = SBI_ENOTSUPP;
+  };
+
+  return Ret;
+}
+
+struct sbi_ecall_extension FirmwareEcall = {
+  .extid_start = SBI_EDK2_FW_EXT,
+  .extid_end = SBI_EDK2_FW_EXT,
+  .handle = SbiEcallFirmwareHandler,
+};
+
+/** Register EDK2's SBI extension with OpenSBI
+
+  This function returns EFI_STATUS, even though it only ever returns
+  EFI_SUCCESS. On error it ASSERTs. Looking at OpenSBI code it appears that
+  registering an extension can only fail if the extension ID is invalid or was
+  already registered. Failure is therefore an error of the programmer.
+
+  @retval EFI_SUCCESS If the extension was successfully registered.
+**/
+EFI_STATUS
+EFIAPI
+RegisterFirmwareSbiExtension (
+  VOID
+  )
+{
+  UINTN Ret;
+  Ret = sbi_ecall_register_extension(&FirmwareEcall);
+  if (Ret) {
+    //
+    // Only fails if the extension ID is invalid or already is registered.
+    //
+    DEBUG ((DEBUG_ERROR, "Failed to register SBI Firmware Extension for EDK2\n"));
+    ASSERT(FALSE);
+  }
+
+  return EFI_SUCCESS;
+}
+/** Transion from SEC phase to PEI phase.
+
+  This function transits to S-mode PEI phase from M-mode SEC phase.
+
+  @param[in]  BootHartId     Hardware thread ID of boot hart.
+  @param[in]  FuncArg1       Arg1 delivered from previous phase.
+
+**/
+VOID EFIAPI PeiCore (
+  IN  UINTN  BootHartId,
+  IN  UINTN  FuncArg1
+  )
+{
+  EFI_SEC_PEI_HAND_OFF        SecCoreData;
+  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);
+
+  SecCoreData.DataSize               = sizeof(EFI_SEC_PEI_HAND_OFF);
+  SecCoreData.BootFirmwareVolumeBase = BootFv;
+  SecCoreData.BootFirmwareVolumeSize = (UINTN) BootFv->FvLength;
+  SecCoreData.TemporaryRamBase       = (VOID*)(UINT64) FixedPcdGet32(PcdTemporaryRamBase);
+  SecCoreData.TemporaryRamSize       = (UINTN)  FixedPcdGet32(PcdTemporaryRamSize);
+  SecCoreData.PeiTemporaryRamBase    = SecCoreData.TemporaryRamBase;
+  SecCoreData.PeiTemporaryRamSize    = SecCoreData.TemporaryRamSize >> 1;
+  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 ++) {
+    SbiGetMscratchHartid (HartId, &ScratchSpace);
+    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
+  // temporary RAM migration.
+  //
+  SbiGetMscratchHartid (BootHartId, &ScratchSpace);
+  ZeroMem ((VOID *)&FirmwareContext, sizeof (EFI_RISCV_OPENSBI_FIRMWARE_CONTEXT));
+  ThisSbiPlatform = (struct sbi_platform *)sbi_platform_ptr(ScratchSpace);
+  if (ThisSbiPlatform->opensbi_version > OPENSBI_VERSION) {
+      DEBUG ((DEBUG_ERROR, "%a: OpenSBI platform table version 0x%x is newer than OpenSBI version 0x%x.\n"
+                           "There maybe be some backward compatable issues.\n",
+              __FUNCTION__,
+             ThisSbiPlatform->opensbi_version,
+             OPENSBI_VERSION
+             ));
+      ASSERT(FALSE);
+  }
+  DEBUG ((DEBUG_INFO, "%a: OpenSBI platform table at address: 0x%x\nFirmware Context is located at 0x%x\n",
+             __FUNCTION__,
+             ThisSbiPlatform,
+             &FirmwareContext
+             ));
+  ThisSbiPlatform->firmware_context = (unsigned long)&FirmwareContext;
+  //
+  // Set firmware context Hart-specific pointer
+  //
+  for (HartId = 0; HartId < SBI_HARTMASK_MAX_BITS; HartId ++) {
+    SbiGetMscratchHartid (HartId, &ScratchSpace);
+    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]
+                 ));
+    }
+  }
+  //
+  // Set supervisor translation mode to Bare mode
+  //
+  DEBUG ((DEBUG_INFO, "%a: Set Supervisor address mode to Bare-mode.\n", __FUNCTION__));
+  RiscVSetSupervisorAddressTranslationRegister ((UINT64)RISCV_SATP_MODE_OFF << RISCV_SATP_MODE_BIT_POSITION);
+
+  //
+  // Transfer the control to the PEI core
+  //
+  (*PeiCoreEntryPoint) (&SecCoreData, (EFI_PEI_PPI_DESCRIPTOR *)&mPrivateDispatchTable);
+}
+
+/**
+  Register firmware SBI extension and launch PeiCore to the mode specified in
+  PcdPeiCorePrivilegeMode;
+
+  To register the SBI extension we stay in M-Mode and then transition here,
+  rather than before in sbi_init.
+
+  @param[in]  ThisHartId     Hardware thread ID.
+  @param[in]  FuncArg1       Arg1 delivered from previous phase.
+
+**/
+VOID
+EFIAPI
+LaunchPeiCore (
+  IN  UINTN  ThisHartId,
+  IN  UINTN  FuncArg1
+  )
+{
+  UINT32 PeiCoreMode;
+
+  DEBUG ((DEBUG_INFO, "%a: Set boot hart done.\n", __FUNCTION__));
+  atomic_write (&BootHartDone, (UINT64)TRUE);
+  RegisterFirmwareSbiExtension ();
+
+  PeiCoreMode = FixedPcdGet32 (PcdPeiCorePrivilegeMode);
+  if (PeiCoreMode == PRV_S) {
+    DEBUG ((DEBUG_INFO, "%a: Switch to S-Mode for PeiCore.\n", __FUNCTION__));
+    sbi_hart_switch_mode (ThisHartId, FuncArg1, (UINTN)PeiCore, PRV_S, FALSE);
+  } else if (PeiCoreMode == PRV_M) {
+    DEBUG ((DEBUG_INFO, "%a: Switch to M-Mode for PeiCore.\n", __FUNCTION__));
+    PeiCore (ThisHartId, FuncArg1);
+  } else {
+    DEBUG ((DEBUG_INFO, "%a: The privilege mode specified in PcdPeiCorePrivilegeMode is not supported.\n", __FUNCTION__));
+    while (TRUE);
+  }
+}
+
+/**
+  Interface to invoke internal mode switch function.
+
+  To register the SBI extension we stay in M-Mode and then transition here,
+  rather than before in sbi_init.
+
+  @param[in]  FuncArg0       Arg0 to pass to next phase entry point address.
+  @param[in]  FuncArg1       Arg1 to pass to next phase entry point address.
+  @param[in]  NextAddr       Entry point of next phase.
+  @param[in]  NextMode       Privilege mode of next phase.
+  @param[in]  NextVirt       Next phase is in virtualiztion.
+
+**/
+VOID
+EFIAPI
+RiscVOpenSbiHartSwitchMode (
+  IN  UINTN   FuncArg0,
+  IN  UINTN   FuncArg1,
+  IN  UINTN   NextAddr,
+  IN  UINTN   NextMode,
+  IN  BOOLEAN NextVirt
+  )
+{
+  sbi_hart_switch_mode(FuncArg0, FuncArg1, NextAddr, NextMode, NextVirt);
+}
+
+/**
+  This function initilizes hart specific information and SBI.
+  For the boot hart, it boots system through PEI core and initial SBI in the DXE IPL.
+  For others, it goes to initial SBI and halt.
+
+  the lay out of memory region for each hart is as below delineates,
+
+                                               _                                        ____
+  |----Scratch ends                             |                                           |
+  |                                             | sizeof (sbi_scratch)                      |
+  |                                            _|                                           |
+  |----Scratch buffer starts                   <----- *Scratch                              |
+  |----Firmware Context Hart-specific ends     _                                            |
+  |                                             |                                           |
+  |                                             | FIRMWARE_CONTEXT_HART_SPECIFIC_SIZE       |
+  |                                             |                                           |  PcdOpenSbiStackSize
+  |                                            _|                                           |
+  |----Firmware Context Hart-specific starts   <----- **HartFirmwareContext                 |
+  |----Hart stack top                          _                                            |
+  |                                             |                                           |
+  |                                             |                                           |
+  |                                             |  Stack                                    |
+  |                                             |                                           |
+  |                                            _|                                       ____|
+  |----Hart stack bottom
+
+  @param[in]  HartId          Hardware thread ID.
+  @param[in]  Scratch         Pointer to sbi_scratch structure.
+
+**/
+VOID EFIAPI SecCoreStartUpWithStack(
+  IN  UINTN HartId,
+  IN  struct sbi_scratch *Scratch
+  )
+{
+  UINT64 BootHartDoneSbiInit;
+  UINT64 NonBootHartMessageLockValue;
+  EFI_RISCV_FIRMWARE_CONTEXT_HART_SPECIFIC *HartFirmwareContext;
+
+  //
+  // Setup EFI_RISCV_FIRMWARE_CONTEXT_HART_SPECIFIC for each hart.
+  //
+  HartFirmwareContext = (EFI_RISCV_FIRMWARE_CONTEXT_HART_SPECIFIC *)((UINT8 *)Scratch - FIRMWARE_CONTEXT_HART_SPECIFIC_SIZE);
+  HartFirmwareContext->IsaExtensionSupported = RiscVReadMachineIsa ();
+  HartFirmwareContext->MachineVendorId.Value64_L = RiscVReadMachineVendorId ();
+  HartFirmwareContext->MachineVendorId.Value64_H = 0;
+  HartFirmwareContext->MachineArchId.Value64_L = RiscVReadMachineArchitectureId ();
+  HartFirmwareContext->MachineArchId.Value64_H = 0;
+  HartFirmwareContext->MachineImplId.Value64_L = RiscVReadMachineImplementId ();
+  HartFirmwareContext->MachineImplId.Value64_H = 0;
+  HartFirmwareContext->HartSwitchMode = RiscVOpenSbiHartSwitchMode;
+
+  if (HartId == FixedPcdGet32(PcdBootHartId)) {
+    Scratch->next_addr = (UINTN)LaunchPeiCore;
+    Scratch->next_mode = PRV_M;
+    DEBUG ((DEBUG_INFO, "%a: Initializing OpenSBI library for booting hart %d\n", __FUNCTION__, HartId));
+    sbi_init(Scratch);
+  }
+
+  //
+  // Initialize the non boot harts
+  //
+  do {
+    BootHartDoneSbiInit = atomic_read (&BootHartDone);
+    //
+    // Below leave some memory cycles to boot hart
+    // for updating BootHartDone.
+    //
+    CpuPause ();
+    CpuPause ();
+    CpuPause ();
+  } while (BootHartDoneSbiInit != (UINT64)TRUE);
+
+  NonBootHartMessageLockValue = atomic_xchg(&NonBootHartMessageLock, TRUE);
+  while (NonBootHartMessageLockValue == TRUE) {
+    CpuPause ();
+    CpuPause ();
+    CpuPause ();
+    NonBootHartMessageLockValue = atomic_xchg(&NonBootHartMessageLock, TRUE);
+  };
+  DEBUG((DEBUG_INFO, "%a: Non boot hart %d initialization.\n", __FUNCTION__, HartId));
+  NonBootHartMessageLockValue = atomic_xchg(&NonBootHartMessageLock, FALSE);
+  //
+  // 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,
+  //
+  sbi_init(Scratch);
+}
+
diff --git a/Platform/RISC-V/PlatformPkg/Universal/Sec/Riscv64/SecEntry.S b/Platform/RISC-V/PlatformPkg/Universal/Sec/Riscv64/SecEntry.S
new file mode 100644
index 0000000000..6b2cdb6c17
--- /dev/null
+++ b/Platform/RISC-V/PlatformPkg/Universal/Sec/Riscv64/SecEntry.S
@@ -0,0 +1,532 @@
+/*
+ * Copyright (c) 2020 , Hewlett Packard Enterprise Development LP. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-2-Clause
+ *
+ * Copyright (c) 2019 Western Digital Corporation or its affiliates.
+ *
+ */
+
+#include <IndustryStandard/RiscVOpensbi.h>
+#include <Base.h>
+#include <RiscVImpl.h>
+#include <sbi/riscv_asm.h>
+#include <sbi/riscv_encoding.h>
+#include <sbi/sbi_platform.h>
+#include <sbi/sbi_scratch.h>
+#include <sbi/sbi_trap.h>
+
+#include <SecMain.h>
+
+.text
+.align 3
+
+ASM_FUNC (_ModuleEntryPoint)
+  /*
+   * Jump to warm-boot if this is not the selected core booting,
+   */
+  csrr  a6, CSR_MHARTID
+  li    a5, FixedPcdGet32 (PcdBootHartId)
+  bne   a6, a5, _wait_for_boot_hart
+
+  li    ra, 0
+  call  _reset_regs
+
+  /* Preload HART details
+   * s7 -> HART Count
+   * s8 -> HART Stack Size
+   */
+  li    s7, FixedPcdGet32 (PcdHartCount)
+  li    s8, FixedPcdGet32 (PcdOpenSbiStackSize)
+
+  /*
+   * Setup scratch space for all the HARTs
+   *
+   * Scratch buffer is on the top of stack buffer
+   * of each hart.
+   *
+   * tp : Base address of scratch buffer.
+   * s7 : HART Count
+   * s8 : HART Stack Size
+  */
+  li    tp, FixedPcdGet32 (PcdScratchRamBase)
+  mul   a5, s7, s8
+  add   tp, tp, a5
+
+  /* Keep a copy of tp */
+  add   t3, tp, zero
+  /* Counter */
+  li    t2, 1
+  /* hartid 0 is mandated by ISA */
+  li    t1, 0
+_scratch_init:
+  add   tp, t3, zero
+  mul   a5, s8, t1
+  sub   tp, tp, a5
+  li    a5, SBI_SCRATCH_SIZE
+  sub   tp, tp, a5
+
+  /* Initialize scratch space */
+
+  /* Firmware range and size */
+  li    a4, FixedPcdGet32 (PcdFwStartAddress)
+  li    a5, FixedPcdGet32 (PcdFwEndAddress)
+  sub   a5, a5, a4
+  sd    a4, SBI_SCRATCH_FW_START_OFFSET(tp)
+  sd    a5, SBI_SCRATCH_FW_SIZE_OFFSET(tp)
+
+  /*
+   * Note: fw_next_arg1() uses a0, a1, and ra
+  */
+  call  fw_next_arg1
+  sd    a0, SBI_SCRATCH_NEXT_ARG1_OFFSET(tp) /* Save agr1 in scratch buffer*/
+  /*
+   Note: fw_next_addr()uses a0, a1, and ra
+  */
+  call  fw_next_addr
+  sd    a0, SBI_SCRATCH_NEXT_ADDR_OFFSET(tp) /* Save next address in scratch buffer*/
+  li    a4, PRV_S
+  sd    a4, SBI_SCRATCH_NEXT_MODE_OFFSET(tp) /* Save next mode in scratch buffer*/
+  la    a4, _start_warm
+  sd    a4, SBI_SCRATCH_WARMBOOT_ADDR_OFFSET(tp) /* Save warm boot address in scratch buffer*/
+  la    a4, platform
+  sd    a4, SBI_SCRATCH_PLATFORM_ADDR_OFFSET(tp) /* Save platfrom table in scratch buffer*/
+  la    a4, _hartid_to_scratch
+  sd    a4, SBI_SCRATCH_HARTID_TO_SCRATCH_OFFSET(tp) /* Save _hartid_to_scratch function in scratch buffer*/
+  sd    zero, SBI_SCRATCH_TMP0_OFFSET(tp)
+
+#ifdef FW_OPTIONS
+  li    a4, FW_OPTIONS
+  sd    a4, SBI_SCRATCH_OPTIONS_OFFSET(tp)
+#else
+  sd    zero, SBI_SCRATCH_OPTIONS_OFFSET(tp)
+#endif
+  add   t1, t1, t2
+  /* Loop to next hart */
+  blt   t1, s7, _scratch_init
+
+  /* Fill-out temporary memory with 55aa*/
+  li    a4, FixedPcdGet32 (PcdTemporaryRamBase)
+  li    a5, FixedPcdGet32 (PcdTemporaryRamSize)
+  add   a5, a4, a5
+1:
+  li    a3, 0x5AA55AA55AA55AA5
+  sd    a3, (a4)
+  add   a4, a4, __SIZEOF_POINTER__
+  blt   a4, a5, 1b
+
+  /* Update boot hart flag */
+  la    a4, _boot_hart_done
+  li    a5, 1
+  sd    a5, (a4)
+
+  /* Wait for boot hart */
+_wait_for_boot_hart:
+  la    a4, _boot_hart_done
+  ld    a5, (a4)
+
+  /* Reduce the bus traffic so that boot hart may proceed faster */
+  nop
+  nop
+  nop
+  beqz  a5, _wait_for_boot_hart
+
+_start_warm:
+  li    ra, 0
+  call  _reset_regs
+
+  /* Disable and clear all interrupts */
+  csrw  CSR_MIE, zero
+  csrw  CSR_MIP, zero
+
+  li    s7, FixedPcdGet32 (PcdBootableHartNumber)
+  li    s8, FixedPcdGet32 (PcdOpenSbiStackSize)
+  la    a4, platform
+
+  /* s9 is hart_index2id array */
+  REG_L s9, SBI_PLATFORM_HART_INDEX2ID_OFFSET(a4)
+
+  /* s6 is this hart ID */
+  csrr  s6, CSR_MHARTID
+
+  /*
+   * Convert hart ID to scratch buffer
+   * Because the hert ID maybe not in sequentially
+   */
+  beqz  s9, 3f
+  li    a4, 0
+1:
+#if __riscv_xlen == 64
+  lwu   a5, (s9)
+#else
+  lw    a5, (s9)
+#endif
+  /* Branch if hart ID is matched */
+  beq   a5, s6, 2f
+  add   s9, s9, 4
+  add   a4, a4, 1
+  /* Loop to next index */
+  blt   a4, s7, 1b
+  li    a4, -1
+2:
+  add   s6, a4, zero
+3:
+  /* Jump to UninitializedHartWait for the non-bootable harts.
+     Be aware that the stack and scratch is not set
+     at this moment for this hart even the resource
+     is preserved eariler for it.
+  */
+  bltu  s6, s7, 4f
+  csrr  a0, CSR_MHARTID
+  j _uninitialized_hart_wait
+4:
+  li    s7, FixedPcdGet32 (PcdHartCount)
+  /* Find the scratch space for this hart
+   *
+   * Scratch buffer is on the top of stack buffer
+   * reserved for opensbi.
+   *
+   * tp: The base address of scratch buffer
+   * s6: Index to scratch buffer fot this hart
+   * s7: Total hart number
+   * s8: Stack size for opebsbi.
+  */
+  li    tp, FixedPcdGet32 (PcdScratchRamBase)
+  mul   a5, s7, s8
+  add   tp, tp, a5
+  mul   a5, s8, s6
+  sub   tp, tp, a5
+  li    a5, SBI_SCRATCH_SIZE
+  sub   tp, tp, a5
+
+  /* update the mscratch */
+  csrw  CSR_MSCRATCH, tp
+
+  /*make room for Hart specific Firmware Context*/
+  li    a5, FIRMWARE_CONTEXT_HART_SPECIFIC_SIZE
+  sub   tp, tp, a5
+
+  /* Setup stack */
+  add   sp, tp, zero
+
+  /* Setup stack for the Hart executing EFI to top of temporary ram*/
+  csrr  a6, CSR_MHARTID
+  li    a5, FixedPcdGet32 (PcdBootHartId)
+  bne   a6, a5, 1f
+
+  li    a4, FixedPcdGet32(PcdTemporaryRamBase)
+  li    a5, FixedPcdGet32(PcdTemporaryRamSize)
+  add   sp, a4, a5
+1:
+
+  /* Setup trap handler */
+  la    a4, _trap_handler
+  csrw  CSR_MTVEC, a4
+
+  /* Make sure that mtvec is updated */
+  1:
+  csrr  a5, CSR_MTVEC
+  bne   a4, a5, 1b
+
+  /* Call library constructors before jup to SEC core */
+  call  ProcessLibraryConstructorList
+
+  /* Jump to SEC Core C
+   * a0: HART ID
+   * a1: Scratch pointer
+  */
+  csrr  a0, CSR_MHARTID
+  csrr  a1, CSR_MSCRATCH
+  call  SecCoreStartUpWithStack
+
+  /* We do not expect to reach here hence just hang */
+  j     _start_hang
+
+  .align 3
+  .section .data, "aw"
+_boot_hart_done:
+  RISCV_PTR 0
+
+  .align 3
+  .section .entry, "ax", %progbits
+  .globl _hartid_to_scratch
+
+  /*
+   * a0 -> HART id (Obsoleted, keep this for the backward compatible)
+   * a1 -> HART index (0-based) of this hart id
+   */
+_hartid_to_scratch:
+  add   sp, sp, -(3 * __SIZEOF_POINTER__)
+  sd    s0, (sp)
+  sd    s1, (__SIZEOF_POINTER__)(sp)
+  sd    s2, (__SIZEOF_POINTER__ * 2)(sp)
+
+  /*
+   * s0 -> HART Stack Size
+   * s1 -> HART Stack End
+   * s2 -> Temporary
+   */
+  la    s2, platform
+#if __riscv_xlen == 64
+  lwu   s0, SBI_PLATFORM_HART_STACK_SIZE_OFFSET(s2)
+#else
+  lw    s0, SBI_PLATFORM_HART_STACK_SIZE_OFFSET(s2)
+#endif
+  li    s2, FixedPcdGet32 (PcdHartCount)
+
+  mul   s2, s2, s0
+  li    s1, FixedPcdGet32 (PcdScratchRamBase)
+  add   s1, s1, s2
+  mul   s2, s0, a1
+  sub   s1, s1, s2
+  li    s2, SBI_SCRATCH_SIZE
+  sub   a0, s1, s2
+  ld    s0, (sp)
+  ld    s1, (__SIZEOF_POINTER__)(sp)
+  ld    s2, (__SIZEOF_POINTER__ * 2)(sp)
+  add   sp, sp, (3 * __SIZEOF_POINTER__)
+  ret
+
+  .align 3
+  .section .entry, "ax", %progbits
+  .globl _start_hang
+_start_hang:
+  wfi
+  j     _start_hang
+
+  /*
+   * Uninitialized hart comes here and wait
+   * for the further process.
+   * a0: hart ID.
+  */
+_uninitialized_hart_wait:
+  wfi
+  j     _uninitialized_hart_wait
+
+  .align 3
+  .section .entry, "ax", %progbits
+  .align 3
+  .globl _trap_handler
+_trap_handler:
+
+  /* Swap TP and MSCRATCH */
+  csrrw tp, CSR_MSCRATCH, tp
+
+  /* Save T0 in scratch space */
+  REG_S t0, SBI_SCRATCH_TMP0_OFFSET(tp)
+
+  /* Check which mode we came from */
+  csrr  t0, CSR_MSTATUS
+  srl   t0, t0, MSTATUS_MPP_SHIFT
+  and   t0, t0, PRV_M
+  xori  t0, t0, PRV_M
+  beq   t0, zero, _trap_handler_m_mode
+
+  /* We came from S-mode or U-mode */
+_trap_handler_s_mode:
+  /* Set T0 to original SP */
+  add   t0, sp, zero
+
+  /* Setup exception stack */
+  add   sp, tp, -(SBI_TRAP_REGS_SIZE)
+
+  /* Jump to code common for all modes */
+  j     _trap_handler_all_mode
+
+  /* We came from M-mode */
+_trap_handler_m_mode:
+  /* Set T0 to original SP */
+  add   t0, sp, zero
+
+  /* Re-use current SP as exception stack */
+  add   sp, sp, -(SBI_TRAP_REGS_SIZE)
+
+_trap_handler_all_mode:
+  /* Save original SP (from T0) on stack */
+  REG_S t0, SBI_TRAP_REGS_OFFSET(sp)(sp)
+
+  /* Restore T0 from scratch space */
+  REG_L t0, SBI_SCRATCH_TMP0_OFFSET(tp)
+
+  /* Save T0 on stack */
+  REG_S t0, SBI_TRAP_REGS_OFFSET(t0)(sp)
+
+  /* Swap TP and MSCRATCH */
+  csrrw tp, CSR_MSCRATCH, tp
+
+  /* Save MEPC and MSTATUS CSRs */
+  csrr  t0, CSR_MEPC
+  REG_S t0, SBI_TRAP_REGS_OFFSET(mepc)(sp)
+  csrr  t0, CSR_MSTATUS
+  REG_S t0, SBI_TRAP_REGS_OFFSET(mstatus)(sp)
+  REG_S zero, SBI_TRAP_REGS_OFFSET(mstatusH)(sp)
+#if __riscv_xlen == 32
+        csrr    t0, CSR_MISA
+        srli    t0, t0, ('H' - 'A')
+        andi    t0, t0, 0x1
+        beq     t0, zero, _skip_mstatush_save
+        csrr    t0, CSR_MSTATUSH
+        REG_S   t0, SBI_TRAP_REGS_OFFSET(mstatusH)(sp)
+_skip_mstatush_save:
+#endif
+
+  /* Save all general registers except SP and T0 */
+  REG_S zero, SBI_TRAP_REGS_OFFSET(zero)(sp)
+  REG_S ra, SBI_TRAP_REGS_OFFSET(ra)(sp)
+  REG_S gp, SBI_TRAP_REGS_OFFSET(gp)(sp)
+  REG_S tp, SBI_TRAP_REGS_OFFSET(tp)(sp)
+  REG_S t1, SBI_TRAP_REGS_OFFSET(t1)(sp)
+  REG_S t2, SBI_TRAP_REGS_OFFSET(t2)(sp)
+  REG_S s0, SBI_TRAP_REGS_OFFSET(s0)(sp)
+  REG_S s1, SBI_TRAP_REGS_OFFSET(s1)(sp)
+  REG_S a0, SBI_TRAP_REGS_OFFSET(a0)(sp)
+  REG_S a1, SBI_TRAP_REGS_OFFSET(a1)(sp)
+  REG_S a2, SBI_TRAP_REGS_OFFSET(a2)(sp)
+  REG_S a3, SBI_TRAP_REGS_OFFSET(a3)(sp)
+  REG_S a4, SBI_TRAP_REGS_OFFSET(a4)(sp)
+  REG_S a5, SBI_TRAP_REGS_OFFSET(a5)(sp)
+  REG_S a6, SBI_TRAP_REGS_OFFSET(a6)(sp)
+  REG_S a7, SBI_TRAP_REGS_OFFSET(a7)(sp)
+  REG_S s2, SBI_TRAP_REGS_OFFSET(s2)(sp)
+  REG_S s3, SBI_TRAP_REGS_OFFSET(s3)(sp)
+  REG_S s4, SBI_TRAP_REGS_OFFSET(s4)(sp)
+  REG_S s5, SBI_TRAP_REGS_OFFSET(s5)(sp)
+  REG_S s6, SBI_TRAP_REGS_OFFSET(s6)(sp)
+  REG_S s7, SBI_TRAP_REGS_OFFSET(s7)(sp)
+  REG_S s8, SBI_TRAP_REGS_OFFSET(s8)(sp)
+  REG_S s9, SBI_TRAP_REGS_OFFSET(s9)(sp)
+  REG_S s10, SBI_TRAP_REGS_OFFSET(s10)(sp)
+  REG_S s11, SBI_TRAP_REGS_OFFSET(s11)(sp)
+  REG_S t3, SBI_TRAP_REGS_OFFSET(t3)(sp)
+  REG_S t4, SBI_TRAP_REGS_OFFSET(t4)(sp)
+  REG_S t5, SBI_TRAP_REGS_OFFSET(t5)(sp)
+  REG_S t6, SBI_TRAP_REGS_OFFSET(t6)(sp)
+
+  /* Call C routine */
+  add   a0, sp, zero
+  call  sbi_trap_handler
+
+  /* Restore all general registers except SP and T0 */
+  REG_L ra, SBI_TRAP_REGS_OFFSET(ra)(sp)
+  REG_L gp, SBI_TRAP_REGS_OFFSET(gp)(sp)
+  REG_L tp, SBI_TRAP_REGS_OFFSET(tp)(sp)
+  REG_L t1, SBI_TRAP_REGS_OFFSET(t1)(sp)
+  REG_L t2, SBI_TRAP_REGS_OFFSET(t2)(sp)
+  REG_L s0, SBI_TRAP_REGS_OFFSET(s0)(sp)
+  REG_L s1, SBI_TRAP_REGS_OFFSET(s1)(sp)
+  REG_L a0, SBI_TRAP_REGS_OFFSET(a0)(sp)
+  REG_L a1, SBI_TRAP_REGS_OFFSET(a1)(sp)
+  REG_L a2, SBI_TRAP_REGS_OFFSET(a2)(sp)
+  REG_L a3, SBI_TRAP_REGS_OFFSET(a3)(sp)
+  REG_L a4, SBI_TRAP_REGS_OFFSET(a4)(sp)
+  REG_L a5, SBI_TRAP_REGS_OFFSET(a5)(sp)
+  REG_L a6, SBI_TRAP_REGS_OFFSET(a6)(sp)
+  REG_L a7, SBI_TRAP_REGS_OFFSET(a7)(sp)
+  REG_L s2, SBI_TRAP_REGS_OFFSET(s2)(sp)
+  REG_L s3, SBI_TRAP_REGS_OFFSET(s3)(sp)
+  REG_L s4, SBI_TRAP_REGS_OFFSET(s4)(sp)
+  REG_L s5, SBI_TRAP_REGS_OFFSET(s5)(sp)
+  REG_L s6, SBI_TRAP_REGS_OFFSET(s6)(sp)
+  REG_L s7, SBI_TRAP_REGS_OFFSET(s7)(sp)
+  REG_L s8, SBI_TRAP_REGS_OFFSET(s8)(sp)
+  REG_L s9, SBI_TRAP_REGS_OFFSET(s9)(sp)
+  REG_L s10, SBI_TRAP_REGS_OFFSET(s10)(sp)
+  REG_L s11, SBI_TRAP_REGS_OFFSET(s11)(sp)
+  REG_L t3, SBI_TRAP_REGS_OFFSET(t3)(sp)
+  REG_L t4, SBI_TRAP_REGS_OFFSET(t4)(sp)
+  REG_L t5, SBI_TRAP_REGS_OFFSET(t5)(sp)
+  REG_L t6, SBI_TRAP_REGS_OFFSET(t6)(sp)
+
+  /* Restore MEPC and MSTATUS CSRs */
+  REG_L t0, SBI_TRAP_REGS_OFFSET(mepc)(sp)
+  csrw  CSR_MEPC, t0
+  REG_L t0, SBI_TRAP_REGS_OFFSET(mstatus)(sp)
+  csrw  CSR_MSTATUS, t0
+#if __riscv_xlen == 32
+  csrr  t0, CSR_MISA
+  srli  t0, t0, ('H' - 'A')
+  andi  t0, t0, 0x1
+  beq   t0, zero, _skip_mstatush_restore
+  REG_L t0, SBI_TRAP_REGS_OFFSET(mstatusH)(sp)
+  csrw  CSR_MSTATUSH, t0
+_skip_mstatush_restore:
+#endif
+
+  /* Restore T0 */
+  REG_L   t0, SBI_TRAP_REGS_OFFSET(t0)(sp)
+
+  /* Restore SP */
+  REG_L   sp, SBI_TRAP_REGS_OFFSET(sp)(sp)
+
+  mret
+
+  .align 3
+  .section .entry, "ax", %progbits
+  .globl _reset_regs
+_reset_regs:
+
+  /* flush the instruction cache */
+  fence.i
+
+  /* Reset all registers except ra, a0,a1 */
+  li    sp, 0
+  li    gp, 0
+  li    tp, 0
+  li    t0, 0
+  li    t1, 0
+  li    t2, 0
+  li    s0, 0
+  li    s1, 0
+  li    a2, 0
+  li    a3, 0
+  li    a4, 0
+  li    a5, 0
+  li    a6, 0
+  li    a7, 0
+  li    s2, 0
+  li    s3, 0
+  li    s4, 0
+  li    s5, 0
+  li    s6, 0
+  li    s7, 0
+  li    s8, 0
+  li    s9, 0
+  li    s10, 0
+  li    s11, 0
+  li    t3, 0
+  li    t4, 0
+  li    t5, 0
+  li    t6, 0
+  csrw  CSR_MSCRATCH, 0
+  ret
+
+  .align 3
+  .section .entry, "ax", %progbits
+  .global fw_prev_arg1
+fw_prev_arg1:
+
+  /* We return previous arg1 in 'a0' */
+  add   a0, zero, zero
+  ret
+
+  .align 3
+  .section .entry, "ax", %progbits
+  .global fw_next_arg1
+fw_next_arg1:
+  /* We return next arg1 in 'a0' */
+  li    a0, FixedPcdGet32(PcdRiscVPeiFvBase)
+  ret
+
+  .align 3
+  .section .entry, "ax", %progbits
+  .global fw_next_addr
+fw_next_addr:
+  /* We return next address in 'a0' */
+   la   a0, _jump_addr
+   ld   a0, (a0)
+   ret
+
+  .align 3
+  .section .entry, "ax", %progbits
+_jump_addr:
+RISCV_PTR SecCoreStartUpWithStack
-- 
2.31.1