On Wed, 7 Sept 2022 at 06:22, Rebecca Cran <quic_rc...@quicinc.com> wrote:
>
> Add support for EFI_MP_SERVICES_PROTOCOL during the DXE phase under
> AArch64.
>
> PSCI_CPU_ON is called to power on the core, the supplied procedure is
> executed and PSCI_CPU_OFF is called to power off the core.
>
> Fixes contributed by Ard Biesheuvel.
>
> Signed-off-by: Rebecca Cran <rebe...@quicinc.com>
> Reviewed-by: Ard Biesheuvel <a...@kernel.org>
I spotted one issue below when double checking the cache maintenance situation;
> ---
> ArmPkg/ArmPkg.dsc | 1 +
> ArmPkg/Drivers/ArmPsciMpServicesDxe/ArmPsciMpServicesDxe.inf | 55 +
> ArmPkg/Drivers/ArmPsciMpServicesDxe/MpServicesInternal.h | 351 ++++
> ArmPkg/Drivers/ArmPsciMpServicesDxe/ArmPsciMpServicesDxe.c | 1774
> ++++++++++++++++++++
> ArmPkg/Drivers/ArmPsciMpServicesDxe/MpFuncs.S | 57 +
> 5 files changed, 2238 insertions(+)
>
...
> diff --git a/ArmPkg/Drivers/ArmPsciMpServicesDxe/ArmPsciMpServicesDxe.c
> b/ArmPkg/Drivers/ArmPsciMpServicesDxe/ArmPsciMpServicesDxe.c
> new file mode 100644
> index 000000000000..8cea203c7e34
> --- /dev/null
> +++ b/ArmPkg/Drivers/ArmPsciMpServicesDxe/ArmPsciMpServicesDxe.c
...
> +/** Initialize multi-processor support.
> +
> + @param ImageHandle Image handle.
> + @param SystemTable System table.
> +
> + @return EFI_SUCCESS on success, or an error code.
> +
> +**/
> +EFI_STATUS
> +EFIAPI
> +ArmPsciMpServicesDxeInitialize (
> + IN EFI_HANDLE ImageHandle,
> + IN EFI_SYSTEM_TABLE *SystemTable
> + )
> +{
> + EFI_STATUS Status;
> + EFI_HANDLE Handle;
> + UINTN MaxCpus;
> + EFI_LOADED_IMAGE_PROTOCOL *Image;
> + EFI_HOB_GENERIC_HEADER *Hob;
> + VOID *HobData;
> + UINTN HobDataSize;
> + CONST ARM_CORE_INFO *CoreInfo;
> +
> + MaxCpus = 1;
> +
> + DEBUG ((DEBUG_INFO, "Starting MP services\n"));
> +
> + Status = gBS->HandleProtocol (
> + ImageHandle,
> + &gEfiLoadedImageProtocolGuid,
> + (VOID **)&Image
> + );
> + ASSERT_EFI_ERROR (Status);
> +
> + //
> + // Parts of the code in this driver may be executed by other cores running
> + // with the MMU off so we need to ensure that everything is clean to the
> + // point of coherency (PoC)
> + //
> + WriteBackDataCacheRange (Image->ImageBase, Image->ImageSize);
> +
> + Status = gBS->HandleProtocol (
> + ImageHandle,
> + &gEfiLoadedImageProtocolGuid,
> + (VOID **)&Image
> + );
> + ASSERT_EFI_ERROR (Status);
> +
> + //
> + // Parts of the code in this driver may be executed by other cores running
> + // with the MMU off so we need to ensure that everything is clean to the
> + // point of coherency (PoC)
> + //
> + WriteBackDataCacheRange (Image->ImageBase, Image->ImageSize);
> +
I don't think doing this twice serves any purpose :-)
> + Hob = GetFirstGuidHob (&gArmMpCoreInfoGuid);
> + if (Hob != NULL) {
> + HobData = GET_GUID_HOB_DATA (Hob);
> + HobDataSize = GET_GUID_HOB_DATA_SIZE (Hob);
> + CoreInfo = (ARM_CORE_INFO *)HobData;
> + MaxCpus = HobDataSize / sizeof (ARM_CORE_INFO);
> + }
> +
> + if (MaxCpus == 1) {
> + DEBUG ((DEBUG_WARN, "Trying to use EFI_MP_SERVICES_PROTOCOL on a UP
> system"));
> + // We are not MP so nothing to do
> + return EFI_NOT_FOUND;
> + }
> +
> + Status = MpServicesInitialize (MaxCpus, CoreInfo);
> + if (Status != EFI_SUCCESS) {
> + ASSERT_EFI_ERROR (Status);
> + return Status;
> + }
> +
> + //
> + // Now install the MP services protocol.
> + //
> + Handle = NULL;
> + Status = gBS->InstallMultipleProtocolInterfaces (
> + &Handle,
> + &gEfiMpServiceProtocolGuid,
> + &mMpServicesProtocol,
> + NULL
> + );
> + ASSERT_EFI_ERROR (Status);
> + return Status;
> +}
> +
> +/** C entry-point for the AP.
> + This function gets called from the assembly function ApEntryPoint.
> +
> +**/
> +VOID
> +ApProcedure (
> + VOID
> + )
> +{
> + ARM_SMC_ARGS Args;
> + EFI_AP_PROCEDURE UserApProcedure;
> + VOID *UserApParameter;
> + UINTN ProcessorIndex;
> +
> + WhoAmI (&mMpServicesProtocol, &ProcessorIndex);
> +
> + /* Fetch the user-supplied procedure and parameter to execute */
> + UserApProcedure = mCpuMpData.CpuData[ProcessorIndex].Procedure;
> + UserApParameter = mCpuMpData.CpuData[ProcessorIndex].Parameter;
> +
> + // Configure the MMU and caches
> + ArmSetTCR (mCpuMpData.CpuData[ProcessorIndex].Tcr);
> + ArmSetTTBR0 (mCpuMpData.CpuData[ProcessorIndex].Ttbr0);
> + ArmSetMAIR (mCpuMpData.CpuData[ProcessorIndex].Mair);
> + ArmDisableAlignmentCheck ();
> + ArmEnableStackAlignmentCheck ();
> + ArmEnableInstructionCache ();
> + ArmEnableDataCache ();
> + ArmEnableMmu ();
> +
> + UserApProcedure (UserApParameter);
> +
> + mCpuMpData.CpuData[ProcessorIndex].State = CpuStateFinished;
> +
> + ArmDataMemoryBarrier ();
> +
> + /* Since we're finished with this AP, turn it off */
> + Args.Arg0 = ARM_SMC_ID_PSCI_CPU_OFF;
> + ArmCallSmc (&Args);
> +
> + /* Should never be reached */
> + ASSERT (FALSE);
> + CpuDeadLoop ();
> +}
> +
> +/** Returns whether the processor executing this function is the BSP.
> +
> + @return Whether the current processor is the BSP.
> +**/
> +STATIC
> +BOOLEAN
> +IsCurrentProcessorBSP (
> + VOID
> + )
> +{
> + EFI_STATUS Status;
> + UINTN ProcessorIndex;
> +
> + Status = WhoAmI (&mMpServicesProtocol, &ProcessorIndex);
> + if (EFI_ERROR (Status)) {
> + ASSERT_EFI_ERROR (Status);
> + return FALSE;
> + }
> +
> + return IsProcessorBSP (ProcessorIndex);
> +}
> +
> +/** Returns whether the specified processor is enabled.
> +
> + @param[in] ProcessorIndex The index of the processor to check.
> +
> + @return TRUE if the processor is enabled, FALSE otherwise.
> +**/
> +STATIC
> +BOOLEAN
> +IsProcessorEnabled (
> + UINTN ProcessorIndex
> + )
> +{
> + EFI_PROCESSOR_INFORMATION *CpuInfo;
> +
> + CpuInfo = &mCpuMpData.CpuData[ProcessorIndex].Info;
> +
> + return (CpuInfo->StatusFlag & PROCESSOR_ENABLED_BIT) != 0;
> +}
> +
> +/** Returns whether all processors are in the idle state.
> +
> + @return Whether all the processors are idle.
> +
> +**/
> +STATIC
> +BOOLEAN
> +CheckAllCpusReady (
> + VOID
> + )
> +{
> + UINTN Index;
> + CPU_AP_DATA *CpuData;
> +
> + for (Index = 0; Index < mCpuMpData.NumberOfProcessors; Index++) {
> + CpuData = &mCpuMpData.CpuData[Index];
> + if (IsProcessorBSP (Index)) {
> + // Skip BSP
> + continue;
> + }
> +
> + if (!IsProcessorEnabled (Index)) {
> + // Skip Disabled processors
> + continue;
> + }
> +
> + if (GetApState (CpuData) != CpuStateIdle) {
> + return FALSE;
> + }
> + }
> +
> + return TRUE;
> +}
> +
> +/** Sets up the state for the StartupAllAPs function.
> +
> + @param SingleThread Whether the APs will execute sequentially.
> +
> +**/
> +STATIC
> +VOID
> +StartupAllAPsPrepareState (
> + IN BOOLEAN SingleThread
> + )
> +{
> + UINTN Index;
> + CPU_STATE APInitialState;
> + CPU_AP_DATA *CpuData;
> +
> + mCpuMpData.FinishCount = 0;
> + mCpuMpData.StartCount = 0;
> + mCpuMpData.SingleThread = SingleThread;
> +
> + APInitialState = CpuStateReady;
> +
> + for (Index = 0; Index < mCpuMpData.NumberOfProcessors; Index++) {
> + CpuData = &mCpuMpData.CpuData[Index];
> +
> + //
> + // Get APs prepared, and put failing APs into FailedCpuList.
> + // If "SingleThread", only 1 AP will put into ready state, other AP will
> be
> + // put into ready state 1 by 1, until the previous 1 finished its task.
> + // If not "SingleThread", all APs are put into ready state from the
> + // beginning
> + //
> +
> + if (IsProcessorBSP (Index)) {
> + // Skip BSP
> + continue;
> + }
> +
> + if (!IsProcessorEnabled (Index)) {
> + // Skip Disabled processors
> + if (mCpuMpData.FailedList != NULL) {
> + mCpuMpData.FailedList[mCpuMpData.FailedListIndex++] = Index;
> + }
> +
> + continue;
> + }
> +
> + ASSERT (GetApState (CpuData) == CpuStateIdle);
> + CpuData->State = APInitialState;
> +
> + mCpuMpData.StartCount++;
> + if (SingleThread) {
> + APInitialState = CpuStateBlocked;
> + }
> + }
> +}
> +
> +/** Handles execution of StartupAllAPs when a WaitEvent has been specified.
> +
> + @param Procedure The user-supplied procedure.
> + @param ProcedureArgument The user-supplied procedure argument.
> + @param WaitEvent The wait event to be signaled when the work is
> + complete or a timeout has occurred.
> + @param TimeoutInMicroseconds The timeout for the work to be completed.
> Zero
> + indicates an infinite timeout.
> +
> + @return EFI_SUCCESS on success.
> +**/
> +STATIC
> +EFI_STATUS
> +StartupAllAPsWithWaitEvent (
> + IN EFI_AP_PROCEDURE Procedure,
> + IN VOID *ProcedureArgument,
> + IN EFI_EVENT WaitEvent,
> + IN UINTN TimeoutInMicroseconds
> + )
> +{
> + EFI_STATUS Status;
> + UINTN Index;
> + CPU_AP_DATA *CpuData;
> +
> + for (Index = 0; Index < mCpuMpData.NumberOfProcessors; Index++) {
> + CpuData = &mCpuMpData.CpuData[Index];
> + if (IsProcessorBSP (Index)) {
> + // Skip BSP
> + continue;
> + }
> +
> + if (!IsProcessorEnabled (Index)) {
> + // Skip Disabled processors
> + continue;
> + }
> +
> + if (GetApState (CpuData) == CpuStateReady) {
> + SetApProcedure (CpuData, Procedure, ProcedureArgument);
> + }
> + }
> +
> + //
> + // Save data into private data structure, and create timer to poll AP state
> + // before exiting
> + //
> + mCpuMpData.Procedure = Procedure;
> + mCpuMpData.ProcedureArgument = ProcedureArgument;
> + mCpuMpData.WaitEvent = WaitEvent;
> + mCpuMpData.Timeout = TimeoutInMicroseconds;
> + mCpuMpData.TimeoutActive = (BOOLEAN)(TimeoutInMicroseconds != 0);
> + Status = gBS->SetTimer (
> + mCpuMpData.CheckAllAPsEvent,
> + TimerPeriodic,
> + POLL_INTERVAL_US
> + );
> + return Status;
> +}
> +
> +/** Handles execution of StartupAllAPs when no wait event has been specified.
> +
> + @param Procedure The user-supplied procedure.
> + @param ProcedureArgument The user-supplied procedure argument.
> + @param TimeoutInMicroseconds The timeout for the work to be completed.
> Zero
> + indicates an infinite timeout.
> + @param SingleThread Whether the APs will execute sequentially.
> + @param FailedCpuList User-supplied pointer for list of failed
> CPUs.
> +
> + @return EFI_SUCCESS on success.
> +**/
> +STATIC
> +EFI_STATUS
> +StartupAllAPsNoWaitEvent (
> + IN EFI_AP_PROCEDURE Procedure,
> + IN VOID *ProcedureArgument,
> + IN UINTN TimeoutInMicroseconds,
> + IN BOOLEAN SingleThread,
> + IN UINTN **FailedCpuList
> + )
> +{
> + EFI_STATUS Status;
> + UINTN Index;
> + UINTN NextIndex;
> + UINTN Timeout;
> + CPU_AP_DATA *CpuData;
> +
> + Timeout = TimeoutInMicroseconds;
> +
> + while (TRUE) {
> + for (Index = 0; Index < mCpuMpData.NumberOfProcessors; Index++) {
> + CpuData = &mCpuMpData.CpuData[Index];
> + if (IsProcessorBSP (Index)) {
> + // Skip BSP
> + continue;
> + }
> +
> + if (!IsProcessorEnabled (Index)) {
> + // Skip Disabled processors
> + continue;
> + }
> +
> + switch (GetApState (CpuData)) {
> + case CpuStateReady:
> + SetApProcedure (CpuData, Procedure, ProcedureArgument);
> + Status = DispatchCpu (Index);
> + if (EFI_ERROR (Status)) {
> + CpuData->State = CpuStateIdle;
> + Status = EFI_NOT_READY;
> + goto Done;
> + }
> +
> + break;
> +
> + case CpuStateFinished:
> + mCpuMpData.FinishCount++;
> + if (SingleThread) {
> + Status = GetNextBlockedNumber (&NextIndex);
> + if (!EFI_ERROR (Status)) {
> + mCpuMpData.CpuData[NextIndex].State = CpuStateReady;
> + }
> + }
> +
> + CpuData->State = CpuStateIdle;
> + break;
> +
> + default:
> + break;
> + }
> + }
> +
> + if (mCpuMpData.FinishCount == mCpuMpData.StartCount) {
> + Status = EFI_SUCCESS;
> + goto Done;
> + }
> +
> + if ((TimeoutInMicroseconds != 0) && (Timeout == 0)) {
> + Status = EFI_TIMEOUT;
> + goto Done;
> + }
> +
> + Timeout -= CalculateAndStallInterval (Timeout);
> + }
> +
> +Done:
> + if (FailedCpuList != NULL) {
> + if (mCpuMpData.FailedListIndex == 0) {
> + FreePool (*FailedCpuList);
> + *FailedCpuList = NULL;
> + }
> + }
> +
> + return Status;
> +}
> diff --git a/ArmPkg/Drivers/ArmPsciMpServicesDxe/MpFuncs.S
> b/ArmPkg/Drivers/ArmPsciMpServicesDxe/MpFuncs.S
> new file mode 100644
> index 000000000000..a90fa8a0075f
> --- /dev/null
> +++ b/ArmPkg/Drivers/ArmPsciMpServicesDxe/MpFuncs.S
> @@ -0,0 +1,57 @@
> +#===============================================================================
> +# Copyright (c) 2022 Qualcomm Innovation Center, Inc. All rights reserved.
> +#
> +# SPDX-License-Identifier: BSD-2-Clause-Patent
> +#===============================================================================
> +
> +.text
> +.align 3
> +
> +#include <AsmMacroIoLibV8.h>
> +#include <IndustryStandard/ArmStdSmc.h>
> +
> +#include "MpServicesInternal.h"
> +
> +GCC_ASM_IMPORT (gApStacksBase)
> +GCC_ASM_IMPORT (gProcessorIDs)
> +GCC_ASM_IMPORT (ApProcedure)
> +GCC_ASM_IMPORT (gApStackSize)
> +
> +GCC_ASM_EXPORT (ApEntryPoint)
> +
> +// Entry-point for the AP
> +// VOID
> +// ApEntryPoint (
> +// VOID
> +// );
> +ASM_PFX(ApEntryPoint):
> + mrs x0, mpidr_el1
> + // Mask the non-affinity bits
> + bic x0, x0, 0x00ff000000
> + and x0, x0, 0xffffffffff
> + ldr x1, gProcessorIDs
> + mov x2, 0 // x2 = processor index
> +
> +// Find index in gProcessorIDs for current processor
> +1:
> + ldr x3, [x1, x2, lsl #3] // x4 = gProcessorIDs + x2 * 8
> + cmp x3, #-1 // check if we've reached the end of
> gProcessorIDs
> + beq ProcessorNotFound
> + add x2, x2, 1 // x2++
> + cmp x0, x3 // if mpidr_el1 != gProcessorIDs[x] then loop
> + bne 1b
> +
> +// Calculate stack address
> + // x2 contains the index for the current processor plus 1
> + ldr x0, gApStacksBase
> + ldr x1, gApStackSize
> + mul x3, x2, x1 // x3 = (ProcessorIndex + 1) * gApStackSize
> + add sp, x0, x3 // sp = gApStacksBase + x3
> + mov x29, xzr
> + bl ApProcedure // doesn't return
> +
> +ProcessorNotFound:
> +// Turn off the processor
> + MOV32 (w0, ARM_SMC_ID_PSCI_CPU_OFF)
> + smc #0
> + b .
> --
> 2.30.2
>
>
>
>
>
>
-=-=-=-=-=-=-=-=-=-=-=-
Groups.io Links: You receive all messages sent to this group.
View/Reply Online (#93360): https://edk2.groups.io/g/devel/message/93360
Mute This Topic: https://groups.io/mt/93518787/21656
Group Owner: devel+ow...@edk2.groups.io
Unsubscribe: https://edk2.groups.io/g/devel/unsub [arch...@mail-archive.com]
-=-=-=-=-=-=-=-=-=-=-=-
