1 files changed, 82 insertions, 4 deletions

diff --git a/arch/arm/mach-tegra/flowctrl.c b/arch/arm/mach-tegra/flowctrl.c
index f07488e0bd3..ce8ab8abf06 100644
--- a/arch/arm/mach-tegra/flowctrl.c
+++ b/arch/arm/mach-tegra/flowctrl.c
@@ -21,19 +21,20 @@
 #include <linux/init.h>
 #include <linux/kernel.h>
 #include <linux/io.h>
-
-#include <mach/iomap.h>
+#include <linux/cpumask.h>
 
 #include "flowctrl.h"
+#include "iomap.h"
+#include "fuse.h"
 
-u8 flowctrl_offset_halt_cpu[] = {
+static u8 flowctrl_offset_halt_cpu[] = {
 	FLOW_CTRL_HALT_CPU0_EVENTS,
 	FLOW_CTRL_HALT_CPU1_EVENTS,
 	FLOW_CTRL_HALT_CPU1_EVENTS + 8,
 	FLOW_CTRL_HALT_CPU1_EVENTS + 16,
 };
 
-u8 flowctrl_offset_cpu_csr[] = {
+static u8 flowctrl_offset_cpu_csr[] = {
 	FLOW_CTRL_CPU0_CSR,
 	FLOW_CTRL_CPU1_CSR,
 	FLOW_CTRL_CPU1_CSR + 8,
@@ -51,6 +52,14 @@ static void flowctrl_update(u8 offset, u32 value)
 	readl_relaxed(addr);
 }
 
+u32 flowctrl_read_cpu_csr(unsigned int cpuid)
+{
+	u8 offset = flowctrl_offset_cpu_csr[cpuid];
+	void __iomem *addr = IO_ADDRESS(TEGRA_FLOW_CTRL_BASE) + offset;
+
+	return readl(addr);
+}
+
 void flowctrl_write_cpu_csr(unsigned int cpuid, u32 value)
 {
 	return flowctrl_update(flowctrl_offset_cpu_csr[cpuid], value);
@@ -60,3 +69,72 @@ void flowctrl_write_cpu_halt(unsigned int cpuid, u32 value)
 {
 	return flowctrl_update(flowctrl_offset_halt_cpu[cpuid], value);
 }
+
+void flowctrl_cpu_suspend_enter(unsigned int cpuid)
+{
+	unsigned int reg;
+	int i;
+
+	reg = flowctrl_read_cpu_csr(cpuid);
+	switch (tegra_chip_id) {
+	case TEGRA20:
+		/* clear wfe bitmap */
+		reg &= ~TEGRA20_FLOW_CTRL_CSR_WFE_BITMAP;
+		/* clear wfi bitmap */
+		reg &= ~TEGRA20_FLOW_CTRL_CSR_WFI_BITMAP;
+		/* pwr gating on wfe */
+		reg |= TEGRA20_FLOW_CTRL_CSR_WFE_CPU0 << cpuid;
+		break;
+	case TEGRA30:
+	case TEGRA114:
+	case TEGRA124:
+		/* clear wfe bitmap */
+		reg &= ~TEGRA30_FLOW_CTRL_CSR_WFE_BITMAP;
+		/* clear wfi bitmap */
+		reg &= ~TEGRA30_FLOW_CTRL_CSR_WFI_BITMAP;
+		/* pwr gating on wfi */
+		reg |= TEGRA30_FLOW_CTRL_CSR_WFI_CPU0 << cpuid;
+		break;
+	}
+	reg |= FLOW_CTRL_CSR_INTR_FLAG;			/* clear intr flag */
+	reg |= FLOW_CTRL_CSR_EVENT_FLAG;		/* clear event flag */
+	reg |= FLOW_CTRL_CSR_ENABLE;			/* pwr gating */
+	flowctrl_write_cpu_csr(cpuid, reg);
+
+	for (i = 0; i < num_possible_cpus(); i++) {
+		if (i == cpuid)
+			continue;
+		reg = flowctrl_read_cpu_csr(i);
+		reg |= FLOW_CTRL_CSR_EVENT_FLAG;
+		reg |= FLOW_CTRL_CSR_INTR_FLAG;
+		flowctrl_write_cpu_csr(i, reg);
+	}
+}
+
+void flowctrl_cpu_suspend_exit(unsigned int cpuid)
+{
+	unsigned int reg;
+
+	/* Disable powergating via flow controller for CPU0 */
+	reg = flowctrl_read_cpu_csr(cpuid);
+	switch (tegra_chip_id) {
+	case TEGRA20:
+		/* clear wfe bitmap */
+		reg &= ~TEGRA20_FLOW_CTRL_CSR_WFE_BITMAP;
+		/* clear wfi bitmap */
+		reg &= ~TEGRA20_FLOW_CTRL_CSR_WFI_BITMAP;
+		break;
+	case TEGRA30:
+	case TEGRA114:
+	case TEGRA124:
+		/* clear wfe bitmap */
+		reg &= ~TEGRA30_FLOW_CTRL_CSR_WFE_BITMAP;
+		/* clear wfi bitmap */
+		reg &= ~TEGRA30_FLOW_CTRL_CSR_WFI_BITMAP;
+		break;
+	}
+	reg &= ~FLOW_CTRL_CSR_ENABLE;			/* clear enable */
+	reg |= FLOW_CTRL_CSR_INTR_FLAG;			/* clear intr */
+	reg |= FLOW_CTRL_CSR_EVENT_FLAG;		/* clear event */
+	flowctrl_write_cpu_csr(cpuid, reg);
+}