ArmPkg/ArmLib: Added new functions to access ARM coprocessors

[mirror_edk2.git] / ArmPkg / Library / ArmLib / ArmV7 / ArmV7Support.S

diff --git a/ArmPkg/Library/ArmLib/ArmV7/ArmV7Support.S b/ArmPkg/Library/ArmLib/ArmV7/ArmV7Support.S
index 3bb601541d2f6e6f79b06edfb5eb10ad9891eb3d..910e50e40182b1578e6536e48adb236b9a5cb7ea 100644 (file)
--- a/ArmPkg/Library/ArmLib/ArmV7/ArmV7Support.S
+++ b/ArmPkg/Library/ArmLib/ArmV7/ArmV7Support.S
@@ -42,6 +42,7 @@ GCC_ASM_EXPORT (ArmV7PerformPoUDataCacheOperation)
 GCC_ASM_EXPORT (ArmDataMemoryBarrier)\r
 GCC_ASM_EXPORT (ArmDataSyncronizationBarrier)\r
 GCC_ASM_EXPORT (ArmInstructionSynchronizationBarrier)\r
+GCC_ASM_EXPORT (ArmReadVBar)\r
 GCC_ASM_EXPORT (ArmWriteVBar)\r
 GCC_ASM_EXPORT (ArmEnableVFP)\r
 GCC_ASM_EXPORT (ArmCallWFI)\r
@@ -330,6 +331,11 @@ ASM_PFX(ArmInstructionSynchronizationBarrier):
   isb\r
   bx      LR\r
 \r
+ASM_PFX(ArmReadVBar):\r
+  # Set the Address of the Vector Table in the VBAR register\r
+  mrc     p15, 0, r0, c12, c0, 0\r
+  bx      lr\r
+\r
 ASM_PFX(ArmWriteVBar):\r
   # Set the Address of the Vector Table in the VBAR register\r
   mcr     p15, 0, r0, c12, c0, 0 \r
@@ -347,6 +353,7 @@ ASM_PFX(ArmEnableVFP):
   orr     r0, r0, #0x00f00000\r
   # Write back CPACR (Coprocessor Access Control Register)\r
   mcr     p15, 0, r0, c1, c0, 2\r
+  isb\r
   # Set EN bit in FPEXC. The Advanced SIMD and VFP extensions are enabled and operate normally.\r
   mov     r0, #0x40000000\r
   mcr     p10,#0x7,r0,c8,c0,#0\r