PI: Ability to startup using PSCI (default), MpPark EL1 or MpPark EL2 based on PreLoader arguments

Driver/PreLoaderDxe/PreLoader.c

@@ -115,8 +115,8 @@ EFIAPI
 PreLoaderDxeInitialize(
     IN EFI_HANDLE ImageHandle, IN EFI_SYSTEM_TABLE *SystemTable)
 {
-  PPRELOADER_ENVIRONMENT Env    = (VOID *)PRELOADER_ENV_ADDR;
-  EFI_STATUS             Status = EFI_SUCCESS;
+  PPRELOADER_ENVIRONMENT_VERSION_2 Env    = (VOID *)PRELOADER_ENV_ADDR;
+  EFI_STATUS                       Status = EFI_SUCCESS;
 
   // Protocols
   Status = gBS->LocateProtocol(

Include/Configuration/Hob.h

@@ -2,17 +2,37 @@
 #define __LOCAL_HOB_H__
 
 #define PRELOADER_ENV_ADDR 0xb0000000
-#define PRELOADER_VERSION_MIN 0x1000
+#define PRELOADER_VERSION_MIN 0x2000
 
 #define PRELOADER_HEADER SIGNATURE_32('B', 'S', 'E', 'N')
 
-typedef struct _PRELOADER_ENVIRONMENT {
+typedef enum _PRELOADER_ENVIRONMENT_BOOT_MODE {
+	BOOT_MODE_PSCI = 0,
+	BOOT_MODE_MPPARK,
+	BOOT_MODE_MPPARK_EL2,
+	BOOT_MODE_MAX
+} PRELOADER_ENVIRONMENT_BOOT_MODE;
+
+typedef struct _PRELOADER_ENVIRONMENT_VERSION_1 {
+  UINT32   Header;
+  UINT32   PreloaderVersion;
+  CHAR8    PreloaderRelease[64];
+  EFI_TIME BootTimeEpoch;
+  UINT32   UefiDisplayInfo[30];
+  UINT32   Crc32;
+} PRELOADER_ENVIRONMENT_VERSION_1, * PPRELOADER_ENVIRONMENT_VERSION_1;
+
+typedef struct _PRELOADER_ENVIRONMENT_VERSION_2 {
   UINT32   Header;
   UINT32   PreloaderVersion;
   CHAR8    PreloaderRelease[64];
   EFI_TIME BootTimeEpoch;
   UINT32   UefiDisplayInfo[30];
   UINT32   Crc32;
-} PRELOADER_ENVIRONMENT, *PPRELOADER_ENVIRONMENT;
+  UINT32   BootMode;
+  UINT32   EnablePlatformSdCardBoot;
+  UINT32   UseQuadCoreConfiguration;
+  UINT32   Crc32v2;
+} PRELOADER_ENVIRONMENT_VERSION_2, *PPRELOADER_ENVIRONMENT_VERSION_2;
 
 #endif

PrePi/El2Redirection.S

@@ -0,0 +1,55 @@
+.set MPIDR_U_BIT,    (30)
+.set MPIDR_U_MASK,   (1 << MPIDR_U_BIT)
+
+// CurrentEL : 0xC = EL3; 8 = EL2; 4 = EL1
+// This only selects between EL1 and EL2 and EL3, else we die.
+// Provide the Macro with a safe temp xreg to use.
+#define EL1_OR_EL2_OR_EL3(SAFE_XREG) \
+        mrs    SAFE_XREG, CurrentEL ;\
+        cmp    SAFE_XREG, #0x8      ;\
+        b.gt   3f                   ;\
+        b.eq   2f                   ;\
+        cbnz   SAFE_XREG, 1f        ;\
+        b      .                    ;// We should never get here
+
+// DAIF bit definitions for writing through msr daifclr/sr daifset
+.set DAIF_WR_FIQ_BIT,   (1 << 0)
+.set DAIF_WR_IRQ_BIT,   (1 << 1)
+.set DAIF_WR_ABORT_BIT, (1 << 2)
+.set DAIF_WR_DEBUG_BIT, (1 << 3)
+.set DAIF_WR_INT_BITS,  (DAIF_WR_FIQ_BIT | DAIF_WR_IRQ_BIT)
+.set DAIF_WR_ALL,       (DAIF_WR_DEBUG_BIT | DAIF_WR_ABORT_BIT | DAIF_WR_INT_BITS)
+
+.set CTRL_M_BIT,      (1 << 0)
+.set CTRL_A_BIT,      (1 << 1)
+.set CTRL_C_BIT,      (1 << 2)
+.set CTRL_SA_BIT,     (1 << 3)
+.set CTRL_I_BIT,      (1 << 12)
+.set CTRL_V_BIT,      (1 << 12)
+.set CPACR_VFP_BITS,  (3 << 20)
+
+// ArmDisableInterrupts
+msr   daifset, #DAIF_WR_INT_BITS
+isb
+
+// ArmDisableCachesAndMmu
+   EL1_OR_EL2_OR_EL3(x1)
+1: mrs     x0, sctlr_el1        // Get control register EL1
+   b       4f
+2: mrs     x0, sctlr_el2        // Get control register EL2
+   b       4f
+3: mrs     x0, sctlr_el3        // Get control register EL3
+4: mov     x1, #~(CTRL_M_BIT | CTRL_C_BIT | CTRL_I_BIT)  // Disable MMU, D & I caches
+   and     x0, x0, x1
+   EL1_OR_EL2_OR_EL3(x1)
+1: msr     sctlr_el1, x0        // Write back control register
+   b       4f
+2: msr     sctlr_el2, x0        // Write back control register
+   b       4f
+3: msr     sctlr_el3, x0        // Write back control register
+4: dsb     sy
+   isb
+
+// Load address and branch
+mov x0, #0x00200000
+br x0

PrePi/El2Redirection.h

@@ -0,0 +1,33 @@
+#ifndef __EL2_REDIR_H__
+#define __EL2_REDIR_H__
+
+#include <Library/ArmHvcLib.h>
+#include <Library/ArmSmcLib.h>
+
+#include <IndustryStandard/ArmStdSmc.h>
+
+extern void _ModuleEntryPoint();
+
+// The EL2 shellcode is pre-compiled. For detailed explanation, see
+// El2Redirection.S in the same directory
+UINT8 El2ShellCode[120] = {
+    0xDF, 0x43, 0x03, 0xD5, 0xDF, 0x3F, 0x03, 0xD5, 0x41, 0x42, 0x38, 0xD5,
+    0x3F, 0x20, 0x00, 0xF1, 0x0C, 0x01, 0x00, 0x54, 0xA0, 0x00, 0x00, 0x54,
+    0x41, 0x00, 0x00, 0xB5, 0x00, 0x00, 0x00, 0x14, 0x00, 0x10, 0x38, 0xD5,
+    0x04, 0x00, 0x00, 0x14, 0x00, 0x10, 0x3C, 0xD5, 0x02, 0x00, 0x00, 0x14,
+    0x00, 0x10, 0x3E, 0xD5, 0xA1, 0x00, 0x82, 0x92, 0x00, 0x00, 0x01, 0x8A,
+    0x41, 0x42, 0x38, 0xD5, 0x3F, 0x20, 0x00, 0xF1, 0x0C, 0x01, 0x00, 0x54,
+    0xA0, 0x00, 0x00, 0x54, 0x41, 0x00, 0x00, 0xB5, 0x00, 0x00, 0x00, 0x14,
+    0x00, 0x10, 0x18, 0xD5, 0x04, 0x00, 0x00, 0x14, 0x00, 0x10, 0x1C, 0xD5,
+    0x02, 0x00, 0x00, 0x14, 0x00, 0x10, 0x1E, 0xD5, 0x9F, 0x3F, 0x03, 0xD5,
+    0xDF, 0x3F, 0x03, 0xD5, 0x00, 0x04, 0xA0, 0xD2, 0x00, 0x00, 0x1F, 0xD6,
+};
+
+VOID WaitForSecondaryCPUs(VOID);
+
+#define EL2REDIR_MAILBOX_READY 0xfffffff1
+#define EL2REDIR_MAILBOX_SIGNAL 0xfffffff2
+#define EL2REDIR_MAILBOX_ACKNOWLEDGE 0xfffffff3
+#define EL2REDIR_MAILBOX_STAY_EL1 0xfffffff4
+
+#endif

PrePi/ModuleEntryPoint.S

@@ -9,12 +9,14 @@
 .align 3
 
 GCC_ASM_IMPORT (CEntryPoint)
+GCC_ASM_IMPORT (SecondaryCEntryPoint)
 GCC_ASM_IMPORT (ArmWriteCpacr)
 GCC_ASM_IMPORT (ArmEnableInstructionCache)
 GCC_ASM_IMPORT (ArmEnableDataCache)
 GCC_ASM_IMPORT (ArmInvalidateTlb)
 
 GCC_ASM_EXPORT (_ModuleEntryPoint)
+GCC_ASM_EXPORT (SecondaryCpuEntry)
 
 .global _StackBase 
 .global _StackSize
@@ -55,9 +57,9 @@ _CpuIndex:
   /* Keep the CPU index in x19 */
   mov x19, x0
 
-  /* Secondary startup is not expected */
+  /* Secondary startup case */
   cmp x19, #0x0
-  bne dead
+  bne SecondaryCpuEntry
 
   /* First ensure all interrupts are disabled */
   bl ASM_PFX(ArmDisableInterrupts)
@@ -93,6 +95,10 @@ _FillVectors:
   cmp x0, #0x5
   b.eq _SetupEl1VBar
 
+_SetupEL2VBar:
+  msr vbar_el2, x0
+  b _SetupPrimaryCoreStack
+
 _SetupEl1VBar:
   msr vbar_el1, x0        
 
@@ -121,6 +127,81 @@ _PrepareArguments:
 
   bl CEntryPoint
 
+SecondaryCpuEntry:
+  /* Keep the incoming index */
+  mov x19, x0
+
+  /* First ensure all interrupts are disabled */
+  bl ASM_PFX(ArmDisableInterrupts)
+
+  /* Ensure that the MMU and caches are off */
+  bl ASM_PFX(ArmDisableCachesAndMmu)
+
+  /* Invalide I-Cache */
+  bl ASM_PFX(ArmInvalidateInstructionCache)
+
+  /* Invalidate TLB */
+  bl ASM_PFX(ArmInvalidateTlb)
+
+  /* Get current EL in x0 */
+  mrs x0, CurrentEl
+
+  /* Check if we are in EL1 */
+  cmp x0, #0x4
+  b.eq _SecondaryCpuEl1Setup
+
+  cmp x0, #0x5
+  b.eq _SecondaryCpuEl1Setup
+
+  /* Otherwise initialize EL2 */
+  /* Get the vector table base for this core */
+  mov x1, #0x800
+  mul x1, x1, x19
+  LoadConstantToReg (FixedPcdGet32(PcdCpuVectorBaseAddress), x0)
+  add x0, x0, x1
+  msr vbar_el2, x0
+
+  b _SecondaryStackSetup
+
+_SecondaryCpuEl1Setup:
+  /* Get the vector table base for this core */
+  mov x1, #0x800
+  mul x1, x1, x19
+  LoadConstantToReg (FixedPcdGet32(PcdCpuVectorBaseAddress), x0)
+  add x0, x0, x1
+  msr vbar_el1, x0
+
+  /* SIMD */
+  mrs x0, CPACR_EL1
+  orr x0, x0, #0x300000
+  msr CPACR_EL1, x0
+
+_SecondaryStackSetup:
+  /* Exception should be DAIF */
+  msr daifset, #0xF
+
+  /* MpPark mailbox region: each core have 0x10000 available, the first 4KB is mailbox */
+  mov x1, #0x10000
+  mul x1, x1, x19
+  ldr x0, _MpParkBase
+  add x0, x0, x1
+
+  /* Setup stack for this core */
+  mov x2, #0x2000
+  /* Stackbase */
+  add x0, x0, x2
+  /* Stacksize */
+  ldr x1, _SecondaryCpuStackSize
+
+  add x4, x0, x1
+  add sp, x4, #0
+
+  bl ArmInvalidateDataCache
+
+  /* Enter MpPark spin */
+  mov x0, x19
+  bl SecondaryCEntryPoint
+
 .align 3
 dead:  
   b dead                      /* We should never get here */