[PATCH v1 12/18] StandaloneMmPkg/CpuMm: Add CPU driver suitable for ARM Platforms.

[Supreeth Venkatesh <supreeth.venkatesh@arm.com>]

This patch adds a simple CPU driver that exports the
EFI_MM_CONFIGURATION_PROTOCOL to allow registration of the Standalone
MM Foundation entry point. It preserves the existing notification
mechanism for the configuration protocol.

Contributed-under: TianoCore Contribution Agreement 1.1
Signed-off-by: Achin Gupta <achin.gupta@arm.com>
Signed-off-by: Supreeth Venkatesh <supreeth.venkatesh@arm.com>
---
 StandaloneMmPkg/Drivers/CpuMm/Arm/Entry.S          |  33 +++
 StandaloneMmPkg/Drivers/CpuMm/Arm/EventHandle.c    | 231 +++++++++++++++++++++
 StandaloneMmPkg/Drivers/CpuMm/Arm/Init.c           | 229 ++++++++++++++++++++
 .../CpuMm/Arm/PiMmStandloneArmTfCpuDriver.h        |  89 ++++++++
 .../CpuMm/Arm/PiMmStandloneArmTfCpuDriver.inf      |  60 ++++++
 StandaloneMmPkg/Drivers/CpuMm/Arm/StateSave.c      |  51 +++++
 StandaloneMmPkg/Include/Guid/MpInformation.h       |  41 ++++
 7 files changed, 734 insertions(+)
 create mode 100644 StandaloneMmPkg/Drivers/CpuMm/Arm/Entry.S
 create mode 100644 StandaloneMmPkg/Drivers/CpuMm/Arm/EventHandle.c
 create mode 100644 StandaloneMmPkg/Drivers/CpuMm/Arm/Init.c
 create mode 100644 StandaloneMmPkg/Drivers/CpuMm/Arm/PiMmStandloneArmTfCpuDriver.h
 create mode 100644 StandaloneMmPkg/Drivers/CpuMm/Arm/PiMmStandloneArmTfCpuDriver.inf
 create mode 100644 StandaloneMmPkg/Drivers/CpuMm/Arm/StateSave.c
 create mode 100644 StandaloneMmPkg/Include/Guid/MpInformation.h

diff --git a/StandaloneMmPkg/Drivers/CpuMm/Arm/Entry.S b/StandaloneMmPkg/Drivers/CpuMm/Arm/Entry.S
new file mode 100644
index 0000000000..0b6e1c330d
--- /dev/null
+++ b/StandaloneMmPkg/Drivers/CpuMm/Arm/Entry.S
@@ -0,0 +1,33 @@
+//
+//  Copyright (c) 2017, ARM Limited. All rights reserved.
+//
+//  This program and the accompanying materials
+//  are licensed and made available under the terms and conditions of the BSD License
+//  which accompanies this distribution.  The full text of the license may be found at
+//  http://opensource.org/licenses/bsd-license.php
+//
+//  THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+//  WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+//
+//
+
+#include <Base.h>
+#include <AutoGen.h>
+#include <IndustryStandard/ArmStdSmc.h>
+
+.text
+.align 3
+
+GCC_ASM_IMPORT(PiMmStandloneArmTfCpuDriverEntry)
+GCC_ASM_EXPORT(_PiMmStandloneArmTfCpuDriverEntry)
+
+// Stub entry point to ensure that the stacks are completely unwound before
+// signalling completion of event handling
+ASM_PFX(_PiMmStandloneArmTfCpuDriverEntry):
+  bl    PiMmStandloneArmTfCpuDriverEntry
+  mov   x1, x0
+  mov   x0, #(ARM_SMC_ID_MM_EVENT_COMPLETE_AARCH64 & 0xffff)
+  movk  x0, #((ARM_SMC_ID_MM_EVENT_COMPLETE_AARCH64 >> 16) & 0xffff), lsl #16
+  svc   #0
+LoopForever:
+  b     LoopForever
diff --git a/StandaloneMmPkg/Drivers/CpuMm/Arm/EventHandle.c b/StandaloneMmPkg/Drivers/CpuMm/Arm/EventHandle.c
new file mode 100644
index 0000000000..7b19f53ecb
--- /dev/null
+++ b/StandaloneMmPkg/Drivers/CpuMm/Arm/EventHandle.c
@@ -0,0 +1,231 @@
+/** @file
+
+  Copyright (c) 2016 HP Development Company, L.P.
+  Copyright (c) 2016 - 2017, ARM Limited. All rights reserved.
+
+  This program and the accompanying materials
+  are licensed and made available under the terms and conditions of the BSD License
+  which accompanies this distribution.  The full text of the license may be found at
+  http://opensource.org/licenses/bsd-license.php
+
+  THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+  WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#include <Base.h>
+#include <Pi/PiMmCis.h>
+
+
+#include <Library/ArmSvcLib.h>
+#include <Library/ArmLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/DebugLib.h>
+#include <Library/HobLib.h>
+
+#include <Protocol/DebugSupport.h> // for EFI_SYSTEM_CONTEXT
+
+#include <Guid/ZeroGuid.h>
+#include <Guid/MmramMemoryReserve.h>
+
+#include <IndustryStandard/ArmStdSmc.h>
+
+#include "PiMmStandloneArmTfCpuDriver.h"
+
+EFI_STATUS
+EFIAPI
+MmFoundationEntryRegister(
+  IN CONST EFI_MM_CONFIGURATION_PROTOCOL  *This,
+  IN EFI_MM_ENTRY_POINT                    MmEntryPoint
+  );
+
+//
+// On ARM platforms every event is expected to have a GUID associated with
+// it. It will be used by the MM Entry point to find the handler for the
+// event. It will either be populated in a EFI_MM_COMMUNICATE_HEADER by the
+// caller of the event (e.g. MM_COMMUNICATE SMC) or by the CPU driver
+// (e.g. during an asynchronous event). In either case, this context is
+// maintained in an array which has an entry for each CPU. The pointer to this
+// array is held in PerCpuGuidedEventContext. Memory is allocated once the
+// number of CPUs in the system are made known through the
+// MP_INFORMATION_HOB_DATA.
+//
+EFI_MM_COMMUNICATE_HEADER **PerCpuGuidedEventContext = NULL;
+
+//
+// When an event is received by the CPU driver, it could correspond to a unique
+// GUID (e.g. interrupt events) or to multiple GUIDs (e.g. MM_COMMUNICATE
+// SMC). A table is used by the CPU driver to find the GUID corresponding to the
+// event id in case there is a 1:1 mapping between the two. If an event id has
+// multiple GUIDs associated with it then such an entry will not be found in
+// this table.
+//
+// TODO: Currently NULL since there are no asynchronous events
+static EFI_GUID *EventIdToGuidLookupTable = NULL;
+
+// Descriptor with whereabouts of memory used for communication with the normal world
+EFI_MMRAM_DESCRIPTOR  mNsCommBuffer;
+
+MP_INFORMATION_HOB_DATA *mMpInformationHobData;
+
+EFI_MM_CONFIGURATION_PROTOCOL mMmConfig = {
+  0,
+  MmFoundationEntryRegister
+};
+
+static EFI_MM_ENTRY_POINT     mMmEntryPoint = NULL;
+
+EFI_STATUS
+PiMmStandloneArmTfCpuDriverEntry (
+  IN UINTN EventId,
+  IN UINTN CpuNumber,
+  IN UINTN NsCommBufferAddr
+  )
+{
+  EFI_MM_COMMUNICATE_HEADER *GuidedEventContext = NULL;
+  EFI_MM_ENTRY_CONTEXT        MmEntryPointContext = {0};
+  EFI_STATUS                  Status;
+  UINTN                       NsCommBufferSize;
+
+  DEBUG ((DEBUG_INFO, "Received event - 0x%x on cpu %d\n", EventId, CpuNumber));
+
+  //
+  // ARM TF passes SMC FID of the MM_COMMUNICATE interface as the Event ID upon
+  // receipt of a synchronous MM request. Use the Event ID to distinguish
+  // between synchronous and asynchronous events.
+  //
+  if (ARM_SMC_ID_MM_COMMUNICATE_AARCH64 != EventId) {
+    // Found a GUID, allocate memory to populate a communication buffer
+    // with the GUID in it
+    Status = mMmst->MmAllocatePool(EfiRuntimeServicesData, sizeof(EFI_MM_COMMUNICATE_HEADER), (VOID **) &GuidedEventContext);
+    if (Status != EFI_SUCCESS) {
+      DEBUG ((DEBUG_INFO, "Mem alloc failed - 0x%x\n", EventId));
+      return Status;
+    }
+
+    // Copy the GUID
+    CopyGuid(&GuidedEventContext->HeaderGuid, &EventIdToGuidLookupTable[EventId]);
+
+    // Message Length is 0 'cause of the assumption mentioned above
+    GuidedEventContext->MessageLength = 0;
+  } else {
+    // TODO: Perform parameter validation of NsCommBufferAddr
+
+    // This event id is the parent of multiple GUIDed handlers. Retrieve
+    // the specific GUID from the communication buffer passed by the
+    // caller.
+
+    if (NsCommBufferAddr && (NsCommBufferAddr < mNsCommBuffer.PhysicalStart))
+      return EFI_INVALID_PARAMETER;
+
+    // Find out the size of the buffer passed
+    NsCommBufferSize = ((EFI_MM_COMMUNICATE_HEADER *) NsCommBufferAddr)->MessageLength +
+                        sizeof(((EFI_MM_COMMUNICATE_HEADER *)NsCommBufferAddr)->MessageLength) +
+                        sizeof(((EFI_MM_COMMUNICATE_HEADER *)NsCommBufferAddr)->HeaderGuid);
+
+    // Alternative approach in case EL3 has preallocated the non-secure
+    // buffer. MM Foundation is told about the buffer through the Hoblist
+    // and is responsible for performing the bounds check.
+    if (NsCommBufferAddr + NsCommBufferSize >=
+      mNsCommBuffer.PhysicalStart + mNsCommBuffer.PhysicalSize)
+        return EFI_INVALID_PARAMETER;
+
+
+    // Now that the secure world can see the normal world buffer, allocate
+    // memory to copy the communication buffer to the secure world.
+    Status = mMmst->MmAllocatePool(EfiRuntimeServicesData, NsCommBufferSize, (VOID **) &GuidedEventContext);
+    if (Status != EFI_SUCCESS) {
+      DEBUG ((DEBUG_INFO, "Mem alloc failed - 0x%x\n", EventId));
+      // TODO: Unmap secure memory before exiting to the normal world
+      return Status;
+    }
+
+    // X1 contains the VA of the normal world memory accessible from
+    // S-EL0
+    CopyMem(GuidedEventContext, (CONST VOID *) NsCommBufferAddr, NsCommBufferSize);
+  }
+
+  // Stash the pointer to the allocated Event Context for this CPU
+  PerCpuGuidedEventContext[CpuNumber] = GuidedEventContext;
+
+  // TODO: Populate entire entry point context with valid information
+  MmEntryPointContext.CurrentlyExecutingCpu = CpuNumber;
+  MmEntryPointContext.NumberOfCpus = mMpInformationHobData->NumberOfProcessors;
+
+  // Populate the MM system table with MP and state information
+  mMmst->CurrentlyExecutingCpu = CpuNumber;
+  mMmst->NumberOfCpus = mMpInformationHobData->NumberOfProcessors;
+  mMmst->CpuSaveStateSize = 0;
+  mMmst->CpuSaveState = NULL;
+
+  mMmEntryPoint(&MmEntryPointContext);
+
+  // Free the memory allocation done earlier and reset the per-cpu context
+  // TODO: Check for the return status of the FreePool API
+  ASSERT (GuidedEventContext);
+  CopyMem ((VOID *)NsCommBufferAddr, (CONST VOID *) GuidedEventContext, NsCommBufferSize);
+  mMmst->MmFreePool((VOID *) GuidedEventContext);
+  PerCpuGuidedEventContext[CpuNumber] = NULL;
+
+  return Status;
+}
+
+EFI_STATUS
+EFIAPI
+MmFoundationEntryRegister(
+  IN CONST EFI_MM_CONFIGURATION_PROTOCOL  *This,
+  IN EFI_MM_ENTRY_POINT                    MmEntryPoint
+  ) {
+  // store the entry point in a global
+  mMmEntryPoint = MmEntryPoint;
+  return EFI_SUCCESS;
+}
+
+/**
+  This function is the main entry point for an MM handler dispatch
+  or communicate-based callback.
+
+  @param  DispatchHandle  The unique handle assigned to this handler by MmiHandlerRegister().
+  @param  Context         Points to an optional handler context which was specified when the handler was registered.
+  @param  CommBuffer      A pointer to a collection of data in memory that will
+                          be conveyed from a non-MM environment into an MM environment.
+  @param  CommBufferSize  The size of the CommBuffer.
+
+  @return Status Code
+
+**/
+EFI_STATUS
+EFIAPI
+PiMmCpuTpFwRootMmiHandler (
+  IN     EFI_HANDLE               DispatchHandle,
+  IN     CONST VOID               *Context,        OPTIONAL
+  IN OUT VOID                     *CommBuffer,     OPTIONAL
+  IN OUT UINTN                    *CommBufferSize  OPTIONAL
+  )
+{
+  EFI_STATUS Status;
+  UINTN      CpuNumber;
+
+  ASSERT (Context == NULL);
+  ASSERT (CommBuffer == NULL);
+  ASSERT (CommBufferSize == NULL);
+
+  CpuNumber = mMmst->CurrentlyExecutingCpu;
+  if (!PerCpuGuidedEventContext[CpuNumber])
+    return EFI_NOT_FOUND;
+
+  DEBUG ((DEBUG_INFO, "CommBuffer - 0x%x, CommBufferSize - 0x%x\n",
+          PerCpuGuidedEventContext[CpuNumber],
+          PerCpuGuidedEventContext[CpuNumber]->MessageLength));
+
+  Status = mMmst->MmiManage(&PerCpuGuidedEventContext[CpuNumber]->HeaderGuid,
+                     NULL,
+                     PerCpuGuidedEventContext[CpuNumber]->Data,
+                     &PerCpuGuidedEventContext[CpuNumber]->MessageLength);
+
+  if (Status != EFI_SUCCESS) {
+    DEBUG ((DEBUG_WARN, "Unable to manage Guided Event - %d\n", Status));
+  }
+
+  return Status;
+}
diff --git a/StandaloneMmPkg/Drivers/CpuMm/Arm/Init.c b/StandaloneMmPkg/Drivers/CpuMm/Arm/Init.c
new file mode 100644
index 0000000000..9b48ea15c1
--- /dev/null
+++ b/StandaloneMmPkg/Drivers/CpuMm/Arm/Init.c
@@ -0,0 +1,229 @@
+/** @file
+
+  Copyright (c) 2008 - 2009, Apple Inc. All rights reserved.<BR>
+  Copyright (c) 2011, ARM Limited. All rights reserved.
+  Copyright (c) 2016 HP Development Company, L.P.
+  Copyright (c) 2016-2017, ARM Limited. All rights reserved.
+
+  This program and the accompanying materials
+  are licensed and made available under the terms and conditions of the BSD License
+  which accompanies this distribution.  The full text of the license may be found at
+  http://opensource.org/licenses/bsd-license.php
+
+  THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+  WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#include <Base.h>
+#include <Pi/PiMmCis.h>
+#include <Library/Arm/StandaloneMmCoreEntryPoint.h>
+#include <Library/DebugLib.h>
+#include <Library/ArmSvcLib.h>
+#include <Library/ArmLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/HobLib.h>
+
+#include <Protocol/DebugSupport.h> // for EFI_SYSTEM_CONTEXT
+
+#include <Guid/ZeroGuid.h>
+#include <Guid/MmramMemoryReserve.h>
+
+
+#include "PiMmStandloneArmTfCpuDriver.h"
+
+// GUID to identify HOB with whereabouts of communication buffer with Normal
+// World
+extern EFI_GUID gEfiStandaloneMmNonSecureBufferGuid;
+
+// GUID to identify HOB where the entry point of this CPU driver will be
+// populated to allow the entry point driver to invoke it upon receipt of an
+// event
+extern EFI_GUID gEfiArmTfCpuDriverEpDescriptorGuid;
+
+//
+// Private copy of the MM system table for future use
+//
+EFI_MM_SYSTEM_TABLE *mMmst = NULL;
+
+//
+// Globals used to initialize the protocol
+//
+static EFI_HANDLE            mMmCpuHandle = NULL;
+
+EFI_STATUS
+GetGuidedHobData (
+  IN  VOID *HobList,
+  IN  CONST EFI_GUID *HobGuid,
+  OUT VOID **HobData)
+{
+  EFI_HOB_GUID_TYPE *Hob;
+
+  if (!HobList || !HobGuid || !HobData)
+    return EFI_INVALID_PARAMETER;
+
+  Hob = GetNextGuidHob (HobGuid, HobList);
+  if (!Hob)
+    return EFI_NOT_FOUND;
+
+  *HobData = GET_GUID_HOB_DATA (Hob);
+  if (!HobData)
+    return EFI_NOT_FOUND;
+
+  return EFI_SUCCESS;
+}
+
+EFI_STATUS
+PiMmStandloneArmTfCpuDriverInitialize (
+  IN EFI_HANDLE         ImageHandle,  // not actual imagehandle
+  IN EFI_MM_SYSTEM_TABLE   *SystemTable  // not actual systemtable
+  )
+{
+  ARM_TF_CPU_DRIVER_EP_DESCRIPTOR *CpuDriverEntryPointDesc;
+  EFI_CONFIGURATION_TABLE         *ConfigurationTable;
+  MP_INFORMATION_HOB_DATA         *MpInformationHobData;
+  EFI_MMRAM_DESCRIPTOR            *NsCommBufMmramRange;
+  EFI_STATUS                       Status;
+  EFI_HANDLE                       DispatchHandle;
+  UINT32                           MpInfoSize;
+  UINTN                            Index;
+  UINTN                            ArraySize;
+  VOID                            *HobStart;
+
+  ASSERT (SystemTable != NULL);
+  mMmst = SystemTable;
+
+  // publish the MM config protocol so the MM core can register its entry point
+  Status = mMmst->MmInstallProtocolInterface (&mMmCpuHandle,
+                                              &gEfiMmConfigurationProtocolGuid,
+                                              EFI_NATIVE_INTERFACE,
+                                              &mMmConfig);
+  if (EFI_ERROR(Status)) {
+    return Status;
+  }
+
+  // publish the MM CPU save state protocol
+  Status = mMmst->MmInstallProtocolInterface (&mMmCpuHandle,
+    &gEfiMmCpuProtocolGuid, EFI_NATIVE_INTERFACE, &mMmCpuState);
+  if (EFI_ERROR(Status)) {
+    return Status;
+  }
+
+  // register the root MMI handler
+  Status = mMmst->MmiHandlerRegister (PiMmCpuTpFwRootMmiHandler,
+                                      NULL,
+                                      &DispatchHandle);
+  if (EFI_ERROR(Status)) {
+    return Status;
+  }
+
+  // Retrieve the Hoblist from the MMST to extract the details of the NS
+  // communication buffer that has been reserved by S-EL1/EL3
+  ConfigurationTable = mMmst->MmConfigurationTable;
+  for (Index = 0; Index < mMmst->NumberOfTableEntries; Index++) {
+    if (CompareGuid (&gEfiHobListGuid, &(ConfigurationTable[Index].VendorGuid))) {
+      break;
+    }
+  }
+
+  // Bail out if the Hoblist could not be found
+  // TODO: This could also mean that
+  // the normal world will never interact synchronously with the MM environment
+  if (Index >= mMmst->NumberOfTableEntries) {
+    DEBUG ((DEBUG_INFO, "Hoblist not found - 0x%x\n", Index));
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  HobStart = ConfigurationTable[Index].VendorTable;
+
+  //
+  // Locate the HOB with the buffer to populate the entry point of this driver
+  //
+  Status = GetGuidedHobData (
+            HobStart,
+            &gEfiArmTfCpuDriverEpDescriptorGuid,
+            (VOID **) &CpuDriverEntryPointDesc);
+  if (EFI_ERROR (Status)) {
+    DEBUG ((DEBUG_INFO, "ArmTfCpuDriverEpDesc HOB data extraction failed - 0x%x\n", Status));
+    return Status;
+  }
+
+  // Share the entry point of the CPU driver
+  DEBUG ((DEBUG_INFO, "Sharing Cpu Driver EP *0x%lx = 0x%lx\n",
+    (UINT64) CpuDriverEntryPointDesc->ArmTfCpuDriverEpPtr,
+    (UINT64) PiMmStandloneArmTfCpuDriverEntry));
+  *(CpuDriverEntryPointDesc->ArmTfCpuDriverEpPtr) = PiMmStandloneArmTfCpuDriverEntry;
+
+  // Find the descriptor that contains the whereabouts of the buffer for
+  // communication with the Normal world.
+  Status = GetGuidedHobData (
+            HobStart,
+            &gEfiStandaloneMmNonSecureBufferGuid,
+            (VOID **) &NsCommBufMmramRange);
+  if (EFI_ERROR(Status)) {
+    DEBUG ((DEBUG_INFO, "NsCommBufMmramRange HOB data extraction failed - 0x%x\n", Status));
+    return Status;
+  }
+
+  DEBUG ((DEBUG_INFO, "mNsCommBuffer.PhysicalStart - 0x%lx\n", (UINT64) NsCommBufMmramRange->PhysicalStart));
+  DEBUG ((DEBUG_INFO, "mNsCommBuffer.PhysicalSize - 0x%lx\n", (UINT64) NsCommBufMmramRange->PhysicalSize));
+
+  CopyMem (&mNsCommBuffer, NsCommBufMmramRange, sizeof(EFI_MMRAM_DESCRIPTOR));
+  DEBUG ((DEBUG_INFO, "mNsCommBuffer: 0x%016lx - 0x%lx\n", mNsCommBuffer.CpuStart, mNsCommBuffer.PhysicalSize));
+
+  //
+  // Extract the MP information from the Hoblist
+  //
+  Status = GetGuidedHobData (HobStart,
+                             &gMpInformationHobGuid,
+                             (VOID **) &MpInformationHobData);
+  if (EFI_ERROR (Status)) {
+    DEBUG ((DEBUG_INFO, "MpInformationHob extraction failed - 0x%x\n", Status));
+    return Status;
+  }
+
+  //
+  // Allocate memory for the MP information and copy over the MP information
+  // passed by Trusted Firmware. Use the number of processors passed in the HOB
+  // to copy the processor information
+  //
+  MpInfoSize = sizeof (MP_INFORMATION_HOB_DATA) +
+               (sizeof (EFI_PROCESSOR_INFORMATION) *
+               MpInformationHobData->NumberOfProcessors);
+  Status = mMmst->MmAllocatePool (EfiRuntimeServicesData,
+                                  MpInfoSize,
+                                  (void **) &mMpInformationHobData);
+  if (EFI_ERROR (Status)) {
+    DEBUG ((DEBUG_INFO, "mMpInformationHobData mem alloc failed - 0x%x\n", Status));
+    return Status;
+  }
+
+  CopyMem (mMpInformationHobData, MpInformationHobData, MpInfoSize);
+
+  // Print MP information
+  DEBUG ((DEBUG_INFO, "mMpInformationHobData: 0x%016lx - 0x%lx\n",
+          mMpInformationHobData->NumberOfProcessors,
+          mMpInformationHobData->NumberOfEnabledProcessors));
+  for (Index = 0; Index < mMpInformationHobData->NumberOfProcessors; Index++) {
+    DEBUG ((DEBUG_INFO, "mMpInformationHobData[0x%lx]: %d, %d, %d\n",
+            mMpInformationHobData->ProcessorInfoBuffer[Index].ProcessorId,
+            mMpInformationHobData->ProcessorInfoBuffer[Index].Location.Package,
+            mMpInformationHobData->ProcessorInfoBuffer[Index].Location.Core,
+            mMpInformationHobData->ProcessorInfoBuffer[Index].Location.Thread));
+  }
+
+  //
+  // Allocate memory for a table to hold pointers to a
+  // EFI_MM_COMMUNICATE_HEADER for each CPU
+  //
+  ArraySize = sizeof (EFI_MM_COMMUNICATE_HEADER *) *
+              mMpInformationHobData->NumberOfEnabledProcessors;
+  Status = mMmst->MmAllocatePool (EfiRuntimeServicesData,
+                                  ArraySize,
+                                  (VOID **) &PerCpuGuidedEventContext);
+  if (EFI_ERROR (Status)) {
+    DEBUG ((DEBUG_INFO, "PerCpuGuidedEventContext mem alloc failed - 0x%x\n", Status));
+    return Status;
+  }
+  return Status;
+}
diff --git a/StandaloneMmPkg/Drivers/CpuMm/Arm/PiMmStandloneArmTfCpuDriver.h b/StandaloneMmPkg/Drivers/CpuMm/Arm/PiMmStandloneArmTfCpuDriver.h
new file mode 100644
index 0000000000..17cefd171c
--- /dev/null
+++ b/StandaloneMmPkg/Drivers/CpuMm/Arm/PiMmStandloneArmTfCpuDriver.h
@@ -0,0 +1,89 @@
+/** @file
+  Private header with declarations and definitions specific to the MM Standalone
+  CPU driver
+
+  Copyright (c) 2017, ARM Limited. All rights reserved.
+  This program and the accompanying materials
+  are licensed and made available under the terms and conditions of the BSD License
+  which accompanies this distribution.  The full text of the license may be found at
+  http://opensource.org/licenses/bsd-license.php
+
+  THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+  WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#ifndef _ARM_TF_CPU_DRIVER_H_
+#define _ARM_TF_CPU_DRIVER_H_
+
+#include <Protocol/MmCommunication.h>
+#include <Protocol/MmConfiguration.h>
+#include <Protocol/MmCpu.h>
+#include <Guid/MpInformation.h>
+
+//
+// Common declarations and definitions
+//
+#define EVENT_ID_MM_COMMUNICATE_SMC     0x10
+
+//
+// CPU driver initialization specific declarations
+//
+extern EFI_MM_SYSTEM_TABLE *mMmst;
+
+//
+// CPU State Save protocol specific declarations
+//
+extern EFI_MM_CPU_PROTOCOL mMmCpuState;
+
+EFI_STATUS
+EFIAPI
+MmReadSaveState (
+  IN CONST EFI_MM_CPU_PROTOCOL   *This,
+  IN UINTN                        Width,
+  IN EFI_MM_SAVE_STATE_REGISTER  Register,
+  IN UINTN                        CpuIndex,
+  OUT VOID                        *Buffer
+  );
+
+EFI_STATUS
+EFIAPI
+MmWriteSaveState (
+  IN CONST EFI_MM_CPU_PROTOCOL   *This,
+  IN UINTN                        Width,
+  IN EFI_MM_SAVE_STATE_REGISTER  Register,
+  IN UINTN                        CpuIndex,
+  IN CONST VOID                   *Buffer
+  );
+
+//
+// MM event handling specific declarations
+//
+extern EFI_MM_COMMUNICATE_HEADER    **PerCpuGuidedEventContext;
+extern EFI_MMRAM_DESCRIPTOR          mNsCommBuffer;
+extern MP_INFORMATION_HOB_DATA       *mMpInformationHobData;
+extern EFI_MM_CONFIGURATION_PROTOCOL mMmConfig;
+
+EFI_STATUS
+PiMmStandloneArmTfCpuDriverEntry (
+  IN UINTN EventId,
+  IN UINTN CpuNumber,
+  IN UINTN NsCommBufferAddr
+  );
+
+EFI_STATUS
+EFIAPI
+PiMmCpuTpFwRootMmiHandler (
+  IN     EFI_HANDLE               DispatchHandle,
+  IN     CONST VOID               *Context,        OPTIONAL
+  IN OUT VOID                     *CommBuffer,     OPTIONAL
+  IN OUT UINTN                    *CommBufferSize  OPTIONAL
+  );
+
+EFI_STATUS _PiMmStandloneArmTfCpuDriverEntry (
+  IN UINTN EventId,
+  IN UINTN CpuNumber,
+  IN UINTN NsCommBufferAddr
+  );
+
+#endif
diff --git a/StandaloneMmPkg/Drivers/CpuMm/Arm/PiMmStandloneArmTfCpuDriver.inf b/StandaloneMmPkg/Drivers/CpuMm/Arm/PiMmStandloneArmTfCpuDriver.inf
new file mode 100644
index 0000000000..baf6d957bb
--- /dev/null
+++ b/StandaloneMmPkg/Drivers/CpuMm/Arm/PiMmStandloneArmTfCpuDriver.inf
@@ -0,0 +1,60 @@
+#/** @file
+#
+#  Standalone MM CPU driver for ARM Standard Platforms
+#
+#  Copyright (c) 2009, Apple Inc. All rights reserved.<BR>
+#  Copyright (c) 2016 HP Development Company, L.P.
+#  Copyright (c) 2017, ARM Limited. All rights reserved.
+#
+#  This program and the accompanying materials
+#  are licensed and made available under the terms and conditions of the BSD License
+#  which accompanies this distribution.  The full text of the license may be found at
+#  http://opensource.org/licenses/bsd-license.php
+#
+#  THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+#  WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+#
+#**/
+
+[Defines]
+  INF_VERSION                    = 0x0001001A
+  BASE_NAME                      = PiMmStandloneArmTfCpuDriver
+  FILE_GUID                      = 58F7A62B-6280-42A7-BC38-10535A64A92C
+  MODULE_TYPE                    = MM_STANDALONE
+  VERSION_STRING                 = 1.0
+  PI_SPECIFICATION_VERSION       = 0x00010032
+  ENTRY_POINT                    = PiMmStandloneArmTfCpuDriverInitialize
+
+[Sources]
+  Init.c
+  EventHandle.c
+  StateSave.c
+
+[Packages]
+  ArmPkg/ArmPkg.dec
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+  StandaloneMmPkg/StandaloneMmPkg.dec
+
+[LibraryClasses]
+  ArmLib
+  ArmSvcLib
+  BaseMemoryLib
+  DebugLib
+  HobLib
+  StandaloneMmDriverEntryPoint
+
+[Protocols]
+  gEfiMmConfigurationProtocolGuid                        # PROTOCOL ALWAYS_PRODUCED
+  gEfiMmCpuProtocolGuid                                  # PROTOCOL ALWAYS_PRODUCED
+
+[Guids]
+  gEfiHobListGuid
+  gEfiMmPeiMmramMemoryReserveGuid
+  gZeroGuid
+  gMpInformationHobGuid
+  gEfiStandaloneMmNonSecureBufferGuid
+  gEfiArmTfCpuDriverEpDescriptorGuid
+
+[Depex]
+  TRUE
diff --git a/StandaloneMmPkg/Drivers/CpuMm/Arm/StateSave.c b/StandaloneMmPkg/Drivers/CpuMm/Arm/StateSave.c
new file mode 100644
index 0000000000..c5155e1b31
--- /dev/null
+++ b/StandaloneMmPkg/Drivers/CpuMm/Arm/StateSave.c
@@ -0,0 +1,51 @@
+/** @file
+
+  Copyright (c) 2016 HP Development Company, L.P.
+  Copyright (c) 2016-2017, ARM Limited. All rights reserved.
+
+  This program and the accompanying materials
+  are licensed and made available under the terms and conditions of the BSD License
+  which accompanies this distribution.  The full text of the license may be found at
+  http://opensource.org/licenses/bsd-license.php
+
+  THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+  WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#include <Base.h>
+#include <Pi/PiSmmCis.h>
+#include <Library/DebugLib.h>
+
+#include "PiMmStandloneArmTfCpuDriver.h"
+
+EFI_MM_CPU_PROTOCOL mMmCpuState = {
+  MmReadSaveState,
+  MmWriteSaveState
+};
+
+EFI_STATUS
+EFIAPI
+MmReadSaveState(
+  IN CONST EFI_MM_CPU_PROTOCOL   *This,
+  IN UINTN                        Width,
+  IN EFI_MM_SAVE_STATE_REGISTER  Register,
+  IN UINTN                        CpuIndex,
+  OUT VOID                        *Buffer
+  ) {
+  // todo: implement
+  return EFI_UNSUPPORTED;
+}
+
+EFI_STATUS
+EFIAPI
+MmWriteSaveState(
+  IN CONST EFI_MM_CPU_PROTOCOL   *This,
+  IN UINTN                        Width,
+  IN EFI_MM_SAVE_STATE_REGISTER  Register,
+  IN UINTN                        CpuIndex,
+  IN CONST VOID                   *Buffer
+  ) {
+  // todo: implement
+  return EFI_UNSUPPORTED;
+}
diff --git a/StandaloneMmPkg/Include/Guid/MpInformation.h b/StandaloneMmPkg/Include/Guid/MpInformation.h
new file mode 100644
index 0000000000..4e9a3c04ec
--- /dev/null
+++ b/StandaloneMmPkg/Include/Guid/MpInformation.h
@@ -0,0 +1,41 @@
+/** @file
+  EFI MP information protocol provides a lightweight MP_SERVICES_PROTOCOL.
+
+  MP information protocol only provides static information of MP processor.
+
+  Copyright (c) 2009, Intel Corporation. All rights reserved.<BR>
+  Copyright (c) 2016 - 2017, ARM Limited. All rights reserved.<BR>
+
+  This program and the accompanying materials
+  are licensed and made available under the terms and conditions of the BSD License
+  which accompanies this distribution.  The full text of the license may be found at
+  http://opensource.org/licenses/bsd-license.php
+
+  THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+  WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#ifndef _MP_INFORMATION_H_
+#define _MP_INFORMATION_H_
+
+#include <Protocol/MpService.h>
+#include <PiPei.h>
+#include <Ppi/SecPlatformInformation.h>
+
+#define MP_INFORMATION_GUID \
+  { \
+    0xba33f15d, 0x4000, 0x45c1, {0x8e, 0x88, 0xf9, 0x16, 0x92, 0xd4, 0x57, 0xe3}  \
+  }
+
+#pragma pack(1)
+typedef struct {
+  UINT64                     NumberOfProcessors;
+  UINT64                     NumberOfEnabledProcessors;
+  EFI_PROCESSOR_INFORMATION  ProcessorInfoBuffer[];
+} MP_INFORMATION_HOB_DATA;
+#pragma pack()
+
+extern EFI_GUID gMpInformationHobGuid;
+
+#endif
-- 
2.16.2