diff --git a/include/lib/cpus/aarch64/denver.h b/include/lib/cpus/aarch64/denver.h
index 0de094a4c94a1bad7ee6b09d27fa9f7425c42197..e08353374029a42ef1b32da0e819f15e2e2a024c 100644
--- a/include/lib/cpus/aarch64/denver.h
+++ b/include/lib/cpus/aarch64/denver.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2015-2016, ARM Limited and Contributors. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions are met:
@@ -44,4 +44,11 @@
 /* CPU state ids - implementation defined */
 #define DENVER_CPU_STATE_POWER_DOWN	0x3
 
+#ifndef __ASSEMBLY__
+
+/* Disable Dynamic Code Optimisation */
+void denver_disable_dco(void);
+
+#endif
+
 #endif /* __DENVER_H__ */
diff --git a/lib/cpus/aarch64/denver.S b/lib/cpus/aarch64/denver.S
index c38515562f233fd1a4c56f9af6caeb1db1182f89..3e238a1c19e6c9e4834b4ce0ce5eefa9696b61c3 100644
--- a/lib/cpus/aarch64/denver.S
+++ b/lib/cpus/aarch64/denver.S
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2015-2016, ARM Limited and Contributors. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions are met:
@@ -35,6 +35,8 @@
 #include <cpu_macros.S>
 #include <plat_macros.S>
 
+	.global	denver_disable_dco
+
 	/* ---------------------------------------------
 	 * Disable debug interfaces
 	 * ---------------------------------------------
@@ -111,22 +113,6 @@ func denver_core_pwr_dwn
 
 	mov	x19, x30
 
-	/* ----------------------------------------------------
-	 * We enter the 'core power gated with ARM state not
-	 * retained' power state during CPU power down. We let
-	 * DCO know that we expect to enter this power state
-	 * by writing to the ACTLR_EL1 register.
- 	 * ----------------------------------------------------
- 	 */
-	mov	x0, #DENVER_CPU_STATE_POWER_DOWN
-	msr	actlr_el1, x0
-
-	/* ---------------------------------------------
-	 * Force DCO to be quiescent
-	 * ---------------------------------------------
-	 */
-	bl	denver_disable_dco
-
 	/* ---------------------------------------------
 	 * Force the debug interfaces to be quiescent
 	 * ---------------------------------------------
diff --git a/plat/nvidia/tegra/common/drivers/memctrl/memctrl_v1.c b/plat/nvidia/tegra/common/drivers/memctrl/memctrl_v1.c
index ac7d1415a24fcf5ae3be90ae4a94be39f89824df..c4170504c43801bbd2985fa053ff2fa9e267d9f2 100644
--- a/plat/nvidia/tegra/common/drivers/memctrl/memctrl_v1.c
+++ b/plat/nvidia/tegra/common/drivers/memctrl/memctrl_v1.c
@@ -89,6 +89,14 @@ void tegra_memctrl_setup(void)
 	tegra_mc_write_32(MC_VIDEO_PROTECT_SIZE_MB, video_mem_size);
 }
 
+/*
+ * Restore Memory Controller settings after "System Suspend"
+ */
+void tegra_memctrl_restore_settings(void)
+{
+	tegra_memctrl_setup();
+}
+
 /*
  * Secure the BL31 DRAM aperture.
  *
@@ -107,6 +115,20 @@ void tegra_memctrl_tzdram_setup(uint64_t phys_base, uint32_t size_in_bytes)
 	tegra_mc_write_32(MC_SECURITY_CFG1_0, size_in_bytes >> 20);
 }
 
+/*
+ * Secure the BL31 TZRAM aperture.
+ *
+ * phys_base = physical base of TZRAM aperture
+ * size_in_bytes = size of aperture in bytes
+ */
+void tegra_memctrl_tzram_setup(uint64_t phys_base, uint32_t size_in_bytes)
+{
+	/*
+	 * The v1 hardware controller does not have any registers
+	 * for setting up the on-chip TZRAM.
+	 */
+}
+
 static void tegra_clear_videomem(uintptr_t non_overlap_area_start,
 				 unsigned long long non_overlap_area_size)
 {
diff --git a/plat/nvidia/tegra/common/tegra_bl31_setup.c b/plat/nvidia/tegra/common/tegra_bl31_setup.c
index 3a9514bda92e9b86c311c680268c783ae315c4c7..72da4b3c0a87158e15da578b79e8c3e0dffa40eb 100644
--- a/plat/nvidia/tegra/common/tegra_bl31_setup.c
+++ b/plat/nvidia/tegra/common/tegra_bl31_setup.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2015-2016, ARM Limited and Contributors. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions are met:
@@ -44,6 +44,7 @@
 #include <platform.h>
 #include <platform_def.h>
 #include <stddef.h>
+#include <tegra_def.h>
 #include <tegra_private.h>
 
 /*******************************************************************************
@@ -183,6 +184,12 @@ void bl31_platform_setup(void)
 	tegra_memctrl_tzdram_setup(plat_bl31_params_from_bl2.tzdram_base,
 			plat_bl31_params_from_bl2.tzdram_size);
 
+	/*
+	 * Set up the TZRAM memory aperture to allow only secure world
+	 * access
+	 */
+	tegra_memctrl_tzram_setup(TEGRA_TZRAM_BASE, TEGRA_TZRAM_SIZE);
+
 	/* Set the next EL to be AArch64 */
 	tmp_reg = SCR_RES1_BITS | SCR_RW_BIT;
 	write_scr(tmp_reg);
@@ -193,6 +200,16 @@ void bl31_platform_setup(void)
 	INFO("BL3-1: Tegra platform setup complete\n");
 }
 
+/*******************************************************************************
+ * Perform any BL3-1 platform runtime setup prior to BL3-1 cold boot exit
+ ******************************************************************************/
+void bl31_plat_runtime_setup(void)
+{
+	/* Initialize the runtime console */
+	console_init(tegra_console_base, TEGRA_BOOT_UART_CLK_IN_HZ,
+		TEGRA_CONSOLE_BAUDRATE);
+}
+
 /*******************************************************************************
  * Perform the very early platform specific architectural setup here. At the
  * moment this only intializes the mmu in a quick and dirty way.
@@ -208,6 +225,7 @@ void bl31_plat_arch_setup(void)
 #if USE_COHERENT_MEM
 	unsigned long coh_start, coh_size;
 #endif
+	plat_params_from_bl2_t *params_from_bl2 = bl31_get_plat_params();
 
 	/* add memory regions */
 	mmap_add_region(total_base, total_base,
@@ -217,6 +235,14 @@ void bl31_plat_arch_setup(void)
 			ro_size,
 			MT_MEMORY | MT_RO | MT_SECURE);
 
+	/* map TZDRAM used by BL31 as coherent memory */
+	if (TEGRA_TZRAM_BASE == tegra_bl31_phys_base) {
+		mmap_add_region(params_from_bl2->tzdram_base,
+				params_from_bl2->tzdram_base,
+				BL31_SIZE,
+				MT_DEVICE | MT_RW | MT_SECURE);
+	}
+
 #if USE_COHERENT_MEM
 	coh_start = total_base + (BL_COHERENT_RAM_BASE - BL31_RO_BASE);
 	coh_size = BL_COHERENT_RAM_END - BL_COHERENT_RAM_BASE;
diff --git a/plat/nvidia/tegra/common/tegra_common.mk b/plat/nvidia/tegra/common/tegra_common.mk
index 82da7fd046a22c917f83f59a81dd6cfd12fee5f3..c9e92557c89e946c77a893186222da4e77159c30 100644
--- a/plat/nvidia/tegra/common/tegra_common.mk
+++ b/plat/nvidia/tegra/common/tegra_common.mk
@@ -59,4 +59,5 @@ BL31_SOURCES		+=	drivers/arm/gic/gic_v2.c			\
 				${COMMON_DIR}/tegra_delay_timer.c		\
 				${COMMON_DIR}/tegra_gic.c			\
 				${COMMON_DIR}/tegra_pm.c			\
+				${COMMON_DIR}/tegra_sip_calls.c			\
 				${COMMON_DIR}/tegra_topology.c
diff --git a/plat/nvidia/tegra/common/tegra_pm.c b/plat/nvidia/tegra/common/tegra_pm.c
index 8b7a0590303d9e798f7b90aa72a85611c6870066..f5ef3e764bfd8d1b044d8eecfd7c64a3de87b2e9 100644
--- a/plat/nvidia/tegra/common/tegra_pm.c
+++ b/plat/nvidia/tegra/common/tegra_pm.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2015-2016, ARM Limited and Contributors. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions are met:
@@ -54,7 +54,9 @@ extern uint64_t tegra_sec_entry_point;
 #pragma weak tegra_soc_pwr_domain_on
 #pragma weak tegra_soc_pwr_domain_off
 #pragma weak tegra_soc_pwr_domain_on_finish
+#pragma weak tegra_soc_pwr_domain_power_down_wfi
 #pragma weak tegra_soc_prepare_system_reset
+#pragma weak tegra_soc_prepare_system_off
 
 int tegra_soc_pwr_domain_suspend(const psci_power_state_t *target_state)
 {
@@ -76,11 +78,22 @@ int tegra_soc_pwr_domain_on_finish(const psci_power_state_t *target_state)
 	return PSCI_E_SUCCESS;
 }
 
+int tegra_soc_pwr_domain_power_down_wfi(const psci_power_state_t *target_state)
+{
+	return PSCI_E_SUCCESS;
+}
+
 int tegra_soc_prepare_system_reset(void)
 {
 	return PSCI_E_SUCCESS;
 }
 
+__dead2 void tegra_soc_prepare_system_off(void)
+{
+	ERROR("Tegra System Off: operation not handled.\n");
+	panic();
+}
+
 /*******************************************************************************
  * This handler is called by the PSCI implementation during the `SYSTEM_SUSPEND`
  * call to get the `power_state` parameter. This allows the platform to encode
@@ -129,7 +142,7 @@ void tegra_pwr_domain_off(const psci_power_state_t *target_state)
 }
 
 /*******************************************************************************
- * Handler called when called when a power domain is about to be suspended. The
+ * Handler called when a power domain is about to be suspended. The
  * target_state encodes the power state that each level should transition to.
  ******************************************************************************/
 void tegra_pwr_domain_suspend(const psci_power_state_t *target_state)
@@ -140,6 +153,24 @@ void tegra_pwr_domain_suspend(const psci_power_state_t *target_state)
 	tegra_gic_cpuif_deactivate();
 }
 
+/*******************************************************************************
+ * Handler called at the end of the power domain suspend sequence. The
+ * target_state encodes the power state that each level should transition to.
+ ******************************************************************************/
+__dead2 void tegra_pwr_domain_power_down_wfi(const psci_power_state_t
+					     *target_state)
+{
+	/* call the chip's power down handler */
+	tegra_soc_pwr_domain_power_down_wfi(target_state);
+
+	/* enter power down state */
+	wfi();
+
+	/* we can never reach here */
+	ERROR("%s: operation not handled.\n", __func__);
+	panic();
+}
+
 /*******************************************************************************
  * Handler called when a power domain has just been powered on after
  * being turned off earlier. The target_state encodes the low power state that
@@ -161,14 +192,10 @@ void tegra_pwr_domain_on_finish(const psci_power_state_t *target_state)
 			PSTATE_ID_SOC_POWERDN) {
 
 		/*
-		 * Lock scratch registers which hold the CPU vectors.
-		 */
-		tegra_pmc_lock_cpu_vectors();
-
-		/*
-		 * SMMU configuration.
+		 * Restore Memory Controller settings as it loses state
+		 * during system suspend.
 		 */
-		tegra_memctrl_setup();
+		tegra_memctrl_restore_settings();
 
 		/*
 		 * Security configuration to allow DRAM/device access.
@@ -199,8 +226,9 @@ void tegra_pwr_domain_suspend_finish(const psci_power_state_t *target_state)
  ******************************************************************************/
 __dead2 void tegra_system_off(void)
 {
-	ERROR("Tegra System Off: operation not handled.\n");
-	panic();
+	INFO("Powering down system...\n");
+
+	tegra_soc_prepare_system_off();
 }
 
 /*******************************************************************************
@@ -208,6 +236,8 @@ __dead2 void tegra_system_off(void)
  ******************************************************************************/
 __dead2 void tegra_system_reset(void)
 {
+	INFO("Restarting system...\n");
+
 	/* per-SoC system reset handler */
 	tegra_soc_prepare_system_reset();
 
@@ -223,13 +253,8 @@ __dead2 void tegra_system_reset(void)
 int32_t tegra_validate_power_state(unsigned int power_state,
 				   psci_power_state_t *req_state)
 {
-	int pwr_lvl = psci_get_pstate_pwrlvl(power_state);
-
 	assert(req_state);
 
-	if (pwr_lvl > PLAT_MAX_PWR_LVL)
-		return PSCI_E_INVALID_PARAMS;
-
 	return tegra_soc_validate_power_state(power_state, req_state);
 }
 
@@ -258,6 +283,7 @@ static const plat_psci_ops_t tegra_plat_psci_ops = {
 	.pwr_domain_suspend		= tegra_pwr_domain_suspend,
 	.pwr_domain_on_finish		= tegra_pwr_domain_on_finish,
 	.pwr_domain_suspend_finish	= tegra_pwr_domain_suspend_finish,
+	.pwr_domain_pwr_down_wfi	= tegra_pwr_domain_power_down_wfi,
 	.system_off			= tegra_system_off,
 	.system_reset			= tegra_system_reset,
 	.validate_power_state		= tegra_validate_power_state,
diff --git a/plat/nvidia/tegra/soc/t210/plat_sip_calls.c b/plat/nvidia/tegra/common/tegra_sip_calls.c
similarity index 83%
rename from plat/nvidia/tegra/soc/t210/plat_sip_calls.c
rename to plat/nvidia/tegra/common/tegra_sip_calls.c
index 7d9838a35a16f91be54128be2f449e5766fac610..3bcd4418f0750d791c429f030bbf2db9e068c1e9 100644
--- a/plat/nvidia/tegra/soc/t210/plat_sip_calls.c
+++ b/plat/nvidia/tegra/common/tegra_sip_calls.c
@@ -32,7 +32,6 @@
 #include <arch_helpers.h>
 #include <assert.h>
 #include <bl_common.h>
-#include <context_mgmt.h>
 #include <debug.h>
 #include <errno.h>
 #include <memctrl.h>
@@ -40,14 +39,30 @@
 #include <tegra_private.h>
 
 /*******************************************************************************
- * Tegra210 SiP SMCs
+ * Common Tegra SiP SMCs
  ******************************************************************************/
 #define TEGRA_SIP_NEW_VIDEOMEM_REGION		0x82000003
 
+/*******************************************************************************
+ * SoC specific SiP handler
+ ******************************************************************************/
+#pragma weak plat_sip_handler
+int plat_sip_handler(uint32_t smc_fid,
+		     uint64_t x1,
+		     uint64_t x2,
+		     uint64_t x3,
+		     uint64_t x4,
+		     void *cookie,
+		     void *handle,
+		     uint64_t flags)
+{
+	return -ENOTSUP;
+}
+
 /*******************************************************************************
  * This function is responsible for handling all SiP calls from the NS world
  ******************************************************************************/
-uint64_t tegra210_sip_handler(uint32_t smc_fid,
+uint64_t tegra_sip_handler(uint32_t smc_fid,
 			   uint64_t x1,
 			   uint64_t x2,
 			   uint64_t x3,
@@ -64,6 +79,11 @@ uint64_t tegra210_sip_handler(uint32_t smc_fid,
 	if (!ns)
 		SMC_RET1(handle, SMC_UNK);
 
+	/* Check if this is a SoC specific SiP */
+	err = plat_sip_handler(smc_fid, x1, x2, x3, x4, cookie, handle, flags);
+	if (err == 0)
+		SMC_RET1(handle, err);
+
 	switch (smc_fid) {
 
 	case TEGRA_SIP_NEW_VIDEOMEM_REGION:
@@ -104,11 +124,11 @@ uint64_t tegra210_sip_handler(uint32_t smc_fid,
 
 /* Define a runtime service descriptor for fast SMC calls */
 DECLARE_RT_SVC(
-	tegra210_sip_fast,
+	tegra_sip_fast,
 
 	OEN_SIP_START,
 	OEN_SIP_END,
 	SMC_TYPE_FAST,
 	NULL,
-	tegra210_sip_handler
+	tegra_sip_handler
 );
diff --git a/plat/nvidia/tegra/include/drivers/memctrl.h b/plat/nvidia/tegra/include/drivers/memctrl.h
index b06b4de789709f9968ae5c30ba6874311464f78a..a3f08755acf8c5bbd87a48547b3ebd5e6e1c1d19 100644
--- a/plat/nvidia/tegra/include/drivers/memctrl.h
+++ b/plat/nvidia/tegra/include/drivers/memctrl.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2015-2016, ARM Limited and Contributors. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions are met:
@@ -32,7 +32,9 @@
 #define __MEMCTRL_H__
 
 void tegra_memctrl_setup(void);
+void tegra_memctrl_restore_settings(void);
 void tegra_memctrl_tzdram_setup(uint64_t phys_base, uint32_t size_in_bytes);
+void tegra_memctrl_tzram_setup(uint64_t phys_base, uint32_t size_in_bytes);
 void tegra_memctrl_videomem_setup(uint64_t phys_base, uint32_t size_in_bytes);
 
 #endif /* __MEMCTRL_H__ */
diff --git a/plat/nvidia/tegra/include/drivers/memctrl_v1.h b/plat/nvidia/tegra/include/drivers/memctrl_v1.h
index e2e05277b100cde34eff994300fb64d2e12dd38f..e44a9ea9240e826aa4242d777d2148d005a5ed12 100644
--- a/plat/nvidia/tegra/include/drivers/memctrl_v1.h
+++ b/plat/nvidia/tegra/include/drivers/memctrl_v1.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2015-2016, ARM Limited and Contributors. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions are met:
diff --git a/plat/nvidia/tegra/include/platform_def.h b/plat/nvidia/tegra/include/platform_def.h
index 92c4c554baaf74f8d64141cb8b920aea7d7305ad..ad245e2f899ce4c7cc0ad71191e20a8056bd8879 100644
--- a/plat/nvidia/tegra/include/platform_def.h
+++ b/plat/nvidia/tegra/include/platform_def.h
@@ -52,12 +52,6 @@
 #define PLAT_NUM_PWR_DOMAINS		(PLATFORM_CORE_COUNT + \
 					 PLATFORM_CLUSTER_COUNT + 1)
 
-/*******************************************************************************
- * Platform power states
- ******************************************************************************/
-#define PLAT_MAX_RET_STATE		1
-#define PLAT_MAX_OFF_STATE		(PSTATE_ID_SOC_POWERDN + 1)
-
 /*******************************************************************************
  * Platform console related constants
  ******************************************************************************/
@@ -74,7 +68,7 @@
 /*******************************************************************************
  * BL31 specific defines.
  ******************************************************************************/
-#define BL31_SIZE			0x20000
+#define BL31_SIZE			0x40000
 #define BL31_BASE			TZDRAM_BASE
 #define BL31_LIMIT			(TZDRAM_BASE + BL31_SIZE - 1)
 #define BL32_BASE			(TZDRAM_BASE + BL31_SIZE)
diff --git a/plat/nvidia/tegra/include/t132/tegra_def.h b/plat/nvidia/tegra/include/t132/tegra_def.h
index 09d9b74290ae3466fda1fa97574b17978f1250a9..318f4def15ae67b43b112c1b4effd14ecb7ab996 100644
--- a/plat/nvidia/tegra/include/t132/tegra_def.h
+++ b/plat/nvidia/tegra/include/t132/tegra_def.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2015-2016, ARM Limited and Contributors. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions are met:
@@ -39,6 +39,15 @@
  ******************************************************************************/
 #define PSTATE_ID_SOC_POWERDN	0xD
 
+/*******************************************************************************
+ * Platform power states (used by PSCI framework)
+ *
+ * - PLAT_MAX_RET_STATE should be less than lowest PSTATE_ID
+ * - PLAT_MAX_OFF_STATE should be greater than the highest PSTATE_ID
+ ******************************************************************************/
+#define PLAT_MAX_RET_STATE		1
+#define PLAT_MAX_OFF_STATE		(PSTATE_ID_SOC_POWERDN + 1)
+
 /*******************************************************************************
  * GIC memory map
  ******************************************************************************/
@@ -89,4 +98,10 @@
  ******************************************************************************/
 #define TEGRA_MC_BASE			0x70019000
 
+/*******************************************************************************
+ * Tegra TZRAM constants
+ ******************************************************************************/
+#define TEGRA_TZRAM_BASE		0x7C010000
+#define TEGRA_TZRAM_SIZE		0x10000
+
 #endif /* __TEGRA_DEF_H__ */
diff --git a/plat/nvidia/tegra/include/t210/tegra_def.h b/plat/nvidia/tegra/include/t210/tegra_def.h
index 8be39bb32b763fa3d515c121a73d08fd77161600..ce85427e434325bef53dd37192eb944e9b043852 100644
--- a/plat/nvidia/tegra/include/t210/tegra_def.h
+++ b/plat/nvidia/tegra/include/t210/tegra_def.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2015-2016, ARM Limited and Contributors. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions are met:
@@ -47,6 +47,15 @@
  ******************************************************************************/
 #define PLAT_SYS_SUSPEND_STATE_ID	PSTATE_ID_SOC_POWERDN
 
+/*******************************************************************************
+ * Platform power states (used by PSCI framework)
+ *
+ * - PLAT_MAX_RET_STATE should be less than lowest PSTATE_ID
+ * - PLAT_MAX_OFF_STATE should be greater than the highest PSTATE_ID
+ ******************************************************************************/
+#define PLAT_MAX_RET_STATE		1
+#define PLAT_MAX_OFF_STATE		(PSTATE_ID_SOC_POWERDN + 1)
+
 /*******************************************************************************
  * GIC memory map
  ******************************************************************************/
@@ -114,4 +123,10 @@
  ******************************************************************************/
 #define TEGRA_MC_BASE			0x70019000
 
+/*******************************************************************************
+ * Tegra TZRAM constants
+ ******************************************************************************/
+#define TEGRA_TZRAM_BASE		0x7C010000
+#define TEGRA_TZRAM_SIZE		0x10000
+
 #endif /* __TEGRA_DEF_H__ */
diff --git a/plat/nvidia/tegra/platform.mk b/plat/nvidia/tegra/platform.mk
index cec7caff36a1b3b3946854409683a61929f5378c..756899cb752428c88f813d080656f593cb047dcc 100644
--- a/plat/nvidia/tegra/platform.mk
+++ b/plat/nvidia/tegra/platform.mk
@@ -1,5 +1,5 @@
 #
-# Copyright (c) 2015, ARM Limited and Contributors. All rights reserved.
+# Copyright (c) 2015-2016, ARM Limited and Contributors. All rights reserved.
 #
 # Redistribution and use in source and binary forms, with or without
 # modification, are permitted provided that the following conditions are met:
@@ -30,6 +30,9 @@
 
 SOC_DIR			:=	plat/nvidia/tegra/soc/${TARGET_SOC}
 
+# Enable PSCI v1.0 extended state ID format
+PSCI_EXTENDED_STATE_ID	:=	1
+
 # Disable the PSCI platform compatibility layer
 ENABLE_PLAT_COMPAT	:=	0
 
diff --git a/plat/nvidia/tegra/soc/t132/plat_psci_handlers.c b/plat/nvidia/tegra/soc/t132/plat_psci_handlers.c
index 48a2fbaa37015b92030f2316d35218f04af069ae..f05f3d0e9f85c5f1cef3fd646d13990bcc19253c 100644
--- a/plat/nvidia/tegra/soc/t132/plat_psci_handlers.c
+++ b/plat/nvidia/tegra/soc/t132/plat_psci_handlers.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2015-2016, ARM Limited and Contributors. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions are met:
@@ -59,36 +59,15 @@ static int cpu_powergate_mask[PLATFORM_MAX_CPUS_PER_CLUSTER];
 int32_t tegra_soc_validate_power_state(unsigned int power_state,
 					psci_power_state_t *req_state)
 {
-	int pwr_lvl = psci_get_pstate_pwrlvl(power_state);
 	int state_id = psci_get_pstate_id(power_state);
 	int cpu = read_mpidr() & MPIDR_CPU_MASK;
 
-	if (pwr_lvl > PLAT_MAX_PWR_LVL)
-		return PSCI_E_INVALID_PARAMS;
-
-	/* Sanity check the requested afflvl */
-	if (psci_get_pstate_type(power_state) == PSTATE_TYPE_STANDBY) {
-		/*
-		 * It's possible to enter standby only on affinity level 0 i.e.
-		 * a cpu on Tegra. Ignore any other affinity level.
-		 */
-		if (pwr_lvl != MPIDR_AFFLVL0)
-			return PSCI_E_INVALID_PARAMS;
-
-		/* power domain in standby state */
-		req_state->pwr_domain_state[pwr_lvl] = PLAT_MAX_RET_STATE;
-
-		return PSCI_E_SUCCESS;
-	}
-
 	/*
 	 * Sanity check the requested state id, power level and CPU number.
 	 * Currently T132 only supports SYSTEM_SUSPEND on last standing CPU
 	 * i.e. CPU 0
 	 */
-	if ((pwr_lvl != PLAT_MAX_PWR_LVL) ||
-	    (state_id != PSTATE_ID_SOC_POWERDN) ||
-	    (cpu != 0)) {
+	if ((state_id != PSTATE_ID_SOC_POWERDN) || (cpu != 0)) {
 		ERROR("unsupported state id @ power level\n");
 		return PSCI_E_INVALID_PARAMS;
 	}
@@ -128,9 +107,26 @@ int tegra_soc_pwr_domain_on(u_register_t mpidr)
 	return PSCI_E_SUCCESS;
 }
 
+int tegra_soc_pwr_domain_on_finish(const psci_power_state_t *target_state)
+{
+	/*
+	 * Lock scratch registers which hold the CPU vectors
+	 */
+	tegra_pmc_lock_cpu_vectors();
+
+	return PSCI_E_SUCCESS;
+}
+
 int tegra_soc_pwr_domain_off(const psci_power_state_t *target_state)
 {
 	tegra_fc_cpu_off(read_mpidr() & MPIDR_CPU_MASK);
+
+	/* Disable DCO operations */
+	denver_disable_dco();
+
+	/* Power down the CPU */
+	write_actlr_el1(DENVER_CPU_STATE_POWER_DOWN);
+
 	return PSCI_E_SUCCESS;
 }
 
@@ -149,7 +145,10 @@ int tegra_soc_pwr_domain_suspend(const psci_power_state_t *target_state)
 	/* Program FC to enter suspend state */
 	tegra_fc_cpu_powerdn(read_mpidr());
 
-	/* Suspend DCO operations */
+	/* Disable DCO operations */
+	denver_disable_dco();
+
+	/* Program the suspend state ID */
 	write_actlr_el1(target_state->pwr_domain_state[PLAT_MAX_PWR_LVL]);
 
 	return PSCI_E_SUCCESS;
diff --git a/plat/nvidia/tegra/soc/t132/plat_sip_calls.c b/plat/nvidia/tegra/soc/t132/plat_sip_calls.c
index 450e1dd3b9f10bed12bbb6eb476a49d7a6f1c60d..6c89944e9fa94661b8a33ae8dded8da9c6cac388 100644
--- a/plat/nvidia/tegra/soc/t132/plat_sip_calls.c
+++ b/plat/nvidia/tegra/soc/t132/plat_sip_calls.c
@@ -35,8 +35,6 @@
 #include <context_mgmt.h>
 #include <debug.h>
 #include <errno.h>
-#include <memctrl.h>
-#include <runtime_svc.h>
 #include <tegra_private.h>
 
 #define NS_SWITCH_AARCH32	1
@@ -45,7 +43,6 @@
 /*******************************************************************************
  * Tegra132 SiP SMCs
  ******************************************************************************/
-#define TEGRA_SIP_NEW_VIDEOMEM_REGION		0x82000003
 #define TEGRA_SIP_AARCH_SWITCH			0x82000004
 
 /*******************************************************************************
@@ -56,55 +53,19 @@
 #define SPSR64		SPSR_64(MODE_EL2, MODE_SP_ELX, DISABLE_ALL_EXCEPTIONS)
 
 /*******************************************************************************
- * This function is responsible for handling all SiP calls from the NS world
+ * This function is responsible for handling all T132 SiP calls
  ******************************************************************************/
-uint64_t tegra132_sip_handler(uint32_t smc_fid,
-			   uint64_t x1,
-			   uint64_t x2,
-			   uint64_t x3,
-			   uint64_t x4,
-			   void *cookie,
-			   void *handle,
-			   uint64_t flags)
+int plat_sip_handler(uint32_t smc_fid,
+		     uint64_t x1,
+		     uint64_t x2,
+		     uint64_t x3,
+		     uint64_t x4,
+		     void *cookie,
+		     void *handle,
+		     uint64_t flags)
 {
-	uint32_t ns;
-	int err;
-
-	/* Determine which security state this SMC originated from */
-	ns = is_caller_non_secure(flags);
-	if (!ns)
-		SMC_RET1(handle, SMC_UNK);
-
 	switch (smc_fid) {
 
-	case TEGRA_SIP_NEW_VIDEOMEM_REGION:
-
-		/* clean up the high bits */
-		x1 = (uint32_t)x1;
-		x2 = (uint32_t)x2;
-
-		/*
-		 * Check if Video Memory overlaps TZDRAM (contains bl31/bl32)
-		 * or falls outside of the valid DRAM range
-		 */
-		err = bl31_check_ns_address(x1, x2);
-		if (err)
-			SMC_RET1(handle, err);
-
-		/*
-		 * Check if Video Memory is aligned to 1MB.
-		 */
-		if ((x1 & 0xFFFFF) || (x2 & 0xFFFFF)) {
-			ERROR("Unaligned Video Memory base address!\n");
-			SMC_RET1(handle, -ENOTSUP);
-		}
-
-		/* new video memory carveout settings */
-		tegra_memctrl_videomem_setup(x1, x2);
-
-		SMC_RET1(handle, 0);
-		break;
-
 	case TEGRA_SIP_AARCH_SWITCH:
 
 		/* clean up the high bits */
@@ -113,7 +74,7 @@ uint64_t tegra132_sip_handler(uint32_t smc_fid,
 
 		if (!x1 || x2 > NS_SWITCH_AARCH32) {
 			ERROR("%s: invalid parameters\n", __func__);
-			SMC_RET1(handle, SMC_UNK);
+			return -EINVAL;
 		}
 
 		/* x1 = ns entry point */
@@ -125,24 +86,12 @@ uint64_t tegra132_sip_handler(uint32_t smc_fid,
 
 		INFO("CPU switched to AARCH%s mode\n",
 			(x2 == NS_SWITCH_AARCH32) ? "32" : "64");
-		SMC_RET1(handle, 0);
-		break;
+		return 0;
 
 	default:
 		ERROR("%s: unhandled SMC (0x%x)\n", __func__, smc_fid);
 		break;
 	}
 
-	SMC_RET1(handle, SMC_UNK);
+	return -ENOTSUP;
 }
-
-/* Define a runtime service descriptor for fast SMC calls */
-DECLARE_RT_SVC(
-	tegra132_sip_fast,
-
-	OEN_SIP_START,
-	OEN_SIP_END,
-	SMC_TYPE_FAST,
-	NULL,
-	tegra132_sip_handler
-);
diff --git a/plat/nvidia/tegra/soc/t210/plat_psci_handlers.c b/plat/nvidia/tegra/soc/t210/plat_psci_handlers.c
index b184063d8f8042bebc9bdc07fb021d71b4883b85..95fb93fe00efa991e37ecc5289e9e52885550a36 100644
--- a/plat/nvidia/tegra/soc/t210/plat_psci_handlers.c
+++ b/plat/nvidia/tegra/soc/t210/plat_psci_handlers.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2015-2016, ARM Limited and Contributors. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions are met:
@@ -58,39 +58,14 @@ static int cpu_powergate_mask[PLATFORM_MAX_CPUS_PER_CLUSTER];
 int32_t tegra_soc_validate_power_state(unsigned int power_state,
 					psci_power_state_t *req_state)
 {
-	int pwr_lvl = psci_get_pstate_pwrlvl(power_state);
 	int state_id = psci_get_pstate_id(power_state);
 
-	if (pwr_lvl > PLAT_MAX_PWR_LVL) {
-		ERROR("%s: unsupported power_state (0x%x)\n", __func__,
-			power_state);
-		return PSCI_E_INVALID_PARAMS;
-	}
-
-	/* Sanity check the requested afflvl */
-	if (psci_get_pstate_type(power_state) == PSTATE_TYPE_STANDBY) {
-		/*
-		 * It's possible to enter standby only on affinity level 0 i.e.
-		 * a cpu on Tegra. Ignore any other affinity level.
-		 */
-		if (pwr_lvl != MPIDR_AFFLVL0)
-			return PSCI_E_INVALID_PARAMS;
-
-		/* power domain in standby state */
-		req_state->pwr_domain_state[pwr_lvl] = PLAT_MAX_RET_STATE;
-
-		return PSCI_E_SUCCESS;
-	}
-
 	/* Sanity check the requested state id */
 	switch (state_id) {
 	case PSTATE_ID_CORE_POWERDN:
 		/*
 		 * Core powerdown request only for afflvl 0
 		 */
-		if (pwr_lvl != MPIDR_AFFLVL0)
-			goto error;
-
 		req_state->pwr_domain_state[MPIDR_AFFLVL0] = state_id & 0xff;
 
 		break;
@@ -100,9 +75,6 @@ int32_t tegra_soc_validate_power_state(unsigned int power_state,
 		/*
 		 * Cluster powerdown/idle request only for afflvl 1
 		 */
-		if (pwr_lvl != MPIDR_AFFLVL1)
-			goto error;
-
 		req_state->pwr_domain_state[MPIDR_AFFLVL1] = state_id;
 		req_state->pwr_domain_state[MPIDR_AFFLVL0] = PLAT_MAX_OFF_STATE;
 
@@ -112,9 +84,6 @@ int32_t tegra_soc_validate_power_state(unsigned int power_state,
 		/*
 		 * System powerdown request only for afflvl 2
 		 */
-		if (pwr_lvl != PLAT_MAX_PWR_LVL)
-			goto error;
-
 		for (int i = MPIDR_AFFLVL0; i < PLAT_MAX_PWR_LVL; i++)
 			req_state->pwr_domain_state[i] = PLAT_MAX_OFF_STATE;
 
@@ -129,10 +98,6 @@ int32_t tegra_soc_validate_power_state(unsigned int power_state,
 	}
 
 	return PSCI_E_SUCCESS;
-
-error:
-	ERROR("%s: unsupported state id (%d)\n", __func__, state_id);
-	return PSCI_E_INVALID_PARAMS;
 }
 
 int tegra_soc_pwr_domain_suspend(const psci_power_state_t *target_state)
@@ -189,6 +154,11 @@ int tegra_soc_pwr_domain_on_finish(const psci_power_state_t *target_state)
 	if (target_state->pwr_domain_state[PLAT_MAX_PWR_LVL] ==
 			PLAT_SYS_SUSPEND_STATE_ID) {
 
+		/*
+		 * Lock scratch registers which hold the CPU vectors
+		 */
+		tegra_pmc_lock_cpu_vectors();
+
 		/*
 		 * Enable WRAP to INCR burst type conversions for
 		 * incoming requests on the AXI slave ports.
diff --git a/plat/nvidia/tegra/soc/t210/platform_t210.mk b/plat/nvidia/tegra/soc/t210/platform_t210.mk
index d83c54db0d33b13cfe9fc51efcc6093303e2d7ba..2c908f9da210968442273dbe9066c287de8c4483 100644
--- a/plat/nvidia/tegra/soc/t210/platform_t210.mk
+++ b/plat/nvidia/tegra/soc/t210/platform_t210.mk
@@ -51,7 +51,6 @@ BL31_SOURCES		+=	lib/cpus/aarch64/cortex_a53.S		\
 				${COMMON_DIR}/drivers/flowctrl/flowctrl.c	\
 				${COMMON_DIR}/drivers/memctrl/memctrl_v1.c	\
 				${SOC_DIR}/plat_psci_handlers.c		\
-				${SOC_DIR}/plat_sip_calls.c		\
 				${SOC_DIR}/plat_setup.c			\
 				${SOC_DIR}/plat_secondary.c
 
diff --git a/services/spd/trusty/trusty.c b/services/spd/trusty/trusty.c
index 78a68ba0bc0035eb209ed24528a95781be6b43a3..b21ce715bceb884acad3b8ea8a37886e776609bd 100644
--- a/services/spd/trusty/trusty.c
+++ b/services/spd/trusty/trusty.c
@@ -395,7 +395,7 @@ DECLARE_RT_SVC(
 DECLARE_RT_SVC(
 	trusty_std,
 
-	OEN_TOS_START,
+	OEN_TAP_START,
 	SMC_ENTITY_SECURE_MONITOR,
 	SMC_TYPE_STD,
 	NULL,