diff --git a/docs/firmware-design.md b/docs/firmware-design.md
index 68cad2e..90ce4b1 100644
--- a/docs/firmware-design.md
+++ b/docs/firmware-design.md
@@ -716,7 +716,7 @@
 |`PSCI_FEATURES`        | Yes     |                                           |
 |`CPU_FREEZE`           | No      |                                           |
 |`CPU_DEFAULT_SUSPEND`  | No      |                                           |
-|`CPU_HW_STATE`         | No      |                                           |
+|`NODE_HW_STATE`        | Yes*    |                                           |
 |`SYSTEM_SUSPEND`       | Yes*    |                                           |
 |`PSCI_SET_SUSPEND_MODE`| No      |                                           |
 |`PSCI_STAT_RESIDENCY`  | Yes*    |                                           |
diff --git a/docs/porting-guide.md b/docs/porting-guide.md
index 8dad4a0..195c937 100644
--- a/docs/porting-guide.md
+++ b/docs/porting-guide.md
@@ -1832,6 +1832,20 @@
 power state encoding for `power_state` parameter of PSCI_STAT_COUNT/RESIDENCY
 APIs as described in Section 5.18 of [PSCI].
 
+#### plat_psci_ops.get_node_hw_state()
+
+This is an optional function. If implemented this function is intended to return
+the power state of a node (identified by the first parameter, the `MPIDR`) in
+the power domain topology (identified by the second parameter, `power_level`),
+as retrieved from a power controller or equivalent component on the platform.
+Upon successful completion, the implementation must map and return the final
+status among `HW_ON`, `HW_OFF` or `HW_STANDBY`. Upon encountering failures, it
+must return either `PSCI_E_INVALID_PARAMS` or `PSCI_E_NOT_SUPPORTED` as
+appropriate.
+
+Implementations are not expected to handle `power_levels` greater than
+`PLAT_MAX_PWR_LVL`.
+
 3.6  Interrupt Management framework (in BL31)
 ----------------------------------------------
 BL31 implements an Interrupt Management Framework (IMF) to manage interrupts
diff --git a/docs/user-guide.md b/docs/user-guide.md
index a6959b1..d545262 100644
--- a/docs/user-guide.md
+++ b/docs/user-guide.md
@@ -64,7 +64,7 @@
 Install the required packages to build Trusted Firmware with the following
 command:
 
-    sudo apt-get install build-essential gcc make git
+    sudo apt-get install build-essential gcc make git libssl-dev
 
 Download and install the AArch64 little-endian GCC cross compiler as indicated
 in the [Linaro instructions][Linaro SW Instructions].
@@ -74,8 +74,6 @@
 *   `device-tree-compiler` package if you need to rebuild the Flattened Device
     Tree (FDT) source files (`.dts` files) provided with this software.
 
-*   `libssl-dev` package if Trusted Board Boot is enabled in the build.
-
 *   For debugging, ARM [Development Studio 5 (DS-5)][DS-5].
 
 
diff --git a/drivers/arm/gic/v3/gicv3_main.c b/drivers/arm/gic/v3/gicv3_main.c
index d13e2c9..ac43372 100644
--- a/drivers/arm/gic/v3/gicv3_main.c
+++ b/drivers/arm/gic/v3/gicv3_main.c
@@ -120,10 +120,12 @@
  ******************************************************************************/
 void gicv3_distif_init(void)
 {
+	unsigned int bitmap = 0;
+
 	assert(driver_data);
 	assert(driver_data->gicd_base);
-	assert(driver_data->g1s_interrupt_array);
-	assert(driver_data->g0_interrupt_array);
+	assert(driver_data->g1s_interrupt_array ||
+	       driver_data->g0_interrupt_array);
 
 	assert(IS_IN_EL3());
 
@@ -146,21 +148,25 @@
 	gicv3_spis_configure_defaults(driver_data->gicd_base);
 
 	/* Configure the G1S SPIs */
-	gicv3_secure_spis_configure(driver_data->gicd_base,
+	if (driver_data->g1s_interrupt_array) {
+		gicv3_secure_spis_configure(driver_data->gicd_base,
 					driver_data->g1s_interrupt_num,
 					driver_data->g1s_interrupt_array,
 					INTR_GROUP1S);
+		bitmap |= CTLR_ENABLE_G1S_BIT;
+	}
 
 	/* Configure the G0 SPIs */
-	gicv3_secure_spis_configure(driver_data->gicd_base,
+	if (driver_data->g0_interrupt_array) {
+		gicv3_secure_spis_configure(driver_data->gicd_base,
 					driver_data->g0_interrupt_num,
 					driver_data->g0_interrupt_array,
 					INTR_GROUP0);
+		bitmap |= CTLR_ENABLE_G0_BIT;
+	}
 
 	/* Enable the secure SPIs now that they have been configured */
-	gicd_set_ctlr(driver_data->gicd_base,
-		      CTLR_ENABLE_G1S_BIT | CTLR_ENABLE_G0_BIT,
-		      RWP_TRUE);
+	gicd_set_ctlr(driver_data->gicd_base, bitmap, RWP_TRUE);
 }
 
 /*******************************************************************************
@@ -177,8 +183,8 @@
 	assert(driver_data->rdistif_base_addrs);
 	assert(driver_data->gicd_base);
 	assert(gicd_read_ctlr(driver_data->gicd_base) & CTLR_ARE_S_BIT);
-	assert(driver_data->g1s_interrupt_array);
-	assert(driver_data->g0_interrupt_array);
+	assert(driver_data->g1s_interrupt_array ||
+	       driver_data->g0_interrupt_array);
 
 	assert(IS_IN_EL3());
 
@@ -188,16 +194,20 @@
 	gicv3_ppi_sgi_configure_defaults(gicr_base);
 
 	/* Configure the G1S SGIs/PPIs */
-	gicv3_secure_ppi_sgi_configure(gicr_base,
-					   driver_data->g1s_interrupt_num,
-					   driver_data->g1s_interrupt_array,
-					   INTR_GROUP1S);
+	if (driver_data->g1s_interrupt_array) {
+		gicv3_secure_ppi_sgi_configure(gicr_base,
+					driver_data->g1s_interrupt_num,
+					driver_data->g1s_interrupt_array,
+					INTR_GROUP1S);
+	}
 
 	/* Configure the G0 SGIs/PPIs */
-	gicv3_secure_ppi_sgi_configure(gicr_base,
-					   driver_data->g0_interrupt_num,
-					   driver_data->g0_interrupt_array,
-					   INTR_GROUP0);
+	if (driver_data->g0_interrupt_array) {
+		gicv3_secure_ppi_sgi_configure(gicr_base,
+					driver_data->g0_interrupt_num,
+					driver_data->g0_interrupt_array,
+					INTR_GROUP0);
+	}
 }
 
 /*******************************************************************************
diff --git a/include/lib/pmf/pmf_asm_macros.S b/include/lib/pmf/pmf_asm_macros.S
index 42dcd79..be7338b 100644
--- a/include/lib/pmf/pmf_asm_macros.S
+++ b/include/lib/pmf/pmf_asm_macros.S
@@ -34,12 +34,11 @@
 #define PMF_TS_SIZE	8
 
 	/*
-	 * This macro calculates the offset into the memory
-	 * region where the per-cpu timestamp value is stored
-	 * for the given service name and timestamp id.
+	 * This macro calculates the address of the per-cpu timestamp
+	 * for the given service name and local timestamp id.
 	 * Clobbers: x0 - x9
 	 */
-	.macro pmf_calc_timestamp_offset _name _tid
+	.macro pmf_calc_timestamp_addr _name _tid
 	mov	x9, x30
 	bl	plat_my_core_pos
 	mov	x30, x9
diff --git a/include/lib/pmf/pmf_helpers.h b/include/lib/pmf/pmf_helpers.h
index 9be6050..bb4242c 100644
--- a/include/lib/pmf/pmf_helpers.h
+++ b/include/lib/pmf/pmf_helpers.h
@@ -71,7 +71,9 @@
  */
 #define PMF_ALLOCATE_TIMESTAMP_MEMORY(_name, _total_id)		\
 	unsigned long long pmf_ts_mem_ ## _name[_total_id]	\
-	__section("pmf_timestamp_array") __used;
+	__aligned(CACHE_WRITEBACK_GRANULE)			\
+	__section("pmf_timestamp_array")			\
+	__used;
 
 /*
  * Convenience macro to validate tid index for the given TS array.
diff --git a/include/lib/psci/psci.h b/include/lib/psci/psci.h
index a583fef..02cbbf3 100644
--- a/include/lib/psci/psci.h
+++ b/include/lib/psci/psci.h
@@ -78,6 +78,8 @@
 #define PSCI_SYSTEM_OFF			0x84000008
 #define PSCI_SYSTEM_RESET		0x84000009
 #define PSCI_FEATURES			0x8400000A
+#define PSCI_NODE_HW_STATE_AARCH32	0x8400000d
+#define PSCI_NODE_HW_STATE_AARCH64	0xc400000d
 #define PSCI_SYSTEM_SUSPEND_AARCH32	0x8400000E
 #define PSCI_SYSTEM_SUSPEND_AARCH64	0xc400000E
 #define PSCI_STAT_RESIDENCY_AARCH32	0x84000010
@@ -200,6 +202,17 @@
 } aff_info_state_t;
 
 /*
+ * These are the power states reported by PSCI_NODE_HW_STATE API for the
+ * specified CPU. The definitions of these states can be found in Section 5.15.3
+ * of PSCI specification (ARM DEN 0022C).
+ */
+typedef enum {
+	HW_ON = 0,
+	HW_OFF = 1,
+	HW_STANDBY = 2
+} node_hw_state_t;
+
+/*
  * Macro to represent invalid affinity level within PSCI.
  */
 #define PSCI_INVALID_PWR_LVL	(PLAT_MAX_PWR_LVL + 1)
@@ -293,6 +306,7 @@
 	int (*translate_power_state_by_mpidr)(u_register_t mpidr,
 				    unsigned int power_state,
 				    psci_power_state_t *output_state);
+	int (*get_node_hw_state)(u_register_t mpidr, unsigned int power_level);
 } plat_psci_ops_t;
 
 /*******************************************************************************
@@ -330,6 +344,8 @@
 int psci_migrate(u_register_t target_cpu);
 int psci_migrate_info_type(void);
 long psci_migrate_info_up_cpu(void);
+int psci_node_hw_state(u_register_t target_cpu,
+		       unsigned int power_level);
 int psci_features(unsigned int psci_fid);
 void __dead2 psci_power_down_wfi(void);
 void psci_arch_setup(void);
diff --git a/include/lib/xlat_tables.h b/include/lib/xlat_tables.h
index d2ac6db..0e9800a 100644
--- a/include/lib/xlat_tables.h
+++ b/include/lib/xlat_tables.h
@@ -32,13 +32,27 @@
 #define __XLAT_TABLES_H__
 
 /* Miscellaneous MMU related constants */
+#define NUM_2MB_IN_GB		(1 << 9)
+#define NUM_4K_IN_2MB		(1 << 9)
+#define NUM_GB_IN_4GB		(1 << 2)
+
+#define TWO_MB_SHIFT		21
+#define ONE_GB_SHIFT		30
 #define FOUR_KB_SHIFT		12
 
+#define ONE_GB_INDEX(x)		((x) >> ONE_GB_SHIFT)
+#define TWO_MB_INDEX(x)		((x) >> TWO_MB_SHIFT)
+#define FOUR_KB_INDEX(x)	((x) >> FOUR_KB_SHIFT)
+
 #define INVALID_DESC		0x0
 #define BLOCK_DESC		0x1 /* Table levels 0-2 */
 #define TABLE_DESC		0x3 /* Table levels 0-2 */
 #define PAGE_DESC		0x3 /* Table level 3 */
 
+#define FIRST_LEVEL_DESC_N	ONE_GB_SHIFT
+#define SECOND_LEVEL_DESC_N	TWO_MB_SHIFT
+#define THIRD_LEVEL_DESC_N	FOUR_KB_SHIFT
+
 #define XN			(1ull << 2)
 #define PXN			(1ull << 1)
 #define CONT_HINT		(1ull << 0)
diff --git a/include/plat/arm/common/plat_arm.h b/include/plat/arm/common/plat_arm.h
index 25aab24..d2e8729 100644
--- a/include/plat/arm/common/plat_arm.h
+++ b/include/plat/arm/common/plat_arm.h
@@ -37,6 +37,12 @@
 #include <utils.h>
 #include <xlat_tables.h>
 
+/*******************************************************************************
+ * Forward declarations
+ ******************************************************************************/
+struct bl31_params;
+struct meminfo;
+
 #define ARM_CASSERT_MMAP						\
 	CASSERT((ARRAY_SIZE(plat_arm_mmap) + ARM_BL_REGIONS)		\
 		<= MAX_MMAP_REGIONS,					\
@@ -145,7 +151,7 @@
 void arm_bl1_plat_arch_setup(void);
 
 /* BL2 utility functions */
-void arm_bl2_early_platform_setup(meminfo_t *mem_layout);
+void arm_bl2_early_platform_setup(struct meminfo *mem_layout);
 void arm_bl2_platform_setup(void);
 void arm_bl2_plat_arch_setup(void);
 uint32_t arm_get_spsr_for_bl32_entry(void);
@@ -158,7 +164,7 @@
 void arm_bl2u_plat_arch_setup(void);
 
 /* BL31 utility functions */
-void arm_bl31_early_platform_setup(bl31_params_t *from_bl2,
+void arm_bl31_early_platform_setup(struct bl31_params *from_bl2,
 				void *plat_params_from_bl2);
 void arm_bl31_platform_setup(void);
 void arm_bl31_plat_runtime_setup(void);
diff --git a/include/plat/arm/css/common/css_def.h b/include/plat/arm/css/common/css_def.h
index 636daf2..173de1b 100644
--- a/include/plat/arm/css/common/css_def.h
+++ b/include/plat/arm/css/common/css_def.h
@@ -148,5 +148,15 @@
 /* Trusted mailbox base address common to all CSS */
 #define PLAT_ARM_TRUSTED_MAILBOX_BASE	ARM_TRUSTED_SRAM_BASE
 
+/*
+ * Parsing of CPU and Cluster states, as returned by 'Get CSS Power State' SCP
+ * command
+ */
+#define CSS_CLUSTER_PWR_STATE_ON	0
+#define CSS_CLUSTER_PWR_STATE_OFF	3
+
+#define CSS_CPU_PWR_STATE_ON		1
+#define CSS_CPU_PWR_STATE_OFF		0
+#define CSS_CPU_PWR_STATE(state, n)	(((state) >> (n)) & 1)
 
 #endif /* __CSS_DEF_H__ */
diff --git a/include/plat/arm/css/common/css_pm.h b/include/plat/arm/css/common/css_pm.h
index ea6a5d2..4a6ca81 100644
--- a/include/plat/arm/css/common/css_pm.h
+++ b/include/plat/arm/css/common/css_pm.h
@@ -45,5 +45,6 @@
 void __dead2 css_system_reset(void);
 void css_cpu_standby(plat_local_state_t cpu_state);
 void css_get_sys_suspend_power_state(psci_power_state_t *req_state);
+int css_node_hw_state(u_register_t mpidr, unsigned int power_level);
 
 #endif /* __CSS_PM_H__ */
diff --git a/lib/psci/psci_main.c b/lib/psci/psci_main.c
index 3ad3dd4..23bd106 100644
--- a/lib/psci/psci_main.c
+++ b/lib/psci/psci_main.c
@@ -295,6 +295,31 @@
 	return resident_cpu_mpidr;
 }
 
+int psci_node_hw_state(u_register_t target_cpu,
+		       unsigned int power_level)
+{
+	int rc;
+
+	/* Validate target_cpu */
+	rc = psci_validate_mpidr(target_cpu);
+	if (rc != PSCI_E_SUCCESS)
+		return PSCI_E_INVALID_PARAMS;
+
+	/* Validate power_level against PLAT_MAX_PWR_LVL */
+	if (power_level > PLAT_MAX_PWR_LVL)
+		return PSCI_E_INVALID_PARAMS;
+
+	/*
+	 * Dispatch this call to platform to query power controller, and pass on
+	 * to the caller what it returns
+	 */
+	assert(psci_plat_pm_ops->get_node_hw_state);
+	rc = psci_plat_pm_ops->get_node_hw_state(target_cpu, power_level);
+	assert((rc >= HW_ON && rc <= HW_STANDBY) || rc == PSCI_E_NOT_SUPPORTED
+			|| rc == PSCI_E_INVALID_PARAMS);
+	return rc;
+}
+
 int psci_features(unsigned int psci_fid)
 {
 	unsigned int local_caps = psci_caps;
@@ -378,6 +403,9 @@
 		case PSCI_MIG_INFO_UP_CPU_AARCH32:
 			return psci_migrate_info_up_cpu();
 
+		case PSCI_NODE_HW_STATE_AARCH32:
+			return psci_node_hw_state(x1, x2);
+
 		case PSCI_SYSTEM_SUSPEND_AARCH32:
 			return psci_system_suspend(x1, x2);
 
@@ -422,6 +450,9 @@
 		case PSCI_MIG_INFO_UP_CPU_AARCH64:
 			return psci_migrate_info_up_cpu();
 
+		case PSCI_NODE_HW_STATE_AARCH64:
+			return psci_node_hw_state(x1, x2);
+
 		case PSCI_SYSTEM_SUSPEND_AARCH64:
 			return psci_system_suspend(x1, x2);
 
diff --git a/lib/psci/psci_private.h b/lib/psci/psci_private.h
index b795c8e..781b3b5 100644
--- a/lib/psci/psci_private.h
+++ b/lib/psci/psci_private.h
@@ -68,6 +68,7 @@
 			define_psci_cap(PSCI_AFFINITY_INFO_AARCH64) |	\
 			define_psci_cap(PSCI_MIG_AARCH64) |		\
 			define_psci_cap(PSCI_MIG_INFO_UP_CPU_AARCH64) |	\
+			define_psci_cap(PSCI_NODE_HW_STATE_AARCH64) |	\
 			define_psci_cap(PSCI_SYSTEM_SUSPEND_AARCH64) |	\
 			define_psci_cap(PSCI_STAT_RESIDENCY_AARCH64) |	\
 			define_psci_cap(PSCI_STAT_COUNT_AARCH64))
diff --git a/lib/psci/psci_setup.c b/lib/psci/psci_setup.c
index 20d0635..263ab68 100644
--- a/lib/psci/psci_setup.c
+++ b/lib/psci/psci_setup.c
@@ -238,6 +238,13 @@
 	plat_setup_psci_ops(mailbox_ep, &psci_plat_pm_ops);
 	assert(psci_plat_pm_ops);
 
+	/*
+	 * Flush `psci_plat_pm_ops` as it will be accessed by secondary CPUs
+	 * during warm boot before data cache is enabled.
+	 */
+	flush_dcache_range((uintptr_t)&psci_plat_pm_ops,
+					sizeof(psci_plat_pm_ops));
+
 	/* Initialize the psci capability */
 	psci_caps = PSCI_GENERIC_CAP;
 
@@ -256,6 +263,8 @@
 		psci_caps |=  define_psci_cap(PSCI_SYSTEM_OFF);
 	if (psci_plat_pm_ops->system_reset)
 		psci_caps |=  define_psci_cap(PSCI_SYSTEM_RESET);
+	if (psci_plat_pm_ops->get_node_hw_state)
+		psci_caps |= define_psci_cap(PSCI_NODE_HW_STATE_AARCH64);
 
 #if ENABLE_PSCI_STAT
 	psci_caps |=  define_psci_cap(PSCI_STAT_RESIDENCY_AARCH64);
diff --git a/plat/arm/board/fvp/fvp_pm.c b/plat/arm/board/fvp/fvp_pm.c
index 3976ef2..66c0c3d 100644
--- a/plat/arm/board/fvp/fvp_pm.c
+++ b/plat/arm/board/fvp/fvp_pm.c
@@ -287,6 +287,42 @@
 	panic();
 }
 
+static int fvp_node_hw_state(u_register_t target_cpu,
+			     unsigned int power_level)
+{
+	unsigned int psysr;
+	int ret;
+
+	/*
+	 * The format of 'power_level' is implementation-defined, but 0 must
+	 * mean a CPU. We also allow 1 to denote the cluster
+	 */
+	if (power_level != ARM_PWR_LVL0 && power_level != ARM_PWR_LVL1)
+		return PSCI_E_INVALID_PARAMS;
+
+	/*
+	 * Read the status of the given MPDIR from FVP power controller. The
+	 * power controller only gives us on/off status, so map that to expected
+	 * return values of the PSCI call
+	 */
+	psysr = fvp_pwrc_read_psysr(target_cpu);
+	if (psysr == PSYSR_INVALID)
+		return PSCI_E_INVALID_PARAMS;
+
+	switch (power_level) {
+	case ARM_PWR_LVL0:
+		ret = (psysr & PSYSR_AFF_L0) ? HW_ON : HW_OFF;
+		break;
+	case ARM_PWR_LVL1:
+		ret = (psysr & PSYSR_AFF_L1) ? HW_ON : HW_OFF;
+		break;
+	default:
+		assert(0);
+	}
+
+	return ret;
+}
+
 /*******************************************************************************
  * Export the platform handlers via plat_arm_psci_pm_ops. The ARM Standard
  * platform layer will take care of registering the handlers with PSCI.
@@ -301,5 +337,6 @@
 	.system_off = fvp_system_off,
 	.system_reset = fvp_system_reset,
 	.validate_power_state = arm_validate_power_state,
-	.validate_ns_entrypoint = arm_validate_ns_entrypoint
+	.validate_ns_entrypoint = arm_validate_ns_entrypoint,
+	.get_node_hw_state = fvp_node_hw_state
 };
diff --git a/plat/arm/board/juno/juno_pm.c b/plat/arm/board/juno/juno_pm.c
index cbf994a..c355d94 100644
--- a/plat/arm/board/juno/juno_pm.c
+++ b/plat/arm/board/juno/juno_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:
@@ -88,5 +88,6 @@
 	.validate_power_state		= juno_validate_power_state,
 	.validate_ns_entrypoint		= arm_validate_ns_entrypoint,
 	.get_sys_suspend_power_state	= css_get_sys_suspend_power_state,
-	.translate_power_state_by_mpidr = juno_translate_power_state_by_mpidr
+	.translate_power_state_by_mpidr = juno_translate_power_state_by_mpidr,
+	.get_node_hw_state		= css_node_hw_state
 };
diff --git a/plat/arm/css/common/css_pm.c b/plat/arm/css/common/css_pm.c
index 801d937..7607f61 100644
--- a/plat/arm/css/common/css_pm.c
+++ b/plat/arm/css/common/css_pm.c
@@ -299,6 +299,43 @@
 }
 
 /*******************************************************************************
+ * Handler to query CPU/cluster power states from SCP
+ ******************************************************************************/
+int css_node_hw_state(u_register_t mpidr, unsigned int power_level)
+{
+	int rc, element;
+	unsigned int cpu_state, cluster_state;
+
+	/*
+	 * The format of 'power_level' is implementation-defined, but 0 must
+	 * mean a CPU. We also allow 1 to denote the cluster
+	 */
+	if (power_level != ARM_PWR_LVL0 && power_level != ARM_PWR_LVL1)
+		return PSCI_E_INVALID_PARAMS;
+
+	/* Query SCP */
+	rc = scpi_get_css_power_state(mpidr, &cpu_state, &cluster_state);
+	if (rc != 0)
+		return PSCI_E_INVALID_PARAMS;
+
+	/* Map power states of CPU and cluster to expected PSCI return codes */
+	if (power_level == ARM_PWR_LVL0) {
+		/*
+		 * The CPU state returned by SCP is an 8-bit bit mask
+		 * corresponding to each CPU in the cluster
+		 */
+		element = mpidr & MPIDR_AFFLVL_MASK;
+		return CSS_CPU_PWR_STATE(cpu_state, element) ==
+			CSS_CPU_PWR_STATE_ON ? HW_ON : HW_OFF;
+	} else {
+		assert(cluster_state == CSS_CLUSTER_PWR_STATE_ON ||
+				cluster_state == CSS_CLUSTER_PWR_STATE_OFF);
+		return cluster_state == CSS_CLUSTER_PWR_STATE_ON ? HW_ON :
+			HW_OFF;
+	}
+}
+
+/*******************************************************************************
  * Export the platform handlers via plat_arm_psci_pm_ops. The ARM Standard
  * platform will take care of registering the handlers with PSCI.
  ******************************************************************************/
@@ -312,5 +349,6 @@
 	.system_off		= css_system_off,
 	.system_reset		= css_system_reset,
 	.validate_power_state	= arm_validate_power_state,
-	.validate_ns_entrypoint = arm_validate_ns_entrypoint
+	.validate_ns_entrypoint = arm_validate_ns_entrypoint,
+	.get_node_hw_state	= css_node_hw_state
 };
diff --git a/plat/arm/css/common/css_scpi.c b/plat/arm/css/common/css_scpi.c
index 02d573c..90a8939 100644
--- a/plat/arm/css/common/css_scpi.c
+++ b/plat/arm/css/common/css_scpi.c
@@ -41,11 +41,18 @@
 #define SCPI_SHARED_MEM_AP_TO_SCP	(PLAT_CSS_SCP_COM_SHARED_MEM_BASE \
 								 + 0x100)
 
+/* Header and payload addresses for commands from AP to SCP */
 #define SCPI_CMD_HEADER_AP_TO_SCP		\
 	((scpi_cmd_t *) SCPI_SHARED_MEM_AP_TO_SCP)
 #define SCPI_CMD_PAYLOAD_AP_TO_SCP		\
 	((void *) (SCPI_SHARED_MEM_AP_TO_SCP + sizeof(scpi_cmd_t)))
 
+/* Header and payload addresses for responses from SCP to AP */
+#define SCPI_RES_HEADER_SCP_TO_AP \
+	((scpi_cmd_t *) SCPI_SHARED_MEM_SCP_TO_AP)
+#define SCPI_RES_PAYLOAD_SCP_TO_AP \
+	((void *) (SCPI_SHARED_MEM_SCP_TO_AP + sizeof(scpi_cmd_t)))
+
 /* ID of the MHU slot used for the SCPI protocol */
 #define SCPI_MHU_SLOT_ID		0
 
@@ -160,7 +167,69 @@
 	 * from the sender, which could interfere with its power state request.
 	 */
 
+	scpi_secure_message_end();
+}
+
+/*
+ * Query and obtain CSS power state from SCP.
+ *
+ * In response to the query, SCP returns power states of all CPUs in all
+ * clusters of the system. The returned response is then filtered based on the
+ * supplied MPIDR. Power states of requested cluster and CPUs within are updated
+ * via. supplied non-NULL pointer arguments.
+ *
+ * Returns 0 on success, or -1 on errors.
+ */
+int scpi_get_css_power_state(unsigned int mpidr, unsigned int *cpu_state_p,
+		unsigned int *cluster_state_p)
+{
+	scpi_cmd_t *cmd;
+	scpi_cmd_t response;
+	int power_state, cpu, cluster, rc = -1;
+
+	/*
+	 * Extract CPU and cluster membership of the given MPIDR. SCPI caters
+	 * for only up to 0xf clusters, and 8 CPUs per cluster
+	 */
+	cpu = mpidr & MPIDR_AFFLVL_MASK;
+	cluster = (mpidr >> MPIDR_AFF1_SHIFT) & MPIDR_AFFLVL_MASK;
+	if (cpu >= 8 || cluster >= 0xf)
+		return -1;
+
+	scpi_secure_message_start();
+
+	/* Populate request headers */
+	cmd = memset(SCPI_CMD_HEADER_AP_TO_SCP, 0, sizeof(*cmd));
+	cmd->id = SCPI_CMD_GET_CSS_POWER_STATE;
+
+	/*
+	 * Send message and wait for SCP's response
+	 */
+	scpi_secure_message_send(0);
+	scpi_secure_message_receive(&response);
+
+	if (response.status != SCP_OK)
+		goto exit;
+
+	/* Validate SCP response */
+	if (!CHECK_RESPONSE(response, cluster))
+		goto exit;
+
+	/* Extract power states for required cluster */
+	power_state = *(((uint16_t *) SCPI_RES_PAYLOAD_SCP_TO_AP) + cluster);
+	if (CLUSTER_ID(power_state) != cluster)
+		goto exit;
+
+	/* Update power state via. pointers */
+	if (cluster_state_p)
+		*cluster_state_p = CLUSTER_POWER_STATE(power_state);
+	if (cpu_state_p)
+		*cpu_state_p = CPU_POWER_STATE(power_state);
+	rc = 0;
+
+exit:
 	scpi_secure_message_end();
+	return rc;
 }
 
 uint32_t scpi_sys_power_state(scpi_system_state_t system_state)
diff --git a/plat/arm/css/common/css_scpi.h b/plat/arm/css/common/css_scpi.h
index 4a601f3..1fb55e4 100644
--- a/plat/arm/css/common/css_scpi.h
+++ b/plat/arm/css/common/css_scpi.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2014-2015, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2014-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:
@@ -81,9 +81,34 @@
 typedef enum {
 	SCPI_CMD_SCP_READY = 0x01,
 	SCPI_CMD_SET_CSS_POWER_STATE = 0x03,
+	SCPI_CMD_GET_CSS_POWER_STATE = 0x04,
 	SCPI_CMD_SYS_POWER_STATE = 0x05
 } scpi_command_t;
 
+/*
+ * Macros to parse SCP response to GET_CSS_POWER_STATE command
+ *
+ *   [3:0] : cluster ID
+ *   [7:4] : cluster state: 0 = on; 3 = off; rest are reserved
+ *   [15:8]: on/off state for individual CPUs in the cluster
+ *
+ * Payload is in little-endian
+ */
+#define CLUSTER_ID(_resp)		((_resp) & 0xf)
+#define CLUSTER_POWER_STATE(_resp)	(((_resp) >> 4) & 0xf)
+
+/* Result is a bit mask of CPU on/off states in the cluster */
+#define CPU_POWER_STATE(_resp)		(((_resp) >> 8) & 0xff)
+
+/*
+ * For GET_CSS_POWER_STATE, SCP returns the power states of every cluster. The
+ * size of response depends on the number of clusters in the system. The
+ * SCP-to-AP payload contains 2 bytes per cluster. Make sure the response is
+ * large enough to contain power states of a given cluster
+ */
+#define CHECK_RESPONSE(_resp, _clus) \
+	(_resp.size >= (((_clus) + 1) * 2))
+
 typedef enum {
 	scpi_power_on = 0,
 	scpi_power_retention = 1,
@@ -101,6 +126,8 @@
 					scpi_power_state_t cpu_state,
 					scpi_power_state_t cluster_state,
 					scpi_power_state_t css_state);
+int scpi_get_css_power_state(unsigned int mpidr, unsigned int *cpu_state_p,
+		unsigned int *cluster_state_p);
 uint32_t scpi_sys_power_state(scpi_system_state_t system_state);
 
 
diff --git a/plat/rockchip/common/include/plat_params.h b/plat/rockchip/common/include/plat_params.h
index cad4535..8c64796 100644
--- a/plat/rockchip/common/include/plat_params.h
+++ b/plat/rockchip/common/include/plat_params.h
@@ -63,11 +63,31 @@
  * alignment fault will occur during accessing its data member.
  */
 
+#define BL31_GPIO_DIR_OUT		0
+#define BL31_GPIO_DIR_IN		1
+
+#define BL31_GPIO_LEVEL_LOW		0
+#define BL31_GPIO_LEVEL_HIGH		1
+
+#define BL31_GPIO_PULL_NONE		0
+#define BL31_GPIO_PULL_UP		1
+#define BL31_GPIO_PULL_DOWN		2
+
 /* param type */
 enum {
 	PARAM_NONE = 0,
 	PARAM_RESET,
 	PARAM_POWEROFF,
+	PARAM_SUSPEND_GPIO,
+	PARAM_SUSPEND_APIO,
+};
+
+struct apio_info {
+	uint8_t apio1 : 1;
+	uint8_t apio2 : 1;
+	uint8_t apio3 : 1;
+	uint8_t apio4 : 1;
+	uint8_t apio5 : 1;
 };
 
 struct gpio_info {
@@ -88,4 +108,9 @@
 	struct gpio_info gpio;
 };
 
+struct bl31_apio_param {
+	struct bl31_plat_param h;
+	struct apio_info apio;
+};
+
 #endif /* __PLAT_PARAMS_H__ */
diff --git a/plat/rockchip/common/include/plat_private.h b/plat/rockchip/common/include/plat_private.h
index b9b634e..ad01266 100644
--- a/plat/rockchip/common/include/plat_private.h
+++ b/plat/rockchip/common/include/plat_private.h
@@ -121,8 +121,10 @@
 
 void platform_cpu_warmboot(void);
 
-void *plat_get_rockchip_gpio_reset(void);
-void *plat_get_rockchip_gpio_poweroff(void);
+struct gpio_info *plat_get_rockchip_gpio_reset(void);
+struct gpio_info *plat_get_rockchip_gpio_poweroff(void);
+struct gpio_info *plat_get_rockchip_suspend_gpio(uint32_t *count);
+struct apio_info *plat_get_rockchip_suspend_apio(void);
 void plat_rockchip_gpio_init(void);
 
 extern const unsigned char rockchip_power_domain_tree_desc[];
diff --git a/plat/rockchip/common/params_setup.c b/plat/rockchip/common/params_setup.c
index 2a49556..646c1e1 100644
--- a/plat/rockchip/common/params_setup.c
+++ b/plat/rockchip/common/params_setup.c
@@ -40,25 +40,39 @@
 #include <plat_private.h>
 #include <string.h>
 
-static struct bl31_plat_param *bl31_params_head;
-static struct bl31_gpio_param param_reset;
-static struct bl31_gpio_param param_poweroff;
+static struct gpio_info param_reset;
+static struct gpio_info param_poweroff;
+static struct bl31_apio_param param_apio;
 static struct gpio_info *rst_gpio;
 static struct gpio_info *poweroff_gpio;
+static struct gpio_info suspend_gpio[10];
+uint32_t suspend_gpio_cnt;
+static struct apio_info *suspend_apio;
 
-void *plat_get_rockchip_gpio_reset(void)
+struct gpio_info *plat_get_rockchip_gpio_reset(void)
 {
 	return rst_gpio;
 }
 
-void *plat_get_rockchip_gpio_poweroff(void)
+struct gpio_info *plat_get_rockchip_gpio_poweroff(void)
 {
 	return poweroff_gpio;
 }
 
+struct gpio_info *plat_get_rockchip_suspend_gpio(uint32_t *count)
+{
+	*count = suspend_gpio_cnt;
+
+	return &suspend_gpio[0];
+}
+
+struct apio_info *plat_get_rockchip_suspend_apio(void)
+{
+	return suspend_apio;
+}
+
 void params_early_setup(void *plat_param_from_bl2)
 {
-	struct bl31_plat_param *param;
 	struct bl31_plat_param *bl2_param;
 	struct bl31_gpio_param *gpio_param;
 
@@ -67,25 +81,38 @@
 	while (bl2_param) {
 		switch (bl2_param->type) {
 		case PARAM_RESET:
-			param = (struct bl31_plat_param *)&param_reset;
-			memcpy((void *)param, (void *)bl2_param,
-				sizeof(struct bl31_gpio_param));
-			gpio_param = (struct bl31_gpio_param *)param;
-			rst_gpio = &gpio_param->gpio;
+			gpio_param = (struct bl31_gpio_param *)bl2_param;
+			memcpy(&param_reset, &gpio_param->gpio,
+			       sizeof(struct gpio_info));
+			rst_gpio = &param_reset;
 			break;
 		case PARAM_POWEROFF:
-			param = (struct bl31_plat_param *)&param_poweroff;
-			memcpy((void *)param, (void *)bl2_param,
-				sizeof(struct bl31_gpio_param));
-			gpio_param = (struct bl31_gpio_param *)param;
-			poweroff_gpio = &gpio_param->gpio;
+			gpio_param = (struct bl31_gpio_param *)bl2_param;
+			memcpy(&param_poweroff, &gpio_param->gpio,
+				sizeof(struct gpio_info));
+			poweroff_gpio = &param_poweroff;
+			break;
+		case PARAM_SUSPEND_GPIO:
+			if (suspend_gpio_cnt >= ARRAY_SIZE(suspend_gpio)) {
+				ERROR("exceed support suspend gpio number\n");
+				break;
+			}
+			gpio_param = (struct bl31_gpio_param *)bl2_param;
+			memcpy(&suspend_gpio[suspend_gpio_cnt],
+			       &gpio_param->gpio,
+			       sizeof(struct gpio_info));
+			suspend_gpio_cnt++;
+			break;
+		case PARAM_SUSPEND_APIO:
+			memcpy(&param_apio, bl2_param,
+			       sizeof(struct bl31_apio_param));
+			suspend_apio = &param_apio.apio;
 			break;
 		default:
-			NOTICE("not expected type found\n");
-			return; /* don't continue if unexpected type found */
+			ERROR("not expected type found %ld\n",
+			      bl2_param->type);
+			break;
 		}
-		param->next = bl31_params_head;
-		bl31_params_head = param;
 		bl2_param = bl2_param->next;
 	}
 }
diff --git a/plat/rockchip/common/plat_pm.c b/plat/rockchip/common/plat_pm.c
old mode 100755
new mode 100644
index 3c2c79a..d28100d
--- a/plat/rockchip/common/plat_pm.c
+++ b/plat/rockchip/common/plat_pm.c
@@ -283,9 +283,10 @@
 		rockchip_ops->cores_pwr_dm_resume();
 	/*
 	 * Program the gic per-cpu distributor or re-distributor interface.
-	 * For sys power domain operation, resuming of the gic needs to operate in
-	 * rockchip_ops->sys_pwr_dm_resume, according to the sys power mode implements.
-	*/
+	 * For sys power domain operation, resuming of the gic needs to operate
+	 * in rockchip_ops->sys_pwr_dm_resume, according to the sys power mode
+	 * implements.
+	 */
 	plat_rockchip_gic_cpuif_enable();
 
 comm_finish:
diff --git a/plat/rockchip/rk3399/drivers/dram/dram.c b/plat/rockchip/rk3399/drivers/dram/dram.c
index ddae84d..94aa076 100644
--- a/plat/rockchip/rk3399/drivers/dram/dram.c
+++ b/plat/rockchip/rk3399/drivers/dram/dram.c
@@ -2147,7 +2147,7 @@
 {
 	int pll_cnt, i;
 
-	pll_cnt = sizeof(dpll_rates_table) / sizeof(struct pll_div);
+	pll_cnt = ARRAY_SIZE(dpll_rates_table);
 
 	/* Assumming rate_table is in descending order */
 	for (i = 0; i < pll_cnt; i++) {
@@ -2155,6 +2155,10 @@
 			break;
 	}
 
+	/* if mhz lower than lowest frequency in table, use lowest frequency */
+	if (i == pll_cnt)
+		i = pll_cnt - 1;
+
 	return i;
 }
 
@@ -2174,7 +2178,7 @@
 	return (24 * fbdiv) / refdiv / postdiv1 / postdiv2;
 }
 
-uint64_t ddr_get_rate(void)
+uint32_t ddr_get_rate(void)
 {
 	uint32_t refdiv, postdiv1, fbdiv, postdiv2;
 
@@ -2464,7 +2468,6 @@
 	 * target freq.
 	 */
 	dram_get_parameter(&rk3399_dram_status.timing_config, &dram_timing);
-
 	gen_rk3399_ctl_params(&rk3399_dram_status.timing_config,
 			      &dram_timing, index);
 	gen_rk3399_pi_params(&rk3399_dram_status.timing_config,
@@ -2494,7 +2497,7 @@
 		tf_printf("%u\n", p[i]);
 }
 
-uint64_t ddr_set_rate(uint64_t hz)
+uint32_t ddr_set_rate(uint32_t hz)
 {
 	uint32_t low_power, index;
 	uint32_t mhz = hz / (1000 * 1000);
@@ -2503,13 +2506,13 @@
 	    rk3399_dram_status.index_freq[rk3399_dram_status.current_index])
 		goto out;
 
+	index = to_get_clk_index(mhz);
+	mhz = dpll_rates_table[index].mhz;
+
 	low_power = exit_low_power();
 	index = prepare_ddr_timing(mhz);
-	if (index > 1) {
-		/* set timing error, quit */
-		mhz = 0;
+	if (index > 1)
 		goto out;
-	}
 
 	dcf_start(mhz, index);
 	wait_dcf_done();
@@ -2526,7 +2529,7 @@
 	return mhz;
 }
 
-uint64_t ddr_round_rate(uint64_t hz)
+uint32_t ddr_round_rate(uint32_t hz)
 {
 	int index;
 	uint32_t mhz = hz / (1000 * 1000);
@@ -2536,7 +2539,7 @@
 	return dpll_rates_table[index].mhz * 1000 * 1000;
 }
 
-uint64_t dts_timing_receive(uint64_t timing, uint64_t index)
+uint32_t dts_timing_receive(uint32_t timing, uint32_t index)
 {
 	uint32_t *p = (uint32_t *) &dts_parameter;
 	static uint32_t receive_nums;
diff --git a/plat/rockchip/rk3399/drivers/dram/dram.h b/plat/rockchip/rk3399/drivers/dram/dram.h
index 62c5170..4f99070 100644
--- a/plat/rockchip/rk3399/drivers/dram/dram.h
+++ b/plat/rockchip/rk3399/drivers/dram/dram.h
@@ -321,9 +321,9 @@
 #define DDR_RESTORE_SP(save_sp)   ddr_save_sp(save_sp)
 
 void ddr_init(void);
-uint64_t ddr_set_rate(uint64_t hz);
-uint64_t ddr_round_rate(uint64_t hz);
-uint64_t ddr_get_rate(void);
+uint32_t ddr_set_rate(uint32_t hz);
+uint32_t ddr_round_rate(uint32_t hz);
+uint32_t ddr_get_rate(void);
 void clr_dcf_irq(void);
-uint64_t dts_timing_receive(uint64_t timing, uint64_t index);
+uint32_t dts_timing_receive(uint32_t timing, uint32_t index);
 #endif
diff --git a/plat/rockchip/rk3399/drivers/gpio/rk3399_gpio.c b/plat/rockchip/rk3399/drivers/gpio/rk3399_gpio.c
index eca9fbc..4995d56 100644
--- a/plat/rockchip/rk3399/drivers/gpio/rk3399_gpio.c
+++ b/plat/rockchip/rk3399/drivers/gpio/rk3399_gpio.c
@@ -52,48 +52,105 @@
 
 #define PMU_GPIO_PORT0	0
 #define PMU_GPIO_PORT1	1
+#define GPIO_PORT2	2
+#define GPIO_PORT3	3
+#define GPIO_PORT4	4
 
 #define PMU_GRF_GPIO0A_P	0x40
 #define GRF_GPIO2A_P		0xe040
 #define GPIO_P_MASK		0x03
 
-/*
- * gpio clock disabled when not operate
- * so need to enable gpio clock before operate gpio
- * after setting, need to disable gpio clock
- * gate 1: disable clock; 0: enable clock
- */
-static void gpio_clk(int gpio, uint32_t gate)
+#define GET_GPIO_PORT(pin)	(pin / 32)
+#define GET_GPIO_NUM(pin)	(pin % 32)
+#define GET_GPIO_BANK(pin)	((pin % 32) / 8)
+#define GET_GPIO_ID(pin)	((pin % 32) % 8)
+
+/* returns old clock state, enables clock, in order to do GPIO access */
+static int gpio_get_clock(uint32_t gpio_number)
 {
-	uint32_t port = gpio / 32;
+	uint32_t port = GET_GPIO_PORT(gpio_number);
+	uint32_t clock_state = 0;
 
 	assert(port < 5);
 
 	switch (port) {
-	case 0:
+	case PMU_GPIO_PORT0:
+		clock_state = (mmio_read_32(PMUCRU_BASE +
+					    CRU_PMU_CLKGATE_CON(1)) >>
+					    PCLK_GPIO0_GATE_SHIFT) & 0x01;
 		mmio_write_32(PMUCRU_BASE + CRU_PMU_CLKGATE_CON(1),
-			      BITS_WITH_WMASK(gate, CLK_GATE_MASK,
+			      BITS_WITH_WMASK(0, CLK_GATE_MASK,
 					      PCLK_GPIO0_GATE_SHIFT));
 		break;
-	case 1:
+	case PMU_GPIO_PORT1:
+		clock_state = (mmio_read_32(PMUCRU_BASE +
+					    CRU_PMU_CLKGATE_CON(1)) >>
+					    PCLK_GPIO1_GATE_SHIFT) & 0x01;
 		mmio_write_32(PMUCRU_BASE + CRU_PMU_CLKGATE_CON(1),
-			      BITS_WITH_WMASK(gate, CLK_GATE_MASK,
+			      BITS_WITH_WMASK(0, CLK_GATE_MASK,
 					      PCLK_GPIO1_GATE_SHIFT));
 		break;
-	case 2:
+	case GPIO_PORT2:
+		clock_state = (mmio_read_32(CRU_BASE +
+					    CRU_CLKGATE_CON(31)) >>
+					    PCLK_GPIO2_GATE_SHIFT) & 0x01;
 		mmio_write_32(CRU_BASE + CRU_CLKGATE_CON(31),
-			      BITS_WITH_WMASK(gate, CLK_GATE_MASK,
+			      BITS_WITH_WMASK(0, CLK_GATE_MASK,
 					      PCLK_GPIO2_GATE_SHIFT));
 		break;
-	case 3:
+	case GPIO_PORT3:
+		clock_state = (mmio_read_32(CRU_BASE +
+					    CRU_CLKGATE_CON(31)) >>
+					    PCLK_GPIO3_GATE_SHIFT) & 0x01;
 		mmio_write_32(CRU_BASE + CRU_CLKGATE_CON(31),
-			      BITS_WITH_WMASK(gate, CLK_GATE_MASK,
+			      BITS_WITH_WMASK(0, CLK_GATE_MASK,
 					      PCLK_GPIO3_GATE_SHIFT));
+		break;
+	case GPIO_PORT4:
+		clock_state = (mmio_read_32(CRU_BASE +
+					    CRU_CLKGATE_CON(31)) >>
+					    PCLK_GPIO4_GATE_SHIFT) & 0x01;
+		mmio_write_32(CRU_BASE + CRU_CLKGATE_CON(31),
+			      BITS_WITH_WMASK(0, CLK_GATE_MASK,
+					      PCLK_GPIO4_GATE_SHIFT));
+		break;
+	default:
+		break;
+	}
+
+	return clock_state;
+}
+
+/* restores old state of gpio clock */
+void gpio_put_clock(uint32_t gpio_number, uint32_t clock_state)
+{
+	uint32_t port = GET_GPIO_PORT(gpio_number);
 
+	switch (port) {
+	case PMU_GPIO_PORT0:
+		mmio_write_32(PMUCRU_BASE + CRU_PMU_CLKGATE_CON(1),
+			      BITS_WITH_WMASK(clock_state, CLK_GATE_MASK,
+					      PCLK_GPIO0_GATE_SHIFT));
+		break;
+	case PMU_GPIO_PORT1:
+		mmio_write_32(PMUCRU_BASE + CRU_PMU_CLKGATE_CON(1),
+			      BITS_WITH_WMASK(clock_state, CLK_GATE_MASK,
+					      PCLK_GPIO1_GATE_SHIFT));
 		break;
-	case 4:
+	case GPIO_PORT2:
 		mmio_write_32(CRU_BASE + CRU_CLKGATE_CON(31),
-			      BITS_WITH_WMASK(gate, CLK_GATE_MASK,
+			      BITS_WITH_WMASK(clock_state, CLK_GATE_MASK,
+					      PCLK_GPIO2_GATE_SHIFT));
+		break;
+	case GPIO_PORT3:
+		mmio_write_32(CRU_BASE + CRU_CLKGATE_CON(31),
+			      BITS_WITH_WMASK(clock_state, CLK_GATE_MASK,
+					      PCLK_GPIO3_GATE_SHIFT));
+
+		break;
+	case GPIO_PORT4:
+		mmio_write_32(CRU_BASE + CRU_CLKGATE_CON(31),
+			      BITS_WITH_WMASK(clock_state, CLK_GATE_MASK,
 					      PCLK_GPIO4_GATE_SHIFT));
 		break;
 	default:
@@ -101,16 +158,58 @@
 	}
 }
 
+static int get_pull(int gpio)
+{
+	uint32_t port = GET_GPIO_PORT(gpio);
+	uint32_t bank = GET_GPIO_BANK(gpio);
+	uint32_t id = GET_GPIO_ID(gpio);
+	uint32_t val, clock_state;
+
+	assert((port < 5) && (bank < 4));
+
+	clock_state = gpio_get_clock(gpio);
+
+	if (port == PMU_GPIO_PORT0 || port == PMU_GPIO_PORT1) {
+		val = mmio_read_32(PMUGRF_BASE + PMU_GRF_GPIO0A_P +
+				   port * 16 + bank * 4);
+		val = (val >> (id * 2)) & GPIO_P_MASK;
+	} else {
+		val = mmio_read_32(GRF_BASE + GRF_GPIO2A_P +
+				   (port - 2) * 16 + bank * 4);
+		val = (val >> (id * 2)) & GPIO_P_MASK;
+	}
+	gpio_put_clock(gpio, clock_state);
+
+	/*
+	 * in gpio0a, gpio0b, gpio2c, gpio2d,
+	 * 00: Z
+	 * 01: pull down
+	 * 10: Z
+	 * 11: pull up
+	 * different with other gpio, so need to correct it
+	 */
+	if (((port == 0) && (bank < 2)) || ((port == 2) && (bank > 1))) {
+		if (val == 3)
+			val = GPIO_PULL_UP;
+		else if (val == 1)
+			val = GPIO_PULL_DOWN;
+		else
+			val = 0;
+	}
+
+	return val;
+}
+
 static void set_pull(int gpio, int pull)
 {
-	uint32_t port = gpio / 32;
-	uint32_t num = gpio % 32;
-	uint32_t bank = num / 8;
-	uint32_t id = num % 8;
+	uint32_t port = GET_GPIO_PORT(gpio);
+	uint32_t bank = GET_GPIO_BANK(gpio);
+	uint32_t id = GET_GPIO_ID(gpio);
+	uint32_t clock_state;
 
-	assert((port < 5) && (num < 32));
+	assert((port < 5) && (bank < 4));
 
-	gpio_clk(gpio, 0);
+	clock_state = gpio_get_clock(gpio);
 
 	/*
 	 * in gpio0a, gpio0b, gpio2c, gpio2d,
@@ -120,7 +219,7 @@
 	 * 11: pull up
 	 * different with other gpio, so need to correct it
 	 */
-	if (((port == 0) && (bank < 2)) || ((port == 2) && (bank > 2))) {
+	if (((port == 0) && (bank < 2)) || ((port == 2) && (bank > 1))) {
 		if (pull == GPIO_PULL_UP)
 			pull = 3;
 		else if (pull == GPIO_PULL_DOWN)
@@ -138,17 +237,18 @@
 			      (port - 2) * 16 + bank * 4,
 			      BITS_WITH_WMASK(pull, GPIO_P_MASK, id * 2));
 	}
-	gpio_clk(gpio, 1);
+	gpio_put_clock(gpio, clock_state);
 }
 
 static void set_direction(int gpio, int direction)
 {
-	uint32_t port = gpio / 32;
-	uint32_t num = gpio % 32;
+	uint32_t port = GET_GPIO_PORT(gpio);
+	uint32_t num = GET_GPIO_NUM(gpio);
+	uint32_t clock_state;
 
 	assert((port < 5) && (num < 32));
 
-	gpio_clk(gpio, 0);
+	clock_state = gpio_get_clock(gpio);
 
 	/*
 	 * in gpio.h
@@ -158,18 +258,18 @@
 	 * so need to revert direction value
 	 */
 	mmio_setbits_32(gpio_port[port] + SWPORTA_DDR, !direction << num);
-	gpio_clk(gpio, 1);
+	gpio_put_clock(gpio, clock_state);
 }
 
 static int get_direction(int gpio)
 {
-	uint32_t port = gpio / 32;
-	uint32_t num = gpio % 32;
-	int direction;
+	uint32_t port = GET_GPIO_PORT(gpio);
+	uint32_t num = GET_GPIO_NUM(gpio);
+	int direction, clock_state;
 
 	assert((port < 5) && (num < 32));
 
-	gpio_clk(gpio, 0);
+	clock_state = gpio_get_clock(gpio);
 
 	/*
 	 * in gpio.h
@@ -180,37 +280,38 @@
 	 */
 	direction = !((mmio_read_32(gpio_port[port] +
 				    SWPORTA_DDR) >> num) & 0x1);
-	gpio_clk(gpio, 1);
+	gpio_put_clock(gpio, clock_state);
 
 	return direction;
 }
 
 static int get_value(int gpio)
 {
-	uint32_t port = gpio / 32;
-	uint32_t num = gpio % 32;
-	int value;
+	uint32_t port = GET_GPIO_PORT(gpio);
+	uint32_t num = GET_GPIO_NUM(gpio);
+	int value, clock_state;
 
 	assert((port < 5) && (num < 32));
 
-	gpio_clk(gpio, 0);
+	clock_state = gpio_get_clock(gpio);
 	value = (mmio_read_32(gpio_port[port] + EXT_PORTA) >> num) & 0x1;
-	gpio_clk(gpio, 1);
+	gpio_put_clock(gpio, clock_state);
 
 	return value;
 }
 
 static void set_value(int gpio, int value)
 {
-	uint32_t port = gpio / 32;
-	uint32_t num = gpio % 32;
+	uint32_t port = GET_GPIO_PORT(gpio);
+	uint32_t num = GET_GPIO_NUM(gpio);
+	uint32_t clock_state;
 
 	assert((port < 5) && (num < 32));
 
-	gpio_clk(gpio, 0);
+	clock_state = gpio_get_clock(gpio);
 	mmio_clrsetbits_32(gpio_port[port] + SWPORTA_DR, 1 << num,
 							 !!value << num);
-	gpio_clk(gpio, 0);
+	gpio_put_clock(gpio, clock_state);
 }
 
 const gpio_ops_t rk3399_gpio_ops = {
@@ -219,6 +320,7 @@
 	.get_value = get_value,
 	.set_value = set_value,
 	.set_pull = set_pull,
+	.get_pull = get_pull,
 };
 
 void plat_rockchip_gpio_init(void)
diff --git a/plat/rockchip/rk3399/drivers/pmu/pmu.c b/plat/rockchip/rk3399/drivers/pmu/pmu.c
old mode 100755
new mode 100644
index 50953d8..07a5b1e
--- a/plat/rockchip/rk3399/drivers/pmu/pmu.c
+++ b/plat/rockchip/rk3399/drivers/pmu/pmu.c
@@ -874,6 +874,190 @@
 	mmio_clrbits_32(PMU_BASE + PMU_BUS_CLR, hw_idle);
 }
 
+static uint32_t iomux_status[12];
+static uint32_t pull_mode_status[12];
+static uint32_t gpio_direction[3];
+static uint32_t gpio_2_4_clk_gate;
+
+static void suspend_apio(void)
+{
+	struct apio_info *suspend_apio;
+	int i;
+
+	suspend_apio = plat_get_rockchip_suspend_apio();
+
+	if (!suspend_apio)
+		return;
+
+	/* save gpio2 ~ gpio4 iomux and pull mode */
+	for (i = 0; i < 12; i++) {
+		iomux_status[i] = mmio_read_32(GRF_BASE +
+				GRF_GPIO2A_IOMUX + i * 4);
+		pull_mode_status[i] = mmio_read_32(GRF_BASE +
+				GRF_GPIO2A_P + i * 4);
+	}
+
+	/* store gpio2 ~ gpio4 clock gate state */
+	gpio_2_4_clk_gate = (mmio_read_32(CRU_BASE + CRU_CLKGATE_CON(31)) >>
+				PCLK_GPIO2_GATE_SHIFT) & 0x07;
+
+	/* enable gpio2 ~ gpio4 clock gate */
+	mmio_write_32(CRU_BASE + CRU_CLKGATE_CON(31),
+		      BITS_WITH_WMASK(0, 0x07, PCLK_GPIO2_GATE_SHIFT));
+
+	/* save gpio2 ~ gpio4 direction */
+	gpio_direction[0] = mmio_read_32(GPIO2_BASE + 0x04);
+	gpio_direction[1] = mmio_read_32(GPIO3_BASE + 0x04);
+	gpio_direction[2] = mmio_read_32(GPIO4_BASE + 0x04);
+
+	/* apio1 charge gpio3a0 ~ gpio3c7 */
+	if (suspend_apio->apio1) {
+
+		/* set gpio3a0 ~ gpio3c7 iomux to gpio */
+		mmio_write_32(GRF_BASE + GRF_GPIO3A_IOMUX,
+			      REG_SOC_WMSK | GRF_IOMUX_GPIO);
+		mmio_write_32(GRF_BASE + GRF_GPIO3B_IOMUX,
+			      REG_SOC_WMSK | GRF_IOMUX_GPIO);
+		mmio_write_32(GRF_BASE + GRF_GPIO3C_IOMUX,
+			      REG_SOC_WMSK | GRF_IOMUX_GPIO);
+
+		/* set gpio3a0 ~ gpio3c7 pull mode to pull none */
+		mmio_write_32(GRF_BASE + GRF_GPIO3A_P, REG_SOC_WMSK | 0);
+		mmio_write_32(GRF_BASE + GRF_GPIO3B_P, REG_SOC_WMSK | 0);
+		mmio_write_32(GRF_BASE + GRF_GPIO3C_P, REG_SOC_WMSK | 0);
+
+		/* set gpio3a0 ~ gpio3c7 to input */
+		mmio_clrbits_32(GPIO3_BASE + 0x04, 0x00ffffff);
+	}
+
+	/* apio2 charge gpio2a0 ~ gpio2b4 */
+	if (suspend_apio->apio2) {
+
+		/* set gpio2a0 ~ gpio2b4 iomux to gpio */
+		mmio_write_32(GRF_BASE + GRF_GPIO2A_IOMUX,
+			      REG_SOC_WMSK | GRF_IOMUX_GPIO);
+		mmio_write_32(GRF_BASE + GRF_GPIO2B_IOMUX,
+			      REG_SOC_WMSK | GRF_IOMUX_GPIO);
+
+		/* set gpio2a0 ~ gpio2b4 pull mode to pull none */
+		mmio_write_32(GRF_BASE + GRF_GPIO2A_P, REG_SOC_WMSK | 0);
+		mmio_write_32(GRF_BASE + GRF_GPIO2B_P, REG_SOC_WMSK | 0);
+
+		/* set gpio2a0 ~ gpio2b4 to input */
+		mmio_clrbits_32(GPIO2_BASE + 0x04, 0x00001fff);
+	}
+
+	/* apio3 charge gpio2c0 ~ gpio2d4*/
+	if (suspend_apio->apio3) {
+
+		/* set gpio2a0 ~ gpio2b4 iomux to gpio */
+		mmio_write_32(GRF_BASE + GRF_GPIO2C_IOMUX,
+			      REG_SOC_WMSK | GRF_IOMUX_GPIO);
+		mmio_write_32(GRF_BASE + GRF_GPIO2D_IOMUX,
+			      REG_SOC_WMSK | GRF_IOMUX_GPIO);
+
+		/* set gpio2c0 ~ gpio2d4 pull mode to pull none */
+		mmio_write_32(GRF_BASE + GRF_GPIO2C_P, REG_SOC_WMSK | 0);
+		mmio_write_32(GRF_BASE + GRF_GPIO2D_P, REG_SOC_WMSK | 0);
+
+		/* set gpio2c0 ~ gpio2d4 to input */
+		mmio_clrbits_32(GPIO2_BASE + 0x04, 0x1fff0000);
+	}
+
+	/* apio4 charge gpio4c0 ~ gpio4c7, gpio4d0 ~ gpio4d6 */
+	if (suspend_apio->apio4) {
+
+		/* set gpio4c0 ~ gpio4d6 iomux to gpio */
+		mmio_write_32(GRF_BASE + GRF_GPIO4C_IOMUX,
+			      REG_SOC_WMSK | GRF_IOMUX_GPIO);
+		mmio_write_32(GRF_BASE + GRF_GPIO4D_IOMUX,
+			      REG_SOC_WMSK | GRF_IOMUX_GPIO);
+
+		/* set gpio4c0 ~ gpio4d6 pull mode to pull none */
+		mmio_write_32(GRF_BASE + GRF_GPIO4C_P, REG_SOC_WMSK | 0);
+		mmio_write_32(GRF_BASE + GRF_GPIO4D_P, REG_SOC_WMSK | 0);
+
+		/* set gpio4c0 ~ gpio4d6 to input */
+		mmio_clrbits_32(GPIO4_BASE + 0x04, 0x7fff0000);
+	}
+
+	/* apio5 charge gpio3d0 ~ gpio3d7, gpio4a0 ~ gpio4a7*/
+	if (suspend_apio->apio5) {
+		/* set gpio3d0 ~ gpio4a7 iomux to gpio */
+		mmio_write_32(GRF_BASE + GRF_GPIO3D_IOMUX,
+			      REG_SOC_WMSK | GRF_IOMUX_GPIO);
+		mmio_write_32(GRF_BASE + GRF_GPIO4A_IOMUX,
+			      REG_SOC_WMSK | GRF_IOMUX_GPIO);
+
+		/* set gpio3d0 ~ gpio4a7 pull mode to pull none */
+		mmio_write_32(GRF_BASE + GRF_GPIO3D_P, REG_SOC_WMSK | 0);
+		mmio_write_32(GRF_BASE + GRF_GPIO4A_P, REG_SOC_WMSK | 0);
+
+		/* set gpio4c0 ~ gpio4d6 to input */
+		mmio_clrbits_32(GPIO3_BASE + 0x04, 0xff000000);
+		mmio_clrbits_32(GPIO4_BASE + 0x04, 0x000000ff);
+	}
+}
+
+static void resume_apio(void)
+{
+	struct apio_info *suspend_apio;
+	int i;
+
+	suspend_apio = plat_get_rockchip_suspend_apio();
+
+	if (!suspend_apio)
+		return;
+
+	for (i = 0; i < 12; i++) {
+		mmio_write_32(GRF_BASE + GRF_GPIO2A_P + i * 4,
+			      REG_SOC_WMSK | pull_mode_status[i]);
+		mmio_write_32(GRF_BASE + GRF_GPIO2A_IOMUX + i * 4,
+			      REG_SOC_WMSK | iomux_status[i]);
+	}
+
+	/* set gpio2 ~ gpio4 direction back to store value */
+	mmio_write_32(GPIO2_BASE + 0x04, gpio_direction[0]);
+	mmio_write_32(GPIO3_BASE + 0x04, gpio_direction[1]);
+	mmio_write_32(GPIO4_BASE + 0x04, gpio_direction[2]);
+
+	/* set gpio2 ~ gpio4 clock gate back to store value */
+	mmio_write_32(CRU_BASE + CRU_CLKGATE_CON(31),
+		      BITS_WITH_WMASK(gpio_2_4_clk_gate, 0x07,
+				      PCLK_GPIO2_GATE_SHIFT));
+}
+
+static void suspend_gpio(void)
+{
+	struct gpio_info *suspend_gpio;
+	uint32_t count;
+	int i;
+
+	suspend_gpio = plat_get_rockchip_suspend_gpio(&count);
+
+	for (i = 0; i < count; i++) {
+		gpio_set_value(suspend_gpio[i].index, suspend_gpio[i].polarity);
+		gpio_set_direction(suspend_gpio[i].index, GPIO_DIR_OUT);
+		udelay(1);
+	}
+}
+
+static void resume_gpio(void)
+{
+	struct gpio_info *suspend_gpio;
+	uint32_t count;
+	int i;
+
+	suspend_gpio = plat_get_rockchip_suspend_gpio(&count);
+
+	for (i = count - 1; i >= 0; i--) {
+		gpio_set_value(suspend_gpio[i].index,
+			       !suspend_gpio[i].polarity);
+		gpio_set_direction(suspend_gpio[i].index, GPIO_DIR_OUT);
+		udelay(1);
+	}
+}
+
 static int sys_pwr_domain_suspend(void)
 {
 	uint32_t wait_cnt = 0;
@@ -919,7 +1103,6 @@
 		}
 	}
 	mmio_setbits_32(PMU_BASE + PMU_PWRDN_CON, BIT(PMU_SCU_B_PWRDWN_EN));
-
 	/*
 	 * Disabling PLLs/PWM/DVFS is approaching WFI which is
 	 * the last steps in suspend.
@@ -928,6 +1111,8 @@
 	disable_dvfs_plls();
 	disable_pwms();
 	disable_nodvfs_plls();
+	suspend_apio();
+	suspend_gpio();
 
 	return 0;
 }
@@ -937,6 +1122,8 @@
 	uint32_t wait_cnt = 0;
 	uint32_t status = 0;
 
+	resume_apio();
+	resume_gpio();
 	enable_nodvfs_plls();
 	enable_pwms();
 	/* PWM regulators take time to come up; give 300us to be safe. */
@@ -952,7 +1139,6 @@
 	 * somewhere.
 	 */
 	mmio_write_32(PMU_BASE + PMU_WAKEUP_STATUS, 0xffffffff);
-
 	mmio_write_32(PMU_BASE + PMU_WKUP_CFG4, 0x00);
 
 	mmio_write_32(SGRF_BASE + SGRF_SOC_CON0_1(1),
@@ -1003,6 +1189,9 @@
 				BIT(PMU_CLR_PERILP) |
 				BIT(PMU_CLR_PMU) |
 				BIT(PMU_CLR_GIC));
+
+	plat_rockchip_gic_cpuif_enable();
+
 	return 0;
 }
 
@@ -1010,7 +1199,7 @@
 {
 	struct gpio_info *rst_gpio;
 
-	rst_gpio = (struct gpio_info *)plat_get_rockchip_gpio_reset();
+	rst_gpio = plat_get_rockchip_gpio_reset();
 
 	if (rst_gpio) {
 		gpio_set_direction(rst_gpio->index, GPIO_DIR_OUT);
@@ -1027,7 +1216,7 @@
 {
 	struct gpio_info *poweroff_gpio;
 
-	poweroff_gpio = (struct gpio_info *)plat_get_rockchip_gpio_poweroff();
+	poweroff_gpio = plat_get_rockchip_gpio_poweroff();
 
 	if (poweroff_gpio) {
 		/*
diff --git a/plat/rockchip/rk3399/drivers/pmu/pmu.h b/plat/rockchip/rk3399/drivers/pmu/pmu.h
index 65fe7db..ab2896b 100644
--- a/plat/rockchip/rk3399/drivers/pmu/pmu.h
+++ b/plat/rockchip/rk3399/drivers/pmu/pmu.h
@@ -875,7 +875,32 @@
 #define MAX_WAIT_COUNT		1000
 
 #define	GRF_SOC_CON4		0x0e210
-#define GRF_GPIO4C_IOMUX	0x0e028
+
+#define GRF_GPIO2A_IOMUX	0xe000
+#define GRF_GPIO2B_IOMUX	0xe004
+#define GRF_GPIO2C_IOMUX	0xe008
+#define GRF_GPIO2D_IOMUX	0xe00c
+#define GRF_GPIO3A_IOMUX	0xe010
+#define GRF_GPIO3B_IOMUX	0xe014
+#define GRF_GPIO3C_IOMUX	0xe018
+#define GRF_GPIO3D_IOMUX	0xe01c
+#define GRF_GPIO4A_IOMUX	0xe020
+#define GRF_GPIO4B_IOMUX	0xe024
+#define GRF_GPIO4C_IOMUX	0xe028
+#define GRF_GPIO4D_IOMUX	0xe02c
+
+#define GRF_GPIO2A_P		0xe040
+#define GRF_GPIO2B_P		0xe044
+#define GRF_GPIO2C_P		0xe048
+#define GRF_GPIO2D_P		0xe04C
+#define GRF_GPIO3A_P		0xe050
+#define GRF_GPIO3B_P		0xe054
+#define GRF_GPIO3C_P		0xe058
+#define GRF_GPIO3D_P		0xe05C
+#define GRF_GPIO4A_P		0xe060
+#define GRF_GPIO4B_P		0xe064
+#define GRF_GPIO4C_P		0xe068
+#define GRF_GPIO4D_P		0xe06C
 
 #define PMUGRF_GPIO0A_SMT	0x0120
 #define PMUGRF_SOC_CON0		0x0180
diff --git a/plat/rockchip/rk3399/plat_sip_calls.c b/plat/rockchip/rk3399/plat_sip_calls.c
index 6069be2..a20ee2d 100644
--- a/plat/rockchip/rk3399/plat_sip_calls.c
+++ b/plat/rockchip/rk3399/plat_sip_calls.c
@@ -31,33 +31,33 @@
 #include <runtime_svc.h>
 #include <dram.h>
 
-#define RK_SIP_DDR_CFG64	0x82000008
-#define CONFIG_DRAM_INIT	0x00
-#define CONFIG_DRAM_SET_RATE	0x01
-#define CONFIG_DRAM_ROUND_RATE	0x02
-#define CONFIG_DRAM_SET_AT_SR	0x03
-#define CONFIG_DRAM_GET_BW	0x04
-#define CONFIG_DRAM_GET_RATE	0x05
-#define CONFIG_DRAM_CLR_IRQ	0x06
-#define CONFIG_DRAM_SET_PARAM   0x07
+#define RK_SIP_DDR_CFG		0x82000008
+#define DRAM_INIT		0x00
+#define DRAM_SET_RATE		0x01
+#define DRAM_ROUND_RATE		0x02
+#define DRAM_SET_AT_SR		0x03
+#define DRAM_GET_BW		0x04
+#define DRAM_GET_RATE		0x05
+#define DRAM_CLR_IRQ		0x06
+#define DRAM_SET_PARAM		0x07
 
-uint64_t ddr_smc_handler(uint64_t arg0, uint64_t arg1, uint64_t id)
+uint32_t ddr_smc_handler(uint64_t arg0, uint64_t arg1, uint64_t id)
 {
 	switch (id) {
-	case CONFIG_DRAM_INIT:
+	case DRAM_INIT:
 		ddr_init();
 		break;
-	case CONFIG_DRAM_SET_RATE:
-		return ddr_set_rate(arg0);
-	case CONFIG_DRAM_ROUND_RATE:
-		return ddr_round_rate(arg0);
-	case CONFIG_DRAM_GET_RATE:
+	case DRAM_SET_RATE:
+		return ddr_set_rate((uint32_t)arg0);
+	case DRAM_ROUND_RATE:
+		return ddr_round_rate((uint32_t)arg0);
+	case DRAM_GET_RATE:
 		return ddr_get_rate();
-	case CONFIG_DRAM_CLR_IRQ:
+	case DRAM_CLR_IRQ:
 		clr_dcf_irq();
 		break;
-	case CONFIG_DRAM_SET_PARAM:
-		dts_timing_receive(arg0, arg1);
+	case DRAM_SET_PARAM:
+		dts_timing_receive((uint32_t)arg0, (uint32_t)arg1);
 		break;
 	default:
 		break;
@@ -76,7 +76,7 @@
 				   uint64_t flags)
 {
 	switch (smc_fid) {
-	case RK_SIP_DDR_CFG64:
+	case RK_SIP_DDR_CFG:
 		SMC_RET1(handle, ddr_smc_handler(x1, x2, x3));
 	default:
 		ERROR("%s: unhandled SMC (0x%x)\n", __func__, smc_fid);
diff --git a/plat/xilinx/zynqmp/aarch64/zynqmp_common.c b/plat/xilinx/zynqmp/aarch64/zynqmp_common.c
index 1ba301d..60a1605 100644
--- a/plat/xilinx/zynqmp/aarch64/zynqmp_common.c
+++ b/plat/xilinx/zynqmp/aarch64/zynqmp_common.c
@@ -29,6 +29,7 @@
  */
 
 #include <debug.h>
+#include <generic_delay_timer.h>
 #include <mmio.h>
 #include <platform.h>
 #include <xlat_tables.h>
@@ -89,6 +90,18 @@
 	return 100000000;
 }
 
+unsigned int zynqmp_get_silicon_id(void)
+{
+	uint32_t id;
+
+	id = mmio_read_32(ZYNQMP_CSU_BASEADDR + ZYNQMP_CSU_IDCODE_OFFSET);
+
+	id &= ZYNQMP_CSU_IDCODE_DEVICE_CODE_MASK | ZYNQMP_CSU_IDCODE_SVD_MASK;
+	id >>= ZYNQMP_CSU_IDCODE_SVD_SHIFT;
+
+	return id;
+}
+
 #if LOG_LEVEL >= LOG_LEVEL_NOTICE
 static const struct {
 	unsigned int id;
@@ -140,18 +153,6 @@
 	},
 };
 
-static unsigned int zynqmp_get_silicon_id(void)
-{
-	uint32_t id;
-
-	id = mmio_read_32(ZYNQMP_CSU_BASEADDR + ZYNQMP_CSU_IDCODE_OFFSET);
-
-	id &= ZYNQMP_CSU_IDCODE_DEVICE_CODE_MASK | ZYNQMP_CSU_IDCODE_SVD_MASK;
-	id >>= ZYNQMP_CSU_IDCODE_SVD_SHIFT;
-
-	return id;
-}
-
 static char *zynqmp_get_silicon_idcode_name(void)
 {
 	unsigned int id;
@@ -289,6 +290,8 @@
 	/* Program freq register in System counter and enable system counter. */
 	mmio_write_32(IOU_SCNTRS_BASEFREQ, zynqmp_get_system_timer_freq());
 	mmio_write_32(IOU_SCNTRS_CONTROL, IOU_SCNTRS_CONTROL_EN);
+
+	generic_delay_timer_init();
 }
 
 unsigned int plat_get_syscnt_freq2(void)
diff --git a/plat/xilinx/zynqmp/bl31_zynqmp_setup.c b/plat/xilinx/zynqmp/bl31_zynqmp_setup.c
index d878b86..c05b094 100644
--- a/plat/xilinx/zynqmp/bl31_zynqmp_setup.c
+++ b/plat/xilinx/zynqmp/bl31_zynqmp_setup.c
@@ -118,11 +118,31 @@
 	NOTICE("BL31: Non secure code at 0x%lx\n", bl33_image_ep_info.pc);
 }
 
+/* Enable the test setup */
+#ifndef ZYNQMP_TESTING
+static void zynqmp_testing_setup(void) { }
+#else
+static void zynqmp_testing_setup(void)
+{
+	uint32_t actlr_el3, actlr_el2;
+
+	/* Enable CPU ACTLR AND L2ACTLR RW access from non-secure world */
+	actlr_el3 = read_actlr_el3();
+	actlr_el2 = read_actlr_el2();
+
+	actlr_el3 |= ACTLR_EL3_L2ACTLR_BIT | ACTLR_EL3_CPUACTLR_BIT;
+	actlr_el2 |= ACTLR_EL3_L2ACTLR_BIT | ACTLR_EL3_CPUACTLR_BIT;
+	write_actlr_el3(actlr_el3);
+	write_actlr_el2(actlr_el2);
+}
+#endif
+
 void bl31_platform_setup(void)
 {
 	/* Initialize the gic cpu and distributor interfaces */
 	plat_arm_gic_driver_init();
 	plat_arm_gic_init();
+	zynqmp_testing_setup();
 }
 
 void bl31_plat_runtime_setup(void)
diff --git a/plat/xilinx/zynqmp/include/platform_def.h b/plat/xilinx/zynqmp/include/platform_def.h
index a35bd12..3c1a9e5 100644
--- a/plat/xilinx/zynqmp/include/platform_def.h
+++ b/plat/xilinx/zynqmp/include/platform_def.h
@@ -56,8 +56,7 @@
  * little space for growth.
  */
 #ifndef ZYNQMP_ATF_MEM_BASE
-# define BL31_BASE			0xfffe5000
-# define BL31_PROGBITS_LIMIT		0xffffa000
+# define BL31_BASE			0xfffea000
 # define BL31_LIMIT			0xffffffff
 #else
 # define BL31_BASE			(ZYNQMP_ATF_MEM_BASE)
@@ -101,11 +100,7 @@
  ******************************************************************************/
 #define ADDR_SPACE_SIZE			(1ull << 32)
 #define MAX_MMAP_REGIONS		7
-#if IMAGE_BL32
-# define MAX_XLAT_TABLES		5
-#else
-# define MAX_XLAT_TABLES		4
-#endif
+#define MAX_XLAT_TABLES			5
 
 #define CACHE_WRITEBACK_SHIFT   6
 #define CACHE_WRITEBACK_GRANULE (1 << CACHE_WRITEBACK_SHIFT)
diff --git a/plat/xilinx/zynqmp/plat_psci.c b/plat/xilinx/zynqmp/plat_psci.c
index 56eb742..55227ea 100644
--- a/plat/xilinx/zynqmp/plat_psci.c
+++ b/plat/xilinx/zynqmp/plat_psci.c
@@ -147,7 +147,7 @@
 	 * invoking CPU_on function, during which resume address will
 	 * be set.
 	 */
-	pm_self_suspend(proc->node_id, MAX_LATENCY, 0, 0);
+	pm_self_suspend(proc->node_id, MAX_LATENCY, PM_STATE_CPU_IDLE, 0);
 }
 
 static void zynqmp_nopmu_pwr_domain_suspend(const psci_power_state_t *target_state)
@@ -179,6 +179,7 @@
 
 static void zynqmp_pwr_domain_suspend(const psci_power_state_t *target_state)
 {
+	unsigned int state;
 	unsigned int cpu_id = plat_my_core_pos();
 	const struct pm_proc *proc = pm_get_proc(cpu_id);
 
@@ -186,15 +187,14 @@
 		VERBOSE("%s: target_state->pwr_domain_state[%lu]=%x\n",
 			__func__, i, target_state->pwr_domain_state[i]);
 
+	state = target_state->pwr_domain_state[1] > PLAT_MAX_RET_STATE ?
+		PM_STATE_SUSPEND_TO_RAM : PM_STATE_CPU_IDLE;
+
 	/* Send request to PMU to suspend this core */
-	pm_self_suspend(proc->node_id, MAX_LATENCY, 0, zynqmp_sec_entry);
+	pm_self_suspend(proc->node_id, MAX_LATENCY, state, zynqmp_sec_entry);
 
 	/* APU is to be turned off */
 	if (target_state->pwr_domain_state[1] > PLAT_MAX_RET_STATE) {
-		/* Power down L2 cache */
-		pm_set_requirement(NODE_L2, 0, 0, REQ_ACK_NO);
-		/* Send request for OCM retention state */
-		set_ocm_retention();
 		/* disable coherency */
 		plat_arm_interconnect_exit_coherency();
 	}
@@ -242,6 +242,13 @@
 
 	/* enable coherency */
 	plat_arm_interconnect_enter_coherency();
+	/* APU was turned off */
+	if (target_state->pwr_domain_state[1] > PLAT_MAX_RET_STATE) {
+		plat_arm_gic_init();
+	} else {
+		gicv2_cpuif_enable();
+		gicv2_pcpu_distif_init();
+	}
 }
 
 /*******************************************************************************
@@ -308,7 +315,20 @@
 {
 	VERBOSE("%s: power_state: 0x%x\n", __func__, power_state);
 
+	int pstate = psci_get_pstate_type(power_state);
+
+	assert(req_state);
+
+	/* Sanity check the requested state */
+	if (pstate == PSTATE_TYPE_STANDBY)
+		req_state->pwr_domain_state[MPIDR_AFFLVL0] = PLAT_MAX_RET_STATE;
+	else
+		req_state->pwr_domain_state[MPIDR_AFFLVL0] = PLAT_MAX_OFF_STATE;
+
+	/* We expect the 'state id' to be zero */
+	if (psci_get_pstate_id(power_state))
+		return PSCI_E_INVALID_PARAMS;
+
-	/* FIXME: populate req_state */
 	return PSCI_E_SUCCESS;
 }
 
diff --git a/plat/xilinx/zynqmp/platform.mk b/plat/xilinx/zynqmp/platform.mk
index 9bde5ff..33c55a0 100644
--- a/plat/xilinx/zynqmp/platform.mk
+++ b/plat/xilinx/zynqmp/platform.mk
@@ -31,6 +31,7 @@
 PSCI_EXTENDED_STATE_ID := 1
 A53_DISABLE_NON_TEMPORAL_HINT := 0
 SEPARATE_CODE_AND_RODATA := 1
+RESET_TO_BL31 := 1
 
 ifdef ZYNQMP_ATF_MEM_BASE
     $(eval $(call add_define,ZYNQMP_ATF_MEM_BASE))
@@ -64,6 +65,8 @@
 
 PLAT_BL_COMMON_SOURCES	:=	lib/xlat_tables/xlat_tables_common.c		\
 				lib/xlat_tables/aarch64/xlat_tables.c		\
+				drivers/delay_timer/delay_timer.c		\
+				drivers/delay_timer/generic_delay_timer.c	\
 				drivers/arm/gic/common/gic_common.c		\
 				drivers/arm/gic/v2/gicv2_main.c			\
 				drivers/arm/gic/v2/gicv2_helpers.c		\
diff --git a/plat/xilinx/zynqmp/pm_service/pm_api_sys.c b/plat/xilinx/zynqmp/pm_service/pm_api_sys.c
index eac7801..e859ee3 100644
--- a/plat/xilinx/zynqmp/pm_service/pm_api_sys.c
+++ b/plat/xilinx/zynqmp/pm_service/pm_api_sys.c
@@ -76,7 +76,7 @@
  * pm_self_suspend() - PM call for processor to suspend itself
  * @nid		Node id of the processor or subsystem
  * @latency	Requested maximum wakeup latency (not supported)
- * @state	Requested state (not supported)
+ * @state	Requested state
  * @address	Resume address
  *
  * This is a blocking call, it will return only once PMU has responded.
@@ -97,7 +97,7 @@
 	 * Do client specific suspend operations
 	 * (e.g. set powerdown request bit)
 	 */
-	pm_client_suspend(proc);
+	pm_client_suspend(proc, state);
 	/* Send request to the PMU */
 	PM_PACK_PAYLOAD6(payload, PM_SELF_SUSPEND, proc->node_id, latency,
 			 state, address, (address >> 32));
@@ -476,7 +476,7 @@
 
 	/* Send request to the PMU */
 	PM_PACK_PAYLOAD4(payload, PM_MMIO_WRITE, address, mask, value);
-	return pm_ipi_send(primary_proc, payload);
+	return pm_ipi_send_sync(primary_proc, payload, NULL);
 }
 
 /**
@@ -497,3 +497,47 @@
 	PM_PACK_PAYLOAD2(payload, PM_MMIO_READ, address);
 	return pm_ipi_send_sync(primary_proc, payload, value);
 }
+
+/**
+ * pm_fpga_load() - Load the bitstream into the PL.
+ *
+ * This function provides access to the xilfpga library to load
+ * the Bit-stream into PL.
+ *
+ * address_low: lower 32-bit Linear memory space address
+ *
+ * address_high: higher 32-bit Linear memory space address
+ *
+ * size:	Number of 32bit words
+ *
+ * @return      Returns status, either success or error+reason
+ */
+enum pm_ret_status pm_fpga_load(uint32_t address_low,
+				uint32_t address_high,
+				uint32_t size,
+				uint32_t flags)
+{
+	uint32_t payload[PAYLOAD_ARG_CNT];
+
+	/* Send request to the PMU */
+	PM_PACK_PAYLOAD5(payload, PM_FPGA_LOAD, address_high, address_low,
+						size, flags);
+	return pm_ipi_send(primary_proc, payload);
+}
+
+/**
+ * pm_fpga_get_status() - Read value from fpga status register
+ * @value       Value to read
+ *
+ * This function provides access to the xilfpga library to get
+ * the fpga status
+ * @return      Returns status, either success or error+reason
+ */
+enum pm_ret_status pm_fpga_get_status(unsigned int *value)
+{
+	uint32_t payload[PAYLOAD_ARG_CNT];
+
+	/* Send request to the PMU */
+	PM_PACK_PAYLOAD1(payload, PM_FPGA_GET_STATUS);
+	return pm_ipi_send_sync(primary_proc, payload, value);
+}
diff --git a/plat/xilinx/zynqmp/pm_service/pm_api_sys.h b/plat/xilinx/zynqmp/pm_service/pm_api_sys.h
index 22bdb47..26d83e7 100644
--- a/plat/xilinx/zynqmp/pm_service/pm_api_sys.h
+++ b/plat/xilinx/zynqmp/pm_service/pm_api_sys.h
@@ -109,4 +109,10 @@
 				 unsigned int mask,
 				 unsigned int value);
 enum pm_ret_status pm_mmio_read(uintptr_t address, unsigned int *value);
+enum pm_ret_status pm_fpga_load(uint32_t address_high,
+				uint32_t address_low,
+				uint32_t size,
+				uint32_t flags);
+enum pm_ret_status pm_fpga_get_status(unsigned int *value);
+
 #endif /* _PM_API_SYS_H_ */
diff --git a/plat/xilinx/zynqmp/pm_service/pm_client.c b/plat/xilinx/zynqmp/pm_service/pm_client.c
index cf0d5f0..b77a1cf 100644
--- a/plat/xilinx/zynqmp/pm_service/pm_client.c
+++ b/plat/xilinx/zynqmp/pm_service/pm_client.c
@@ -33,26 +33,25 @@
  * for getting information about and changing state of the APU.
  */
 
+#include <assert.h>
 #include <bakery_lock.h>
 #include <gicv2.h>
+#include <gic_common.h>
 #include <bl_common.h>
 #include <mmio.h>
+#include <string.h>
 #include <utils.h>
 #include "pm_api_sys.h"
 #include "pm_client.h"
 #include "pm_ipi.h"
 #include "../zynqmp_def.h"
 
-#define OCM_BANK_0	0xFFFC0000
-#define OCM_BANK_1	(OCM_BANK_0 + 0x10000)
-#define OCM_BANK_2	(OCM_BANK_1 + 0x10000)
-#define OCM_BANK_3	(OCM_BANK_2 + 0x10000)
-
+#define IRQ_MAX		84
+#define NUM_GICD_ISENABLER	((IRQ_MAX >> 5) + 1)
 #define UNDEFINED_CPUID		(~0)
+
 DEFINE_BAKERY_LOCK(pm_client_secure_lock);
 
-/* Declaration of linker defined symbol */
-extern unsigned long __BL31_END__;
 extern const struct pm_ipi apu_ipi;
 
 /* Order in pm_procs_all array must match cpu ids */
@@ -79,36 +78,146 @@
 	},
 };
 
+/* Interrupt to PM node ID map */
+static enum pm_node_id irq_node_map[IRQ_MAX + 1] = {
+	NODE_UNKNOWN,
+	NODE_UNKNOWN,
+	NODE_UNKNOWN,
+	NODE_UNKNOWN,	/* 3 */
+	NODE_UNKNOWN,
+	NODE_UNKNOWN,
+	NODE_UNKNOWN,
+	NODE_UNKNOWN,	/* 7 */
+	NODE_UNKNOWN,
+	NODE_UNKNOWN,
+	NODE_UNKNOWN,
+	NODE_UNKNOWN,	/* 11 */
+	NODE_UNKNOWN,
+	NODE_UNKNOWN,
+	NODE_NAND,
+	NODE_QSPI,	/* 15 */
+	NODE_GPIO,
+	NODE_I2C_0,
+	NODE_I2C_1,
+	NODE_SPI_0,	/* 19 */
+	NODE_SPI_1,
+	NODE_UART_0,
+	NODE_UART_1,
+	NODE_CAN_0,	/* 23 */
+	NODE_CAN_1,
+	NODE_UNKNOWN,
+	NODE_RTC,
+	NODE_RTC,	/* 27 */
+	NODE_UNKNOWN,
+	NODE_UNKNOWN,
+	NODE_UNKNOWN,
+	NODE_UNKNOWN,	/* 31 */
+	NODE_UNKNOWN,
+	NODE_UNKNOWN,
+	NODE_UNKNOWN,
+	NODE_UNKNOWN,	/* 35, NODE_IPI_APU */
+	NODE_TTC_0,
+	NODE_TTC_0,
+	NODE_TTC_0,
+	NODE_TTC_1,	/* 39 */
+	NODE_TTC_1,
+	NODE_TTC_1,
+	NODE_TTC_2,
+	NODE_TTC_2,	/* 43 */
+	NODE_TTC_2,
+	NODE_TTC_3,
+	NODE_TTC_3,
+	NODE_TTC_3,	/* 47 */
+	NODE_SD_0,
+	NODE_SD_1,
+	NODE_SD_0,
+	NODE_SD_1,	/* 51 */
+	NODE_UNKNOWN,
+	NODE_UNKNOWN,
+	NODE_UNKNOWN,
+	NODE_UNKNOWN,	/* 55 */
+	NODE_UNKNOWN,
+	NODE_ETH_0,
+	NODE_ETH_0,
+	NODE_ETH_1,	/* 59 */
+	NODE_ETH_1,
+	NODE_ETH_2,
+	NODE_ETH_2,
+	NODE_ETH_3,	/* 63 */
+	NODE_ETH_3,
+	NODE_USB_0,
+	NODE_USB_0,
+	NODE_USB_0,	/* 67 */
+	NODE_USB_0,
+	NODE_USB_0,
+	NODE_USB_1,
+	NODE_USB_1,	/* 71 */
+	NODE_USB_1,
+	NODE_USB_1,
+	NODE_USB_1,
+	NODE_USB_0,	/* 75 */
+	NODE_USB_0,
+	NODE_ADMA,
+	NODE_ADMA,
+	NODE_ADMA,	/* 79 */
+	NODE_ADMA,
+	NODE_ADMA,
+	NODE_ADMA,
+	NODE_ADMA,	/* 83 */
+	NODE_ADMA,
+};
+
 /**
- * set_ocm_retention() - Configure OCM memory banks for retention
+ * irq_to_pm_node - Get PM node ID corresponding to the interrupt number
+ * @irq:	Interrupt number
  *
- * APU specific requirements for suspend action:
- * OCM has to enter retention state in order to preserve saved
- * context after suspend request. OCM banks are determined by
- * __BL31_END__ linker symbol.
- *
- * Return:	Returns status, either success or error+reason
+ * Return:	PM node ID corresponding to the specified interrupt
  */
-enum pm_ret_status set_ocm_retention(void)
+static enum pm_node_id irq_to_pm_node(unsigned int irq)
 {
-	enum pm_ret_status ret;
+	assert(irq <= IRQ_MAX);
+	return irq_node_map[irq];
+}
 
-	/* OCM_BANK_0 will always be occupied */
-	ret = pm_set_requirement(NODE_OCM_BANK_0, PM_CAP_CONTEXT, 0,
-				 REQ_ACK_NO);
+/**
+ * pm_client_set_wakeup_sources - Set all slaves with enabled interrupts as wake
+ *				sources in the PMU firmware
+ */
+static void pm_client_set_wakeup_sources(void)
+{
+	uint32_t reg_num;
+	uint8_t pm_wakeup_nodes_set[NODE_MAX];
+	uintptr_t isenabler1 = BASE_GICD_BASE + GICD_ISENABLER + 4;
 
-	/* Check for other OCM banks  */
-	if ((unsigned long)&__BL31_END__ >= OCM_BANK_1)
-		ret = pm_set_requirement(NODE_OCM_BANK_1, PM_CAP_CONTEXT, 0,
-					 REQ_ACK_NO);
-	if ((unsigned long)&__BL31_END__ >= OCM_BANK_2)
-		ret = pm_set_requirement(NODE_OCM_BANK_2, PM_CAP_CONTEXT, 0,
-					 REQ_ACK_NO);
-	if ((unsigned long)&__BL31_END__ >= OCM_BANK_3)
-		ret = pm_set_requirement(NODE_OCM_BANK_3, PM_CAP_CONTEXT, 0,
-					 REQ_ACK_NO);
+	memset(&pm_wakeup_nodes_set, 0, sizeof(pm_wakeup_nodes_set));
 
-	return ret;
+	for (reg_num = 0; reg_num < NUM_GICD_ISENABLER; reg_num++) {
+		uint32_t base_irq = reg_num << ISENABLER_SHIFT;
+		uint32_t reg = mmio_read_32(isenabler1 + (reg_num << 2));
+
+		if (!reg)
+			continue;
+
+		while (reg) {
+			enum pm_node_id node;
+			uint32_t idx, ret, irq, lowest_set = reg & (-reg);
+
+			idx = __builtin_ctz(lowest_set);
+			irq = base_irq + idx;
+
+			if (irq > IRQ_MAX)
+				break;
+
+			node = irq_to_pm_node(irq);
+			reg &= ~lowest_set;
+
+			if ((node != NODE_UNKNOWN) &&
+			    (!pm_wakeup_nodes_set[node])) {
+				ret = pm_set_wakeup_source(NODE_APU, node, 1);
+				pm_wakeup_nodes_set[node] = !ret;
+			}
+		}
+	}
 }
 
 /**
@@ -162,11 +271,15 @@
  *
  * This function should contain any PU-specific actions
  * required prior to sending suspend request to PMU
+ * Actions taken depend on the state system is suspending to.
  */
-void pm_client_suspend(const struct pm_proc *proc)
+void pm_client_suspend(const struct pm_proc *proc, unsigned int state)
 {
 	bakery_lock_get(&pm_client_secure_lock);
 
+	if (state == PM_STATE_SUSPEND_TO_RAM)
+		pm_client_set_wakeup_sources();
+
 	/* Set powerdown request */
 	mmio_write_32(APU_PWRCTL, mmio_read_32(APU_PWRCTL) | proc->pwrdn_mask);
 
diff --git a/plat/xilinx/zynqmp/pm_service/pm_client.h b/plat/xilinx/zynqmp/pm_service/pm_client.h
index 9483b0d..7f80d5b 100644
--- a/plat/xilinx/zynqmp/pm_service/pm_client.h
+++ b/plat/xilinx/zynqmp/pm_service/pm_client.h
@@ -40,7 +40,7 @@
 #include "pm_common.h"
 
 /* Functions to be implemented by each PU */
-void pm_client_suspend(const struct pm_proc *proc);
+void pm_client_suspend(const struct pm_proc *proc, unsigned int state);
 void pm_client_abort_suspend(void);
 void pm_client_wakeup(const struct pm_proc *proc);
 enum pm_ret_status set_ocm_retention(void);
diff --git a/plat/xilinx/zynqmp/pm_service/pm_defs.h b/plat/xilinx/zynqmp/pm_service/pm_defs.h
index adeec64..aec335a 100644
--- a/plat/xilinx/zynqmp/pm_service/pm_defs.h
+++ b/plat/xilinx/zynqmp/pm_service/pm_defs.h
@@ -53,6 +53,10 @@
 #define MAX_LATENCY	(~0U)
 #define MAX_QOS		100U
 
+/* State arguments of the self suspend */
+#define PM_STATE_CPU_IDLE		0x0U
+#define PM_STATE_SUSPEND_TO_RAM		0xFU
+
 /*********************************************************************
  * Enum definitions
  ********************************************************************/
@@ -82,6 +86,10 @@
 	PM_RESET_GET_STATUS,
 	PM_MMIO_WRITE,
 	PM_MMIO_READ,
+	PM_INIT,
+	PM_FPGA_LOAD,
+	PM_FPGA_GET_STATUS,
+	PM_GET_CHIPID,
 	PM_API_MAX
 };
 
@@ -143,6 +151,12 @@
 	NODE_IOPLL,
 	NODE_DDR,
 	NODE_IPI_APU,
+	NODE_IPI_RPU_0,
+	NODE_GPU,
+	NODE_PCIE,
+	NODE_PCAP,
+	NODE_RTC,
+	NODE_MAX
 };
 
 enum pm_request_ack {
diff --git a/plat/xilinx/zynqmp/pm_service/pm_svc_main.c b/plat/xilinx/zynqmp/pm_service/pm_svc_main.c
index e3c25c3..9c08ffb 100644
--- a/plat/xilinx/zynqmp/pm_service/pm_svc_main.c
+++ b/plat/xilinx/zynqmp/pm_service/pm_svc_main.c
@@ -228,6 +228,22 @@
 		ret = pm_mmio_read(pm_arg[0], &value);
 		SMC_RET1(handle, (uint64_t)ret | ((uint64_t)value) << 32);
 	}
+
+	case PM_FPGA_LOAD:
+		ret = pm_fpga_load(pm_arg[0], pm_arg[1], pm_arg[2], pm_arg[3]);
+		SMC_RET1(handle, (uint64_t)ret);
+
+	case PM_FPGA_GET_STATUS:
+	{
+		uint32_t value;
+
+		ret = pm_fpga_get_status(&value);
+		SMC_RET1(handle, (uint64_t)ret | ((uint64_t)value) << 32);
+	}
+
+	case PM_GET_CHIPID:
+		SMC_RET1(handle, zynqmp_get_silicon_id());
+
 	default:
 		WARN("Unimplemented PM Service Call: 0x%x\n", smc_fid);
 		SMC_RET1(handle, SMC_UNK);
diff --git a/plat/xilinx/zynqmp/zynqmp_def.h b/plat/xilinx/zynqmp/zynqmp_def.h
index 4bb332e..65bc25f 100644
--- a/plat/xilinx/zynqmp/zynqmp_def.h
+++ b/plat/xilinx/zynqmp/zynqmp_def.h
@@ -201,4 +201,8 @@
 
 #define ZYNQMP_CSU_VERSION_OFFSET	0x44
 
+/* Access control register defines */
+#define ACTLR_EL3_L2ACTLR_BIT	(1 << 6)
+#define ACTLR_EL3_CPUACTLR_BIT	(1 << 0)
+
 #endif /* __ZYNQMP_DEF_H__ */
diff --git a/plat/xilinx/zynqmp/zynqmp_private.h b/plat/xilinx/zynqmp/zynqmp_private.h
index ddef37b..abcdebc 100644
--- a/plat/xilinx/zynqmp/zynqmp_private.h
+++ b/plat/xilinx/zynqmp/zynqmp_private.h
@@ -39,6 +39,7 @@
 unsigned int zynqmp_get_uart_clk(void);
 int zynqmp_is_pmu_up(void);
 unsigned int zynqmp_get_bootmode(void);
+unsigned int zynqmp_get_silicon_id(void);
 
 /* For FSBL handover */
 void fsbl_atf_handover(entry_point_info_t *bl32_image_ep_info,
diff --git a/tools/fiptool/Makefile b/tools/fiptool/Makefile
index 3bc372a..df76a75 100644
--- a/tools/fiptool/Makefile
+++ b/tools/fiptool/Makefile
@@ -44,6 +44,7 @@
 else
   CFLAGS += -O2
 endif
+LDLIBS := -lcrypto
 
 ifeq (${V},0)
   Q := @
@@ -62,7 +63,7 @@
 
 ${PROJECT}: ${OBJECTS} Makefile
 	@echo "  LD      $@"
-	${Q}${CC} ${OBJECTS} -o $@
+	${Q}${CC} ${OBJECTS} -o $@ ${LDLIBS}
 	@${ECHO_BLANK_LINE}
 	@echo "Built $@ successfully"
 	@${ECHO_BLANK_LINE}
diff --git a/tools/fiptool/fiptool.c b/tools/fiptool/fiptool.c
index 68ddcf5..b3f02f6 100644
--- a/tools/fiptool/fiptool.c
+++ b/tools/fiptool/fiptool.c
@@ -42,6 +42,8 @@
 #include <string.h>
 #include <unistd.h>
 
+#include <openssl/sha.h>
+
 #include "fiptool.h"
 #include "firmware_image_package.h"
 #include "tbbr_config.h"
@@ -354,6 +356,14 @@
 	opts[idx].val = val;
 }
 
+static void md_print(unsigned char *md, size_t len)
+{
+	size_t i;
+
+	for (i = 0; i < len; i++)
+		printf("%02x", md[i]);
+}
+
 static int info_cmd(int argc, char *argv[])
 {
 	image_t *image;
@@ -363,7 +373,7 @@
 	int i;
 
 	if (argc != 2)
-		usage();
+		info_usage();
 	argc--, argv++;
 
 	parse_fip(argv[0], &toc_header);
@@ -391,10 +401,16 @@
 		    (unsigned long long)image_offset,
 		    (unsigned long long)image_size);
 		if (image->toc_entry != NULL)
-			printf(", cmdline=\"--%s\"\n",
+			printf(", cmdline=\"--%s\"",
 			    image->toc_entry->cmdline_name);
-		else
-			putchar('\n');
+		if (verbose) {
+			unsigned char md[SHA256_DIGEST_LENGTH];
+
+			SHA256(image->buffer, image_size, md);
+			printf(", sha256=");
+			md_print(md, sizeof(md));
+		}
+		putchar('\n');
 		image_offset += image_size;
 	}
 
@@ -405,6 +421,7 @@
 static void info_usage(void)
 {
 	printf("fiptool info FIP_FILENAME\n");
+	exit(1);
 }
 
 static int pack_images(char *filename, uint64_t toc_flags)
@@ -538,7 +555,7 @@
 	int i;
 
 	if (argc < 2)
-		usage();
+		create_usage();
 
 	i = fill_common_opts(opts, required_argument);
 	add_opt(opts, i, "plat-toc-flags", required_argument,
@@ -567,14 +584,14 @@
 			parse_plat_toc_flags(optarg, &toc_flags);
 			break;
 		default:
-			usage();
+			create_usage();
 		}
 	}
 	argc -= optind;
 	argv += optind;
 
 	if (argc == 0)
-		usage();
+		create_usage();
 
 	update_fip();
 
@@ -595,6 +612,7 @@
 	for (; toc_entry->cmdline_name != NULL; toc_entry++)
 		printf("  --%-16s FILENAME\t%s\n", toc_entry->cmdline_name,
 		    toc_entry->name);
+	exit(1);
 }
 
 static int update_cmd(int argc, char *argv[])
@@ -607,7 +625,7 @@
 	int i;
 
 	if (argc < 2)
-		usage();
+		update_usage();
 
 	i = fill_common_opts(opts, required_argument);
 	add_opt(opts, i, "out", required_argument, 'o');
@@ -642,14 +660,14 @@
 			snprintf(outfile, sizeof(outfile), "%s", optarg);
 			break;
 		default:
-			usage();
+			update_usage();
 		}
 	}
 	argc -= optind;
 	argv += optind;
 
 	if (argc == 0)
-		usage();
+		update_usage();
 
 	if (outfile[0] == '\0')
 		snprintf(outfile, sizeof(outfile), "%s", argv[0]);
@@ -682,6 +700,7 @@
 	for (; toc_entry->cmdline_name != NULL; toc_entry++)
 		printf("  --%-16s FILENAME\t%s\n", toc_entry->cmdline_name,
 		    toc_entry->name);
+	exit(1);
 }
 
 static int unpack_cmd(int argc, char *argv[])
@@ -694,7 +713,7 @@
 	int i;
 
 	if (argc < 2)
-		usage();
+		unpack_usage();
 
 	i = fill_common_opts(opts, required_argument);
 	add_opt(opts, i, "force", no_argument, 'f');
@@ -724,14 +743,14 @@
 			snprintf(outdir, sizeof(outdir), "%s", optarg);
 			break;
 		default:
-			usage();
+			unpack_usage();
 		}
 	}
 	argc -= optind;
 	argv += optind;
 
 	if (argc == 0)
-		usage();
+		unpack_usage();
 
 	parse_fip(argv[0], NULL);
 
@@ -806,6 +825,7 @@
 		    toc_entry->name);
 	fputc('\n', stderr);
 	printf("If no options are provided, all images will be unpacked.\n");
+	exit(1);
 }
 
 static int remove_cmd(int argc, char *argv[])
@@ -818,7 +838,7 @@
 	int i;
 
 	if (argc < 2)
-		usage();
+		remove_usage();
 
 	i = fill_common_opts(opts, no_argument);
 	add_opt(opts, i, "force", no_argument, 'f');
@@ -844,14 +864,14 @@
 			snprintf(outfile, sizeof(outfile), "%s", optarg);
 			break;
 		default:
-			usage();
+			remove_usage();
 		}
 	}
 	argc -= optind;
 	argv += optind;
 
 	if (argc == 0)
-		usage();
+		remove_usage();
 
 	if (outfile[0] != '\0' && access(outfile, F_OK) == 0 && !fflag)
 		log_errx("File %s already exists, use --force to overwrite it",
@@ -896,6 +916,7 @@
 	for (; toc_entry->cmdline_name != NULL; toc_entry++)
 		printf("  --%-16s\t%s\n", toc_entry->cmdline_name,
 		    toc_entry->name);
+	exit(1);
 }
 
 static int version_cmd(int argc, char *argv[])
@@ -912,6 +933,7 @@
 static void version_usage(void)
 {
 	printf("fiptool version\n");
+	exit(1);
 }
 
 static int help_cmd(int argc, char *argv[])
@@ -924,10 +946,8 @@
 
 	for (i = 0; i < NELEM(cmds); i++) {
 		if (strcmp(cmds[i].name, argv[0]) == 0 &&
-		    cmds[i].usage != NULL) {
+		    cmds[i].usage != NULL)
 			cmds[i].usage();
-			break;
-		}
 	}
 	if (i == NELEM(cmds))
 		printf("No help for subcommand '%s'\n", argv[0]);