--- /dev/null
 -#include <asm/arch_gicv3.h>
+ // SPDX-License-Identifier: GPL-2.0-only
+ /*
+  * Low-level idle sequences
+  */
+ 
+ #include <linux/cpu.h>
+ #include <linux/irqflags.h>
+ 
 -static void noinstr __cpu_do_idle(void)
 -{
 -      dsb(sy);
 -      wfi();
 -}
 -
 -static void noinstr __cpu_do_idle_irqprio(void)
 -{
 -      unsigned long pmr;
 -      unsigned long daif_bits;
 -
 -      daif_bits = read_sysreg(daif);
 -      write_sysreg(daif_bits | PSR_I_BIT | PSR_F_BIT, daif);
 -
 -      /*
 -       * Unmask PMR before going idle to make sure interrupts can
 -       * be raised.
 -       */
 -      pmr = gic_read_pmr();
 -      gic_write_pmr(GIC_PRIO_IRQON | GIC_PRIO_PSR_I_SET);
 -
 -      __cpu_do_idle();
 -
 -      gic_write_pmr(pmr);
 -      write_sysreg(daif_bits, daif);
 -}
 -
+ #include <asm/barrier.h>
++#include <asm/cpuidle.h>
+ #include <asm/cpufeature.h>
+ #include <asm/sysreg.h>
+ 
 -      if (system_uses_irq_prio_masking())
 -              __cpu_do_idle_irqprio();
 -      else
 -              __cpu_do_idle();
+ /*
+  *    cpu_do_idle()
+  *
+  *    Idle the processor (wait for interrupt).
+  *
+  *    If the CPU supports priority masking we must do additional work to
+  *    ensure that interrupts are not masked at the PMR (because the core will
+  *    not wake up if we block the wake up signal in the interrupt controller).
+  */
+ void noinstr cpu_do_idle(void)
+ {
++      struct arm_cpuidle_irq_context context;
++
++      arm_cpuidle_save_irq_context(&context);
++
++      dsb(sy);
++      wfi();
++
++      arm_cpuidle_restore_irq_context(&context);
+ }
+ 
+ /*
+  * This is our default idle handler.
+  */
+ void noinstr arch_cpu_idle(void)
+ {
+       /*
+        * This should do all the clock switching and wait for interrupt
+        * tricks
+        */
+       cpu_do_idle();
+       raw_local_irq_enable();
+ }