ArmPkg/ArmLib: Update Arm11 port

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

6 files changed, 349 insertions, 138 deletions

diff --git a/ArmPkg/Library/ArmLib/Arm11/Arm11Lib.c b/ArmPkg/Library/ArmLib/Arm11/Arm11Lib.c
index 1257be8cf6..c512c03557 100644
--- a/ArmPkg/Library/ArmLib/Arm11/Arm11Lib.c
+++ b/ArmPkg/Library/ArmLib/Arm11/Arm11Lib.c
@@ -1,133 +1,49 @@
-/** @file
-
-  Copyright (c) 2008 - 2009, Apple Inc. 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.
-
-**/
-
-#include <Chipset/ARM1176JZ-S.h>
-#include <Library/ArmLib.h>
-#include <Library/BaseMemoryLib.h>
-#include <Library/MemoryAllocationLib.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_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_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 - 1 ) / TT_DESCRIPTOR_SECTION_SIZE ) + 1 );
-  
-  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++;
-  }
-
-  ArmSetTTBR0(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();  
-}
-
-
-
-
+/** @file

+

+  Copyright (c) 2008 - 2009, Apple Inc. All rights reserved.<BR>

+  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 <Chipset/ARM1176JZ-S.h>

+

+#include <Library/ArmLib.h>

+#include <Library/DebugLib.h>

+#include <Library/PcdLib.h>

+

+VOID

+EFIAPI

+ArmWriteVBar (

+  IN  UINT32   VectorBase

+  )

+{

+  ASSERT(FeaturePcdGet (PcdRelocateVectorTable) == TRUE);

+

+  if (VectorBase == 0x0) {

+    ArmSetLowVectors ();

+  } else if (VectorBase == 0xFFFF0000) {

+    ArmSetHighVectors ();

+  } else {

+    // Feature not supported by ARM11. The Vector Table is either at 0x0 or 0xFFFF0000

+    ASSERT(0);

+  }

+}

+

+UINT32

+EFIAPI

+ArmReadVBar (

+  VOID

+  )

+{

+  ASSERT((FeaturePcdGet (PcdRelocateVectorTable) == TRUE) && ((PcdGet32 (PcdCpuVectorBaseAddress) == 0x0) || (PcdGet32 (PcdCpuVectorBaseAddress) == 0xFFFF0000)));

+  return PcdGet32 (PcdCpuVectorBaseAddress);

+}

+

diff --git a/ArmPkg/Library/ArmLib/Arm11/Arm11ArmLib.inf b/ArmPkg/Library/ArmLib/Arm11/Arm11Lib.inf
index cb5b8aabee..b28a4578b0 100644
--- a/ArmPkg/Library/ArmLib/Arm11/Arm11ArmLib.inf
+++ b/ArmPkg/Library/ArmLib/Arm11/Arm11Lib.inf
@@ -28,8 +28,9 @@
   

   Arm11Support.S    | GCC

   Arm11Support.asm  | RVCT

-  

-  Arm11Lib.c

+

+  Arm11Lib.c  

+  Arm11LibMem.c

   ../Arm9/Arm9CacheInformation.c

 

 [Packages]

@@ -42,5 +43,9 @@
 [Protocols]

   gEfiCpuArchProtocolGuid

 

+[FeaturePcd]

+  gArmTokenSpaceGuid.PcdRelocateVectorTable

+

 [FixedPcd]

   gArmTokenSpaceGuid.PcdArmCacheOperationThreshold

+  gArmTokenSpaceGuid.PcdCpuVectorBaseAddress

diff --git a/ArmPkg/Library/ArmLib/Arm11/Arm11LibMem.c b/ArmPkg/Library/ArmLib/Arm11/Arm11LibMem.c
new file mode 100644
index 0000000000..1257be8cf6
--- /dev/null
+++ b/ArmPkg/Library/ArmLib/Arm11/Arm11LibMem.c
@@ -0,0 +1,133 @@
+/** @file
+
+  Copyright (c) 2008 - 2009, Apple Inc. 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.
+
+**/
+
+#include <Chipset/ARM1176JZ-S.h>
+#include <Library/ArmLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/MemoryAllocationLib.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_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_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 - 1 ) / TT_DESCRIPTOR_SECTION_SIZE ) + 1 );
+  
+  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++;
+  }
+
+  ArmSetTTBR0(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();  
+}
+
+
+
+
diff --git a/ArmPkg/Library/ArmLib/Arm11/Arm11ArmLibPrePi.inf b/ArmPkg/Library/ArmLib/Arm11/Arm11LibPrePi.inf
index e3047b28f4..cd36d8b3c7 100644
--- a/ArmPkg/Library/ArmLib/Arm11/Arm11ArmLibPrePi.inf
+++ b/ArmPkg/Library/ArmLib/Arm11/Arm11LibPrePi.inf
@@ -28,8 +28,9 @@
   

   Arm11Support.S    | GCC

   Arm11Support.asm  | RVCT

-  

-  Arm11Lib.c

+

+  Arm11Lib.c  

+  Arm11LibMem.c

   ../Arm9/Arm9CacheInformation.c

 

 [Packages]

@@ -42,5 +43,9 @@
 [Protocols]

   gEfiCpuArchProtocolGuid

 

+[FeaturePcd]

+  gArmTokenSpaceGuid.PcdRelocateVectorTable

+

 [FixedPcd]

   gArmTokenSpaceGuid.PcdArmCacheOperationThreshold

+  gArmTokenSpaceGuid.PcdCpuVectorBaseAddress

diff --git a/ArmPkg/Library/ArmLib/Arm11/Arm11LibSec.inf b/ArmPkg/Library/ArmLib/Arm11/Arm11LibSec.inf
new file mode 100644
index 0000000000..4c3566e3c5
--- /dev/null
+++ b/ArmPkg/Library/ArmLib/Arm11/Arm11LibSec.inf
@@ -0,0 +1,47 @@
+#/** @file

+# Semihosting  serail port lib

+#

+# Copyright (c) 2008 - 2010, Apple Inc. 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.

+#

+#

+#**/

+

+[Defines]

+  INF_VERSION                    = 0x00010005

+  BASE_NAME                      = Arm11LibSec

+  FILE_GUID                      = bfecdbc7-a860-4993-bc09-8e3ea762a758

+  MODULE_TYPE                    = BASE

+  VERSION_STRING                 = 1.0

+  LIBRARY_CLASS                  = ArmLib

+

+[Sources.common]

+  ../Common/ArmLibSupport.S    | GCC

+  ../Common/ArmLibSupport.asm  | RVCT

+  ../Common/ArmLib.c

+  

+  Arm11Support.S    | GCC

+  Arm11Support.asm  | RVCT

+  

+  Arm11Lib.c

+  ../Arm9/Arm9CacheInformation.c

+

+[Packages]

+  ArmPkg/ArmPkg.dec

+  MdePkg/MdePkg.dec

+

+[Protocols]

+  gEfiCpuArchProtocolGuid

+

+[FeaturePcd]

+  gArmTokenSpaceGuid.PcdRelocateVectorTable

+

+[FixedPcd]

+  gArmTokenSpaceGuid.PcdArmCacheOperationThreshold

+  gArmTokenSpaceGuid.PcdCpuVectorBaseAddress

diff --git a/ArmPkg/Library/ArmLib/Arm11/Arm11Support.S b/ArmPkg/Library/ArmLib/Arm11/Arm11Support.S
index 35774a8e62..7ff377c023 100644
--- a/ArmPkg/Library/ArmLib/Arm11/Arm11Support.S
+++ b/ArmPkg/Library/ArmLib/Arm11/Arm11Support.S
@@ -1,6 +1,7 @@
 #------------------------------------------------------------------------------ 
 #
 # Copyright (c) 2008 - 2009, Apple Inc. All rights reserved.<BR>
+# 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
@@ -16,6 +17,8 @@
 
 .text
 .align 2
+GCC_ASM_EXPORT(ArmDisableCachesAndMmu)
+GCC_ASM_EXPORT(ArmInvalidateInstructionAndDataTlb)
 GCC_ASM_EXPORT(ArmCleanInvalidateDataCache)
 GCC_ASM_EXPORT(ArmCleanDataCache)
 GCC_ASM_EXPORT(ArmInvalidateDataCache)
@@ -35,11 +38,39 @@ GCC_ASM_EXPORT(ArmDisableBranchPrediction)
 GCC_ASM_EXPORT(ArmDataMemoryBarrier)
 GCC_ASM_EXPORT(ArmDataSyncronizationBarrier)
 GCC_ASM_EXPORT(ArmInstructionSynchronizationBarrier)
+GCC_ASM_EXPORT(ArmSetLowVectors)
+GCC_ASM_EXPORT(ArmSetHighVectors)
+GCC_ASM_EXPORT(ArmIsMpCore)
+GCC_ASM_EXPORT(ArmCallWFI)
+GCC_ASM_EXPORT(ArmReadMpidr)
+GCC_ASM_EXPORT(ArmUpdateTranslationTableEntry)
+GCC_ASM_EXPORT(ArmEnableFiq)
+GCC_ASM_EXPORT(ArmDisableFiq)
+GCC_ASM_EXPORT(ArmEnableInterrupts)
+GCC_ASM_EXPORT(ArmDisableInterrupts)
+GCC_ASM_EXPORT (ArmEnableVFP)
 
+Arm11PartNumberMask:  .word       0xFFF0
+Arm11PartNumber:      .word	      0xB020
 
 .set DC_ON, (0x1<<2)
 .set IC_ON, (0x1<<12)
 .set XP_ON, (0x1<<23)
+.set CTRL_M_BIT,  (1 << 0)
+.set CTRL_C_BIT,  (1 << 2)
+.set CTRL_I_BIT,  (1 << 12)
+
+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
+  bx      LR
+
+ASM_PFX(ArmInvalidateInstructionAndDataTlb):
+  mcr     p15, 0, r0, c8, c7, 0      @ Invalidate Inst TLB and Data TLB
+  bx lr
 
 ASM_PFX(ArmInvalidateDataCacheEntryByMVA):
   mcr     p15, 0, r0, c7, c6, 1   @invalidate single data cache line                                           
@@ -152,6 +183,80 @@ ASM_PFX(ArmInstructionSynchronizationBarrier):
   mov R0, #0
   mcr P15, #0, R0, C7, C5, #4 
   bx      LR
-  
+
+ASM_PFX(ArmSetLowVectors):
+  mrc     p15, 0, r0, c1, c0, 0 @ Read SCTLR into R0 (Read control register configuration data)
+  bic     r0, r0, #0x00002000   @ clear V bit
+  mcr     p15, 0, r0, c1, c0, 0 @ Write R0 into SCTLR (Write control register configuration data)
+  bx      LR
+
+ASM_PFX(ArmSetHighVectors):
+  mrc     p15, 0, r0, c1, c0, 0 @ Read SCTLR into R0 (Read control register configuration data)
+  orr     r0, r0, #0x00002000   @ clear V bit
+  mcr     p15, 0, r0, c1, c0, 0 @ Write R0 into SCTLR (Write control register configuration data)
+  bx      LR
+
+ASM_PFX(ArmIsMpCore):
+  push    { r1 }
+  mrc     p15, 0, r0, c0, c0, 0
+  # Extract Part Number to check it is an ARM11MP core (0xB02)
+  LoadConstantToReg (Arm11PartNumberMask, r1)
+  and     r0, r0, r1
+  LoadConstantToReg (Arm11PartNumber, r1)
+  cmp     r0, r1
+  movne   r0, #0
+  pop     { r1 }
+  bx      lr     
+
+ASM_PFX(ArmCallWFI):
+  wfi
+  bx      lr
+
+ASM_PFX(ArmReadMpidr):
+  mrc     p15, 0, r0, c0, c0, 5       @ read MPIDR
+  bx      lr
+
+ASM_PFX(ArmEnableFiq):
+  mrs     R0,CPSR
+  bic     R0,R0,#0x40    @Enable FIQ interrupts
+  msr     CPSR_c,R0
+  bx      LR
+
+ASM_PFX(ArmDisableFiq):
+  mrs     R0,CPSR
+  orr     R1,R0,#0x40    @Disable FIQ interrupts
+  msr     CPSR_c,R1
+  tst     R0,#0x80
+  moveq   R0,#1
+  movne   R0,#0
+  bx      LR
+
+ASM_PFX(ArmEnableInterrupts):
+  mrs     R0,CPSR
+  bic     R0,R0,#0x80    @Enable IRQ interrupts
+  msr     CPSR_c,R0
+  bx      LR
+
+ASM_PFX(ArmDisableInterrupts):
+  mrs     R0,CPSR
+  orr     R1,R0,#0x80    @Disable IRQ interrupts
+  msr     CPSR_c,R1
+  tst     R0,#0x80
+  moveq   R0,#1
+  movne   R0,#0
+  bx      LR
+
+ASM_PFX(ArmEnableVFP):
+  # Read CPACR (Coprocessor Access Control Register)
+  mrc     p15, 0, r0, c1, c0, 2
+  # Enable VPF access (Full Access to CP10, CP11) (V* instructions)
+  orr     r0, r0, #0x00f00000
+  # Write back CPACR (Coprocessor Access Control Register)
+  mcr     p15, 0, r0, c1, c0, 2
+  # Set EN bit in FPEXC. The Advanced SIMD and VFP extensions are enabled and operate normally.
+  mov     r0, #0x40000000
+  #TODO: Fixme - need compilation flag
+  #fmxr    FPEXC, r0
+  bx      lr
 
 ASM_FUNCTION_REMOVE_IF_UNREFERENCED