ARM: TC2: reset CPUs spuriously woken up on cluster power up

On TC2, all CPUs in a cluster are woken up when an IRQ event triggers for a
CPU in a cluster in shutdown state.

This patch puts spuriously woken CPUs back in reset by checking the
pending IRQ status in the SPC wake-up interrupt status register; if the
CPU has no pending IRQ routed to it, the core reexecutes wfi and it is put
in reset by FW straight away.

Tested-by: Viresh Kumar <viresh.kumar2@arm.com>
Signed-off-by: Lorenzo Pieralisi <lorenzo.pieralisi@arm.com>

diff --git a/arch/arm/mach-vexpress/tc2_pm.c b/arch/arm/mach-vexpress/tc2_pm.c
index a5e18df801db1262fbfb2518be80a13aa1d32cb7..54757301ca5a4302b099c045e9c0b74f8fb2fecf 100644 (file)
--- a/arch/arm/mach-vexpress/tc2_pm.c
+++ b/arch/arm/mach-vexpress/tc2_pm.c
@@ -172,13 +172,15 @@ static void tc2_pm_power_down(void)
 
 static void tc2_pm_suspend(u64 residency)
 {
+       extern void tc2_resume(void);
        unsigned int mpidr, cpu, cluster;
 
        mpidr = read_cpuid_mpidr();
        cpu = MPIDR_AFFINITY_LEVEL(mpidr, 0);
        cluster = MPIDR_AFFINITY_LEVEL(mpidr, 1);
        vexpress_spc_write_bxaddr_reg(cluster, cpu,
-                                     virt_to_phys(mcpm_entry_point));
+                                     virt_to_phys(tc2_resume));
+
        tc2_pm_down(residency);
 }
 

diff --git a/arch/arm/mach-vexpress/tc2_pm_setup.S b/arch/arm/mach-vexpress/tc2_pm_setup.S
index 046890e5ff8485339b4b75dec7873d819ca50780..4728f83731a4119272491fa33642f9eb2d6b51b6 100644 (file)
--- a/arch/arm/mach-vexpress/tc2_pm_setup.S
+++ b/arch/arm/mach-vexpress/tc2_pm_setup.S
@@ -16,6 +16,7 @@
 
 
 #define SPC_PHYS_BASE          0x7FFF0000
+#define SPC_WAKE_INT_STAT      0xb2c
 
 #define SNOOP_CTL_A15          0x404
 #define SNOOP_CTL_A7           0x504
@@ -42,6 +43,19 @@
 #define CCI_A7_OFFSET          CCI_SLAVE_OFFSET(CCI_SLAVE_A7)
 
 
+ENTRY(tc2_resume)
+       mrc     p15, 0, r0, c0, c0, 5
+       ubfx    r1, r0, #0, #4          @ r1 = cpu
+       ubfx    r2, r0, #8, #4          @ r2 = cluster
+       add     r1, r1, r2, lsl #2      @ r1 = index of CPU in WAKE_INT_STAT
+       ldr     r3, =SPC_PHYS_BASE + SPC_WAKE_INT_STAT
+       ldr     r3, [r3]
+       lsr     r3, r1
+       tst     r3, #1
+       wfieq                           @ if no pending IRQ reenters wfi
+       b       mcpm_entry_point
+ENDPROC(tc2_resume)
+
 /*
  * Enable cluster-level coherency, in preparation for turning on the MMU.
  * The ACTLR SMP bit does not need to be set here, because cpu_resume()