3 files changed, 165 insertions, 6 deletions

diff --git a/arch/arm64/include/asm/assembler.h b/arch/arm64/include/asm/assembler.h
index 750bac4e637e..0a0a97f3c3c1 100644
--- a/arch/arm64/include/asm/assembler.h
+++ b/arch/arm64/include/asm/assembler.h
@@ -23,6 +23,7 @@
 #ifndef __ASM_ASSEMBLER_H
 #define __ASM_ASSEMBLER_H
 
+#include <linux/irqchip/arm-gic-v3.h>
 #include <asm/ptrace.h>
 #include <asm/thread_info.h>
 
@@ -39,26 +40,79 @@
 	.endm
 
 /*
+ * Enable and disable pseudo NMI.
+ */
+	.macro disable_nmi
+#ifdef CONFIG_USE_ICC_SYSREGS_FOR_IRQFLAGS
+	msr	daifset, #2
+#endif
+	.endm
+
+	.macro enable_nmi
+#ifdef CONFIG_USE_ICC_SYSREGS_FOR_IRQFLAGS
+	msr	daifclr, #2
+#endif
+	.endm
+
+/*
+ * Save/disable and restore pseudo NMI.
+ */
+	.macro	save_and_disable_nmis, olddaif
+#ifdef CONFIG_USE_ICC_SYSREGS_FOR_IRQFLAGS
+	mrs	\olddaif, daif			/* Get flags		      */
+	disable_nmi
+#endif
+	.endm
+
+	.macro	restore_nmis, olddaif
+#ifdef CONFIG_USE_ICC_SYSREGS_FOR_IRQFLAGS
+	msr	daif, \olddaif
+#endif
+	.endm
+
+/*
  * Enable and disable interrupts.
  */
-	.macro	disable_irq
+	.macro	disable_irq, tmp
+#ifdef CONFIG_USE_ICC_SYSREGS_FOR_IRQFLAGS
+	mov	\tmp, #ICC_PMR_EL1_MASKED
+	msr_s	ICC_PMR_EL1, \tmp
+	isb
+#else
 	msr	daifset, #2
+#endif
 	.endm
 
-	.macro	enable_irq
+	.macro	enable_irq, tmp
+#ifdef CONFIG_USE_ICC_SYSREGS_FOR_IRQFLAGS
+	enable_nmi
+	mov     \tmp, #ICC_PMR_EL1_UNMASKED
+	msr_s   ICC_PMR_EL1, \tmp
+	isb
+#else
 	msr	daifclr, #2
+#endif
 	.endm
 
 /*
  * Save/disable and restore interrupts.
  */
-	.macro	save_and_disable_irqs, olddaif
-	mrs	\olddaif, daif
-	disable_irq
+	.macro	save_and_disable_irqs, olddaif, tmp
+#ifdef CONFIG_USE_ICC_SYSREGS_FOR_IRQFLAGS
+	mrs_s	\olddaif, ICC_PMR_EL1		/* Get PMR		      */
+#else
+	mrs	\olddaif, daif			/* Get flags		      */
+#endif
+	disable_irq \tmp
 	.endm
 
 	.macro	restore_irqs, olddaif
+#ifdef CONFIG_USE_ICC_SYSREGS_FOR_IRQFLAGS
+	msr_s	ICC_PMR_EL1, \olddaif		  /* Write to PMR	      */
+	isb
+#else
 	msr	daif, \olddaif
+#endif
 	.endm
 
 /*
@@ -90,13 +144,19 @@
 9990:
 	.endm
 
+
 /*
  * Enable both debug exceptions and interrupts. This is likely to be
  * faster than two daifclr operations, since writes to this register
  * are self-synchronising.
  */
-	.macro	enable_dbg_and_irq
+	.macro	enable_dbg_and_irq, tmp
+#ifdef CONFIG_USE_ICC_SYSREGS_FOR_IRQFLAGS
+	enable_dbg
+	enable_irq \tmp
+#else
 	msr	daifclr, #(8 | 2)
+#endif
 	.endm
 
 /*
diff --git a/arch/arm64/include/asm/irqflags.h b/arch/arm64/include/asm/irqflags.h
index df7477af6389..7b6866022f82 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,93 @@ static inline int arch_irqs_disabled_flags(unsigned long flags)
 	return flags & PSR_I_BIT;
 }
 
+#else /* CONFIG_IRQFLAGS_GIC_MASKING */
+
+#include <linux/irqchip/arm-gic-v3.h>
+
+/*
+ * 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"
+		: "=&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"
+		:
+		: "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"
+		:
+		: "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"
+		: "=r" (flags)
+		:
+		: "memory");
+
+	return flags;
+}
+
+/*
+ * restore saved IRQ state
+ */
+static inline void arch_local_irq_restore(unsigned long flags)
+{
+	asm volatile(
+		"// arch_local_irq_restore\n"
+		"msr_s  " __stringify(ICC_PMR_EL1) ",%0\n"
+		"isb\n"
+	:
+	: "r" (flags)
+	: "memory");
+}
+
+static inline int arch_irqs_disabled_flags(unsigned long flags)
+{
+	return !(flags & ICC_PMR_EL1_G_BIT);
+}
+
+#endif /* CONFIG_IRQFLAGS_GIC_MASKING */
+
 #define local_fiq_enable()	asm("msr	daifclr, #1" : : : "memory")
 #define local_fiq_disable()	asm("msr	daifset, #1" : : : "memory")
 
diff --git a/arch/arm64/include/asm/ptrace.h b/arch/arm64/include/asm/ptrace.h
index a379596e0888..da4c593b596d 100644
--- a/arch/arm64/include/asm/ptrace.h
+++ b/arch/arm64/include/asm/ptrace.h
@@ -25,6 +25,16 @@
 #define CurrentEL_EL1		(1 << 2)
 #define CurrentEL_EL2		(2 << 2)
 
+/* PMR values used to mask/unmask interrupts */
+#define ICC_PMR_EL1_G_SHIFT     6
+#define ICC_PMR_EL1_G_BIT       (1 << ICC_PMR_EL1_G_SHIFT)
+#define ICC_PMR_EL1_UNMASKED    0xf0
+#define ICC_PMR_EL1_MASKED      (ICC_PMR_EL1_UNMASKED ^ ICC_PMR_EL1_G_BIT)
+
+#define PSR_G_SHIFT             22
+#define PSR_G_PMR_G_SHIFT	(PSR_G_SHIFT - ICC_PMR_EL1_G_SHIFT)
+#define PSR_I_PMR_G_SHIFT	(7 - ICC_PMR_EL1_G_SHIFT)
+
 /* AArch32-specific ptrace requests */
 #define COMPAT_PTRACE_GETREGS		12
 #define COMPAT_PTRACE_SETREGS		13