1 files changed, 40 insertions, 0 deletions

diff --git a/arch/arm/mach-tegra/irq.c b/arch/arm/mach-tegra/irq.c
index 0de4eed1493d..1a74d562dca1 100644
--- a/arch/arm/mach-tegra/irq.c
+++ b/arch/arm/mach-tegra/irq.c
@@ -18,10 +18,12 @@
  */
 
 #include <linux/kernel.h>
+#include <linux/cpu_pm.h>
 #include <linux/interrupt.h>
 #include <linux/irq.h>
 #include <linux/io.h>
 #include <linux/of.h>
+#include <linux/of_address.h>
 #include <linux/irqchip/arm-gic.h>
 #include <linux/syscore_ops.h>
 
@@ -65,6 +67,7 @@ static u32 cpu_ier[TEGRA_MAX_NUM_ICTLRS];
 static u32 cpu_iep[TEGRA_MAX_NUM_ICTLRS];
 
 static u32 ictlr_wake_mask[TEGRA_MAX_NUM_ICTLRS];
+static void __iomem *tegra_gic_cpu_base;
 #endif
 
 bool tegra_pending_sgi(void)
@@ -213,8 +216,43 @@ int tegra_legacy_irq_syscore_init(void)
 
         return 0;
 }
+
+static int tegra_gic_notifier(struct notifier_block *self,
+                              unsigned long cmd, void *v)
+{
+        switch (cmd) {
+        case CPU_PM_ENTER:
+                writel_relaxed(0x1E0, tegra_gic_cpu_base + GIC_CPU_CTRL);
+                break;
+        }
+
+        return NOTIFY_OK;
+}
+
+static struct notifier_block tegra_gic_notifier_block = {
+        .notifier_call = tegra_gic_notifier,
+};
+
+static const struct of_device_id tegra114_dt_gic_match[] __initconst = {
+        { .compatible = "arm,cortex-a15-gic" },
+        { }
+};
+
+static void tegra114_gic_cpu_pm_registration(void)
+{
+        struct device_node *dn;
+
+        dn = of_find_matching_node(NULL, tegra114_dt_gic_match);
+        if (!dn)
+                return;
+
+        tegra_gic_cpu_base = of_iomap(dn, 1);
+
+        cpu_pm_register_notifier(&tegra_gic_notifier_block);
+}
 #else
 #define tegra_set_wake NULL
+static void tegra114_gic_cpu_pm_registration(void) { }
 #endif
 
 void __init tegra_init_irq(void)
@@ -252,4 +290,6 @@ void __init tegra_init_irq(void)
         if (!of_have_populated_dt())
                 gic_init(0, 29, distbase,
                         IO_ADDRESS(TEGRA_ARM_PERIF_BASE + 0x100));
+
+        tegra114_gic_cpu_pm_registration();
 }