diff --git a/arch/arm/cpu/armv7/sunxi/psci.c b/arch/arm/cpu/armv7/sunxi/psci.c index 1ac50f558a4..3448fe2edca 100644 --- a/arch/arm/cpu/armv7/sunxi/psci.c +++ b/arch/arm/cpu/armv7/sunxi/psci.c @@ -38,6 +38,15 @@ #define SUN8I_R40_PWR_CLAMP(cpu) (0x120 + (cpu) * 0x4) #define SUN8I_R40_SRAMC_SOFT_ENTRY_REG0 (0xbc) +static inline u32 __secure cp15_read_mpidr(void) +{ + u32 val; + + asm volatile ("mrc p15, 0, %0, c0, c0, 5" : "=r" (val)); + + return val; +} + static void __secure cp15_write_cntp_tval(u32 tval) { asm volatile ("mcr p15, 0, %0, c14, c2, 0" : : "r" (tval)); @@ -281,9 +290,14 @@ s32 __secure psci_cpu_off(void) { psci_cpu_off_common(); - /* Ask CPU0 via SGI15 to pull the rug... */ - writel(BIT(16) | 15, GICD_BASE + GICD_SGIR); - dsb(); + if (cp15_read_mpidr() & 3) { + /* Ask CPU0 via SGI15 to pull the rug... */ + writel(BIT(16) | 15, GICD_BASE + GICD_SGIR); + dsb(); + } else { + /* Unmask FIQs to service SGI15. */ + asm volatile ("cpsie f"); + } /* Wait to be turned off */ while (1)