ARM: tegra11x: Define ncpu residency for 2 clusters
[linux-3.10.git] / arch / arm / mach-tegra / pm.c
index e2bdb79..a63471d 100644 (file)
@@ -3,7 +3,7 @@
  *
  * CPU complex suspend & resume functions for Tegra SoCs
  *
- * Copyright (c) 2009-2012, NVIDIA Corporation.
+ * Copyright (c) 2009-2013, NVIDIA Corporation.
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License as published by
@@ -47,6 +47,7 @@
 #include <linux/tegra_audio.h>
 
 #include <trace/events/power.h>
+#include <trace/events/nvsecurity.h>
 
 #include <asm/cacheflush.h>
 #include <asm/idmap.h>
 #include <asm/pgtable.h>
 #include <asm/tlbflush.h>
 #include <asm/suspend.h>
+#include <asm/smp_plat.h>
 
 #include <mach/irqs.h>
 #include <mach/powergate.h>
+#include <mach/hardware.h>
 
 #include "board.h"
 #include "clock.h"
+#include "common.h"
 #include "cpuidle.h"
-#include "flowctrl.h"
 #include "fuse.h"
 #include "gic.h"
 #include "iomap.h"
@@ -74,6 +77,9 @@
 #include "dvfs.h"
 #include "cpu-tegra.h"
 
+#define CREATE_TRACE_POINTS
+#include <trace/events/nvpower.h>
+
 struct suspend_context {
        /*
         * The next 7 values are referenced by offset in __restart_plls
@@ -122,6 +128,7 @@ struct suspend_context tegra_sctx;
 #define TEGRA_POWER_EFFECT_LP0         (1 << 14)  /* enter LP0 when CPU pwr gated */
 #define TEGRA_POWER_CPU_PWRREQ_POLARITY (1 << 15)  /* CPU power request polarity */
 #define TEGRA_POWER_CPU_PWRREQ_OE      (1 << 16)  /* CPU power request enable */
+#define TEGRA_POWER_CPUPWRGOOD_EN      (1 << 19)  /* CPU power good enable */
 
 #define PMC_CTRL               0x0
 #define PMC_CTRL_LATCH_WAKEUPS (1 << 5)
@@ -130,6 +137,12 @@ struct suspend_context tegra_sctx;
 #define PMC_DPAD_ORIDE         0x1C
 #define PMC_WAKE_DELAY         0xe0
 #define PMC_DPD_SAMPLE         0x20
+#ifdef CONFIG_ARCH_TEGRA_14x_SOC
+#define PMC_DPD_ENABLE         0x24
+#endif
+#define PMC_IO_DPD_REQ          0x1B8
+#define PMC_IO_DPD2_REQ         0x1C0
+
 
 #define PMC_WAKE_STATUS                0x14
 #define PMC_SW_WAKE_STATUS     0x18
@@ -176,8 +189,9 @@ struct suspend_context tegra_sctx;
 #define MC_SECURITY_SIZE       0x70
 #define MC_SECURITY_CFG2       0x7c
 
-struct dvfs_rail *tegra_cpu_rail;
-static struct dvfs_rail *tegra_core_rail;
+#ifdef CONFIG_ARCH_TEGRA_HAS_CL_DVFS
+static struct clk *tegra_dfll;
+#endif
 static struct clk *tegra_pclk;
 static const struct tegra_suspend_platform_data *pdata;
 static enum tegra_suspend_mode current_suspend_mode = TEGRA_SUSPEND_NONE;
@@ -242,6 +256,33 @@ unsigned long tegra_cpu_lp2_min_residency(void)
        return pdata->cpu_lp2_min_residency;
 }
 
+#ifdef CONFIG_ARCH_TEGRA_HAS_SYMMETRIC_CPU_PWR_GATE
+unsigned long tegra_min_residency_crail(void)
+{
+       return pdata->min_residency_crail;
+}
+#endif
+
+static void suspend_cpu_dfll_mode(void)
+{
+#ifdef CONFIG_ARCH_TEGRA_HAS_CL_DVFS
+       /* If DFLL is used as CPU clock source go to open loop mode */
+       if (!is_lp_cluster() && tegra_dfll &&
+           tegra_dvfs_rail_is_dfll_mode(tegra_cpu_rail))
+               tegra_clk_cfg_ex(tegra_dfll, TEGRA_CLK_DFLL_LOCK, 0);
+#endif
+}
+
+static void resume_cpu_dfll_mode(void)
+{
+#ifdef CONFIG_ARCH_TEGRA_HAS_CL_DVFS
+       /* If DFLL is used as CPU clock source restore closed loop mode */
+       if (!is_lp_cluster() && tegra_dfll &&
+           tegra_dvfs_rail_is_dfll_mode(tegra_cpu_rail))
+               tegra_clk_cfg_ex(tegra_dfll, TEGRA_CLK_DFLL_LOCK, 1);
+#endif
+}
+
 /*
  * create_suspend_pgtable
  *
@@ -309,11 +350,18 @@ static void set_power_timers(unsigned long us_on, unsigned long us_off,
  */
 static void restore_cpu_complex(u32 mode)
 {
-       int cpu = smp_processor_id();
-       unsigned int reg, policy;
-
-       BUG_ON(cpu != 0);
+       int cpu = cpu_logical_map(smp_processor_id());
+       unsigned int reg;
+#if defined(CONFIG_ARCH_TEGRA_2x_SOC) || defined(CONFIG_ARCH_TEGRA_3x_SOC)
+       unsigned int policy;
+#endif
 
+/*
+ * On Tegra11x PLLX and CPU burst policy is either preserved across LP2,
+ * or restored by common clock suspend/resume procedures. Hence, we don't
+ * need it here.
+ */
+#if defined(CONFIG_ARCH_TEGRA_2x_SOC) || defined(CONFIG_ARCH_TEGRA_3x_SOC)
        /* Is CPU complex already running on PLLX? */
        reg = readl(clk_rst + CLK_RESET_CCLK_BURST);
        policy = (reg >> CLK_RESET_CCLK_BURST_POLICY_SHIFT) & 0xF;
@@ -357,27 +405,30 @@ static void restore_cpu_complex(u32 mode)
                writel(tegra_sctx.cpu_burst, clk_rst +
                       CLK_RESET_CCLK_BURST);
        }
-
+#endif
        writel(tegra_sctx.clk_csite_src, clk_rst + CLK_RESET_SOURCE_CSITE);
 
        /* Do not power-gate CPU 0 when flow controlled */
-       reg = flowctrl_read_cpu_csr(cpu);
+       reg = readl(FLOW_CTRL_CPU_CSR(cpu));
        reg &= ~FLOW_CTRL_CSR_WFE_BITMAP;       /* clear wfe bitmap */
        reg &= ~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(cpu, reg);
+       flowctrl_writel(reg, FLOW_CTRL_CPU_CSR(cpu));
 
        /* If an immedidate cluster switch is being perfomed, restore the
           local timer registers. For calls resulting from CPU LP2 in
           idle or system suspend, the local timer was shut down and
           timekeeping switched over to the global system timer. In this
           case keep local timer disabled, and restore only periodic load. */
+#ifdef CONFIG_HAVE_ARM_TWD
        if (!(mode & (TEGRA_POWER_CLUSTER_MASK |
-                     TEGRA_POWER_CLUSTER_IMMEDIATE)))
+                     TEGRA_POWER_CLUSTER_IMMEDIATE))) {
                tegra_sctx.twd.twd_ctrl = 0;
+       }
        tegra_twd_resume(&tegra_sctx.twd);
+#endif
 }
 
 /*
@@ -390,7 +441,7 @@ static void restore_cpu_complex(u32 mode)
  */
 static void suspend_cpu_complex(u32 mode)
 {
-       int cpu = smp_processor_id();
+       int cpu = cpu_logical_map(smp_processor_id());
        unsigned int reg;
        int i;
 
@@ -409,9 +460,11 @@ static void suspend_cpu_complex(u32 mode)
        tegra_sctx.pllp_misc = readl(clk_rst + CLK_RESET_PLLP_MISC);
        tegra_sctx.cclk_divider = readl(clk_rst + CLK_RESET_CCLK_DIVIDER);
 
+#ifdef CONFIG_HAVE_ARM_TWD
        tegra_twd_suspend(&tegra_sctx.twd);
+#endif
 
-       reg = flowctrl_read_cpu_csr(cpu);
+       reg = readl(FLOW_CTRL_CPU_CSR(cpu));
        reg &= ~FLOW_CTRL_CSR_WFE_BITMAP;       /* clear wfe bitmap */
        reg &= ~FLOW_CTRL_CSR_WFI_BITMAP;       /* clear wfi bitmap */
        reg |= FLOW_CTRL_CSR_INTR_FLAG;         /* clear intr flag */
@@ -422,27 +475,21 @@ static void suspend_cpu_complex(u32 mode)
        reg |= FLOW_CTRL_CSR_WFI_CPU0 << cpu;   /* enable power gating on wfi */
 #endif
        reg |= FLOW_CTRL_CSR_ENABLE;            /* enable power gating */
-       flowctrl_write_cpu_csr(cpu, reg);
+       flowctrl_writel(reg, FLOW_CTRL_CPU_CSR(cpu));
 
        for (i = 0; i < num_possible_cpus(); i++) {
                if (i == cpu)
                        continue;
-               reg = flowctrl_read_cpu_csr(i);
+               reg = readl(FLOW_CTRL_CPU_CSR(i));
                reg |= FLOW_CTRL_CSR_EVENT_FLAG;
                reg |= FLOW_CTRL_CSR_INTR_FLAG;
-               flowctrl_write_cpu_csr(i, reg);
+               flowctrl_writel(reg, FLOW_CTRL_CPU_CSR(i));
        }
 
-       tegra_gic_cpu_disable();
-#ifndef CONFIG_ARCH_TEGRA_2x_SOC
-       /* Tegra3 enters LPx states via WFI - do not propagate legacy IRQs
-          to CPU core to avoid fall through WFI (IRQ-to-flow controller wake
-          path is not affected). */
-       tegra_gic_pass_through_disable();
-#endif
+       tegra_gic_cpu_disable(true);
 }
 
-void tegra_clear_cpu_in_lp2(int cpu)
+void tegra_clear_cpu_in_pd(int cpu)
 {
        spin_lock(&tegra_lp2_lock);
        BUG_ON(!cpumask_test_cpu(cpu, &tegra_in_lp2));
@@ -458,7 +505,7 @@ void tegra_clear_cpu_in_lp2(int cpu)
        spin_unlock(&tegra_lp2_lock);
 }
 
-bool tegra_set_cpu_in_lp2(int cpu)
+bool tegra_set_cpu_in_pd(int cpu)
 {
        bool last_cpu = false;
 
@@ -488,14 +535,23 @@ static void tegra_sleep_core(enum tegra_suspend_mode mode,
                             unsigned long v2p)
 {
 #ifdef CONFIG_TRUSTED_FOUNDATIONS
+       outer_flush_range(__pa(&tegra_resume_timestamps_start),
+                         __pa(&tegra_resume_timestamps_end));
+
        if (mode == TEGRA_SUSPEND_LP0) {
+               trace_smc_sleep_core(NVSEC_SMC_START);
+
                tegra_generic_smc(0xFFFFFFFC, 0xFFFFFFE3,
                                  virt_to_phys(tegra_resume));
        } else {
+               trace_smc_sleep_core(NVSEC_SMC_START);
+
                tegra_generic_smc(0xFFFFFFFC, 0xFFFFFFE6,
                                  (TEGRA_RESET_HANDLER_BASE +
                                   tegra_cpu_reset_handler_offset));
        }
+
+       trace_smc_sleep_core(NVSEC_SMC_DONE);
 #endif
 #ifdef CONFIG_ARCH_TEGRA_2x_SOC
        cpu_suspend(v2p, tegra2_sleep_core_finish);
@@ -506,30 +562,28 @@ static void tegra_sleep_core(enum tegra_suspend_mode mode,
 
 static inline void tegra_sleep_cpu(unsigned long v2p)
 {
-#ifdef CONFIG_TRUSTED_FOUNDATIONS
-       tegra_generic_smc(0xFFFFFFFC, 0xFFFFFFE4,
-                         (TEGRA_RESET_HANDLER_BASE +
-                          tegra_cpu_reset_handler_offset));
-#endif
        cpu_suspend(v2p, tegra_sleep_cpu_finish);
 }
 
-unsigned int tegra_idle_lp2_last(unsigned int sleep_time, unsigned int flags)
+unsigned int tegra_idle_power_down_last(unsigned int sleep_time,
+                                       unsigned int flags)
 {
-       u32 mode;       /* hardware + software power mode flags */
+       u32 reg;
        unsigned int remain;
+#if defined(CONFIG_CACHE_L2X0) && !defined(CONFIG_ARCH_TEGRA_14x_SOC)
        pgd_t *pgd;
+#endif
 
        /* Only the last cpu down does the final suspend steps */
-       mode = readl(pmc + PMC_CTRL);
-       mode |= TEGRA_POWER_CPU_PWRREQ_OE;
+       reg = readl(pmc + PMC_CTRL);
+       reg |= TEGRA_POWER_CPU_PWRREQ_OE;
        if (pdata->combined_req)
-               mode &= ~TEGRA_POWER_PWRREQ_OE;
+               reg &= ~TEGRA_POWER_PWRREQ_OE;
        else
-               mode |= TEGRA_POWER_PWRREQ_OE;
-       mode &= ~TEGRA_POWER_EFFECT_LP0;
-       writel(mode, pmc + PMC_CTRL);
-       mode |= flags;
+               reg |= TEGRA_POWER_PWRREQ_OE;
+
+       reg &= ~TEGRA_POWER_EFFECT_LP0;
+       writel(reg, pmc + PMC_CTRL);
 
        tegra_cluster_switch_time(flags, tegra_cluster_switch_time_id_start);
 
@@ -538,8 +592,8 @@ unsigned int tegra_idle_lp2_last(unsigned int sleep_time, unsigned int flags)
         * are in LP2 state and irqs are disabled
         */
        if (flags & TEGRA_POWER_CLUSTER_MASK) {
-               trace_cpu_cluster(POWER_CPU_CLUSTER_START);
-               set_power_timers(pdata->cpu_timer, 0,
+               trace_nvcpu_cluster(NVPOWER_CPU_CLUSTER_START);
+               set_power_timers(pdata->cpu_timer, 2,
                        clk_get_rate_all_locked(tegra_pclk));
                if (flags & TEGRA_POWER_CLUSTER_G) {
                        /*
@@ -547,23 +601,54 @@ unsigned int tegra_idle_lp2_last(unsigned int sleep_time, unsigned int flags)
                         * transition. Before the transition, enable
                         * the vdd_cpu rail.
                         */
-                       if (is_lp_cluster())
+                       if (is_lp_cluster()) {
+#if defined(CONFIG_ARCH_TEGRA_HAS_SYMMETRIC_CPU_PWR_GATE)
+                               reg = readl(FLOW_CTRL_CPU_PWR_CSR);
+                               reg |= FLOW_CTRL_CPU_PWR_CSR_RAIL_ENABLE;
+                               writel(reg, FLOW_CTRL_CPU_PWR_CSR);
+#else
                                writel(UN_PWRGATE_CPU,
                                       pmc + PMC_PWRGATE_TOGGLE);
+#endif
+                       }
                }
-               tegra_cluster_switch_prolog(mode);
+               tegra_cluster_switch_prolog(flags);
        } else {
+               suspend_cpu_dfll_mode();
                set_power_timers(pdata->cpu_timer, pdata->cpu_off_timer,
                        clk_get_rate_all_locked(tegra_pclk));
+#if defined(CONFIG_ARCH_TEGRA_HAS_SYMMETRIC_CPU_PWR_GATE)
+               reg = readl(FLOW_CTRL_CPU_CSR(0));
+               reg &= ~FLOW_CTRL_CSR_ENABLE_EXT_MASK;
+               if (is_lp_cluster()) {
+                       /* for LP cluster, there is no option for rail gating */
+                       if ((flags & TEGRA_POWER_CLUSTER_PART_MASK) ==
+                                               TEGRA_POWER_CLUSTER_PART_MASK)
+                               reg |= FLOW_CTRL_CSR_ENABLE_EXT_EMU;
+                       else if (flags)
+                               reg |= FLOW_CTRL_CSR_ENABLE_EXT_NCPU;
+               }
+               else {
+                       if (flags & TEGRA_POWER_CLUSTER_PART_CRAIL)
+                               reg |= FLOW_CTRL_CSR_ENABLE_EXT_CRAIL;
+                       if (flags & TEGRA_POWER_CLUSTER_PART_NONCPU)
+                               reg |= FLOW_CTRL_CSR_ENABLE_EXT_NCPU;
+               }
+               writel(reg, FLOW_CTRL_CPU_CSR(0));
+#endif
        }
 
        if (sleep_time)
-               tegra_lp2_set_trigger(sleep_time);
+               tegra_pd_set_trigger(sleep_time);
 
        cpu_cluster_pm_enter();
-       suspend_cpu_complex(mode);
+       suspend_cpu_complex(flags);
        tegra_cluster_switch_time(flags, tegra_cluster_switch_time_id_prolog);
+#ifdef CONFIG_CACHE_L2X0
        flush_cache_all();
+#ifdef CONFIG_ARCH_TEGRA_14x_SOC
+       outer_flush_all();
+#else
        /*
         * No need to flush complete L2. Cleaning kernel and IO mappings
         * is enough for the LP code sequence that has L2 disabled but
@@ -572,22 +657,40 @@ unsigned int tegra_idle_lp2_last(unsigned int sleep_time, unsigned int flags)
        pgd = cpu_get_pgd();
        outer_clean_range(__pa(pgd + USER_PTRS_PER_PGD),
                          __pa(pgd + PTRS_PER_PGD));
+#endif
        outer_disable();
-
+#endif
        tegra_sleep_cpu(PHYS_OFFSET - PAGE_OFFSET);
 
+#ifdef CONFIG_ARCH_TEGRA_14x_SOC
+       tegra_init_cache(true);
+#else
        tegra_init_cache(false);
+#endif
+
+#ifdef CONFIG_TRUSTED_FOUNDATIONS
+#ifndef CONFIG_ARCH_TEGRA_11x_SOC
+       trace_smc_wake(tegra_resume_smc_entry_time, NVSEC_SMC_START);
+       trace_smc_wake(tegra_resume_smc_exit_time, NVSEC_SMC_DONE);
+#endif
+#endif
+
        tegra_cluster_switch_time(flags, tegra_cluster_switch_time_id_switch);
-       restore_cpu_complex(mode);
+       restore_cpu_complex(flags);
        cpu_cluster_pm_exit();
 
-       remain = tegra_lp2_timer_remain();
+       remain = tegra_pd_timer_remain();
        if (sleep_time)
-               tegra_lp2_set_trigger(0);
+               tegra_pd_set_trigger(0);
 
        if (flags & TEGRA_POWER_CLUSTER_MASK) {
-               tegra_cluster_switch_epilog(mode);
-               trace_cpu_cluster(POWER_CPU_CLUSTER_DONE);
+               tegra_cluster_switch_epilog(flags);
+               if (is_idle_task(current))
+                       trace_nvcpu_cluster_rcuidle(NVPOWER_CPU_CLUSTER_DONE);
+               else
+                       trace_nvcpu_cluster(NVPOWER_CPU_CLUSTER_DONE);
+       } else {
+               resume_cpu_dfll_mode();
        }
        tegra_cluster_switch_time(flags, tegra_cluster_switch_time_id_epilog);
 
@@ -630,8 +733,10 @@ static void tegra_common_resume(void)
        void __iomem *emc = IO_ADDRESS(TEGRA_EMC_BASE);
 #endif
 
-       /* Clear DPD sample */
-       writel(0x0, pmc + PMC_DPD_SAMPLE);
+#ifdef CONFIG_ARCH_TEGRA_14x_SOC
+       /* Clear DPD Enable */
+       writel(0x0, pmc + PMC_DPD_ENABLE);
+#endif
 
        writel(tegra_sctx.mc[0], mc + MC_SECURITY_START);
        writel(tegra_sctx.mc[1], mc + MC_SECURITY_SIZE);
@@ -639,9 +744,9 @@ static void tegra_common_resume(void)
 #ifdef CONFIG_ARCH_TEGRA_2x_SOC
        /* trigger emc mode write */
        writel(EMC_MRW_DEV_NONE, emc + EMC_MRW_0);
-#endif
        /* clear scratch registers shared by suspend and the reset pen */
        writel(0x0, pmc + PMC_SCRATCH39);
+#endif
        writel(0x0, pmc + PMC_SCRATCH41);
 
        /* restore IRAM */
@@ -686,16 +791,27 @@ static void tegra_pm_set(enum tegra_suspend_mode mode)
                        reg &= ~TEGRA_POWER_CPU_PWRREQ_OE;
                }
 
+#ifdef CONFIG_ARCH_TEGRA_2x_SOC
                /*
                 * LP0 boots through the AVP, which then resumes the AVP to
                 * the address in scratch 39, and the cpu to the address in
                 * scratch 41 to tegra_resume
                 */
                writel(0x0, pmc + PMC_SCRATCH39);
+#endif
 
                /* Enable DPD sample to trigger sampling pads data and direction
                 * in which pad will be driven during lp0 mode*/
                writel(0x1, pmc + PMC_DPD_SAMPLE);
+#if !defined(CONFIG_ARCH_TEGRA_3x_SOC) && !defined(CONFIG_ARCH_TEGRA_2x_SOC)
+               writel(0x800fdfff, pmc + PMC_IO_DPD_REQ);
+               writel(0x80001fff, pmc + PMC_IO_DPD2_REQ);
+#endif
+
+#ifdef CONFIG_ARCH_TEGRA_11x_SOC
+               /* this is needed only for T11x, not for other chips */
+               reg &= ~TEGRA_POWER_CPUPWRGOOD_EN;
+#endif
 
                /* Set warmboot flag */
                boot_flag = readl(pmc + PMC_SCRATCH0);
@@ -783,10 +899,11 @@ static void tegra_suspend_check_pwr_stats(void)
 
        int partid;
 
-       for_each_set_bit(partid, &pwrgate_partid_mask, BITS_PER_LONG)
-               if (tegra_powergate_is_powered(partid) == 1)
-                       pr_warning("partition %s is left on before suspend\n",
-                               tegra_powergate_get_name(partid));
+       for (partid = 0; partid < TEGRA_NUM_POWERGATE; partid++)
+               if ((1 << partid) & pwrgate_partid_mask)
+                       if (tegra_powergate_is_powered(partid))
+                               pr_warning("partition %s is left on before suspend\n",
+                                       tegra_powergate_get_name(partid));
 
        return;
 }
@@ -795,6 +912,7 @@ int tegra_suspend_dram(enum tegra_suspend_mode mode, unsigned int flags)
 {
        int err = 0;
        u32 scratch37 = 0xDEADBEEF;
+       u32 reg;
 
        if (WARN_ON(mode <= TEGRA_SUSPEND_NONE ||
                mode >= TEGRA_MAX_SUSPEND_MODE)) {
@@ -803,8 +921,6 @@ int tegra_suspend_dram(enum tegra_suspend_mode mode, unsigned int flags)
        }
 
        if (tegra_is_voice_call_active()) {
-               u32 reg;
-
                /* backup the current value of scratch37 */
                scratch37 = readl(pmc + PMC_SCRATCH37);
 
@@ -830,26 +946,42 @@ int tegra_suspend_dram(enum tegra_suspend_mode mode, unsigned int flags)
 
        local_fiq_disable();
 
-       trace_cpu_suspend(CPU_SUSPEND_START);
+       trace_cpu_suspend(CPU_SUSPEND_START, tegra_rtc_read_ms());
 
        if (mode == TEGRA_SUSPEND_LP0) {
 #ifdef CONFIG_TEGRA_CLUSTER_CONTROL
-               u32 reg = readl(pmc + PMC_SCRATCH4);
+               reg = readl(pmc + PMC_SCRATCH4);
                if (is_lp_cluster())
                        reg |= PMC_SCRATCH4_WAKE_CLUSTER_MASK;
                else
                        reg &= (~PMC_SCRATCH4_WAKE_CLUSTER_MASK);
                pmc_32kwritel(reg, PMC_SCRATCH4);
 #endif
+               tegra_tsc_suspend();
                tegra_lp0_suspend_mc();
                tegra_cpu_reset_handler_save();
-
+               tegra_tsc_wait_for_suspend();
+#ifndef CONFIG_TEGRA_SIMULATION_PLATFORM
+               tegra_smp_clear_power_mask();
+#endif
        }
        else if (mode == TEGRA_SUSPEND_LP1)
                *iram_cpu_lp1_mask = 1;
 
        suspend_cpu_complex(flags);
 
+#if defined(CONFIG_ARCH_TEGRA_HAS_SYMMETRIC_CPU_PWR_GATE)
+       /* In case of LP0/1, program external power gating accordinly */
+       if (mode == TEGRA_SUSPEND_LP0 || mode == TEGRA_SUSPEND_LP1) {
+               reg = readl(FLOW_CTRL_CPU_CSR(0));
+               if (is_lp_cluster())
+                       reg |= FLOW_CTRL_CSR_ENABLE_EXT_NCPU; /* Non CPU */
+               else
+                       reg |= FLOW_CTRL_CSR_ENABLE_EXT_CRAIL;  /* CRAIL */
+               flowctrl_writel(reg, FLOW_CTRL_CPU_CSR(0));
+       }
+#endif
+
        flush_cache_all();
        outer_flush_all();
        outer_disable();
@@ -861,9 +993,33 @@ int tegra_suspend_dram(enum tegra_suspend_mode mode, unsigned int flags)
 
        tegra_init_cache(true);
 
+#ifdef CONFIG_TRUSTED_FOUNDATIONS
+#ifndef CONFIG_ARCH_TEGRA_11x_SOC
+       trace_smc_wake(tegra_resume_smc_entry_time, NVSEC_SMC_START);
+       trace_smc_wake(tegra_resume_smc_exit_time, NVSEC_SMC_DONE);
+#endif
+
+       if (mode == TEGRA_SUSPEND_LP0) {
+               trace_secureos_init(tegra_resume_entry_time,
+                       NVSEC_SUSPEND_EXIT_DONE);
+       }
+#endif
+
        if (mode == TEGRA_SUSPEND_LP0) {
+
+               /* CPUPWRGOOD_EN is not enabled in HW so disabling this, *
+               * Otherwise it is creating issue in cluster switch after LP0 *
+#ifdef CONFIG_ARCH_TEGRA_11x_SOC
+               reg = readl(pmc+PMC_CTRL);
+               reg |= TEGRA_POWER_CPUPWRGOOD_EN;
+               pmc_32kwritel(reg, PMC_CTRL);
+#endif
+               */
+
+               tegra_tsc_resume();
                tegra_cpu_reset_handler_restore();
                tegra_lp0_resume_mc();
+               tegra_tsc_wait_for_resume();
        } else if (mode == TEGRA_SUSPEND_LP1)
                *iram_cpu_lp1_mask = 0;
 
@@ -878,7 +1034,6 @@ int tegra_suspend_dram(enum tegra_suspend_mode mode, unsigned int flags)
         * of LP0 state by temporarily enabling both requests
         */
        if (mode == TEGRA_SUSPEND_LP0 && pdata->combined_req) {
-               u32 reg;
                reg = readl(pmc + PMC_CTRL);
                reg |= TEGRA_POWER_CPU_PWRREQ_OE;
                pmc_32kwritel(reg, PMC_CTRL);
@@ -889,7 +1044,7 @@ int tegra_suspend_dram(enum tegra_suspend_mode mode, unsigned int flags)
        if (pdata && pdata->board_resume)
                pdata->board_resume(mode, TEGRA_RESUME_AFTER_CPU);
 
-       trace_cpu_suspend(CPU_SUSPEND_DONE);
+       trace_cpu_suspend(CPU_SUSPEND_DONE, tegra_rtc_read_ms());
 
        local_fiq_enable();
 
@@ -1017,8 +1172,10 @@ void __init tegra_init_suspend(struct tegra_suspend_platform_data *plat)
        u32 reg;
        u32 mode;
 
-       tegra_cpu_rail = tegra_dvfs_get_rail_by_name("vdd_cpu");
-       tegra_core_rail = tegra_dvfs_get_rail_by_name("vdd_core");
+#ifdef CONFIG_ARCH_TEGRA_HAS_CL_DVFS
+       tegra_dfll = clk_get_sys(NULL, "dfll_cpu");
+       BUG_ON(IS_ERR(tegra_dfll));
+#endif
        tegra_pclk = clk_get_sys(NULL, "pclk");
        BUG_ON(IS_ERR(tegra_pclk));
        pdata = plat;
@@ -1042,7 +1199,7 @@ void __init tegra_init_suspend(struct tegra_suspend_platform_data *plat)
                goto fail;
        }
 
-       if ((tegra_chip_id == TEGRA30) &&
+       if ((tegra_get_chipid() == TEGRA_CHIPID_TEGRA3) &&
            (tegra_revision == TEGRA_REVISION_A01) &&
            (plat->suspend_mode == TEGRA_SUSPEND_LP0)) {
                /* Tegra 3 A01 supports only LP1 */
@@ -1098,11 +1255,28 @@ out:
                plat->suspend_mode = TEGRA_SUSPEND_LP2;
        }
 
+#ifdef CONFIG_TEGRA_LP1_950
+       if (pdata->lp1_lowvolt_support) {
+               u32 lp1_core_lowvolt, lp1_core_highvolt;
+               memcpy(tegra_lp1_register_pmuslave_addr(), &pdata->pmuslave_addr, 4);
+               memcpy(tegra_lp1_register_i2c_base_addr(), &pdata->i2c_base_addr, 4);
+
+               lp1_core_lowvolt = 0;
+               lp1_core_lowvolt = (pdata->lp1_core_volt_low << 8) | pdata->core_reg_addr;
+               memcpy(tegra_lp1_register_core_lowvolt(), &lp1_core_lowvolt, 4);
+
+               lp1_core_highvolt = 0;
+               lp1_core_highvolt = (pdata->lp1_core_volt_high << 8) | pdata->core_reg_addr;
+               memcpy(tegra_lp1_register_core_highvolt(), &lp1_core_highvolt, 4);
+       }
+#endif
        /* !!!FIXME!!! THIS IS TEGRA2 ONLY */
        /* Initialize scratch registers used for CPU LP2 synchronization */
        writel(0, pmc + PMC_SCRATCH37);
        writel(0, pmc + PMC_SCRATCH38);
+#ifdef CONFIG_ARCH_TEGRA_2x_SOC
        writel(0, pmc + PMC_SCRATCH39);
+#endif
        writel(0, pmc + PMC_SCRATCH41);
 
        /* Always enable CPU power request; just normal polarity is supported */
@@ -1153,10 +1327,14 @@ out:
 
        iram_cpu_lp2_mask = tegra_cpu_lp2_mask;
        iram_cpu_lp1_mask = tegra_cpu_lp1_mask;
+
+       /* clear io dpd settings before kernel */
+       tegra_bl_io_dpd_cleanup();
+
 fail:
 #endif
        if (plat->suspend_mode == TEGRA_SUSPEND_NONE)
-               tegra_lp2_in_idle(false);
+               tegra_pd_in_idle(false);
 
        current_suspend_mode = plat->suspend_mode;
 }
@@ -1237,13 +1415,13 @@ EXPORT_SYMBOL(debug_uart_clk);
 void tegra_console_uart_suspend(void)
 {
        if (console_suspend_enabled && debug_uart_clk)
-               clk_disable(debug_uart_clk);
+               tegra_clk_disable_unprepare(debug_uart_clk);
 }
 
 void tegra_console_uart_resume(void)
 {
        if (console_suspend_enabled && debug_uart_clk)
-               clk_enable(debug_uart_clk);
+               tegra_clk_prepare_enable(debug_uart_clk);
 }
 
 static int tegra_debug_uart_syscore_init(void)