Sync up ArmPkg with patch from mailing list. Changed name of BdsLib.h to BdsUnixLib.h and fixed a lot of issues with Xcode building.

git-svn-id: https://edk2.svn.sourceforge.net/svnroot/edk2/trunk/edk2@11293 6f19259b-4bc3-4df7-8a09-765794883524

16 files changed, 931 insertions, 264 deletions

diff --git a/ArmPkg/Library/ArmLib/ArmV7/ArmLibSupport.S b/ArmPkg/Library/ArmLib/ArmV7/ArmLibSupport.S
index 8ca37f0be0..54f36174bb 100644
--- a/ArmPkg/Library/ArmLib/ArmV7/ArmLibSupport.S
+++ b/ArmPkg/Library/ArmLib/ArmV7/ArmLibSupport.S
@@ -12,44 +12,36 @@
 #
 #------------------------------------------------------------------------------
 
-.globl ASM_PFX(Cp15IdCode)
-INTERWORK_FUNC(Cp15IdCode)
-.globl ASM_PFX(Cp15CacheInfo)
-INTERWORK_FUNC(Cp15CacheInfo)
-.globl ASM_PFX(ArmEnableInterrupts)
-INTERWORK_FUNC(ArmEnableInterrupts)
-.globl ASM_PFX(ArmDisableInterrupts)
-INTERWORK_FUNC(ArmDisableInterrupts)
-.globl ASM_PFX(ArmGetInterruptState)
-INTERWORK_FUNC(ArmGetInterruptState)
-.globl ASM_PFX(ArmEnableFiq)
-INTERWORK_FUNC(ArmEnableFiq)
-.globl ASM_PFX(ArmDisableFiq)
-INTERWORK_FUNC(ArmDisableFiq)
-.globl ASM_PFX(ArmGetFiqState)
-INTERWORK_FUNC(ArmGetFiqState)
-.globl ASM_PFX(ArmInvalidateTlb)
-INTERWORK_FUNC(ArmInvalidateTlb)
-.globl ASM_PFX(ArmSetTranslationTableBaseAddress)
-INTERWORK_FUNC(ArmSetTranslationTableBaseAddress)
-.globl ASM_PFX(ArmGetTranslationTableBaseAddress)
-INTERWORK_FUNC(ArmGetTranslationTableBaseAddress)
-.globl ASM_PFX(ArmSetDomainAccessControl)
-INTERWORK_FUNC(ArmSetDomainAccessControl)
-.globl ASM_PFX(ArmUpdateTranslationTableEntry)
-INTERWORK_FUNC(ArmUpdateTranslationTableEntry)
-.globl ASM_PFX(CPSRMaskInsert)
-INTERWORK_FUNC(CPSRMaskInsert)
-.globl ASM_PFX(CPSRRead)
-INTERWORK_FUNC(CPSRRead)
-.globl ASM_PFX(ReadCCSIDR)
-INTERWORK_FUNC(ReadCCSIDR)
-.globl ASM_PFX(ReadCLIDR)
-INTERWORK_FUNC(ReadCLIDR)
+#include <AsmMacroIoLib.h>
 
 .text
 .align 2
 
+GCC_ASM_EXPORT(Cp15IdCode)
+GCC_ASM_EXPORT(Cp15CacheInfo)
+GCC_ASM_EXPORT(ArmIsMPCore)
+GCC_ASM_EXPORT(ArmEnableAsynchronousAbort)
+GCC_ASM_EXPORT(ArmDisableAsynchronousAbort)
+GCC_ASM_EXPORT(ArmEnableIrq)
+GCC_ASM_EXPORT(ArmDisableIrq)
+GCC_ASM_EXPORT(ArmGetInterruptState)
+GCC_ASM_EXPORT(ArmEnableFiq)
+GCC_ASM_EXPORT(ArmDisableFiq)
+GCC_ASM_EXPORT(ArmEnableInterrupts)
+GCC_ASM_EXPORT(ArmDisableInterrupts)
+GCC_ASM_EXPORT(ArmGetFiqState)
+GCC_ASM_EXPORT(ArmInvalidateTlb)
+GCC_ASM_EXPORT(ArmSetTTBR0)
+GCC_ASM_EXPORT(ArmGetTTBR0BaseAddress)
+GCC_ASM_EXPORT(ArmSetDomainAccessControl)
+GCC_ASM_EXPORT(ArmUpdateTranslationTableEntry)
+GCC_ASM_EXPORT(CPSRMaskInsert)
+GCC_ASM_EXPORT(CPSRRead)
+GCC_ASM_EXPORT(ReadCCSIDR)
+GCC_ASM_EXPORT(ReadCLIDR)
+
+
+
 #------------------------------------------------------------------------------
 
 ASM_PFX(Cp15IdCode):
@@ -60,35 +52,67 @@ ASM_PFX(Cp15CacheInfo):
   mrc     p15,0,R0,c0,c0,1
   bx      LR
 
-ASM_PFX(ArmEnableInterrupts):
+ASM_PFX(ArmIsMPCore):
+  mrc     p15,0,R0,c0,c0,5
+  // Get Multiprocessing extension (bit31) & U bit (bit30)
+  and     R0, R0, #0xC0000000
+  // if bit30 == 0 then the processor is part of a multiprocessor system)
+  and     R0, R0, #0x80000000
+  bx      LR
+
+ASM_PFX(ArmEnableAsynchronousAbort):
+  cpsie   a
+  isb
+  bx      LR
+
+ASM_PFX(ArmDisableAsynchronousAbort):
+  cpsid   a
+  isb
+  bx      LR
+
+ASM_PFX(ArmEnableIrq):
   cpsie   i
-	bx      LR
+  isb
+  bx      LR
 
-ASM_PFX(ArmDisableInterrupts):
+ASM_PFX(ArmDisableIrq):
   cpsid   i
-	bx      LR
+  isb
+  bx      LR
 
 ASM_PFX(ArmGetInterruptState):
-	mrs     R0,CPSR
-	tst     R0,#0x80	    @Check if IRQ is enabled.
-	moveq   R0,#1
-	movne   R0,#0
-	bx      LR
+  mrs     R0,CPSR
+  tst     R0,#0x80      @Check if IRQ is enabled.
+  moveq   R0,#1
+  movne   R0,#0
+  bx      LR
 
 ASM_PFX(ArmEnableFiq):
   cpsie   f
-	bx      LR
+  isb
+  bx      LR
 
 ASM_PFX(ArmDisableFiq):
   cpsid   f
-	bx      LR
+  isb
+  bx      LR
+
+ASM_PFX(ArmEnableInterrupts):
+  cpsie   if
+  isb
+  bx      LR
+
+ASM_PFX(ArmDisableInterrupts):
+  cpsid   if
+  isb
+  bx      LR
 
 ASM_PFX(ArmGetFiqState):
-	mrs     R0,CPSR
-	tst     R0,#0x40	    @Check if FIQ is enabled.
-	moveq   R0,#1
-	movne   R0,#0
-	bx      LR
+  mrs     R0,CPSR
+  tst     R0,#0x40      @Check if FIQ is enabled.
+  moveq   R0,#1
+  movne   R0,#0
+  bx      LR
 
 ASM_PFX(ArmInvalidateTlb):
   mov     r0,#0
@@ -98,13 +122,15 @@ ASM_PFX(ArmInvalidateTlb):
   isb
   bx      lr
 
-ASM_PFX(ArmSetTranslationTableBaseAddress):
+ASM_PFX(ArmSetTTBR0):
   mcr     p15,0,r0,c2,c0,0
   isb
   bx      lr
 
-ASM_PFX(ArmGetTranslationTableBaseAddress):
+ASM_PFX(ArmGetTTBR0BaseAddress):
   mrc     p15,0,r0,c2,c0,0
+  LoadConstantToReg(0xFFFFC000, r1)
+  and     r0, r0, r1
   isb
   bx      lr
 
diff --git a/ArmPkg/Library/ArmLib/ArmV7/ArmLibSupport.asm b/ArmPkg/Library/ArmLib/ArmV7/ArmLibSupport.asm
index 08a2d900fd..7099ced8f4 100644
--- a/ArmPkg/Library/ArmLib/ArmV7/ArmLibSupport.asm
+++ b/ArmPkg/Library/ArmLib/ArmV7/ArmLibSupport.asm
@@ -15,15 +15,20 @@
 
     EXPORT  Cp15IdCode
     EXPORT  Cp15CacheInfo
-    EXPORT  ArmEnableInterrupts
-    EXPORT  ArmDisableInterrupts
+    EXPORT  ArmIsMPCore
+    EXPORT  ArmEnableAsynchronousAbort
+    EXPORT  ArmDisableAsynchronousAbort
+    EXPORT  ArmEnableIrq
+    EXPORT  ArmDisableIrq
     EXPORT  ArmGetInterruptState
     EXPORT  ArmEnableFiq
     EXPORT  ArmDisableFiq
+    EXPORT  ArmEnableInterrupts
+    EXPORT  ArmDisableInterrupts
     EXPORT  ArmGetFiqState
     EXPORT  ArmInvalidateTlb
-    EXPORT  ArmSetTranslationTableBaseAddress
-    EXPORT  ArmGetTranslationTableBaseAddress
+    EXPORT  ArmSetTTBR0
+    EXPORT  ArmGetTTBR0BaseAddress
     EXPORT  ArmSetDomainAccessControl
     EXPORT  ArmUpdateTranslationTableEntry
     EXPORT  CPSRMaskInsert
@@ -44,27 +49,59 @@ Cp15CacheInfo
   mrc     p15,0,R0,c0,c0,1
   bx      LR
 
-ArmEnableInterrupts
+ArmIsMPCore
+  mrc     p15,0,R0,c0,c0,5
+  // Get Multiprocessing extension (bit31) & U bit (bit30)
+  and     R0, R0, #0xC0000000
+  // if bit30 == 0 then the processor is part of a multiprocessor system)
+  and     R0, R0, #0x80000000
+  bx      LR
+
+ArmEnableAsynchronousAbort
+  cpsie   a
+  isb
+  bx      LR
+
+ArmDisableAsynchronousAbort
+  cpsid   a
+  isb
+  bx      LR
+
+ArmEnableIrq
   cpsie   i
+  isb
 	bx      LR
 
-ArmDisableInterrupts
+ArmDisableIrq
   cpsid   i
-	bx      LR
-
-ArmGetInterruptState
-	mrs     R0,CPSR
-	tst     R0,#0x80	    ;Check if IRQ is enabled.
-	moveq   R0,#1
-	movne   R0,#0
+  isb
 	bx      LR
 
 ArmEnableFiq
   cpsie   f
+  isb
 	bx      LR
 
 ArmDisableFiq
   cpsid   f
+  isb
+  bx      LR
+
+ArmEnableInterrupts
+  cpsie   if
+  isb
+  bx      LR
+
+ArmDisableInterrupts
+  cpsid   if
+  isb
+  bx      LR
+
+ArmGetInterruptState
+  mrs     R0,CPSR
+  tst     R0,#0x80	    ;Check if IRQ is enabled.
+  moveq   R0,#1
+  movne   R0,#0
 	bx      LR
 
 ArmGetFiqState
@@ -82,13 +119,15 @@ ArmInvalidateTlb
   isb
   bx      lr
 
-ArmSetTranslationTableBaseAddress
+ArmSetTTBR0
   mcr     p15,0,r0,c2,c0,0
   isb
   bx      lr
 
-ArmGetTranslationTableBaseAddress
+ArmGetTTBR0BaseAddress
   mrc     p15,0,r0,c2,c0,0
+  ldr	  r1, = 0xFFFFC000
+  and     r0, r0, r1
   isb
   bx      lr
 
@@ -150,7 +189,4 @@ ReadCLIDR
   mrc p15,1,r0,c0,c0,1 ; Read CP15 Cache Level ID Register
   bx  lr
   
-
 END
-
-
diff --git a/ArmPkg/Library/ArmLib/ArmV7/ArmV7Lib.c b/ArmPkg/Library/ArmLib/ArmV7/ArmV7Lib.c
index cad3c13ba4..4329bd7239 100644
--- a/ArmPkg/Library/ArmLib/ArmV7/ArmV7Lib.c
+++ b/ArmPkg/Library/ArmLib/ArmV7/ArmV7Lib.c
@@ -15,113 +15,10 @@
 #include <Chipset/ArmV7.h>
 #include <Library/ArmLib.h>
 #include <Library/BaseLib.h>
-#include <Library/BaseMemoryLib.h>
-#include <Library/MemoryAllocationLib.h>
+#include <Library/IoLib.h>
 #include "ArmV7Lib.h"
 #include "ArmLibPrivate.h"
 
-VOID
-FillTranslationTable (
-  IN  UINT32                        *TranslationTable,
-  IN  ARM_MEMORY_REGION_DESCRIPTOR  *MemoryRegion
-  )
-{
-  UINT32  *Entry;
-  UINTN   Sections;
-  UINTN   Index;
-  UINT32  Attributes;
-  UINT32  PhysicalBase = MemoryRegion->PhysicalBase;
-  
-  switch (MemoryRegion->Attributes) {
-    case ARM_MEMORY_REGION_ATTRIBUTE_WRITE_BACK:
-      Attributes = TT_DESCRIPTOR_SECTION_WRITE_BACK;
-      break;
-    case ARM_MEMORY_REGION_ATTRIBUTE_WRITE_THROUGH:
-      Attributes = TT_DESCRIPTOR_SECTION_WRITE_THROUGH;
-      break;
-    case ARM_MEMORY_REGION_ATTRIBUTE_DEVICE:
-      Attributes = TT_DESCRIPTOR_SECTION_DEVICE;
-      break;
-    case ARM_MEMORY_REGION_ATTRIBUTE_UNCACHED_UNBUFFERED:
-    default:
-      Attributes = TT_DESCRIPTOR_SECTION_UNCACHED;
-      break;
-  }
-  
-  Entry    = TRANSLATION_TABLE_ENTRY_FOR_VIRTUAL_ADDRESS(TranslationTable, MemoryRegion->VirtualBase);
-  Sections = MemoryRegion->Length / TT_DESCRIPTOR_SECTION_SIZE;
-  
-  for (Index = 0; Index < Sections; Index++) {
-    *Entry++     =  TT_DESCRIPTOR_SECTION_BASE_ADDRESS(PhysicalBase) | Attributes;
-    PhysicalBase += TT_DESCRIPTOR_SECTION_SIZE;
-  }
-}
-
-VOID
-EFIAPI
-ArmConfigureMmu (
-  IN  ARM_MEMORY_REGION_DESCRIPTOR  *MemoryTable,
-  OUT VOID                          **TranslationTableBase OPTIONAL,
-  OUT UINTN                         *TranslationTableSize  OPTIONAL
-  )
-{
-  VOID  *TranslationTable;
-
-  // Allocate pages for translation table.
-  TranslationTable = AllocatePages(EFI_SIZE_TO_PAGES(TRANSLATION_TABLE_SIZE + TRANSLATION_TABLE_ALIGNMENT));
-  TranslationTable = (VOID *)(((UINTN)TranslationTable + TRANSLATION_TABLE_ALIGNMENT_MASK) & ~TRANSLATION_TABLE_ALIGNMENT_MASK);
-
-  if (TranslationTableBase != NULL) {
-    *TranslationTableBase = TranslationTable;
-  }
-  
-  if (TranslationTableBase != NULL) {
-    *TranslationTableSize = TRANSLATION_TABLE_SIZE;
-  }
-
-  ZeroMem(TranslationTable, TRANSLATION_TABLE_SIZE);
-
-  ArmCleanInvalidateDataCache();
-  ArmInvalidateInstructionCache();
-  ArmInvalidateTlb();
-
-  ArmDisableDataCache();
-  ArmDisableInstructionCache();
-  ArmDisableMmu();
-
-  // Make sure nothing sneaked into the cache
-  ArmCleanInvalidateDataCache();
-  ArmInvalidateInstructionCache();
-
-  while (MemoryTable->Length != 0) {
-    FillTranslationTable(TranslationTable, MemoryTable);
-    MemoryTable++;
-  }
-
-  ArmSetTranslationTableBaseAddress(TranslationTable);
-    
-  ArmSetDomainAccessControl(DOMAIN_ACCESS_CONTROL_NONE(15) |
-                            DOMAIN_ACCESS_CONTROL_NONE(14) |
-                            DOMAIN_ACCESS_CONTROL_NONE(13) |
-                            DOMAIN_ACCESS_CONTROL_NONE(12) |
-                            DOMAIN_ACCESS_CONTROL_NONE(11) |
-                            DOMAIN_ACCESS_CONTROL_NONE(10) |
-                            DOMAIN_ACCESS_CONTROL_NONE( 9) |
-                            DOMAIN_ACCESS_CONTROL_NONE( 8) |
-                            DOMAIN_ACCESS_CONTROL_NONE( 7) |
-                            DOMAIN_ACCESS_CONTROL_NONE( 6) |
-                            DOMAIN_ACCESS_CONTROL_NONE( 5) |
-                            DOMAIN_ACCESS_CONTROL_NONE( 4) |
-                            DOMAIN_ACCESS_CONTROL_NONE( 3) |
-                            DOMAIN_ACCESS_CONTROL_NONE( 2) |
-                            DOMAIN_ACCESS_CONTROL_NONE( 1) |
-                            DOMAIN_ACCESS_CONTROL_MANAGER(0));
-    
-  ArmEnableInstructionCache();
-  ArmEnableDataCache();
-  ArmEnableMmu();
-}
-
 ARM_CACHE_TYPE
 EFIAPI
 ArmCacheType (
@@ -139,7 +36,7 @@ ArmCacheArchitecture (
 {
   UINT32 CLIDR = ReadCLIDR ();
 
-  return CLIDR; // BugBug Fix Me
+  return (ARM_CACHE_ARCHITECTURE)CLIDR; // BugBug Fix Me
 }
 
 BOOLEAN
@@ -173,7 +70,7 @@ ArmDataCacheSize (
   UINT32 LineSize;
   UINT32 CCSIDR = ReadCCSIDR (0);
   
-  LineSize      = (1 << (CCSIDR + 2));
+  LineSize      = (1 << ((CCSIDR & 0x7) + 2));
   Associativity = ((CCSIDR >> 3) & 0x3ff) + 1;
   NumSets       = ((CCSIDR >> 13) & 0x7fff) + 1;
 
@@ -245,7 +142,7 @@ ArmInstructionCacheSize (
   UINT32 LineSize;
   UINT32 CCSIDR = ReadCCSIDR (1);
   
-  LineSize      = (1 << (CCSIDR + 2));
+  LineSize      = (1 << ((CCSIDR & 0x7) + 2));
   Associativity = ((CCSIDR >> 3) & 0x3ff) + 1;
   NumSets       = ((CCSIDR >> 13) & 0x7fff) + 1;
 
@@ -299,6 +196,8 @@ ArmV7DataCacheOperation (
   UINTN     SavedInterruptState;
 
   SavedInterruptState = ArmGetInterruptState ();
+  ArmDisableInterrupts();
+  
 
   ArmV7AllDataCachesOperation (DataCacheOperation);
   
@@ -335,3 +234,14 @@ ArmCleanDataCache (
 {
   ArmV7DataCacheOperation (ArmCleanDataCacheEntryBySetWay);
 }
+
+VOID
+EFIAPI
+ArmSetAuxCrBit (
+  IN  UINT32    Bits
+  )
+{
+  UINT32 val = ArmReadAuxCr();
+  val |= Bits;
+  ArmWriteAuxCr(val);
+}
diff --git a/ArmPkg/Library/ArmLib/ArmV7/ArmV7Lib.h b/ArmPkg/Library/ArmLib/ArmV7/ArmV7Lib.h
index 2e8c39b3b3..7eaeb15661 100644
--- a/ArmPkg/Library/ArmLib/ArmV7/ArmV7Lib.h
+++ b/ArmPkg/Library/ArmLib/ArmV7/ArmV7Lib.h
@@ -40,5 +40,41 @@ ArmCleanInvalidateDataCacheEntryBySetWay (
   IN  UINT32   SetWayFormat
   );
 
+VOID
+EFIAPI
+ArmEnableAsynchronousAbort (
+  VOID
+  );
+
+UINTN
+EFIAPI
+ArmDisableAsynchronousAbort (
+  VOID
+  );
+
+VOID
+EFIAPI
+ArmEnableIrq (
+  VOID
+  );
+
+UINTN
+EFIAPI
+ArmDisableIrq (
+  VOID
+  );
+
+VOID
+EFIAPI
+ArmEnableFiq (
+  VOID
+  );
+
+UINTN
+EFIAPI
+ArmDisableFiq (
+  VOID
+  );
+
 #endif // __ARM_V7_LIB_H__
 
diff --git a/ArmPkg/Library/ArmLib/ArmV7/ArmV7Lib.inf b/ArmPkg/Library/ArmLib/ArmV7/ArmV7Lib.inf
index bbd2576460..2452c28734 100644
--- a/ArmPkg/Library/ArmLib/ArmV7/ArmV7Lib.inf
+++ b/ArmPkg/Library/ArmLib/ArmV7/ArmV7Lib.inf
@@ -30,6 +30,7 @@
   ArmV7Support.asm  | RVCT

 

   ArmV7Lib.c

+  ArmV7Mmu.c

 

 [Packages]

   ArmPkg/ArmPkg.dec

diff --git a/ArmPkg/Library/ArmLib/ArmV7/ArmV7LibPrePi.inf b/ArmPkg/Library/ArmLib/ArmV7/ArmV7LibPrePi.inf
index cbdef6cfb9..f97b5a4cee 100644
--- a/ArmPkg/Library/ArmLib/ArmV7/ArmV7LibPrePi.inf
+++ b/ArmPkg/Library/ArmLib/ArmV7/ArmV7LibPrePi.inf
@@ -30,6 +30,7 @@
   ArmV7Support.asm  | RVCT

 

   ArmV7Lib.c

+  ArmV7Mmu.c

 

 [Packages]

   ArmPkg/ArmPkg.dec

diff --git a/ArmPkg/Library/ArmLib/ArmV7/ArmV7LibSec.inf b/ArmPkg/Library/ArmLib/ArmV7/ArmV7LibSec.inf
new file mode 100644
index 0000000000..73bf09a5a3
--- /dev/null
+++ b/ArmPkg/Library/ArmLib/ArmV7/ArmV7LibSec.inf
@@ -0,0 +1,40 @@
+#/* @file

+#  Copyright (c) 2011, 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                      = ArmV7Lib

+  FILE_GUID                      = 411cdfd8-f964-4b9d-a3e3-1719a9c15559

+  MODULE_TYPE                    = BASE

+  VERSION_STRING                 = 1.0

+  LIBRARY_CLASS                  = ArmLib

+

+[Sources.common]

+  ArmLibSupport.S    | GCC

+  ArmLibSupport.asm  | RVCT

+  ../Common/ArmLib.c

+  

+  ArmV7Support.S    | GCC

+  ArmV7Support.asm  | RVCT

+

+  ArmV7Lib.c

+

+[Packages]

+  ArmPkg/ArmPkg.dec

+  MdePkg/MdePkg.dec

+

+[Protocols]

+  gEfiCpuArchProtocolGuid

+

+[FixedPcd]

+  gArmTokenSpaceGuid.PcdArmCacheOperationThreshold

diff --git a/ArmPkg/Library/ArmLib/ArmV7/ArmV7MPCore.c b/ArmPkg/Library/ArmLib/ArmV7/ArmV7MPCore.c
new file mode 100644
index 0000000000..65c09a1a35
--- /dev/null
+++ b/ArmPkg/Library/ArmLib/ArmV7/ArmV7MPCore.c
@@ -0,0 +1,57 @@
+/** @file
+*
+*  Copyright (c) 2011, 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 <Uefi.h>
+#include <Chipset/ArmV7.h>
+#include <Library/ArmLib.h>
+#include <Library/BaseLib.h>
+#include <Library/IoLib.h>
+#include "ArmV7Lib.h"
+#include "ArmLibPrivate.h"
+
+VOID
+EFIAPI
+ArmSetupSmpNonSecure (
+  IN  UINTN         CoreId
+  )
+{
+    INTN          scu_base;
+
+    ArmSetAuxCrBit (A9_FEATURE_SMP);
+
+    if (CoreId == 0) {
+        scu_base = ArmGetScuBaseAddress();
+
+        // Allow NS access to SCU register
+        MmioOr32(scu_base + SCU_SACR_OFFSET, 0xf);  
+        // Allow NS access to Private Peripherals
+        MmioOr32(scu_base + SCU_SSACR_OFFSET, 0xfff);
+    }
+}
+
+VOID
+EFIAPI
+ArmInvalidScu (
+  VOID
+  )
+{
+    INTN          scu_base;
+
+    scu_base = ArmGetScuBaseAddress();
+
+    /* Invalidate all: write -1 to SCU Invalidate All register */
+    MmioWrite32(scu_base + SCU_INVALL_OFFSET, 0xffffffff);
+    /* Enable SCU */
+    MmioWrite32(scu_base + SCU_CONTROL_OFFSET, 0x1);
+}
diff --git a/ArmPkg/Library/ArmLib/ArmV7/ArmV7MPCoreHelper.S b/ArmPkg/Library/ArmLib/ArmV7/ArmV7MPCoreHelper.S
new file mode 100644
index 0000000000..0636897b87
--- /dev/null
+++ b/ArmPkg/Library/ArmLib/ArmV7/ArmV7MPCoreHelper.S
@@ -0,0 +1,48 @@
+#

+#  Copyright (c) 2011, 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 <AsmMacroIoLib.h>

+#include <Base.h>

+#include <Library/PcdLib.h>

+#include <Chipset/ArmV7.h>

+#include <AutoGen.h>

+#.include AsmMacroIoLib.inc

+

+.text

+.align 2

+

+GCC_ASM_EXPORT(ArmIsScuEnable)

+GCC_ASM_EXPORT(ArmGetScuBaseAddress)

+

+# IN None

+# OUT r0 = SCU Base Address

+ASM_PFX(ArmGetScuBaseAddress):

+  # Read Configuration Base Address Register. ArmCBar cannot be called to get

+  # the Configuration BAR as a stack is not necessary setup. The SCU is at the

+  # offset 0x0000 from the Private Memory Region.

+  mrc   p15, 4, r0, c15, c0, 0

+  bx	lr

+

+# IN  None

+# OUT r1 = SCU enabled (boolean)

+ASM_PFX(ArmIsScuEnable):

+  # Read Configuration Base Address Register. ArmCBar cannot be called to get

+  # the Configuration BAR as a stack is not necessary setup. The SCU is at the

+  # offset 0x0000 from the Private Memory Region.

+  mrc   p15, 4, r0, c15, c0, 0

+  add   r1, r0, #SCU_CONTROL_OFFSET

+  ldr   r1, [r1]

+  and   r1, r1, #1

+  bx    lr

+

+ASM_FUNCTION_REMOVE_IF_UNREFERENCED 

diff --git a/ArmPkg/Library/ArmLib/ArmV7/ArmV7MPCoreHelper.asm b/ArmPkg/Library/ArmLib/ArmV7/ArmV7MPCoreHelper.asm
new file mode 100644
index 0000000000..08528fc4ec
--- /dev/null
+++ b/ArmPkg/Library/ArmLib/ArmV7/ArmV7MPCoreHelper.asm
@@ -0,0 +1,48 @@
+//

+//  Copyright (c) 2011, 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 <AsmMacroIoLib.h>

+#include <Base.h>

+#include <Library/PcdLib.h>

+#include <Chipset/ArmV7.h>

+#include <AutoGen.h>

+

+  INCLUDE AsmMacroIoLib.inc

+

+  EXPORT    ArmIsScuEnable

+  EXPORT    ArmGetScuBaseAddress

+    

+  AREA ArmV7MPCore, CODE, READONLY

+

+// IN None

+// OUT r0 = SCU Base Address

+ArmGetScuBaseAddress

+  // Read Configuration Base Address Register. ArmCBar cannot be called to get

+  // the Configuration BAR as a stack is not necessary setup. The SCU is at the

+  // offset 0x0000 from the Private Memory Region.

+  mrc   p15, 4, r0, c15, c0, 0

+  bx	lr

+

+// IN  None

+// OUT r1 = SCU enabled (boolean)

+ArmIsScuEnable

+  // Read Configuration Base Address Register. ArmCBar cannot be called to get

+  // the Configuration BAR as a stack is not necessary setup. The SCU is at the

+  // offset 0x0000 from the Private Memory Region.

+  mrc   p15, 4, r0, c15, c0, 0

+  add   r1, r0, #SCU_CONTROL_OFFSET

+  ldr   r1, [r1]

+  and   r1, r1, #1

+  bx    lr

+

+  END

diff --git a/ArmPkg/Library/ArmLib/ArmV7/ArmV7MPCoreLib.inf b/ArmPkg/Library/ArmLib/ArmV7/ArmV7MPCoreLib.inf
new file mode 100644
index 0000000000..c847d89eca
--- /dev/null
+++ b/ArmPkg/Library/ArmLib/ArmV7/ArmV7MPCoreLib.inf
@@ -0,0 +1,51 @@
+#/** @file

+# Helper Library for ARMv7 MPCore architecture

+

+#  Copyright (c) 2011, 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                      = ArmV7Lib

+  FILE_GUID                      = 411cdfd8-f964-4b9d-a3e3-1719a9c15559

+  MODULE_TYPE                    = DXE_DRIVER

+  VERSION_STRING                 = 1.0

+  LIBRARY_CLASS                  = ArmLib

+

+[Sources.common]

+  ArmLibSupport.S    | GCC

+  ArmLibSupport.asm  | RVCT

+  ../Common/ArmLib.c

+  

+  ArmV7Support.S    | GCC

+  ArmV7Support.asm  | RVCT

+

+  ArmV7Lib.c

+  ArmV7Mmu.c

+

+  ArmV7MPCore.c

+  ArmV7MPCoreHelper.S   | GCC

+  ArmV7MPCoreHelper.asm   | RVCT

+

+[Packages]

+  ArmPkg/ArmPkg.dec

+  MdePkg/MdePkg.dec

+

+[LibraryClasses]

+  MemoryAllocationLib

+

+[Protocols]

+  gEfiCpuArchProtocolGuid

+

+[FixedPcd]

+  gArmTokenSpaceGuid.PcdArmCacheOperationThreshold

diff --git a/ArmPkg/Library/ArmLib/ArmV7/ArmV7MPCoreLibPrePi.inf b/ArmPkg/Library/ArmLib/ArmV7/ArmV7MPCoreLibPrePi.inf
new file mode 100644
index 0000000000..e1de26f426
--- /dev/null
+++ b/ArmPkg/Library/ArmLib/ArmV7/ArmV7MPCoreLibPrePi.inf
@@ -0,0 +1,49 @@
+#/** @file

+#

+#  Copyright (c) 2011, 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                      = ArmV7LibPrePi

+  FILE_GUID                      = A150FA0C-F4E8-4207-9BEB-CD6DFB430D73

+  MODULE_TYPE                    = BASE

+  VERSION_STRING                 = 1.0

+  LIBRARY_CLASS                  = ArmLib

+

+[Sources.common]

+  ArmLibSupport.S    | GCC

+  ArmLibSupport.asm  | RVCT

+  ../Common/ArmLib.c

+  

+  ArmV7Support.S    | GCC

+  ArmV7Support.asm  | RVCT

+

+  ArmV7Lib.c

+  ArmV7Mmu.c

+

+  ArmV7MPCore.c

+  ArmV7MPCoreHelper.S     | GCC

+  ArmV7MPCoreHelper.asm   | RVCT

+

+[Packages]

+  ArmPkg/ArmPkg.dec

+  MdePkg/MdePkg.dec

+

+[LibraryClasses]

+  PrePiLib

+  

+[Protocols]

+  gEfiCpuArchProtocolGuid

+

+[FixedPcd]

+  gArmTokenSpaceGuid.PcdArmCacheOperationThreshold

diff --git a/ArmPkg/Library/ArmLib/ArmV7/ArmV7MPCoreLibSec.inf b/ArmPkg/Library/ArmLib/ArmV7/ArmV7MPCoreLibSec.inf
new file mode 100644
index 0000000000..05c8d362f1
--- /dev/null
+++ b/ArmPkg/Library/ArmLib/ArmV7/ArmV7MPCoreLibSec.inf
@@ -0,0 +1,43 @@
+#/* @file

+#  Copyright (c) 2011, 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                      = ArmV7Lib

+  FILE_GUID                      = 411cdfd8-f964-4b9d-a3e3-1719a9c15559

+  MODULE_TYPE                    = BASE

+  VERSION_STRING                 = 1.0

+  LIBRARY_CLASS                  = ArmLib

+

+[Sources.common]

+  ArmLibSupport.S    | GCC

+  ArmLibSupport.asm  | RVCT

+  ../Common/ArmLib.c

+  

+  ArmV7Support.S    | GCC

+  ArmV7Support.asm  | RVCT

+

+  ArmV7Lib.c

+  ArmV7MPCore.c

+  ArmV7MPCoreHelper.S   | GCC

+  ArmV7MPCoreHelper.asm   | RVCT

+

+[Packages]

+  ArmPkg/ArmPkg.dec

+  MdePkg/MdePkg.dec

+

+[Protocols]

+  gEfiCpuArchProtocolGuid

+

+[FixedPcd]

+  gArmTokenSpaceGuid.PcdArmCacheOperationThreshold

diff --git a/ArmPkg/Library/ArmLib/ArmV7/ArmV7Mmu.c b/ArmPkg/Library/ArmLib/ArmV7/ArmV7Mmu.c
new file mode 100644
index 0000000000..9bb3c2690f
--- /dev/null
+++ b/ArmPkg/Library/ArmLib/ArmV7/ArmV7Mmu.c
@@ -0,0 +1,162 @@
+/** @file

+*  File managing the MMU for ARMv7 architecture

+*

+*  Copyright (c) 2011, 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 <Uefi.h> 

+#include <Chipset/ArmV7.h>

+#include <Library/BaseMemoryLib.h>

+#include <Library/MemoryAllocationLib.h>

+#include <Library/ArmLib.h>

+#include <Library/BaseLib.h>

+#include "ArmV7Lib.h"

+#include "ArmLibPrivate.h"

+

+VOID

+FillTranslationTable (

+  IN  UINT32                        *TranslationTable,

+  IN  ARM_MEMORY_REGION_DESCRIPTOR  *MemoryRegion

+  )

+{

+  UINT32  *Entry;

+  UINTN   Sections;

+  UINTN   Index;

+  UINT32  Attributes;

+  UINT32  PhysicalBase = MemoryRegion->PhysicalBase;

+  

+  switch (MemoryRegion->Attributes) {

+    case ARM_MEMORY_REGION_ATTRIBUTE_WRITE_BACK:

+      Attributes = TT_DESCRIPTOR_SECTION_WRITE_BACK(0);

+      break;

+    case ARM_MEMORY_REGION_ATTRIBUTE_WRITE_THROUGH:

+      Attributes = TT_DESCRIPTOR_SECTION_WRITE_THROUGH(0);

+      break;

+    case ARM_MEMORY_REGION_ATTRIBUTE_DEVICE:

+      Attributes = TT_DESCRIPTOR_SECTION_DEVICE(0);

+      break;

+    case ARM_MEMORY_REGION_ATTRIBUTE_UNCACHED_UNBUFFERED:

+      Attributes = TT_DESCRIPTOR_SECTION_UNCACHED(0);

+      break;

+    case ARM_MEMORY_REGION_ATTRIBUTE_SECURE_WRITE_BACK:

+      Attributes = TT_DESCRIPTOR_SECTION_WRITE_BACK(1);

+      break;

+    case ARM_MEMORY_REGION_ATTRIBUTE_SECURE_WRITE_THROUGH:

+      Attributes = TT_DESCRIPTOR_SECTION_WRITE_THROUGH(1);

+      break;

+    case ARM_MEMORY_REGION_ATTRIBUTE_SECURE_DEVICE:

+      Attributes = TT_DESCRIPTOR_SECTION_DEVICE(1);

+      break;

+    case ARM_MEMORY_REGION_ATTRIBUTE_SECURE_UNCACHED_UNBUFFERED:

+      Attributes = TT_DESCRIPTOR_SECTION_UNCACHED(1);

+      break;

+    default:

+      Attributes = TT_DESCRIPTOR_SECTION_UNCACHED(0);

+      break;

+  }

+  

+  Entry    = TRANSLATION_TABLE_ENTRY_FOR_VIRTUAL_ADDRESS(TranslationTable, MemoryRegion->VirtualBase);

+  Sections = MemoryRegion->Length / TT_DESCRIPTOR_SECTION_SIZE;

+  

+  for (Index = 0; Index < Sections; Index++) {

+    *Entry++     =  TT_DESCRIPTOR_SECTION_BASE_ADDRESS(PhysicalBase) | Attributes;

+    PhysicalBase += TT_DESCRIPTOR_SECTION_SIZE;

+  }

+}

+

+VOID

+EFIAPI

+ArmConfigureMmu (

+  IN  ARM_MEMORY_REGION_DESCRIPTOR  *MemoryTable,

+  OUT VOID                          **TranslationTableBase OPTIONAL,

+  OUT UINTN                         *TranslationTableSize  OPTIONAL

+  )

+{

+  UINTN                         TranslationTable;

+  ARM_MEMORY_REGION_ATTRIBUTES  TranslationTableAttribute;

+  UINT32                        TTBRAttributes;

+

+  // Allocate pages for translation table.

+  TranslationTable = (UINTN)AllocatePages(EFI_SIZE_TO_PAGES(TRANSLATION_TABLE_SECTION_SIZE + TRANSLATION_TABLE_SECTION_ALIGNMENT));

+  TranslationTable = ((UINTN)TranslationTable + TRANSLATION_TABLE_SECTION_ALIGNMENT_MASK) & ~TRANSLATION_TABLE_SECTION_ALIGNMENT_MASK;

+

+  if (TranslationTableBase != NULL) {

+    *TranslationTableBase = (VOID *)TranslationTable;

+  }

+  

+  if (TranslationTableBase != NULL) {

+    *TranslationTableSize = TRANSLATION_TABLE_SECTION_SIZE;

+  }

+

+  ZeroMem ((VOID *)TranslationTable, TRANSLATION_TABLE_SECTION_SIZE);

+

+  ArmCleanInvalidateDataCache();

+  ArmInvalidateInstructionCache();

+  ArmInvalidateTlb();

+

+  ArmDisableDataCache();

+  ArmDisableInstructionCache();

+  ArmDisableMmu();

+

+  // Make sure nothing sneaked into the cache

+  ArmCleanInvalidateDataCache();

+  ArmInvalidateInstructionCache();

+

+  TranslationTableAttribute = 0;

+  while (MemoryTable->Length != 0) {

+    // Find the memory attribute for the Translation Table

+    if ((TranslationTable >= MemoryTable->PhysicalBase) && (TranslationTable < MemoryTable->PhysicalBase + MemoryTable->Length)) {

+      TranslationTableAttribute = MemoryTable->Attributes;

+    }

+

+    FillTranslationTable ((VOID *)TranslationTable, MemoryTable);

+    MemoryTable++;

+  }

+

+  // Translate the Memory Attributes into Translation Table Register Attributes

+  if ((TranslationTableAttribute == ARM_MEMORY_REGION_ATTRIBUTE_UNCACHED_UNBUFFERED) || 

+      (TranslationTableAttribute == ARM_MEMORY_REGION_ATTRIBUTE_SECURE_UNCACHED_UNBUFFERED)) {

+    TTBRAttributes = TTBR_NON_CACHEABLE;

+  } else if ((TranslationTableAttribute == ARM_MEMORY_REGION_ATTRIBUTE_WRITE_BACK) || 

+      (TranslationTableAttribute == ARM_MEMORY_REGION_ATTRIBUTE_SECURE_WRITE_BACK)) {

+    TTBRAttributes = TTBR_WRITE_BACK_ALLOC;

+  } else if ((TranslationTableAttribute == ARM_MEMORY_REGION_ATTRIBUTE_WRITE_THROUGH) || 

+      (TranslationTableAttribute == ARM_MEMORY_REGION_ATTRIBUTE_SECURE_WRITE_THROUGH)) {

+    TTBRAttributes = TTBR_WRITE_THROUGH_NO_ALLOC;

+  } else {

+    //TODO: We should raise an error here

+    TTBRAttributes = TTBR_NON_CACHEABLE;

+  }

+

+  ArmSetTTBR0 ((VOID *)(UINTN)((TranslationTable & 0xFFFFC000) | (TTBRAttributes & 0x7F)));

+    

+  ArmSetDomainAccessControl (DOMAIN_ACCESS_CONTROL_NONE(15) |

+                             DOMAIN_ACCESS_CONTROL_NONE(14) |

+                             DOMAIN_ACCESS_CONTROL_NONE(13) |

+                             DOMAIN_ACCESS_CONTROL_NONE(12) |

+                             DOMAIN_ACCESS_CONTROL_NONE(11) |

+                             DOMAIN_ACCESS_CONTROL_NONE(10) |

+                             DOMAIN_ACCESS_CONTROL_NONE( 9) |

+                             DOMAIN_ACCESS_CONTROL_NONE( 8) |

+                             DOMAIN_ACCESS_CONTROL_NONE( 7) |

+                             DOMAIN_ACCESS_CONTROL_NONE( 6) |

+                             DOMAIN_ACCESS_CONTROL_NONE( 5) |

+                             DOMAIN_ACCESS_CONTROL_NONE( 4) |

+                             DOMAIN_ACCESS_CONTROL_NONE( 3) |

+                             DOMAIN_ACCESS_CONTROL_NONE( 2) |

+                             DOMAIN_ACCESS_CONTROL_NONE( 1) |

+                             DOMAIN_ACCESS_CONTROL_MANAGER(0));

+    

+  ArmEnableInstructionCache();

+  ArmEnableDataCache();

+  ArmEnableMmu();

+}

diff --git a/ArmPkg/Library/ArmLib/ArmV7/ArmV7Support.S b/ArmPkg/Library/ArmLib/ArmV7/ArmV7Support.S
index 991de89a96..cfbb8f545c 100644
--- a/ArmPkg/Library/ArmLib/ArmV7/ArmV7Support.S
+++ b/ArmPkg/Library/ArmLib/ArmV7/ArmV7Support.S
@@ -12,55 +12,51 @@
 #
 #------------------------------------------------------------------------------
 
-.globl ASM_PFX(ArmInvalidateInstructionCache)
-INTERWORK_FUNC(ArmInvalidateInstructionCache)
-.globl ASM_PFX(ArmInvalidateDataCacheEntryByMVA)
-INTERWORK_FUNC(ArmInvalidateDataCacheEntryByMVA)
-.globl ASM_PFX(ArmCleanDataCacheEntryByMVA)
-INTERWORK_FUNC(ArmCleanDataCacheEntryByMVA)
-.globl ASM_PFX(ArmCleanInvalidateDataCacheEntryByMVA)
-INTERWORK_FUNC(ArmCleanInvalidateDataCacheEntryByMVA)
-.globl ASM_PFX(ArmInvalidateDataCacheEntryBySetWay)
-INTERWORK_FUNC(ArmInvalidateDataCacheEntryBySetWay)
-.globl ASM_PFX(ArmCleanDataCacheEntryBySetWay)
-INTERWORK_FUNC(ArmCleanDataCacheEntryBySetWay)
-.globl ASM_PFX(ArmCleanInvalidateDataCacheEntryBySetWay)
-INTERWORK_FUNC(ArmCleanInvalidateDataCacheEntryBySetWay)
-.globl ASM_PFX(ArmDrainWriteBuffer)
-INTERWORK_FUNC(ArmDrainWriteBuffer)
-.globl ASM_PFX(ArmEnableMmu)
-INTERWORK_FUNC(ArmEnableMmu)
-.globl ASM_PFX(ArmDisableMmu)
-INTERWORK_FUNC(ArmDisableMmu)
-.globl ASM_PFX(ArmMmuEnabled)
-INTERWORK_FUNC(ArmMmuEnabled)
-.globl ASM_PFX(ArmEnableDataCache)
-INTERWORK_FUNC(ArmEnableDataCache)
-.globl ASM_PFX(ArmDisableDataCache)
-INTERWORK_FUNC(ArmDisableDataCache)
-.globl ASM_PFX(ArmEnableInstructionCache)
-INTERWORK_FUNC(ArmEnableInstructionCache)
-.globl ASM_PFX(ArmDisableInstructionCache)
-INTERWORK_FUNC(ArmDisableInstructionCache)
-.globl ASM_PFX(ArmEnableBranchPrediction)
-INTERWORK_FUNC(ArmEnableBranchPrediction)
-.globl ASM_PFX(ArmDisableBranchPrediction)
-INTERWORK_FUNC(ArmDisableBranchPrediction)
-.globl ASM_PFX(ArmV7AllDataCachesOperation)
-INTERWORK_FUNC(ArmV7AllDataCachesOperation)
-.globl ASM_PFX(ArmDataMemoryBarrier)
-INTERWORK_FUNC(ArmDataMemoryBarrier)
-.globl ASM_PFX(ArmDataSyncronizationBarrier) 
-INTERWORK_FUNC(ArmDataSyncronizationBarrier)
-.globl ASM_PFX(ArmInstructionSynchronizationBarrier)
-INTERWORK_FUNC(ArmInstructionSynchronizationBarrier)
-
 .text
 .align 2
 
+GCC_ASM_EXPORT (ArmInvalidateInstructionCache)
+GCC_ASM_EXPORT (ArmInvalidateDataCacheEntryByMVA)
+GCC_ASM_EXPORT (ArmCleanDataCacheEntryByMVA)
+GCC_ASM_EXPORT (ArmCleanInvalidateDataCacheEntryByMVA)
+GCC_ASM_EXPORT (ArmInvalidateDataCacheEntryBySetWay)
+GCC_ASM_EXPORT (ArmCleanDataCacheEntryBySetWay)
+GCC_ASM_EXPORT (ArmCleanInvalidateDataCacheEntryBySetWay)
+GCC_ASM_EXPORT (ArmDrainWriteBuffer)
+GCC_ASM_EXPORT (ArmEnableMmu)
+GCC_ASM_EXPORT (ArmDisableMmu)
+GCC_ASM_EXPORT (ArmDisableCachesAndMmu)
+GCC_ASM_EXPORT (ArmMmuEnabled)
+GCC_ASM_EXPORT (ArmEnableDataCache)
+GCC_ASM_EXPORT (ArmDisableDataCache)
+GCC_ASM_EXPORT (ArmEnableInstructionCache)
+GCC_ASM_EXPORT (ArmDisableInstructionCache)
+GCC_ASM_EXPORT (ArmEnableSWPInstruction)
+GCC_ASM_EXPORT (ArmEnableBranchPrediction)
+GCC_ASM_EXPORT (ArmDisableBranchPrediction)
+GCC_ASM_EXPORT (ArmV7AllDataCachesOperation)
+GCC_ASM_EXPORT (ArmDataMemoryBarrier)
+GCC_ASM_EXPORT (ArmDataSyncronizationBarrier)
+GCC_ASM_EXPORT (ArmInstructionSynchronizationBarrier)
+GCC_ASM_EXPORT (ArmWriteNsacr)
+GCC_ASM_EXPORT (ArmWriteScr)
+GCC_ASM_EXPORT (ArmWriteVMBar)
+GCC_ASM_EXPORT (ArmWriteVBar)
+GCC_ASM_EXPORT (ArmWriteCPACR)
+GCC_ASM_EXPORT (ArmEnableVFP)
+GCC_ASM_EXPORT (ArmCallWFI)
+GCC_ASM_EXPORT (ArmWriteAuxCr)
+GCC_ASM_EXPORT (ArmReadAuxCr)
+GCC_ASM_EXPORT (ArmReadCbar)
+GCC_ASM_EXPORT (ArmInvalidateInstructionAndDataTlb)
+GCC_ASM_EXPORT (ArmReadMpidr)
+
 .set DC_ON, (0x1<<2)
 .set IC_ON, (0x1<<12)
-
+.set CTRL_M_BIT,  (1 << 0)
+.set CTRL_C_BIT,  (1 << 2)
+.set CTRL_B_BIT,  (1 << 7)
+.set CTRL_I_BIT,  (1 << 12)
 
 
 ASM_PFX(ArmInvalidateDataCacheEntryByMVA):
@@ -69,7 +65,6 @@ ASM_PFX(ArmInvalidateDataCacheEntryByMVA):
   isb
   bx      lr
 
-
 ASM_PFX(ArmCleanDataCacheEntryByMVA):
   mcr     p15, 0, r0, c7, c10, 1  @clean single data cache line     
   dsb
@@ -104,7 +99,6 @@ ASM_PFX(ArmCleanDataCacheEntryBySetWay):
   isb
   bx      lr
 
-
 ASM_PFX(ArmInvalidateInstructionCache):
   mcr     p15,0,R0,c7,c5,0      @Invalidate entire instruction cache
   dsb
@@ -119,10 +113,6 @@ ASM_PFX(ArmEnableMmu):
   isb
   bx      LR
 
-ASM_PFX(ArmMmuEnabled):
-  mrc     p15,0,R0,c1,c0,0
-  and     R0,R0,#1
-  bx      LR
 
 ASM_PFX(ArmDisableMmu):
   mrc     p15,0,R0,c1,c0,0
@@ -135,6 +125,21 @@ ASM_PFX(ArmDisableMmu):
   isb
   bx      LR
 
+ASM_PFX(ArmDisableCachesAndMmu):
+  mrc   p15, 0, r0, c1, c0, 0           @ Get control register
+  bic   r0, r0, #CTRL_M_BIT             @ Disable MMU
+  bic   r0, r0, #CTRL_C_BIT             @ Disable D Cache
+  bic   r0, r0, #CTRL_I_BIT             @ Disable I Cache
+  mcr   p15, 0, r0, c1, c0, 0           @ Write control register
+  dsb
+  isb
+  bx      LR
+
+ASM_PFX(ArmMmuEnabled):
+  mrc     p15,0,R0,c1,c0,0
+  and     R0,R0,#1
+  bx      LR  
+
 ASM_PFX(ArmEnableDataCache):
   ldr     R1,=DC_ON
   mrc     p15,0,R0,c1,c0,0      @Read control register configuration data
@@ -171,6 +176,13 @@ ASM_PFX(ArmDisableInstructionCache):
   isb
   bx      LR
 
+ASM_PFX(ArmEnableSWPInstruction):
+  mrc     p15, 0, r0, c1, c0, 0
+  orr     r0, r0, #0x00000400
+  mcr     p15, 0, r0, c1, c0, 0
+  isb
+  bx      LR
+
 ASM_PFX(ArmEnableBranchPrediction):
   mrc     p15, 0, r0, c1, c0, 0
   orr     r0, r0, #0x00000800
@@ -254,5 +266,59 @@ ASM_PFX(ArmInstructionSynchronizationBarrier):
   isb
   bx      LR
  
+ASM_PFX(ArmWriteNsacr):
+  mcr     p15, 0, r0, c1, c1, 2
+  bx      lr
+
+ASM_PFX(ArmWriteScr):
+  mcr     p15, 0, r0, c1, c1, 0
+  bx      lr
+
+ASM_PFX(ArmWriteAuxCr):
+  mcr     p15, 0, r0, c1, c0, 1
+  bx      lr
+
+ASM_PFX(ArmReadAuxCr):
+  mrc     p15, 0, r0, c1, c0, 1
+  bx      lr  
+
+ASM_PFX(ArmWriteVMBar):
+  mcr     p15, 0, r0, c12, c0, 1
+  bx      lr
+
+ASM_PFX(ArmWriteVBar):
+  mcr     p15, 0, r0, c12, c0, 0 
+  bx      lr
+
+ASM_PFX(ArmWriteCPACR):
+  mcr     p15, 0, r0, c1, c0, 2
+  bx      lr
+
+ASM_PFX(ArmEnableVFP):
+  // Enable VFP registers
+  mrc     p15, 0, r0, c1, c0, 2
+  orr     r0, r0, #0x00f00000   // Enable VPF access (V* instructions)
+  mcr     p15, 0, r0, c1, c0, 2
+  mov     r0, #0x40000000       // Set EN bit in FPEXC
+  mcr     p10,#0x7,r0,c8,c0,#0  // msr     FPEXC,r0 in ARM assembly
+  bx      lr
+
+ASM_PFX(ArmCallWFI):
+  wfi
+  bx      lr
+
+//Note: Return 0 in Uniprocessor implementation
+ASM_PFX(ArmReadCbar):
+  mrc     p15, 4, r0, c15, c0, 0	//Read Configuration Base Address Register
+  bx      lr
+
+ASM_PFX(ArmInvalidateInstructionAndDataTlb):
+  mcr     p15, 0, r0, c8, c7, 0      @ Invalidate Inst TLB and Data TLB
+  dsb
+  bx lr
+
+ASM_PFX(ArmReadMpidr):
+  mrc     p15, 0, r0, c0, c0, 5	     @ read MPIDR
+  bx      lr
 
 ASM_FUNCTION_REMOVE_IF_UNREFERENCED
diff --git a/ArmPkg/Library/ArmLib/ArmV7/ArmV7Support.asm b/ArmPkg/Library/ArmLib/ArmV7/ArmV7Support.asm
index 8758b80ba5..7b4ca425bd 100644
--- a/ArmPkg/Library/ArmLib/ArmV7/ArmV7Support.asm
+++ b/ArmPkg/Library/ArmLib/ArmV7/ArmV7Support.asm
@@ -22,24 +22,42 @@
     EXPORT  ArmDrainWriteBuffer
     EXPORT  ArmEnableMmu
     EXPORT  ArmDisableMmu
+    EXPORT  ArmDisableCachesAndMmu
     EXPORT  ArmMmuEnabled
     EXPORT  ArmEnableDataCache
     EXPORT  ArmDisableDataCache
     EXPORT  ArmEnableInstructionCache
     EXPORT  ArmDisableInstructionCache
+    EXPORT  ArmEnableSWPInstruction
     EXPORT  ArmEnableBranchPrediction
     EXPORT  ArmDisableBranchPrediction
     EXPORT  ArmV7AllDataCachesOperation
     EXPORT  ArmDataMemoryBarrier
     EXPORT  ArmDataSyncronizationBarrier
     EXPORT  ArmInstructionSynchronizationBarrier
+    EXPORT  ArmWriteNsacr
+    EXPORT  ArmWriteScr
+    EXPORT  ArmWriteVMBar
+    EXPORT  ArmWriteVBar
+    EXPORT  ArmReadVBar
+    EXPORT  ArmWriteCPACR
+    EXPORT  ArmEnableVFP
+    EXPORT  ArmCallWFI
+    EXPORT  ArmWriteAuxCr
+    EXPORT  ArmReadAuxCr
+    EXPORT  ArmReadCbar
+    EXPORT  ArmInvalidateInstructionAndDataTlb
+    EXPORT  ArmReadMpidr
 
     AREA    ArmCacheLib, CODE, READONLY
     PRESERVE8
 
-DC_ON       EQU     ( 0x1:SHL:2 )
-IC_ON       EQU     ( 0x1:SHL:12 )
-
+DC_ON           EQU     ( 0x1:SHL:2 )
+IC_ON           EQU     ( 0x1:SHL:12 )
+CTRL_M_BIT      EQU     (1 << 0)
+CTRL_C_BIT      EQU     (1 << 2)
+CTRL_B_BIT      EQU     (1 << 7)
+CTRL_I_BIT      EQU     (1 << 12)
 
 
 ArmInvalidateDataCacheEntryByMVA
@@ -90,75 +108,91 @@ ArmInvalidateInstructionCache
   bx      LR
 
 ArmEnableMmu
-  mrc     p15,0,R0,c1,c0,0
-  orr     R0,R0,#1
-  mcr     p15,0,R0,c1,c0,0
+  mrc     p15,0,R0,c1,c0,0      ; Read SCTLR into R0 (Read control register configuration data)
+  orr     R0,R0,#1              ; Set SCTLR.M bit : Enable MMU
+  mcr     p15,0,R0,c1,c0,0      ; Write R0 into SCTLR (Write control register configuration data)
   dsb
   isb
   bx      LR
 
 ArmMmuEnabled
-  mrc     p15,0,R0,c1,c0,0
+  mrc     p15,0,R0,c1,c0,0      ; Read SCTLR into R0 (Read control register configuration data)
   and     R0,R0,#1
   bx      LR
 
 ArmDisableMmu
-  mrc     p15,0,R0,c1,c0,0
-  bic     R0,R0,#1
-  mcr     p15,0,R0,c1,c0,0      ;Disable MMU
+  mrc     p15,0,R0,c1,c0,0      ; Read SCTLR into R0 (Read control register configuration data)
+  bic     R0,R0,#1              ; Clear SCTLR.M bit : Disable MMU
+  mcr     p15,0,R0,c1,c0,0      ; Write R0 into SCTLR (Write control register configuration data)
 
-	mcr 		p15,0,R0,c8,c7,0      ;Invalidate TLB
-  mcr     p15,0,R0,c7,c5,6      ;Invalidate Branch predictor array
+  mcr 	  p15,0,R0,c8,c7,0      ; TLBIALL : Invalidate unified TLB
+  mcr     p15,0,R0,c7,c5,6      ; BPIALL  : Invalidate entire branch predictor array
   dsb
   isb
   bx      LR
 
+ArmDisableCachesAndMmu
+  mrc   p15, 0, r0, c1, c0, 0           ; Get control register
+  bic   r0, r0, #CTRL_M_BIT             ; Disable MMU
+  bic   r0, r0, #CTRL_C_BIT             ; Disable D Cache
+  bic   r0, r0, #CTRL_I_BIT             ; Disable I Cache
+  mcr   p15, 0, r0, c1, c0, 0           ; Write control register
+  dsb
+  isb
+  bx      LR
 
 ArmEnableDataCache
-  ldr     R1,=DC_ON
-  mrc     p15,0,R0,c1,c0,0      ;Read control register configuration data
-  orr     R0,R0,R1              ;Set C bit
-  mcr     p15,0,R0,c1,c0,0      ;Write control register configuration data
+  ldr     R1,=DC_ON             ; Specify SCTLR.C bit : (Data) Cache enable bit
+  mrc     p15,0,R0,c1,c0,0      ; Read SCTLR into R0 (Read control register configuration data)
+  orr     R0,R0,R1              ; Set SCTLR.C bit : Data and unified caches enabled
+  mcr     p15,0,R0,c1,c0,0      ; Write R0 into SCTLR (Write control register configuration data)
   dsb
   isb
   bx      LR
     
 ArmDisableDataCache
-  ldr     R1,=DC_ON
-  mrc     p15,0,R0,c1,c0,0      ;Read control register configuration data
-  bic     R0,R0,R1              ;Clear C bit
-  mcr     p15,0,R0,c1,c0,0      ;Write control register configuration data
+  ldr     R1,=DC_ON             ; Specify SCTLR.C bit : (Data) Cache enable bit
+  mrc     p15,0,R0,c1,c0,0      ; Read SCTLR into R0 (Read control register configuration data)
+  bic     R0,R0,R1              ; Clear SCTLR.C bit : Data and unified caches disabled
+  mcr     p15,0,R0,c1,c0,0      ; Write R0 into SCTLR (Write control register configuration data)
   isb
   bx      LR
 
 ArmEnableInstructionCache
-  ldr     R1,=IC_ON
-  mrc     p15,0,R0,c1,c0,0      ;Read control register configuration data
-  orr     R0,R0,R1              ;Set I bit
-  mcr     p15,0,R0,c1,c0,0      ;Write control register configuration data
+  ldr     R1,=IC_ON             ; Specify SCTLR.I bit : Instruction cache enable bit
+  mrc     p15,0,R0,c1,c0,0      ; Read SCTLR into R0 (Read control register configuration data)
+  orr     R0,R0,R1              ; Set SCTLR.I bit : Instruction caches enabled
+  mcr     p15,0,R0,c1,c0,0      ; Write R0 into SCTLR (Write control register configuration data)
   dsb
   isb
   bx      LR
   
 ArmDisableInstructionCache
-  ldr     R1,=IC_ON
-  mrc     p15,0,R0,c1,c0,0     ;Read control register configuration data
-  BIC     R0,R0,R1             ;Clear I bit.
-  mcr     p15,0,R0,c1,c0,0     ;Write control register configuration data
+  ldr     R1,=IC_ON             ; Specify SCTLR.I bit : Instruction cache enable bit
+  mrc     p15,0,R0,c1,c0,0      ; Read SCTLR into R0 (Read control register configuration data)
+  BIC     R0,R0,R1              ; Clear SCTLR.I bit : Instruction caches disabled
+  mcr     p15,0,R0,c1,c0,0      ; Write R0 into SCTLR (Write control register configuration data)
   isb
   bx      LR
 
-ArmEnableBranchPrediction
+ArmEnableSWPInstruction
   mrc     p15, 0, r0, c1, c0, 0
-  orr     r0, r0, #0x00000800
+  orr     r0, r0, #0x00000400
   mcr     p15, 0, r0, c1, c0, 0
   isb
   bx      LR
 
+ArmEnableBranchPrediction
+  mrc     p15, 0, r0, c1, c0, 0 ; Read SCTLR into R0 (Read control register configuration data)
+  orr     r0, r0, #0x00000800   ;
+  mcr     p15, 0, r0, c1, c0, 0 ; Write R0 into SCTLR (Write control register configuration data)
+  isb
+  bx      LR
+
 ArmDisableBranchPrediction
-  mrc     p15, 0, r0, c1, c0, 0
-  bic     r0, r0, #0x00000800
-  mcr     p15, 0, r0, c1, c0, 0
+  mrc     p15, 0, r0, c1, c0, 0 ; Read SCTLR into R0 (Read control register configuration data)
+  bic     r0, r0, #0x00000800   ;
+  mcr     p15, 0, r0, c1, c0, 0 ; Write R0 into SCTLR (Write control register configuration data)
   isb
   bx      LR
 
@@ -173,9 +207,9 @@ ArmV7AllDataCachesOperation
   mov   R10, #0
 
 Loop1   
-  add   R2, R10, R10, LSR #1  ; Work out 3xcachelevel
-  mov   R12, R6, LSR R2       ; bottom 3 bits are the Cache type for this level
-  and   R12, R12, #7          ; get those 3 bits alone
+  add   R2, R10, R10, LSR #1    ; Work out 3xcachelevel
+  mov   R12, R6, LSR R2         ; bottom 3 bits are the Cache type for this level
+  and   R12, R12, #7            ; get those 3 bits alone
   cmp   R12, #2
   blt   Skip                    ; no cache or only instruction cache at this level
   mcr   p15, 2, R10, c0, c0, 0  ; write the Cache Size selection register (CSSELR) // OR in 1 for Instruction
@@ -226,5 +260,64 @@ ArmInstructionSynchronizationBarrier
   isb
   bx      LR
 
-    END
+ArmWriteNsacr
+  mcr     p15, 0, r0, c1, c1, 2
+  bx      lr
+
+ArmWriteScr
+  mcr     p15, 0, r0, c1, c1, 0
+  bx      lr
+
+ArmWriteAuxCr
+  mcr     p15, 0, r0, c1, c0, 1
+  bx      lr
+
+ArmReadAuxCr
+  mrc     p15, 0, r0, c1, c0, 1
+  bx      lr  
+
+ArmWriteVMBar
+  mcr     p15, 0, r0, c12, c0, 1
+  bx      lr
+
+ArmWriteVBar
+  mcr     p15, 0, r0, c12, c0, 0 
+  bx      lr
+
+ArmReadVBar
+  mrc     p15, 0, r0, c12, c0, 0 
+  bx      lr
+
+ArmWriteCPACR
+  mcr     p15, 0, r0, c1, c0, 2
+  bx      lr
+
+ArmEnableVFP
+  // Enable VFP registers
+  mrc     p15, 0, r0, c1, c0, 2
+  orr     r0, r0, #0x00f00000   // Enable VPF access (V* instructions)
+  mcr     p15, 0, r0, c1, c0, 2
+  mov     r0, #0x40000000       // Set EN bit in FPEXC
+  mcr     p10,#0x7,r0,c8,c0,#0  // msr     FPEXC,r0 in ARM assembly
+  bx      lr
+
+ArmCallWFI
+  wfi
+  bx      lr
+
+//Note: Return 0 in Uniprocessor implementation
+ArmReadCbar
+  mrc     p15, 4, r0, c15, c0, 0	//Read Configuration Base Address Register
+  bx      lr
+
+ArmInvalidateInstructionAndDataTlb
+  mcr     p15, 0, r0, c8, c7, 0      ; Invalidate Inst TLB and Data TLB
+  dsb
+  bx lr
+
+ArmReadMpidr
+  mrc     p15, 0, r0, c0, c0, 5		; read MPIDR
+  bx      lr
+
+  END