ARM: tegra30: flowctrl: add cpu_suspend_exter/exit function

The flow controller can help CPU to go into suspend mode (powered-down
state). When CPU go into powered-down state, it needs some careful
settings before getting into and after leaving. The enter and exit
functions do that by configuring appropriate mode for flow controller.

Signed-off-by: Joseph Lo <josephl@nvidia.com>
Signed-off-by: Stephen Warren <swarren@nvidia.com>

1 files changed, 47 insertions, 0 deletions

diff --git a/arch/arm/mach-tegra/flowctrl.c b/arch/arm/mach-tegra/flowctrl.c
index ffaa286a71e1..a2250ddae797 100644
--- a/arch/arm/mach-tegra/flowctrl.c
+++ b/arch/arm/mach-tegra/flowctrl.c
@@ -21,6 +21,7 @@
 #include <linux/init.h>
 #include <linux/kernel.h>
 #include <linux/io.h>
+#include <linux/cpumask.h>
 
 #include "flowctrl.h"
 #include "iomap.h"
@@ -50,6 +51,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);
@@ -59,3 +68,41 @@ 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);
+        reg &= ~TEGRA30_FLOW_CTRL_CSR_WFE_BITMAP;       /* clear wfe bitmap */
+        reg &= ~TEGRA30_FLOW_CTRL_CSR_WFI_BITMAP;       /* clear wfi bitmap */
+        reg |= FLOW_CTRL_CSR_INTR_FLAG;                 /* clear intr flag */
+        reg |= FLOW_CTRL_CSR_EVENT_FLAG;                /* clear event flag */
+        reg |= TEGRA30_FLOW_CTRL_CSR_WFI_CPU0 << cpuid; /* pwr gating on wfi */
+        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);
+        reg &= ~TEGRA30_FLOW_CTRL_CSR_WFE_BITMAP;       /* clear wfe bitmap */
+        reg &= ~TEGRA30_FLOW_CTRL_CSR_WFI_BITMAP;       /* clear wfi bitmap */
+        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);
+}