diff --git a/arch/arm/mach-vexpress/spc.c b/arch/arm/mach-vexpress/spc.c
index 033d34dcbd3fb8a8e1325900ddecdeb64090e874..c26ef5b92ca78587ce35b0f597a9cea66f9d592a 100644
--- a/arch/arm/mach-vexpress/spc.c
+++ b/arch/arm/mach-vexpress/spc.c
@@ -53,6 +53,11 @@
#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)))
+
/* SPC system config interface registers */
#define SYSCFG_WDATA 0x70
#define SYSCFG_RDATA 0x74
@@ -213,6 +218,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;
+}
+
static int ve_spc_get_performance(int cluster, u32 *freq)
{
struct ve_spc_opp *opps = info->opps[cluster];
diff --git a/arch/arm/mach-vexpress/spc.h b/arch/arm/mach-vexpress/spc.h
index dbd44c3720f98e711e5cabf28e6734847d8c7ea8..793d065243b9e469300be86a3325f777d7a37b4c 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 05a364c5077a7a40f4c3a348bced3a70e22ea280..29e7785a54bcbbb3e4e7fa3f2f46180e430ad91c 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>
@@ -32,11 +33,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 << (16 + (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
@@ -190,6 +197,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 at 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;
@@ -232,10 +288,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)
@@ -269,7 +326,6 @@ static void __naked tc2_pm_power_up_setup(unsigned int affinity_level)
static int __init tc2_pm_init(void)
{
int ret, irq;
- void __iomem *scc;
u32 a15_cluster_id, a7_cluster_id, sys_info;
struct device_node *np;