tegra power: Add save/restore code for ClockReset block.

LP0 (deep sleep) requires a great deal of context to be saved prior to shutdown.
This change lays the initial groundwork for the context save/restore routines.

Change-Id: Ib7084b2fe8df6b1b834b1e1044c1056943b1e27e

5 files changed, 591 insertions, 36 deletions

diff --git a/arch/arm/mach-tegra/Makefile b/arch/arm/mach-tegra/Makefile
index 88296db9a1f6..534270fba050 100644
--- a/arch/arm/mach-tegra/Makefile
+++ b/arch/arm/mach-tegra/Makefile
@@ -39,6 +39,7 @@ obj-$(CONFIG_LOCAL_TIMERS)		+= localtimer.o
 ifeq ($(CONFIG_HOTPLUG_CPU),y)
 obj-y					+= hotplug.o
 obj-$(CONFIG_ARCH_TEGRA_2x_SOC)		+= power-t2.o
+obj-$(CONFIG_ARCH_TEGRA_2x_SOC)		+= power-context-t2.o
 obj-$(CONFIG_ARCH_TEGRA_2x_SOC)		+= power-lp.o
 endif
 
diff --git a/arch/arm/mach-tegra/include/ap20/nvboot_pmc_scratch_map.h b/arch/arm/mach-tegra/include/ap20/nvboot_pmc_scratch_map.h
new file mode 100644
index 000000000000..8a2a44d1b68a
--- /dev/null
+++ b/arch/arm/mach-tegra/include/ap20/nvboot_pmc_scratch_map.h
@@ -0,0 +1,44 @@
+/*
+ * Copyright (c) 2010 NVIDIA Corporation.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * Neither the name of the NVIDIA Corporation nor the names of its contributors
+ * may be used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+/**
+ * Defines fields in the PMC scratch registers used by the Boot ROM code.
+ */
+
+#ifndef INCLUDED_NVBOOT_PMC_SCRATCH_MAP_H
+#define INCLUDED_NVBOOT_PMC_SCRATCH_MAP_H
+
+#define APBDEV_PMC_SCRATCH0_0_WARM_BOOT0_FLAG_RANGE        0:0
+#define APBDEV_PMC_SCRATCH0_0_WARM_BOOT0_FLAG_SHIFT        _MK_SHIFT_CONST(0)
+#define APBDEV_PMC_SCRATCH0_0_WARM_BOOT0_FLAG_DEFAULT_MASK _MK_MASK_CONST(0x00000001)
+
+#endif // INCLUDED_NVBOOT_APBDEV_PMC_SCRATCH_MAP_H
diff --git a/arch/arm/mach-tegra/power-context-t2.c b/arch/arm/mach-tegra/power-context-t2.c
new file mode 100644
index 000000000000..83e4bd7b35df
--- /dev/null
+++ b/arch/arm/mach-tegra/power-context-t2.c
@@ -0,0 +1,379 @@
+/*
+ * arch/arm/mach-tegra/power-context-t2.c
+ *
+ * Module save/restore routines for Tegra 2 SoCs
+ *
+ * Copyright (c) 2010, 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
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+#include "power.h"
+
+extern NvRmDeviceHandle s_hRmGlobal;
+extern uintptr_t g_resume, g_contextSavePA;
+
+static void update_registers_for_lp0(void);
+static void wake_pad_configure(NvRmDeviceHandle hRmDeviceHandle);
+static void setup_wakeup_events(void);
+static NvU32* save_clockreset_context(PowerModuleContext Context,
+				struct power_context *pAnchor, NvU32 *pCM);
+NvU32* perform_context_operation(PowerModuleContext Context);
+void prepare_for_wb0(void);
+
+struct power_context *s_pModulesContextAnchor = NULL;
+
+static void update_registers_for_lp0(void)
+{
+	NvU32 Reg;
+
+	//SCRATCH0 : Warmbootflag
+	Reg = NV_REGR(s_hRmGlobal, NvRmModuleID_Pmif, 0,
+				APBDEV_PMC_SCRATCH0_0);
+	Reg = NV_FLD_SET_DRF_NUM (APBDEV_PMC, SCRATCH0, WARM_BOOT0_FLAG, 0, Reg);
+	NV_REGW(s_hRmGlobal, NvRmModuleID_Pmif, 0,
+				APBDEV_PMC_SCRATCH0_0, Reg);
+
+	//SCRATCH1 : AVP-side recovery physical address.
+	//FIXME: Need to get the AVP restore address from the bootloader
+
+	//SCRATCH41 : CPU-side recovery physical address.
+	//LP0 needs the resume address in SCRATCH41
+	NV_REGW(s_hRmGlobal, NvRmModuleID_Pmif, 0,
+			APBDEV_PMC_SCRATCH41_0, g_resume);
+}
+
+static void wake_pad_configure(NvRmDeviceHandle hRmDeviceHandle)
+{
+	NvU32 WakePadCount;
+	const NvOdmWakeupPadInfo* pWakePadInfo =
+		NvOdmQueryGetWakeupPadTable(&WakePadCount);
+	NvU32 WakeMask, WakeLevel, WakeStatus, WakeCntrl, i;
+
+	if ((WakePadCount == 0) || (pWakePadInfo == NULL))
+		return;
+
+	//Clear wake status register
+	NV_REGW(s_hRmGlobal, NvRmModuleID_Pmif, 0,
+		APBDEV_PMC_SW_WAKE_STATUS_0, 0);
+	NvOsWaitUS(WAKE_PAD_MIN_LATCH_TIME_US);
+
+	//Sample current wake pad status (on 1=>0 transition of latch
+	//enable bit in PMC control register)
+	WakeCntrl = NV_REGR( s_hRmGlobal, NvRmModuleID_Pmif, 0,
+					APBDEV_PMC_CNTRL_0);
+	WakeCntrl = NV_FLD_SET_DRF_DEF(
+		APBDEV_PMC, CNTRL, LATCHWAKE_EN, ENABLE, WakeCntrl);
+	NV_REGW(s_hRmGlobal, NvRmModuleID_Pmif, 0,
+		APBDEV_PMC_CNTRL_0, WakeCntrl);
+	NvOsWaitUS(WAKE_PAD_MIN_SAMPLE_TIME_US);
+	WakeCntrl = NV_FLD_SET_DRF_DEF(
+		APBDEV_PMC, CNTRL, LATCHWAKE_EN, DISABLE, WakeCntrl);
+	NV_REGW(s_hRmGlobal, NvRmModuleID_Pmif, 0,
+		APBDEV_PMC_CNTRL_0, WakeCntrl);
+	NvOsWaitUS(WAKE_PAD_MIN_LATCH_TIME_US);
+
+	//Read current wake mask, level, and latched status
+	WakeMask = NV_REGR( s_hRmGlobal, NvRmModuleID_Pmif, 0,
+				APBDEV_PMC_WAKE_MASK_0);
+	WakeLevel = NV_REGR( s_hRmGlobal, NvRmModuleID_Pmif, 0,
+					APBDEV_PMC_WAKE_LVL_0);
+	WakeStatus = NV_REGR( s_hRmGlobal, NvRmModuleID_Pmif, 0,
+		APBDEV_PMC_SW_WAKE_STATUS_0);
+	/*
+	 * Enable/disable wake pad, and configure polarity for enabled pads.
+	 * For "any edge" wake pads toggle level if pad status has been sampled
+	 * as active; it is possible that wake input has changed during sampling
+	 * and configured polarity may be incorrect. However, this change would
+	 * activate the respective driver via interrupt controller. Hence, this
+	 * configuration routine will be re-entered after driver becomes idle
+	 * again, before actual suspend happens.
+	 */
+	for (i = 0; i < WakePadCount; i++)
+	{
+		NvU32 mask = 0x1 << pWakePadInfo[i].WakeupPadNumber;
+		if (pWakePadInfo[i].enable)
+		{
+			WakeMask |= mask;
+			switch (pWakePadInfo[i].Polarity) {
+			case NvOdmWakeupPadPolarity_Low:
+				WakeLevel &= (~mask);
+				break;
+			case NvOdmWakeupPadPolarity_High:
+				WakeLevel |= mask;
+				break;
+			case NvOdmWakeupPadPolarity_AnyEdge:
+				if (WakeStatus & mask)
+					WakeLevel ^= mask;
+				break;
+			default:
+				break;
+			}
+		}
+		else
+		{
+			WakeMask &= (~mask);
+		}
+	}
+	//Write back updated wake mask, and level
+	NV_REGW(s_hRmGlobal, NvRmModuleID_Pmif, 0,
+		APBDEV_PMC_WAKE_MASK_0, WakeMask);
+	NV_REGW(s_hRmGlobal, NvRmModuleID_Pmif, 0,
+		APBDEV_PMC_WAKE_LVL_0, WakeLevel);
+}
+
+static void setup_wakeup_events(void)
+{
+	NvU32 Reg;
+
+	//Configure wake pads, per their current status.
+	wake_pad_configure(s_hRmGlobal);
+
+	//Enable RTC wake event.
+	Reg = NV_REGR(s_hRmGlobal, NvRmModuleID_Pmif, 0,
+			APBDEV_PMC_WAKE_MASK_0);
+	Reg = NV_FLD_SET_DRF_DEF(APBDEV_PMC, WAKE_MASK, RTC, ENABLE, Reg);
+	NV_REGW(s_hRmGlobal, NvRmModuleID_Pmif, 0,
+			APBDEV_PMC_WAKE_MASK_0, Reg);
+
+	//Set RTC wake level.
+	Reg = NV_REGR(s_hRmGlobal, NvRmModuleID_Pmif, 0,
+				APBDEV_PMC_WAKE_LVL_0);
+	Reg = NV_FLD_SET_DRF_DEF(APBDEV_PMC, WAKE_LVL, RTC, ACTIVE_HIGH, Reg);
+	NV_REGW(s_hRmGlobal, NvRmModuleID_Pmif, 0,
+				APBDEV_PMC_WAKE_LVL_0, Reg);
+}
+
+static NvU32* save_clockreset_context(
+		PowerModuleContext Context,
+		struct power_context *pAnchor,
+		NvU32 *pCM)
+{
+	NvU32			Aperture;		// Register base address
+	NvU32			ApertureSize;	// Register set size
+	NvRmModuleID	ModuleId;		// Composite module id & instance
+	NvU32			Offset;			// Register offset
+	NvU32			OscCtrl1;		// Scratch register
+	NvU32			OscCtrl2;		// Scratch register
+	NvU32			BitVal;			// Scratch register
+	NvU32*			pBase;			// Pointer to register base addresses
+
+	//Get a pointer to the base address for the registers.
+	pBase = pAnchor->clock_reset.pBase;
+
+	switch (Context) {
+	case PowerModuleContext_Init:
+		//Already initialized?
+		if (pBase == NULL)
+		{
+			//Generate the composite module id and instance.
+			ModuleId = NVRM_MODULE_ID(NvRmPrivModuleID_ClockAndReset, 0);
+
+			//Retrieve the register base PA
+			NvRmModuleGetBaseAddress(s_hRmGlobal, ModuleId,
+					&Aperture, &ApertureSize);
+
+			//Get the corresponding VA
+			if (NvOsPhysicalMemMap(Aperture,
+					ApertureSize,
+					NvOsMemAttribute_Uncached,
+					NVOS_MEM_READ_WRITE,
+					(void*)&(pAnchor->clock_reset.pBase)))
+			{
+				printk("Failed to map the context save area!\n");
+				goto fail;
+			}
+		}
+		break;
+	case PowerModuleContext_Save:
+		//Register base address must have been set
+		//by PowerModuleContext_Init.
+		if (pBase == NULL);
+			goto fail;
+
+		//Anchor the starting point for this controller's context.
+		pAnchor->clock_reset.pContext = pCM;
+
+		//Save clock sources for individual modules
+		Offset = CAR_CLK_SOURCES_OFFSET_START;
+		do
+		{
+			//Skip saving MC and EMC CAR values.
+			if (Offset != CLK_RST_CONTROLLER_CLK_SOURCE_EMC_0)
+			{
+				*pCM++ = NV_CAR_REGR_OFFSET(pBase, Offset);
+			}
+			Offset += sizeof(NvU32);
+		} while (Offset <= CAR_CLK_SOURCES_OFFSET_END);
+
+		//Save oscillator control register
+		*pCM++ = NV_CAR_REGR(pBase, OSC_CTRL);
+
+		//Save module reset state
+		*pCM++ = NV_CAR_REGR(pBase, RST_DEVICES_L);
+		*pCM++ = NV_CAR_REGR(pBase, RST_DEVICES_H);
+
+		//Save module clock enable state
+		*pCM++ = NV_CAR_REGR(pBase, CLK_OUT_ENB_L);
+		*pCM++ = NV_CAR_REGR(pBase, CLK_OUT_ENB_H);
+
+		//Save clock mask register
+		*pCM++ = NV_CAR_REGR(pBase, CLK_MASK_ARM);
+		break;
+	case PowerModuleContext_Restore:
+		//Register base address must have been set
+		//by PowerModuleContext_Init.
+		if (pBase == NULL);
+			goto fail;
+
+		//We should be at the same place in the context
+		//as when we saved things.
+		if (pCM != pAnchor->clock_reset.pContext)
+			goto fail;
+
+		//Retrieve the anchored starting point for this controller's context.
+		pCM = pAnchor->clock_reset.pContext;
+
+		if (pCM == NULL)
+			goto fail;
+
+		//Force PLLA in bypass mode, until Rm gets a chance to
+		//initialize it back again. This is to ensure all
+		//downstream modules depending on PLLA get a known clock
+		//when they are taken out of reset.
+		NV_CAR_REGW(pBase, PLLA_BASE,
+			NV_DRF_NUM(CLK_RST_CONTROLLER, PLLA_BASE, PLLA_BYPASS, 0x1));
+
+		//Force PLLP in bypass mode [bug 455955], until Rm gets a
+		//chance to initialize it back again. This is to ensure
+		//that all downstream modules depending on PLLP get a known
+		//clock when they are taken out of reset.
+		NV_CAR_REGW(pBase, PLLP_BASE,
+			NV_DRF_NUM(CLK_RST_CONTROLLER, PLLP_BASE, PLLP_BYPASS, 0x1));
+
+		//The restore sequence below follows a h/w recommended
+		//way of bringing up the chip. Remember, in our case the
+		//bootrom would have run on a LP0 exit. Hopefully, it does the
+		//correct thing.
+
+		//Enable clock for all modules
+		NV_CAR_REGW(pBase, CLK_OUT_ENB_L,
+			CLK_RST_CONTROLLER_CLK_OUT_ENB_L_0_RESET_MASK);
+		NV_CAR_REGW(pBase, CLK_OUT_ENB_H,
+			CLK_RST_CONTROLLER_CLK_OUT_ENB_H_0_RESET_MASK);
+
+		//Restore clock sources for individual modules
+		Offset = CAR_CLK_SOURCES_OFFSET_START;
+		do
+		{
+			//Skip MC and EMC restoration. Keep bootrom settings until
+			//DVFS gets a chance to change it.
+			if (Offset != CLK_RST_CONTROLLER_CLK_SOURCE_EMC_0)
+			{
+				NV_CAR_REGW_OFFSET(pBase, Offset, *pCM++);
+			}
+			Offset += sizeof(NvU32);
+		} while (Offset <= CAR_CLK_SOURCES_OFFSET_END);
+
+		//Restore oscillator control register-drive strength+osc bypass only.
+		OscCtrl1 = *pCM++;
+		OscCtrl2 = NV_CAR_REGR(pBase, OSC_CTRL);
+		BitVal   = NV_DRF_VAL(CLK_RST_CONTROLLER,
+						OSC_CTRL, XOFS, OscCtrl1);
+		OscCtrl2 = NV_FLD_SET_DRF_NUM(CLK_RST_CONTROLLER,
+						OSC_CTRL, XOFS, BitVal, OscCtrl2);
+		BitVal   = NV_DRF_VAL(CLK_RST_CONTROLLER,
+						OSC_CTRL, XOBP, OscCtrl1);
+		OscCtrl2 = NV_FLD_SET_DRF_NUM(CLK_RST_CONTROLLER,
+						OSC_CTRL, XOBP, BitVal, OscCtrl2);
+		NV_CAR_REGW(pBase, OSC_CTRL, OscCtrl2);
+
+		//Restore module reset state
+		NV_CAR_REGW(pBase, RST_DEVICES_L, *pCM++);
+		NV_CAR_REGW(pBase, RST_DEVICES_H, *pCM++);
+
+		//Restore module clock enable state
+		NV_CAR_REGW(pBase, CLK_OUT_ENB_L, *pCM++);
+		NV_CAR_REGW(pBase, CLK_OUT_ENB_H, *pCM++);
+
+		//Restore clock mask register
+		NV_CAR_REGW(pBase, CLK_MASK_ARM, *pCM++);
+		break;
+
+	case PowerModuleContext_SaveLP1:
+	case PowerModuleContext_RestoreLP1:
+	case PowerModuleContext_DisableInterrupt:
+		//Do nothing.
+		break;
+
+	default:
+		break;
+	}
+
+	return pCM;
+
+fail:
+	return NULL;
+}
+
+NvU32* perform_context_operation(PowerModuleContext Context)
+{
+	NvU32*  pCM;	// Pointer to next available save area location
+
+	//First save area location available for module use is located just
+	//beyond the anchor structure.
+	//NV_ASSERT(s_pModulesContextAnchor != NULL);
+
+	//Initializing?
+	if (Context == PowerModuleContext_Init)
+	{
+		//The context pointer start immediately following the anchor structure.
+		pCM = (NvU32*)(((NvU8*)s_pModulesContextAnchor) +
+					sizeof(*s_pModulesContextAnchor));
+	}
+	else
+	{
+		//Use the starting point for module contexts.
+		pCM = s_pModulesContextAnchor->first_context_location;
+	}
+
+	pCM = save_clockreset_context(Context, s_pModulesContextAnchor, pCM);
+	//TODO: Save rest of modules here
+
+	//Check if we went past the context area limits.
+	if (pCM >= ((NvU32*)s_pModulesContextAnchor +
+			s_pModulesContextAnchor->context_size_words))
+		return NULL;
+
+	return pCM;
+}
+
+void prepare_for_wb0(void)
+{
+	update_registers_for_lp0();
+
+	//Save pointer to context in the scratch register reserved
+	//for that purpose.
+	NV_REGW(s_hRmGlobal, NvRmModuleID_Pmif, 0,
+				APBDEV_PMC_SCRATCH37_0, g_contextSavePA);
+
+	//Setup our wakeup events. RM can set something on it's own too,
+	//let's not destroy those.
+	setup_wakeup_events();
+
+	//Save module context that should be restored after LP0
+	//Interrupt, gpio, pin mux, clock management etc
+	perform_context_operation(PowerModuleContext_Save);
+}
diff --git a/arch/arm/mach-tegra/power-t2.c b/arch/arm/mach-tegra/power-t2.c
index 29916b8b8c3b..f4c05718d05b 100644
--- a/arch/arm/mach-tegra/power-t2.c
+++ b/arch/arm/mach-tegra/power-t2.c
@@ -3,7 +3,7 @@
  *
  * CPU shutdown routines for Tegra 2 SoCs
  *
- * Copyright (c) 2009, NVIDIA Corporation.
+ * Copyright (c) 2010, 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
@@ -20,40 +20,14 @@
  * 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
  */
 
-#include "nvos.h"
-#include "nvrm_init.h"
-#include "nvrm_drf.h"
-#include "ap20/arapbpm.h"
-#include "nvrm_module.h"
-#include "ap20/arflow_ctlr.h"
-#include "ap20/arclk_rst.h"
-#include "nvrm_hardware_access.h"
-#include "nvrm_interrupt.h"
-#include "nvrm_power.h"
-#include "nvrm_power_private.h"
-#include <linux/kernel.h>
-#include <linux/platform_device.h>
-#include <linux/interrupt.h>
-
-typedef enum
-{
-    PowerPllA = 0,
-    PowerPllC,
-    PowerPllM,
-    PowerPllP,
-    PowerPllX
-} PowerPll;
-
-typedef enum
-{
-    POWER_STATE_LP2,
-    POWER_STATE_LP1,
-    POWER_STATE_LP0,
-} PowerState;
+#include "power.h"
 
 extern void NvPrivAp20MaskIrq(unsigned int irq);
 extern void NvPrivAp20UnmaskIrq(unsigned int irq);
 extern int enter_power_state(PowerState state, unsigned int proc_id);
+extern void prepare_for_wb0(void);
+extern NvU32* perform_context_operation(PowerModuleContext Context);
+
 void cpu_ap20_do_lp2(void);
 void resume(unsigned int state);
 static NvU32 select_wakeup_pll(void);
@@ -63,8 +37,6 @@ void do_suspend_prep(void);
 void reset_cpu(unsigned int cpu, unsigned int reset);
 extern NvRmDeviceHandle s_hRmGlobal;
 
-#define NUM_LOCAL_TIMER_REGISTERS 3
-
 uintptr_t g_resume = 0, g_contextSavePA = 0, g_contextSaveVA = 0;
 NvU32 g_modifiedPlls;
 NvU32 g_wakeupCcbp = 0, g_NumActiveCPUs, g_Sync = 0, g_ArmPerif = 0;
@@ -72,6 +44,63 @@ NvU32 g_enterLP2PA = 0;
 NvU32 g_localTimerLoadRegister, g_localTimerCntrlRegister;
 NvU32 g_coreSightClock, g_currentCcbp;
 
+void cpu_ap20_do_lp0(void)
+{
+	//NOTE: Once we enter this routine, there is no way to avert a LP0.
+	//If there is a potential interrupt waiting, we will enter LP0
+	//and exit immediately as soon the PMC samples it and the
+	//power good timer expires.
+	NvU32   Reg;
+	NvOdmPmuProperty	PmuProperty;
+	NvBool HasPmuProperty = NvOdmQueryGetPmuProperty(&PmuProperty);
+
+	//Inform RM about entry to LP0 state
+	NvRmPrivPowerSetState(s_hRmGlobal, NvRmPowerState_LP0);
+
+	if (HasPmuProperty && PmuProperty.CombinedPowerReq)
+	{
+		//Enable core power request, and tristate CPU power request outputs
+		//(both requests are active, so there are no glitches as long as
+		//proper external pu/pd are set)
+		Reg = NV_REGR(s_hRmGlobal, NvRmModuleID_Pmif, 0,
+				APBDEV_PMC_CNTRL_0);
+		Reg = NV_FLD_SET_DRF_DEF(APBDEV_PMC, CNTRL, PWRREQ_OE, ENABLE, Reg);
+		NV_REGW(s_hRmGlobal, NvRmModuleID_Pmif, 0,
+				APBDEV_PMC_CNTRL_0, Reg);
+		// FIXME: do we need an explicit delay?
+		Reg = NV_REGR(s_hRmGlobal, NvRmModuleID_Pmif, 0,
+			APBDEV_PMC_CNTRL_0);
+		Reg = NV_FLD_SET_DRF_DEF(APBDEV_PMC, CNTRL, CPUPWRREQ_OE, DISABLE, Reg);
+	}
+
+	// Enter low power LP0 mode
+	prepare_for_wb0();
+
+	//TODO: Shadow required state here
+
+	if (HasPmuProperty && PmuProperty.CombinedPowerReq)
+	{
+		//Enable CPU power request and tristate core power request outputs
+		//(both requests are active, so there are no glitches as long as
+		//proper external pu/pd are set)
+		Reg = NV_REGR(s_hRmGlobal, NvRmModuleID_Pmif, 0,
+				APBDEV_PMC_CNTRL_0);
+		Reg = NV_FLD_SET_DRF_DEF(APBDEV_PMC, CNTRL, CPUPWRREQ_OE, ENABLE, Reg);
+		NV_REGW(s_hRmGlobal, NvRmModuleID_Pmif, 0,
+				APBDEV_PMC_CNTRL_0, Reg);
+		// FIXME: do we need an explicit delay ?
+		Reg = NV_REGR(s_hRmGlobal, NvRmModuleID_Pmif, 0,
+				APBDEV_PMC_CNTRL_0);
+		Reg = NV_FLD_SET_DRF_DEF(APBDEV_PMC, CNTRL, PWRREQ_OE, DISABLE, Reg);
+		NV_REGW(s_hRmGlobal, NvRmModuleID_Pmif, 0,
+				APBDEV_PMC_CNTRL_0, Reg);
+	}
+
+	//Restore the saved module(s) context
+	//Interrupt, gpio, pin mux, clock management etc
+	perform_context_operation(PowerModuleContext_Restore);
+}
+
 void cpu_ap20_do_lp2(void)
 {
     NvU32 irq, moduleId;
@@ -83,9 +112,6 @@ void cpu_ap20_do_lp2(void)
     //Save our context ptrs to scratch regs
     NV_REGW(s_hRmGlobal, NvRmModuleID_Pmif, 0,
             APBDEV_PMC_SCRATCH1_0, g_resume);
-    //LP0 needs the resume address in SCRATCH41
-    NV_REGW(s_hRmGlobal, NvRmModuleID_Pmif, 0,
-            APBDEV_PMC_SCRATCH41_0, g_resume);
     NV_REGW(s_hRmGlobal, NvRmModuleID_Pmif, 0,
             APBDEV_PMC_SCRATCH37_0, g_contextSavePA);
 
diff --git a/arch/arm/mach-tegra/power.h b/arch/arm/mach-tegra/power.h
new file mode 100644
index 000000000000..e18863badaf4
--- /dev/null
+++ b/arch/arm/mach-tegra/power.h
@@ -0,0 +1,105 @@
+/*
+ * arch/arm/mach-tegra/power.h
+ *
+ * Header for tegra power
+ *
+ * Copyright (c) 2010, 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
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+#include "nvos.h"
+#include "nvrm_init.h"
+#include "nvrm_drf.h"
+#include "ap20/arapbpm.h"
+#include "nvrm_module.h"
+#include "ap20/arflow_ctlr.h"
+#include "ap20/arclk_rst.h"
+#include "nvrm_hardware_access.h"
+#include "nvrm_interrupt.h"
+#include "nvrm_power.h"
+#include "nvrm_power_private.h"
+#include <linux/kernel.h>
+#include <linux/platform_device.h>
+#include <linux/interrupt.h>
+#include "ap20/nvboot_pmc_scratch_map.h"
+
+extern NvRmDeviceHandle s_hRmGlobal;
+
+#define NUM_LOCAL_TIMER_REGISTERS 3
+#define WAKE_PAD_MIN_LATCH_TIME_US 130
+#define WAKE_PAD_MIN_SAMPLE_TIME_US 70
+
+#define NV_CAR_REGR(pBase, reg)\
+		NV_READ32( (((NvUPtr)(pBase)) + CLK_RST_CONTROLLER_##reg##_0))
+#define NV_CAR_REGW(pBase, reg, val)\
+		NV_WRITE32( (((NvUPtr)(pBase)) + CLK_RST_CONTROLLER_##reg##_0), (val))
+#define NV_CAR_REGR_OFFSET(pBase, off)\
+		NV_READ32( (((NvUPtr)(pBase)) + off))
+#define NV_CAR_REGW_OFFSET(pBase, off, val)\
+		NV_WRITE32( (((NvUPtr)(pBase)) + off), (val))
+
+#define CAR_CLK_SOURCES_OFFSET_START	CLK_RST_CONTROLLER_CLK_SOURCE_I2S1_0
+#define CAR_CLK_SOURCES_OFFSET_END		CLK_RST_CONTROLLER_CLK_SOURCE_OSC_0
+#define CAR_CLK_SOURCES_REGISTER_COUNT\
+	((CAR_CLK_SOURCES_OFFSET_END - CAR_CLK_SOURCES_OFFSET_START +\
+		sizeof(NvU32)) / sizeof(NvU32))
+
+typedef struct
+{
+	NvU32 *pBase;
+	NvU32 *pContext;
+}  power_module_context;
+
+struct power_context
+{
+	NvU32 context_size_words;
+	NvU32 *first_context_location;
+	power_module_context interrupt;
+	power_module_context misc;
+	power_module_context clock_reset;
+	power_module_context apb_dma;
+	power_module_context apb_dma_chan;
+	power_module_context gpio;
+	power_module_context vde;
+	power_module_context mc;
+};
+
+typedef enum
+{
+	PowerModuleContext_Init,
+	PowerModuleContext_Save,
+	PowerModuleContext_Restore,
+	PowerModuleContext_DisableInterrupt,
+	PowerModuleContext_SaveLP1,
+	PowerModuleContext_RestoreLP1,
+	PowerModuleContext_Force32 = 0x7fffffff
+} PowerModuleContext;
+
+typedef enum
+{
+	PowerPllA = 0,
+	PowerPllC,
+	PowerPllM,
+	PowerPllP,
+	PowerPllX
+} PowerPll;
+
+typedef enum
+{
+	POWER_STATE_LP2,
+	POWER_STATE_LP1,
+	POWER_STATE_LP0,
+} PowerState;