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

["Rebecca Cran" <quic_rcran@quicinc.com>]

Thanks.
I've also noticed a problem with cache maintenance on the N2 FVP: it 
only works if I add manual cache flushes after enabling the MMU and 
caches (and also flush ApFunction to memory).
I'm working on debugging it.

-- 
Rebecca Cran

On 9/7/22 02:13, Ard Biesheuvel wrote:
> 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
>>
>>
>>
>> 
>>
>>