Merge pull request #1359 from danielboulby-arm/db/match_flags_type

Ensure read and write of flags defined in the console struct are 32 bit
diff --git a/drivers/arm/gic/v2/gicv2_main.c b/drivers/arm/gic/v2/gicv2_main.c
index e25e501..bbe73fb 100644
--- a/drivers/arm/gic/v2/gicv2_main.c
+++ b/drivers/arm/gic/v2/gicv2_main.c
@@ -178,9 +178,6 @@
 #pragma GCC diagnostic push
 #pragma GCC diagnostic ignored "-Wdeprecated-declarations"
 
-		/* The platform should provide a list of secure interrupts */
-		assert(plat_driver_data->g0_interrupt_array);
-
 		/*
 		 * If there are no interrupts of a particular type, then the
 		 * number of interrupts of that type should be 0 and vice-versa.
@@ -195,8 +192,8 @@
 		WARN("Please migrate to using an interrupt_prop_t array\n");
 	}
 #else
-	assert(plat_driver_data->interrupt_props != NULL);
-	assert(plat_driver_data->interrupt_props_num > 0);
+	assert(plat_driver_data->interrupt_props_num > 0 ?
+			plat_driver_data->interrupt_props != NULL : 1);
 #endif
 
 	/* Ensure that this is a GICv2 system */
diff --git a/drivers/arm/gic/v3/gicv3_helpers.c b/drivers/arm/gic/v3/gicv3_helpers.c
index 69c6951..020ec1b 100644
--- a/drivers/arm/gic/v3/gicv3_helpers.c
+++ b/drivers/arm/gic/v3/gicv3_helpers.c
@@ -433,8 +433,7 @@
 	unsigned int ctlr_enable = 0;
 
 	/* Make sure there's a valid property array */
-	assert(interrupt_props != NULL);
-	assert(interrupt_props_num > 0);
+	assert(interrupt_props_num > 0 ? interrupt_props != NULL : 1);
 
 	for (i = 0; i < interrupt_props_num; i++) {
 		current_prop = &interrupt_props[i];
@@ -556,8 +555,7 @@
 	unsigned int ctlr_enable = 0;
 
 	/* Make sure there's a valid property array */
-	assert(interrupt_props != NULL);
-	assert(interrupt_props_num > 0);
+	assert(interrupt_props_num > 0 ? interrupt_props != NULL : 1);
 
 	for (i = 0; i < interrupt_props_num; i++) {
 		current_prop = &interrupt_props[i];
diff --git a/drivers/arm/gic/v3/gicv3_main.c b/drivers/arm/gic/v3/gicv3_main.c
index d8fc7d6..82f43d0 100644
--- a/drivers/arm/gic/v3/gicv3_main.c
+++ b/drivers/arm/gic/v3/gicv3_main.c
@@ -103,8 +103,8 @@
 		WARN("Please migrate to using interrupt_prop_t arrays\n");
 	}
 #else
-	assert(plat_driver_data->interrupt_props != NULL);
-	assert(plat_driver_data->interrupt_props_num > 0);
+	assert(plat_driver_data->interrupt_props_num > 0 ?
+	       plat_driver_data->interrupt_props != NULL : 1);
 #endif
 
 	/* Check for system register support */
diff --git a/plat/arm/css/common/css_topology.c b/plat/arm/css/common/css_topology.c
index bccf2c8..42f9455 100644
--- a/plat/arm/css/common/css_topology.c
+++ b/plat/arm/css/common/css_topology.c
@@ -6,6 +6,7 @@
 
 #include <plat_arm.h>
 #include <platform.h>
+#include <assert.h>
 
 #if ARM_PLAT_MT
 #pragma weak plat_arm_get_cpu_pe_count
@@ -19,9 +20,12 @@
  *****************************************************************************/
 int plat_core_pos_by_mpidr(u_register_t mpidr)
 {
-	if (arm_check_mpidr(mpidr) == 0)
+	if (arm_check_mpidr(mpidr) == 0) {
+#if ARM_PLAT_MT
+		assert((read_mpidr_el1() & MPIDR_MT_MASK) != 0);
+#endif
 		return plat_arm_calc_core_pos(mpidr);
-
+	}
 	return -1;
 }
 
diff --git a/plat/arm/css/sgi/aarch64/sgi_helper.S b/plat/arm/css/sgi/aarch64/sgi_helper.S
index c435d8b..aaa5156 100644
--- a/plat/arm/css/sgi/aarch64/sgi_helper.S
+++ b/plat/arm/css/sgi/aarch64/sgi_helper.S
@@ -31,19 +31,37 @@
 endfunc plat_is_my_cpu_primary
 
 	/* -----------------------------------------------------
-	*  unsigned int plat_arm_calc_core_pos(uint64_t mpidr)
-	*  Helper function to calculate the core position.
-	* -----------------------------------------------------
-	*/
+	 * unsigned int plat_arm_calc_core_pos(u_register_t mpidr)
+	 *
+	 * Helper function to calculate the core position.
+	 * (ClusterId * CSS_SGI_MAX_CPUS_PER_CLUSTER * CSS_SGI_MAX_PE_PER_CPU) +
+	 * (CPUId * CSS_SGI_MAX_PE_PER_CPU) +
+	 * ThreadId
+	 *
+	 * which can be simplified as:
+	 *
+	 * ((ClusterId * CSS_SGI_MAX_CPUS_PER_CLUSTER + CPUId) *
+	 * CSS_SGI_MAX_PE_PER_CPU) + ThreadId
+	 * ------------------------------------------------------
+	 */
+
 func plat_arm_calc_core_pos
-	mrs     x2, mpidr_el1
-	ands    x2, x2, #MPIDR_MT_MASK
-	beq     1f
-	lsr     x0, x0, #MPIDR_AFF1_SHIFT
-1:
-	and	x1, x0, #MPIDR_CPU_MASK
-	and	x0, x0, #MPIDR_CLUSTER_MASK
-	add	x0, x1, x0, LSR #6
-	and     x0, x0, #MPIDR_AFFLVL_MASK
+	mov	x3, x0
+
+	/*
+	 * The MT bit in MPIDR is always set for SGI platforms
+	 * and the affinity level 0 corresponds to thread affinity level.
+	 */
+
+	/* Extract individual affinity fields from MPIDR */
+	ubfx    x0, x3, #MPIDR_AFF0_SHIFT, #MPIDR_AFFINITY_BITS
+	ubfx    x1, x3, #MPIDR_AFF1_SHIFT, #MPIDR_AFFINITY_BITS
+	ubfx    x2, x3, #MPIDR_AFF2_SHIFT, #MPIDR_AFFINITY_BITS
+
+	/* Compute linear position */
+	mov     x4, #CSS_SGI_MAX_CPUS_PER_CLUSTER
+	madd    x1, x2, x4, x1
+	mov     x5, #CSS_SGI_MAX_PE_PER_CPU
+	madd    x0, x1, x5, x0
 	ret
 endfunc plat_arm_calc_core_pos
diff --git a/plat/arm/css/sgi/include/platform_def.h b/plat/arm/css/sgi/include/platform_def.h
index 49a33ad..0f02407 100644
--- a/plat/arm/css/sgi/include/platform_def.h
+++ b/plat/arm/css/sgi/include/platform_def.h
@@ -14,12 +14,14 @@
 #include <css_def.h>
 #include <soc_css_def.h>
 
-#define CSS_SGI_MAX_CORES_PER_CLUSTER	4
+#define CSS_SGI_MAX_CPUS_PER_CLUSTER	4
 
 /* CPU topology */
 #define PLAT_ARM_CLUSTER_COUNT		2
+#define CSS_SGI_MAX_PE_PER_CPU		1
 #define PLATFORM_CORE_COUNT		(PLAT_ARM_CLUSTER_COUNT *	\
-					CSS_SGI_MAX_CORES_PER_CLUSTER)
+					CSS_SGI_MAX_CPUS_PER_CLUSTER * \
+					CSS_SGI_MAX_PE_PER_CPU)
 
 #if ARM_BOARD_OPTIMISE_MEM
 
diff --git a/plat/arm/css/sgi/sgi-common.mk b/plat/arm/css/sgi/sgi-common.mk
index 659351a..f6ef95a 100644
--- a/plat/arm/css/sgi/sgi-common.mk
+++ b/plat/arm/css/sgi/sgi-common.mk
@@ -19,7 +19,6 @@
 				drivers/arm/gic/v3/gicv3_helpers.c	\
 				plat/common/plat_gicv3.c		\
 				plat/arm/common/arm_gicv3.c		\
-				${CSS_ENT_BASE}/sgi_gic_config.c	\
 				drivers/arm/gic/v3/gic600.c
 
 
diff --git a/plat/arm/css/sgi/sgi_gic_config.c b/plat/arm/css/sgi/sgi_gic_config.c
deleted file mode 100644
index dfccc1b..0000000
--- a/plat/arm/css/sgi/sgi_gic_config.c
+++ /dev/null
@@ -1,18 +0,0 @@
-/*
- * Copyright (c) 2018, ARM Limited and Contributors. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-#include <sgi_plat_config.h>
-
-void plat_arm_gic_driver_init(void)
-{
-	/*
-	 * The GICv3 driver is initialized in EL3 and does not need
-	 * to be initialized again in S-EL1. This is because the S-EL1
-	 * can use GIC system registers to manage interrupts and does
-	 * not need GIC interface base addresses to be configured.
-	 */
-	gicv3_driver_init(get_plat_config()->gic_data);
-}
diff --git a/plat/arm/css/sgi/sgi_plat_config.c b/plat/arm/css/sgi/sgi_plat_config.c
index 96d31e2..29b99a3 100644
--- a/plat/arm/css/sgi/sgi_plat_config.c
+++ b/plat/arm/css/sgi/sgi_plat_config.c
@@ -18,23 +18,6 @@
 /* The GICv3 driver only needs to be initialized in EL3 */
 uintptr_t rdistif_base_addrs[PLATFORM_CORE_COUNT];
 
-const interrupt_prop_t sgi575_interrupt_props[] = {
-	CSS_G1S_IRQ_PROPS(INTR_GROUP1S),
-	ARM_G0_IRQ_PROPS(INTR_GROUP0),
-};
-
-/* Special definition for SGI575 */
-/* GIC configuration for SGI575 */
-const gicv3_driver_data_t sgi575_gic_data = {
-		.gicd_base = PLAT_ARM_GICD_BASE,
-		.gicr_base = PLAT_ARM_GICR_BASE,
-		.interrupt_props = sgi575_interrupt_props,
-		.interrupt_props_num = ARRAY_SIZE(sgi575_interrupt_props),
-		.rdistif_num = PLATFORM_CORE_COUNT,
-		.rdistif_base_addrs = rdistif_base_addrs,
-		.mpidr_to_core_pos = plat_arm_calc_core_pos
-		};
-
 /* Interconnect configuration for SGI575 */
 const css_inteconn_config_t sgi575_inteconn = {
 	.ip_type = ARM_CMN,
@@ -43,7 +26,6 @@
 
 /* Configuration structure for SGI575 */
 css_plat_config_t sgi575_config = {
-	.gic_data = &sgi575_gic_data,
 	.inteconn = &sgi575_inteconn,
 };
 
diff --git a/plat/arm/css/sgi/sgi_topology.c b/plat/arm/css/sgi/sgi_topology.c
index 2136591..1d2e027 100644
--- a/plat/arm/css/sgi/sgi_topology.c
+++ b/plat/arm/css/sgi/sgi_topology.c
@@ -16,14 +16,14 @@
  */
 const unsigned char sgi_pd_tree_desc[] = {
 	PLAT_ARM_CLUSTER_COUNT,
-	CSS_SGI_MAX_CORES_PER_CLUSTER,
-	CSS_SGI_MAX_CORES_PER_CLUSTER
+	CSS_SGI_MAX_CPUS_PER_CLUSTER,
+	CSS_SGI_MAX_CPUS_PER_CLUSTER
 };
 
 /* Topology configuration for sgi platform */
 const css_topology_t sgi_topology = {
 	.power_tree = sgi_pd_tree_desc,
-	.plat_cluster_core_count = CSS_SGI_MAX_CORES_PER_CLUSTER
+	.plat_cluster_core_count = CSS_SGI_MAX_CPUS_PER_CLUSTER
 };
 
 /*******************************************************************************
diff --git a/plat/rockchip/rk3399/drivers/dram/dfs.c b/plat/rockchip/rk3399/drivers/dram/dfs.c
index 70d9423..e6d39a1 100644
--- a/plat/rockchip/rk3399/drivers/dram/dfs.c
+++ b/plat/rockchip/rk3399/drivers/dram/dfs.c
@@ -1964,9 +1964,6 @@
 
 static void m0_configure_ddr(struct pll_div pll_div, uint32_t ddr_index)
 {
-	/* set PARAM to M0_FUNC_DRAM */
-	mmio_write_32(M0_PARAM_ADDR + PARAM_M0_FUNC, M0_FUNC_DRAM);
-
 	mmio_write_32(M0_PARAM_ADDR + PARAM_DPLL_CON0, FBDIV(pll_div.fbdiv));
 	mmio_write_32(M0_PARAM_ADDR + PARAM_DPLL_CON1,
 		      POSTDIV2(pll_div.postdiv2) | POSTDIV1(pll_div.postdiv1) |
@@ -1976,6 +1973,7 @@
 
 	mmio_write_32(M0_PARAM_ADDR + PARAM_FREQ_SELECT, ddr_index << 4);
 	dmbst();
+	m0_configure_execute_addr(M0_BINCODE_BASE);
 }
 
 static uint32_t prepare_ddr_timing(uint32_t mhz)
diff --git a/plat/rockchip/rk3399/drivers/dram/suspend.c b/plat/rockchip/rk3399/drivers/dram/suspend.c
index 2a80bcb..dd97581 100644
--- a/plat/rockchip/rk3399/drivers/dram/suspend.c
+++ b/plat/rockchip/rk3399/drivers/dram/suspend.c
@@ -9,6 +9,8 @@
 #include <dram.h>
 #include <plat_private.h>
 #include <platform_def.h>
+#include <pmu.h>
+#include <pmu_bits.h>
 #include <pmu_regs.h>
 #include <rk3399_def.h>
 #include <secure.h>
@@ -656,6 +658,30 @@
 		;
 }
 
+__pmusramfunc static void pmusram_enable_watchdog(void)
+{
+	/* Make the watchdog use the first global reset. */
+	mmio_write_32(CRU_BASE + CRU_GLB_RST_CON, 1 << 1);
+
+	/*
+	 * This gives the system ~8 seconds before reset. The pclk for the
+	 * watchdog is 4MHz on reset. The value of 0x9 in WDT_TORR means that
+	 * the watchdog will wait for 0x1ffffff cycles before resetting.
+	 */
+	mmio_write_32(WDT0_BASE + 4, 0x9);
+
+	/* Enable the watchdog */
+	mmio_setbits_32(WDT0_BASE, 0x1);
+
+	/* Magic reset the watchdog timer value for WDT_CRR. */
+	mmio_write_32(WDT0_BASE + 0xc, 0x76);
+
+	secure_watchdog_ungate();
+
+	/* The watchdog is in PD_ALIVE, so deidle it. */
+	mmio_clrbits_32(PMU_BASE + PMU_BUS_CLR, PMU_CLR_ALIVE);
+}
+
 void dmc_suspend(void)
 {
 	struct rk3399_sdram_params *sdram_params = &sdram_config;
@@ -726,6 +752,9 @@
 	uint32_t channel_mask = 0;
 	uint32_t channel;
 
+	pmusram_enable_watchdog();
+	pmu_sgrf_rst_hld_release();
+	restore_pmu_rsthold();
 	sram_secure_timer_init();
 
 	/*
diff --git a/plat/rockchip/rk3399/drivers/m0/Makefile b/plat/rockchip/rk3399/drivers/m0/Makefile
index f6bdbf2..79e09f0 100644
--- a/plat/rockchip/rk3399/drivers/m0/Makefile
+++ b/plat/rockchip/rk3399/drivers/m0/Makefile
@@ -12,6 +12,7 @@
 
 # Build platform
 PLAT_M0		?= rk3399m0
+PLAT_M0_PMU	?= rk3399m0pmu
 
 ifeq (${V},0)
 	Q=@
@@ -26,11 +27,10 @@
 			   -I../../include/shared/
 
 # NOTE: Add C source files here
-C_SOURCES		:= src/startup.c \
-			   src/main.c	\
-			   src/suspend.c \
-			   src/dram.c	\
+C_SOURCES_COMMON	:= src/startup.c
+C_SOURCES		:= src/dram.c	\
 			   src/stopwatch.c
+C_SOURCES_PMU		:= src/suspend.c
 
 # Flags definition
 COMMON_FLAGS		:= -g -mcpu=$(ARCH) -mthumb -Wall -O3 -nostdlib -mfloat-abi=soft
@@ -54,12 +54,19 @@
 	$(notdir $(patsubst %.S,%.o,$(filter %.S,$(1))))
 endef
 
+SOURCES_COMMON		:= $(C_SOURCES_COMMON)
 SOURCES 		:= $(C_SOURCES)
+SOURCES_PMU		:= $(C_SOURCES_PMU)
+OBJS_COMMON		:= $(addprefix $(BUILD)/,$(call SOURCES_TO_OBJS,$(SOURCES_COMMON)))
 OBJS 			:= $(addprefix $(BUILD)/,$(call SOURCES_TO_OBJS,$(SOURCES)))
+OBJS_PMU 		:= $(addprefix $(BUILD)/,$(call SOURCES_TO_OBJS,$(SOURCES_PMU)))
 LINKERFILE		:= $(BUILD)/$(PLAT_M0).ld
 MAPFILE			:= $(BUILD)/$(PLAT_M0).map
+MAPFILE_PMU		:= $(BUILD)/$(PLAT_M0_PMU).map
 ELF 			:= $(BUILD)/$(PLAT_M0).elf
+ELF_PMU			:= $(BUILD)/$(PLAT_M0_PMU).elf
 BIN 			:= $(BUILD)/$(PLAT_M0).bin
+BIN_PMU			:= $(BUILD)/$(PLAT_M0_PMU).bin
 LINKERFILE_SRC		:= src/$(PLAT_M0).ld.S
 
 # Function definition related compilation
@@ -92,18 +99,27 @@
 	$(and $(REMAIN),$(error Unexpected source files present: $(REMAIN)))
 endef
 
-.DEFAULT_GOAL := $(BIN)
+.PHONY: all
+all: $(BIN) $(BIN_PMU)
+
+.DEFAULT_GOAL := all
 
 $(LINKERFILE): $(LINKERFILE_SRC)
 	$(CC) $(COMMON_FLAGS) $(INCLUDES) -P -E -D__LINKER__ -MMD -MF $@.d -MT $@ -o $@ $<
 -include $(LINKERFILE).d
 
-$(ELF) : $(OBJS) $(LINKERFILE)
+$(ELF) : $(OBJS) $(OBJS_COMMON) $(LINKERFILE)
 	@echo "  LD      $@"
-	$(Q)$(CC) -o $@ $(COMMON_FLAGS) $(LDFLAGS) -Wl,-Map=$(MAPFILE) -Wl,-T$(LINKERFILE) $(OBJS)
+	$(Q)$(CC) -o $@ $(COMMON_FLAGS) $(LDFLAGS) -Wl,-Map=$(MAPFILE) -Wl,-T$(LINKERFILE) $(OBJS) $(OBJS_COMMON)
 
-$(BIN) : $(ELF)
+%.bin : %.elf
 	@echo "  BIN     $@"
 	$(Q)$(OC) -O binary $< $@
 
+$(ELF_PMU) : $(OBJS_COMMON) $(OBJS_PMU) $(LINKERFILE)
+	@echo "  LD      $@"
+	$(Q)$(CC) -o $@ $(COMMON_FLAGS) $(LDFLAGS) -Wl,-Map=$(MAPFILE_PMU) -Wl,-T$(LINKERFILE) $(OBJS_PMU) $(OBJS_COMMON)
+
+$(eval $(call MAKE_OBJS,$(BUILD),$(SOURCES_COMMON),$(1)))
 $(eval $(call MAKE_OBJS,$(BUILD),$(SOURCES),$(1)))
+$(eval $(call MAKE_OBJS,$(BUILD),$(SOURCES_PMU),$(1)))
diff --git a/plat/rockchip/rk3399/drivers/m0/include/rk3399_mcu.h b/plat/rockchip/rk3399/drivers/m0/include/rk3399_mcu.h
index 472cbc9..176af3a 100644
--- a/plat/rockchip/rk3399/drivers/m0/include/rk3399_mcu.h
+++ b/plat/rockchip/rk3399/drivers/m0/include/rk3399_mcu.h
@@ -25,8 +25,6 @@
 #define MIN(a, b) ((a) < (b) ? (a) : (b))
 #define MAX(a, b) ((a) > (b) ? (a) : (b))
 
-void handle_suspend(void);
-void handle_dram(void);
 void stopwatch_init_usecs_expire(unsigned int usecs);
 int stopwatch_expired(void);
 void stopwatch_reset(void);
diff --git a/plat/rockchip/rk3399/drivers/m0/src/dram.c b/plat/rockchip/rk3399/drivers/m0/src/dram.c
index c6a9259..b939a96 100644
--- a/plat/rockchip/rk3399/drivers/m0/src/dram.c
+++ b/plat/rockchip/rk3399/drivers/m0/src/dram.c
@@ -55,7 +55,7 @@
 	mmio_write_32(CRU_BASE + CRU_DPLL_CON3, PLL_MODE(PLL_NORMAL_MODE));
 }
 
-void handle_dram(void)
+__attribute__((noreturn)) void main(void)
 {
 	mmio_setbits_32(PHY_REG(0, 927), (1 << 22));
 	mmio_setbits_32(PHY_REG(1, 927), (1 << 22));
@@ -76,4 +76,9 @@
 	deidle_port();
 	mmio_clrbits_32(PHY_REG(0, 927), (1 << 22));
 	mmio_clrbits_32(PHY_REG(1, 927), (1 << 22));
+
+	mmio_write_32(PARAM_ADDR + PARAM_M0_DONE, M0_DONE_FLAG);
+
+	for (;;)
+		__asm__ volatile ("wfi");
 }
diff --git a/plat/rockchip/rk3399/drivers/m0/src/main.c b/plat/rockchip/rk3399/drivers/m0/src/main.c
deleted file mode 100644
index 0ed818d..0000000
--- a/plat/rockchip/rk3399/drivers/m0/src/main.c
+++ /dev/null
@@ -1,27 +0,0 @@
-/*
- * Copyright (c) 2016, ARM Limited and Contributors. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-#include <m0_param.h>
-#include "rk3399_mcu.h"
-
-__attribute__((noreturn)) void main(void)
-{
-	switch (mmio_read_32(PARAM_ADDR + PARAM_M0_FUNC)) {
-	case M0_FUNC_SUSPEND:
-		handle_suspend();
-		break;
-	case M0_FUNC_DRAM:
-		handle_dram();
-		break;
-	default:
-		break;
-	}
-
-	mmio_write_32(PARAM_ADDR + PARAM_M0_DONE, M0_DONE_FLAG);
-
-	for (;;)
-		__asm__ volatile ("wfi");
-}
diff --git a/plat/rockchip/rk3399/drivers/m0/src/startup.c b/plat/rockchip/rk3399/drivers/m0/src/startup.c
index 68f5b2d..dba0313 100644
--- a/plat/rockchip/rk3399/drivers/m0/src/startup.c
+++ b/plat/rockchip/rk3399/drivers/m0/src/startup.c
@@ -7,7 +7,7 @@
 #include "rk3399_mcu.h"
 
 /* Stack configuration */
-#define STACK_SIZE	0x00000100
+#define STACK_SIZE	0x00000040
 __attribute__ ((section(".co_stack")))
 unsigned long pstack[STACK_SIZE];
 
diff --git a/plat/rockchip/rk3399/drivers/m0/src/suspend.c b/plat/rockchip/rk3399/drivers/m0/src/suspend.c
index af29a11..39dfd11 100644
--- a/plat/rockchip/rk3399/drivers/m0/src/suspend.c
+++ b/plat/rockchip/rk3399/drivers/m0/src/suspend.c
@@ -11,18 +11,52 @@
 
 #define SCR_SLEEPDEEP_SHIFT	(1 << 2)
 
-void handle_suspend(void)
+__attribute__((noreturn)) void main(void)
 {
 	unsigned int status_value;
 
+	/*
+	 * PMU sometimes doesn't clear power mode bit as it's supposed to due
+	 * to a hardware bug. Make the M0 clear it manually to be sure,
+	 * otherwise interrupts some cases with concurrent wake interrupts
+	 * we stay asleep forever.
+	 */
 	while (1) {
 		status_value = mmio_read_32(PMU_BASE + PMU_POWER_ST);
 		if (status_value) {
 			mmio_clrbits_32(PMU_BASE + PMU_PWRMODE_CON, 0x01);
-			return;
+			break;
 		}
 	}
 
-	/* m0 enter deep sleep mode */
-	mmio_setbits_32(M0_SCR, SCR_SLEEPDEEP_SHIFT);
+	/*
+	 * FSM power secquence is .. -> ST_INPUT_CLAMP(step.17) -> .. ->
+	 * ST_WAKEUP_RESET -> ST_EXT_PWRUP-> ST_RELEASE_CLAMP ->
+	 * ST_24M_OSC_EN -> .. -> ST_WAKEUP_RESET_CLR(step.26) -> ..,
+	 * INPUT_CLAMP and WAKEUP_RESET will hold the SOC not affect by
+	 * power or other single glitch, but WAKEUP_RESET need work with 24MHz,
+	 * so between RELEASE_CLAMP and 24M_OSC_EN, there have a chance
+	 * that glitch will affect SOC, and mess up SOC status, so we
+	 * addressmap_shared software clamp between ST_INPUT_CLAMP and
+	 * ST_WAKEUP_RESET_CLR to avoid this happen.
+	 */
+	while (1) {
+		status_value = mmio_read_32(PMU_BASE + PMU_POWER_ST);
+		if (status_value >= 17)  {
+			mmio_setbits_32(PMU_BASE + PMU_SFT_CON, 0x02);
+			break;
+		}
+
+	}
+
+	while (1) {
+		status_value = mmio_read_32(PMU_BASE + PMU_POWER_ST);
+		if (status_value >= 26)  {
+			mmio_clrbits_32(PMU_BASE + PMU_SFT_CON, 0x02);
+			break;
+		}
+	}
+
+	for (;;)
+		__asm__ volatile ("wfi");
 }
diff --git a/plat/rockchip/rk3399/drivers/pmu/m0_ctl.c b/plat/rockchip/rk3399/drivers/pmu/m0_ctl.c
index 61849e5..3f258b7 100644
--- a/plat/rockchip/rk3399/drivers/pmu/m0_ctl.c
+++ b/plat/rockchip/rk3399/drivers/pmu/m0_ctl.c
@@ -21,14 +21,6 @@
 	mmio_write_32(SGRF_BASE + SGRF_PMU_CON(0), WMSK_BIT(7));
 	mmio_write_32(SGRF_BASE + SGRF_SOC_CON(6), WMSK_BIT(12));
 
-	/* set the execute address for M0 */
-	mmio_write_32(SGRF_BASE + SGRF_PMU_CON(3),
-		      BITS_WITH_WMASK((M0_BINCODE_BASE >> 12) & 0xffff,
-				      0xffff, 0));
-	mmio_write_32(SGRF_BASE + SGRF_PMU_CON(7),
-		      BITS_WITH_WMASK((M0_BINCODE_BASE >> 28) & 0xf,
-				      0xf, 0));
-
 	/* document is wrong, PMU_CRU_GATEDIS_CON0 do not need set MASK BIT */
 	mmio_setbits_32(PMUCRU_BASE + PMUCRU_GATEDIS_CON0, 0x02);
 
@@ -46,6 +38,17 @@
 	mmio_write_32(PMUCRU_BASE + PMUCRU_CLKGATE_CON2, WMSK_BIT(5));
 }
 
+void m0_configure_execute_addr(uintptr_t addr)
+{
+	/* set the execute address for M0 */
+	mmio_write_32(SGRF_BASE + SGRF_PMU_CON(3),
+		      BITS_WITH_WMASK((addr >> 12) & 0xffff,
+				      0xffff, 0));
+	mmio_write_32(SGRF_BASE + SGRF_PMU_CON(7),
+		      BITS_WITH_WMASK((addr >> 28) & 0xf,
+				      0xf, 0));
+}
+
 void m0_start(void)
 {
 	/* enable clocks for M0 */
diff --git a/plat/rockchip/rk3399/drivers/pmu/m0_ctl.h b/plat/rockchip/rk3399/drivers/pmu/m0_ctl.h
index b313ec6..cb323c4 100644
--- a/plat/rockchip/rk3399/drivers/pmu/m0_ctl.h
+++ b/plat/rockchip/rk3399/drivers/pmu/m0_ctl.h
@@ -11,13 +11,18 @@
 
 #define M0_BINCODE_BASE 	((uintptr_t)rk3399m0_bin)
 #define M0_PARAM_ADDR		(M0_BINCODE_BASE + PARAM_ADDR)
+#define M0PMU_BINCODE_BASE	((uintptr_t)rk3399m0pmu_bin)
 
 /* pmu_fw.c */
 extern char rk3399m0_bin[];
 extern char rk3399m0_bin_end[];
 
+extern char rk3399m0pmu_bin[];
+extern char rk3399m0pmu_bin_end[];
+
 extern void m0_init(void);
 extern void m0_start(void);
 extern void m0_stop(void);
 extern void m0_wait_done(void);
+extern void m0_configure_execute_addr(uintptr_t addr);
 #endif /* __M0_CTL_H__ */
diff --git a/plat/rockchip/rk3399/drivers/pmu/pmu.c b/plat/rockchip/rk3399/drivers/pmu/pmu.c
index ed1ea8b..04446d6 100644
--- a/plat/rockchip/rk3399/drivers/pmu/pmu.c
+++ b/plat/rockchip/rk3399/drivers/pmu/pmu.c
@@ -836,6 +836,7 @@
 		      BIT_WITH_WMSK(PMU_CLR_GIC2_CORE_L_HW));
 
 	slp_mode_cfg = BIT(PMU_PWR_MODE_EN) |
+		       BIT(PMU_WKUP_RST_EN) |
 		       BIT(PMU_INPUT_CLAMP_EN) |
 		       BIT(PMU_POWER_OFF_REQ_CFG) |
 		       BIT(PMU_CPU0_PD_EN) |
@@ -853,7 +854,6 @@
 		       BIT(PMU_DDRIO0_RET_DE_REQ) |
 		       BIT(PMU_DDRIO1_RET_EN) |
 		       BIT(PMU_DDRIO1_RET_DE_REQ) |
-		       BIT(PMU_DDRIO_RET_HW_DE_REQ) |
 		       BIT(PMU_CENTER_PD_EN) |
 		       BIT(PMU_PERILP_PD_EN) |
 		       BIT(PMU_CLK_PERILP_SRC_GATE_EN) |
@@ -1064,12 +1064,6 @@
 	}
 }
 
-static void m0_configure_suspend(void)
-{
-	/* set PARAM to M0_FUNC_SUSPEND */
-	mmio_write_32(M0_PARAM_ADDR + PARAM_M0_FUNC, M0_FUNC_SUSPEND);
-}
-
 void sram_save(void)
 {
 	size_t text_size = (char *)&__bl31_sram_text_real_end -
@@ -1344,7 +1338,7 @@
 	set_pmu_rsthold();
 	sys_slp_config();
 
-	m0_configure_suspend();
+	m0_configure_execute_addr(M0PMU_BINCODE_BASE);
 	m0_start();
 
 	pmu_sgrf_rst_hld();
@@ -1374,7 +1368,7 @@
 	mmio_setbits_32(PMU_BASE + PMU_PWRDN_CON, BIT(PMU_SCU_B_PWRDWN_EN));
 
 	wdt_register_save();
-	secure_watchdog_disable();
+	secure_watchdog_gate();
 
 	/*
 	 * Disabling PLLs/PWM/DVFS is approaching WFI which is
@@ -1403,6 +1397,7 @@
 	plat_rockchip_restore_gpio();
 	cru_register_restore();
 	grf_register_restore();
+	wdt_register_restore();
 	resume_uart();
 	resume_apio();
 	resume_gpio();
@@ -1412,10 +1407,8 @@
 	udelay(300);
 	enable_dvfs_plls();
 
-	secure_watchdog_enable();
 	secure_sgrf_init();
 	secure_sgrf_ddr_rgn_init();
-	wdt_register_restore();
 
 	/* restore clk_ddrc_bpll_src_en gate */
 	mmio_write_32(CRU_BASE + CRU_CLKGATE_CON(3),
@@ -1466,12 +1459,10 @@
 		udelay(1);
 	}
 
-	pmu_sgrf_rst_hld_release();
 	pmu_scu_b_pwrup();
 	pmu_power_domains_resume();
 
 	restore_abpll();
-	restore_pmu_rsthold();
 	clr_hw_idle(BIT(PMU_CLR_CENTER1) |
 				BIT(PMU_CLR_ALIVE) |
 				BIT(PMU_CLR_MSCH0) |
diff --git a/plat/rockchip/rk3399/drivers/pmu/pmu_fw.c b/plat/rockchip/rk3399/drivers/pmu/pmu_fw.c
index d299116..a09ad21 100644
--- a/plat/rockchip/rk3399/drivers/pmu/pmu_fw.c
+++ b/plat/rockchip/rk3399/drivers/pmu/pmu_fw.c
@@ -5,9 +5,9 @@
  */
 
 /* convoluted way to make sure that the define is pasted just the right way */
-#define _INCBIN(file, sym) \
+#define _INCBIN(file, sym, sec) \
 	__asm__( \
-		".section .sram.incbin\n" \
+		".section " #sec "\n" \
 		".global " #sym "\n" \
 		".type " #sym ", %object\n" \
 		".align 4\n" \
@@ -18,6 +18,7 @@
 		#sym "_end:\n" \
 	)
 
-#define INCBIN(file, sym) _INCBIN(file, sym)
+#define INCBIN(file, sym, sec) _INCBIN(file, sym, sec)
 
-INCBIN(RK3399M0FW, rk3399m0_bin);
+INCBIN(RK3399M0FW, rk3399m0_bin, ".sram.incbin");
+INCBIN(RK3399M0PMUFW, rk3399m0pmu_bin, ".pmusram.incbin");
diff --git a/plat/rockchip/rk3399/drivers/secure/secure.c b/plat/rockchip/rk3399/drivers/secure/secure.c
index 589d833..1937b13 100644
--- a/plat/rockchip/rk3399/drivers/secure/secure.c
+++ b/plat/rockchip/rk3399/drivers/secure/secure.c
@@ -77,7 +77,7 @@
 		      BIT_WITH_WMSK(rgn));
 }
 
-void secure_watchdog_disable(void)
+void secure_watchdog_gate(void)
 {
 	/**
 	 * Disable CA53 and CM0 wdt pclk
@@ -89,7 +89,7 @@
 		      BIT_WITH_WMSK(PCLK_WDT_CM0_GATE_SHIFT));
 }
 
-void secure_watchdog_enable(void)
+__pmusramfunc void secure_watchdog_ungate(void)
 {
 	/**
 	 * Enable CA53 and CM0 wdt pclk
diff --git a/plat/rockchip/rk3399/drivers/secure/secure.h b/plat/rockchip/rk3399/drivers/secure/secure.h
index 334805d..8e80f2b 100644
--- a/plat/rockchip/rk3399/drivers/secure/secure.h
+++ b/plat/rockchip/rk3399/drivers/secure/secure.h
@@ -95,8 +95,8 @@
 #define PCLK_WDT_CM0_GATE_SHIFT		10
 
 /* export secure operating APIs */
-void secure_watchdog_disable(void);
-void secure_watchdog_enable(void);
+void secure_watchdog_gate(void);
+__pmusramfunc void secure_watchdog_ungate(void);
 void secure_timer_init(void);
 void secure_sgrf_init(void);
 void secure_sgrf_ddr_rgn_init(void);
diff --git a/plat/rockchip/rk3399/drivers/soc/soc.c b/plat/rockchip/rk3399/drivers/soc/soc.c
index 7dd0b72..741adde 100644
--- a/plat/rockchip/rk3399/drivers/soc/soc.c
+++ b/plat/rockchip/rk3399/drivers/soc/soc.c
@@ -43,6 +43,9 @@
 /* sleep data for pll suspend */
 static struct deepsleep_data_s slp_data;
 
+/* sleep data that needs to be accessed from pmusram */
+__pmusramdata struct pmu_sleep_data pmu_slp_data;
+
 static void set_pll_slow_mode(uint32_t pll_id)
 {
 	if (pll_id == PPLL_ID)
@@ -229,9 +232,9 @@
 	uint32_t rstnhold_cofig0;
 	uint32_t rstnhold_cofig1;
 
-	slp_data.pmucru_rstnhold_con0 = mmio_read_32(PMUCRU_BASE +
+	pmu_slp_data.pmucru_rstnhold_con0 = mmio_read_32(PMUCRU_BASE +
 					    PMUCRU_RSTNHOLD_CON0);
-	slp_data.pmucru_rstnhold_con1 = mmio_read_32(PMUCRU_BASE +
+	pmu_slp_data.pmucru_rstnhold_con1 = mmio_read_32(PMUCRU_BASE +
 					    PMUCRU_RSTNHOLD_CON1);
 	rstnhold_cofig0 = BIT_WITH_WMSK(PRESETN_NOC_PMU_HOLD) |
 			  BIT_WITH_WMSK(PRESETN_INTMEM_PMU_HOLD) |
@@ -257,12 +260,33 @@
 	mmio_write_32(PMUCRU_BASE + PMUCRU_RSTNHOLD_CON1, rstnhold_cofig1);
 }
 
+void pmu_sgrf_rst_hld(void)
+{
+	mmio_write_32(PMUCRU_BASE + CRU_PMU_RSTHOLD_CON(1),
+		      CRU_PMU_SGRF_RST_HOLD);
+}
+
-void restore_pmu_rsthold(void)
+/*
+ * When system reset in running state, we want the cpus to be reboot
+ * from maskrom (system reboot),
+ * the pmusgrf reset-hold bits needs to be released.
+ * When system wake up from system deep suspend, some soc will be reset
+ * when waked up,
+ * we want the bootcpu to be reboot from pmusram,
+ * the pmusgrf reset-hold bits needs to be held.
+ */
+__pmusramfunc void pmu_sgrf_rst_hld_release(void)
+{
+	mmio_write_32(PMUCRU_BASE + CRU_PMU_RSTHOLD_CON(1),
+		      CRU_PMU_SGRF_RST_RLS);
+}
+
+__pmusramfunc void restore_pmu_rsthold(void)
 {
 	mmio_write_32(PMUCRU_BASE + PMUCRU_RSTNHOLD_CON0,
-		      slp_data.pmucru_rstnhold_con0 | REG_SOC_WMSK);
+		      pmu_slp_data.pmucru_rstnhold_con0 | REG_SOC_WMSK);
 	mmio_write_32(PMUCRU_BASE + PMUCRU_RSTNHOLD_CON1,
-		      slp_data.pmucru_rstnhold_con1 | REG_SOC_WMSK);
+		      pmu_slp_data.pmucru_rstnhold_con1 | REG_SOC_WMSK);
 }
 
 /**
diff --git a/plat/rockchip/rk3399/drivers/soc/soc.h b/plat/rockchip/rk3399/drivers/soc/soc.h
index 6100d95..bc41933 100644
--- a/plat/rockchip/rk3399/drivers/soc/soc.h
+++ b/plat/rockchip/rk3399/drivers/soc/soc.h
@@ -134,6 +134,9 @@
 	uint32_t plls_con[END_PLL_ID][PLL_CON_COUNT];
 	uint32_t cru_gate_con[CRU_GATE_COUNT];
 	uint32_t pmucru_gate_con[PMUCRU_GATE_COUNT];
+};
+
+struct pmu_sleep_data {
 	uint32_t pmucru_rstnhold_con0;
 	uint32_t pmucru_rstnhold_con1;
 };
@@ -263,27 +266,6 @@
 #define PMUCRU_GATEDIS_CON0 0x0130
 #define PMUCRU_SOFTRST_CON(n)   (PMUCRU_SOFTRST_CON0 + (n) * 4)
 
-/*
- * When system reset in running state, we want the cpus to be reboot
- * from maskrom (system reboot),
- * the pmusgrf reset-hold bits needs to be released.
- * When system wake up from system deep suspend, some soc will be reset
- * when waked up,
- * we want the bootcpu to be reboot from pmusram,
- * the pmusgrf reset-hold bits needs to be held.
- */
-static inline void pmu_sgrf_rst_hld_release(void)
-{
-	mmio_write_32(PMUCRU_BASE + CRU_PMU_RSTHOLD_CON(1),
-		      CRU_PMU_SGRF_RST_RLS);
-}
-
-static inline void pmu_sgrf_rst_hld(void)
-{
-	mmio_write_32(PMUCRU_BASE + CRU_PMU_RSTHOLD_CON(1),
-		      CRU_PMU_SGRF_RST_HOLD);
-}
-
 /* export related and operating SoC APIs */
 void __dead2 soc_global_soft_reset(void);
 void disable_dvfs_plls(void);
@@ -296,5 +278,7 @@
 void clk_gate_con_disable(void);
 void clk_gate_con_restore(void);
 void set_pmu_rsthold(void);
-void restore_pmu_rsthold(void);
+void pmu_sgrf_rst_hld(void);
+__pmusramfunc void pmu_sgrf_rst_hld_release(void);
+__pmusramfunc void restore_pmu_rsthold(void);
 #endif /* __SOC_H__ */
diff --git a/plat/rockchip/rk3399/include/plat.ld.S b/plat/rockchip/rk3399/include/plat.ld.S
index 85f4dc3..528c150 100644
--- a/plat/rockchip/rk3399/include/plat.ld.S
+++ b/plat/rockchip/rk3399/include/plat.ld.S
@@ -77,14 +77,21 @@
 		ASSERT(. == ALIGN(64 * 1024),
 			".pmusram.entry request 64K aligned.");
 		*(.pmusram.entry)
+
 		__bl31_pmusram_text_start = .;
 		*(.pmusram.text)
 		*(.pmusram.rodata)
 		__bl31_pmusram_text_end = .;
+
+		/* M0 start address request 4K align */
+		. = ALIGN(4096);
+		__pmusram_incbin_start = .;
+		*(.pmusram.incbin)
+		__pmusram_incbin_end = .;
+
 		__bl31_pmusram_data_start = .;
 		*(.pmusram.data)
 		__bl31_pmusram_data_end = .;
-
 	} >PMUSRAM
 }
 
diff --git a/plat/rockchip/rk3399/include/shared/m0_param.h b/plat/rockchip/rk3399/include/shared/m0_param.h
index 3edbf89..044e797 100644
--- a/plat/rockchip/rk3399/include/shared/m0_param.h
+++ b/plat/rockchip/rk3399/include/shared/m0_param.h
@@ -7,13 +7,6 @@
 #ifndef __M0_PARAM_H__
 #define __M0_PARAM_H__
 
-#ifndef __LINKER__
-enum {
-	M0_FUNC_SUSPEND = 0,
-	M0_FUNC_DRAM	= 1,
-};
-#endif /* __LINKER__ */
-
 #define PARAM_ADDR		0xc0
 
 #define PARAM_M0_FUNC		0x00
diff --git a/plat/rockchip/rk3399/platform.mk b/plat/rockchip/rk3399/platform.mk
index 1997dfc..fc386f0 100644
--- a/plat/rockchip/rk3399/platform.mk
+++ b/plat/rockchip/rk3399/platform.mk
@@ -82,12 +82,15 @@
 RK3399M0FW=${BUILD_M0}/${PLAT_M0}.bin
 $(eval $(call add_define,RK3399M0FW))
 
+RK3399M0PMUFW=${BUILD_M0}/${PLAT_M0}pmu.bin
+$(eval $(call add_define,RK3399M0PMUFW))
+
 HDCPFW=${RK_PLAT_SOC}/drivers/dp/hdcp.bin
 $(eval $(call add_define,HDCPFW))
 
 # CCACHE_EXTRAFILES is needed because ccache doesn't handle .incbin
 export CCACHE_EXTRAFILES
-${BUILD_PLAT}/bl31/pmu_fw.o: CCACHE_EXTRAFILES=$(RK3399M0FW)
+${BUILD_PLAT}/bl31/pmu_fw.o: CCACHE_EXTRAFILES=$(RK3399M0FW):$(RK3399M0PMUFW)
 ${RK_PLAT_SOC}/drivers/pmu/pmu_fw.c: $(RK3399M0FW)
 
 ${BUILD_PLAT}/bl31/cdn_dp.o: CCACHE_EXTRAFILES=$(HDCPFW)
diff --git a/plat/xilinx/zynqmp/aarch64/zynqmp_common.c b/plat/xilinx/zynqmp/aarch64/zynqmp_common.c
index fd054be..b144c84 100644
--- a/plat/xilinx/zynqmp/aarch64/zynqmp_common.c
+++ b/plat/xilinx/zynqmp/aarch64/zynqmp_common.c
@@ -8,8 +8,11 @@
 #include <generic_delay_timer.h>
 #include <mmio.h>
 #include <platform.h>
+#include <stdbool.h>
+#include <string.h>
 #include <xlat_tables.h>
 #include "../zynqmp_private.h"
+#include "pm_api_sys.h"
 
 /*
  * Table of regions to map using the MMU.
@@ -59,40 +62,103 @@
 #if LOG_LEVEL >= LOG_LEVEL_NOTICE
 static const struct {
 	unsigned int id;
+	unsigned int ver;
 	char *name;
+	bool evexists;
 } zynqmp_devices[] = {
 	{
 		.id = 0x10,
 		.name = "3EG",
 	},
 	{
+		.id = 0x10,
+		.ver = 0x2c,
+		.name = "3CG",
+	},
+	{
 		.id = 0x11,
 		.name = "2EG",
 	},
 	{
+		.id = 0x11,
+		.ver = 0x2c,
+		.name = "2CG",
+	},
+	{
 		.id = 0x20,
 		.name = "5EV",
+		.evexists = true,
+	},
+	{
+		.id = 0x20,
+		.ver = 0x100,
+		.name = "5EG",
+		.evexists = true,
 	},
 	{
+		.id = 0x20,
+		.ver = 0x12c,
+		.name = "5CG",
+	},
+	{
 		.id = 0x21,
 		.name = "4EV",
+		.evexists = true,
+	},
+	{
+		.id = 0x21,
+		.ver = 0x100,
+		.name = "4EG",
+		.evexists = true,
+	},
+	{
+		.id = 0x21,
+		.ver = 0x12c,
+		.name = "4CG",
 	},
 	{
 		.id = 0x30,
 		.name = "7EV",
+		.evexists = true,
 	},
 	{
+		.id = 0x30,
+		.ver = 0x100,
+		.name = "7EG",
+		.evexists = true,
+	},
+	{
+		.id = 0x30,
+		.ver = 0x12c,
+		.name = "7CG",
+	},
+	{
 		.id = 0x38,
 		.name = "9EG",
 	},
 	{
+		.id = 0x38,
+		.ver = 0x2c,
+		.name = "9CG",
+	},
+	{
 		.id = 0x39,
 		.name = "6EG",
 	},
 	{
+		.id = 0x39,
+		.ver = 0x2c,
+		.name = "6CG",
+	},
+	{
 		.id = 0x40,
 		.name = "11EG",
 	},
+	{ /* For testing purpose only */
+		.id = 0x50,
+		.ver = 0x2c,
+		.name = "15CG",
+	},
 	{
 		.id = 0x50,
 		.name = "15EG",
@@ -105,30 +171,75 @@
 		.id = 0x59,
 		.name = "17EG",
 	},
+	{
+		.id = 0x60,
+		.name = "28DR",
+	},
+	{
+		.id = 0x61,
+		.name = "21DR",
+	},
+	{
+		.id = 0x62,
+		.name = "29DR",
+	},
+	{
+		.id = 0x63,
+		.name = "23DR",
+	},
+	{
+		.id = 0x64,
+		.name = "27DR",
+	},
+	{
+		.id = 0x65,
+		.name = "25DR",
+	},
 };
 
+#define ZYNQMP_PL_STATUS_BIT	9
+#define ZYNQMP_PL_STATUS_MASK	BIT(ZYNQMP_PL_STATUS_BIT)
+#define ZYNQMP_CSU_VERSION_MASK	~(ZYNQMP_PL_STATUS_MASK)
+
-static unsigned int zynqmp_get_silicon_id(void)
+static char *zynqmp_get_silicon_idcode_name(void)
 {
-	uint32_t id;
+	uint32_t id, ver, chipid[2];
+	size_t i, j, len;
+	enum pm_ret_status ret;
+	const char *name = "EG/EV";
 
-	id = mmio_read_32(ZYNQMP_CSU_BASEADDR + ZYNQMP_CSU_IDCODE_OFFSET);
+	ret = pm_get_chipid(chipid);
+	if (ret)
+		return "UNKN";
 
-	id &= ZYNQMP_CSU_IDCODE_DEVICE_CODE_MASK | ZYNQMP_CSU_IDCODE_SVD_MASK;
+	id = chipid[0] & (ZYNQMP_CSU_IDCODE_DEVICE_CODE_MASK |
+			  ZYNQMP_CSU_IDCODE_SVD_MASK);
 	id >>= ZYNQMP_CSU_IDCODE_SVD_SHIFT;
+	ver = chipid[1] >> ZYNQMP_EFUSE_IPDISABLE_SHIFT;
 
-	return id;
-}
+	for (i = 0; i < ARRAY_SIZE(zynqmp_devices); i++) {
+		if (zynqmp_devices[i].id == id &&
+		    zynqmp_devices[i].ver == (ver & ZYNQMP_CSU_VERSION_MASK))
+			break;
+	}
 
-static char *zynqmp_get_silicon_idcode_name(void)
-{
-	unsigned int id;
+	if (i >= ARRAY_SIZE(zynqmp_devices))
+		return "UNKN";
 
-	id = zynqmp_get_silicon_id();
-	for (size_t i = 0; i < ARRAY_SIZE(zynqmp_devices); i++) {
-		if (zynqmp_devices[i].id == id)
-			return zynqmp_devices[i].name;
+	if (!zynqmp_devices[i].evexists)
+		return zynqmp_devices[i].name;
+
+	if (ver & ZYNQMP_PL_STATUS_MASK)
+		return zynqmp_devices[i].name;
+
+	len = strlen(zynqmp_devices[i].name) - 2;
+	for (j = 0; j < strlen(name); j++) {
+		zynqmp_devices[i].name[len] = name[j];
+		len++;
 	}
-	return "UNKN";
+	zynqmp_devices[i].name[len] = '\0';
+
+	return zynqmp_devices[i].name;
 }
 
 static unsigned int zynqmp_get_rtl_ver(void)
@@ -195,60 +306,29 @@
 		break;
 	}
 
-	NOTICE("ATF running on XCZU%s/%s v%d/RTL%d.%d at 0x%x%s\n",
+	NOTICE("ATF running on XCZU%s/%s v%d/RTL%d.%d at 0x%x\n",
 	       zynqmp_print_silicon_idcode(), label, zynqmp_get_ps_ver(),
-	       (rtl & 0xf0) >> 4, rtl & 0xf, BL31_BASE,
-	       zynqmp_is_pmu_up() ? ", with PMU firmware" : "");
+	       (rtl & 0xf0) >> 4, rtl & 0xf, BL31_BASE);
 }
 #else
 static inline void zynqmp_print_platform_name(void) { }
 #endif
 
-/*
- * Indicator for PMUFW discovery:
- *   0 = No FW found
- *   non-zero = FW is present
- */
-static int zynqmp_pmufw_present;
-
-/*
- * zynqmp_discover_pmufw - Discover presence of PMUFW
- *
- * Discover the presence of PMUFW and store it for later run-time queries
- * through zynqmp_is_pmu_up.
- * NOTE: This discovery method is fragile and will break if:
- *  - setting FW_PRESENT is done by PMUFW itself and could be left out in PMUFW
- *    (be it by error or intentionally)
- *  - XPPU/XMPU may restrict ATF's access to the PMU address space
- */
-static int zynqmp_discover_pmufw(void)
+unsigned int zynqmp_get_bootmode(void)
 {
-	zynqmp_pmufw_present = mmio_read_32(PMU_GLOBAL_CNTRL);
-	zynqmp_pmufw_present &= PMU_GLOBAL_CNTRL_FW_IS_PRESENT;
+	uint32_t r;
+	unsigned int ret;
 
-	return !!zynqmp_pmufw_present;
-}
-
-/*
- * zynqmp_is_pmu_up - Find if PMU firmware is up and running
- *
- * Return 0 if firmware is not available, non 0 otherwise
- */
-int zynqmp_is_pmu_up(void)
-{
-	return zynqmp_pmufw_present;
-}
+	ret = pm_mmio_read(CRL_APB_BOOT_MODE_USER, &r);
 
-unsigned int zynqmp_get_bootmode(void)
-{
-	uint32_t r = mmio_read_32(CRL_APB_BOOT_MODE_USER);
+	if (ret != PM_RET_SUCCESS)
+		r = mmio_read_32(CRL_APB_BOOT_MODE_USER);
 
 	return r & CRL_APB_BOOT_MODE_MASK;
 }
 
 void zynqmp_config_setup(void)
 {
-	zynqmp_discover_pmufw();
 	zynqmp_print_platform_name();
 	generic_delay_timer_init();
 }
diff --git a/plat/xilinx/zynqmp/bl31_zynqmp_setup.c b/plat/xilinx/zynqmp/bl31_zynqmp_setup.c
index 1edbd0f..0b3106f 100644
--- a/plat/xilinx/zynqmp/bl31_zynqmp_setup.c
+++ b/plat/xilinx/zynqmp/bl31_zynqmp_setup.c
@@ -36,6 +36,19 @@
 }
 
 /*
+ * Set the build time defaults. We want to do this when doing a JTAG boot
+ * or if we can't find any other config data.
+ */
+static inline void bl31_set_default_config(void)
+{
+	bl32_image_ep_info.pc = BL32_BASE;
+	bl32_image_ep_info.spsr = arm_get_spsr_for_bl32_entry();
+	bl33_image_ep_info.pc = plat_get_ns_image_entrypoint();
+	bl33_image_ep_info.spsr = SPSR_64(MODE_EL2, MODE_SP_ELX,
+					  DISABLE_ALL_EXCEPTIONS);
+}
+
+/*
  * Perform any BL31 specific platform actions. Here is an opportunity to copy
  * parameters passed by the calling EL (S-EL1 in BL2 & S-EL3 in BL1) before they
  * are lost (potentially). This needs to be done before the MMU is initialized
@@ -69,15 +82,15 @@
 	SET_SECURITY_STATE(bl33_image_ep_info.h.attr, NON_SECURE);
 
 	if (zynqmp_get_bootmode() == ZYNQMP_BOOTMODE_JTAG) {
-		/* use build time defaults in JTAG boot mode */
-		bl32_image_ep_info.pc = BL32_BASE;
-		bl32_image_ep_info.spsr = arm_get_spsr_for_bl32_entry();
-		bl33_image_ep_info.pc = plat_get_ns_image_entrypoint();
-		bl33_image_ep_info.spsr = SPSR_64(MODE_EL2, MODE_SP_ELX,
-						  DISABLE_ALL_EXCEPTIONS);
+		bl31_set_default_config();
 	} else {
 		/* use parameters from FSBL */
-		fsbl_atf_handover(&bl32_image_ep_info, &bl33_image_ep_info);
+		enum fsbl_handoff ret = fsbl_atf_handover(&bl32_image_ep_info,
+							  &bl33_image_ep_info);
+		if (ret == FSBL_HANDOFF_NO_STRUCT)
+			bl31_set_default_config();
+		else if (ret != FSBL_HANDOFF_SUCCESS)
+			panic();
 	}
 
 	NOTICE("BL31: Secure code at 0x%lx\n", bl32_image_ep_info.pc);
@@ -103,6 +116,39 @@
 }
 #endif
 
+#if ZYNQMP_WDT_RESTART
+static interrupt_type_handler_t type_el3_interrupt_table[MAX_INTR_EL3];
+
+int request_intr_type_el3(uint32_t id, interrupt_type_handler_t handler)
+{
+	/* Validate 'handler' and 'id' parameters */
+	if (!handler || id >= MAX_INTR_EL3)
+		return -EINVAL;
+
+	/* Check if a handler has already been registered */
+	if (type_el3_interrupt_table[id])
+		return -EALREADY;
+
+	type_el3_interrupt_table[id] = handler;
+
+	return 0;
+}
+
+static uint64_t rdo_el3_interrupt_handler(uint32_t id, uint32_t flags,
+					  void *handle, void *cookie)
+{
+	uint32_t intr_id;
+	interrupt_type_handler_t handler;
+
+	intr_id = plat_ic_get_pending_interrupt_id();
+	handler = type_el3_interrupt_table[intr_id];
+	if (handler != NULL)
+		handler(intr_id, flags, handle, cookie);
+
+	return 0;
+}
+#endif
+
 void bl31_platform_setup(void)
 {
 	/* Initialize the gic cpu and distributor interfaces */
@@ -113,6 +159,16 @@
 
 void bl31_plat_runtime_setup(void)
 {
+#if ZYNQMP_WDT_RESTART
+	uint64_t flags = 0;
+	uint64_t rc;
+
+	set_interrupt_rm_flag(flags, NON_SECURE);
+	rc = register_interrupt_type_handler(INTR_TYPE_EL3,
+					     rdo_el3_interrupt_handler, flags);
+	if (rc)
+		panic();
+#endif
 }
 
 /*
diff --git a/plat/xilinx/zynqmp/include/platform_def.h b/plat/xilinx/zynqmp/include/platform_def.h
index e74b9bb..ebbc8c2 100644
--- a/plat/xilinx/zynqmp/include/platform_def.h
+++ b/plat/xilinx/zynqmp/include/platform_def.h
@@ -96,6 +96,7 @@
  * terminology. On a GICv2 system or mode, the lists will be merged and treated
  * as Group 0 interrupts.
  */
+#if !ZYNQMP_WDT_RESTART
 #define PLAT_ARM_G1S_IRQ_PROPS(grp) \
 	INTR_PROP_DESC(ARM_IRQ_SEC_PHY_TIMER, GIC_HIGHEST_SEC_PRIORITY, grp, \
 			GIC_INTR_CFG_LEVEL), \
@@ -115,6 +116,29 @@
 			GIC_INTR_CFG_EDGE), \
 	INTR_PROP_DESC(ARM_IRQ_SEC_SGI_7, GIC_HIGHEST_SEC_PRIORITY, grp, \
 			GIC_INTR_CFG_EDGE)
+#else
+#define PLAT_ARM_G1S_IRQ_PROPS(grp) \
+	INTR_PROP_DESC(ARM_IRQ_SEC_PHY_TIMER, GIC_HIGHEST_SEC_PRIORITY, grp, \
+			GIC_INTR_CFG_LEVEL), \
+	INTR_PROP_DESC(IRQ_TTC3_1, GIC_HIGHEST_SEC_PRIORITY, grp, \
+			GIC_INTR_CFG_EDGE), \
+	INTR_PROP_DESC(ARM_IRQ_SEC_SGI_0, GIC_HIGHEST_SEC_PRIORITY, grp, \
+			GIC_INTR_CFG_EDGE), \
+	INTR_PROP_DESC(ARM_IRQ_SEC_SGI_1, GIC_HIGHEST_SEC_PRIORITY, grp, \
+			GIC_INTR_CFG_EDGE), \
+	INTR_PROP_DESC(ARM_IRQ_SEC_SGI_2, GIC_HIGHEST_SEC_PRIORITY, grp, \
+			GIC_INTR_CFG_EDGE), \
+	INTR_PROP_DESC(ARM_IRQ_SEC_SGI_3, GIC_HIGHEST_SEC_PRIORITY, grp, \
+			GIC_INTR_CFG_EDGE), \
+	INTR_PROP_DESC(ARM_IRQ_SEC_SGI_4, GIC_HIGHEST_SEC_PRIORITY, grp, \
+			GIC_INTR_CFG_EDGE), \
+	INTR_PROP_DESC(ARM_IRQ_SEC_SGI_5, GIC_HIGHEST_SEC_PRIORITY, grp, \
+			GIC_INTR_CFG_EDGE), \
+	INTR_PROP_DESC(ARM_IRQ_SEC_SGI_6, GIC_HIGHEST_SEC_PRIORITY, grp, \
+			GIC_INTR_CFG_EDGE), \
+	INTR_PROP_DESC(ARM_IRQ_SEC_SGI_7, GIC_HIGHEST_SEC_PRIORITY, grp, \
+			GIC_INTR_CFG_EDGE)
+#endif
 
 #define PLAT_ARM_G0_IRQ_PROPS(grp)
 
diff --git a/plat/xilinx/zynqmp/plat_psci.c b/plat/xilinx/zynqmp/plat_psci.c
index c9fd361..a82f696 100644
--- a/plat/xilinx/zynqmp/plat_psci.c
+++ b/plat/xilinx/zynqmp/plat_psci.c
@@ -27,46 +27,6 @@
 	wfi();
 }
 
-static int zynqmp_nopmu_pwr_domain_on(u_register_t mpidr)
-{
-	uint32_t r;
-	unsigned int cpu_id = plat_core_pos_by_mpidr(mpidr);
-
-	VERBOSE("%s: mpidr: 0x%lx\n", __func__, mpidr);
-
-	if (cpu_id == -1)
-		return PSCI_E_INTERN_FAIL;
-
-	/* program RVBAR */
-	mmio_write_32(APU_RVBAR_L_0 + (cpu_id << 3), zynqmp_sec_entry);
-	mmio_write_32(APU_RVBAR_H_0 + (cpu_id << 3), zynqmp_sec_entry >> 32);
-
-	/* clear VINITHI */
-	r = mmio_read_32(APU_CONFIG_0);
-	r &= ~(1 << APU_CONFIG_0_VINITHI_SHIFT << cpu_id);
-	mmio_write_32(APU_CONFIG_0, r);
-
-	/* clear power down request */
-	r = mmio_read_32(APU_PWRCTL);
-	r &= ~(1 << cpu_id);
-	mmio_write_32(APU_PWRCTL, r);
-
-	/* power up island */
-	mmio_write_32(PMU_GLOBAL_REQ_PWRUP_EN, 1 << cpu_id);
-	mmio_write_32(PMU_GLOBAL_REQ_PWRUP_TRIG, 1 << cpu_id);
-	/* FIXME: we should have a way to break out */
-	while (mmio_read_32(PMU_GLOBAL_REQ_PWRUP_STATUS) & (1 << cpu_id))
-		;
-
-	/* release core reset */
-	r = mmio_read_32(CRF_APB_RST_FPD_APU);
-	r &= ~((CRF_APB_RST_FPD_APU_ACPU_PWRON_RESET |
-			CRF_APB_RST_FPD_APU_ACPU_RESET) << cpu_id);
-	mmio_write_32(CRF_APB_RST_FPD_APU, r);
-
-	return PSCI_E_SUCCESS;
-}
-
 static int zynqmp_pwr_domain_on(u_register_t mpidr)
 {
 	unsigned int cpu_id = plat_core_pos_by_mpidr(mpidr);
@@ -78,6 +38,8 @@
 		return PSCI_E_INTERN_FAIL;
 
 	proc = pm_get_proc(cpu_id);
+	/* Clear power down request */
+	pm_client_wakeup(proc);
 
 	/* Send request to PMU to wake up selected APU CPU core */
 	pm_req_wakeup(proc->node_id, 1, zynqmp_sec_entry, REQ_ACK_BLOCKING);
@@ -85,24 +47,6 @@
 	return PSCI_E_SUCCESS;
 }
 
-static void zynqmp_nopmu_pwr_domain_off(const psci_power_state_t *target_state)
-{
-	uint32_t r;
-	unsigned int cpu_id = plat_my_core_pos();
-
-	for (size_t i = 0; i <= PLAT_MAX_PWR_LVL; i++)
-		VERBOSE("%s: target_state->pwr_domain_state[%lu]=%x\n",
-			__func__, i, target_state->pwr_domain_state[i]);
-
-	/* Prevent interrupts from spuriously waking up this cpu */
-	gicv2_cpuif_disable();
-
-	/* set power down request */
-	r = mmio_read_32(APU_PWRCTL);
-	r |= (1 << cpu_id);
-	mmio_write_32(APU_PWRCTL, r);
-}
-
 static void zynqmp_pwr_domain_off(const psci_power_state_t *target_state)
 {
 	unsigned int cpu_id = plat_my_core_pos();
@@ -126,33 +70,6 @@
 	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)
-{
-	uint32_t r;
-	unsigned int cpu_id = plat_my_core_pos();
-
-	for (size_t i = 0; i <= PLAT_MAX_PWR_LVL; i++)
-		VERBOSE("%s: target_state->pwr_domain_state[%lu]=%x\n",
-			__func__, i, target_state->pwr_domain_state[i]);
-
-	/* set power down request */
-	r = mmio_read_32(APU_PWRCTL);
-	r |= (1 << cpu_id);
-	mmio_write_32(APU_PWRCTL, r);
-
-	/* program RVBAR */
-	mmio_write_32(APU_RVBAR_L_0 + (cpu_id << 3), zynqmp_sec_entry);
-	mmio_write_32(APU_RVBAR_H_0 + (cpu_id << 3), zynqmp_sec_entry >> 32);
-
-	/* clear VINITHI */
-	r = mmio_read_32(APU_CONFIG_0);
-	r &= ~(1 << APU_CONFIG_0_VINITHI_SHIFT << cpu_id);
-	mmio_write_32(APU_CONFIG_0, r);
-
-	/* enable power up on IRQ */
-	mmio_write_32(PMU_GLOBAL_REQ_PWRUP_EN, 1 << cpu_id);
-}
-
 static void zynqmp_pwr_domain_suspend(const psci_power_state_t *target_state)
 {
 	unsigned int state;
@@ -186,24 +103,6 @@
 	gicv2_pcpu_distif_init();
 }
 
-static void zynqmp_nopmu_pwr_domain_suspend_finish(const psci_power_state_t *target_state)
-{
-	uint32_t r;
-	unsigned int cpu_id = plat_my_core_pos();
-
-	for (size_t i = 0; i <= PLAT_MAX_PWR_LVL; i++)
-		VERBOSE("%s: target_state->pwr_domain_state[%lu]=%x\n",
-			__func__, i, target_state->pwr_domain_state[i]);
-
-	/* disable power up on IRQ */
-	mmio_write_32(PMU_GLOBAL_REQ_PWRUP_DIS, 1 << cpu_id);
-
-	/* clear powerdown bit */
-	r = mmio_read_32(APU_PWRCTL);
-	r &= ~(1 << cpu_id);
-	mmio_write_32(APU_PWRCTL, r);
-}
-
 static void zynqmp_pwr_domain_suspend_finish(const psci_power_state_t *target_state)
 {
 	unsigned int cpu_id = plat_my_core_pos();
@@ -230,15 +129,6 @@
 /*******************************************************************************
  * ZynqMP handlers to shutdown/reboot the system
  ******************************************************************************/
-static void __dead2 zynqmp_nopmu_system_off(void)
-{
-	ERROR("ZynqMP System Off: operation not handled.\n");
-
-	/* disable coherency */
-	plat_arm_interconnect_exit_coherency();
-
-	panic();
-}
 
 static void __dead2 zynqmp_system_off(void)
 {
@@ -247,34 +137,12 @@
 
 	/* Send the power down request to the PMU */
 	pm_system_shutdown(PMF_SHUTDOWN_TYPE_SHUTDOWN,
-			   PMF_SHUTDOWN_SUBTYPE_SUBSYSTEM);
+			   pm_get_shutdown_scope());
 
 	while (1)
 		wfi();
 }
 
-static void __dead2 zynqmp_nopmu_system_reset(void)
-{
-	/*
-	 * This currently triggers a system reset. I.e. the whole
-	 * system will be reset! Including RPUs, PMU, PL, etc.
-	 */
-
-	/* disable coherency */
-	plat_arm_interconnect_exit_coherency();
-
-	/* bypass RPLL (needed on 1.0 silicon) */
-	uint32_t reg = mmio_read_32(CRL_APB_RPLL_CTRL);
-	reg |= CRL_APB_RPLL_CTRL_BYPASS;
-	mmio_write_32(CRL_APB_RPLL_CTRL, reg);
-
-	/* trigger system reset */
-	mmio_write_32(CRL_APB_RESET_CTRL, CRL_APB_RESET_CTRL_SOFT_RESET);
-
-	while (1)
-		wfi();
-}
-
 static void __dead2 zynqmp_system_reset(void)
 {
 	/* disable coherency */
@@ -282,7 +150,7 @@
 
 	/* Send the system reset request to the PMU */
 	pm_system_shutdown(PMF_SHUTDOWN_TYPE_RESET,
-			   PMF_SHUTDOWN_SUBTYPE_SUBSYSTEM);
+			   pm_get_shutdown_scope());
 
 	while (1)
 		wfi();
@@ -341,20 +209,6 @@
 	.get_sys_suspend_power_state	= zynqmp_get_sys_suspend_power_state,
 };
 
-static const struct plat_psci_ops zynqmp_nopmu_psci_ops = {
-	.cpu_standby			= zynqmp_cpu_standby,
-	.pwr_domain_on			= zynqmp_nopmu_pwr_domain_on,
-	.pwr_domain_off			= zynqmp_nopmu_pwr_domain_off,
-	.pwr_domain_suspend		= zynqmp_nopmu_pwr_domain_suspend,
-	.pwr_domain_on_finish		= zynqmp_pwr_domain_on_finish,
-	.pwr_domain_suspend_finish	= zynqmp_nopmu_pwr_domain_suspend_finish,
-	.system_off			= zynqmp_nopmu_system_off,
-	.system_reset			= zynqmp_nopmu_system_reset,
-	.validate_power_state		= zynqmp_validate_power_state,
-	.validate_ns_entrypoint		= zynqmp_validate_ns_entrypoint,
-	.get_sys_suspend_power_state	= zynqmp_get_sys_suspend_power_state,
-};
-
 /*******************************************************************************
  * Export the platform specific power ops.
  ******************************************************************************/
@@ -363,10 +217,7 @@
 {
 	zynqmp_sec_entry = sec_entrypoint;
 
-	if (zynqmp_is_pmu_up())
-		*psci_ops = &zynqmp_psci_ops;
-	else
-		*psci_ops = &zynqmp_nopmu_psci_ops;
+	*psci_ops = &zynqmp_psci_ops;
 
 	return 0;
 }
diff --git a/plat/xilinx/zynqmp/plat_startup.c b/plat/xilinx/zynqmp/plat_startup.c
index 18d150c..d3e182c 100644
--- a/plat/xilinx/zynqmp/plat_startup.c
+++ b/plat/xilinx/zynqmp/plat_startup.c
@@ -9,6 +9,7 @@
 #include <debug.h>
 #include <mmio.h>
 #include "zynqmp_def.h"
+#include "zynqmp_private.h"
 
 /*
  * ATFHandoffParams
@@ -147,8 +148,11 @@
  *
  * Process the handoff paramters from the FSBL and populate the BL32 and BL33
  * image info structures accordingly.
+ *
+ * Return: Return the status of the handoff. The value will be from the
+ *         fsbl_handoff enum.
  */
-void fsbl_atf_handover(entry_point_info_t *bl32, entry_point_info_t *bl33)
+enum fsbl_handoff fsbl_atf_handover(entry_point_info_t *bl32, entry_point_info_t *bl33)
 {
 	uint64_t atf_handoff_addr;
 	const struct xfsbl_atf_handoff_params *ATFHandoffParams;
@@ -157,8 +161,8 @@
 	assert((atf_handoff_addr < BL31_BASE) ||
 	       (atf_handoff_addr > (uint64_t)&__BL31_END__));
 	if (!atf_handoff_addr) {
-		ERROR("BL31: No ATF handoff structure passed\n");
-		panic();
+		WARN("BL31: No ATF handoff structure passed\n");
+		return FSBL_HANDOFF_NO_STRUCT;
 	}
 
 	ATFHandoffParams = (struct xfsbl_atf_handoff_params *)atf_handoff_addr;
@@ -168,7 +172,7 @@
 	    (ATFHandoffParams->magic[3] != 'X')) {
 		ERROR("BL31: invalid ATF handoff structure at %llx\n",
 		      atf_handoff_addr);
-		panic();
+		return FSBL_HANDOFF_INVAL_STRUCT;
 	}
 
 	VERBOSE("BL31: ATF handoff params at:0x%llx, entries:%u\n",
@@ -176,7 +180,7 @@
 	if (ATFHandoffParams->num_entries > FSBL_MAX_PARTITIONS) {
 		ERROR("BL31: ATF handoff params: too many partitions (%u/%u)\n",
 		      ATFHandoffParams->num_entries, FSBL_MAX_PARTITIONS);
-		panic();
+		return FSBL_HANDOFF_TOO_MANY_PARTS;
 	}
 
 	/*
@@ -261,4 +265,6 @@
 		else
 			EP_SET_EE(image->h.attr, EP_EE_LITTLE);
 	}
+
+	return FSBL_HANDOFF_SUCCESS;
 }
diff --git a/plat/xilinx/zynqmp/platform.mk b/plat/xilinx/zynqmp/platform.mk
index e49a9cd..3ac9db9 100644
--- a/plat/xilinx/zynqmp/platform.mk
+++ b/plat/xilinx/zynqmp/platform.mk
@@ -9,6 +9,7 @@
 PSCI_EXTENDED_STATE_ID := 1
 A53_DISABLE_NON_TEMPORAL_HINT := 0
 SEPARATE_CODE_AND_RODATA := 1
+ZYNQMP_WDT_RESTART := 0
 override RESET_TO_BL31 := 1
 
 # Do not enable SVE
@@ -41,6 +42,10 @@
 ZYNQMP_CONSOLE	?=	cadence
 $(eval $(call add_define_val,ZYNQMP_CONSOLE,ZYNQMP_CONSOLE_ID_${ZYNQMP_CONSOLE}))
 
+ifdef ZYNQMP_WDT_RESTART
+$(eval $(call add_define,ZYNQMP_WDT_RESTART))
+endif
+
 PLAT_INCLUDES		:=	-Iinclude/plat/arm/common/			\
 				-Iinclude/plat/arm/common/aarch64/		\
 				-Iplat/xilinx/zynqmp/include/			\
diff --git a/plat/xilinx/zynqmp/pm_service/pm_api_sys.c b/plat/xilinx/zynqmp/pm_service/pm_api_sys.c
index d75f7c0..133043d 100644
--- a/plat/xilinx/zynqmp/pm_service/pm_api_sys.c
+++ b/plat/xilinx/zynqmp/pm_service/pm_api_sys.c
@@ -19,6 +19,19 @@
 #include "pm_common.h"
 #include "pm_ipi.h"
 
+/* default shutdown/reboot scope is system(2) */
+static unsigned int pm_shutdown_scope = PMF_SHUTDOWN_SUBTYPE_SYSTEM;
+
+/**
+ * pm_get_shutdown_scope() - Get the currently set shutdown scope
+ *
+ * @return	Shutdown scope value
+ */
+unsigned int pm_get_shutdown_scope(void)
+{
+	return pm_shutdown_scope;
+}
+
 /**
  * Assigning of argument values into array elements.
  */
@@ -130,10 +143,7 @@
 {
 	uint32_t payload[PAYLOAD_ARG_CNT];
 	uint64_t encoded_address;
-	const struct pm_proc *proc = pm_get_proc_by_node(target);
 
-	/* invoke APU-specific code for waking up another APU core */
-	pm_client_wakeup(proc);
 
 	/* encode set Address into 1st bit of address */
 	encoded_address = address;
@@ -218,7 +228,8 @@
 
 /**
  * pm_system_shutdown() - PM call to request a system shutdown or restart
- * @restart	Shutdown or restart? 0 for shutdown, 1 for restart
+ * @type	Shutdown or restart? 0=shutdown, 1=restart, 2=setscope
+ * @subtype	Scope: 0=APU-subsystem, 1=PS, 2=system
  *
  * @return	Returns status, either success or error+reason
  */
@@ -226,8 +237,14 @@
 {
 	uint32_t payload[PAYLOAD_ARG_CNT];
 
+	if (type == PMF_SHUTDOWN_TYPE_SETSCOPE_ONLY) {
+		/* Setting scope for subsequent PSCI reboot or shutdown */
+		pm_shutdown_scope = subtype;
+		return PM_RET_SUCCESS;
+	}
+
 	PM_PACK_PAYLOAD3(payload, PM_SYSTEM_SHUTDOWN, type, subtype);
-	return pm_ipi_send(primary_proc, payload);
+	return pm_ipi_send_non_blocking(primary_proc, payload);
 }
 
 /* APIs for managing PM slaves: */
@@ -342,18 +359,38 @@
 }
 
 /**
- * pm_get_node_status() - PM call to request a node's current power state
- * @nid		Node id of the slave
+ * pm_init_finalize() - Call to notify PMU firmware that master has power
+ *			management enabled and that it has finished its
+ *			initialization
+ *
+ * @return	Status returned by the PMU firmware
+ */
+enum pm_ret_status pm_init_finalize(void)
+{
+	uint32_t payload[PAYLOAD_ARG_CNT];
+
+	/* Send request to the PMU */
+	PM_PACK_PAYLOAD1(payload, PM_INIT_FINALIZE);
+	return pm_ipi_send_sync(primary_proc, payload, NULL, 0);
+}
+
+/**
+ * pm_get_node_status() - PM call to request a node's current status
+ * @nid		Node id
+ * @ret_buff	Buffer for the return values:
+ *		[0] - Current power state of the node
+ *		[1] - Current requirements for the node (slave nodes only)
+ *		[2] - Current usage status for the node (slave nodes only)
  *
  * @return	Returns status, either success or error+reason
  */
-enum pm_ret_status pm_get_node_status(enum pm_node_id nid)
+enum pm_ret_status pm_get_node_status(enum pm_node_id nid,
+				      uint32_t *ret_buff)
 {
-	/* TODO: Add power state argument!! */
 	uint32_t payload[PAYLOAD_ARG_CNT];
 
 	PM_PACK_PAYLOAD2(payload, PM_GET_NODE_STATUS, nid);
-	return pm_ipi_send(primary_proc, payload);
+	return pm_ipi_send_sync(primary_proc, payload, ret_buff, 3);
 }
 
 /**
@@ -501,7 +538,7 @@
 	/* 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);
+	return pm_ipi_send_sync(primary_proc, payload, NULL, 0);
 }
 
 /**
@@ -538,15 +575,30 @@
 }
 
 /**
- * pm_get_callbackdata() - Read from IPI response buffer
- * @data - array of PAYLOAD_ARG_CNT elements
+ * pm_secure_rsaaes() - Load the secure images.
+ *
+ * This function provides access to the xilsecure library to load
+ * the authenticated, encrypted, and authenicated/encrypted images.
+ *
+ * address_low: lower 32-bit Linear memory space address
+ *
+ * address_high: higher 32-bit Linear memory space address
+ *
+ * size:	Number of 32bit words
  *
- * Read value from ipi buffer response buffer.
+ * @return      Returns status, either success or error+reason
  */
-void pm_get_callbackdata(uint32_t *data, size_t count)
+enum pm_ret_status pm_secure_rsaaes(uint32_t address_low,
+				uint32_t address_high,
+				uint32_t size,
+				uint32_t flags)
 {
-	pm_ipi_buff_read_callb(data, count);
-	pm_ipi_irq_clear(primary_proc);
+	uint32_t payload[PAYLOAD_ARG_CNT];
+
+	/* Send request to the PMU */
+	PM_PACK_PAYLOAD5(payload, PM_SECURE_RSA_AES, address_high, address_low,
+			 size, flags);
+	return pm_ipi_send_sync(primary_proc, payload, NULL, 0);
 }
 
 /**
@@ -1074,3 +1126,43 @@
 
 	return ret;
 }
+
+enum pm_ret_status pm_sha_hash(uint32_t address_high,
+				    uint32_t address_low,
+				    uint32_t size,
+				    uint32_t flags)
+{
+	uint32_t payload[PAYLOAD_ARG_CNT];
+
+	/* Send request to the PMU */
+	PM_PACK_PAYLOAD5(payload, PM_SECURE_SHA, address_high, address_low,
+				 size, flags);
+	return pm_ipi_send_sync(primary_proc, payload, NULL, 0);
+}
+
+enum pm_ret_status pm_rsa_core(uint32_t address_high,
+				    uint32_t address_low,
+				    uint32_t size,
+				    uint32_t flags)
+{
+	uint32_t payload[PAYLOAD_ARG_CNT];
+
+	/* Send request to the PMU */
+	PM_PACK_PAYLOAD5(payload, PM_SECURE_RSA, address_high, address_low,
+				 size, flags);
+	return pm_ipi_send_sync(primary_proc, payload, NULL, 0);
+}
+
+enum pm_ret_status pm_secure_image(uint32_t address_low,
+				   uint32_t address_high,
+				   uint32_t key_lo,
+				   uint32_t key_hi,
+				   uint32_t *value)
+{
+	uint32_t payload[PAYLOAD_ARG_CNT];
+
+	/* Send request to the PMU */
+	PM_PACK_PAYLOAD5(payload, PM_SECURE_IMAGE, address_high, address_low,
+			 key_hi, key_lo);
+	return pm_ipi_send_sync(primary_proc, payload, value, 2);
+}
diff --git a/plat/xilinx/zynqmp/pm_service/pm_api_sys.h b/plat/xilinx/zynqmp/pm_service/pm_api_sys.h
index c6de560..55a8a6e 100644
--- a/plat/xilinx/zynqmp/pm_service/pm_api_sys.h
+++ b/plat/xilinx/zynqmp/pm_service/pm_api_sys.h
@@ -76,7 +76,9 @@
 /* Miscellaneous API functions */
 enum pm_ret_status pm_get_api_version(unsigned int *version);
 enum pm_ret_status pm_set_configuration(unsigned int phys_addr);
-enum pm_ret_status pm_get_node_status(enum pm_node_id node);
+enum pm_ret_status pm_init_finalize(void);
+enum pm_ret_status pm_get_node_status(enum pm_node_id node,
+				      uint32_t *ret_buff);
 enum pm_ret_status pm_register_notifier(enum pm_node_id nid,
 					unsigned int event,
 					unsigned int wake,
@@ -107,7 +109,11 @@
 enum pm_ret_status pm_fpga_get_status(unsigned int *value);
 
 enum pm_ret_status pm_get_chipid(uint32_t *value);
-void pm_get_callbackdata(uint32_t *data, size_t count);
+enum pm_ret_status pm_secure_rsaaes(uint32_t address_high,
+				    uint32_t address_low,
+				    uint32_t size,
+				    uint32_t flags);
+unsigned int pm_get_shutdown_scope(void);
 enum pm_ret_status pm_pinctrl_request(unsigned int pin);
 enum pm_ret_status pm_pinctrl_release(unsigned int pin);
 enum pm_ret_status pm_pinctrl_get_function(unsigned int pin,
@@ -146,4 +152,17 @@
 				 unsigned int arg2,
 				 unsigned int arg3,
 				 unsigned int *data);
+enum pm_ret_status pm_sha_hash(uint32_t address_high,
+				    uint32_t address_low,
+				    uint32_t size,
+				    uint32_t flags);
+enum pm_ret_status pm_rsa_core(uint32_t address_high,
+				    uint32_t address_low,
+				    uint32_t size,
+				    uint32_t flags);
+enum pm_ret_status pm_secure_image(uint32_t address_low,
+				   uint32_t address_high,
+				   uint32_t key_lo,
+				   uint32_t key_hi,
+				   uint32_t *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 9016fd6..874b8a9 100644
--- a/plat/xilinx/zynqmp/pm_service/pm_client.c
+++ b/plat/xilinx/zynqmp/pm_service/pm_client.c
@@ -26,10 +26,15 @@
 #define NUM_GICD_ISENABLER	((IRQ_MAX >> 5) + 1)
 #define UNDEFINED_CPUID		(~0)
 
+#define PM_SUSPEND_MODE_STD		0
+#define PM_SUSPEND_MODE_POWER_OFF	1
+
 DEFINE_BAKERY_LOCK(pm_client_secure_lock);
 
 extern const struct pm_ipi apu_ipi;
 
+static uint32_t suspend_mode = PM_SUSPEND_MODE_STD;
+
 /* Order in pm_procs_all array must match cpu ids */
 static const struct pm_proc pm_procs_all[] = {
 	{
@@ -165,6 +170,19 @@
 	uint8_t pm_wakeup_nodes_set[NODE_MAX];
 	uintptr_t isenabler1 = BASE_GICD_BASE + GICD_ISENABLER + 4;
 
+	/* In case of power-off suspend, only NODE_EXTERN must be set */
+	if (suspend_mode == PM_SUSPEND_MODE_POWER_OFF) {
+		enum pm_ret_status ret;
+
+		ret = pm_set_wakeup_source(NODE_APU, NODE_EXTERN, 1);
+		/**
+		 * If NODE_EXTERN could not be set as wake source, proceed with
+		 * standard suspend (no one will wake the system otherwise)
+		 */
+		if (ret == PM_RET_SUCCESS)
+			return;
+	}
+
 	zeromem(&pm_wakeup_nodes_set, sizeof(pm_wakeup_nodes_set));
 
 	for (reg_num = 0; reg_num < NUM_GICD_ISENABLER; reg_num++) {
@@ -305,3 +323,13 @@
 
 	bakery_lock_release(&pm_client_secure_lock);
 }
+
+enum pm_ret_status pm_set_suspend_mode(uint32_t mode)
+{
+	if ((mode != PM_SUSPEND_MODE_STD) &&
+	    (mode != PM_SUSPEND_MODE_POWER_OFF))
+		return PM_RET_ERROR_ARGS;
+
+	suspend_mode = mode;
+	return PM_RET_SUCCESS;
+}
diff --git a/plat/xilinx/zynqmp/pm_service/pm_client.h b/plat/xilinx/zynqmp/pm_service/pm_client.h
index 16e37d5..070db89 100644
--- a/plat/xilinx/zynqmp/pm_service/pm_client.h
+++ b/plat/xilinx/zynqmp/pm_service/pm_client.h
@@ -20,6 +20,7 @@
 void pm_client_abort_suspend(void);
 void pm_client_wakeup(const struct pm_proc *proc);
 enum pm_ret_status set_ocm_retention(void);
+enum pm_ret_status pm_set_suspend_mode(uint32_t mode);
 
 /* Global variables to be set in pm_client.c */
 extern const struct pm_proc *primary_proc;
diff --git a/plat/xilinx/zynqmp/pm_service/pm_defs.h b/plat/xilinx/zynqmp/pm_service/pm_defs.h
index 0c46e73..9a8026f 100644
--- a/plat/xilinx/zynqmp/pm_service/pm_defs.h
+++ b/plat/xilinx/zynqmp/pm_service/pm_defs.h
@@ -62,7 +62,7 @@
 	PM_RESET_GET_STATUS,
 	PM_MMIO_WRITE,
 	PM_MMIO_READ,
-	PM_INIT,
+	PM_INIT_FINALIZE,
 	PM_FPGA_LOAD,
 	PM_FPGA_GET_STATUS,
 	PM_GET_CHIPID,
@@ -88,6 +88,7 @@
 	PM_CLOCK_GETRATE,
 	PM_CLOCK_SETPARENT,
 	PM_CLOCK_GETPARENT,
+	PM_SECURE_IMAGE,
 	PM_API_MAX
 };
 
@@ -141,7 +142,7 @@
 	NODE_GPIO,
 	NODE_CAN_0,
 	NODE_CAN_1,
-	NODE_AFI,
+	NODE_EXTERN,
 	NODE_APLL,
 	NODE_VPLL,
 	NODE_DPLL,
@@ -239,11 +240,22 @@
 	PM_BOOT_ERROR,
 };
 
+/**
+ * @PMF_SHUTDOWN_TYPE_SHUTDOWN:		shutdown
+ * @PMF_SHUTDOWN_TYPE_RESET:		reset/reboot
+ * @PMF_SHUTDOWN_TYPE_SETSCOPE_ONLY:	set the shutdown/reboot scope
+ */
 enum pm_shutdown_type {
 	PMF_SHUTDOWN_TYPE_SHUTDOWN,
 	PMF_SHUTDOWN_TYPE_RESET,
+	PMF_SHUTDOWN_TYPE_SETSCOPE_ONLY,
 };
 
+/**
+ * @PMF_SHUTDOWN_SUBTYPE_SUBSYSTEM:	shutdown/reboot APU subsystem only
+ * @PMF_SHUTDOWN_SUBTYPE_PS_ONLY:	shutdown/reboot entire PS (but not PL)
+ * @PMF_SHUTDOWN_SUBTYPE_SYSTEM:	shutdown/reboot entire system
+ */
 enum pm_shutdown_subtype {
 	PMF_SHUTDOWN_SUBTYPE_SUBSYSTEM,
 	PMF_SHUTDOWN_SUBTYPE_PS_ONLY,
diff --git a/plat/xilinx/zynqmp/pm_service/pm_ipi.c b/plat/xilinx/zynqmp/pm_service/pm_ipi.c
index 58faf0e..dc1ea4d 100644
--- a/plat/xilinx/zynqmp/pm_service/pm_ipi.c
+++ b/plat/xilinx/zynqmp/pm_service/pm_ipi.c
@@ -26,6 +26,9 @@
 #define IPI_BUFFER_REQ_OFFSET	0x0U
 #define IPI_BUFFER_RESP_OFFSET	0x20U
 
+#define IPI_BLOCKING		1
+#define IPI_NON_BLOCKING	0
+
 DEFINE_BAKERY_LOCK(pm_secure_lock);
 
 const struct pm_ipi apu_ipi = {
@@ -63,7 +66,8 @@
  * @return	Returns status, either success or error+reason
  */
 static enum pm_ret_status pm_ipi_send_common(const struct pm_proc *proc,
-					     uint32_t payload[PAYLOAD_ARG_CNT])
+					     uint32_t payload[PAYLOAD_ARG_CNT],
+					     uint32_t is_blocking)
 {
 	unsigned int offset = 0;
 	uintptr_t buffer_base = proc->ipi->buffer_base +
@@ -75,13 +79,39 @@
 		mmio_write_32(buffer_base + offset, payload[i]);
 		offset += PAYLOAD_ARG_SIZE;
 	}
+
 	/* Generate IPI to PMU */
-	ipi_mb_notify(proc->ipi->apu_ipi_id, proc->ipi->pmu_ipi_id, 1);
+	ipi_mb_notify(proc->ipi->apu_ipi_id, proc->ipi->pmu_ipi_id,
+		      is_blocking);
 
 	return PM_RET_SUCCESS;
 }
 
 /**
+ * pm_ipi_send_non_blocking() - Sends IPI request to the PMU without blocking
+ *			        notification
+ * @proc	Pointer to the processor who is initiating request
+ * @payload	API id and call arguments to be written in IPI buffer
+ *
+ * Send an IPI request to the power controller.
+ *
+ * @return	Returns status, either success or error+reason
+ */
+enum pm_ret_status pm_ipi_send_non_blocking(const struct pm_proc *proc,
+					    uint32_t payload[PAYLOAD_ARG_CNT])
+{
+	enum pm_ret_status ret;
+
+	bakery_lock_get(&pm_secure_lock);
+
+	ret = pm_ipi_send_common(proc, payload, IPI_NON_BLOCKING);
+
+	bakery_lock_release(&pm_secure_lock);
+
+	return ret;
+}
+
+/**
  * pm_ipi_send() - Sends IPI request to the PMU
  * @proc	Pointer to the processor who is initiating request
  * @payload	API id and call arguments to be written in IPI buffer
@@ -97,7 +127,7 @@
 
 	bakery_lock_get(&pm_secure_lock);
 
-	ret = pm_ipi_send_common(proc, payload);
+	ret = pm_ipi_send_common(proc, payload, IPI_BLOCKING);
 
 	bakery_lock_release(&pm_secure_lock);
 
@@ -179,7 +209,7 @@
 
 	bakery_lock_get(&pm_secure_lock);
 
-	ret = pm_ipi_send_common(proc, payload);
+	ret = pm_ipi_send_common(proc, payload, IPI_BLOCKING);
 	if (ret != PM_RET_SUCCESS)
 		goto unlock;
 
diff --git a/plat/xilinx/zynqmp/pm_service/pm_ipi.h b/plat/xilinx/zynqmp/pm_service/pm_ipi.h
index e6b36f5..439dcf4 100644
--- a/plat/xilinx/zynqmp/pm_service/pm_ipi.h
+++ b/plat/xilinx/zynqmp/pm_service/pm_ipi.h
@@ -13,6 +13,8 @@
 
 enum pm_ret_status pm_ipi_send(const struct pm_proc *proc,
 			       uint32_t payload[PAYLOAD_ARG_CNT]);
+enum pm_ret_status pm_ipi_send_non_blocking(const struct pm_proc *proc,
+					    uint32_t payload[PAYLOAD_ARG_CNT]);
 enum pm_ret_status pm_ipi_send_sync(const struct pm_proc *proc,
 				    uint32_t payload[PAYLOAD_ARG_CNT],
 				    unsigned int *value, size_t count);
diff --git a/plat/xilinx/zynqmp/pm_service/pm_svc_main.c b/plat/xilinx/zynqmp/pm_service/pm_svc_main.c
index 34b3ad4..dd9bbc8 100644
--- a/plat/xilinx/zynqmp/pm_service/pm_svc_main.c
+++ b/plat/xilinx/zynqmp/pm_service/pm_svc_main.c
@@ -10,19 +10,30 @@
  */
 
 #include <errno.h>
-#include <gic_common.h>
 #include <runtime_svc.h>
-#include <string.h>
 #include "../zynqmp_private.h"
 #include "pm_api_sys.h"
 #include "pm_client.h"
 #include "pm_ipi.h"
+#if ZYNQMP_WDT_RESTART
+#include <arch_helpers.h>
+#include <gicv2.h>
+#include <mmio.h>
+#include <platform.h>
+#include <spinlock.h>
+#endif
 
-#define PM_GET_CALLBACK_DATA	0xa01
+#define PM_SET_SUSPEND_MODE	0xa02
 #define PM_GET_TRUSTZONE_VERSION	0xa03
 
-/* 0 - UP, !0 - DOWN */
-static int32_t pm_down = !0;
+/* !0 - UP, 0 - DOWN */
+static int32_t pm_up = 0;
+
+#if ZYNQMP_WDT_RESTART
+static spinlock_t inc_lock;
+static int active_cores = 0;
+#endif
+
 
 /**
  * pm_context - Structure which contains data for power management
@@ -35,7 +46,143 @@
 	uint32_t payload[PAYLOAD_ARG_CNT];
 } pm_ctx;
 
+#if ZYNQMP_WDT_RESTART
+/**
+ * trigger_wdt_restart() - Trigger warm restart event to APU cores
+ *
+ * This function triggers SGI for all active APU CPUs. SGI handler then
+ * power down CPU and call system reset.
+ */
+static void trigger_wdt_restart(void)
+{
+	uint32_t core_count = 0;
+	uint32_t core_status[3];
+	uint32_t target_cpu_list = 0;
+	int i;
+
+	for (i = 0; i < 4; i++) {
+		pm_get_node_status(NODE_APU_0 + i, core_status);
+		if (core_status[0] == 1) {
+			core_count++;
+			target_cpu_list |= (1 << i);
+		}
+	}
+
+	spin_lock(&inc_lock);
+	active_cores = core_count;
+	spin_unlock(&inc_lock);
+
+	INFO("Active Cores: %d\n", active_cores);
+
+	/* trigger SGI to active cores */
+	gicv2_raise_sgi(ARM_IRQ_SEC_SGI_7, target_cpu_list);
+}
+
+/**
+ * ttc_fiq_handler() - TTC Handler for timer event
+ * @id         number of the highest priority pending interrupt of the type
+ *             that this handler was registered for
+ * @flags      security state, bit[0]
+ * @handler    pointer to 'cpu_context' structure of the current CPU for the
+ *             security state specified in the 'flags' parameter
+ * @cookie     unused
+ *
+ * Function registered as INTR_TYPE_EL3 interrupt handler
+ *
+ * When WDT event is received in PMU, PMU needs to notify master to do cleanup
+ * if required. PMU sets up timer and starts timer to overflow in zero time upon
+ * WDT event. ATF handles this timer event and takes necessary action required
+ * for warm restart.
+ *
+ * In presence of non-secure software layers (EL1/2) sets the interrupt
+ * at registered entrance in GIC and informs that PMU responsed or demands
+ * action.
+ */
+static uint64_t ttc_fiq_handler(uint32_t id, uint32_t flags, void *handle,
+                               void *cookie)
+{
+	INFO("BL31: Got TTC FIQ\n");
+
+	/* Clear TTC interrupt by reading interrupt register */
+	mmio_read_32(TTC3_INTR_REGISTER_1);
+
+	/* Disable the timer interrupts */
+	mmio_write_32(TTC3_INTR_ENABLE_1, 0);
+
+	trigger_wdt_restart();
+
+	return 0;
+}
+
+/**
+ * zynqmp_sgi7_irq() - Handler for SGI7 IRQ
+ * @id         number of the highest priority pending interrupt of the type
+ *             that this handler was registered for
+ * @flags      security state, bit[0]
+ * @handler    pointer to 'cpu_context' structure of the current CPU for the
+ *             security state specified in the 'flags' parameter
+ * @cookie     unused
+ *
+ * Function registered as INTR_TYPE_EL3 interrupt handler
+ *
+ * On receiving WDT event from PMU, ATF generates SGI7 to all running CPUs.
+ * In response to SGI7 interrupt, each CPUs do clean up if required and last
+ * running CPU calls system restart.
+ */
+static uint64_t __unused __dead2 zynqmp_sgi7_irq(uint32_t id, uint32_t flags,
+                                                void *handle, void *cookie)
+{
+	int i;
+	/* enter wfi and stay there */
+	INFO("Entering wfi\n");
+
+	spin_lock(&inc_lock);
+	active_cores--;
+
+	for (i = 0; i < 4; i++) {
+		mmio_write_32(BASE_GICD_BASE + GICD_CPENDSGIR + 4 * i,
+				0xffffffff);
+	}
+
+	spin_unlock(&inc_lock);
+
+	if (active_cores == 0) {
+		pm_system_shutdown(PMF_SHUTDOWN_TYPE_RESET,
+				PMF_SHUTDOWN_SUBTYPE_SUBSYSTEM);
+	}
+
+	/* enter wfi and stay there */
+	while (1)
+		wfi();
+}
+
 /**
+ * pm_wdt_restart_setup() - Setup warm restart interrupts
+ *
+ * This function sets up handler for SGI7 and TTC interrupts
+ * used for warm restart.
+ */
+static int pm_wdt_restart_setup(void)
+{
+	int ret;
+
+	/* register IRQ handler for SGI7 */
+	ret = request_intr_type_el3(ARM_IRQ_SEC_SGI_7, zynqmp_sgi7_irq);
+	if (ret) {
+		WARN("BL31: registering SGI7 interrupt failed\n");
+		goto err;
+	}
+
+	ret = request_intr_type_el3(IRQ_TTC3_1, ttc_fiq_handler);
+	if (ret)
+		WARN("BL31: registering TTC3 interrupt failed\n");
+
+err:
+	return ret;
+}
+#endif
+
+/**
  * pm_setup() - PM service setup
  *
  * @return	On success, the initialization function must return 0.
@@ -52,11 +199,14 @@
 {
 	int status, ret;
 
-	if (!zynqmp_is_pmu_up())
-		return -ENODEV;
-
 	status = pm_ipi_init(primary_proc);
 
+#if ZYNQMP_WDT_RESTART
+	status = pm_wdt_restart_setup();
+	if (status)
+		WARN("BL31: warm-restart setup failed\n");
+#endif
+
 	if (status >= 0) {
 		INFO("BL31: PM Service Init Complete: API v%d.%d\n",
 		     PM_VERSION_MAJOR, PM_VERSION_MINOR);
@@ -66,7 +216,7 @@
 		ret = status;
 	}
 
-	pm_down = status;
+	pm_up = !status;
 
 	return ret;
 }
@@ -95,7 +245,7 @@
 	uint32_t pm_arg[4];
 
 	/* Handle case where PM wasn't initialized properly */
-	if (pm_down)
+	if (!pm_up)
 		SMC_RET1(handle, SMC_UNK);
 
 	pm_arg[0] = (uint32_t)x1;
@@ -116,9 +266,16 @@
 		SMC_RET1(handle, (uint64_t)ret);
 
 	case PM_REQ_WAKEUP:
-		ret = pm_req_wakeup(pm_arg[0], pm_arg[1], pm_arg[2],
+	{
+		/* Use address flag is encoded in the 1st bit of the low-word */
+		unsigned int set_addr = pm_arg[1] & 0x1;
+		uint64_t address = (uint64_t)pm_arg[2] << 32;
+
+		address |= pm_arg[1] & (~0x1);
+		ret = pm_req_wakeup(pm_arg[0], set_addr, address,
 				    pm_arg[3]);
 		SMC_RET1(handle, (uint64_t)ret);
+	}
 
 	case PM_FORCE_POWERDOWN:
 		ret = pm_force_powerdown(pm_arg[0], pm_arg[1]);
@@ -175,10 +332,19 @@
 		ret = pm_set_configuration(pm_arg[0]);
 		SMC_RET1(handle, (uint64_t)ret);
 
-	case PM_GET_NODE_STATUS:
-		ret = pm_get_node_status(pm_arg[0]);
+	case PM_INIT_FINALIZE:
+		ret = pm_init_finalize();
 		SMC_RET1(handle, (uint64_t)ret);
 
+	case PM_GET_NODE_STATUS:
+	{
+		uint32_t buff[3];
+
+		ret = pm_get_node_status(pm_arg[0], buff);
+		SMC_RET2(handle, (uint64_t)ret | ((uint64_t)buff[0] << 32),
+			 (uint64_t)buff[1] | ((uint64_t)buff[2] << 32));
+	}
+
 	case PM_GET_OP_CHARACTERISTIC:
 	{
 		uint32_t result;
@@ -239,15 +405,10 @@
 			 result[1]);
 	}
 
-	case PM_GET_CALLBACK_DATA:
-	{
-		uint32_t result[4];
-
-		pm_get_callbackdata(result, sizeof(result));
-		SMC_RET2(handle,
-			 (uint64_t)result[0] | ((uint64_t)result[1] << 32),
-			 (uint64_t)result[2] | ((uint64_t)result[3] << 32));
-	}
+	case PM_SECURE_RSA_AES:
+		ret = pm_secure_rsaaes(pm_arg[0], pm_arg[1], pm_arg[2],
+				       pm_arg[3]);
+		SMC_RET1(handle, (uint64_t)ret);
 
 	case PM_PINCTRL_REQUEST:
 		ret = pm_pinctrl_request(pm_arg[0]);
@@ -361,6 +522,30 @@
 		SMC_RET1(handle, (uint64_t)PM_RET_SUCCESS |
 			 ((uint64_t)ZYNQMP_TZ_VERSION << 32));
 
+	case PM_SET_SUSPEND_MODE:
+		ret = pm_set_suspend_mode(pm_arg[0]);
+		SMC_RET1(handle, (uint64_t)ret);
+
+	case PM_SECURE_SHA:
+		ret = pm_sha_hash(pm_arg[0], pm_arg[1], pm_arg[2],
+				pm_arg[3]);
+		SMC_RET1(handle, (uint64_t)ret);
+
+	case PM_SECURE_RSA:
+		ret = pm_rsa_core(pm_arg[0], pm_arg[1], pm_arg[2],
+				       pm_arg[3]);
+		SMC_RET1(handle, (uint64_t)ret);
+
+	case PM_SECURE_IMAGE:
+	{
+		uint32_t result[2];
+
+		ret = pm_secure_image(pm_arg[0], pm_arg[1], pm_arg[2],
+				      pm_arg[3], &result[0]);
+		SMC_RET2(handle, (uint64_t)ret | ((uint64_t)result[0] << 32),
+			 result[1]);
+	}
+
 	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 60df187..22256eb 100644
--- a/plat/xilinx/zynqmp/zynqmp_def.h
+++ b/plat/xilinx/zynqmp/zynqmp_def.h
@@ -101,6 +101,14 @@
 #define BASE_GICH_BASE		0xF9040000
 #define BASE_GICV_BASE		0xF9060000
 
+#if ZYNQMP_WDT_RESTART
+#define IRQ_SEC_IPI_APU		67
+#define IRQ_TTC3_1		77
+#define TTC3_BASE_ADDR		0xFF140000
+#define TTC3_INTR_REGISTER_1	(TTC3_BASE_ADDR + 0x54)
+#define TTC3_INTR_ENABLE_1	(TTC3_BASE_ADDR + 0x60)
+#endif
+
 #define ARM_IRQ_SEC_PHY_TIMER		29
 
 #define ARM_IRQ_SEC_SGI_0		8
@@ -158,7 +166,8 @@
 #define ZYNQMP_CSU_IDCODE_XILINX_ID		0x093
 
 #define ZYNQMP_CSU_IDCODE_SVD_SHIFT		12
-#define ZYNQMP_CSU_IDCODE_SVD_MASK		(0xE << ZYNQMP_CSU_IDCODE_SVD_SHIFT)
+#define ZYNQMP_CSU_IDCODE_SVD_MASK		(0x7 << \
+						 ZYNQMP_CSU_IDCODE_SVD_SHIFT)
 #define ZYNQMP_CSU_IDCODE_DEVICE_CODE_SHIFT	15
 #define ZYNQMP_CSU_IDCODE_DEVICE_CODE_MASK	(0xF << ZYNQMP_CSU_IDCODE_DEVICE_CODE_SHIFT)
 #define ZYNQMP_CSU_IDCODE_SUB_FAMILY_SHIFT	19
@@ -173,6 +182,12 @@
 
 #define ZYNQMP_CSU_VERSION_OFFSET	0x44
 
+/* Efuse */
+#define EFUSE_BASEADDR		0xFFCC0000
+#define EFUSE_IPDISABLE_OFFSET	0x1018
+#define EFUSE_IPDISABLE_VERSION	0x1FFU
+#define ZYNQMP_EFUSE_IPDISABLE_SHIFT	20
+
 /* Access control register defines */
 #define ACTLR_EL3_L2ACTLR_BIT	(1 << 6)
 #define ACTLR_EL3_CPUACTLR_BIT	(1 << 0)
diff --git a/plat/xilinx/zynqmp/zynqmp_ipi.c b/plat/xilinx/zynqmp/zynqmp_ipi.c
index 755a3b7..5038f84 100644
--- a/plat/xilinx/zynqmp/zynqmp_ipi.c
+++ b/plat/xilinx/zynqmp/zynqmp_ipi.c
@@ -84,7 +84,7 @@
 	{
 		.ipi_bit_mask = 0x20000,
 		.ipi_reg_base = 0xFF331000,
-		.secure_only = IPI_SECURE_MASK,
+		.secure_only = 0,
 	},
 	/* PMU2 IPI */
 	{
diff --git a/plat/xilinx/zynqmp/zynqmp_private.h b/plat/xilinx/zynqmp/zynqmp_private.h
index 94a99f4..08a5410 100644
--- a/plat/xilinx/zynqmp/zynqmp_private.h
+++ b/plat/xilinx/zynqmp/zynqmp_private.h
@@ -7,17 +7,32 @@
 #ifndef __ZYNQMP_PRIVATE_H__
 #define __ZYNQMP_PRIVATE_H__
 
+#include <bl_common.h>
 #include <interrupt_mgmt.h>
 
 void zynqmp_config_setup(void);
 
 /* ZynqMP specific functions */
 unsigned int zynqmp_get_uart_clk(void);
-int zynqmp_is_pmu_up(void);
 unsigned int zynqmp_get_bootmode(void);
 
 /* For FSBL handover */
-void fsbl_atf_handover(entry_point_info_t *bl32_image_ep_info,
+enum fsbl_handoff {
+	FSBL_HANDOFF_SUCCESS = 0,
+	FSBL_HANDOFF_NO_STRUCT,
+	FSBL_HANDOFF_INVAL_STRUCT,
+	FSBL_HANDOFF_TOO_MANY_PARTS,
+};
+
+#if ZYNQMP_WDT_RESTART
+/*
+ * Register handler to specific GIC entrance
+ * for INTR_TYPE_EL3 type of interrupt
+ */
+int request_intr_type_el3(uint32_t, interrupt_type_handler_t);
+#endif
+
+enum fsbl_handoff fsbl_atf_handover(entry_point_info_t *bl32_image_ep_info,
 		       entry_point_info_t *bl33_image_ep_info);
 
 #endif /* __ZYNQMP_PRIVATE_H__ */