Merge pull request #443 from achingupta/sb/el3_payloads-cb_single_cpu

Add support to boot EL3 payloads and only a single CPU at cold reset

17 files changed, 323 insertions, 37 deletions

diff --git a/Makefile b/Makefile
index 7a4be44a..0c3303f8 100644
--- a/Makefile
+++ b/Makefile
@@ -89,6 +89,12 @@ TRUSTED_BOARD_BOOT		:= 0
 PROGRAMMABLE_RESET_ADDRESS	:= 0
 # Build flag to treat usage of deprecated platform and framework APIs as error.
 ERROR_DEPRECATED		:= 0
+# By default, consider that the platform may release several CPUs out of reset.
+# The platform Makefile is free to override this value.
+COLD_BOOT_SINGLE_CPU		:= 0
+# Flag to introduce an infinite loop in BL1 just before it exits into the next
+# image. This is meant to help debugging the post-BL2 phase.
+SPIN_ON_BL1_EXIT		:= 0
 
 
 ################################################################################
@@ -232,6 +238,10 @@ INCLUDE_TBBR_MK		:=	1
 ################################################################################
 
 ifneq (${SPD},none)
+ifdef EL3_PAYLOAD_BASE
+        $(warning "SPD and EL3_PAYLOAD_BASE are incompatible build options.")
+        $(warning "The SPD and its BL32 companion will be present but ignored.")
+endif
         # We expect to locate an spd.mk under the specified SPD directory
         SPD_MAKE	:=	$(shell m="services/spd/${SPD}/${SPD}.mk"; [ -f "$$m" ] && echo "$$m")
 
@@ -297,7 +307,12 @@ endif
 # supplied for the FIP and Certificate generation tools. This flag can be
 # overridden by the platform.
 ifdef BL2_SOURCES
+ifndef EL3_PAYLOAD_BASE
 NEED_BL33		?=	yes
+else
+# The BL33 image is not needed when booting an EL3 payload.
+NEED_BL33		:=	no
+endif
 endif
 
 # Process TBB related flags
@@ -345,9 +360,11 @@ $(eval $(call assert_boolean,CREATE_KEYS))
 $(eval $(call assert_boolean,SAVE_KEYS))
 $(eval $(call assert_boolean,TRUSTED_BOARD_BOOT))
 $(eval $(call assert_boolean,PROGRAMMABLE_RESET_ADDRESS))
+$(eval $(call assert_boolean,COLD_BOOT_SINGLE_CPU))
 $(eval $(call assert_boolean,PSCI_EXTENDED_STATE_ID))
 $(eval $(call assert_boolean,ERROR_DEPRECATED))
 $(eval $(call assert_boolean,ENABLE_PLAT_COMPAT))
+$(eval $(call assert_boolean,SPIN_ON_BL1_EXIT))
 
 
 ################################################################################
@@ -367,9 +384,15 @@ $(eval $(call add_define,LOG_LEVEL))
 $(eval $(call add_define,USE_COHERENT_MEM))
 $(eval $(call add_define,TRUSTED_BOARD_BOOT))
 $(eval $(call add_define,PROGRAMMABLE_RESET_ADDRESS))
+$(eval $(call add_define,COLD_BOOT_SINGLE_CPU))
 $(eval $(call add_define,PSCI_EXTENDED_STATE_ID))
 $(eval $(call add_define,ERROR_DEPRECATED))
 $(eval $(call add_define,ENABLE_PLAT_COMPAT))
+$(eval $(call add_define,SPIN_ON_BL1_EXIT))
+# Define the EL3_PAYLOAD_BASE flag only if it is provided.
+ifdef EL3_PAYLOAD_BASE
+$(eval $(call add_define,EL3_PAYLOAD_BASE))
+endif
 
 
 ################################################################################
@@ -387,9 +410,13 @@ include bl2/bl2.mk
 endif
 
 ifdef BL31_SOURCES
+# When booting an EL3 payload, there is no need to compile the BL31 image nor
+# put it in the FIP.
+ifndef EL3_PAYLOAD_BASE
 NEED_BL31 := yes
 include bl31/bl31.mk
 endif
+endif
 
 
 ################################################################################
diff --git a/bl1/aarch64/bl1_entrypoint.S b/bl1/aarch64/bl1_entrypoint.S
index 4fc52918..83594f21 100644
--- a/bl1/aarch64/bl1_entrypoint.S
+++ b/bl1/aarch64/bl1_entrypoint.S
@@ -51,7 +51,7 @@ func bl1_entrypoint
 	el3_entrypoint_common					\
 		_set_endian=1					\
 		_warm_boot_mailbox=!PROGRAMMABLE_RESET_ADDRESS	\
-		_secondary_cold_boot=1				\
+		_secondary_cold_boot=!COLD_BOOT_SINGLE_CPU	\
 		_init_memory=1					\
 		_init_c_runtime=1				\
 		_exception_vectors=bl1_exceptions
diff --git a/bl1/aarch64/bl1_exceptions.S b/bl1/aarch64/bl1_exceptions.S
index 0bd04857..5415d395 100644
--- a/bl1/aarch64/bl1_exceptions.S
+++ b/bl1/aarch64/bl1_exceptions.S
@@ -207,6 +207,13 @@ func smc_handler64
 	bl	disable_mmu_icache_el3
 	tlbi	alle3
 
+#if SPIN_ON_BL1_EXIT
+	bl	print_debug_loop_message
+debug_loop:
+	b	debug_loop
+#endif
+
+	mov	x0, x20
 	bl	bl1_plat_prepare_exit
 
 	ldp	x6, x7, [x20, #(ENTRY_POINT_INFO_ARGS_OFFSET + 0x30)]
diff --git a/bl1/bl1_main.c b/bl1/bl1_main.c
index f62f31d5..73f023ab 100644
--- a/bl1/bl1_main.c
+++ b/bl1/bl1_main.c
@@ -208,3 +208,11 @@ void bl1_print_bl31_ep_info(const entry_point_info_t *bl31_ep_info)
 	NOTICE("BL1: Booting BL3-1\n");
 	print_entry_point_info(bl31_ep_info);
 }
+
+#if SPIN_ON_BL1_EXIT
+void print_debug_loop_message(void)
+{
+	NOTICE("BL1: Debug loop, spinning forever\n");
+	NOTICE("BL1: Please connect the debugger to continue\n");
+}
+#endif
diff --git a/bl2/bl2_main.c b/bl2/bl2_main.c
index f8a2372f..a3f016f9 100644
--- a/bl2/bl2_main.c
+++ b/bl2/bl2_main.c
@@ -83,6 +83,7 @@ static int load_bl30(void)
 	return e;
 }
 
+#ifndef EL3_PAYLOAD_BASE
 /*******************************************************************************
  * Load the BL3-1 image.
  * The bl2_to_bl31_params and bl31_ep_info params will be updated with the
@@ -190,6 +191,7 @@ static int load_bl33(bl31_params_t *bl2_to_bl31_params)
 
 	return e;
 }
+#endif /* EL3_PAYLOAD_BASE */
 
 /*******************************************************************************
  * The only thing to do in BL2 is to load further images and pass control to
@@ -232,6 +234,22 @@ void bl2_main(void)
 	bl2_to_bl31_params = bl2_plat_get_bl31_params();
 	bl31_ep_info = bl2_plat_get_bl31_ep_info();
 
+#ifdef EL3_PAYLOAD_BASE
+	/*
+	 * In the case of an EL3 payload, we don't need to load any further
+	 * images. Just update the BL31 entrypoint info structure to make BL1
+	 * jump to the EL3 payload.
+	 * The pointer to the memory the platform has set aside to pass
+	 * information to BL3-1 in the normal boot flow is reused here, even
+	 * though only a fraction of the information contained in the
+	 * bl31_params_t structure makes sense in the context of EL3 payloads.
+	 * This will be refined in the future.
+	 */
+	VERBOSE("BL2: Populating the entrypoint info for the EL3 payload\n");
+	bl31_ep_info->pc = EL3_PAYLOAD_BASE;
+	bl31_ep_info->args.arg0 = (unsigned long) bl2_to_bl31_params;
+	bl2_plat_set_bl31_ep_info(NULL, bl31_ep_info);
+#else
 	e = load_bl31(bl2_to_bl31_params, bl31_ep_info);
 	if (e) {
 		ERROR("Failed to load BL3-1 (%i)\n", e);
@@ -253,6 +271,7 @@ void bl2_main(void)
 		ERROR("Failed to load BL3-3 (%i)\n", e);
 		plat_error_handler(e);
 	}
+#endif /* EL3_PAYLOAD_BASE */
 
 	/* Flush the params to be passed to memory */
 	bl2_plat_flush_bl31_params();
diff --git a/bl31/aarch64/bl31_entrypoint.S b/bl31/aarch64/bl31_entrypoint.S
index 636b1d28..45aa85dc 100644
--- a/bl31/aarch64/bl31_entrypoint.S
+++ b/bl31/aarch64/bl31_entrypoint.S
@@ -85,7 +85,7 @@ func bl31_entrypoint
 	el3_entrypoint_common					\
 		_set_endian=1					\
 		_warm_boot_mailbox=!PROGRAMMABLE_RESET_ADDRESS	\
-		_secondary_cold_boot=1				\
+		_secondary_cold_boot=!COLD_BOOT_SINGLE_CPU	\
 		_init_memory=1					\
 		_init_c_runtime=1				\
 		_exception_vectors=runtime_exceptions
diff --git a/docs/porting-guide.md b/docs/porting-guide.md
index 8c0d7b7c..e5b4a9c7 100644
--- a/docs/porting-guide.md
+++ b/docs/porting-guide.md
@@ -459,7 +459,7 @@ type of reset nor to query the warm reset entrypoint. Therefore, implementing
 this function is not required on such platforms.
 
 
-### Function : plat_secondary_cold_boot_setup() [mandatory]
+### Function : plat_secondary_cold_boot_setup() [mandatory when COLD_BOOT_SINGLE_CPU == 0]
 
     Argument : void
 
@@ -468,14 +468,20 @@ for placing the executing secondary CPU in a platform-specific state until the
 primary CPU performs the necessary actions to bring it out of that state and
 allow entry into the OS. This function must not return.
 
-In the ARM FVP port, each secondary CPU powers itself off. The primary CPU is
-responsible for powering up the secondary CPU when normal world software
-requires them.
+In the ARM FVP port, when using the normal boot flow, each secondary CPU powers
+itself off. The primary CPU is responsible for powering up the secondary CPUs
+when normal world software requires them. When booting an EL3 payload instead,
+they stay powered on and are put in a holding pen until their mailbox gets
+populated.
 
 This function fulfills requirement 2 above.
 
+Note that for platforms that can't release secondary CPUs out of reset, only the
+primary CPU will execute the cold boot code. Therefore, implementing this
+function is not required on such platforms.
 
-### Function : plat_is_my_cpu_primary() [mandatory]
+
+### Function : plat_is_my_cpu_primary() [mandatory when COLD_BOOT_SINGLE_CPU == 0]
 
     Argument : void
     Return   : unsigned int
@@ -485,6 +491,11 @@ secondary CPU. A return value of zero indicates that the CPU is not the
 primary CPU, while a non-zero return value indicates that the CPU is the
 primary CPU.
 
+Note that for platforms that can't release secondary CPUs out of reset, only the
+primary CPU will execute the cold boot code. Therefore, there is no need to
+distinguish between primary and secondary CPUs and implementing this function is
+not required.
+
 
 ### Function : platform_mem_init() [mandatory]
 
@@ -810,13 +821,14 @@ represents the entry point system state for BL2.
 
 ### Function : bl1_plat_prepare_exit() [optional]
 
-    Argument : void
+    Argument : entry_point_info_t *
     Return   : void
 
-This function is called prior to exiting BL1 in response to the `RUN_IMAGE_SMC`
+This function is called prior to exiting BL1 in response to the `RUN_IMAGE` SMC
 request raised by BL2. It should be used to perform platform specific clean up
-or bookkeeping operations before transferring control to the next image. This
-function runs with MMU disabled.
+or bookkeeping operations before transferring control to the next image. It
+receives the address of the `entry_point_info_t` structure passed from BL2.
+This function runs with MMU disabled.
 
 
 3.2 Boot Loader Stage 2 (BL2)
@@ -1000,10 +1012,13 @@ structure in BL2 memory.
     Argument : image_info *, entry_point_info *
     Return   : void
 
-This function is called after loading BL3-1 image and it can be used to
-overwrite the entry point set by loader and also set the security state
-and SPSR which represents the entry point system state for BL3-1.
+In the normal boot flow, this function is called after loading BL3-1 image and
+it can be used to overwrite the entry point set by loader and also set the
+security state and SPSR which represents the entry point system state for BL3-1.
 
+When booting an EL3 payload instead, this function is called after populating
+its entry point address and can be used for the same purpose for the payload
+image. It receives a null pointer as its first argument in this case.
 
 ### Function : bl2_plat_set_bl32_ep_info() [mandatory]
 
diff --git a/docs/user-guide.md b/docs/user-guide.md
index b7b152e4..4fcd2b64 100644
--- a/docs/user-guide.md
+++ b/docs/user-guide.md
@@ -9,9 +9,10 @@ Contents :
 4.  [Getting the Trusted Firmware source code](#4--getting-the-trusted-firmware-source-code)
 5.  [Building the Trusted Firmware](#5--building-the-trusted-firmware)
 6.  [Building the rest of the software stack](#6--building-the-rest-of-the-software-stack)
-7.  [Preparing the images to run on FVP](#7--preparing-the-images-to-run-on-fvp)
-8.  [Running the software on FVP](#8--running-the-software-on-fvp)
-9.  [Running the software on Juno](#9--running-the-software-on-juno)
+7.  [EL3 payloads alternative boot flow](#7--el3-payloads-alternative-boot-flow)
+8.  [Preparing the images to run on FVP](#8--preparing-the-images-to-run-on-fvp)
+9.  [Running the software on FVP](#9--running-the-software-on-fvp)
+10. [Running the software on Juno](#10--running-the-software-on-juno)
 
 
 1.  Introduction
@@ -349,9 +350,18 @@ performed.
     either 0 (fixed) or 1 (programmable). Default is 0. If the platform has a
     programmable reset address, it is expected that a CPU will start executing
     code directly at the right address, both on a cold and warm reset. In this
-    case, there is no need to identify the entrypoint on boot and this has
-    implication for `plat_get_my_entrypoint()` platform porting interface.
-    (see the [Porting Guide] for details)
+    case, there is no need to identify the entrypoint on boot and the boot path
+    can be optimised. The `plat_get_my_entrypoint()` platform porting interface
+    does not need to be implemented in this case.
+
+*   `COLD_BOOT_SINGLE_CPU`: This option indicates whether the platform may
+    release several CPUs out of reset. It can take either 0 (several CPUs may be
+    brought up) or 1 (only one CPU will ever be brought up during cold reset).
+    Default is 0. If the platform always brings up a single CPU, there is no
+    need to distinguish between primary and secondary CPUs and the boot path can
+    be optimised. The `plat_is_my_cpu_primary()` and
+    `plat_secondary_cold_boot_setup()` platform porting interfaces do not need
+    to be implemented in this case.
 
 *   `PSCI_EXTENDED_STATE_ID`: As per PSCI1.0 Specification, there are 2 formats
     possible for the PSCI power-state parameter viz original and extended
@@ -367,6 +377,17 @@ performed.
     Firmware as error. It can take the value 1 (flag the use of deprecated
     APIs as error) or 0. The default is 0.
 
+*   `SPIN_ON_BL1_EXIT`: This option introduces an infinite loop in BL1. It can
+    take either 0 (no loop) or 1 (add a loop). 0 is the default. This loop stops
+    execution in BL1 just before handing over to BL31. At this point, all
+    firmware images have been loaded in memory and the MMU as well as the caches
+    are turned off. Refer to the "Debugging options" section for more details.
+
+*   `EL3_PAYLOAD_BASE`: This option enables booting an EL3 payload instead of
+    the normal boot flow. It must specify the entry point address of the EL3
+    payload. Please refer to the "Booting an EL3 payload" section for more
+    details.
+
 #### ARM development platform specific build options
 
 *   `ARM_TSP_RAM_LOCATION`: location of the TSP binary. Options:
@@ -495,6 +516,25 @@ Extra debug options can be passed to the build system by setting `CFLAGS`:
     BL33=<path-to>/<bl33_image>                                \
     make PLAT=<platform> DEBUG=1 V=1 all fip
 
+It is also possible to introduce an infinite loop to help in debugging the
+post-BL2 phase of the Trusted Firmware. This can be done by rebuilding BL1 with
+the `SPIN_ON_BL1_EXIT=1` build flag. Refer to the "Summary of build options"
+section. In this case, the developer may take control of the target using a
+debugger when indicated by the console output. When using DS-5, the following
+commands can be used:
+
+    # Stop target execution
+    interrupt
+
+    #
+    # Prepare your debugging environment, e.g. set breakpoints
+    #
+
+    # Jump over the debug loop
+    set var $AARCH64::$Core::$PC = $AARCH64::$Core::$PC + 4
+
+    # Resume execution
+    continue
 
 ### Building the Test Secure Payload
 
@@ -694,9 +734,49 @@ above. The EDK2 binary for use with the ARM Trusted Firmware can be found here:
     instructions in the "Building the Trusted Firmware" section.
 
 
-7.  Preparing the images to run on FVP
+7.  EL3 payloads alternative boot flow
+--------------------------------------
+
+On a pre-production system, the ability to execute arbitrary, bare-metal code at
+the highest exception level is required. It allows full, direct access to the
+hardware, for example to run silicon soak tests.
+
+Although it is possible to implement some baremetal secure firmware from
+scratch, this is a complex task on some platforms, depending on the level of
+configuration required to put the system in the expected state.
+
+Rather than booting a baremetal application, a possible compromise is to boot
+`EL3 payloads` through the Trusted Firmware instead. This is implemented as an
+alternative boot flow, where a modified BL2 boots an EL3 payload, instead of
+loading the other BL images and passing control to BL31. It reduces the
+complexity of developing EL3 baremetal code by:
+
+*   putting the system into a known architectural state;
+*   taking care of platform secure world initialization;
+*   loading the BL30 image if required by the platform.
+
+When booting an EL3 payload on ARM standard platforms, the configuration of the
+TrustZone controller is simplified such that only region 0 is enabled and is
+configured to permit secure access only. This gives full access to the whole
+DRAM to the EL3 payload.
+
+The system is left in the same state as when entering BL31 in the default boot
+flow. In particular:
+
+*   Running in EL3;
+*   Current state is AArch64;
+*   Little-endian data access;
+*   All exceptions disabled;
+*   MMU disabled;
+*   Caches disabled.
+
+
+8.  Preparing the images to run on FVP
 --------------------------------------
 
+Note: This section can be ignored when booting an EL3 payload, as no Flattened
+Device Tree or kernel image is needed in this case.
+
 ### Obtaining the Flattened Device Trees
 
 Depending on the FVP configuration and Linux configuration used, different
@@ -744,7 +824,7 @@ Copy the kernel image file `linux/arch/arm64/boot/Image` to the directory from
 which the FVP is launched. Alternatively a symbolic link may be used.
 
 
-8.  Running the software on FVP
+9.  Running the software on FVP
 -------------------------------
 
 This version of the ARM Trusted Firmware has been tested on the following ARM
@@ -1043,9 +1123,41 @@ The `bp.variant` parameter corresponds to the build variant field of the
 `SYS_ID` register.  Setting this to `0x0` allows the ARM Trusted Firmware to
 detect the legacy VE memory map while configuring the GIC.
 
+### Booting an EL3 payload on FVP
+
+Booting an EL3 payload on FVP requires a couple of changes to the way the
+model is normally invoked.
+
+First of all, the EL3 payload image is not part of the FIP and is not loaded by
+the Trusted Firmware. Therefore, it must be loaded in memory some other way.
+There are 2 ways of doing that:
+
+1.  It can be loaded over JTAG at the appropriate time. The infinite loop
+    introduced in BL1 when compiling the Trusted Firmware with
+    `SPIN_ON_BL1_EXIT=1` stops execution at the right moment for a debugger to
+    take control of the target and load the payload.
+
+2.  It can be pre-loaded in the FVP memory using the following model parameter:
 
-9.  Running the software on Juno
---------------------------------
+        --data="<path-to-binary>"@<base-address-of-binary>
+
+    The base address provided to the FVP must match the `EL3_PAYLOAD_BASE`
+    address used when building the Trusted Firmware.
+
+Secondly, the EL3 payloads boot flow requires the CPUs mailbox to be cleared
+at reset for the secondary CPUs holding pen to work properly. Unfortunately,
+its reset value is undefined on FVP. One way to clear it is to create an
+8-byte file containing all zero bytes and pre-load it into the FVP memory at the
+mailbox address (i.e. `0x04000000`) using the same `--data` FVP parameter as
+described above.
+
+The following command creates such a file called `mailbox.dat`:
+
+    dd if=/dev/zero of=mailbox.dat bs=1 count=8
+
+
+10.  Running the software on Juno
+---------------------------------
 
 This version of the ARM Trusted Firmware has been tested on Juno r0 and Juno r1.
 
@@ -1138,6 +1250,5 @@ _Copyright (c) 2013-2015, ARM Limited and Contributors. All rights reserved._
 [Juno Software Guide]:         http://community.arm.com/docs/DOC-8396
 [DS-5]:                        http://www.arm.com/products/tools/software-tools/ds-5/index.php
 [mbedTLS Repository]:          https://github.com/ARMmbed/mbedtls.git
-[Porting Guide]:               ./porting-guide.md
 [PSCI]:                        http://infocenter.arm.com/help/topic/com.arm.doc.den0022c/DEN0022C_Power_State_Coordination_Interface.pdf "Power State Coordination Interface PDD (ARM DEN 0022C)"
 [Trusted Board Boot]:          trusted-board-boot.md
diff --git a/include/plat/arm/common/plat_arm.h b/include/plat/arm/common/plat_arm.h
index 044e18ef..aadf58d8 100644
--- a/include/plat/arm/common/plat_arm.h
+++ b/include/plat/arm/common/plat_arm.h
@@ -149,6 +149,7 @@ int arm_validate_power_state(unsigned int power_state,
 			    psci_power_state_t *req_state);
 int arm_validate_ns_entrypoint(uintptr_t entrypoint);
 void arm_system_pwr_domain_resume(void);
+void arm_program_trusted_mailbox(uintptr_t address);
 
 /* Topology utility function */
 int arm_check_mpidr(u_register_t mpidr);
diff --git a/plat/arm/board/fvp/aarch64/fvp_helpers.S b/plat/arm/board/fvp/aarch64/fvp_helpers.S
index 865c615a..2c24e61f 100644
--- a/plat/arm/board/fvp/aarch64/fvp_helpers.S
+++ b/plat/arm/board/fvp/aarch64/fvp_helpers.S
@@ -60,6 +60,7 @@
 	 * -----------------------------------------------------
 	 */
 func plat_secondary_cold_boot_setup
+#ifndef EL3_PAYLOAD_BASE
 	/* ---------------------------------------------
 	 * Power down this cpu.
 	 * TODO: Do we need to worry about powering the
@@ -93,6 +94,18 @@ func plat_secondary_cold_boot_setup
 	wfi
 cb_panic:
 	b	cb_panic
+#else
+	mov_imm	x0, PLAT_ARM_TRUSTED_MAILBOX_BASE
+
+	/* Wait until the entrypoint gets populated */
+poll_mailbox:
+	ldr	x1, [x0]
+	cbz	x1, 1f
+	br	x1
+1:
+	wfe
+	b	poll_mailbox
+#endif /* EL3_PAYLOAD_BASE */
 endfunc plat_secondary_cold_boot_setup
 
 	/* ---------------------------------------------------------------------
diff --git a/plat/arm/common/arm_bl1_setup.c b/plat/arm/common/arm_bl1_setup.c
index ddf383fe..887223cf 100644
--- a/plat/arm/common/arm_bl1_setup.c
+++ b/plat/arm/common/arm_bl1_setup.c
@@ -145,6 +145,20 @@ void bl1_platform_setup(void)
 	arm_bl1_platform_setup();
 }
 
+void bl1_plat_prepare_exit(entry_point_info_t *ep_info)
+{
+#ifdef EL3_PAYLOAD_BASE
+	/*
+	 * Program the EL3 payload's entry point address into the CPUs mailbox
+	 * in order to release secondary CPUs from their holding pen and make
+	 * them jump there.
+	 */
+	arm_program_trusted_mailbox(ep_info->pc);
+	dsbsy();
+	sev();
+#endif
+}
+
 /*******************************************************************************
  * Before calling this function BL2 is loaded in memory and its entrypoint
  * is set by load_image. This is a placeholder for the platform to change
diff --git a/plat/arm/common/arm_common.mk b/plat/arm/common/arm_common.mk
index eb5ae111..7b235275 100644
--- a/plat/arm/common/arm_common.mk
+++ b/plat/arm/common/arm_common.mk
@@ -81,6 +81,11 @@ BL1_SOURCES		+=	drivers/arm/cci/cci.c				\
 				plat/arm/common/arm_bl1_setup.c			\
 				plat/arm/common/arm_io_storage.c		\
 				plat/common/aarch64/platform_up_stack.S
+ifdef EL3_PAYLOAD_BASE
+# Need the arm_program_trusted_mailbox() function to release secondary CPUs from
+# their holding pen
+BL1_SOURCES		+=	plat/arm/common/arm_pm.c
+endif
 
 BL2_SOURCES		+=	drivers/arm/tzc400/tzc400.c			\
 				drivers/io/io_fip.c				\
diff --git a/plat/arm/common/arm_pm.c b/plat/arm/common/arm_pm.c
index d13d2683..cae65970 100644
--- a/plat/arm/common/arm_pm.c
+++ b/plat/arm/common/arm_pm.c
@@ -175,7 +175,7 @@ void arm_system_pwr_domain_resume(void)
  * from reset. This function assumes that the Trusted mail box base is within
  * the ARM_SHARED_RAM region
  ******************************************************************************/
-static void arm_program_trusted_mailbox(uintptr_t address)
+void arm_program_trusted_mailbox(uintptr_t address)
 {
 	uintptr_t *mailbox = (void *) PLAT_ARM_TRUSTED_MAILBOX_BASE;
 
diff --git a/plat/arm/common/arm_security.c b/plat/arm/common/arm_security.c
index 990d8d4a..8b46aaed 100644
--- a/plat/arm/common/arm_security.c
+++ b/plat/arm/common/arm_security.c
@@ -40,8 +40,13 @@
 
 /*******************************************************************************
  * Initialize the TrustZone Controller for ARM standard platforms.
- * Configure Region 0 with no access, Region 1 with secure access only, and
- * the remaining DRAM regions access from the given Non-Secure masters.
+ * Configure:
+ *   - Region 0 with no access;
+ *   - Region 1 with secure access only;
+ *   - the remaining DRAM regions access from the given Non-Secure masters.
+ *
+ * When booting an EL3 payload, this is simplified: we configure region 0 with
+ * secure access only and do not enable any other region.
  ******************************************************************************/
 void arm_tzc_setup(void)
 {
@@ -52,6 +57,7 @@ void arm_tzc_setup(void)
 	/* Disable filters. */
 	tzc_disable_filters();
 
+#ifndef EL3_PAYLOAD_BASE
 	/* Region 0 set to no access by default */
 	tzc_configure_region0(TZC_REGION_S_NONE, 0);
 
@@ -73,6 +79,10 @@ void arm_tzc_setup(void)
 			ARM_DRAM2_BASE, ARM_DRAM2_END,
 			TZC_REGION_S_NONE,
 			PLAT_ARM_TZC_NS_DEV_ACCESS);
+#else
+	/* Allow secure access only to DRAM for EL3 payloads. */
+	tzc_configure_region0(TZC_REGION_S_RDWR, 0);
+#endif /* EL3_PAYLOAD_BASE */
 
 	/*
 	 * Raise an exception if a NS device tries to access secure memory
diff --git a/plat/arm/css/common/aarch64/css_helpers.S b/plat/arm/css/common/aarch64/css_helpers.S
index d1702708..27476186 100644
--- a/plat/arm/css/common/aarch64/css_helpers.S
+++ b/plat/arm/css/common/aarch64/css_helpers.S
@@ -37,19 +37,36 @@
 	.globl	css_calc_core_pos_swap_cluster
 	.weak	plat_is_my_cpu_primary
 
-	/* -----------------------------------------------------
-	 * void plat_secondary_cold_boot_setup (void);
+	/* ---------------------------------------------------------------------
+	 * void plat_secondary_cold_boot_setup(void);
 	 *
-	 * This function performs any platform specific actions
-	 * needed for a secondary cpu after a cold reset e.g
-	 * mark the cpu's presence, mechanism to place it in a
-	 * holding pen etc.
-	 * -----------------------------------------------------
+	 * In the normal boot flow, cold-booting secondary CPUs is not yet
+	 * implemented and they panic.
+	 *
+	 * When booting an EL3 payload, secondary CPUs are placed in a holding
+	 * pen, waiting for their mailbox to be populated. Note that all CPUs
+	 * share the same mailbox ; therefore, populating it will release all
+	 * CPUs from their holding pen. If finer-grained control is needed then
+	 * this should be handled in the code that secondary CPUs jump to.
+	 * ---------------------------------------------------------------------
 	 */
 func plat_secondary_cold_boot_setup
-	/* todo: Implement secondary CPU cold boot setup on CSS platforms */
+#ifndef EL3_PAYLOAD_BASE
+	/* TODO: Implement secondary CPU cold boot setup on CSS platforms */
 cb_panic:
 	b	cb_panic
+#else
+	mov_imm	x0, PLAT_ARM_TRUSTED_MAILBOX_BASE
+
+	/* Wait until the mailbox gets populated */
+poll_mailbox:
+	ldr	x1, [x0]
+	cbz	x1, 1f
+	br	x1
+1:
+	wfe
+	b	poll_mailbox
+#endif /* EL3_PAYLOAD_BASE */
 endfunc plat_secondary_cold_boot_setup
 
 	/* ---------------------------------------------------------------------
diff --git a/plat/arm/css/common/css_bl2_setup.c b/plat/arm/css/common/css_bl2_setup.c
index 2e423d96..6054f7a5 100644
--- a/plat/arm/css/common/css_bl2_setup.c
+++ b/plat/arm/css/common/css_bl2_setup.c
@@ -29,7 +29,11 @@
  */
 
 #include <bl_common.h>
+#include <css_def.h>
 #include <debug.h>
+#include <mmio.h>
+#include <plat_arm.h>
+#include <string.h>
 #include "css_scp_bootloader.h"
 
 /* Weak definition may be overridden in specific CSS based platform */
@@ -55,3 +59,38 @@ int bl2_plat_handle_bl30(image_info_t *bl30_image_info)
 
 	return ret;
 }
+
+#ifdef EL3_PAYLOAD_BASE
+/*
+ * We need to override some of the platform functions when booting an EL3
+ * payload.
+ */
+
+static unsigned int scp_boot_config;
+
+void bl2_early_platform_setup(meminfo_t *mem_layout)
+{
+	arm_bl2_early_platform_setup(mem_layout);
+
+	/* Save SCP Boot config before it gets overwritten by BL30 loading */
+	scp_boot_config = mmio_read_32(SCP_BOOT_CFG_ADDR);
+	VERBOSE("BL2: Saved SCP Boot config = 0x%x\n", scp_boot_config);
+}
+
+void bl2_platform_setup(void)
+{
+	arm_bl2_platform_setup();
+
+	/*
+	 * Before releasing the AP cores out of reset, the SCP writes some data
+	 * at the beginning of the Trusted SRAM. It is is overwritten before
+	 * reaching this function. We need to restore this data, as if the
+	 * target had just come out of reset. This implies:
+	 *  - zeroing the first 128 bytes of Trusted SRAM;
+	 *  - restoring the SCP boot configuration.
+	 */
+	VERBOSE("BL2: Restoring SCP reset data in Trusted SRAM\n");
+	memset((void *) ARM_TRUSTED_SRAM_BASE, 0, 128);
+	mmio_write_32(SCP_BOOT_CFG_ADDR, scp_boot_config);
+}
+#endif /* EL3_PAYLOAD_BASE */
diff --git a/plat/common/aarch64/platform_helpers.S b/plat/common/aarch64/platform_helpers.S
index 56b88bc0..29f01ce9 100644
--- a/plat/common/aarch64/platform_helpers.S
+++ b/plat/common/aarch64/platform_helpers.S
@@ -115,7 +115,7 @@ func plat_disable_acp
 endfunc plat_disable_acp
 
 	/* -----------------------------------------------------
-	 * void bl1_plat_prepare_exit(void);
+	 * void bl1_plat_prepare_exit(entry_point_info_t *ep_info);
 	 * Called before exiting BL1. Default: do nothing
 	 * -----------------------------------------------------
 	 */