Added FIQ interrupt primatives. Update exception handler to disable/reenable FIQ when updating in case debug agent library is using FIQ.

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

ArmPkg/Drivers/CpuDxe/Exception.c

@@ -178,16 +178,23 @@ InitializeExceptions (
   UINTN                Offset;
   UINTN                Length;
   UINTN                Index;
-  BOOLEAN              Enabled;
+  BOOLEAN              IrqEnabled;
+  BOOLEAN              FiqEnabled;
   EFI_PHYSICAL_ADDRESS Base;
   UINT32               *VectorBase;
 
   //
   // Disable interrupts
   //
-  Cpu->GetInterruptState (Cpu, &Enabled);
+  Cpu->GetInterruptState (Cpu, &IrqEnabled);
   Cpu->DisableInterrupt (Cpu);
 
+  //
+  // EFI does not use the FIQ, but a debugger might so we must disable 
+  // as we take over the exception vectors. 
+  //
+  FiqEnabled = ArmGetFiqState ();
+  ArmDisableFiq ();
 
   //
   // Copy an implementation of the ARM exception vectors to PcdCpuVectorBaseAddress.
@@ -236,7 +243,11 @@ InitializeExceptions (
   // Flush Caches since we updated executable stuff
   InvalidateInstructionCacheRange ((VOID *)PcdGet32(PcdCpuVectorBaseAddress), Length);
 
-  if (Enabled) {
+  if (FiqEnabled) {
+    ArmEnableFiq ();
+  }
+
+  if (IrqEnabled) {
     // 
     // Restore interrupt state
     //

ArmPkg/Include/Library/ArmLib.h

@@ -241,6 +241,23 @@ EFIAPI
 ArmGetInterruptState (
   VOID
   );
+VOID
+EFIAPI
+ArmEnableFiq (
+  VOID
+  );
+
+UINTN
+EFIAPI
+ArmDisableFiq (
+  VOID
+  );
+  
+BOOLEAN
+EFIAPI
+ArmGetFiqState (
+  VOID
+  );
 
 VOID
 EFIAPI

ArmPkg/Library/ArmLib/ArmV7/ArmLibSupport.S

@@ -19,6 +19,9 @@
 .globl ASM_PFX(ArmEnableInterrupts)
 .globl ASM_PFX(ArmDisableInterrupts)
 .globl ASM_PFX(ArmGetInterruptState)
+.globl ASM_PFX(ArmEnableFiq)
+.globl ASM_PFX(ArmDisableFiq)
+.globl ASM_PFX(ArmGetFiqState)
 .globl ASM_PFX(ArmInvalidateTlb)
 .globl ASM_PFX(ArmSetTranslationTableBaseAddress)
 .globl ASM_PFX(ArmGetTranslationTableBaseAddress)
@@ -54,6 +57,21 @@ ASM_PFX(ArmGetInterruptState):
 	movne   R0,#0
 	bx      LR
 
+ASM_PFX(ArmEnableFiq):
+  cpsie   f
+	bx      LR
+
+ASM_PFX(ArmDisableFiq):
+  cpsid   f
+	bx      LR
+
+ASM_PFX(ArmGetFiqState):
+	mrs     R0,CPSR
+	tst     R0,#0x30	    @Check if IRQ is enabled.
+	moveq   R0,#1
+	movne   R0,#0
+	bx      LR
+
 ASM_PFX(ArmInvalidateTlb):
   mov     r0,#0
   mcr     p15,0,r0,c8,c7,0

ArmPkg/Library/ArmLib/ArmV7/ArmLibSupport.asm

@@ -18,6 +18,9 @@
     EXPORT  ArmEnableInterrupts
     EXPORT  ArmDisableInterrupts
     EXPORT  ArmGetInterruptState
+    EXPORT  ArmEnableFiq
+    EXPORT  ArmDisableFiq
+    EXPORT  ArmGetFiqState
     EXPORT  ArmInvalidateTlb
     EXPORT  ArmSetTranslationTableBaseAddress
     EXPORT  ArmGetTranslationTableBaseAddress
@@ -52,6 +55,21 @@ ArmGetInterruptState
 	movne   R0,#0
 	bx      LR
 	
+ArmEnableFiq
+  CPSIE   f
+	bx      LR
+
+ArmDisableFiq
+  CPSID   f
+	bx      LR
+
+ArmGetFiqState
+	mrs     R0,CPSR
+	tst     R0,#0x40	    ;Check if IRQ is enabled.
+	moveq   R0,#1
+	movne   R0,#0
+	bx      LR
+  
 ArmInvalidateTlb
   mov     r0,#0
   mcr     p15,0,r0,c8,c7,0

ArmPkg/Library/ArmLib/Common/ArmLibSupport.S

@@ -19,6 +19,9 @@
 .globl ASM_PFX(ArmEnableInterrupts)
 .globl ASM_PFX(ArmDisableInterrupts)
 .globl ASM_PFX(ArmGetInterruptState)
+.globl ASM_PFX(ArmEnableFiq)
+.globl ASM_PFX(ArmDisableFiq)
+.globl ASM_PFX(ArmGetFiqState)
 .globl ASM_PFX(ArmInvalidateTlb)
 .globl ASM_PFX(ArmSetTranslationTableBaseAddress)
 .globl ASM_PFX(ArmGetTranslationTableBaseAddress)
@@ -58,6 +61,28 @@ ASM_PFX(ArmGetInterruptState):
 	movne   R0,#0
 	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(ArmGetFiqState):
+	mrs     R0,CPSR
+	tst     R0,#0x80	    @Check if FIQ is enabled.
+	moveq   R0,#1
+	movne   R0,#0
+	bx      LR
+
 ASM_PFX(ArmInvalidateTlb):
   mov     r0,#0
   mcr     p15,0,r0,c8,c7,0

ArmPkg/Library/ArmLib/Common/ArmLibSupport.asm

@@ -18,6 +18,9 @@
     EXPORT  ArmEnableInterrupts
     EXPORT  ArmDisableInterrupts
     EXPORT  ArmGetInterruptState
+    EXPORT  ArmEnableFiq
+    EXPORT  ArmDisableFiq
+    EXPORT  ArmGetFiqState
     EXPORT  ArmInvalidateTlb
     EXPORT  ArmSetTranslationTableBaseAddress
     EXPORT  ArmGetTranslationTableBaseAddress
@@ -57,6 +60,28 @@ ArmGetInterruptState
 	movne   R0,#0
 	bx      LR
 
+ArmEnableFiq
+	mrs     R0,CPSR
+	bic     R0,R0,#0x40		;Enable IRQ interrupts
+	msr     CPSR_c,R0
+	bx      LR
+
+ArmDisableFiq
+	mrs     R0,CPSR
+	orr     R1,R0,#0x40		;Disable IRQ interrupts
+	msr     CPSR_c,R1
+  tst     R0,#0x40
+  moveq   R0,#1
+  movne   R0,#0
+	bx      LR
+
+ArmGetFiqState
+	mrs     R0,CPSR
+	tst     R0,#0x40	    ;Check if IRQ is enabled.
+	moveq   R0,#1
+	movne   R0,#0
+	bx      LR
+  
 ArmInvalidateTlb
   mov     r0,#0
   mcr     p15,0,r0,c8,c7,0