plat: intel: Add BL31 support to Intel Stratix10 SoCFPGA platform

This adds BL31 support to Intel Stratix10 SoCFPGA platform. BL31 in TF-A
supports:
- PSCI calls to enable 4 CPU cores
- PSCI mailbox calls for FPGA reconfiguration
Signed-off-by: Loh Tien Hock <tien.hock.loh@intel.com>

docs/plat/intel-stratix10.rst

+Description
+===========
+
+Stratix 10 SoCFPGA is a FPGA with integrated quad-core 64-bit Arm Cortex A53 processor.
+
+Upon boot, Boot ROM loads bl2 into OCRAM. Bl2 subsequently initializes
+the hardware, then loads bl31 and bl33 (UEFI) into DDR and boots to bl33.
+
+::
+
+    Boot ROM --> Trusted Firmware-A --> UEFI
+
+How to build
+============
+
+Code Locations
+--------------
+
+-  Trusted Firmware-A:
+   `link <https://github.com/ARM-software/arm-trusted-firmware>`__
+
+-  UEFI (to be updated with new upstreamed UEFI):
+   `link <https://github.com/altera-opensource/uefi-socfpga>`__
+
+Build Procedure
+---------------
+
+-  Fetch all the above 2 repositories into local host.
+   Make all the repositories in the same ${BUILD\_PATH}.
+
+-  Prepare the AARCH64 toolchain.
+
+-  Build UEFI using Stratix 10 platform as configuration
+   This will be updated to use an updated UEFI using the latest EDK2 source
+
+.. code:: bash
+
+       make CROSS_COMPILE=aarch64-linux-gnu- device=s10
+
+-  Build atf providing the previously generated UEFI as the BL33 image
+
+.. code:: bash
+
+       make CROSS_COMPILE=aarch64-linux-gnu- bl2 fip PLAT=stratix10
+       BL33=PEI.ROM
+
+Install Procedure
+-----------------
+
+- dd fip.bin to a A2 partition on the MMC drive to be booted in Stratix 10
+  board.
+
+- Generate a SOF containing bl2
+
+.. code:: bash
+        aarch64-linux-gnu-objcopy -I binary -O ihex --change-addresses 0xffe00000 bl2.bin bl2.hex
+        quartus_cpf --bootloader bl2.hex <quartus_generated_sof> <output_sof_with_bl2>
+
+- Configure SOF to board
+
+.. code:: bash
+        nios2-configure-sof <output_sof_with_bl2>
+
+Boot trace
+==========
+
+::
+         INFO:    DDR: DRAM calibration success.
+         INFO:    ECC is disabled.
+         INFO:    Init HPS NOC's DDR Scheduler.
+         NOTICE:  BL2: v2.0(debug):v2.0-809-g7f8474a-dirty
+         NOTICE:  BL2: Built : 17:38:19, Feb 18 2019
+         INFO:    BL2: Doing platform setup
+         INFO:    BL2: Loading image id 3
+         INFO:    Loading image id=3 at address 0xffe1c000
+         INFO:    Image id=3 loaded: 0xffe1c000 - 0xffe24034
+         INFO:    BL2: Loading image id 5
+         INFO:    Loading image id=5 at address 0x50000
+         INFO:    Image id=5 loaded: 0x50000 - 0x550000
+         NOTICE:  BL2: Booting BL31
+         INFO:    Entry point address = 0xffe1c000
+         INFO:    SPSR = 0x3cd
+         NOTICE:  BL31: v2.0(debug):v2.0-810-g788c436-dirty
+         NOTICE:  BL31: Built : 15:17:16, Feb 20 2019
+         INFO:    ARM GICv2 driver initialized
+         INFO:    BL31: Initializing runtime services
+         WARNING: BL31: cortex_a53: CPU workaround for 855873 was missing!
+         INFO:    BL31: Preparing for EL3 exit to normal world
+         INFO:    Entry point address = 0x50000
+         INFO:    SPSR = 0x3c9
+         UEFI firmware (version 1.0 built at 11:26:18 on Nov  7 2018)

plat/intel/soc/stratix10/aarch64/plat_helpers.S

@@ -19,8 +19,6 @@
 	.globl	platform_mem_init
 
 	.globl plat_get_my_entrypoint
-	.globl stratix10_sec_entry
-	.globl cpuid_release
 
 	/* -----------------------------------------------------
 	 * void plat_secondary_cold_boot_setup (void);
@@ -34,11 +32,11 @@
 func plat_secondary_cold_boot_setup
 	/* Wait until the it gets reset signal from rstmgr gets populated */
 poll_mailbox:
-	 wfi
+	wfi
 
-	adr	x0, stratix10_sec_entry
+	mov_imm	x0, PLAT_S10_SEC_ENTRY
 	ldr	x1, [x0]
-	adr	x2, cpuid_release
+	mov_imm	x2, PLAT_CPUID_RELEASE
 	ldr	x3, [x2]
 	mrs	x4, mpidr_el1
 	and	x4, x4, #0xff
@@ -68,7 +66,7 @@ func plat_my_core_pos
 endfunc plat_my_core_pos
 
 func plat_get_my_entrypoint
-	adr	x1,stratix10_sec_entry
+	mov_imm	x1, PLAT_S10_SEC_ENTRY
 	ldr	x0, [x1]
 	ret
 endfunc plat_get_my_entrypoint
@@ -121,9 +119,3 @@ endfunc platform_mem_init
 	.data
 	.align 3
 
-stratix10_sec_entry:
-	.quad 0
-
-cpuid_release:
-	.quad 0
-

plat/intel/soc/stratix10/bl31_plat_setup.c

+/*
+ * Copyright (c) 2019, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <arch.h>
+#include <arch_helpers.h>
+#include <common/bl_common.h>
+#include <common/debug.h>
+#include <drivers/console.h>
+#include <drivers/delay_timer.h>
+#include <drivers/arm/gic_common.h>
+#include <drivers/arm/gicv2.h>
+#include <drivers/ti/uart/uart_16550.h>
+#include <drivers/generic_delay_timer.h>
+#include <drivers/arm/gicv2.h>
+#include <s10_mailbox.h>
+#include <lib/xlat_tables/xlat_tables.h>
+#include <lib/mmio.h>
+#include <plat/common/platform.h>
+#include <platform_def.h>
+#include <platform_private.h>
+
+#include "aarch64/stratix10_private.h"
+#include "s10_handoff.h"
+#include "s10_reset_manager.h"
+#include "s10_memory_controller.h"
+#include "s10_pinmux.h"
+#include "s10_clock_manager.h"
+#include "s10_system_manager.h"
+
+static entry_point_info_t bl32_image_ep_info;
+static entry_point_info_t bl33_image_ep_info;
+
+entry_point_info_t *bl31_plat_get_next_image_ep_info(uint32_t type)
+{
+	entry_point_info_t *next_image_info;
+
+	next_image_info = (type == NON_SECURE) ?
+			  &bl33_image_ep_info : &bl32_image_ep_info;
+
+	/* None of the images on this platform can have 0x0 as the entrypoint */
+	if (next_image_info->pc)
+		return next_image_info;
+	else
+		return NULL;
+}
+
+void bl31_early_platform_setup2(u_register_t arg0, u_register_t arg1,
+				u_register_t arg2, u_register_t arg3)
+{
+	static console_16550_t console;
+
+	console_16550_register(PLAT_UART0_BASE, PLAT_UART_CLOCK, PLAT_BAUDRATE,
+		&console);
+	/*
+	 * Check params passed from BL31 should not be NULL,
+	 */
+	void *from_bl2 = (void *) arg0;
+
+	bl_params_t *params_from_bl2 = (bl_params_t *)from_bl2;
+
+	assert(params_from_bl2 != NULL);
+	assert(params_from_bl2->h.type == PARAM_BL_PARAMS);
+	assert(params_from_bl2->h.version >= VERSION_2);
+
+	/*
+	 * Copy BL32 (if populated by BL31) and BL33 entry point information.
+	 * They are stored in Secure RAM, in BL31's address space.
+	 */
+
+	bl_params_node_t *bl_params = params_from_bl2->head;
+
+	while (bl_params) {
+		if (bl_params->image_id == BL33_IMAGE_ID)
+			bl33_image_ep_info = *bl_params->ep_info;
+
+		bl_params = bl_params->next_params_info;
+	}
+	SET_SECURITY_STATE(bl33_image_ep_info.h.attr, NON_SECURE);
+}
+
+static const interrupt_prop_t s10_interrupt_props[] = {
+	PLAT_INTEL_S10_G1S_IRQ_PROPS(GICV2_INTR_GROUP0),
+	PLAT_INTEL_S10_G0_IRQ_PROPS(GICV2_INTR_GROUP0)
+};
+
+static unsigned int target_mask_array[PLATFORM_CORE_COUNT];
+
+static const gicv2_driver_data_t plat_gicv2_gic_data = {
+	.gicd_base = PLAT_INTEL_S10_GICD_BASE,
+	.gicc_base = PLAT_INTEL_S10_GICC_BASE,
+	.interrupt_props = s10_interrupt_props,
+	.interrupt_props_num = ARRAY_SIZE(s10_interrupt_props),
+	.target_masks = target_mask_array,
+	.target_masks_num = ARRAY_SIZE(target_mask_array),
+};
+
+/*******************************************************************************
+ * Perform any BL3-1 platform setup code
+ ******************************************************************************/
+void bl31_platform_setup(void)
+{
+	/* Initialize the gic cpu and distributor interfaces */
+	gicv2_driver_init(&plat_gicv2_gic_data);
+	gicv2_distif_init();
+	gicv2_pcpu_distif_init();
+	gicv2_cpuif_enable();
+}
+
+const mmap_region_t plat_stratix10_mmap[] = {
+	MAP_REGION_FLAT(DRAM_BASE, DRAM_SIZE, MT_MEMORY | MT_RW | MT_NS),
+	MAP_REGION_FLAT(DEVICE1_BASE, DEVICE1_SIZE, MT_DEVICE | MT_RW | MT_NS),
+	MAP_REGION_FLAT(DEVICE2_BASE, DEVICE2_SIZE, MT_DEVICE | MT_RW | MT_NS),
+	MAP_REGION_FLAT(OCRAM_BASE, OCRAM_SIZE,
+		MT_NON_CACHEABLE | MT_RW | MT_SECURE),
+	MAP_REGION_FLAT(DEVICE3_BASE, DEVICE3_SIZE,
+		MT_DEVICE | MT_RW | MT_SECURE),
+	MAP_REGION_FLAT(MEM64_BASE, MEM64_SIZE, MT_DEVICE | MT_RW | MT_NS),
+	MAP_REGION_FLAT(DEVICE4_BASE, DEVICE4_SIZE, MT_DEVICE | MT_RW | MT_NS),
+	{0},
+};
+
+/*******************************************************************************
+ * Perform the very early platform specific architectural setup here. At the
+ * moment this is only intializes the mmu in a quick and dirty way.
+ ******************************************************************************/
+void bl31_plat_arch_setup(void)
+{
+	const mmap_region_t bl_regions[] = {
+		MAP_REGION_FLAT(BL31_BASE, BL31_END - BL31_BASE,
+			MT_MEMORY | MT_RW | MT_SECURE),
+		MAP_REGION_FLAT(BL_CODE_BASE, BL_CODE_END - BL_CODE_BASE,
+			MT_CODE | MT_SECURE),
+		MAP_REGION_FLAT(BL_RO_DATA_BASE,
+			BL_RO_DATA_END - BL_RO_DATA_BASE,
+			MT_RO_DATA | MT_SECURE),
+#if USE_COHERENT_MEM
+		MAP_REGION_FLAT(BL_COHERENT_RAM_BASE,
+			BL_COHERENT_RAM_END - BL_COHERENT_RAM_BASE,
+			MT_DEVICE | MT_RW | MT_SECURE),
+#endif
+		{0},
+	};
+
+	setup_page_tables(bl_regions, plat_stratix10_mmap);
+	enable_mmu_el3(0);
+}
+

plat/intel/soc/stratix10/include/plat_macros.S

@@ -17,9 +17,6 @@
 	 * ---------------------------------------------
 	 */
 	.macro plat_crash_print_regs
-	mov_imm	x17, PLAT_GICC_BASE
-	mov_imm	x16, PLAT_GICD_BASE
-	arm_print_gic_regs
 	.endm
 
 #endif /* __PLAT_MACROS_S__ */

plat/intel/soc/stratix10/include/s10_mailbox.h

+/*
+ * Copyright (c) 2019, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef __S10_MBOX__
+#define __S10_MBOX__
+
+#define MBOX_OFFSET		0xffa30000
+
+#define MBOX_ATF_CLIENT_ID	0x1
+#define MBOX_JOB_ID		0x1
+
+/* Mailbox interrupt flags and masks */
+#define MBOX_INT_FLAG_COE	0x1
+#define MBOX_INT_FLAG_RIE	0x2
+#define MBOX_INT_FLAG_UAE	0x100
+#define MBOX_COE_BIT(INTERRUPT)	((INTERRUPT) & 0x3)
+#define MBOX_UAE_BIT(INTERRUPT)	(((INTERRUPT) & (1<<4)))
+
+/* Mailbox response and status */
+#define MBOX_RESP_BUFFER_SIZE	16
+#define MBOX_RESP_ERR(BUFFER)	((BUFFER) & 0x00000fff)
+#define MBOX_RESP_LEN(BUFFER)	(((BUFFER) & 0x007ff000) >> 12)
+#define MBOX_RESP_CLIENT_ID(BUFFER)	(((BUFFER) & 0xf0000000) >> 28)
+#define MBOX_RESP_JOB_ID(BUFFER)	(((BUFFER) & 0x0f000000) >> 24)
+#define MBOX_STATUS_UA_MASK	(1<<8)
+
+/* Mailbox command and response */
+#define MBOX_CMD_FREE_OFFSET	0x14
+#define MBOX_CMD_BUFFER_SIZE	32
+#define MBOX_CLIENT_ID_CMD(CLIENT_ID)	((CLIENT_ID) << 28)
+#define MBOX_JOB_ID_CMD(JOB_ID)	(JOB_ID<<24)
+#define MBOX_CMD_LEN_CMD(CMD_LEN)	((CMD_LEN) << 12)
+#define MBOX_INDIRECT			(1 << 11)
+#define MBOX_INSUFFICIENT_BUFFER	-2
+#define MBOX_CIN			0x00
+#define MBOX_ROUT			0x04
+#define MBOX_URG			0x08
+#define MBOX_INT			0x0C
+#define MBOX_COUT			0x20
+#define MBOX_RIN			0x24
+#define MBOX_STATUS			0x2C
+#define MBOX_CMD_BUFFER			0x40
+#define MBOX_RESP_BUFFER		0xC0
+
+#define MBOX_RESP_BUFFER_SIZE		16
+#define MBOX_RESP_OK			0
+#define MBOX_RESP_INVALID_CMD		1
+#define MBOX_RESP_UNKNOWN_BR		2
+#define MBOX_RESP_UNKNOWN		3
+#define MBOX_RESP_NOT_CONFIGURED	256
+
+/* Mailbox SDM doorbell */
+#define MBOX_DOORBELL_TO_SDM		0x400
+#define MBOX_DOORBELL_FROM_SDM		0x480
+
+/* Mailbox QSPI commands */
+#define MBOX_CMD_RESTART		2
+#define MBOX_CMD_QSPI_OPEN		50
+#define MBOX_CMD_QSPI_CLOSE		51
+#define MBOX_CMD_QSPI_DIRECT		59
+#define MBOX_CMD_GET_IDCODE		16
+#define MBOX_CMD_QSPI_SET_CS		52
+
+/* Mailbox REBOOT commands */
+#define MBOX_CMD_REBOOT_HPS		71
+
+/* Generic error handling */
+#define MBOX_TIMEOUT			-2047
+#define MBOX_NO_RESPONSE		-2
+#define MBOX_WRONG_ID			-3
+
+/* Mailbox status */
+#define RECONFIG_STATUS_STATE		0
+#define RECONFIG_STATUS_PIN_STATUS	2
+#define RECONFIG_STATUS_SOFTFUNC_STATUS 3
+#define PIN_STATUS_NSTATUS		(1 << 31)
+#define SOFTFUNC_STATUS_SEU_ERROR	(1 << 3)
+#define SOFTFUNC_STATUS_INIT_DONE	(1 << 1)
+#define SOFTFUNC_STATUS_CONF_DONE	(1 << 0)
+#define MBOX_CFGSTAT_STATE_CONFIG	0x10000000
+
+/* SMC function IDs for SiP Service queries */
+#define SIP_SVC_CALL_COUNT	0x8200ff00
+#define SIP_SVC_UID		0x8200ff01
+#define SIP_SVC_VERSION		0x8200ff03
+
+/* SiP Service Calls version numbers */
+#define SIP_SVC_VERSION_MAJOR	0
+#define SIP_SVC_VERSION_MINOR	1
+
+/* Mailbox reconfiguration commands */
+#define MBOX_RECONFIG		6
+#define MBOX_RECONFIG_DATA	8
+#define MBOX_RECONFIG_STATUS	9
+
+/* Sip get memory */
+#define INTEL_SIP_SMC_FPGA_CONFIG_START			0xC2000001
+#define INTEL_SIP_SMC_FPGA_CONFIG_GET_MEM		0xC2000005
+#define INTEL_SIP_SMC_FPGA_CONFIG_ISDONE		0xC2000004
+#define INTEL_SIP_SMC_FPGA_CONFIG_WRITE			0x42000002
+#define INTEL_SIP_SMC_FPGA_CONFIG_COMPLETED_WRITE	0xC2000003
+#define INTEL_SIP_SMC_STATUS_OK				0
+#define INTEL_SIP_SMC_STATUS_ERROR			0x4
+#define INTEL_SIP_SMC_STATUS_BUSY			0x1
+#define INTEL_SIP_SMC_STATUS_REJECTED			0x2
+#define INTEL_SIP_SMC_FPGA_CONFIG_ADDR			0x1000
+#define INTEL_SIP_SMC_FPGA_CONFIG_SIZE			16777216
+
+void mailbox_set_int(int interrupt_input);
+int mailbox_init(void);
+void mailbox_set_qspi_close(void);
+void mailbox_set_qspi_open(void);
+void mailbox_set_qspi_direct(void);
+int mailbox_send_cmd(int job_id, unsigned int cmd, uint32_t *args,
+				int len, int urgent, uint32_t *response);
+void mailbox_send_cmd_async(int job_id, unsigned int cmd, uint32_t *args,
+				int len, int urgent);
+int mailbox_read_response(int job_id, uint32_t *response);
+int mailbox_get_qspi_clock(void);
+void mailbox_reset_cold(void);
+
+#endif

plat/intel/soc/stratix10/plat_psci.c

+/*
+ * Copyright (c) 2019, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <arch_helpers.h>
+#include <assert.h>
+#include <common/debug.h>
+#include <errno.h>
+#include <lib/mmio.h>
+#include <drivers/arm/gic_common.h>
+#include <drivers/arm/gicv2.h>
+#include <plat/common/platform.h>
+#include <lib/psci/psci.h>
+
+#include "platform_def.h"
+#include "platform_private.h"
+#include "s10_reset_manager.h"
+#include "s10_mailbox.h"
+
+#define S10_RSTMGR_OFST			0xffd11000
+#define S10_RSTMGR_MPUMODRST_OFST	0x20
+
+uintptr_t *stratix10_sec_entry = (uintptr_t *) PLAT_S10_SEC_ENTRY;
+uintptr_t *cpuid_release = (uintptr_t *) PLAT_CPUID_RELEASE;
+
+/*******************************************************************************
+ * plat handler called when a CPU is about to enter standby.
+ ******************************************************************************/
+void plat_cpu_standby(plat_local_state_t cpu_state)
+{
+	/*
+	 * Enter standby state
+	 * dsb is good practice before using wfi to enter low power states
+	 */
+	VERBOSE("%s: cpu_state: 0x%x\n", __func__, cpu_state);
+	dsb();
+	wfi();
+}
+
+/*******************************************************************************
+ * plat handler called when a power domain is about to be turned on. The
+ * mpidr determines the CPU to be turned on.
+ ******************************************************************************/
+int plat_pwr_domain_on(u_register_t mpidr)
+{
+	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;
+
+	*cpuid_release = cpu_id;
+
+	/* release core reset */
+	mmio_setbits_32(S10_RSTMGR_OFST + S10_RSTMGR_MPUMODRST_OFST,
+		1 << cpu_id);
+	return PSCI_E_SUCCESS;
+}
+
+/*******************************************************************************
+ * plat handler called when a power domain is about to be turned off. The
+ * target_state encodes the power state that each level should transition to.
+ ******************************************************************************/
+void plat_pwr_domain_off(const psci_power_state_t *target_state)
+{
+	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]);
+
+	/* TODO: Prevent interrupts from spuriously waking up this cpu */
+	/* gicv2_cpuif_disable(); */
+
+	/* assert core reset */
+	mmio_setbits_32(S10_RSTMGR_OFST + S10_RSTMGR_MPUMODRST_OFST,
+		1 << cpu_id);
+}
+
+/*******************************************************************************
+ * plat handler called when a power domain is about to be suspended. The
+ * target_state encodes the power state that each level should transition to.
+ ******************************************************************************/
+void plat_pwr_domain_suspend(const psci_power_state_t *target_state)
+{
+	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]);
+	/* assert core reset */
+	mmio_setbits_32(S10_RSTMGR_OFST + S10_RSTMGR_MPUMODRST_OFST,
+		1 << cpu_id);
+
+}
+
+/*******************************************************************************
+ * plat handler called when a power domain has just been powered on after
+ * being turned off earlier. The target_state encodes the low power state that
+ * each level has woken up from.
+ ******************************************************************************/
+void plat_pwr_domain_on_finish(const psci_power_state_t *target_state)
+{
+	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]);
+
+	/* Program the gic per-cpu distributor or re-distributor interface */
+	gicv2_pcpu_distif_init();
+	gicv2_set_pe_target_mask(plat_my_core_pos());
+
+	/* Enable the gic cpu interface */
+	gicv2_cpuif_enable();
+}
+
+/*******************************************************************************
+ * plat handler called when a power domain has just been powered on after
+ * having been suspended earlier. The target_state encodes the low power state
+ * that each level has woken up from.
+ * TODO: At the moment we reuse the on finisher and reinitialize the secure
+ * context. Need to implement a separate suspend finisher.
+ ******************************************************************************/
+void plat_pwr_domain_suspend_finish(const psci_power_state_t *target_state)
+{
+	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]);
+
+	/* release core reset */
+	mmio_clrbits_32(S10_RSTMGR_OFST + S10_RSTMGR_MPUMODRST_OFST,
+		1 << cpu_id);
+}
+
+/*******************************************************************************
+ * plat handlers to shutdown/reboot the system
+ ******************************************************************************/
+static void __dead2 plat_system_off(void)
+{
+	wfi();
+	ERROR("System Off: operation not handled.\n");
+	panic();
+}
+
+static void __dead2 plat_system_reset(void)
+{
+	INFO("assert Peripheral from Reset\r\n");
+
+	deassert_peripheral_reset();
+	mailbox_reset_cold();
+
+	while (1)
+		wfi();
+}
+
+int plat_validate_power_state(unsigned int power_state,
+				psci_power_state_t *req_state)
+{
+	VERBOSE("%s: power_state: 0x%x\n", __func__, power_state);
+
+	return PSCI_E_SUCCESS;
+}
+
+int plat_validate_ns_entrypoint(unsigned long ns_entrypoint)
+{
+	VERBOSE("%s: ns_entrypoint: 0x%lx\n", __func__, ns_entrypoint);
+	return PSCI_E_SUCCESS;
+}
+
+void plat_get_sys_suspend_power_state(psci_power_state_t *req_state)
+{
+	req_state->pwr_domain_state[PSCI_CPU_PWR_LVL] = PLAT_MAX_OFF_STATE;
+	req_state->pwr_domain_state[1] = PLAT_MAX_OFF_STATE;
+}
+
+/*******************************************************************************
+ * Export the platform handlers via plat_arm_psci_pm_ops. The ARM Standard
+ * platform layer will take care of registering the handlers with PSCI.
+ ******************************************************************************/
+const plat_psci_ops_t plat_psci_pm_ops = {
+	.cpu_standby = plat_cpu_standby,
+	.pwr_domain_on = plat_pwr_domain_on,
+	.pwr_domain_off = plat_pwr_domain_off,
+	.pwr_domain_suspend = plat_pwr_domain_suspend,
+	.pwr_domain_on_finish = plat_pwr_domain_on_finish,
+	.pwr_domain_suspend_finish = plat_pwr_domain_suspend_finish,
+	.system_off = plat_system_off,
+	.system_reset = plat_system_reset,
+	.validate_power_state = plat_validate_power_state,
+	.validate_ns_entrypoint = plat_validate_ns_entrypoint,
+	.get_sys_suspend_power_state = plat_get_sys_suspend_power_state
+};
+
+/*******************************************************************************
+ * Export the platform specific power ops.
+ ******************************************************************************/
+int plat_setup_psci_ops(uintptr_t sec_entrypoint,
+			const struct plat_psci_ops **psci_ops)
+{
+	/* Save warm boot entrypoint.*/
+	*stratix10_sec_entry = sec_entrypoint;
+
+	*psci_ops = &plat_psci_pm_ops;
+	return 0;
+}

plat/intel/soc/stratix10/plat_sip_svc.c

+/*
+ * Copyright (c) 2019, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <common/debug.h>
+#include <common/runtime_svc.h>
+#include <lib/mmio.h>
+#include <s10_mailbox.h>
+#include <tools_share/uuid.h>
+
+/* Number of SiP Calls implemented */
+#define SIP_NUM_CALLS		0x3
+
+/* Total buffer the driver can hold */
+#define FPGA_CONFIG_BUFFER_SIZE 4
+
+int current_block;
+int current_buffer;
+int current_id = 1;
+int max_blocks;
+uint32_t bytes_per_block;
+uint32_t blocks_submitted;
+uint32_t blocks_completed;
+
+struct fpga_config_info {
+	uint32_t addr;
+	int size;
+	int size_written;
+	uint32_t write_requested;
+	int subblocks_sent;
+	int block_number;
+};
+
+/*  SiP Service UUID */
+DEFINE_SVC_UUID2(intl_svc_uid,
+		0xa85273b0, 0xe85a, 0x4862, 0xa6, 0x2a,
+		0xfa, 0x88, 0x88, 0x17, 0x68, 0x81);
+
+uint64_t plat_sip_handler(uint32_t smc_fid,
+				   uint64_t x1,
+				   uint64_t x2,
+				   uint64_t x3,
+				   uint64_t x4,
+				   void *cookie,
+				   void *handle,
+				   uint64_t flags)
+{
+	ERROR("%s: unhandled SMC (0x%x)\n", __func__, smc_fid);
+	SMC_RET1(handle, SMC_UNK);
+}
+
+struct fpga_config_info fpga_config_buffers[FPGA_CONFIG_BUFFER_SIZE];
+
+static void intel_fpga_sdm_write_buffer(struct fpga_config_info *buffer)
+{
+	uint32_t args[3];
+
+	while (max_blocks > 0 && buffer->size > buffer->size_written) {
+		if (buffer->size - buffer->size_written <=
+			bytes_per_block) {
+			args[0] = (1<<8);
+			args[1] = buffer->addr + buffer->size_written;
+			args[2] = buffer->size - buffer->size_written;
+			buffer->size_written +=
+				buffer->size - buffer->size_written;
+			buffer->subblocks_sent++;
+			mailbox_send_cmd_async(0x4,
+				MBOX_RECONFIG_DATA,
+				args, 3, 0);
+			current_buffer++;
+			current_buffer %= FPGA_CONFIG_BUFFER_SIZE;
+		} else {
+			args[0] = (1<<8);
+			args[1] = buffer->addr + buffer->size_written;
+			args[2] = bytes_per_block;
+			buffer->size_written += bytes_per_block;
+			mailbox_send_cmd_async(0x4,
+				MBOX_RECONFIG_DATA,
+				args, 3, 0);
+			buffer->subblocks_sent++;
+		}
+		max_blocks--;
+	}
+}
+
+static int intel_fpga_sdm_write_all(void)
+{
+	int i;
+
+	for (i = 0; i < FPGA_CONFIG_BUFFER_SIZE; i++)
+		intel_fpga_sdm_write_buffer(
+			&fpga_config_buffers[current_buffer]);
+
+	return 0;
+}
+
+uint32_t intel_mailbox_fpga_config_isdone(void)
+{
+	uint32_t args[2];
+	uint32_t response[6];
+	int status;
+
+	status = mailbox_send_cmd(1, MBOX_RECONFIG_STATUS, args, 0, 0,
+				response);
+
+	if (status < 0)
+		return INTEL_SIP_SMC_STATUS_ERROR;
+
+	if (response[RECONFIG_STATUS_STATE] &&
+		response[RECONFIG_STATUS_STATE] != MBOX_CFGSTAT_STATE_CONFIG)
+		return INTEL_SIP_SMC_STATUS_ERROR;
+
+	if (!(response[RECONFIG_STATUS_PIN_STATUS] & PIN_STATUS_NSTATUS))
+		return INTEL_SIP_SMC_STATUS_ERROR;
+
+	if (response[RECONFIG_STATUS_SOFTFUNC_STATUS] &
+		SOFTFUNC_STATUS_SEU_ERROR)
+		return INTEL_SIP_SMC_STATUS_ERROR;
+
+	if ((response[RECONFIG_STATUS_SOFTFUNC_STATUS] &
+		SOFTFUNC_STATUS_CONF_DONE) &&
+		(response[RECONFIG_STATUS_SOFTFUNC_STATUS] &
+		SOFTFUNC_STATUS_INIT_DONE))
+		return INTEL_SIP_SMC_STATUS_OK;
+
+	return INTEL_SIP_SMC_STATUS_ERROR;
+}
+
+static int mark_last_buffer_xfer_completed(uint32_t *buffer_addr_completed)
+{
+	int i;
+
+	for (i = 0; i < FPGA_CONFIG_BUFFER_SIZE; i++) {
+		if (fpga_config_buffers[i].block_number == current_block) {
+			fpga_config_buffers[i].subblocks_sent--;
+			if (fpga_config_buffers[i].subblocks_sent == 0
+			&& fpga_config_buffers[i].size <=
+			fpga_config_buffers[i].size_written) {
+				fpga_config_buffers[i].write_requested = 0;
+				current_block++;
+				*buffer_addr_completed =
+					fpga_config_buffers[i].addr;
+				return 0;
+			}
+		}
+	}
+
+	return -1;
+}
+
+unsigned int address_in_ddr(uint32_t *addr)
+{
+	if (((unsigned long long)addr > DRAM_BASE) &&
+		((unsigned long long)addr < DRAM_BASE + DRAM_SIZE))
+		return 0;
+
+	return -1;
+}
+
+int intel_fpga_config_completed_write(uint32_t *completed_addr,
+					uint32_t *count)
+{
+	uint32_t status = INTEL_SIP_SMC_STATUS_OK;
+	*count = 0;
+	int resp_len = 0;
+	uint32_t resp[5];
+	int all_completed = 1;
+	int count_check = 0;
+
+	if (address_in_ddr(completed_addr) != 0 || address_in_ddr(count) != 0)
+		return INTEL_SIP_SMC_STATUS_ERROR;
+
+	for (count_check = 0; count_check < 3; count_check++)
+		if (address_in_ddr(&completed_addr[*count + count_check]) != 0)
+			return INTEL_SIP_SMC_STATUS_ERROR;
+
+	resp_len = mailbox_read_response(0x4, resp);
+
+	while (resp_len >= 0 && *count < 3) {
+		max_blocks++;
+		if (mark_last_buffer_xfer_completed(
+			&completed_addr[*count]) == 0)
+			*count = *count + 1;
+		else
+			break;
+		resp_len = mailbox_read_response(0x4, resp);
+	}
+
+	if (*count <= 0) {
+		if (resp_len != MBOX_NO_RESPONSE &&
+			resp_len != MBOX_TIMEOUT && resp_len != 0) {
+			return INTEL_SIP_SMC_STATUS_ERROR;
+		}
+
+		*count = 0;
+	}
+
+	intel_fpga_sdm_write_all();
+
+	if (*count > 0)
+		status = INTEL_SIP_SMC_STATUS_OK;
+	else if (*count == 0)
+		status = INTEL_SIP_SMC_STATUS_BUSY;
+
+	for (int i = 0; i < FPGA_CONFIG_BUFFER_SIZE; i++) {
+		if (fpga_config_buffers[i].write_requested != 0) {
+			all_completed = 0;
+			break;
+		}
+	}
+
+	if (all_completed == 1)
+		return INTEL_SIP_SMC_STATUS_OK;
+
+	return status;
+}
+
+int intel_fpga_config_start(uint32_t config_type)
+{
+	uint32_t response[3];
+	int status = 0;
+
+	status = mailbox_send_cmd(2, MBOX_RECONFIG, 0, 0, 0,
+			response);
+
+	if (status < 0)
+		return status;
+
+	max_blocks = response[0];
+	bytes_per_block = response[1];
+
+	for (int i = 0; i < FPGA_CONFIG_BUFFER_SIZE; i++) {
+		fpga_config_buffers[i].size = 0;
+		fpga_config_buffers[i].size_written = 0;
+		fpga_config_buffers[i].addr = 0;
+		fpga_config_buffers[i].write_requested = 0;
+		fpga_config_buffers[i].block_number = 0;
+		fpga_config_buffers[i].subblocks_sent = 0;
+	}
+
+	blocks_submitted = 0;
+	current_block = 0;
+	current_buffer = 0;
+
+	return 0;
+}
+
+
+uint32_t intel_fpga_config_write(uint64_t mem, uint64_t size)
+{
+	int i = 0;
+	uint32_t status = INTEL_SIP_SMC_STATUS_OK;
+
+	if (mem < DRAM_BASE || mem > DRAM_BASE + DRAM_SIZE)
+		status = INTEL_SIP_SMC_STATUS_REJECTED;
+
+	if (mem + size > DRAM_BASE + DRAM_SIZE)
+		status = INTEL_SIP_SMC_STATUS_REJECTED;
+
+	for (i = 0; i < FPGA_CONFIG_BUFFER_SIZE; i++) {
+		if (!fpga_config_buffers[i].write_requested) {
+			fpga_config_buffers[i].addr = mem;
+			fpga_config_buffers[i].size = size;
+			fpga_config_buffers[i].size_written = 0;
+			fpga_config_buffers[i].write_requested = 1;
+			fpga_config_buffers[i].block_number =
+				blocks_submitted++;
+			fpga_config_buffers[i].subblocks_sent = 0;
+			break;
+		}
+	}
+
+
+	if (i == FPGA_CONFIG_BUFFER_SIZE) {
+		status = INTEL_SIP_SMC_STATUS_REJECTED;
+		return status;
+	} else if (i == FPGA_CONFIG_BUFFER_SIZE - 1) {
+		status = INTEL_SIP_SMC_STATUS_BUSY;
+	}
+
+	intel_fpga_sdm_write_all();
+
+	return status;
+}
+
+/*
+ * This function is responsible for handling all SiP calls from the NS world
+ */
+
+uintptr_t sip_smc_handler(uint32_t smc_fid,
+			 u_register_t x1,
+			 u_register_t x2,
+			 u_register_t x3,
+			 u_register_t x4,
+			 void *cookie,
+			 void *handle,
+			 u_register_t flags)
+{
+	uint32_t status = INTEL_SIP_SMC_STATUS_OK;
+	uint32_t completed_addr[3];
+	uint32_t count = 0;
+
+	switch (smc_fid) {
+	case SIP_SVC_UID:
+		/* Return UID to the caller */
+		SMC_UUID_RET(handle, intl_svc_uid);
+		break;
+	case INTEL_SIP_SMC_FPGA_CONFIG_ISDONE:
+		status = intel_mailbox_fpga_config_isdone();
+		SMC_RET4(handle, status, 0, 0, 0);
+		break;
+	case INTEL_SIP_SMC_FPGA_CONFIG_GET_MEM:
+		SMC_RET3(handle, INTEL_SIP_SMC_STATUS_OK,
+			INTEL_SIP_SMC_FPGA_CONFIG_ADDR,
+			INTEL_SIP_SMC_FPGA_CONFIG_SIZE -
+				INTEL_SIP_SMC_FPGA_CONFIG_ADDR);
+		break;
+	case INTEL_SIP_SMC_FPGA_CONFIG_START:
+		status = intel_fpga_config_start(x1);
+		SMC_RET4(handle, status, 0, 0, 0);
+		break;
+	case INTEL_SIP_SMC_FPGA_CONFIG_WRITE:
+		status = intel_fpga_config_write(x1, x2);
+		SMC_RET4(handle, status, 0, 0, 0);
+		break;
+	case INTEL_SIP_SMC_FPGA_CONFIG_COMPLETED_WRITE:
+		status = intel_fpga_config_completed_write(completed_addr,
+								&count);
+		switch (count) {
+		case 1:
+			SMC_RET4(handle, INTEL_SIP_SMC_STATUS_OK,
+				completed_addr[0], 0, 0);
+			break;
+		case 2:
+			SMC_RET4(handle, INTEL_SIP_SMC_STATUS_OK,
+				completed_addr[0],
+				completed_addr[1], 0);
+			break;
+		case 3:
+			SMC_RET4(handle, INTEL_SIP_SMC_STATUS_OK,
+				completed_addr[0],
+				completed_addr[1],
+				completed_addr[2]);
+			break;
+		case 0:
+			SMC_RET4(handle, status, 0, 0, 0);
+			break;
+		default:
+			SMC_RET1(handle, INTEL_SIP_SMC_STATUS_ERROR);
+		}
+		break;
+
+	default:
+		return plat_sip_handler(smc_fid, x1, x2, x3, x4,
+			cookie, handle, flags);
+	}
+}
+
+DECLARE_RT_SVC(
+	s10_sip_svc,
+	OEN_SIP_START,
+	OEN_SIP_END,
+	SMC_TYPE_FAST,
+	NULL,
+	sip_smc_handler
+);
+
+DECLARE_RT_SVC(
+	s10_sip_svc_std,
+	OEN_SIP_START,
+	OEN_SIP_END,
+	SMC_TYPE_YIELD,
+	NULL,
+	sip_smc_handler
+);

plat/intel/soc/stratix10/plat_topology.c

+/*
+ * Copyright (c) 2019, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <arch.h>
+#include <platform_def.h>
+#include <lib/psci/psci.h>
+static const unsigned char plat_power_domain_tree_desc[] = {1, 4};
+
+/*******************************************************************************
+ * This function returns the default topology tree information.
+ ******************************************************************************/
+const unsigned char *plat_get_power_domain_tree_desc(void)
+{
+	return plat_power_domain_tree_desc;
+}
+
+/*******************************************************************************
+ * This function implements a part of the critical interface between the psci
+ * generic layer and the platform that allows the former to query the platform
+ * to convert an MPIDR to a unique linear index. An error code (-1) is returned
+ * in case the MPIDR is invalid.
+ ******************************************************************************/
+int plat_core_pos_by_mpidr(u_register_t mpidr)
+{
+	unsigned int cluster_id, cpu_id;
+
+	mpidr &= MPIDR_AFFINITY_MASK;
+
+	if (mpidr & ~(MPIDR_CLUSTER_MASK | MPIDR_CPU_MASK))
+		return -1;
+
+	cluster_id = (mpidr >> MPIDR_AFF1_SHIFT) & MPIDR_AFFLVL_MASK;
+	cpu_id = (mpidr >> MPIDR_AFF0_SHIFT) & MPIDR_AFFLVL_MASK;
+
+	if (cluster_id >= PLATFORM_CLUSTER_COUNT)
+		return -1;
+
+	/*
+	 * Validate cpu_id by checking whether it represents a CPU in
+	 * one of the two clusters present on the platform.
+	 */
+	if (cpu_id >= PLATFORM_MAX_CPUS_PER_CLUSTER)
+		return -1;
+
+	return (cpu_id + (cluster_id * 4));
+}
+

plat/intel/soc/stratix10/platform.mk

@@ -46,8 +46,23 @@ BL2_SOURCES     +=	\
 		plat/intel/soc/stratix10/soc/s10_system_manager.c	\
                 common/desc_image_load.c
 
-#		plat/intel/soc/stratix10/plat_topology.c		\
+BL31_SOURCES	+=	drivers/arm/cci/cci.c				\
+		lib/cpus/aarch64/cortex_a53.S				\
+			lib/cpus/aarch64/aem_generic.S			\
+			lib/cpus/aarch64/cortex_a53.S			\
+			plat/common/plat_psci_common.c			\
+			plat/intel/soc/stratix10/plat_sip_svc.c		\
+			plat/intel/soc/stratix10/bl31_plat_setup.c 	\
+			plat/intel/soc/stratix10/plat_psci.c		\
+			plat/intel/soc/stratix10/plat_topology.c	\
+			plat/intel/soc/stratix10/plat_delay_timer.c	\
+			plat/intel/soc/stratix10/soc/s10_reset_manager.c\
+			plat/intel/soc/stratix10/soc/s10_pinmux.c	\
+			plat/intel/soc/stratix10/soc/s10_clock_manager.c\
+			plat/intel/soc/stratix10/soc/s10_handoff.c	\
+			plat/intel/soc/stratix10/soc/s10_mailbox.c	\
 
 PROGRAMMABLE_RESET_ADDRESS	:= 0
 BL2_AT_EL3			:= 1
 MULTI_CONSOLE_API		:= 1
+USE_COHERENT_MEM		:= 1

plat/intel/soc/stratix10/platform_def.h

@@ -15,6 +15,9 @@
 #include <plat/common/common_def.h>
 
 
+#define PLAT_CPUID_RELEASE			0xffe1b000
+#define PLAT_S10_SEC_ENTRY			0xffe1b008
+
 /* Define next boot image name and offset */
 #define PLAT_NS_IMAGE_OFFSET			0x50000
 #define PLAT_HANDOFF_OFFSET			0xFFE3F000
@@ -75,7 +78,7 @@
 #define DRAM_SIZE				(0x80000000)
 
 #define OCRAM_BASE				(0xFFE00000)
-#define OCRAM_SIZE				(0x00100000)
+#define OCRAM_SIZE				(0x00040000)
 
 #define MEM64_BASE				(0x0100000000)
 #define MEM64_SIZE				(0x1F00000000)
@@ -111,10 +114,10 @@
 #define BL1_RW_SIZE	(0x14000)
 
 #define BL2_BASE	(0xffe00000)
-#define BL2_LIMIT	(0xffe1c000)
+#define BL2_LIMIT	(0xffe1b000)
 
 #define BL31_BASE	(0xffe1c000)
-#define BL31_LIMIT	(0xffe3ffff)
+#define BL31_LIMIT	(0xffe3bfff)
 
 /*******************************************************************************
  * Platform specific page table and MMU setup constants

plat/intel/soc/stratix10/soc/s10_mailbox.c

+/*
+ * Copyright (c) 2019, Intel Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <lib/mmio.h>
+#include <common/debug.h>
+#include "s10_mailbox.h"
+
+static int fill_mailbox_circular_buffer(uint32_t header_cmd, uint32_t *args,
+					int len)
+{
+	uint32_t cmd_free_offset;
+	int i;
+
+	cmd_free_offset = mmio_read_32(MBOX_OFFSET + MBOX_CIN);
+
+	if (cmd_free_offset >= MBOX_CMD_BUFFER_SIZE) {
+		INFO("Insufficient buffer in mailbox\n");
+		return MBOX_INSUFFICIENT_BUFFER;
+	}
+
+
+	mmio_write_32(MBOX_OFFSET + MBOX_CMD_BUFFER + (cmd_free_offset++ * 4),
+			header_cmd);
+
+
+	for (i = 0; i < len; i++) {
+		cmd_free_offset %= MBOX_CMD_BUFFER_SIZE;
+		mmio_write_32(MBOX_OFFSET + MBOX_CMD_BUFFER +
+				(cmd_free_offset++ * 4), args[i]);
+	}
+
+	cmd_free_offset %= MBOX_CMD_BUFFER_SIZE;
+	mmio_write_32(MBOX_OFFSET + MBOX_CIN, cmd_free_offset);
+
+	return 0;
+}
+
+int mailbox_read_response(int job_id, uint32_t *response)
+{
+	int rin = 0;
+	int rout = 0;
+	int response_length = 0;
+	int resp = 0;
+	int total_resp_len = 0;
+	int timeout = 100000;
+
+	mmio_write_32(MBOX_OFFSET + MBOX_DOORBELL_TO_SDM, 1);
+
+	while (mmio_read_32(MBOX_OFFSET + MBOX_DOORBELL_FROM_SDM) != 1) {
+		if (timeout-- < 0)
+			return MBOX_NO_RESPONSE;
+	}
+
+	mmio_write_32(MBOX_OFFSET + MBOX_DOORBELL_FROM_SDM, 0);
+
+	rin = mmio_read_32(MBOX_OFFSET + MBOX_RIN);
+	rout = mmio_read_32(MBOX_OFFSET + MBOX_ROUT);
+
+	while (rout != rin) {
+		resp = mmio_read_32(MBOX_OFFSET +
+				    MBOX_RESP_BUFFER + ((rout++)*4));
+
+		rout %= MBOX_RESP_BUFFER_SIZE;
+		mmio_write_32(MBOX_OFFSET + MBOX_ROUT, rout);
+
+		if (MBOX_RESP_CLIENT_ID(resp) != MBOX_ATF_CLIENT_ID ||
+		   MBOX_RESP_JOB_ID(resp) != job_id) {
+			return MBOX_WRONG_ID;
+		}
+
+		if (MBOX_RESP_ERR(resp) > 0) {
+			INFO("Error in response: %x\n", resp);
+			return -resp;
+		}
+		response_length = MBOX_RESP_LEN(resp);
+
+		while (response_length) {
+
+			response_length--;
+			resp = mmio_read_32(MBOX_OFFSET +
+						MBOX_RESP_BUFFER +
+						(rout)*4);
+			if (response) {
+				*(response + total_resp_len) = resp;
+				total_resp_len++;
+			}
+			rout++;
+			rout %= MBOX_RESP_BUFFER_SIZE;
+			mmio_write_32(MBOX_OFFSET + MBOX_ROUT, rout);
+		}
+		return total_resp_len;
+	}
+
+	return MBOX_NO_RESPONSE;
+}
+
+
+int mailbox_poll_response(int job_id, int urgent, uint32_t *response)
+{
+	int timeout = 80000;
+	int rin = 0;
+	int rout = 0;
+	int response_length = 0;
+	int resp = 0;
+	int total_resp_len = 0;
+
+	mmio_write_32(MBOX_OFFSET + MBOX_DOORBELL_TO_SDM, 1);
+
+	while (1) {
+		while (timeout > 0 &&
+			mmio_read_32(MBOX_OFFSET +
+				MBOX_DOORBELL_FROM_SDM) != 1) {
+			timeout--;
+		}
+
+		if (mmio_read_32(MBOX_OFFSET + MBOX_DOORBELL_FROM_SDM) != 1) {
+			INFO("Timed out waiting for SDM");
+			return MBOX_TIMEOUT;
+		}
+
+		mmio_write_32(MBOX_OFFSET + MBOX_DOORBELL_FROM_SDM, 0);
+
+		if (urgent & 1) {
+			if ((mmio_read_32(MBOX_OFFSET + MBOX_STATUS) &
+				MBOX_STATUS_UA_MASK) ^
+				(urgent & MBOX_STATUS_UA_MASK)) {
+				mmio_write_32(MBOX_OFFSET + MBOX_URG, 0);
+				return 0;
+			}
+
+			mmio_write_32(MBOX_OFFSET + MBOX_URG, 0);
+			INFO("Error: Mailbox did not get UA");
+			return -1;
+		}
+
+		rin = mmio_read_32(MBOX_OFFSET + MBOX_RIN);
+		rout = mmio_read_32(MBOX_OFFSET + MBOX_ROUT);
+
+		while (rout != rin) {
+			resp = mmio_read_32(MBOX_OFFSET +
+					    MBOX_RESP_BUFFER + ((rout++)*4));
+
+			rout %= MBOX_RESP_BUFFER_SIZE;
+			mmio_write_32(MBOX_OFFSET + MBOX_ROUT, rout);
+
+			if (MBOX_RESP_CLIENT_ID(resp) != MBOX_ATF_CLIENT_ID ||
+			   MBOX_RESP_JOB_ID(resp) != job_id)
+				continue;
+
+			if (MBOX_RESP_ERR(resp) > 0) {
+				INFO("Error in response: %x\n", resp);
+				return -MBOX_RESP_ERR(resp);
+			}
+			response_length = MBOX_RESP_LEN(resp);
+
+			while (response_length) {
+
+				response_length--;
+				resp = mmio_read_32(MBOX_OFFSET +
+							MBOX_RESP_BUFFER +
+							(rout)*4);
+				if (response) {
+					*(response + total_resp_len) = resp;
+					total_resp_len++;
+				}
+				rout++;
+				rout %= MBOX_RESP_BUFFER_SIZE;
+				mmio_write_32(MBOX_OFFSET + MBOX_ROUT, rout);
+			}
+			return total_resp_len;
+		}
+	}
+}
+
+void mailbox_send_cmd_async(int job_id, unsigned int cmd, uint32_t *args,
+			  int len, int urgent)
+{
+	if (urgent)
+		mmio_write_32(MBOX_OFFSET + MBOX_URG, 1);
+
+	fill_mailbox_circular_buffer(MBOX_CLIENT_ID_CMD(MBOX_ATF_CLIENT_ID) |
+					MBOX_JOB_ID_CMD(job_id) |
+					MBOX_CMD_LEN_CMD(len) |
+					MBOX_INDIRECT |
+					cmd, args, len);
+}
+
+int mailbox_send_cmd(int job_id, unsigned int cmd, uint32_t *args,
+			  int len, int urgent, uint32_t *response)
+{
+	int status;
+
+	if (urgent) {
+		urgent |= mmio_read_32(MBOX_OFFSET + MBOX_STATUS) &
+					MBOX_STATUS_UA_MASK;
+		mmio_write_32(MBOX_OFFSET + MBOX_URG, 1);
+	}
+
+	status = fill_mailbox_circular_buffer(
+			MBOX_CLIENT_ID_CMD(MBOX_ATF_CLIENT_ID) |
+			MBOX_JOB_ID_CMD(job_id) |
+			cmd, args, len);
+
+	if (status)
+		return status;
+
+	return mailbox_poll_response(job_id, urgent, response);
+}
+
+void mailbox_set_int(int interrupt)
+{
+
+	mmio_write_32(MBOX_OFFSET+MBOX_INT, MBOX_COE_BIT(interrupt) |
+			MBOX_UAE_BIT(interrupt));
+}
+
+
+void mailbox_set_qspi_open(void)
+{
+	mailbox_set_int(MBOX_INT_FLAG_COE | MBOX_INT_FLAG_RIE);
+	mailbox_send_cmd(MBOX_JOB_ID, MBOX_CMD_QSPI_OPEN, 0, 0, 0, 0);
+}
+
+void mailbox_set_qspi_direct(void)
+{
+	mailbox_send_cmd(MBOX_JOB_ID, MBOX_CMD_QSPI_DIRECT, 0, 0, 0, 0);
+}
+
+void mailbox_set_qspi_close(void)
+{
+	mailbox_set_int(MBOX_INT_FLAG_COE | MBOX_INT_FLAG_RIE);
+	mailbox_send_cmd(MBOX_JOB_ID, MBOX_CMD_QSPI_CLOSE, 0, 0, 0, 0);
+}
+
+int mailbox_get_qspi_clock(void)
+{
+	mailbox_set_int(MBOX_INT_FLAG_COE | MBOX_INT_FLAG_RIE);
+	return mailbox_send_cmd(MBOX_JOB_ID, MBOX_CMD_QSPI_DIRECT, 0, 0, 0, 0);
+}
+
+void mailbox_qspi_set_cs(int device_select)
+{
+	uint32_t cs_setting = device_select;
+
+	/* QSPI device select settings at 31:28 */
+	cs_setting = (cs_setting << 28);
+	mailbox_set_int(MBOX_INT_FLAG_COE | MBOX_INT_FLAG_RIE);
+	mailbox_send_cmd(MBOX_JOB_ID, MBOX_CMD_QSPI_SET_CS, &cs_setting,
+		1, 0, 0);
+}
+
+void mailbox_reset_cold(void)
+{
+	mailbox_set_int(MBOX_INT_FLAG_COE | MBOX_INT_FLAG_RIE);
+	mailbox_send_cmd(MBOX_JOB_ID, MBOX_CMD_REBOOT_HPS, 0, 0, 0, 0);
+}
+
+int mailbox_init(void)
+{
+	int status = 0;
+
+	mailbox_set_int(MBOX_INT_FLAG_COE | MBOX_INT_FLAG_RIE);
+	status = mailbox_send_cmd(0, MBOX_CMD_RESTART, 0, 0, 1, 0);
+
+	if (status)
+		return status;
+
+	mailbox_set_int(MBOX_INT_FLAG_COE | MBOX_INT_FLAG_RIE);
+
+	return 0;
+}
+