3 files changed, 89 insertions, 3 deletions
diff --git a/ArmPkg/Drivers/ArmCpuLib/ArmCortexA5Lib/ArmCortexA5Lib.c b/ArmPkg/Drivers/ArmCpuLib/ArmCortexA5Lib/ArmCortexA5Lib.c
new file mode 100644
index 000000000..5151039c0
--- /dev/null
+++ b/ArmPkg/Drivers/ArmCpuLib/ArmCortexA5Lib/ArmCortexA5Lib.c
@@ -0,0 +1,49 @@
+/** @file
+
+  Copyright (c) 2011-2012, 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 <Library/ArmLib.h>
+#include <Library/ArmCpuLib.h>
+#include <Library/DebugLib.h>
+#include <Library/IoLib.h>
+#include <Library/PcdLib.h>
+
+#include <Chipset/ArmCortexA5.h>
+
+VOID
+ArmCpuSetup (
+  IN  UINTN         MpId
+  )
+{
+  // Enable SWP instructions
+  ArmEnableSWPInstruction ();
+
+  // Enable program flow prediction, if supported.
+  ArmEnableBranchPrediction ();
+
+  // If MPCore then Enable the SCU
+  if (ArmIsMpCore()) {
+    // Turn on SMP coherency
+    ArmSetAuxCrBit (A5_FEATURE_SMP);
+  }
+
+}
+
+
+VOID
+ArmCpuSetupSmpNonSecure (
+  IN  UINTN         MpId
+  )
+{
+}
diff --git a/ArmPkg/Drivers/ArmCpuLib/ArmCortexA5Lib/ArmCortexA5Lib.inf b/ArmPkg/Drivers/ArmCpuLib/ArmCortexA5Lib/ArmCortexA5Lib.inf
new file mode 100644
index 000000000..36ae596b2
--- /dev/null
+++ b/ArmPkg/Drivers/ArmCpuLib/ArmCortexA5Lib/ArmCortexA5Lib.inf
@@ -0,0 +1,38 @@
+#/* @file
+#  Copyright (c) 2011-2012, 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                    = 0x00010005
+  BASE_NAME                      = ArmCortexA5Lib
+  FILE_GUID                      = c9709ea3-1beb-4806-889a-8a1d5e5e1697
+  MODULE_TYPE                    = BASE
+  VERSION_STRING                 = 1.0
+  LIBRARY_CLASS                  = ArmCpuLib
+
+[Packages]
+  MdePkg/MdePkg.dec
+  ArmPkg/ArmPkg.dec
+
+[LibraryClasses]
+  ArmLib
+  IoLib
+  PcdLib
+
+[Sources.common]
+  ArmCortexA5Lib.c
+
+[FeaturePcd]
+
+[FixedPcd]
+  gArmTokenSpaceGuid.PcdArmPrimaryCoreMask
+  gArmTokenSpaceGuid.PcdArmPrimaryCore
diff --git a/ArmPkg/Drivers/ArmCpuLib/ArmCortexA9Lib/ArmCortexA9Lib.c b/ArmPkg/Drivers/ArmCpuLib/ArmCortexA9Lib/ArmCortexA9Lib.c
index 4fc6a08b9..aed43cb3e 100644
--- a/ArmPkg/Drivers/ArmCpuLib/ArmCortexA9Lib/ArmCortexA9Lib.c
+++ b/ArmPkg/Drivers/ArmCpuLib/ArmCortexA9Lib/ArmCortexA9Lib.c
@@ -49,9 +49,6 @@ ArmCpuSetup (
 
   // If MPCore then Enable the SCU
   if (ArmIsMpCore()) {
-    // Signals the Cortex-A9 processor is taking part in coherency
-    ArmSetAuxCrBit (A9_FEATURE_SMP);
-
     ArmEnableScu ();
   }
 }
@@ -64,6 +61,8 @@ ArmCpuSetupSmpNonSecure (
 {
   INTN          ScuBase;
 
+  ArmSetAuxCrBit (A9_FEATURE_SMP);
+
   // Make the SCU accessible in Non Secure world
   if (ArmPlatformIsPrimaryCore (MpId)) {
     ScuBase = ArmGetScuBaseAddress();