diff --git a/Driver/PreLoaderDxe/PreLoader.c b/Driver/PreLoaderDxe/PreLoader.c index 253d6a8f..1053275c 100644 --- a/Driver/PreLoaderDxe/PreLoader.c +++ b/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( diff --git a/Include/Configuration/Hob.h b/Include/Configuration/Hob.h index 0bc24d70..69b8d3a3 100644 --- a/Include/Configuration/Hob.h +++ b/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 \ No newline at end of file diff --git a/PrePi/El2Redirection.S b/PrePi/El2Redirection.S new file mode 100644 index 00000000..0d15dc44 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/PrePi/El2Redirection.h b/PrePi/El2Redirection.h new file mode 100644 index 00000000..69075968 --- /dev/null +++ b/PrePi/El2Redirection.h @@ -0,0 +1,33 @@ +#ifndef __EL2_REDIR_H__ +#define __EL2_REDIR_H__ + +#include +#include + +#include + +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 \ No newline at end of file diff --git a/PrePi/ModuleEntryPoint.S b/PrePi/ModuleEntryPoint.S index 923a615e..49805c26 100644 --- a/PrePi/ModuleEntryPoint.S +++ b/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 */ diff --git a/PrePi/Pi.c b/PrePi/Pi.c index 2bd81b44..676fbdfb 100644 --- a/PrePi/Pi.c +++ b/PrePi/Pi.c @@ -32,8 +32,15 @@ #include #include +#include "El2Redirection.h" + VOID EFIAPI ProcessLibraryConstructorList(VOID); +static UINT32 ProcessorIdMapping[8] = { + 0x00000000, 0x00000001, 0x00000002, 0x00000003, + 0x00000100, 0x00000101, 0x00000102, 0x00000103, +}; + STATIC VOID UartInit(VOID) { SerialPortInitialize(); @@ -44,6 +51,178 @@ STATIC VOID UartInit(VOID) (CHAR16 *)PcdGetPtr(PcdFirmwareVersionString), __TIME__, __DATE__)); } +VOID WaitForSecondaryCPUs(VOID) +{ + while (1) { + + BOOLEAN IsAllCpuLaunched = TRUE; + + for (UINTN Index = 1; Index < FixedPcdGet32(PcdCoreCount); Index++) { + EFI_PHYSICAL_ADDRESS MailboxAddress = + FixedPcdGet64(SecondaryCpuMpParkRegionBase) + 0x10000 * Index + + 0x1000; + PEFI_PROCESSOR_MAILBOX pMailbox = + (PEFI_PROCESSOR_MAILBOX)(VOID *)MailboxAddress; + + ArmDataSynchronizationBarrier(); + if (pMailbox->El2JumpFlag != EL2REDIR_MAILBOX_READY) { + IsAllCpuLaunched = FALSE; + break; + } + } + + // All CPU started. Continue. + if (IsAllCpuLaunched) { + break; + } + } +} + +VOID InstallEl2Patch(VOID) +{ + // The big EL2 HVC call handler is located at 0x06c01984, + // but if secondary CPUs are launched, exception vector will be + // fixed so CPU0 call would fail. Therefore we patched + // PSCI_CPU_SUSPEND_AARCH64 handler at 0x06c03aa8. + DEBUG((EFI_D_ERROR, "Injecting shellcode...\n")); + EFI_PHYSICAL_ADDRESS PsciCpuSuspendHandlerAddr = FixedPcdGet64(PsciCpuSuspendAddress); + UINT8 *PsciCpuSuspendHandler = (UINT8 *)(VOID *)PsciCpuSuspendHandlerAddr; + CopyMem(PsciCpuSuspendHandler, El2ShellCode, sizeof(El2ShellCode)); + ArmDataSynchronizationBarrier(); + ArmInvalidateDataCache(); + ArmInvalidateInstructionCache(); + DEBUG((EFI_D_ERROR, "You have been served\n")); +} + +STATIC VOID MpParkEl2Init(VOID) +{ + // Which EL? + if (ArmReadCurrentEL() == AARCH64_EL2) { + DEBUG((EFI_D_ERROR, "Running at EL2 \n")); + } + else { + DEBUG((EFI_D_ERROR, "Running at EL1 \n")); + } + + // Immediately launch all CPUs, 7 CPUs hold + DEBUG((EFI_D_LOAD | EFI_D_INFO, "Launching CPUs\n")); + + // Launch all CPUs, hold and jump to EL2 (only for EL1) + if (ArmReadCurrentEL() == AARCH64_EL1) { + if (ArmReadMpidr() == 0x80000000) { + for (UINTN i = 1; i < FixedPcdGet32(PcdCoreCount); i++) { + ARM_HVC_ARGS ArmHvcArgs; + ArmHvcArgs.Arg0 = ARM_SMC_ID_PSCI_CPU_ON_AARCH64; + ArmHvcArgs.Arg1 = ProcessorIdMapping[i]; + ArmHvcArgs.Arg2 = (UINTN)&_ModuleEntryPoint; + ArmHvcArgs.Arg3 = i; + + ArmCallHvc(&ArmHvcArgs); + ASSERT(ArmHvcArgs.Arg0 == ARM_SMC_PSCI_RET_SUCCESS); + } + } + + DEBUG((EFI_D_ERROR, "Waiting for all CPUs...\n")); + WaitForSecondaryCPUs(); + DEBUG((EFI_D_ERROR, "All CPU started.\n")); + + // Install patch + InstallEl2Patch(); + + // Looks good. Notify all secondary CPUs to jump! + for (UINTN Index = 1; Index < FixedPcdGet32(PcdCoreCount); Index++) { + EFI_PHYSICAL_ADDRESS MailboxAddress = + FixedPcdGet64(SecondaryCpuMpParkRegionBase) + 0x10000 * Index + + 0x1000; + PEFI_PROCESSOR_MAILBOX pMailbox = + (PEFI_PROCESSOR_MAILBOX)(VOID *)MailboxAddress; + + pMailbox->El2JumpFlag = EL2REDIR_MAILBOX_SIGNAL; + ArmDataSynchronizationBarrier(); + } + + // Make sure they are all initialized + DEBUG((EFI_D_ERROR, "Waiting for all CPUs...\n")); + WaitForSecondaryCPUs(); + DEBUG((EFI_D_ERROR, "All CPU started.\n")); + ArmDataSynchronizationBarrier(); + + DEBUG((EFI_D_ERROR, "Jump CPU0 to EL2.\n")); + ArmDataSynchronizationBarrier(); + + // Install patch again + InstallEl2Patch(); + + // Jump overself + ARM_HVC_ARGS StubArg; + StubArg.Arg0 = ARM_SMC_ID_PSCI_CPU_SUSPEND_AARCH64; + ArmCallHvc(&StubArg); + + // We should not reach here + ASSERT(FALSE); + } +} + +STATIC VOID MpParkNotifySecondaryCPUs(VOID) +{ + // Notify secondary CPUs that GIC is set up + for (UINTN Index = 1; Index < FixedPcdGet32(PcdCoreCount); Index++) { + EFI_PHYSICAL_ADDRESS MailboxAddress = + FixedPcdGet64(SecondaryCpuMpParkRegionBase) + 0x10000 * Index + 0x1000; + PEFI_PROCESSOR_MAILBOX pMailbox = + (PEFI_PROCESSOR_MAILBOX)(VOID *)MailboxAddress; + + pMailbox->El2JumpFlag = EL2REDIR_MAILBOX_SIGNAL; + ArmDataSynchronizationBarrier(); + } + + DEBUG((EFI_D_ERROR, "Waiting for all CPUs...\n")); + WaitForSecondaryCPUs(); + DEBUG((EFI_D_ERROR, "All CPU entered MpPark.\n")); + ArmDataSynchronizationBarrier(); +} + +STATIC VOID MpParkEl1Init(VOID) +{ + // Immediately launch all CPUs, 7 CPUs hold + DEBUG((EFI_D_LOAD | EFI_D_INFO, "Launching CPUs\n")); + + if (ArmReadMpidr() == 0x80000000) { + for (UINTN i = 1; i < FixedPcdGet32(PcdCoreCount); i++) { + ARM_HVC_ARGS ArmHvcArgs; + ArmHvcArgs.Arg0 = ARM_SMC_ID_PSCI_CPU_ON_AARCH64; + ArmHvcArgs.Arg1 = ProcessorIdMapping[i]; + ArmHvcArgs.Arg2 = (UINTN)&_ModuleEntryPoint; + ArmHvcArgs.Arg3 = i; + + ArmCallHvc(&ArmHvcArgs); + ASSERT(ArmHvcArgs.Arg0 == ARM_SMC_PSCI_RET_SUCCESS); + } + } + + DEBUG((EFI_D_ERROR, "Waiting for all CPUs...\n")); + WaitForSecondaryCPUs(); + DEBUG((EFI_D_ERROR, "All CPU started.\n")); + + // Looks good. Notify all secondary CPUs to jump! + for (UINTN Index = 1; Index < FixedPcdGet32(PcdCoreCount); Index++) { + EFI_PHYSICAL_ADDRESS MailboxAddress = + FixedPcdGet64(SecondaryCpuMpParkRegionBase) + 0x10000 * Index + + 0x1000; + PEFI_PROCESSOR_MAILBOX pMailbox = + (PEFI_PROCESSOR_MAILBOX)(VOID *)MailboxAddress; + + pMailbox->El2JumpFlag = EL2REDIR_MAILBOX_STAY_EL1; + ArmDataSynchronizationBarrier(); + } + + // Make sure they are all initialized + DEBUG((EFI_D_ERROR, "Waiting for all CPUs...\n")); + WaitForSecondaryCPUs(); + DEBUG((EFI_D_ERROR, "All CPU started.\n")); + ArmDataSynchronizationBarrier(); +} + STATIC VOID PsciFixupInit(VOID) { EFI_PHYSICAL_ADDRESS WakeFromPowerGatePatchOffset; @@ -82,18 +261,70 @@ STATIC VOID PsciFixupInit(VOID) VOID Main(IN VOID *StackBase, IN UINTN StackSize, IN UINT64 StartTimeStamp) { - EFI_HOB_HANDOFF_INFO_TABLE *HobList; - PRELOADER_ENVIRONMENT * PreEnv = (VOID *)PRELOADER_ENV_ADDR; - UINT32 Crc32 = 0; - EFI_STATUS Status; + EFI_HOB_HANDOFF_INFO_TABLE * HobList; + PRELOADER_ENVIRONMENT_VERSION_2 *PreEnv = (VOID *)PRELOADER_ENV_ADDR; + UINT32 Crc32 = 0; + EFI_STATUS Status; UINTN MemoryBase = 0; UINTN MemorySize = 0; UINTN UefiMemoryBase = 0; UINTN UefiMemorySize = 0; - // PSCI fixup init - PsciFixupInit(); + UINTN BootMode = BOOT_MODE_PSCI; + UINTN EnablePlatformSdCardBoot = 0; + UINTN UseQuadCoreConfiguration = 0; + + // Initialize (fake) UART. + UartInit(); + + // Initialize Platform PreLoader HOBs + if (PreEnv->Header == PRELOADER_HEADER) { + Crc32 = PreEnv->Crc32; + PreEnv->Crc32 = 0x0; + if (CalculateCrc32(PreEnv, sizeof(PRELOADER_ENVIRONMENT_VERSION_1)) == Crc32) { + PreEnv->Crc32 = Crc32; + DEBUG((EFI_D_INFO, "CRC32 check succeeded \n")); + } + else { + // Hey we have memory corrpution + DEBUG((EFI_D_ERROR, "CRC32 check failed \n")); + ASSERT(FALSE); + } + + if (PreEnv->PreloaderVersion >= PRELOADER_VERSION_MIN) { + Crc32 = PreEnv->Crc32v2; + PreEnv->Crc32v2 = 0x0; + if (CalculateCrc32(PreEnv, sizeof(PRELOADER_ENVIRONMENT_VERSION_2)) == Crc32) { + PreEnv->Crc32v2 = Crc32; + DEBUG((EFI_D_INFO, "CRC32v2 check succeeded \n")); + } + else { + // Hey we have memory corrpution + DEBUG((EFI_D_ERROR, "CRC32v2 check failed \n")); + ASSERT(FALSE); + } + + BootMode = PreEnv->BootMode; + EnablePlatformSdCardBoot = PreEnv->EnablePlatformSdCardBoot; + UseQuadCoreConfiguration = PreEnv->UseQuadCoreConfiguration; + } + } + + switch (BootMode) { + case BOOT_MODE_PSCI: + // PSCI fixup init + PsciFixupInit(); + break; + case BOOT_MODE_MPPARK: + // EL1 init + MpParkEl1Init(); + break; + case BOOT_MODE_MPPARK_EL2: + // EL2 init + MpParkEl2Init(); + break; + } // Architecture-specific initialization // Enable Floating Point @@ -102,9 +333,6 @@ VOID Main(IN VOID *StackBase, IN UINTN StackSize, IN UINT64 StartTimeStamp) /* Enable program flow prediction, if supported */ ArmEnableBranchPrediction(); - // Initialize (fake) UART. - UartInit(); - // Declare UEFI region MemoryBase = FixedPcdGet32(PcdSystemMemoryBase); MemorySize = FixedPcdGet32(PcdSystemMemorySize); @@ -149,6 +377,14 @@ VOID Main(IN VOID *StackBase, IN UINTN StackSize, IN UINT64 StartTimeStamp) } } + switch (BootMode) { + case BOOT_MODE_MPPARK: + case BOOT_MODE_MPPARK_EL2: + // Notify secondary CPUs that GIC is setup up + MpParkNotifySecondaryCPUs(); + break; + } + // Add HOBs BuildStackHob((UINTN)StackBase, StackSize); @@ -162,21 +398,6 @@ VOID Main(IN VOID *StackBase, IN UINTN StackSize, IN UINT64 StartTimeStamp) Status = PlatformPeim(); ASSERT_EFI_ERROR(Status); - // Initialize Platform PreLoader HOBs - if (PreEnv->Header == PRELOADER_HEADER) { - Crc32 = PreEnv->Crc32; - PreEnv->Crc32 = 0x0; - if (CalculateCrc32(PreEnv, sizeof(PRELOADER_ENVIRONMENT)) == Crc32) { - PreEnv->Crc32 = Crc32; - DEBUG((EFI_D_INFO, "CRC32 check succeeded \n")); - } - else { - // Hey we have memory corrpution - DEBUG((EFI_D_ERROR, "CRC32 check failed \n")); - ASSERT(FALSE); - } - } - // Now, the HOB List has been initialized, we can register performance // information PERF_START (NULL, "PEI", NULL, StartTimeStamp); @@ -200,3 +421,134 @@ VOID CEntryPoint(IN VOID *StackBase, IN UINTN StackSize) { Main(StackBase, StackSize, 0); } + +VOID SecondaryCEntryPoint(IN UINTN Index) +{ + ASSERT(Index >= 1 && Index <= 7); + + EFI_PHYSICAL_ADDRESS MailboxAddress = + FixedPcdGet64(SecondaryCpuMpParkRegionBase) + 0x10000 * Index + 0x1000; + PEFI_PROCESSOR_MAILBOX pMailbox = + (PEFI_PROCESSOR_MAILBOX)(VOID *)MailboxAddress; + + UINT32 CurrentProcessorId = 0; + VOID (*SecondaryStart)(VOID * pMailbox); + UINTN SecondaryEntryAddr; + UINTN InterruptId; + UINTN AcknowledgeInterrupt; + + // MMU, cache and branch predicton must be disabled + // Cache is disabled in CRT startup code + ArmDisableMmu(); + ArmDisableBranchPrediction(); + + // Clear mailbox + pMailbox->JumpAddress = 0; + pMailbox->ProcessorId = 0xffffffff; + + CurrentProcessorId = ProcessorIdMapping[Index]; + + // Notify the main processor that we are here + pMailbox->El2JumpFlag = EL2REDIR_MAILBOX_READY; + ArmDataSynchronizationBarrier(); + + // Check Exception Level and hold if we are in EL1 + if (ArmReadCurrentEL() == AARCH64_EL1) { + while (1) { + ArmDataSynchronizationBarrier(); + if (pMailbox->El2JumpFlag == EL2REDIR_MAILBOX_SIGNAL) { + /* Acknowledge and jump */ + pMailbox->El2JumpFlag = EL2REDIR_MAILBOX_ACKNOWLEDGE; + ArmDataSynchronizationBarrier(); + + ARM_HVC_ARGS StubArg; + StubArg.Arg0 = ARM_SMC_ID_PSCI_CPU_SUSPEND_AARCH64; + ArmCallHvc(&StubArg); + /* We should not reach here */ + CpuDeadLoop(); + } + else if (pMailbox->El2JumpFlag == EL2REDIR_MAILBOX_STAY_EL1) { + // Hold at EL1, until CPU0 tell us that GIC is set up + break; + } + } + + ArmDataSynchronizationBarrier(); + if (pMailbox->El2JumpFlag == EL2REDIR_MAILBOX_STAY_EL1) { + // Notify the main processor that we are here + pMailbox->El2JumpFlag = EL2REDIR_MAILBOX_READY; + ArmDataSynchronizationBarrier(); + + // Hold at EL1, until CPU0 tell us that GIC is set up + while (1) { + ArmDataSynchronizationBarrier(); + if (pMailbox->El2JumpFlag == EL2REDIR_MAILBOX_SIGNAL) { + /* Just set to ready and continue setup */ + pMailbox->El2JumpFlag = EL2REDIR_MAILBOX_READY; + ArmDataSynchronizationBarrier(); + break; + } + } + } + } + else { + // Hold at EL2, until CPU0 tell us that GIC is set up + while (1) { + ArmDataSynchronizationBarrier(); + if (pMailbox->El2JumpFlag == EL2REDIR_MAILBOX_SIGNAL) { + /* Just set to ready and continue setup */ + pMailbox->El2JumpFlag = EL2REDIR_MAILBOX_READY; + ArmDataSynchronizationBarrier(); + break; + } + } + } + + // Turn on GIC CPU interface as well as SGI interrupts + ArmGicEnableInterruptInterface(FixedPcdGet64(PcdGicInterruptInterfaceBase)); + MmioWrite32(FixedPcdGet64(PcdGicInterruptInterfaceBase) + 0x4, 0xf0); + + // But turn off interrupts + ArmDisableInterrupts(); + + do { + // Technically we should do a WFI + // But we just spin here instead + ArmDataSynchronizationBarrier(); + + // Technically the CPU ID should be checked + // against request per MpPark spec, + // but the actual Windows implementation guarantees + // that no CPU will be started simultaneously, + // so the check was made optional. + // + // This also enables "spin-table" startup method + // for Linux. + // + // Example usage: + // enable-method = "spin-table"; + // cpu-release-addr = <0 0x00311008>; + if (FixedPcdGetBool(SecondaryCpuIgnoreCpuIdCheck) || + pMailbox->ProcessorId == Index) { + SecondaryEntryAddr = pMailbox->JumpAddress; + } + + AcknowledgeInterrupt = ArmGicAcknowledgeInterrupt( + FixedPcdGet64(PcdGicInterruptInterfaceBase), &InterruptId); + if (InterruptId < + ArmGicGetMaxNumInterrupts(FixedPcdGet64(PcdGicDistributorBase))) { + // Got a valid SGI number hence signal End of Interrupt + ArmGicEndOfInterrupt( + FixedPcdGet64(PcdGicInterruptInterfaceBase), AcknowledgeInterrupt); + } + } while (SecondaryEntryAddr == 0); + + // Acknowledge this one + pMailbox->JumpAddress = 0; + + SecondaryStart = (VOID(*)())SecondaryEntryAddr; + SecondaryStart(pMailbox); + + // Should never reach here + ASSERT(FALSE); +} \ No newline at end of file