Re: [edk2-devel] [PATCH v2 1/2] ArmPkg: implement EFI_MP_SERVICES_PROTOCOL based on PSCI calls

["Ard Biesheuvel" <ardb@kernel.org>]

On Wed, 7 Sept 2022 at 06:22, Rebecca Cran <quic_rcran@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 <rebecca@quicinc.com>
> Reviewed-by: Ard Biesheuvel <ardb@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
>
>
>
> 
>
>