1 files changed, 103 insertions, 0 deletions
diff --git a/arch/arm64/include/asm/irqflags.h b/arch/arm64/include/asm/irqflags.h
index df7477af6389..5e8b4c39ebe4 100644
--- a/arch/arm64/include/asm/irqflags.h
+++ b/arch/arm64/include/asm/irqflags.h
@@ -20,6 +20,8 @@
 
 #include <asm/ptrace.h>
 
+#ifndef CONFIG_USE_ICC_SYSREGS_FOR_IRQFLAGS
+
 /*
  * CPU interrupt mask handling.
  */
@@ -84,6 +86,107 @@ static inline int arch_irqs_disabled_flags(unsigned long flags)
 	return flags & PSR_I_BIT;
 }
 
+#else /* CONFIG_IRQFLAGS_GIC_MASKING */
+
+#include <linux/stringify.h>
+#include <asm/sysreg.h>
+
+/* from arm-gic-v3.h */
+#define ICC_PMR_EL1 sys_reg(3, 0, 4, 6, 0)
+
+#define ICC_PMR_EL1_UNMASKED 0xf0
+#define ICC_PMR_EL1_MASKED 0x70
+
+/*
+ * CPU interrupt mask handling.
+ */
+static inline unsigned long arch_local_irq_save(void)
+{
+	unsigned long flags, masked = ICC_PMR_EL1_MASKED;
+
+	asm volatile(
+		"// arch_local_irq_save\n"
+		"mrs_s  %0, " __stringify(ICC_PMR_EL1) "\n"
+		"msr_s	" __stringify(ICC_PMR_EL1) ",%1\n"
+		"isb\n"
+		"mrs	%0, daif\n"
+		"msr	daifset, #2"
+		: "=&r" (flags)
+		: "r" (masked)
+		: "memory");
+	return flags;
+}
+
+static inline void arch_local_irq_enable(void)
+{
+	unsigned long unmasked = ICC_PMR_EL1_UNMASKED;
+
+	asm volatile(
+		"// arch_local_irq_enable\n"
+		"msr_s  " __stringify(ICC_PMR_EL1) ",%0\n"
+		"isb\n"
+		"msr	daifclr, #2"
+		:
+		: "r" (unmasked)
+		: "memory");
+}
+
+static inline void arch_local_irq_disable(void)
+{
+	unsigned long masked = ICC_PMR_EL1_MASKED;
+
+	asm volatile(
+		"// arch_local_irq_disable\n"
+		"msr_s  " __stringify(ICC_PMR_EL1) ",%0\n"
+		"isb\n"
+		"msr	daifset, #2"
+		:
+		: "r" (masked)
+		: "memory");
+}
+
+/*
+ * Save the current interrupt enable state.
+ */
+static inline unsigned long arch_local_save_flags(void)
+{
+	unsigned long flags;
+
+	asm volatile(
+		"// arch_local_save_flags\n"
+		"mrs_s  %0, " __stringify(ICC_PMR_EL1) "\n"
+		"mrs	%0, daif"
+		: "=r" (flags)
+		:
+		: "memory");
+	return flags;
+}
+
+/*
+ * restore saved IRQ state
+ */
+static inline void arch_local_irq_restore(unsigned long flags)
+{
+	unsigned long secondary_flags =
+	    flags & PSR_I_BIT ? ICC_PMR_EL1_MASKED : ICC_PMR_EL1_UNMASKED;
+
+	asm volatile(
+		"// arch_local_irq_restore\n"
+		"msr_s  " __stringify(ICC_PMR_EL1) ",%1\n"
+		"isb\n"
+		"msr	daif, %0"
+	:
+	: "r" (flags), "r" (secondary_flags)
+	: "memory");
+}
+
+static inline int arch_irqs_disabled_flags(unsigned long flags)
+{
+	return flags & PSR_I_BIT;
+}
+
+#endif /* CONFIG_IRQFLAGS_GIC_MASKING */
+
 #define local_fiq_enable()	asm("msr	daifclr, #1" : : : "memory")
 #define local_fiq_disable()	asm("msr	daifset, #1" : : : "memory")