Merge pull request #650 from Xilinx/zynqmp-updates

Zynqmp updates

17 files changed, 427 insertions, 137 deletions

diff --git a/plat/xilinx/zynqmp/aarch64/zynqmp_common.c b/plat/xilinx/zynqmp/aarch64/zynqmp_common.c
index f89cdce1..1ba301de 100644
--- a/plat/xilinx/zynqmp/aarch64/zynqmp_common.c
+++ b/plat/xilinx/zynqmp/aarch64/zynqmp_common.c
@@ -28,12 +28,8 @@
  * POSSIBILITY OF SUCH DAMAGE.
  */
 
-#include <arch_helpers.h>
-#include <cci.h>
 #include <debug.h>
-#include <gicv2.h>
 #include <mmio.h>
-#include <plat_arm.h>
 #include <platform.h>
 #include <xlat_tables.h>
 #include "../zynqmp_private.h"
@@ -187,11 +183,9 @@ static char *zynqmp_print_silicon_idcode(void)
 
 	tmp = id;
 	tmp &= ZYNQMP_CSU_IDCODE_XILINX_ID_MASK |
-	       ZYNQMP_CSU_IDCODE_FAMILY_MASK |
-	       ZYNQMP_CSU_IDCODE_REVISION_MASK;
+	       ZYNQMP_CSU_IDCODE_FAMILY_MASK;
 	maskid = ZYNQMP_CSU_IDCODE_XILINX_ID << ZYNQMP_CSU_IDCODE_XILINX_ID_SHIFT |
-		 ZYNQMP_CSU_IDCODE_FAMILY << ZYNQMP_CSU_IDCODE_FAMILY_SHIFT |
-		 ZYNQMP_CSU_IDCODE_REVISION << ZYNQMP_CSU_IDCODE_REVISION_SHIFT;
+		 ZYNQMP_CSU_IDCODE_FAMILY << ZYNQMP_CSU_IDCODE_FAMILY_SHIFT;
 	if (tmp != maskid) {
 		ERROR("Incorrect XILINX IDCODE 0x%x, maskid 0x%x\n", id, maskid);
 		return "UNKN";
@@ -275,13 +269,13 @@ int zynqmp_is_pmu_up(void)
 	return zynqmp_pmufw_present;
 }
 
-/*
- * A single boot loader stack is expected to work on both the Foundation ZYNQMP
- * models and the two flavours of the Base ZYNQMP models (AEMv8 & Cortex). The
- * SYS_ID register provides a mechanism for detecting the differences between
- * these platforms. This information is stored in a per-BL array to allow the
- * code to take the correct path.Per BL platform configuration.
- */
+unsigned int zynqmp_get_bootmode(void)
+{
+	uint32_t r = mmio_read_32(CRL_APB_BOOT_MODE_USER);
+
+	return r & CRL_APB_BOOT_MODE_MASK;
+}
+
 void zynqmp_config_setup(void)
 {
 	zynqmp_discover_pmufw();
diff --git a/plat/xilinx/zynqmp/bl31_zynqmp_setup.c b/plat/xilinx/zynqmp/bl31_zynqmp_setup.c
index 9c9d18d1..7b8c5670 100644
--- a/plat/xilinx/zynqmp/bl31_zynqmp_setup.c
+++ b/plat/xilinx/zynqmp/bl31_zynqmp_setup.c
@@ -113,25 +113,25 @@ void bl31_early_platform_setup(bl31_params_t *from_bl2,
 	 * present.
 	 */
 
-	/* Populate entry point information for BL32 and BL33 */
+	/* Populate common information for BL32 and BL33 */
 	SET_PARAM_HEAD(&bl32_image_ep_info, PARAM_EP, VERSION_1, 0);
 	SET_SECURITY_STATE(bl32_image_ep_info.h.attr, SECURE);
-	bl32_image_ep_info.pc = BL32_BASE;
-	bl32_image_ep_info.spsr = arm_get_spsr_for_bl32_entry();
-
-	NOTICE("BL31: Secure code at 0x%lx\n", bl32_image_ep_info.pc);
-
 	SET_PARAM_HEAD(&bl33_image_ep_info, PARAM_EP, VERSION_1, 0);
-
-	/*
-	 * Tell BL31 where the non-trusted software image
-	 * is located and the entry state information
-	 */
-	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);
 	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);
+	} else {
+		/* use parameters from FSBL */
+		fsbl_atf_handover(&bl32_image_ep_info, &bl33_image_ep_info);
+	}
+
+	NOTICE("BL31: Secure code at 0x%lx\n", bl32_image_ep_info.pc);
 	NOTICE("BL31: Non secure code at 0x%lx\n", bl33_image_ep_info.pc);
 }
 
diff --git a/plat/xilinx/zynqmp/include/platform_def.h b/plat/xilinx/zynqmp/include/platform_def.h
index 947a0f3b..76a52de8 100644
--- a/plat/xilinx/zynqmp/include/platform_def.h
+++ b/plat/xilinx/zynqmp/include/platform_def.h
@@ -50,64 +50,62 @@
 /*******************************************************************************
  * BL31 specific defines.
  ******************************************************************************/
-
-#define ZYNQMP_BL31_SIZE	0x1b000
 /*
  * Put BL31 at the top of the Trusted SRAM (just below the shared memory, if
  * present). BL31_BASE is calculated using the current BL31 debug size plus a
  * little space for growth.
  */
-#if ZYNQMP_ATF_LOCATION_ID == ZYNQMP_IN_TRUSTED_SRAM
-# define BL31_BASE			(ZYNQMP_TRUSTED_SRAM_LIMIT - \
-					 ZYNQMP_BL31_SIZE)
-# define BL31_PROGBITS_LIMIT		(ZYNQMP_TRUSTED_SRAM_LIMIT - 0x6000)
-# define BL31_LIMIT			ZYNQMP_TRUSTED_SRAM_LIMIT
-#elif ZYNQMP_ATF_LOCATION_ID == ZYNQMP_IN_TRUSTED_DRAM
-# define BL31_BASE			(ZYNQMP_TRUSTED_DRAM_LIMIT - \
-					 ZYNQMP_BL31_SIZE)
-# define BL31_PROGBITS_LIMIT		(ZYNQMP_TRUSTED_DRAM_LIMIT - 0x6000)
-# define BL31_LIMIT			(ZYNQMP_TRUSTED_DRAM_BASE + \
-					ZYNQMP_TRUSTED_DRAM_SIZE)
+#ifndef ZYNQMP_ATF_MEM_BASE
+# define BL31_BASE			0xfffe5000
+# define BL31_PROGBITS_LIMIT		0xffffa000
+# define BL31_LIMIT			0xffffffff
 #else
-# error "Unsupported ZYNQMP_ATF_LOCATION_ID value"
+# define BL31_BASE			(ZYNQMP_ATF_MEM_BASE)
+# define BL31_LIMIT			(ZYNQMP_ATF_MEM_BASE + ZYNQMP_ATF_MEM_SIZE - 1)
+# ifdef ZYNQMP_ATF_MEM_PROGBITS_SIZE
+#  define BL31_PROGBITS_LIMIT		(ZYNQMP_ATF_MEM_BASE + ZYNQMP_ATF_MEM_PROGBITS_SIZE - 1)
+# endif
 #endif
 
 /*******************************************************************************
  * BL32 specific defines.
  ******************************************************************************/
-/*
- * On ZYNQMP, the TSP can execute either from Trusted SRAM or Trusted DRAM.
- */
-#if ZYNQMP_TSP_RAM_LOCATION_ID == ZYNQMP_IN_TRUSTED_SRAM
-# define TSP_SEC_MEM_BASE		ZYNQMP_TRUSTED_SRAM_BASE
-# define TSP_SEC_MEM_SIZE		ZYNQMP_TRUSTED_SRAM_SIZE
-# define TSP_PROGBITS_LIMIT		(ZYNQMP_TRUSTED_SRAM_LIMIT - \
-					 ZYNQMP_BL31_SIZE)
-# define BL32_BASE			ZYNQMP_TRUSTED_SRAM_BASE
-# define BL32_LIMIT			(ZYNQMP_TRUSTED_SRAM_LIMIT - \
-					 ZYNQMP_BL31_SIZE)
-#elif ZYNQMP_TSP_RAM_LOCATION_ID == ZYNQMP_IN_TRUSTED_DRAM
-# define TSP_SEC_MEM_BASE		ZYNQMP_TRUSTED_DRAM_BASE
-# define TSP_SEC_MEM_SIZE		(ZYNQMP_TRUSTED_DRAM_LIMIT - \
-					 ZYNQMP_BL31_SIZE)
-# define BL32_BASE			ZYNQMP_TRUSTED_DRAM_BASE
-# define BL32_LIMIT			(ZYNQMP_TRUSTED_DRAM_LIMIT - \
-					 ZYNQMP_BL31_SIZE)
+#ifndef ZYNQMP_BL32_MEM_BASE
+# define BL32_BASE			0x60000000
+# define BL32_LIMIT			0x7fffffff
 #else
-# error "Unsupported ZYNQMP_TSP_RAM_LOCATION_ID value"
+# define BL32_BASE			(ZYNQMP_BL32_MEM_BASE)
+# define BL32_LIMIT			(ZYNQMP_BL32_MEM_BASE + ZYNQMP_BL32_MEM_SIZE - 1)
 #endif
 
-/*
- * ID of the secure physical generic timer interrupt used by the TSP.
- */
+/*******************************************************************************
+ * BL33 specific defines.
+ ******************************************************************************/
+#ifndef PRELOADED_BL33_BASE
+# define PLAT_ARM_NS_IMAGE_OFFSET	0x8000000
+#else
+# define PLAT_ARM_NS_IMAGE_OFFSET	PRELOADED_BL33_BASE
+#endif
+
+/*******************************************************************************
+ * TSP  specific defines.
+ ******************************************************************************/
+#define TSP_SEC_MEM_BASE		BL32_BASE
+#define TSP_SEC_MEM_SIZE		(BL32_LIMIT - BL32_BASE + 1)
+
+/* ID of the secure physical generic timer interrupt used by the TSP */
 #define TSP_IRQ_SEC_PHY_TIMER		ARM_IRQ_SEC_PHY_TIMER
 
 /*******************************************************************************
  * Platform specific page table and MMU setup constants
  ******************************************************************************/
 #define ADDR_SPACE_SIZE			(1ull << 32)
-#define MAX_XLAT_TABLES			5
-#define MAX_MMAP_REGIONS		7
+#define MAX_MMAP_REGIONS		6
+#if IMAGE_BL32
+# define MAX_XLAT_TABLES		5
+#else
+# define MAX_XLAT_TABLES		4
+#endif
 
 #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 d693a2db..56eb742a 100644
--- a/plat/xilinx/zynqmp/plat_psci.c
+++ b/plat/xilinx/zynqmp/plat_psci.c
@@ -104,7 +104,7 @@ static int zynqmp_pwr_domain_on(u_register_t mpidr)
 	proc = pm_get_proc(cpu_id);
 
 	/* Send request to PMU to wake up selected APU CPU core */
-	pm_req_wakeup(proc->node_id, 1, zynqmp_sec_entry, REQ_ACK_NO);
+	pm_req_wakeup(proc->node_id, 1, zynqmp_sec_entry, REQ_ACK_BLOCKING);
 
 	return PSCI_E_SUCCESS;
 }
diff --git a/plat/xilinx/zynqmp/plat_startup.c b/plat/xilinx/zynqmp/plat_startup.c
new file mode 100644
index 00000000..c2c9bfb0
--- /dev/null
+++ b/plat/xilinx/zynqmp/plat_startup.c
@@ -0,0 +1,288 @@
+/*
+ * 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:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <arch_helpers.h>
+#include <assert.h>
+#include <debug.h>
+#include <mmio.h>
+#include "zynqmp_def.h"
+
+/*
+ * ATFHandoffParams
+ * Parameter		bitfield	encoding
+ * -----------------------------------------------------------------------------
+ * Exec State		0		0 -> Aarch64, 1-> Aarch32
+ * endianness		1		0 -> LE, 1 -> BE
+ * secure (TZ)		2		0 -> Non secure, 1 -> secure
+ * EL			3:4		00 -> EL0, 01 -> EL1, 10 -> EL2, 11 -> EL3
+ * CPU#			5:6		00 -> A53_0, 01 -> A53_1, 10 -> A53_2, 11 -> A53_3
+ */
+
+#define FSBL_FLAGS_ESTATE_SHIFT		0
+#define FSBL_FLAGS_ESTATE_MASK		(1 << FSBL_FLAGS_ESTATE_SHIFT)
+#define FSBL_FLAGS_ESTATE_A64		0
+#define FSBL_FLAGS_ESTATE_A32		1
+
+#define FSBL_FLAGS_ENDIAN_SHIFT		1
+#define FSBL_FLAGS_ENDIAN_MASK		(1 << FSBL_FLAGS_ENDIAN_SHIFT)
+#define FSBL_FLAGS_ENDIAN_LE		0
+#define FSBL_FLAGS_ENDIAN_BE		1
+
+#define FSBL_FLAGS_TZ_SHIFT		2
+#define FSBL_FLAGS_TZ_MASK		(1 << FSBL_FLAGS_TZ_SHIFT)
+#define FSBL_FLAGS_NON_SECURE		0
+#define FSBL_FLAGS_SECURE		1
+
+#define FSBL_FLAGS_EL_SHIFT		3
+#define FSBL_FLAGS_EL_MASK		(3 << FSBL_FLAGS_EL_SHIFT)
+#define FSBL_FLAGS_EL0			0
+#define FSBL_FLAGS_EL1			1
+#define FSBL_FLAGS_EL2			2
+#define FSBL_FLAGS_EL3			3
+
+#define FSBL_FLAGS_CPU_SHIFT		5
+#define FSBL_FLAGS_CPU_MASK		(3 << FSBL_FLAGS_CPU_SHIFT)
+#define FSBL_FLAGS_A53_0		0
+#define FSBL_FLAGS_A53_1		1
+#define FSBL_FLAGS_A53_2		2
+#define FSBL_FLAGS_A53_3		3
+
+#define FSBL_MAX_PARTITIONS		8
+
+/* Structure corresponding to each partition entry */
+struct xfsbl_partition {
+	uint64_t entry_point;
+	uint64_t flags;
+};
+
+/* Structure for handoff parameters to ARM Trusted Firmware (ATF) */
+struct xfsbl_atf_handoff_params {
+	uint8_t magic[4];
+	uint32_t num_entries;
+	struct xfsbl_partition partition[FSBL_MAX_PARTITIONS];
+};
+
+/**
+ * @partition: Pointer to partition struct
+ *
+ * Get the target CPU for @partition.
+ *
+ * Return: FSBL_FLAGS_A53_0, FSBL_FLAGS_A53_1, FSBL_FLAGS_A53_2 or FSBL_FLAGS_A53_3
+ */
+static int get_fsbl_cpu(const struct xfsbl_partition *partition)
+{
+	uint64_t flags = partition->flags & FSBL_FLAGS_CPU_MASK;
+
+	return flags >> FSBL_FLAGS_CPU_SHIFT;
+}
+
+/**
+ * @partition: Pointer to partition struct
+ *
+ * Get the target exception level for @partition.
+ *
+ * Return: FSBL_FLAGS_EL0, FSBL_FLAGS_EL1, FSBL_FLAGS_EL2 or FSBL_FLAGS_EL3
+ */
+static int get_fsbl_el(const struct xfsbl_partition *partition)
+{
+	uint64_t flags = partition->flags & FSBL_FLAGS_EL_MASK;
+
+	return flags >> FSBL_FLAGS_EL_SHIFT;
+}
+
+/**
+ * @partition: Pointer to partition struct
+ *
+ * Get the target security state for @partition.
+ *
+ * Return: FSBL_FLAGS_NON_SECURE or FSBL_FLAGS_SECURE
+ */
+static int get_fsbl_ss(const struct xfsbl_partition *partition)
+{
+	uint64_t flags = partition->flags & FSBL_FLAGS_TZ_MASK;
+
+	return flags >> FSBL_FLAGS_TZ_SHIFT;
+}
+
+/**
+ * @partition: Pointer to partition struct
+ *
+ * Get the target endianness for @partition.
+ *
+ * Return: SPSR_E_LITTLE or SPSR_E_BIG
+ */
+static int get_fsbl_endian(const struct xfsbl_partition *partition)
+{
+	uint64_t flags = partition->flags & FSBL_FLAGS_ENDIAN_MASK;
+
+	flags >>= FSBL_FLAGS_ENDIAN_SHIFT;
+
+	if (flags == FSBL_FLAGS_ENDIAN_BE)
+		return SPSR_E_BIG;
+	else
+		return SPSR_E_LITTLE;
+}
+
+/**
+ * @partition: Pointer to partition struct
+ *
+ * Get the target execution state for @partition.
+ *
+ * Return: FSBL_FLAGS_ESTATE_A32 or FSBL_FLAGS_ESTATE_A64
+ */
+static int get_fsbl_estate(const struct xfsbl_partition *partition)
+{
+	uint64_t flags = partition->flags & FSBL_FLAGS_ESTATE_MASK;
+
+	return flags >> FSBL_FLAGS_ESTATE_SHIFT;
+}
+
+/**
+ * Populates the bl32 and bl33 image info structures
+ * @bl32:	BL32 image info structure
+ * @bl33:	BL33 image info structure
+ *
+ * Process the handoff paramters from the FSBL and populate the BL32 and BL33
+ * image info structures accordingly.
+ */
+void 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;
+
+	atf_handoff_addr = mmio_read_32(PMU_GLOBAL_GEN_STORAGE6);
+	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();
+	}
+
+	ATFHandoffParams = (struct xfsbl_atf_handoff_params *)atf_handoff_addr;
+	if ((ATFHandoffParams->magic[0] != 'X') ||
+	    (ATFHandoffParams->magic[1] != 'L') ||
+	    (ATFHandoffParams->magic[2] != 'N') ||
+	    (ATFHandoffParams->magic[3] != 'X')) {
+		ERROR("BL31: invalid ATF handoff structure at %lx\n",
+		      atf_handoff_addr);
+		panic();
+	}
+
+	VERBOSE("BL31: ATF handoff params at:0x%lx, entries:%u\n",
+		atf_handoff_addr, ATFHandoffParams->num_entries);
+	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();
+	}
+
+	/*
+	 * we loop over all passed entries but only populate two image structs
+	 * (bl32, bl33). I.e. the last applicable images in the handoff
+	 * structure will be used for the hand off
+	 */
+	for (size_t i = 0; i < ATFHandoffParams->num_entries; i++) {
+		entry_point_info_t *image;
+		int target_estate, target_secure;
+		int target_cpu, target_endianness, target_el;
+
+		VERBOSE("BL31: %zd: entry:0x%lx, flags:0x%lx\n", i,
+			ATFHandoffParams->partition[i].entry_point,
+			ATFHandoffParams->partition[i].flags);
+
+		target_cpu = get_fsbl_cpu(&ATFHandoffParams->partition[i]);
+		if (target_cpu != FSBL_FLAGS_A53_0) {
+			WARN("BL31: invalid target CPU (%i)\n", target_cpu);
+			continue;
+		}
+
+		target_el = get_fsbl_el(&ATFHandoffParams->partition[i]);
+		if ((target_el == FSBL_FLAGS_EL3) ||
+		    (target_el == FSBL_FLAGS_EL0)) {
+			WARN("BL31: invalid exception level (%i)\n", target_el);
+			continue;
+		}
+
+		target_secure = get_fsbl_ss(&ATFHandoffParams->partition[i]);
+		if (target_secure == FSBL_FLAGS_SECURE &&
+		    target_el == FSBL_FLAGS_EL2) {
+			WARN("BL31: invalid security state (%i) for exception level (%i)\n",
+			     target_secure, target_el);
+			continue;
+		}
+
+		target_estate = get_fsbl_estate(&ATFHandoffParams->partition[i]);
+		target_endianness = get_fsbl_endian(&ATFHandoffParams->partition[i]);
+
+		if (target_secure == FSBL_FLAGS_SECURE) {
+			image = bl32;
+
+			if (target_estate == FSBL_FLAGS_ESTATE_A32)
+				bl32->spsr = SPSR_MODE32(MODE32_svc, SPSR_T_ARM,
+							 target_endianness,
+							 DISABLE_ALL_EXCEPTIONS);
+			else
+				bl32->spsr = SPSR_64(MODE_EL1, MODE_SP_ELX,
+						     DISABLE_ALL_EXCEPTIONS);
+		} else {
+			image = bl33;
+
+			if (target_estate == FSBL_FLAGS_ESTATE_A32) {
+				if (target_el == FSBL_FLAGS_EL2)
+					target_el = MODE32_hyp;
+				else
+					target_el = MODE32_sys;
+
+				bl33->spsr = SPSR_MODE32(target_el, SPSR_T_ARM,
+							 target_endianness,
+							 DISABLE_ALL_EXCEPTIONS);
+			} else {
+				if (target_el == FSBL_FLAGS_EL2)
+					target_el = MODE_EL2;
+				else
+					target_el = MODE_EL1;
+
+				bl33->spsr = SPSR_64(target_el, MODE_SP_ELX,
+						     DISABLE_ALL_EXCEPTIONS);
+			}
+		}
+
+		VERBOSE("Setting up %s entry point to:%lx, el:%x\n",
+			target_secure == FSBL_FLAGS_SECURE ? "BL32" : "BL33",
+			ATFHandoffParams->partition[i].entry_point,
+			target_el);
+		image->pc = ATFHandoffParams->partition[i].entry_point;
+
+		if (target_endianness == SPSR_E_BIG)
+			EP_SET_EE(image->h.attr, EP_EE_BIG);
+		else
+			EP_SET_EE(image->h.attr, EP_EE_LITTLE);
+	}
+}
diff --git a/plat/xilinx/zynqmp/plat_topology.c b/plat/xilinx/zynqmp/plat_topology.c
index 34e7b53b..e1263e0e 100644
--- a/plat/xilinx/zynqmp/plat_topology.c
+++ b/plat/xilinx/zynqmp/plat_topology.c
@@ -28,9 +28,6 @@
  * POSSIBILITY OF SUCH DAMAGE.
  */
 
-#include <platform_def.h>
-#include <psci.h>
-
 static const unsigned char plat_power_domain_tree_desc[] = {1, 4};
 
 const unsigned char *plat_get_power_domain_tree_desc(void)
diff --git a/plat/xilinx/zynqmp/platform.mk b/plat/xilinx/zynqmp/platform.mk
index febff29f..abbb15a5 100644
--- a/plat/xilinx/zynqmp/platform.mk
+++ b/plat/xilinx/zynqmp/platform.mk
@@ -31,29 +31,27 @@ PROGRAMMABLE_RESET_ADDRESS := 1
 PSCI_EXTENDED_STATE_ID := 1
 A53_DISABLE_NON_TEMPORAL_HINT := 0
 
-ZYNQMP_ATF_LOCATION	?=	tsram
-ifeq (${ZYNQMP_ATF_LOCATION}, tsram)
-  ZYNQMP_ATF_LOCATION_ID := ZYNQMP_IN_TRUSTED_SRAM
-else ifeq (${ZYNQMP_ATF_LOCATION}, tdram)
-  ZYNQMP_ATF_LOCATION_ID := ZYNQMP_IN_TRUSTED_DRAM
-else
-  $(error "Unsupported ZYNQMP_ATF_LOCATION value")
-endif
+ifdef ZYNQMP_ATF_MEM_BASE
+    $(eval $(call add_define,ZYNQMP_ATF_MEM_BASE))
+
+    ifndef ZYNQMP_ATF_MEM_SIZE
+        $(error "ZYNQMP_ATF_BASE defined without ZYNQMP_ATF_SIZE")
+    endif
+    $(eval $(call add_define,ZYNQMP_ATF_MEM_SIZE))
 
-# On ZYNQMP, the TSP can execute either from Trusted SRAM or Trusted DRAM.
-# Trusted SRAM is the default.
-ZYNQMP_TSP_RAM_LOCATION	?=	tsram
-ifeq (${ZYNQMP_TSP_RAM_LOCATION}, tsram)
-  ZYNQMP_TSP_RAM_LOCATION_ID := ZYNQMP_IN_TRUSTED_SRAM
-else ifeq (${ZYNQMP_TSP_RAM_LOCATION}, tdram)
-  ZYNQMP_TSP_RAM_LOCATION_ID := ZYNQMP_IN_TRUSTED_DRAM
-else
-  $(error "Unsupported ZYNQMP_TSP_RAM_LOCATION value")
+    ifdef ZYNQMP_ATF_MEM_PROGBITS_SIZE
+        $(eval $(call add_define,ZYNQMP_ATF_MEM_PROGBITS_SIZE))
+    endif
 endif
 
-# Process flags
-$(eval $(call add_define,ZYNQMP_ATF_LOCATION_ID))
-$(eval $(call add_define,ZYNQMP_TSP_RAM_LOCATION_ID))
+ifdef ZYNQMP_BL32_MEM_BASE
+    $(eval $(call add_define,ZYNQMP_BL32_MEM_BASE))
+
+    ifndef ZYNQMP_BL32_MEM_SIZE
+        $(error "ZYNQMP_BL32_BASE defined without ZYNQMP_BL32_SIZE")
+    endif
+    $(eval $(call add_define,ZYNQMP_BL32_MEM_SIZE))
+endif
 
 PLAT_INCLUDES		:=	-Iinclude/plat/arm/common/			\
 				-Iinclude/plat/arm/common/aarch64/		\
@@ -84,6 +82,7 @@ BL31_SOURCES		+=	drivers/arm/cci/cci.c				\
 				plat/xilinx/zynqmp/bl31_zynqmp_setup.c		\
 				plat/xilinx/zynqmp/plat_psci.c			\
 				plat/xilinx/zynqmp/plat_zynqmp.c		\
+				plat/xilinx/zynqmp/plat_startup.c		\
 				plat/xilinx/zynqmp/plat_topology.c		\
 				plat/xilinx/zynqmp/sip_svc_setup.c		\
 				plat/xilinx/zynqmp/pm_service/pm_svc_main.c	\
diff --git a/plat/xilinx/zynqmp/pm_service/pm_api_sys.c b/plat/xilinx/zynqmp/pm_service/pm_api_sys.c
index 2f2e2edf..eac7801b 100644
--- a/plat/xilinx/zynqmp/pm_service/pm_api_sys.c
+++ b/plat/xilinx/zynqmp/pm_service/pm_api_sys.c
@@ -36,6 +36,7 @@
 #include <arch_helpers.h>
 #include <platform.h>
 #include "pm_client.h"
+#include "pm_ipi.h"
 #include "pm_common.h"
 #include "pm_api_sys.h"
 
@@ -390,21 +391,34 @@ enum pm_ret_status pm_register_notifier(enum pm_node_id nid,
 					unsigned int wake,
 					unsigned int enable)
 {
-	return PM_RET_ERROR_NOTSUPPORTED;
+	uint32_t payload[PAYLOAD_ARG_CNT];
+
+	PM_PACK_PAYLOAD5(payload, PM_REGISTER_NOTIFIER,
+			 nid, event, wake, enable);
+
+	return pm_ipi_send(primary_proc, payload);
 }
 
 /**
- * pm_get_op_characteristic() - PM call to get a particular operating
- *				characteristic of a node
- * @nid	Node ID
- * @type	Operating characterstic type to be returned
+ * pm_get_op_characteristic() - PM call to request operating characteristics
+ *				of a node
+ * @nid		Node id of the slave
+ * @type	Type of the operating characteristic
+ *		(power, temperature and latency)
+ * @result	Returns the operating characteristic for the requested node,
+ *		specified by the type
  *
  * @return	Returns status, either success or error+reason
  */
 enum pm_ret_status pm_get_op_characteristic(enum pm_node_id nid,
-					    enum pm_opchar_type type)
+					    enum pm_opchar_type type,
+					    uint32_t *result)
 {
-	return PM_RET_ERROR_NOTSUPPORTED;
+	uint32_t payload[PAYLOAD_ARG_CNT];
+
+	/* Send request to the PMU */
+	PM_PACK_PAYLOAD3(payload, PM_GET_OP_CHARACTERISTIC, nid, type);
+	return pm_ipi_send_sync(primary_proc, payload, result);
 }
 
 /* Direct-Control API functions */
diff --git a/plat/xilinx/zynqmp/pm_service/pm_api_sys.h b/plat/xilinx/zynqmp/pm_service/pm_api_sys.h
index f0365cd9..22bdb477 100644
--- a/plat/xilinx/zynqmp/pm_service/pm_api_sys.h
+++ b/plat/xilinx/zynqmp/pm_service/pm_api_sys.h
@@ -91,7 +91,8 @@ enum pm_ret_status pm_register_notifier(enum pm_node_id nid,
 					unsigned int wake,
 					unsigned int enable);
 enum pm_ret_status pm_get_op_characteristic(enum pm_node_id nid,
-					    enum pm_opchar_type type);
+					    enum pm_opchar_type type,
+					    uint32_t *result);
 enum pm_ret_status pm_acknowledge_cb(enum pm_node_id nid,
 				     enum pm_ret_status status,
 				     unsigned int oppoint);
diff --git a/plat/xilinx/zynqmp/pm_service/pm_client.c b/plat/xilinx/zynqmp/pm_service/pm_client.c
index eb986bca..d3396dfa 100644
--- a/plat/xilinx/zynqmp/pm_service/pm_client.c
+++ b/plat/xilinx/zynqmp/pm_service/pm_client.c
@@ -33,6 +33,7 @@
  * for getting information about and changing state of the APU.
  */
 
+#include <bakery_lock.h>
 #include <gicv2.h>
 #include <bl_common.h>
 #include <mmio.h>
@@ -47,6 +48,7 @@
 #define OCM_BANK_3	(OCM_BANK_2 + 0x10000)
 
 #define UNDEFINED_CPUID		(~0)
+DEFINE_BAKERY_LOCK(pm_client_secure_lock);
 
 /* Declaration of linker defined symbol */
 extern unsigned long __BL31_END__;
@@ -162,8 +164,12 @@ const struct pm_proc *primary_proc = &pm_procs_all[0];
  */
 void pm_client_suspend(const struct pm_proc *proc)
 {
+	bakery_lock_get(&pm_client_secure_lock);
+
 	/* Set powerdown request */
 	mmio_write_32(APU_PWRCTL, mmio_read_32(APU_PWRCTL) | proc->pwrdn_mask);
+
+	bakery_lock_release(&pm_client_secure_lock);
 }
 
 
@@ -177,9 +183,14 @@ void pm_client_abort_suspend(void)
 {
 	/* Enable interrupts at processor level (for current cpu) */
 	gicv2_cpuif_enable();
+
+	bakery_lock_get(&pm_client_secure_lock);
+
 	/* Clear powerdown request */
 	mmio_write_32(APU_PWRCTL,
 		 mmio_read_32(APU_PWRCTL) & ~primary_proc->pwrdn_mask);
+
+	bakery_lock_release(&pm_client_secure_lock);
 }
 
 /**
@@ -195,8 +206,12 @@ void pm_client_wakeup(const struct pm_proc *proc)
 	if (cpuid == UNDEFINED_CPUID)
 		return;
 
+	bakery_lock_get(&pm_client_secure_lock);
+
 	/* clear powerdown bit for affected cpu */
 	uint32_t val = mmio_read_32(APU_PWRCTL);
 	val &= ~(proc->pwrdn_mask);
 	mmio_write_32(APU_PWRCTL, val);
+
+	bakery_lock_release(&pm_client_secure_lock);
 }
diff --git a/plat/xilinx/zynqmp/pm_service/pm_client.h b/plat/xilinx/zynqmp/pm_service/pm_client.h
index b9f196d7..9483b0d1 100644
--- a/plat/xilinx/zynqmp/pm_service/pm_client.h
+++ b/plat/xilinx/zynqmp/pm_service/pm_client.h
@@ -40,11 +40,6 @@
 #include "pm_common.h"
 
 /* Functions to be implemented by each PU */
-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_sync(const struct pm_proc *proc,
-				    uint32_t payload[PAYLOAD_ARG_CNT],
-				    uint32_t *val);
 void pm_client_suspend(const struct pm_proc *proc);
 void pm_client_abort_suspend(void);
 void pm_client_wakeup(const struct pm_proc *proc);
diff --git a/plat/xilinx/zynqmp/pm_service/pm_defs.h b/plat/xilinx/zynqmp/pm_service/pm_defs.h
index 08f4aab7..adeec64b 100644
--- a/plat/xilinx/zynqmp/pm_service/pm_defs.h
+++ b/plat/xilinx/zynqmp/pm_service/pm_defs.h
@@ -142,6 +142,7 @@ enum pm_node_id {
 	NODE_RPLL,
 	NODE_IOPLL,
 	NODE_DDR,
+	NODE_IPI_APU,
 };
 
 enum pm_request_ack {
@@ -171,8 +172,8 @@ enum pm_ram_state {
 
 enum pm_opchar_type {
 	PM_OPCHAR_TYPE_POWER = 1,
-	PM_OPCHAR_TYPE_ENERGY,
 	PM_OPCHAR_TYPE_TEMP,
+	PM_OPCHAR_TYPE_LATENCY,
 };
 
 /**
diff --git a/plat/xilinx/zynqmp/pm_service/pm_ipi.c b/plat/xilinx/zynqmp/pm_service/pm_ipi.c
index ef2b4fe6..c3e7ccb5 100644
--- a/plat/xilinx/zynqmp/pm_service/pm_ipi.c
+++ b/plat/xilinx/zynqmp/pm_service/pm_ipi.c
@@ -66,8 +66,7 @@
 #define IPI_APU_ISR		(IPI_BASEADDR + 0X00000010)
 #define IPI_APU_IER		(IPI_BASEADDR + 0X00000018)
 #define IPI_APU_IDR		(IPI_BASEADDR + 0X0000001C)
-#define IPI_APU_ISR_PMU_0_MASK		0X00010000
-#define IPI_APU_IER_PMU_0_MASK		0X00010000
+#define IPI_APU_IXR_PMU_0_MASK		(1 << 16)
 
 #define IPI_TRIG_OFFSET		0
 #define IPI_OBS_OFFSET		4
@@ -75,14 +74,14 @@
 /* Power Management IPI interrupt number */
 #define PM_INT_NUM		0
 #define IPI_PMU_PM_INT_BASE	(IPI_PMU_0_TRIG + (PM_INT_NUM * 0x1000))
-#define IPI_PMU_PM_INT_MASK	(IPI_APU_ISR_PMU_0_MASK << PM_INT_NUM)
+#define IPI_PMU_PM_INT_MASK	(IPI_APU_IXR_PMU_0_MASK << PM_INT_NUM)
 #if (PM_INT_NUM < 0 || PM_INT_NUM > 3)
 	#error PM_INT_NUM value out of range
 #endif
 
 #define IPI_APU_MASK		1U
 
-static bakery_lock_t pm_secure_lock;
+DEFINE_BAKERY_LOCK(pm_secure_lock);
 
 const struct pm_ipi apu_ipi = {
 	.mask = IPI_APU_MASK,
diff --git a/plat/xilinx/zynqmp/pm_service/pm_svc_main.c b/plat/xilinx/zynqmp/pm_service/pm_svc_main.c
index 67440650..e3c25c31 100644
--- a/plat/xilinx/zynqmp/pm_service/pm_svc_main.c
+++ b/plat/xilinx/zynqmp/pm_service/pm_svc_main.c
@@ -191,8 +191,12 @@ uint64_t pm_smc_handler(uint32_t smc_fid, uint64_t x1, uint64_t x2, uint64_t x3,
 		SMC_RET1(handle, (uint64_t)ret);
 
 	case PM_GET_OP_CHARACTERISTIC:
-		ret = pm_get_op_characteristic(pm_arg[0], pm_arg[1]);
-		SMC_RET1(handle, (uint64_t)ret);
+	{
+		uint32_t result;
+
+		ret = pm_get_op_characteristic(pm_arg[0], pm_arg[1], &result);
+		SMC_RET1(handle, (uint64_t)ret | ((uint64_t)result << 32));
+	}
 
 	case PM_REGISTER_NOTIFIER:
 		ret = pm_register_notifier(pm_arg[0], pm_arg[1], pm_arg[2],
diff --git a/plat/xilinx/zynqmp/tsp/tsp_plat_setup.c b/plat/xilinx/zynqmp/tsp/tsp_plat_setup.c
index 291ccbac..d600450d 100644
--- a/plat/xilinx/zynqmp/tsp/tsp_plat_setup.c
+++ b/plat/xilinx/zynqmp/tsp/tsp_plat_setup.c
@@ -32,9 +32,7 @@
 #include <console.h>
 #include <debug.h>
 #include <platform_tsp.h>
-#include <xlat_tables.h>
 #include <plat_arm.h>
-#include "../zynqmp_def.h"
 #include "../zynqmp_private.h"
 
 /*
diff --git a/plat/xilinx/zynqmp/zynqmp_def.h b/plat/xilinx/zynqmp/zynqmp_def.h
index 32190e41..4ff1f468 100644
--- a/plat/xilinx/zynqmp/zynqmp_def.h
+++ b/plat/xilinx/zynqmp/zynqmp_def.h
@@ -43,24 +43,11 @@
 /*******************************************************************************
  * ZYNQMP memory map related constants
  ******************************************************************************/
-
-#define ZYNQMP_TRUSTED_SRAM_BASE	0xFFFC0000
-#define ZYNQMP_TRUSTED_SRAM_SIZE	0x00040000
-#define ZYNQMP_TRUSTED_SRAM_LIMIT	(ZYNQMP_TRUSTED_SRAM_BASE + \
-					 ZYNQMP_TRUSTED_SRAM_SIZE)
-
-
-/* Location of trusted dram on the base zynqmp */
-#define ZYNQMP_TRUSTED_DRAM_BASE	0x30000000 /* Can't overlap TZROM area */
-#define ZYNQMP_TRUSTED_DRAM_SIZE	0x10000000
-#define ZYNQMP_TRUSTED_DRAM_LIMIT	(ZYNQMP_TRUSTED_DRAM_BASE + \
-					 ZYNQMP_TRUSTED_DRAM_SIZE)
-
 /* Aggregate of all devices in the first GB */
 #define DEVICE0_BASE		0xFF000000
 #define DEVICE0_SIZE		0x00E00000
 #define DEVICE1_BASE		0xF9000000
-#define DEVICE1_SIZE		0x01000000
+#define DEVICE1_SIZE		0x00800000
 
 /* For cpu reset APU space here too 0xFE5F1000 CRF_APB*/
 #define CRF_APB_BASE		0xFD1A0000
@@ -76,6 +63,7 @@
 #define CRL_APB_BASE			0xFF5E0000
 #define CRL_APB_RPLL_CTRL		(CRL_APB_BASE + 0x30)
 #define CRL_APB_TIMESTAMP_REF_CTRL	(CRL_APB_BASE + 0x128)
+#define CRL_APB_BOOT_MODE_USER		(CRL_APB_BASE + 0x200)
 #define CRL_APB_RESET_CTRL		(CRL_APB_BASE + 0x218)
 
 #define CRL_APB_TIMESTAMP_REF_CTRL_CLKACT_BIT	(1 << 24)
@@ -84,6 +72,9 @@
 
 #define CRL_APB_RESET_CTRL_SOFT_RESET	(1 << 4)
 
+#define CRL_APB_BOOT_MODE_MASK		(0xf << 0)
+#define ZYNQMP_BOOTMODE_JTAG		0
+
 /* system counter registers and bitfields */
 #define IOU_SCNTRS_BASE			0xFF260000
 #define IOU_SCNTRS_CONTROL		(IOU_SCNTRS_BASE + 0)
@@ -107,6 +98,7 @@
 /* PMU registers and bitfields */
 #define PMU_GLOBAL_BASE			0xFFD80000
 #define PMU_GLOBAL_CNTRL		(PMU_GLOBAL_BASE + 0)
+#define PMU_GLOBAL_GEN_STORAGE6		(PMU_GLOBAL_BASE + 0x48)
 #define PMU_GLOBAL_REQ_PWRUP_STATUS	(PMU_GLOBAL_BASE + 0x110)
 #define PMU_GLOBAL_REQ_PWRUP_EN		(PMU_GLOBAL_BASE + 0x118)
 #define PMU_GLOBAL_REQ_PWRUP_DIS	(PMU_GLOBAL_BASE + 0x11c)
@@ -114,16 +106,6 @@
 
 #define PMU_GLOBAL_CNTRL_FW_IS_PRESENT	(1 << 4)
 
-#define DRAM1_BASE		0x00000000ull
-#define DRAM1_SIZE		0x10000000ull
-#define DRAM1_END		(DRAM1_BASE + DRAM1_SIZE - 1)
-
-#define DRAM_BASE		DRAM1_BASE
-#define DRAM_SIZE		DRAM1_SIZE
-
-/* Load address of BL33 in the ZYNQMP port */
-#define PLAT_ARM_NS_IMAGE_OFFSET	(DRAM1_BASE + 0x8000000) /* DRAM + 128MB */
-
 /*******************************************************************************
  * CCI-400 related constants
  ******************************************************************************/
diff --git a/plat/xilinx/zynqmp/zynqmp_private.h b/plat/xilinx/zynqmp/zynqmp_private.h
index 1f5be39b..ddef37b8 100644
--- a/plat/xilinx/zynqmp/zynqmp_private.h
+++ b/plat/xilinx/zynqmp/zynqmp_private.h
@@ -38,5 +38,10 @@ 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,
+		       entry_point_info_t *bl33_image_ep_info);
 
 #endif /* __ZYNQMP_PRIVATE_H__ */