[PATCH v2 1/2] ArmPkg/ArmGicLib: Add ArmGicSetInterruptPriority() helper function

["Quan Nguyen" <quan@os.amperecomputing.com>]

According to ARM IHI 0069F, section 11.9.18 GICD_IPRIORITYR<n>,
Interrupt Priority Registers, n = 0 - 254, when affinity routing is
enabled for the Security state of an interrupt, GICR_IPRIORITYR<n>
is used instead of GICD_IPRIORITYR<n> where n = 0 to 7 (that is, for
SGIs and PPIs).

As setting interrupt priority for SGIs and PPIs are handled using
difference registers depends on the mode, this patch instroduces
ArmGicSetInterruptPriority() helper function to handle the discrepancy.

Cc: Leif Lindholm <leif@nuviainc.com>
Cc: Ard Biesheuvel <ard.biesheuvel@arm.com>
Signed-off-by: Quan Nguyen <quan@os.amperecomputing.com>
---
 ArmPkg/Drivers/ArmGic/ArmGicLib.c  | 44 ++++++++++++++++++++++++++++++
 ArmPkg/Include/Library/ArmGicLib.h |  9 ++++++
 2 files changed, 53 insertions(+)

diff --git a/ArmPkg/Drivers/ArmGic/ArmGicLib.c b/ArmPkg/Drivers/ArmGic/ArmGicLib.c
index 001e6b143104..8ef32b33a154 100644
--- a/ArmPkg/Drivers/ArmGic/ArmGicLib.c
+++ b/ArmPkg/Drivers/ArmGic/ArmGicLib.c
@@ -199,6 +199,50 @@ ArmGicEndOfInterrupt (
   }
 }
 
+VOID
+EFIAPI
+ArmGicSetInterruptPriority (
+  IN UINTN                  GicDistributorBase,
+  IN UINTN                  GicRedistributorBase,
+  IN UINTN                  Source,
+  IN UINTN                  Priority
+  )
+{
+  UINT32                RegOffset;
+  UINTN                 RegShift;
+  ARM_GIC_ARCH_REVISION Revision;
+  UINTN                 GicCpuRedistributorBase;
+
+  // Calculate register offset and bit position
+  RegOffset = Source / 4;
+  RegShift = (Source % 4) * 8;
+
+  Revision = ArmGicGetSupportedArchRevision ();
+  if ((Revision == ARM_GIC_ARCH_REVISION_2) ||
+      FeaturePcdGet (PcdArmGicV3WithV2Legacy) ||
+      SourceIsSpi (Source)) {
+    MmioAndThenOr32 (
+      GicDistributorBase + ARM_GIC_ICDIPR + (4 * RegOffset),
+      ~(0xff << RegShift),
+      Priority << RegShift
+      );
+  } else {
+    GicCpuRedistributorBase = GicGetCpuRedistributorBase (
+                                GicRedistributorBase,
+                                Revision
+                                );
+    if (GicCpuRedistributorBase == 0) {
+      return;
+    }
+
+    MmioAndThenOr32 (
+      GicCpuRedistributorBase + ARM_GIC_ICDIPR + (4 * RegOffset),
+      ~(0xff << RegShift),
+      Priority << RegShift
+      );
+  }
+}
+
 VOID
 EFIAPI
 ArmGicEnableInterrupt (
diff --git a/ArmPkg/Include/Library/ArmGicLib.h b/ArmPkg/Include/Library/ArmGicLib.h
index 55093189638b..7bcfc001115b 100644
--- a/ArmPkg/Include/Library/ArmGicLib.h
+++ b/ArmPkg/Include/Library/ArmGicLib.h
@@ -208,6 +208,15 @@ ArmGicSetPriorityMask (
   IN  INTN          PriorityMask
   );
 
+VOID
+EFIAPI
+ArmGicSetInterruptPriority (
+  IN UINTN                  GicDistributorBase,
+  IN UINTN                  GicRedistributorBase,
+  IN UINTN                  Source,
+  IN UINTN                  Priority
+  );
+
 VOID
 EFIAPI
 ArmGicEnableInterrupt (
-- 
2.28.0