[PATCH v3 0/3] MCPM/TC2 support for CPU powerdown synchronisation

[PATCH v3 0/3] MCPM/TC2 support for CPU powerdown synchronisation

[Dave Martin]

This series adds MCPM support for detecting when a CPU is safely powered
down, and provides an implementation for TC2.

It should be possible to implement the same thing for PSCI using the
AFFINITY_INFO call (I need to check the semantics with Charles)

This is sufficient to for working kexec with real power management on
TC2.  To test it, you'll also need:

  * CONFIG_KEXEC=y
  * CONFIG_PROC_DEVICE_TREE=y
  * CONFIG_MCPM=y
  * CONFIG_ARCH_VEXPRESS_TC2_PM=y
  * sufficiently new kexec-tools
    (git://git.kernel.org/pub/scm/utils/kernel/kexec/kexec-tools.git
    v2.0.4 worked for me)

This build on Nico's patch
http://www.arm.linux.org.uk/developer/patches/viewpatch.php?id=7842/1
(MCPM: don't explode if invoked without being initialized first)

To prevent CPUs from running off into the weeds across kexec, this
series requires Lorenzo's patch
http://lists.infradead.org/pipermail/linux-arm-kernel/2013-September/200917.html
(arm: vexpress: tc2: fix hotplug/idle/kexec race on cluster power down).

Changes since v2:

 * Return a proper failure code if mcpm_cpu_power_down_finish() is
   called with no mcpm platforms_ops registered, or a NULL
   power_down_finish() method.  (Thanks again Nico)

 * Minor refactoring of the loop in tc2_pm_power_down_finish() to
   avoid the goto.

Changes between v1 and v2:

 * "Fix" erroneous documentation comment by switching to -errno return
   value convention for power_down_finish(), which is more informative.
   tc2_pm now returns -ETIMEDOUT on timeout.  The return is adapted to
   bool convention on return from smp_ops.cpu_kill() instead.  (Thanks,
   Nico).

 * For consistency, BUG_ON out of range cpu or cluster values
   tc2_pm_power_down_finish(), as for tc2_pm_power_down().

Changes between RFC and v1:

 * Print a big fat warning instead of branching to null if the
   power_down_finish() method is not supplied by the backend, or not
   registered.

 * Add a generous timeout of 1 second for the TC2 implementation.

 * Relax the polling interval to 10ms for TC2, since the need to poll
   more than once is rare and this is not a performance-critical path.

 * Fix some silly typos.


Dave Martin (3):
  ARM: mcpm: Factor out logical-to-physical CPU translation
  ARM: mcpm: Implement cpu_kill() to synchronise on powerdown
  ARM: vexpress/TC2: Implement MCPM power_down_finish()

 arch/arm/common/mcpm_entry.c    |   15 +++++++++
 arch/arm/common/mcpm_platsmp.c  |   27 +++++++++++++---
 arch/arm/include/asm/mcpm.h     |   31 ++++++++++++++++++
 arch/arm/mach-vexpress/spc.c    |   39 +++++++++++++++++++++++
 arch/arm/mach-vexpress/spc.h    |    1 +
 arch/arm/mach-vexpress/tc2_pm.c |   66 ++++++++++++++++++++++++++++++++++++---
 6 files changed, 170 insertions(+), 9 deletions(-)

-- 
1.7.9.5

[PATCH v3 1/3] ARM: mcpm: Factor out logical-to-physical CPU translation

[Dave Martin]

This patch factors the logical-to-physical CPU translation out of
mcpm_boot_secondary(), so that it can be reused elsewhere.

Signed-off-by: Dave Martin <Dave.Martin@arm.com>
Acked-by: Nicolas Pitre <nico@linaro.org>
---
 arch/arm/common/mcpm_platsmp.c |   17 +++++++++++++----
 1 file changed, 13 insertions(+), 4 deletions(-)

diff --git a/arch/arm/common/mcpm_platsmp.c b/arch/arm/common/mcpm_platsmp.c
index 1bc34c7..c0c3cd7 100644
--- a/arch/arm/common/mcpm_platsmp.c
+++ b/arch/arm/common/mcpm_platsmp.c
@@ -19,14 +19,23 @@
 #include <asm/smp.h>
 #include <asm/smp_plat.h>
 
+static void cpu_to_pcpu(unsigned int cpu,
+			unsigned int *pcpu, unsigned int *pcluster)
+{
+	unsigned int mpidr;
+
+	mpidr = cpu_logical_map(cpu);
+	*pcpu = MPIDR_AFFINITY_LEVEL(mpidr, 0);
+	*pcluster = MPIDR_AFFINITY_LEVEL(mpidr, 1);
+}
+
 static int mcpm_boot_secondary(unsigned int cpu, struct task_struct *idle)
 {
-	unsigned int mpidr, pcpu, pcluster, ret;
+	unsigned int pcpu, pcluster, ret;
 	extern void secondary_startup(void);
 
-	mpidr = cpu_logical_map(cpu);
-	pcpu = MPIDR_AFFINITY_LEVEL(mpidr, 0);
-	pcluster = MPIDR_AFFINITY_LEVEL(mpidr, 1);
+	cpu_to_pcpu(cpu, &pcpu, &pcluster);
+
 	pr_debug("%s: logical CPU %d is physical CPU %d cluster %d\n",
 		 __func__, cpu, pcpu, pcluster);
 
-- 
1.7.9.5

[PATCH v3 2/3] ARM: mcpm: Implement cpu_kill() to synchronise on powerdown

[Dave Martin]

CPU hotplug and kexec rely on smp_ops.cpu_kill(), which is supposed
to wait for the CPU to park or power down, and perform the last
rites (such as disabling clocks etc., where the platform doesn't do
this automatically).

kexec in particular is unsafe without performing this
synchronisation to park secondaries.  Without it, the secondaries
might not be parked when kexec trashes the kernel.

There is no generic way to do this synchronisation, so a new mcpm
platform_ops method power_down_finish() is added by this patch.

The new method is mandatory.  A platform which provides no way to
detect when CPUs are parked is likely broken.

Signed-off-by: Dave Martin <Dave.Martin@arm.com>
---
 arch/arm/common/mcpm_entry.c   |   15 +++++++++++++++
 arch/arm/common/mcpm_platsmp.c |   10 ++++++++++
 arch/arm/include/asm/mcpm.h    |   31 +++++++++++++++++++++++++++++++
 3 files changed, 56 insertions(+)

diff --git a/arch/arm/common/mcpm_entry.c b/arch/arm/common/mcpm_entry.c
index 9902509..6c03d01 100644
--- a/arch/arm/common/mcpm_entry.c
+++ b/arch/arm/common/mcpm_entry.c
@@ -90,6 +90,21 @@ void mcpm_cpu_power_down(void)
 	BUG();
 }
 
+int mcpm_cpu_power_down_finish(unsigned int cpu, unsigned int cluster)
+{
+	int ret;
+
+	if (WARN_ON_ONCE(!platform_ops || !platform_ops->power_down_finish))
+		return -EUNATCH;
+
+	ret = platform_ops->power_down_finish(cpu, cluster);
+	if (ret)
+		pr_warn("%s: cpu %u, cluster %u failed to power down (%d)\n",
+			__func__, cpu, cluster, ret);
+
+	return ret;
+}
+
 void mcpm_cpu_suspend(u64 expected_residency)
 {
 	phys_reset_t phys_reset;
diff --git a/arch/arm/common/mcpm_platsmp.c b/arch/arm/common/mcpm_platsmp.c
index c0c3cd7..177251a 100644
--- a/arch/arm/common/mcpm_platsmp.c
+++ b/arch/arm/common/mcpm_platsmp.c
@@ -56,6 +56,15 @@ static void mcpm_secondary_init(unsigned int cpu)
 
 #ifdef CONFIG_HOTPLUG_CPU
 
+static int mcpm_cpu_kill(unsigned int cpu)
+{
+	unsigned int pcpu, pcluster;
+
+	cpu_to_pcpu(cpu, &pcpu, &pcluster);
+
+	return !mcpm_cpu_power_down_finish(pcpu, pcluster);
+}
+
 static int mcpm_cpu_disable(unsigned int cpu)
 {
 	/*
@@ -82,6 +91,7 @@ static struct smp_operations __initdata mcpm_smp_ops = {
 	.smp_boot_secondary	= mcpm_boot_secondary,
 	.smp_secondary_init	= mcpm_secondary_init,
 #ifdef CONFIG_HOTPLUG_CPU
+	.cpu_kill		= mcpm_cpu_kill,
 	.cpu_disable		= mcpm_cpu_disable,
 	.cpu_die		= mcpm_cpu_die,
 #endif
diff --git a/arch/arm/include/asm/mcpm.h b/arch/arm/include/asm/mcpm.h
index fc82a88..1cf2601 100644
--- a/arch/arm/include/asm/mcpm.h
+++ b/arch/arm/include/asm/mcpm.h
@@ -81,10 +81,40 @@ int mcpm_cpu_power_up(unsigned int cpu, unsigned int cluster);
  *
  * This will return if mcpm_platform_register() has not been called
  * previously in which case the caller should take appropriate action.
+ *
+ * On success, the CPU is not guaranteed to be truly halted until
+ * mcpm_cpu_power_down_finish() subsequently returns non-zero for the
+ * specified cpu.  Until then, other CPUs should make sure they do not
+ * trash memory the target CPU might be executing/accessing.
  */
 void mcpm_cpu_power_down(void);
 
 /**
+ * mcpm_cpu_power_down_finish - wait for a specified CPU to halt, and
+ *	make sure it is powered off
+ *
+ * @cpu: CPU number within given cluster
+ * @cluster: cluster number for the CPU
+ *
+ * Call this function to ensure that a pending powerdown has taken
+ * effect and the CPU is safely parked before performing non-mcpm
+ * operations that may affect the CPU (such as kexec trashing the
+ * kernel text).
+ *
+ * It is *not* necessary to call this function if you only need to
+ * serialise a pending powerdown with mcpm_cpu_power_up() or a wakeup
+ * event.
+ *
+ * Do not call this function unless the specified CPU has already
+ * called mcpm_cpu_power_down() or has committed to doing so.
+ *
+ * @return:
+ *	- zero if the CPU is in a safely parked state
+ *	- nonzero otherwise (e.g., timeout)
+ */
+int mcpm_cpu_power_down_finish(unsigned int cpu, unsigned int cluster);
+
+/**
  * mcpm_cpu_suspend - bring the calling CPU in a suspended state
  *
  * @expected_residency: duration in microseconds the CPU is expected
@@ -126,6 +156,7 @@ int mcpm_cpu_powered_up(void);
 struct mcpm_platform_ops {
 	int (*power_up)(unsigned int cpu, unsigned int cluster);
 	void (*power_down)(void);
+	int (*power_down_finish)(unsigned int cpu, unsigned int cluster);
 	void (*suspend)(u64);
 	void (*powered_up)(void);
 };
-- 
1.7.9.5

[PATCH v3 2/3] ARM: mcpm: Implement cpu_kill() to synchronise on powerdown

[Nicolas Pitre]

On Tue, 1 Oct 2013, Dave Martin wrote:

> CPU hotplug and kexec rely on smp_ops.cpu_kill(), which is supposed
> to wait for the CPU to park or power down, and perform the last
> rites (such as disabling clocks etc., where the platform doesn't do
> this automatically).
> 
> kexec in particular is unsafe without performing this
> synchronisation to park secondaries.  Without it, the secondaries
> might not be parked when kexec trashes the kernel.
> 
> There is no generic way to do this synchronisation, so a new mcpm
> platform_ops method power_down_finish() is added by this patch.
> 
> The new method is mandatory.  A platform which provides no way to
> detect when CPUs are parked is likely broken.
> 
> Signed-off-by: Dave Martin <Dave.Martin@arm.com>

Reviewed-by: Nicolas Pitre <nico@linaro.org>

> ---
>  arch/arm/common/mcpm_entry.c   |   15 +++++++++++++++
>  arch/arm/common/mcpm_platsmp.c |   10 ++++++++++
>  arch/arm/include/asm/mcpm.h    |   31 +++++++++++++++++++++++++++++++
>  3 files changed, 56 insertions(+)
> 
> diff --git a/arch/arm/common/mcpm_entry.c b/arch/arm/common/mcpm_entry.c
> index 9902509..6c03d01 100644
> --- a/arch/arm/common/mcpm_entry.c
> +++ b/arch/arm/common/mcpm_entry.c
> @@ -90,6 +90,21 @@ void mcpm_cpu_power_down(void)
>  	BUG();
>  }
>  
> +int mcpm_cpu_power_down_finish(unsigned int cpu, unsigned int cluster)
> +{
> +	int ret;
> +
> +	if (WARN_ON_ONCE(!platform_ops || !platform_ops->power_down_finish))
> +		return -EUNATCH;
> +
> +	ret = platform_ops->power_down_finish(cpu, cluster);
> +	if (ret)
> +		pr_warn("%s: cpu %u, cluster %u failed to power down (%d)\n",
> +			__func__, cpu, cluster, ret);
> +
> +	return ret;
> +}
> +
>  void mcpm_cpu_suspend(u64 expected_residency)
>  {
>  	phys_reset_t phys_reset;
> diff --git a/arch/arm/common/mcpm_platsmp.c b/arch/arm/common/mcpm_platsmp.c
> index c0c3cd7..177251a 100644
> --- a/arch/arm/common/mcpm_platsmp.c
> +++ b/arch/arm/common/mcpm_platsmp.c
> @@ -56,6 +56,15 @@ static void mcpm_secondary_init(unsigned int cpu)
>  
>  #ifdef CONFIG_HOTPLUG_CPU
>  
> +static int mcpm_cpu_kill(unsigned int cpu)
> +{
> +	unsigned int pcpu, pcluster;
> +
> +	cpu_to_pcpu(cpu, &pcpu, &pcluster);
> +
> +	return !mcpm_cpu_power_down_finish(pcpu, pcluster);
> +}
> +
>  static int mcpm_cpu_disable(unsigned int cpu)
>  {
>  	/*
> @@ -82,6 +91,7 @@ static struct smp_operations __initdata mcpm_smp_ops = {
>  	.smp_boot_secondary	= mcpm_boot_secondary,
>  	.smp_secondary_init	= mcpm_secondary_init,
>  #ifdef CONFIG_HOTPLUG_CPU
> +	.cpu_kill		= mcpm_cpu_kill,
>  	.cpu_disable		= mcpm_cpu_disable,
>  	.cpu_die		= mcpm_cpu_die,
>  #endif
> diff --git a/arch/arm/include/asm/mcpm.h b/arch/arm/include/asm/mcpm.h
> index fc82a88..1cf2601 100644
> --- a/arch/arm/include/asm/mcpm.h
> +++ b/arch/arm/include/asm/mcpm.h
> @@ -81,10 +81,40 @@ int mcpm_cpu_power_up(unsigned int cpu, unsigned int cluster);
>   *
>   * This will return if mcpm_platform_register() has not been called
>   * previously in which case the caller should take appropriate action.
> + *
> + * On success, the CPU is not guaranteed to be truly halted until
> + * mcpm_cpu_power_down_finish() subsequently returns non-zero for the
> + * specified cpu.  Until then, other CPUs should make sure they do not
> + * trash memory the target CPU might be executing/accessing.
>   */
>  void mcpm_cpu_power_down(void);
>  
>  /**
> + * mcpm_cpu_power_down_finish - wait for a specified CPU to halt, and
> + *	make sure it is powered off
> + *
> + * @cpu: CPU number within given cluster
> + * @cluster: cluster number for the CPU
> + *
> + * Call this function to ensure that a pending powerdown has taken
> + * effect and the CPU is safely parked before performing non-mcpm
> + * operations that may affect the CPU (such as kexec trashing the
> + * kernel text).
> + *
> + * It is *not* necessary to call this function if you only need to
> + * serialise a pending powerdown with mcpm_cpu_power_up() or a wakeup
> + * event.
> + *
> + * Do not call this function unless the specified CPU has already
> + * called mcpm_cpu_power_down() or has committed to doing so.
> + *
> + * @return:
> + *	- zero if the CPU is in a safely parked state
> + *	- nonzero otherwise (e.g., timeout)
> + */
> +int mcpm_cpu_power_down_finish(unsigned int cpu, unsigned int cluster);
> +
> +/**
>   * mcpm_cpu_suspend - bring the calling CPU in a suspended state
>   *
>   * @expected_residency: duration in microseconds the CPU is expected
> @@ -126,6 +156,7 @@ int mcpm_cpu_powered_up(void);
>  struct mcpm_platform_ops {
>  	int (*power_up)(unsigned int cpu, unsigned int cluster);
>  	void (*power_down)(void);
> +	int (*power_down_finish)(unsigned int cpu, unsigned int cluster);
>  	void (*suspend)(u64);
>  	void (*powered_up)(void);
>  };
> -- 
> 1.7.9.5
> 

[PATCH v3 2/3] ARM: mcpm: Implement cpu_kill() to synchronise on powerdown

[Dave Martin]

On Tue, Oct 01, 2013 at 01:35:17PM -0400, Nicolas Pitre wrote:
> On Tue, 1 Oct 2013, Dave Martin wrote:
> 
> > CPU hotplug and kexec rely on smp_ops.cpu_kill(), which is supposed
> > to wait for the CPU to park or power down, and perform the last
> > rites (such as disabling clocks etc., where the platform doesn't do
> > this automatically).
> > 
> > kexec in particular is unsafe without performing this
> > synchronisation to park secondaries.  Without it, the secondaries
> > might not be parked when kexec trashes the kernel.
> > 
> > There is no generic way to do this synchronisation, so a new mcpm
> > platform_ops method power_down_finish() is added by this patch.
> > 
> > The new method is mandatory.  A platform which provides no way to
> > detect when CPUs are parked is likely broken.
> > 
> > Signed-off-by: Dave Martin <Dave.Martin@arm.com>
> 
> Reviewed-by: Nicolas Pitre <nico@linaro.org>

Thanks!

I can recommend kexec -- speeds up my reboots no end.


I can send the MCPM patches to the patch system, but the TC2 patch
probably ought to go via arm-soc when Russell has a stable branch
available.

Lorenzo's patch fixes something relevant but independent, so should be
mergeable separately.

Any concerns?

Cheers
---Dave

[PATCH v3 2/3] ARM: mcpm: Implement cpu_kill() to synchronise on powerdown

[Nicolas Pitre]

On Tue, 1 Oct 2013, Dave Martin wrote:

> On Tue, Oct 01, 2013 at 01:35:17PM -0400, Nicolas Pitre wrote:
> > On Tue, 1 Oct 2013, Dave Martin wrote:
> > 
> > > CPU hotplug and kexec rely on smp_ops.cpu_kill(), which is supposed
> > > to wait for the CPU to park or power down, and perform the last
> > > rites (such as disabling clocks etc., where the platform doesn't do
> > > this automatically).
> > > 
> > > kexec in particular is unsafe without performing this
> > > synchronisation to park secondaries.  Without it, the secondaries
> > > might not be parked when kexec trashes the kernel.
> > > 
> > > There is no generic way to do this synchronisation, so a new mcpm
> > > platform_ops method power_down_finish() is added by this patch.
> > > 
> > > The new method is mandatory.  A platform which provides no way to
> > > detect when CPUs are parked is likely broken.
> > > 
> > > Signed-off-by: Dave Martin <Dave.Martin@arm.com>
> > 
> > Reviewed-by: Nicolas Pitre <nico@linaro.org>
> 
> Thanks!
> 
> I can recommend kexec -- speeds up my reboots no end.
> 
> 
> I can send the MCPM patches to the patch system, but the TC2 patch
> probably ought to go via arm-soc when Russell has a stable branch
> available.

Agreed.  You may ask Russell explicitly for it.

Yet my patch #7842/1 upon which yours are based should go upstream 
before v3.12-final whereas your patches are meant for the next merge 
window.  This should be mentioned explicitly to avoid potential 
confusion.

> Lorenzo's patch fixes something relevant but independent, so should be
> mergeable separately.

Agreed.  And it is on its way already.


Nicolas

[PATCH v3 3/3] ARM: vexpress/TC2: Implement MCPM power_down_finish()

[Dave Martin]

This patch implements the power_down_finish() method for TC2, to
enable the kernel to confirm when CPUs are safely powered down.

The information required for determining when a CPU is parked
cannot be obtained from any single place, so a few sources of
information must be combined:

  * mcpm_cpu_power_down() must be pending for the CPU, so that we
    don't get confused by false STANDBYWFI positives arising from
    CPUidle.  This is detected by waiting for the tc2_pm use count
    for the target CPU to reach 0.

  * Either the SPC must report that the CPU has asserted
    STANDBYWFI, or the TC2 tile's reset control logic must be
    holding the CPU in reset.

    Just checking for STANDBYWFI is not sufficient, because this
    signal is not latched when the the cluster is clamped off and
    powered down: the relevant status bits just drop to zero.  This
    means that STANDBYWFI status cannot be used for reliable
    detection of the last CPU in a cluster reaching WFI.

This patch is required in order for kexec to work with MCPM on TC2.

Signed-off-by: Dave Martin <Dave.Martin@arm.com>
Acked-by: Nicolas Pitre <nico@linaro.org>
---
 arch/arm/mach-vexpress/spc.c    |   39 +++++++++++++++++++++++
 arch/arm/mach-vexpress/spc.h    |    1 +
 arch/arm/mach-vexpress/tc2_pm.c |   66 ++++++++++++++++++++++++++++++++++++---
 3 files changed, 101 insertions(+), 5 deletions(-)

diff --git a/arch/arm/mach-vexpress/spc.c b/arch/arm/mach-vexpress/spc.c
index eefb029..6f6ac56 100644
--- a/arch/arm/mach-vexpress/spc.c
+++ b/arch/arm/mach-vexpress/spc.c
@@ -35,6 +35,10 @@
 /* SPC per-CPU mailboxes */
 #define A15_BX_ADDR0		0x68
 #define A7_BX_ADDR0		0x78
+/* SPC CPU/cluster reset statue */
+#define STANDBYWFI_STAT		0x3c
+#define STANDBYWFI_STAT_A15_CPU_MASK(cpu)	(1 << (cpu))
+#define STANDBYWFI_STAT_A7_CPU_MASK(cpu)	(1 << (3 + (cpu)))
 
 /* wake-up interrupt masks */
 #define GBL_WAKEUP_INT_MSK	(0x3 << 10)
@@ -157,6 +161,41 @@ void ve_spc_powerdown(u32 cluster, bool enable)
 	writel_relaxed(enable, info->baseaddr + pwdrn_reg);
 }
 
+static u32 standbywfi_cpu_mask(u32 cpu, u32 cluster)
+{
+	return cluster_is_a15(cluster) ?
+		  STANDBYWFI_STAT_A15_CPU_MASK(cpu)
+		: STANDBYWFI_STAT_A7_CPU_MASK(cpu);
+}
+
+/**
+ * ve_spc_cpu_in_wfi(u32 cpu, u32 cluster)
+ *
+ * @cpu: mpidr[7:0] bitfield describing CPU affinity level within cluster
+ * @cluster: mpidr[15:8] bitfield describing cluster affinity level
+ *
+ * @return: non-zero if and only if the specified CPU is in WFI
+ *
+ * Take care when interpreting the result of this function: a CPU might
+ * be in WFI temporarily due to idle, and is not necessarily safely
+ * parked.
+ */
+int ve_spc_cpu_in_wfi(u32 cpu, u32 cluster)
+{
+	int ret;
+	u32 mask = standbywfi_cpu_mask(cpu, cluster);
+
+	if (cluster >= MAX_CLUSTERS)
+		return 1;
+
+	ret = readl_relaxed(info->baseaddr + STANDBYWFI_STAT);
+
+	pr_debug("%s: PCFGREG[0x%X] = 0x%08X, mask = 0x%X\n",
+		 __func__, STANDBYWFI_STAT, ret, mask);
+
+	return ret & mask;
+}
+
 int __init ve_spc_init(void __iomem *baseaddr, u32 a15_clusid)
 {
 	info = kzalloc(sizeof(*info), GFP_KERNEL);
diff --git a/arch/arm/mach-vexpress/spc.h b/arch/arm/mach-vexpress/spc.h
index 5f7e4a4..edb1b06 100644
--- a/arch/arm/mach-vexpress/spc.h
+++ b/arch/arm/mach-vexpress/spc.h
@@ -20,5 +20,6 @@ void ve_spc_global_wakeup_irq(bool set);
 void ve_spc_cpu_wakeup_irq(u32 cluster, u32 cpu, bool set);
 void ve_spc_set_resume_addr(u32 cluster, u32 cpu, u32 addr);
 void ve_spc_powerdown(u32 cluster, bool enable);
+int ve_spc_cpu_in_wfi(u32 cpu, u32 cluster);
 
 #endif
diff --git a/arch/arm/mach-vexpress/tc2_pm.c b/arch/arm/mach-vexpress/tc2_pm.c
index e6eb481..cae43db 100644
--- a/arch/arm/mach-vexpress/tc2_pm.c
+++ b/arch/arm/mach-vexpress/tc2_pm.c
@@ -12,6 +12,7 @@
  * published by the Free Software Foundation.
  */
 
+#include <linux/delay.h>
 #include <linux/init.h>
 #include <linux/io.h>
 #include <linux/kernel.h>
@@ -31,11 +32,17 @@
 #include "spc.h"
 
 /* SCC conf registers */
+#define RESET_CTRL		0x018
+#define RESET_A15_NCORERESET(cpu)	(1 << (2 + (cpu)))
+#define RESET_A7_NCORERESET(cpu)	(1 << (19 + (cpu)))
+
 #define A15_CONF		0x400
 #define A7_CONF			0x500
 #define SYS_INFO		0x700
 #define SPC_BASE		0xb00
 
+static void __iomem *scc;
+
 /*
  * We can't use regular spinlocks. In the switcher case, it is possible
  * for an outbound CPU to call power_down() after its inbound counterpart
@@ -233,6 +240,55 @@ static void tc2_pm_power_down(void)
 	tc2_pm_down(0);
 }
 
+static int tc2_core_in_reset(unsigned int cpu, unsigned int cluster)
+{
+	u32 mask = cluster ?
+		  RESET_A7_NCORERESET(cpu)
+		: RESET_A15_NCORERESET(cpu);
+
+	return !(readl_relaxed(scc + RESET_CTRL) & mask);
+}
+
+#define POLL_MSEC 10
+#define TIMEOUT_MSEC 1000
+
+static int tc2_pm_power_down_finish(unsigned int cpu, unsigned int cluster)
+{
+	unsigned tries;
+
+	pr_debug("%s: cpu %u cluster %u\n", __func__, cpu, cluster);
+	BUG_ON(cluster >= TC2_CLUSTERS || cpu >= TC2_MAX_CPUS_PER_CLUSTER);
+
+	for (tries = 0; tries < TIMEOUT_MSEC / POLL_MSEC; ++tries) {
+		/*
+		 * Only examine the hardware state if the target CPU has
+		 * caught up@least as far as tc2_pm_down():
+		 */
+		if (ACCESS_ONCE(tc2_pm_use_count[cpu][cluster]) == 0) {
+			pr_debug("%s(cpu=%u, cluster=%u): RESET_CTRL = 0x%08X\n",
+				 __func__, cpu, cluster,
+				 readl_relaxed(scc + RESET_CTRL));
+
+			/*
+			 * We need the CPU to reach WFI, but the power
+			 * controller may put the cluster in reset and
+			 * power it off as soon as that happens, before
+			 * we have a chance to see STANDBYWFI.
+			 *
+			 * So we need to check for both conditions:
+			 */
+			if (tc2_core_in_reset(cpu, cluster) ||
+			    ve_spc_cpu_in_wfi(cpu, cluster))
+				return 0; /* success: the CPU is halted */
+		}
+
+		/* Otherwise, wait and retry: */
+		msleep(POLL_MSEC);
+	}
+
+	return -ETIMEDOUT; /* timeout */
+}
+
 static void tc2_pm_suspend(u64 residency)
 {
 	unsigned int mpidr, cpu, cluster;
@@ -275,10 +331,11 @@ static void tc2_pm_powered_up(void)
 }
 
 static const struct mcpm_platform_ops tc2_pm_power_ops = {
-	.power_up	= tc2_pm_power_up,
-	.power_down	= tc2_pm_power_down,
-	.suspend	= tc2_pm_suspend,
-	.powered_up	= tc2_pm_powered_up,
+	.power_up		= tc2_pm_power_up,
+	.power_down		= tc2_pm_power_down,
+	.power_down_finish	= tc2_pm_power_down_finish,
+	.suspend		= tc2_pm_suspend,
+	.powered_up		= tc2_pm_powered_up,
 };
 
 static bool __init tc2_pm_usage_count_init(void)
@@ -312,7 +369,6 @@ static void __naked tc2_pm_power_up_setup(unsigned int affinity_level)
 static int __init tc2_pm_init(void)
 {
 	int ret;
-	void __iomem *scc;
 	u32 a15_cluster_id, a7_cluster_id, sys_info;
 	struct device_node *np;
 
-- 
1.7.9.5