[RFC PATCH 16/36] arm64: cpuidle: use new helpers to bypass interrupt priority masking

[Vladimir Murzin <vladimir.murzin@arm.com>]

From: Ada Couprie Diaz <ada.coupriediaz@arm.com>

When the CPU goes into WFI and pseudo-NMIs are in use, we need to make
sure that interrupts are not masked by the PMR otherwise they would not
reach the CPU and wake it up.

Now that we have a proper API that handles both DAIF and PMR, extend it
to provide helpers allowing to temporarily switch to masking interrupts
via DAIF only when interrupt priorities are in use.
This could allow other parts of the code to make use of it and makes it
easier to check for proper use.

Replace the custom cpuidle helper with those new helpers.

Signed-off-by: Ada Couprie Diaz <ada.coupriediaz@arm.com>
Signed-off-by: Vladimir Murzin <vladimir.murzin@arm.com>
---
 arch/arm64/include/asm/cpuidle.h            | 14 ++---
 arch/arm64/include/asm/interrupts/masking.h | 58 +++++++++++++++++++++
 2 files changed, 62 insertions(+), 10 deletions(-)

diff --git a/arch/arm64/include/asm/cpuidle.h b/arch/arm64/include/asm/cpuidle.h
index 2047713e097d..0c7ccdaf1832 100644
--- a/arch/arm64/include/asm/cpuidle.h
+++ b/arch/arm64/include/asm/cpuidle.h
@@ -5,22 +5,17 @@
 #include <asm/proc-fns.h>
 
 #ifdef CONFIG_ARM64_PSEUDO_NMI
-#include <asm/arch_gicv3.h>
+#include <asm/interrupts/masking.h>
 
 struct arm_cpuidle_irq_context {
-	unsigned long pmr;
-	unsigned long daif_bits;
+	arm64_exc_hwstates_t arm64_context;
 };
 
 #define arm_cpuidle_save_irq_context(__c)				\
 	do {								\
 		struct arm_cpuidle_irq_context *c = __c;		\
 		if (system_uses_irq_prio_masking()) {			\
-			c->daif_bits = read_sysreg(daif);		\
-			write_sysreg(c->daif_bits | PSR_I_BIT | PSR_F_BIT, \
-				     daif);				\
-			c->pmr = gic_read_pmr();			\
-			gic_write_pmr(GIC_PRIO_IRQON | GIC_PRIO_PSR_I_SET); \
+			c->arm64_context = local_all_irqs_force_daif_save(); \
 		}							\
 	} while (0)
 
@@ -28,8 +23,7 @@ struct arm_cpuidle_irq_context {
 	do {								\
 		struct arm_cpuidle_irq_context *c = __c;		\
 		if (system_uses_irq_prio_masking()) {			\
-			gic_write_pmr(c->pmr);				\
-			write_sysreg(c->daif_bits, daif);		\
+			local_all_irqs_force_daif_restore(c->arm64_context); \
 		}							\
 	} while (0)
 #else
diff --git a/arch/arm64/include/asm/interrupts/masking.h b/arch/arm64/include/asm/interrupts/masking.h
index 19d32618b2b9..9dc37a571094 100644
--- a/arch/arm64/include/asm/interrupts/masking.h
+++ b/arch/arm64/include/asm/interrupts/masking.h
@@ -99,6 +99,64 @@ static inline void local_all_irqs_final_mask(void)
 }
 #endif /* CONFIG_DEBUG_IRQFLAGS */
 
+/*
+ * In some cases, WFI or guest entry for example, we always want interrupts
+ * to reach the CPU even if masked. Masking via the PMR prevents them from
+ * reaching the CPU and waking it up.
+ * Force IRQ masking using DAIF by raising the priority mask
+ * and setting the IF flags.
+ *
+ * Should only be called when IRQs are already masked.
+ */
+static inline arm64_exc_hwstates_t local_all_irqs_force_daif_save(void)
+{
+	arm64_exc_hwstates_t states = {};
+	/*
+	 * Cannot use lockdep_assert here as idle entry enables hardirqs
+	 * while keeping interrupts masked.
+	 */
+	WARN_ON_ONCE(!irqs_disabled());
+
+	if (system_uses_irq_prio_masking()) {
+	        states.saved.daif = read_sysreg(daif);
+	        states.saved.pmr = read_sysreg_s(SYS_ICC_PMR_EL1);
+		/*
+		 * We might have IF set or unset. In case IF already set orring with IF
+		 * won't change anything, combined with PMR set to IRQON we might become
+		 * one of contexts: CRITICAL, ERROR or NONMI. In case IF is unset (which
+		 * also implies that DA is unset) orring with IF and combining with PMR
+		 * set to IRQON would effectively gives us NONMI context.
+		 */
+		states.expected.daif = states.saved.daif | DAIF_PROCCTX_NOIRQ;
+		states.expected.pmr = GIC_PRIO_IRQON;
+
+		arm64_update_exc_hwstate(states.expected, true);
+	}
+
+	return states;
+}
+
+/*
+ * Return to masking with the PMR, restoring previously saved DAIF and PMR.
+ *
+ * IRQs or interrupt priority masking should not have been re-enabled in between
+ * the save and restore.
+ */
+static inline
+void local_all_irqs_force_daif_restore(arm64_exc_hwstates_t states)
+{
+	/*
+	 * Cannot use lockdep_assert here as idle entry enables hardirqs
+	 * while keeping interrupts masked.
+	 */
+	WARN_ON_ONCE(!irqs_disabled());
+
+	if (system_uses_irq_prio_masking()) {
+		arm64_debug_exc_hwstate(states.expected);
+		arm64_update_exc_hwstate(states.saved, true);
+	}
+}
+
 /*
  * During early boot, we unmask PSR.DA before the GIC has been set up.
  * If we use IRQ priority masking, the PMR and PSR will be out of sync
-- 
2.34.1