Merge pull request #100 from jcastillo-arm:jc/tf-issues/149-v4
diff --git a/bl2/bl2_main.c b/bl2/bl2_main.c
index 33c63ee..81fecd6 100644
--- a/bl2/bl2_main.c
+++ b/bl2/bl2_main.c
@@ -134,32 +134,34 @@
 	bl2_plat_set_bl33_ep_info(bl2_to_bl31_params->bl33_image_info,
 				bl2_to_bl31_params->bl33_ep_info);
 
+
+#ifdef BL32_BASE
 	/*
 	 * Load the BL32 image if there's one. It is upto to platform
 	 * to specify where BL32 should be loaded if it exists. It
 	 * could create space in the secure sram or point to a
-	 * completely different memory. A zero size indicates that the
-	 * platform does not want to load a BL32 image.
+	 * completely different memory.
+	 *
+	 * If a platform does not want to attempt to load BL3-2 image
+	 * it must leave BL32_BASE undefined
 	 */
 	bl2_plat_get_bl32_meminfo(&bl32_mem_info);
-	if (bl32_mem_info.total_size) {
-		e = load_image(&bl32_mem_info,
-			       BL32_IMAGE_NAME,
-			       bl32_mem_info.attr &
-			       LOAD_MASK,
-			       BL32_BASE,
-			       bl2_to_bl31_params->bl32_image_info,
-			       bl2_to_bl31_params->bl32_ep_info);
+	e = load_image(&bl32_mem_info,
+		       BL32_IMAGE_NAME,
+		       bl32_mem_info.attr & LOAD_MASK,
+		       BL32_BASE,
+		       bl2_to_bl31_params->bl32_image_info,
+		       bl2_to_bl31_params->bl32_ep_info);
 
-		/* Halt if failed to load normal world firmware. */
-		if (e) {
-			WARN("Failed to load BL3-2.\n");
-		} else {
-			bl2_plat_set_bl32_ep_info(
-				bl2_to_bl31_params->bl32_image_info,
-				bl2_to_bl31_params->bl32_ep_info);
-		}
+	/* Issue a diagnostic if no Secure Payload could be loaded */
+	if (e) {
+		WARN("Failed to load BL3-2.\n");
+	} else {
+		bl2_plat_set_bl32_ep_info(
+			bl2_to_bl31_params->bl32_image_info,
+			bl2_to_bl31_params->bl32_ep_info);
 	}
+#endif /* BL32_BASE */
 
 	/*
 	 * Run BL31 via an SMC to BL1. Information on how to pass control to
diff --git a/bl31/bl31_main.c b/bl31/bl31_main.c
index 1dc0a7a..ff7caf1 100644
--- a/bl31/bl31_main.c
+++ b/bl31/bl31_main.c
@@ -168,9 +168,15 @@
 	assert(next_image_info);
 
 	scr = read_scr();
+	scr &= ~SCR_NS_BIT;
 	if (image_type == NON_SECURE)
 		scr |= SCR_NS_BIT;
 
+	scr &= ~SCR_RW_BIT;
+	if ((next_image_info->spsr & (1 << MODE_RW_SHIFT)) ==
+				(MODE_RW_64 << MODE_RW_SHIFT))
+		scr |= SCR_RW_BIT;
+
 	/*
 	 * Tell the context mgmt. library to ensure that SP_EL3 points to
 	 * the right context to exit from EL3 correctly.
diff --git a/bl32/tsp/tsp.ld.S b/bl32/tsp/tsp.ld.S
index 53bce7d..559ae3d 100644
--- a/bl32/tsp/tsp.ld.S
+++ b/bl32/tsp/tsp.ld.S
@@ -36,7 +36,7 @@
 
 
 MEMORY {
-    RAM (rwx): ORIGIN = TZDRAM_BASE, LENGTH = TZDRAM_SIZE
+    RAM (rwx): ORIGIN = TSP_SEC_MEM_BASE, LENGTH = TSP_SEC_MEM_SIZE
 }
 
 
@@ -113,11 +113,11 @@
         __COHERENT_RAM_END__ = .;
     } >RAM
 
-    __BL2_END__ = .;
+    __BL32_END__ = .;
 
     __BSS_SIZE__ = SIZEOF(.bss);
     __COHERENT_RAM_UNALIGNED_SIZE__ =
         __COHERENT_RAM_END_UNALIGNED__ - __COHERENT_RAM_START__;
 
-    ASSERT(. <= TZDRAM_BASE + (1 << 21), "BL32 image does not fit in the first 2MB of Trusted DRAM.")
+    ASSERT(. <= BL32_LIMIT, "BL3-2 image does not fit.")
 }
diff --git a/docs/firmware-design.md b/docs/firmware-design.md
index a40ddac..76c27f7 100644
--- a/docs/firmware-design.md
+++ b/docs/firmware-design.md
@@ -219,6 +219,15 @@
 abstraction layer is initialized which is used to load further bootloader
 images.
 
+#### BL3-0 (System Control Processor Firmware) image load
+
+Some systems have a separate System Control Processor (SCP) for power, clock,
+reset and system control. BL2 loads the optional BL3-0 image from platform
+storage into a platform-specific region of secure memory. The subsequent
+handling of BL3-0 is platform specific. Typically the image is transferred into
+SCP memory using a platform-specific protocol. The SCP executes BL3-0 and
+signals to the Application Processor (AP) for BL2 execution to continue.
+
 #### BL3-1 (EL3 Runtime Firmware) image load
 
 BL2 loads the BL3-1 image from platform storage into a platform-specific address
diff --git a/docs/porting-guide.md b/docs/porting-guide.md
index 721ba1b..eb203a7 100644
--- a/docs/porting-guide.md
+++ b/docs/porting-guide.md
@@ -191,9 +191,36 @@
     image. Must be aligned on a page-size boundary.
 
 *   **#define : NS_IMAGE_OFFSET**
+
     Defines the base address in non-secure DRAM where BL2 loads the BL3-3 binary
     image. Must be aligned on a page-size boundary.
 
+If the BL3-2 image is supported by the platform, the following constants must
+be defined as well:
+
+*   **#define : TSP_SEC_MEM_BASE**
+
+    Defines the base address of the secure memory used by the BL3-2 image on the
+    platform.
+
+*   **#define : TSP_SEC_MEM_SIZE**
+
+    Defines the size of the secure memory used by the BL3-2 image on the
+    platform.
+
+*   **#define : BL32_BASE**
+
+    Defines the base address in secure memory where BL2 loads the BL3-2 binary
+    image. Must be inside the secure memory identified by `TSP_SEC_MEM_BASE` and
+    `TSP_SEC_MEM_SIZE` constants. Must also be aligned on a page-size boundary.
+
+*   **#define : BL32_LIMIT**
+
+    Defines the maximum address that the BL3-2 image can occupy. Must be inside
+    the secure memory identified by `TSP_SEC_MEM_BASE` and `TSP_SEC_MEM_SIZE`
+    constants.
+
+
 ### File : platform_macros.S [mandatory]
 
 Each platform must export a file of this name with the following
@@ -555,23 +582,24 @@
     if the platform wants to restrict the amount of memory visible to BL3-1.
     Details of this function are given below.
 
-4.  Loading the BL3-2 binary image (if present) in platform provided memory
-    using semi-hosting. To load the BL3-2 image, BL2 makes use of the
-    `bl32_meminfo` field in the `bl31_args` structure to which a pointer is
+4.  (Optional) Loading the BL3-2 binary image (if present) from platform
+    provided non-volatile storage. To load the BL3-2 image, BL2 makes use of
+    the `bl32_meminfo` field in the `bl31_args` structure to which a pointer is
     returned by the `bl2_get_bl31_args_ptr()` function. The platform also
-    defines the address in memory where BL3-2 is loaded through the constant
-    `BL32_BASE`. BL2 uses this information to determine if there is enough
-    memory to load the BL3-2 image.
+    defines the address in memory where BL3-2 is loaded through the optional
+    constant `BL32_BASE`. BL2 uses this information to determine if there is
+    enough memory to load the BL3-2 image. If `BL32_BASE` is not defined then
+    this and the following two steps are not performed.
 
-5.  Arranging to pass control to the BL3-2 image (if present) that has been
-    pre-loaded at `BL32_BASE`. BL2 populates an `el_change_info` structure
-    in memory provided by the platform with information about how BL3-1 should
-    pass control to the BL3-2 image. This structure follows the
+5.  (Optional) Arranging to pass control to the BL3-2 image (if present) that
+    has been pre-loaded at `BL32_BASE`. BL2 populates an `el_change_info`
+    structure in memory provided by the platform with information about how
+    BL3-1 should pass control to the BL3-2 image. This structure follows the
     `el_change_info` structure populated for the normal world BL image in 2.
     above.
 
-6.  Populating a `meminfo` structure with the following information in
-    memory that is accessible by BL3-1 immediately upon entry.
+6.  (Optional) Populating a `meminfo` structure with the following information
+    in memory that is accessible by BL3-1 immediately upon entry.
 
         meminfo.total_base = Base address of memory visible to BL3-2
         meminfo.total_size = Size of memory visible to BL3-2
@@ -581,7 +609,7 @@
                              BL3-2
 
     BL2 populates this information in the `bl32_meminfo` field of the pointer
-    returned by the `bl2_get_bl31_args_ptr() function.
+    returned by the `bl2_get_bl31_args_ptr()` function.
 
 The following functions must be implemented by the platform port to enable BL2
 to perform the above tasks.
diff --git a/docs/user-guide.md b/docs/user-guide.md
index 3359fee..6050831 100644
--- a/docs/user-guide.md
+++ b/docs/user-guide.md
@@ -89,7 +89,8 @@
     By default this produces a release version of the build. To produce a debug
     version instead, refer to the "Debugging options" section below. UEFI can be
     used as the BL3-3 image, refer to the "Obtaining the normal world software"
-    section below.
+    section below. By default this won't compile the TSP in, refer to the
+    "Building the Test Secure Payload" section below.
 
     The build process creates products in a `build` directory tree, building
     the objects and binaries for each boot loader stage in separate
@@ -243,6 +244,48 @@
 NOTE: The Foundation FVP does not provide a debugger interface.
 
 
+### Building the Test Secure Payload
+
+The TSP is coupled with a companion runtime service in the BL3-1 firmware,
+called the TSPD. Therefore, if you intend to use the TSP, the BL3-1 image
+must be recompiled as well. For more information on SPs and SPDs, see the
+"Secure-EL1 Payloads and Dispatchers" section in the [Firmware Design].
+
+First clean the Trusted Firmware build directory to get rid of any previous
+BL3-1 binary. Then to build the TSP image and include it into the FIP use:
+
+    CROSS_COMPILE=<path-to-aarch64-gcc>/bin/aarch64-none-elf- \
+    BL33=<path-to>/<bl33_image>                               \
+    make PLAT=fvp SPD=tspd all fip
+
+An additional boot loader binary file is created in the `build` directory:
+
+    *   `build/<platform>/<build-type>/bl32.bin`
+
+The Firmware Package contains this new image:
+
+    Firmware Image Package ToC:
+    ---------------------------
+    - Trusted Boot Firmware BL2: offset=0xD8, size=0x6000
+      file: './build/fvp/release/bl2.bin'
+    - EL3 Runtime Firmware BL3-1: offset=0x60D8, size=0x9000
+      file: './build/fvp/release/bl31.bin'
+    - Secure Payload BL3-2 (Trusted OS): offset=0xF0D8, size=0x3000
+      file: './build/fvp/release/bl32.bin'
+    - Non-Trusted Firmware BL3-3: offset=0x120D8, size=0x280000
+      file: '../FVP_AARCH64_EFI.fd'
+    ---------------------------
+    Creating "build/fvp/release/fip.bin"
+
+On FVP, the TSP binary runs from Trusted SRAM by default. It is also possible
+to run it from Trusted DRAM. This is controlled by the build configuration
+`TSP_RAM_LOCATION`:
+
+    CROSS_COMPILE=<path-to-aarch64-gcc>/bin/aarch64-none-elf- \
+    BL33=<path-to>/<bl33_image>                               \
+    make PLAT=fvp SPD=tspd TSP_RAM_LOCATION=tdram all fip
+
+
 ### Checking source code style
 
 When making changes to the source for submission to the project, the source
diff --git a/drivers/io/io_fip.c b/drivers/io/io_fip.c
index 4848a70..8ed7f47 100644
--- a/drivers/io/io_fip.c
+++ b/drivers/io/io_fip.c
@@ -65,7 +65,10 @@
 static const plat_fip_name_uuid_t name_uuid[] = {
 	{BL2_IMAGE_NAME, UUID_TRUSTED_BOOT_FIRMWARE_BL2},
 	{BL31_IMAGE_NAME, UUID_EL3_RUNTIME_FIRMWARE_BL31},
+#ifdef BL32_IMAGE_NAME
+	/* BL3-2 is optional in the platform */
 	{BL32_IMAGE_NAME, UUID_SECURE_PAYLOAD_BL32},
+#endif /* BL32_IMAGE_NAME */
 	{BL33_IMAGE_NAME, UUID_NON_TRUSTED_FIRMWARE_BL33},
 };
 
diff --git a/include/bl31/services/psci.h b/include/bl31/services/psci.h
index 570fe5b..b6e272c 100644
--- a/include/bl31/services/psci.h
+++ b/include/bl31/services/psci.h
@@ -190,6 +190,7 @@
 extern int psci_cpu_on(unsigned long,
 		       unsigned long,
 		       unsigned long);
+extern void __dead2 psci_power_down_wfi(void);
 extern void psci_aff_on_finish_entry(void);
 extern void psci_aff_suspend_finish_entry(void);
 extern void psci_register_spd_pm_hook(const spd_pm_ops_t *);
diff --git a/include/drivers/arm/pl011.h b/include/drivers/arm/pl011.h
index 1254920..281330e 100644
--- a/include/drivers/arm/pl011.h
+++ b/include/drivers/arm/pl011.h
@@ -107,32 +107,32 @@
  * Pl011 CPU interface accessors for writing registers
  ******************************************************************************/
 
-static inline void pl011_write_ibrd(unsigned int base, unsigned int val)
+static inline void pl011_write_ibrd(unsigned long base, unsigned int val)
 {
 	mmio_write_32(base + UARTIBRD, val);
 }
 
-static inline void pl011_write_fbrd(unsigned int base, unsigned int val)
+static inline void pl011_write_fbrd(unsigned long base, unsigned int val)
 {
 	mmio_write_32(base + UARTFBRD, val);
 }
 
-static inline void pl011_write_lcr_h(unsigned int base, unsigned int val)
+static inline void pl011_write_lcr_h(unsigned long base, unsigned int val)
 {
 	mmio_write_32(base + UARTLCR_H, val);
 }
 
-static inline void pl011_write_ecr(unsigned int base, unsigned int val)
+static inline void pl011_write_ecr(unsigned long base, unsigned int val)
 {
 	mmio_write_32(base + UARTECR, val);
 }
 
-static inline void pl011_write_cr(unsigned int base, unsigned int val)
+static inline void pl011_write_cr(unsigned long base, unsigned int val)
 {
 	mmio_write_32(base + UARTCR, val);
 }
 
-static inline void pl011_write_dr(unsigned int base, unsigned int val)
+static inline void pl011_write_dr(unsigned long base, unsigned int val)
 {
 	mmio_write_32(base + UARTDR, val);
 }
@@ -141,12 +141,12 @@
  * Pl011 CPU interface accessors for reading registers
  ******************************************************************************/
 
-static inline unsigned int pl011_read_fr(unsigned int base)
+static inline unsigned int pl011_read_fr(unsigned long base)
 {
 	return mmio_read_32(base + UARTFR);
 }
 
-static inline unsigned int pl011_read_dr(unsigned int base)
+static inline unsigned int pl011_read_dr(unsigned long base)
 {
 	return mmio_read_32(base + UARTDR);
 }
diff --git a/lib/aarch64/xlat_tables.c b/lib/aarch64/xlat_tables.c
index 6e04f65..48b0714 100644
--- a/lib/aarch64/xlat_tables.c
+++ b/lib/aarch64/xlat_tables.c
@@ -173,7 +173,7 @@
 	unsigned level_size_shift = L1_XLAT_ADDRESS_SHIFT - (level - 1) *
 						XLAT_TABLE_ENTRIES_SHIFT;
 	unsigned level_size = 1 << level_size_shift;
-	unsigned level_index_mask = XLAT_TABLE_ENTRIES_MASK << level_size_shift;
+	unsigned long level_index_mask = XLAT_TABLE_ENTRIES_MASK << level_size_shift;
 
 	assert(level <= 3);
 
diff --git a/plat/fvp/bl2_plat_setup.c b/plat/fvp/bl2_plat_setup.c
index 8c006e9..cf987ff 100644
--- a/plat/fvp/bl2_plat_setup.c
+++ b/plat/fvp/bl2_plat_setup.c
@@ -98,11 +98,13 @@
 {
 	bl2_to_bl31_params_mem_t *bl31_params_mem;
 
+#if TSP_RAM_LOCATION_ID == TSP_IN_TZDRAM
 	/*
 	 * Ensure that the secure DRAM memory used for passing BL31 arguments
 	 * does not overlap with the BL32_BASE.
 	 */
 	assert(BL32_BASE > PARAMS_BASE + sizeof(bl2_to_bl31_params_mem_t));
+#endif
 
 	/*
 	 * Allocate the memory for all the arguments that needs to
@@ -265,15 +267,13 @@
 {
 	/*
 	 * Populate the extents of memory available for loading BL32.
-	 * TODO: We are temporarily executing BL2 from TZDRAM;
-	 * will eventually move to Trusted SRAM
 	 */
 	bl32_meminfo->total_base = BL32_BASE;
 	bl32_meminfo->free_base = BL32_BASE;
 	bl32_meminfo->total_size =
-			(TZDRAM_BASE + TZDRAM_SIZE) - BL32_BASE;
+			(TSP_SEC_MEM_BASE + TSP_SEC_MEM_SIZE) - BL32_BASE;
 	bl32_meminfo->free_size =
-			(TZDRAM_BASE + TZDRAM_SIZE) - BL32_BASE;
+			(TSP_SEC_MEM_BASE + TSP_SEC_MEM_SIZE) - BL32_BASE;
 	bl32_meminfo->attr = BOT_LOAD;
 	bl32_meminfo->next = 0;
 }
diff --git a/plat/fvp/platform.h b/plat/fvp/platform.h
index b50df00..6ab6862 100644
--- a/plat/fvp/platform.h
+++ b/plat/fvp/platform.h
@@ -247,17 +247,35 @@
 /*******************************************************************************
  * BL2 specific defines.
  ******************************************************************************/
-#define BL2_BASE			0x0402D000
+#define BL2_BASE			(TZRAM_BASE + TZRAM_SIZE - 0xc000)
 
 /*******************************************************************************
  * BL31 specific defines.
  ******************************************************************************/
-#define BL31_BASE			0x0400C000
+#define BL31_BASE			(TZRAM_BASE + 0x6000)
 
 /*******************************************************************************
  * BL32 specific defines.
  ******************************************************************************/
-#define BL32_BASE			(TZDRAM_BASE + 0x2000)
+/*
+ * On FVP, the TSP can execute either from Trusted SRAM or Trusted DRAM.
+ */
+#define TSP_IN_TZRAM			0
+#define TSP_IN_TZDRAM			1
+
+#if TSP_RAM_LOCATION_ID == TSP_IN_TZRAM
+# define TSP_SEC_MEM_BASE		TZRAM_BASE
+# define TSP_SEC_MEM_SIZE		TZRAM_SIZE
+# define BL32_BASE			(TZRAM_BASE + TZRAM_SIZE - 0x1c000)
+# define BL32_LIMIT			BL2_BASE
+#elif TSP_RAM_LOCATION_ID == TSP_IN_TZDRAM
+# define TSP_SEC_MEM_BASE		TZDRAM_BASE
+# define TSP_SEC_MEM_SIZE		TZDRAM_SIZE
+# define BL32_BASE			(TZDRAM_BASE + 0x2000)
+# define BL32_LIMIT			(TZDRAM_BASE + (1 << 21))
+#else
+# error "Unsupported TSP_RAM_LOCATION_ID value"
+#endif
 
 /*******************************************************************************
  * Platform specific page table and MMU setup constants
diff --git a/plat/fvp/platform.mk b/plat/fvp/platform.mk
index 4de001b..82bafed 100644
--- a/plat/fvp/platform.mk
+++ b/plat/fvp/platform.mk
@@ -28,6 +28,21 @@
 # POSSIBILITY OF SUCH DAMAGE.
 #
 
+# On FVP, the TSP can execute either from Trusted SRAM or Trusted DRAM.
+# Trusted SRAM is the default.
+TSP_RAM_LOCATION	:=	tsram
+
+ifeq (${TSP_RAM_LOCATION}, tsram)
+  TSP_RAM_LOCATION_ID := TSP_IN_TZRAM
+else ifeq (${TSP_RAM_LOCATION}, tdram)
+  TSP_RAM_LOCATION_ID := TSP_IN_TZDRAM
+else
+  $(error "Unsupported TSP_RAM_LOCATION value")
+endif
+
+# Process TSP_RAM_LOCATION_ID flag
+$(eval $(call add_define,TSP_RAM_LOCATION_ID))
+
 PLAT_INCLUDES		:=	-Iplat/fvp/include/
 
 PLAT_BL_COMMON_SOURCES	:=	drivers/arm/pl011/pl011.c			\
diff --git a/services/std_svc/psci/psci_entry.S b/services/std_svc/psci/psci_entry.S
index 256c538..3d0181a 100644
--- a/services/std_svc/psci/psci_entry.S
+++ b/services/std_svc/psci/psci_entry.S
@@ -37,6 +37,7 @@
 	.globl	psci_aff_suspend_finish_entry
 	.globl	__psci_cpu_off
 	.globl	__psci_cpu_suspend
+	.globl	psci_power_down_wfi
 
 	/* -----------------------------------------------------
 	 * This cpu has been physically powered up. Depending
@@ -120,9 +121,6 @@
 	mrs	x0, mpidr_el1
 	bl	platform_set_coherent_stack
 	bl	psci_cpu_off
-	mov	x1, #PSCI_E_SUCCESS
-	cmp	x0, x1
-	b.eq	final_wfi
 	mov	sp, x19
 	ldp	x19, x20, [sp,#0]
 	add	sp, sp, #0x10
@@ -144,9 +142,6 @@
 	mov	x1, x21
 	mov	x2, x22
 	bl	psci_cpu_suspend
-	mov	x1, #PSCI_E_SUCCESS
-	cmp	x0, x1
-	b.eq	final_wfi
 	mov	sp, x19
 	ldp	x21, x22, [sp,#0x10]
 	ldp	x19, x20, [sp,#0]
@@ -154,7 +149,16 @@
 	func_epilogue
 	ret
 
-func final_wfi
+	/* --------------------------------------------
+	 * This function is called to indicate to the
+	 * power controller that it is safe to power
+	 * down this cpu. It should not exit the wfi
+	 * and will be released from reset upon power
+	 * up. 'wfi_spill' is used to catch erroneous
+	 * exits from wfi.
+	 * --------------------------------------------
+	 */
+func psci_power_down_wfi
 	dsb	sy		// ensure write buffer empty
 	wfi
 wfi_spill:
diff --git a/services/std_svc/psci/psci_main.c b/services/std_svc/psci/psci_main.c
index 1bcf216..c0866fb 100644
--- a/services/std_svc/psci/psci_main.c
+++ b/services/std_svc/psci/psci_main.c
@@ -90,23 +90,37 @@
 	if (target_afflvl > MPIDR_MAX_AFFLVL)
 		return PSCI_E_INVALID_PARAMS;
 
+	/* Determine the 'state type' in the 'power_state' parameter */
 	pstate_type = psci_get_pstate_type(power_state);
+
+	/*
+	 * Ensure that we have a platform specific handler for entering
+	 * a standby state.
+	 */
 	if (pstate_type == PSTATE_TYPE_STANDBY) {
-		if  (psci_plat_pm_ops->affinst_standby)
-			rc = psci_plat_pm_ops->affinst_standby(power_state);
-		else
+		if  (!psci_plat_pm_ops->affinst_standby)
 			return PSCI_E_INVALID_PARAMS;
-	} else {
-		mpidr = read_mpidr();
-		rc = psci_afflvl_suspend(mpidr,
-					 entrypoint,
-					 context_id,
-					 power_state,
-					 MPIDR_AFFLVL0,
-					 target_afflvl);
+
+		rc = psci_plat_pm_ops->affinst_standby(power_state);
+		assert(rc == PSCI_E_INVALID_PARAMS || rc == PSCI_E_SUCCESS);
+		return rc;
 	}
 
-	assert(rc == PSCI_E_INVALID_PARAMS || rc == PSCI_E_SUCCESS);
+	/*
+	 * Do what is needed to enter the power down state. Upon success,
+	 * enter the final wfi which will power down this cpu else return
+	 * an error.
+	 */
+	mpidr = read_mpidr();
+	rc = psci_afflvl_suspend(mpidr,
+				 entrypoint,
+				 context_id,
+				 power_state,
+				 MPIDR_AFFLVL0,
+				 target_afflvl);
+	if (rc == PSCI_E_SUCCESS)
+		psci_power_down_wfi();
+	assert(rc == PSCI_E_INVALID_PARAMS);
 	return rc;
 }
 
@@ -127,10 +141,18 @@
 	rc = psci_afflvl_off(mpidr, MPIDR_AFFLVL0, target_afflvl);
 
 	/*
+	 * Check if all actions needed to safely power down this cpu have
+	 * successfully completed. Enter a wfi loop which will allow the
+	 * power controller to physically power down this cpu.
+	 */
+	if (rc == PSCI_E_SUCCESS)
+		psci_power_down_wfi();
+
+	/*
 	 * The only error cpu_off can return is E_DENIED. So check if that's
 	 * indeed the case.
 	 */
-	assert (rc == PSCI_E_SUCCESS || rc == PSCI_E_DENIED);
+	assert (rc == PSCI_E_DENIED);
 
 	return rc;
 }
diff --git a/tools/fip_create/fip_create.c b/tools/fip_create/fip_create.c
index c97204a..d1802b7 100644
--- a/tools/fip_create/fip_create.c
+++ b/tools/fip_create/fip_create.c
@@ -53,7 +53,7 @@
  * const char* format_type_str[] = { "RAW", "ELF", "PIC" };
  */
 
-/* Currently only BL2 and BL31 images are supported. */
+/* The images used depends on the platform. */
 static entry_lookup_list_t toc_entry_lookup_list[] = {
 	{ "Trusted Boot Firmware BL2", UUID_TRUSTED_BOOT_FIRMWARE_BL2,
 	  "bl2", NULL, FLAG_FILENAME },