Merge tag 'arm64-upstream' of git://git.kernel.org/pub/scm/linux/kernel/git/arm64...

[linux-2.6-microblaze.git] / arch / arm64 / include / asm / irqflags.h

diff --git a/arch/arm64/include/asm/irqflags.h b/arch/arm64/include/asm/irqflags.h
index 66853fd..7872f26 100644 (file)
--- a/arch/arm64/include/asm/irqflags.h
+++ b/arch/arm64/include/asm/irqflags.h
@@ -29,6 +29,12 @@
  */
 static inline void arch_local_irq_enable(void)
 {
+       if (system_has_prio_mask_debugging()) {
+               u32 pmr = read_sysreg_s(SYS_ICC_PMR_EL1);
+
+               WARN_ON_ONCE(pmr != GIC_PRIO_IRQON && pmr != GIC_PRIO_IRQOFF);
+       }
+
        asm volatile(ALTERNATIVE(
                "msr    daifclr, #2             // arch_local_irq_enable\n"
                "nop",
@@ -42,6 +48,12 @@ static inline void arch_local_irq_enable(void)
 
 static inline void arch_local_irq_disable(void)
 {
+       if (system_has_prio_mask_debugging()) {
+               u32 pmr = read_sysreg_s(SYS_ICC_PMR_EL1);
+
+               WARN_ON_ONCE(pmr != GIC_PRIO_IRQON && pmr != GIC_PRIO_IRQOFF);
+       }
+
        asm volatile(ALTERNATIVE(
                "msr    daifset, #2             // arch_local_irq_disable",
                __msr_s(SYS_ICC_PMR_EL1, "%0"),
@@ -56,43 +68,46 @@ static inline void arch_local_irq_disable(void)
  */
 static inline unsigned long arch_local_save_flags(void)
 {
-       unsigned long daif_bits;
        unsigned long flags;
 
-       daif_bits = read_sysreg(daif);
-
-       /*
-        * The asm is logically equivalent to:
-        *
-        * if (system_uses_irq_prio_masking())
-        *      flags = (daif_bits & PSR_I_BIT) ?
-        *                      GIC_PRIO_IRQOFF :
-        *                      read_sysreg_s(SYS_ICC_PMR_EL1);
-        * else
-        *      flags = daif_bits;
-        */
        asm volatile(ALTERNATIVE(
-                       "mov    %0, %1\n"
-                       "nop\n"
-                       "nop",
-                       __mrs_s("%0", SYS_ICC_PMR_EL1)
-                       "ands   %1, %1, " __stringify(PSR_I_BIT) "\n"
-                       "csel   %0, %0, %2, eq",
-                       ARM64_HAS_IRQ_PRIO_MASKING)
-               : "=&r" (flags), "+r" (daif_bits)
-               : "r" ((unsigned long) GIC_PRIO_IRQOFF)
+               "mrs    %0, daif",
+               __mrs_s("%0", SYS_ICC_PMR_EL1),
+               ARM64_HAS_IRQ_PRIO_MASKING)
+               : "=&r" (flags)
+               :
                : "memory");
 
        return flags;
 }
 
+static inline int arch_irqs_disabled_flags(unsigned long flags)
+{
+       int res;
+
+       asm volatile(ALTERNATIVE(
+               "and    %w0, %w1, #" __stringify(PSR_I_BIT),
+               "eor    %w0, %w1, #" __stringify(GIC_PRIO_IRQON),
+               ARM64_HAS_IRQ_PRIO_MASKING)
+               : "=&r" (res)
+               : "r" ((int) flags)
+               : "memory");
+
+       return res;
+}
+
 static inline unsigned long arch_local_irq_save(void)
 {
        unsigned long flags;
 
        flags = arch_local_save_flags();
 
-       arch_local_irq_disable();
+       /*
+        * There are too many states with IRQs disabled, just keep the current
+        * state if interrupts are already disabled/masked.
+        */
+       if (!arch_irqs_disabled_flags(flags))
+               arch_local_irq_disable();
 
        return flags;
 }
@@ -108,26 +123,10 @@ static inline void arch_local_irq_restore(unsigned long flags)
                        __msr_s(SYS_ICC_PMR_EL1, "%0")
                        "dsb    sy",
                        ARM64_HAS_IRQ_PRIO_MASKING)
-               : "+r" (flags)
                :
+               : "r" (flags)
                : "memory");
 }
 
-static inline int arch_irqs_disabled_flags(unsigned long flags)
-{
-       int res;
-
-       asm volatile(ALTERNATIVE(
-                       "and    %w0, %w1, #" __stringify(PSR_I_BIT) "\n"
-                       "nop",
-                       "cmp    %w1, #" __stringify(GIC_PRIO_IRQOFF) "\n"
-                       "cset   %w0, ls",
-                       ARM64_HAS_IRQ_PRIO_MASKING)
-               : "=&r" (res)
-               : "r" ((int) flags)
-               : "memory");
-
-       return res;
-}
 #endif
 #endif