From 0d49b82e4fa5d7eaeb5f73dfb93a932893832437 Mon Sep 17 00:00:00 2001
From: Quan Nguyen <quan@os.amperecomputing.com>
Date: Wed, 16 Dec 2020 20:25:20 +0700
Subject: [PATCH] ArmPkg/ArmGicLib: Add ArmGicSetInterruptPriority() helper
 function

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>
Signed-off-by: Quan Nguyen <quan@os.amperecomputing.com>
Reviewed-off-by: Ard Biesheuvel <ard.biesheuvel@arm.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 001e6b1431..8ef32b33a1 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 5509318963..7bcfc00111 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 (