diff --git a/plat/intel/soc/agilex/bl31_plat_setup.c b/plat/intel/soc/agilex/bl31_plat_setup.c
index 436538b..168236b 100644
--- a/plat/intel/soc/agilex/bl31_plat_setup.c
+++ b/plat/intel/soc/agilex/bl31_plat_setup.c
@@ -101,6 +101,8 @@
  ******************************************************************************/
 void bl31_platform_setup(void)
 {
+	socfpga_delay_timer_init();
+
 	/* Initialize the gic cpu and distributor interfaces */
 	gicv2_driver_init(&plat_gicv2_gic_data);
 	gicv2_distif_init();
diff --git a/plat/intel/soc/agilex/platform.mk b/plat/intel/soc/agilex/platform.mk
index 814d9c6..bf5cc14 100644
--- a/plat/intel/soc/agilex/platform.mk
+++ b/plat/intel/soc/agilex/platform.mk
@@ -25,7 +25,8 @@
 			lib/xlat_tables/aarch64/xlat_tables.c 		\
 			lib/xlat_tables/xlat_tables_common.c 		\
 			plat/intel/soc/common/aarch64/platform_common.c \
-			plat/intel/soc/common/aarch64/plat_helpers.S
+			plat/intel/soc/common/aarch64/plat_helpers.S	\
+			plat/intel/soc/common/socfpga_delay_timer.c
 
 BL2_SOURCES     +=	\
 		common/desc_image_load.c				\
@@ -44,7 +45,6 @@
 		plat/intel/soc/agilex/soc/agilex_mmc.c			\
 		plat/intel/soc/agilex/soc/agilex_pinmux.c		\
                 plat/intel/soc/common/bl2_plat_mem_params_desc.c	\
-		plat/intel/soc/common/socfpga_delay_timer.c		\
 		plat/intel/soc/common/socfpga_image_load.c		\
 		plat/intel/soc/common/socfpga_storage.c			\
 		plat/intel/soc/common/soc/socfpga_emac.c		\
diff --git a/plat/intel/soc/common/include/socfpga_mailbox.h b/plat/intel/soc/common/include/socfpga_mailbox.h
index b250b3e..923c4f1 100644
--- a/plat/intel/soc/common/include/socfpga_mailbox.h
+++ b/plat/intel/soc/common/include/socfpga_mailbox.h
@@ -12,9 +12,10 @@
 
 #define MBOX_OFFSET			0xffa30000
 
-#define MBOX_MAX_JOB_ID			0xf
-#define MBOX_ATF_CLIENT_ID		0x1
-#define MBOX_JOB_ID			0x1
+#define MBOX_ATF_CLIENT_ID		0x1U
+#define MBOX_MAX_JOB_ID			0xFU
+#define MBOX_MAX_IND_JOB_ID		(MBOX_MAX_JOB_ID - 1U)
+#define MBOX_JOB_ID			MBOX_MAX_JOB_ID
 
 
 /* Mailbox Shared Memory Register Map */
@@ -64,6 +65,7 @@
 /* Mailbox Definitions */
 
 #define CMD_DIRECT			0
+#define CMD_INDIRECT			1
 #define CMD_CASUAL			0
 #define CMD_URGENT			1
 
@@ -80,6 +82,7 @@
 #define MBOX_RET_ERROR			-1
 #define MBOX_NO_RESPONSE		-2
 #define MBOX_WRONG_ID			-3
+#define MBOX_BUFFER_FULL		-4
 #define MBOX_TIMEOUT			-2047
 
 /* Reconfig Status Response */
@@ -123,7 +126,8 @@
 #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_INDIRECT(val)		((val) << 11)
+#define MBOX_CMD_MASK(header)		((header) & 0x7ff)
 
 /* RSU Macros */
 #define RSU_VERSION_ACMF		BIT(8)
@@ -132,16 +136,22 @@
 
 /* Mailbox Function Definitions */
 
-void mailbox_set_int(int interrupt_input);
+void mailbox_set_int(uint32_t 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, int resp_len);
-int 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 resp_len);
+
+int mailbox_send_cmd(uint32_t job_id, uint32_t cmd, uint32_t *args,
+			unsigned int len, uint32_t urgent, uint32_t *response,
+			unsigned int resp_len);
+int mailbox_send_cmd_async(uint32_t *job_id, uint32_t cmd, uint32_t *args,
+			unsigned int len, unsigned int indirect);
+int mailbox_read_response(uint32_t *job_id, uint32_t *response,
+			unsigned int resp_len);
+unsigned int iterate_resp(uint32_t mbox_resp_len, uint32_t *resp_buf,
+			unsigned int resp_len);
+
 void mailbox_reset_cold(void);
 void mailbox_clear_response(void);
 
diff --git a/plat/intel/soc/common/include/socfpga_sip_svc.h b/plat/intel/soc/common/include/socfpga_sip_svc.h
index 22e54e8..92adfa3 100644
--- a/plat/intel/soc/common/include/socfpga_sip_svc.h
+++ b/plat/intel/soc/common/include/socfpga_sip_svc.h
@@ -44,7 +44,7 @@
 
 /* FPGA config helpers */
 #define INTEL_SIP_SMC_FPGA_CONFIG_ADDR			0x400000
-#define INTEL_SIP_SMC_FPGA_CONFIG_SIZE			16777216
+#define INTEL_SIP_SMC_FPGA_CONFIG_SIZE			0x2000000
 
 /* SMC function IDs for SiP Service queries */
 #define SIP_SVC_CALL_COUNT	0x8200ff00
@@ -55,4 +55,19 @@
 #define SIP_SVC_VERSION_MAJOR	0
 #define SIP_SVC_VERSION_MINOR	1
 
+
+/* Structure Definitions */
+struct fpga_config_info {
+	uint32_t addr;
+	int size;
+	int size_written;
+	uint32_t write_requested;
+	int subblocks_sent;
+	int block_number;
+};
+
+/* Function Definitions */
+
+bool is_address_in_ddr_range(uint64_t addr, uint64_t size);
+
 #endif /* SOCFPGA_SIP_SVC_H */
diff --git a/plat/intel/soc/common/soc/socfpga_mailbox.c b/plat/intel/soc/common/soc/socfpga_mailbox.c
index d066f27..aec94af 100644
--- a/plat/intel/soc/common/soc/socfpga_mailbox.c
+++ b/plat/intel/soc/common/soc/socfpga_mailbox.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2019, Intel Corporation. All rights reserved.
+ * Copyright (c) 2020, Intel Corporation. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -11,188 +11,324 @@
 #include "socfpga_mailbox.h"
 #include "socfpga_sip_svc.h"
 
+
+static bool is_mailbox_cmdbuf_full(uint32_t cin)
+{
+	uint32_t cout = mmio_read_32(MBOX_OFFSET + MBOX_COUT);
+
+	return (((cin + 1U) % MBOX_CMD_BUFFER_SIZE) == cout);
+}
+
+static bool is_mailbox_cmdbuf_empty(uint32_t cin)
+{
+	uint32_t cout = mmio_read_32(MBOX_OFFSET + MBOX_COUT);
+
+	return (((cout + 1U) % MBOX_CMD_BUFFER_SIZE) == cin);
+}
+
+static int wait_for_mailbox_cmdbuf_empty(uint32_t cin)
+{
+	unsigned int timeout = 200U;
+
+	do {
+		if (is_mailbox_cmdbuf_empty(cin)) {
+			break;
+		}
+		mdelay(10U);
+	} while (--timeout != 0U);
+
+	if (timeout == 0U) {
+		return MBOX_TIMEOUT;
+	}
+
+	return MBOX_RET_OK;
+}
+
+static int write_mailbox_cmd_buffer(uint32_t *cin, uint32_t cout,
+				    uint32_t data,
+				    bool *is_doorbell_triggered)
+{
+	unsigned int timeout = 100U;
+
+	do {
+		if (is_mailbox_cmdbuf_full(*cin)) {
+			if (!(*is_doorbell_triggered)) {
+				mmio_write_32(MBOX_OFFSET +
+					      MBOX_DOORBELL_TO_SDM, 1U);
+				*is_doorbell_triggered = true;
+			}
+			mdelay(10U);
+		} else {
+			mmio_write_32(MBOX_OFFSET + MBOX_CMD_BUFFER +
+				      (*cin * 4), data);
+			(*cin)++;
+			*cin %= MBOX_CMD_BUFFER_SIZE;
+			mmio_write_32(MBOX_OFFSET + MBOX_CIN, *cin);
+			break;
+		}
+	} while (--timeout != 0U);
+
+	if (timeout == 0U) {
+		return MBOX_TIMEOUT;
+	}
+
+	if (*is_doorbell_triggered) {
+		int ret = wait_for_mailbox_cmdbuf_empty(*cin);
+		return ret;
+	}
+
+	return MBOX_RET_OK;
+}
+
 static int fill_mailbox_circular_buffer(uint32_t header_cmd, uint32_t *args,
-					int len)
+					unsigned int len)
 {
-	uint32_t cmd_free_offset;
-	int i;
+	uint32_t sdm_read_offset, cmd_free_offset;
+	unsigned int i;
+	int ret;
+	bool is_doorbell_triggered = false;
 
 	cmd_free_offset = mmio_read_32(MBOX_OFFSET + MBOX_CIN);
+	sdm_read_offset = mmio_read_32(MBOX_OFFSET + MBOX_COUT);
 
-	mmio_write_32(MBOX_OFFSET + MBOX_CMD_BUFFER + (cmd_free_offset++ * 4),
-			header_cmd);
+	ret = write_mailbox_cmd_buffer(&cmd_free_offset, sdm_read_offset,
+				       header_cmd, &is_doorbell_triggered);
+	if (ret != 0) {
+		goto restart_mailbox;
+	}
 
+	for (i = 0U; i < len; i++) {
+		is_doorbell_triggered = false;
+		ret = write_mailbox_cmd_buffer(&cmd_free_offset,
+					       sdm_read_offset, args[i],
+					       &is_doorbell_triggered);
+		if (ret != 0) {
+			goto restart_mailbox;
+		}
+	}
 
-	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]);
+	if (!is_doorbell_triggered) {
+		mmio_write_32(MBOX_OFFSET + MBOX_DOORBELL_TO_SDM, 1U);
 	}
 
-	cmd_free_offset %= MBOX_CMD_BUFFER_SIZE;
-	mmio_write_32(MBOX_OFFSET + MBOX_CIN, cmd_free_offset);
+	return MBOX_RET_OK;
 
-	return 0;
+restart_mailbox:
+	/*
+	 * Attempt to restart mailbox if the driver not able to write
+	 * into mailbox command buffer
+	 */
+	if (MBOX_CMD_MASK(header_cmd) != MBOX_CMD_RESTART) {
+		INFO("Mailbox timed out: Attempting mailbox reset\n");
+		ret = mailbox_init();
+
+		if (ret == MBOX_TIMEOUT) {
+			INFO("Error: Mailbox fail to restart\n");
+		}
+	}
+
+	return MBOX_TIMEOUT;
 }
 
-int mailbox_read_response(int job_id, uint32_t *response, int resp_len)
+int mailbox_read_response(unsigned int *job_id, uint32_t *response,
+				unsigned int resp_len)
 {
-	int rin = 0;
-	int rout = 0;
-	int response_length = 0;
-	int resp = 0;
-	int total_resp_len = 0;
+	uint32_t rin;
+	uint32_t rout;
+	uint32_t resp_data;
+	unsigned int ret_resp_len;
 
-	if (mmio_read_32(MBOX_OFFSET + MBOX_DOORBELL_FROM_SDM))
-		mmio_write_32(MBOX_OFFSET + MBOX_DOORBELL_FROM_SDM, 0);
+	if (mmio_read_32(MBOX_OFFSET + MBOX_DOORBELL_FROM_SDM) == 1U) {
+		mmio_write_32(MBOX_OFFSET + MBOX_DOORBELL_FROM_SDM, 0U);
+	}
 
 	rin = mmio_read_32(MBOX_OFFSET + MBOX_RIN);
 	rout = mmio_read_32(MBOX_OFFSET + MBOX_ROUT);
 
 	if (rout != rin) {
-		resp = mmio_read_32(MBOX_OFFSET +
-				    MBOX_RESP_BUFFER + ((rout++)*4));
+		resp_data = mmio_read_32(MBOX_OFFSET +
+				    MBOX_RESP_BUFFER + ((rout++)*4U));
 
 		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) {
+
+		if (MBOX_RESP_CLIENT_ID(resp_data) != MBOX_ATF_CLIENT_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);
+		*job_id = MBOX_RESP_JOB_ID(resp_data);
 
-		while (response_length) {
+		ret_resp_len = MBOX_RESP_LEN(resp_data);
 
-			response_length--;
-			resp = mmio_read_32(MBOX_OFFSET +
-						MBOX_RESP_BUFFER +
-						(rout)*4);
-			if (response && resp_len) {
-				*(response + total_resp_len) = resp;
-				resp_len--;
-				total_resp_len++;
-			}
-			rout++;
-			rout %= MBOX_RESP_BUFFER_SIZE;
-			mmio_write_32(MBOX_OFFSET + MBOX_ROUT, rout);
+		if (ret_resp_len != 0U) {
+			ret_resp_len = iterate_resp(ret_resp_len, response,
+						    resp_len);
 		}
-		return total_resp_len;
-	}
+
+		if (MBOX_RESP_ERR(resp_data) > 0U) {
+			INFO("Error in response: %x\n", resp_data);
+			return -MBOX_RESP_ERR(resp_data);
+		}
 
+		return ret_resp_len;
+	}
 	return MBOX_NO_RESPONSE;
 }
 
 
-int mailbox_poll_response(int job_id, int urgent, uint32_t *response,
-				int resp_len)
+int mailbox_poll_response(uint32_t job_id, uint32_t urgent, uint32_t *response,
+				unsigned int resp_len)
 {
-	int timeout = 0xFFFFFF;
-	int rin = 0;
-	int rout = 0;
-	int response_length = 0;
-	int resp = 0;
-	int total_resp_len = 0;
+	unsigned int timeout = 40U;
+	unsigned int sdm_loop = 255U;
+	unsigned int ret_resp_len;
+	uint32_t rin;
+	uint32_t rout;
+	uint32_t resp_data;
 
-	while (1) {
+	while (sdm_loop != 0U) {
 
-		while (timeout > 0 &&
-			!(mmio_read_32(MBOX_OFFSET +
-				MBOX_DOORBELL_FROM_SDM) & 1)) {
-			timeout--;
-		}
+		do {
+			if (mmio_read_32(MBOX_OFFSET + MBOX_DOORBELL_FROM_SDM)
+				== 1U) {
+				break;
+			}
+			mdelay(10U);
+		} while (--timeout != 0U);
 
-		if (!timeout) {
-			INFO("Timed out waiting for SDM");
-			return MBOX_TIMEOUT;
+		if (timeout == 0U) {
+			break;
 		}
 
-		mmio_write_32(MBOX_OFFSET + MBOX_DOORBELL_FROM_SDM, 0);
+		mmio_write_32(MBOX_OFFSET + MBOX_DOORBELL_FROM_SDM, 0U);
 
-		if (urgent & 1) {
-			mdelay(5);
+		if ((urgent & 1U) != 0U) {
+			mdelay(5U);
 			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, 0U);
+				return MBOX_RET_OK;
 			}
 
-			mmio_write_32(MBOX_OFFSET + MBOX_URG, 0);
+			mmio_write_32(MBOX_OFFSET + MBOX_URG, 0U);
 			INFO("Error: Mailbox did not get UA");
-			return -1;
+			return MBOX_RET_ERROR;
 		}
 
 		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));
+			resp_data = mmio_read_32(MBOX_OFFSET +
+					    MBOX_RESP_BUFFER + ((rout++)*4U));
 
 			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)
+			if (MBOX_RESP_CLIENT_ID(resp_data) != MBOX_ATF_CLIENT_ID
+				|| MBOX_RESP_JOB_ID(resp_data) != job_id) {
 				continue;
+			}
 
-			if (MBOX_RESP_ERR(resp) > 0) {
-				INFO("Error in response: %x\n", resp);
-				return -MBOX_RESP_ERR(resp);
+			ret_resp_len = MBOX_RESP_LEN(resp_data);
+
+			if (ret_resp_len != 0U) {
+				ret_resp_len = iterate_resp(ret_resp_len,
+							    response,
+							    resp_len);
 			}
-			response_length = MBOX_RESP_LEN(resp);
 
-			while (response_length) {
-				response_length--;
-				resp = mmio_read_32(MBOX_OFFSET +
-							MBOX_RESP_BUFFER +
-							(rout)*4);
-				if (response && resp_len) {
-					*(response + total_resp_len) = resp;
-					resp_len--;
-					total_resp_len++;
-				}
-				rout++;
-				rout %= MBOX_RESP_BUFFER_SIZE;
-				mmio_write_32(MBOX_OFFSET + MBOX_ROUT, rout);
+			if (MBOX_RESP_ERR(resp_data) > 0U) {
+				INFO("Error in response: %x\n", resp_data);
+				return -MBOX_RESP_ERR(resp_data);
 			}
-			return total_resp_len;
+
+			return ret_resp_len;
+		}
+
+	sdm_loop--;
+	}
+
+	INFO("Timed out waiting for SDM\n");
+	return MBOX_TIMEOUT;
+}
+
+unsigned int iterate_resp(uint32_t mbox_resp_len, uint32_t *resp_buf,
+			unsigned int resp_len)
+{
+	unsigned int timeout, total_resp_len = 0U;
+	uint32_t resp_data;
+	uint32_t rin = mmio_read_32(MBOX_OFFSET + MBOX_RIN);
+	uint32_t rout = mmio_read_32(MBOX_OFFSET + MBOX_ROUT);
+
+	while (mbox_resp_len > 0U) {
+		timeout = 100U;
+		mbox_resp_len--;
+		resp_data = mmio_read_32(MBOX_OFFSET +
+					MBOX_RESP_BUFFER +
+					(rout)*4U);
+
+		if ((resp_buf != NULL) && (resp_len != 0U)) {
+			*(resp_buf + total_resp_len)
+					= resp_data;
+			resp_len--;
+			total_resp_len++;
+		}
+		rout++;
+		rout %= MBOX_RESP_BUFFER_SIZE;
+		mmio_write_32(MBOX_OFFSET + MBOX_ROUT, rout);
+
+		do {
+			rin = mmio_read_32(MBOX_OFFSET + MBOX_RIN);
+			if (rout == rin) {
+				mdelay(10U);
+			} else {
+				break;
+			}
+			timeout--;
+		} while ((mbox_resp_len > 0U) && (timeout != 0U));
+
+		if (timeout == 0U) {
+			INFO("Timed out waiting for SDM\n");
+			return MBOX_TIMEOUT;
 		}
 	}
+	return total_resp_len;
 }
 
-int mailbox_send_cmd_async(int job_id, unsigned int cmd, uint32_t *args,
-			  int len, int urgent)
+int mailbox_send_cmd_async(uint32_t *job_id, uint32_t cmd, uint32_t *args,
+			  unsigned int len, unsigned int indirect)
 {
-	if (urgent)
-		mmio_write_32(MBOX_OFFSET + MBOX_URG, 1);
+	int status;
 
-	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);
+	status = 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(indirect) |
+				cmd, args, len);
+	if (status < 0) {
+		return status;
+	}
 
-	mmio_write_32(MBOX_OFFSET + MBOX_DOORBELL_TO_SDM, 1);
+	*job_id = (*job_id + 1U) % MBOX_MAX_IND_JOB_ID;
 
-	return 0;
+	return MBOX_RET_OK;
 }
 
-int mailbox_send_cmd(int job_id, unsigned int cmd, uint32_t *args,
-			int len, int urgent, uint32_t *response, int resp_len)
+int mailbox_send_cmd(uint32_t job_id, uint32_t cmd, uint32_t *args,
+			unsigned int len, uint32_t urgent, uint32_t *response,
+			unsigned int resp_len)
 {
 	int status = 0;
 
-	if (urgent) {
+	if (urgent != 0U) {
 		urgent |= mmio_read_32(MBOX_OFFSET + MBOX_STATUS) &
 					MBOX_STATUS_UA_MASK;
 		mmio_write_32(MBOX_OFFSET + MBOX_URG, cmd);
+		mmio_write_32(MBOX_OFFSET + MBOX_DOORBELL_TO_SDM, 1U);
 	}
 
 	else {
@@ -203,10 +339,10 @@
 			cmd, args, len);
 	}
 
-	if (status)
+	if (status != 0) {
 		return status;
+	}
 
-	mmio_write_32(MBOX_OFFSET + MBOX_DOORBELL_TO_SDM, 1);
 	status = mailbox_poll_response(job_id, urgent, response, resp_len);
 
 	return status;
@@ -218,7 +354,7 @@
 		mmio_read_32(MBOX_OFFSET + MBOX_RIN));
 }
 
-void mailbox_set_int(int interrupt)
+void mailbox_set_int(uint32_t interrupt)
 {
 
 	mmio_write_32(MBOX_OFFSET+MBOX_INT, MBOX_COE_BIT(interrupt) |
@@ -229,48 +365,45 @@
 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, NULL, 0);
+	mailbox_send_cmd(MBOX_JOB_ID, MBOX_CMD_QSPI_OPEN, NULL, 0U,
+				CMD_CASUAL, NULL, 0U);
 }
 
 void mailbox_set_qspi_direct(void)
 {
-	mailbox_send_cmd(MBOX_JOB_ID, MBOX_CMD_QSPI_DIRECT, 0, 0, 0, NULL, 0);
+	mailbox_send_cmd(MBOX_JOB_ID, MBOX_CMD_QSPI_DIRECT, NULL, 0U,
+				CMD_CASUAL, NULL, 0U);
 }
 
 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, NULL, 0);
+	mailbox_send_cmd(MBOX_JOB_ID, MBOX_CMD_QSPI_CLOSE, NULL, 0U,
+				CMD_CASUAL, NULL, 0U);
 }
 
-int mailbox_get_qspi_clock(void)
+void mailbox_qspi_set_cs(uint32_t device_select)
 {
-	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,
-				NULL, 0);
-}
-
-void mailbox_qspi_set_cs(int device_select)
-{
-	uint32_t cs_setting = device_select;
+	uint32_t cs_setting;
 
 	/* QSPI device select settings at 31:28 */
-	cs_setting = (cs_setting << 28);
+	cs_setting = (device_select << 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, NULL, 0);
+				1U, CMD_CASUAL, NULL, 0U);
 }
 
 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, NULL, 0);
+	mailbox_send_cmd(MBOX_JOB_ID, MBOX_CMD_REBOOT_HPS, NULL, 0U,
+				CMD_CASUAL, NULL, 0U);
 }
 
-int mailbox_rsu_get_spt_offset(uint32_t *resp_buf, uint32_t resp_buf_len)
+int mailbox_rsu_get_spt_offset(uint32_t *resp_buf, unsigned int resp_buf_len)
 {
 	return mailbox_send_cmd(MBOX_JOB_ID, MBOX_GET_SUBPARTITION_TABLE,
-				NULL, 0, 0, (uint32_t *)resp_buf,
+				NULL, 0U, CMD_CASUAL, resp_buf,
 				resp_buf_len);
 }
 
@@ -284,22 +417,26 @@
 	uint32_t retry_counter;
 };
 
-int mailbox_rsu_status(uint32_t *resp_buf, uint32_t resp_buf_len)
+int mailbox_rsu_status(uint32_t *resp_buf, unsigned int resp_buf_len)
 {
 	int ret;
 	struct rsu_status_info *info = (struct rsu_status_info *)resp_buf;
 
-	info->retry_counter = ~0;
+	info->retry_counter = ~0U;
 
-	ret = mailbox_send_cmd(MBOX_JOB_ID, MBOX_RSU_STATUS, NULL, 0, 0,
-			       (uint32_t *)resp_buf, resp_buf_len);
+	ret = mailbox_send_cmd(MBOX_JOB_ID, MBOX_RSU_STATUS, NULL, 0U,
+				CMD_CASUAL, resp_buf,
+				resp_buf_len);
 
-	if (ret < 0)
+	if (ret < 0) {
 		return ret;
+	}
 
-	if (info->retry_counter != ~0)
-		if (!(info->version & RSU_VERSION_ACMF_MASK))
+	if (info->retry_counter != ~0U) {
+		if ((info->version & RSU_VERSION_ACMF_MASK) == 0U) {
 			info->version |= RSU_VERSION_ACMF;
+		}
+	}
 
 	return ret;
 }
@@ -307,33 +444,37 @@
 int mailbox_rsu_update(uint32_t *flash_offset)
 {
 	return mailbox_send_cmd(MBOX_JOB_ID, MBOX_RSU_UPDATE,
-				flash_offset, 2, 0, NULL, 0);
+				flash_offset, 2U,
+				CMD_CASUAL, NULL, 0U);
 }
 
 int mailbox_hps_stage_notify(uint32_t execution_stage)
 {
 	return mailbox_send_cmd(MBOX_JOB_ID, MBOX_HPS_STAGE_NOTIFY,
-				&execution_stage, 1, 0, NULL, 0);
+				&execution_stage, 1U, CMD_CASUAL,
+				NULL, 0U);
 }
 
 int mailbox_init(void)
 {
-	int status = 0;
+	int status;
 
 	mailbox_set_int(MBOX_INT_FLAG_COE | MBOX_INT_FLAG_RIE |
 			MBOX_INT_FLAG_UAE);
-	mmio_write_32(MBOX_OFFSET + MBOX_URG, 0);
-	mmio_write_32(MBOX_OFFSET + MBOX_DOORBELL_FROM_SDM, 0);
+	mmio_write_32(MBOX_OFFSET + MBOX_URG, 0U);
+	mmio_write_32(MBOX_OFFSET + MBOX_DOORBELL_FROM_SDM, 0U);
 
-	status = mailbox_send_cmd(0, MBOX_CMD_RESTART, 0, 0, 1, NULL, 0);
+	status = mailbox_send_cmd(0U, MBOX_CMD_RESTART, NULL, 0U,
+					CMD_URGENT, NULL, 0U);
 
-	if (status)
+	if (status != 0) {
 		return status;
+	}
 
 	mailbox_set_int(MBOX_INT_FLAG_COE | MBOX_INT_FLAG_RIE |
 			MBOX_INT_FLAG_UAE);
 
-	return 0;
+	return MBOX_RET_OK;
 }
 
 int intel_mailbox_get_config_status(uint32_t cmd)
@@ -341,27 +482,32 @@
 	int status;
 	uint32_t res, response[6];
 
-	status = mailbox_send_cmd(1, cmd, NULL, 0, 0, response,
-		sizeof(response) / sizeof(response[0]));
+	status = mailbox_send_cmd(MBOX_JOB_ID, cmd, NULL, 0U, CMD_CASUAL,
+				response, ARRAY_SIZE(response));
 
-	if (status < 0)
+	if (status < 0) {
 		return status;
+	}
 
 	res = response[RECONFIG_STATUS_STATE];
-	if (res && res != MBOX_CFGSTAT_STATE_CONFIG)
+	if ((res != 0U) && (res != MBOX_CFGSTAT_STATE_CONFIG)) {
 		return res;
+	}
 
 	res = response[RECONFIG_STATUS_PIN_STATUS];
-	if (!(res & PIN_STATUS_NSTATUS))
+	if ((res & PIN_STATUS_NSTATUS) == 0U) {
 		return MBOX_CFGSTAT_STATE_ERROR_HARDWARE;
+	}
 
 	res = response[RECONFIG_STATUS_SOFTFUNC_STATUS];
-	if (res & SOFTFUNC_STATUS_SEU_ERROR)
+	if ((res & SOFTFUNC_STATUS_SEU_ERROR) != 0U) {
 		return MBOX_CFGSTAT_STATE_ERROR_HARDWARE;
+	}
 
-	if ((res & SOFTFUNC_STATUS_CONF_DONE) &&
-		(res & SOFTFUNC_STATUS_INIT_DONE))
-		return 0;
+	if ((res & SOFTFUNC_STATUS_CONF_DONE) != 0U &&
+		(res & SOFTFUNC_STATUS_INIT_DONE) != 0U) {
+		return MBOX_RET_OK;
+	}
 
 	return MBOX_CFGSTAT_STATE_CONFIG;
 }
@@ -370,8 +516,9 @@
 {
 	int ret = intel_mailbox_get_config_status(MBOX_RECONFIG_STATUS);
 
-	if (ret && ret != MBOX_CFGSTAT_STATE_CONFIG)
+	if ((ret != MBOX_RET_OK) && (ret != MBOX_CFGSTAT_STATE_CONFIG)) {
 		ret = intel_mailbox_get_config_status(MBOX_CONFIG_STATUS);
+	}
 
 	return ret;
 }
diff --git a/plat/intel/soc/common/socfpga_sip_svc.c b/plat/intel/soc/common/socfpga_sip_svc.c
index a20baab..86a4455 100644
--- a/plat/intel/soc/common/socfpga_sip_svc.c
+++ b/plat/intel/soc/common/socfpga_sip_svc.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2019, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2019-2020, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -14,30 +14,15 @@
 #include "socfpga_reset_manager.h"
 #include "socfpga_sip_svc.h"
 
-/* Number of SiP Calls implemented */
-#define SIP_NUM_CALLS		0x3
 
 /* Total buffer the driver can hold */
 #define FPGA_CONFIG_BUFFER_SIZE 4
 
-static int current_block;
-static int read_block;
-static int current_buffer;
-static int send_id;
-static int rcv_id;
-static int max_blocks;
-static uint32_t bytes_per_block;
-static uint32_t blocks_submitted;
-static int is_partial_reconfig;
+static int current_block, current_buffer;
+static int read_block, max_blocks, is_partial_reconfig;
+static uint32_t send_id, rcv_id;
+static uint32_t bytes_per_block, blocks_submitted;
 
-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,
@@ -74,10 +59,8 @@
 			args[2] = bytes_per_block;
 
 		buffer->size_written += args[2];
-		mailbox_send_cmd_async(
-			send_id++ % MBOX_MAX_JOB_ID,
-			MBOX_RECONFIG_DATA,
-			args, 3, 0);
+		mailbox_send_cmd_async(&send_id, MBOX_RECONFIG_DATA, args,
+					3U, CMD_INDIRECT);
 
 		buffer->subblocks_sent++;
 		max_blocks--;
@@ -143,7 +126,7 @@
 }
 
 static int intel_fpga_config_completed_write(uint32_t *completed_addr,
-					uint32_t *count)
+					uint32_t *count, uint32_t *job_id)
 {
 	uint32_t status = INTEL_SIP_SMC_STATUS_OK;
 	*count = 0;
@@ -153,14 +136,13 @@
 
 	while (*count < 3) {
 
-		resp_len = mailbox_read_response(rcv_id % MBOX_MAX_JOB_ID,
-				resp, sizeof(resp) / sizeof(resp[0]));
+		resp_len = mailbox_read_response(job_id,
+				resp, ARRAY_SIZE(resp));
 
 		if (resp_len < 0)
 			break;
 
 		max_blocks++;
-		rcv_id++;
 
 		if (mark_last_buffer_xfer_completed(
 			&completed_addr[*count]) == 0)
@@ -208,10 +190,10 @@
 
 	mailbox_clear_response();
 
-	mailbox_send_cmd(1, MBOX_CMD_CANCEL, 0, 0, 0, NULL, 0);
+	mailbox_send_cmd(1U, MBOX_CMD_CANCEL, NULL, 0U, CMD_CASUAL, NULL, 0U);
 
-	status = mailbox_send_cmd(1, MBOX_RECONFIG, 0, 0, 0,
-			response, sizeof(response) / sizeof(response[0]));
+	status = mailbox_send_cmd(1U, MBOX_RECONFIG, NULL, 0U, CMD_CASUAL,
+			response, ARRAY_SIZE(response));
 
 	if (status < 0)
 		return status;
@@ -232,8 +214,6 @@
 	current_block = 0;
 	read_block = 0;
 	current_buffer = 0;
-	send_id = 0;
-	rcv_id = 0;
 
 	/* full reconfiguration */
 	if (!is_partial_reconfig) {
@@ -252,7 +232,7 @@
 	return true;
 }
 
-static bool is_address_in_ddr_range(uint64_t addr, uint64_t size)
+bool is_address_in_ddr_range(uint64_t addr, uint64_t size)
 {
 	if (size > (UINT64_MAX - addr))
 		return false;
@@ -371,7 +351,7 @@
 /* Intel Remote System Update (RSU) services */
 uint64_t intel_rsu_update_address;
 
-static uint32_t intel_rsu_status(uint64_t *respbuf, uint32_t respbuf_sz)
+static uint32_t intel_rsu_status(uint64_t *respbuf, unsigned int respbuf_sz)
 {
 	if (mailbox_rsu_status((uint32_t *)respbuf, respbuf_sz) < 0)
 		return INTEL_SIP_SMC_RSU_ERROR;
@@ -404,9 +384,9 @@
 }
 
 /* Mailbox services */
-static uint32_t intel_mbox_send_cmd(uint32_t cmd, uint32_t *args, int len,
-				    int urgent, uint32_t *response,
-				    int resp_len, int *mbox_status,
+static uint32_t intel_mbox_send_cmd(uint32_t cmd, uint32_t *args, uint32_t len,
+				    uint32_t urgent, uint32_t *response,
+				    uint32_t resp_len, int *mbox_status,
 				    int *len_in_resp)
 {
 	*len_in_resp = 0;
@@ -441,14 +421,14 @@
 			 void *handle,
 			 u_register_t flags)
 {
-	uint32_t val = 0;
+	uint32_t retval = 0;
 	uint32_t status = INTEL_SIP_SMC_STATUS_OK;
 	uint32_t completed_addr[3];
 	uint64_t rsu_respbuf[9];
-	uint32_t count = 0;
 	u_register_t x5, x6;
 	int mbox_status, len_in_resp;
 
+
 	switch (smc_fid) {
 	case SIP_SVC_UID:
 		/* Return UID to the caller */
@@ -474,8 +454,8 @@
 
 	case INTEL_SIP_SMC_FPGA_CONFIG_COMPLETED_WRITE:
 		status = intel_fpga_config_completed_write(completed_addr,
-								&count);
-		switch (count) {
+							&retval, &rcv_id);
+		switch (retval) {
 		case 1:
 			SMC_RET4(handle, INTEL_SIP_SMC_STATUS_OK,
 				completed_addr[0], 0, 0);
@@ -500,17 +480,17 @@
 		}
 
 	case INTEL_SIP_SMC_REG_READ:
-		status = intel_secure_reg_read(x1, &val);
-		SMC_RET3(handle, status, val, x1);
+		status = intel_secure_reg_read(x1, &retval);
+		SMC_RET3(handle, status, retval, x1);
 
 	case INTEL_SIP_SMC_REG_WRITE:
-		status = intel_secure_reg_write(x1, (uint32_t)x2, &val);
-		SMC_RET3(handle, status, val, x1);
+		status = intel_secure_reg_write(x1, (uint32_t)x2, &retval);
+		SMC_RET3(handle, status, retval, x1);
 
 	case INTEL_SIP_SMC_REG_UPDATE:
 		status = intel_secure_reg_update(x1, (uint32_t)x2,
-						 (uint32_t)x3, &val);
-		SMC_RET3(handle, status, val, x1);
+						 (uint32_t)x3, &retval);
+		SMC_RET3(handle, status, retval, x1);
 
 	case INTEL_SIP_SMC_RSU_STATUS:
 		status = intel_rsu_status(rsu_respbuf,
@@ -532,11 +512,11 @@
 
 	case INTEL_SIP_SMC_RSU_RETRY_COUNTER:
 		status = intel_rsu_retry_counter((uint32_t *)rsu_respbuf,
-						ARRAY_SIZE(rsu_respbuf), &val);
+						ARRAY_SIZE(rsu_respbuf), &retval);
 		if (status) {
 			SMC_RET1(handle, status);
 		} else {
-			SMC_RET2(handle, status, val);
+			SMC_RET2(handle, status, retval);
 		}
 
 	case INTEL_SIP_SMC_MBOX_SEND_CMD:
diff --git a/plat/intel/soc/stratix10/bl31_plat_setup.c b/plat/intel/soc/stratix10/bl31_plat_setup.c
index e0c3054..128a808 100644
--- a/plat/intel/soc/stratix10/bl31_plat_setup.c
+++ b/plat/intel/soc/stratix10/bl31_plat_setup.c
@@ -109,6 +109,8 @@
  ******************************************************************************/
 void bl31_platform_setup(void)
 {
+	socfpga_delay_timer_init();
+
 	/* Initialize the gic cpu and distributor interfaces */
 	gicv2_driver_init(&plat_gicv2_gic_data);
 	gicv2_distif_init();
diff --git a/plat/intel/soc/stratix10/platform.mk b/plat/intel/soc/stratix10/platform.mk
index 3bd6af9..8bbd010 100644
--- a/plat/intel/soc/stratix10/platform.mk
+++ b/plat/intel/soc/stratix10/platform.mk
@@ -25,7 +25,8 @@
 			lib/xlat_tables/aarch64/xlat_tables.c 		\
 			lib/xlat_tables/xlat_tables_common.c 		\
 			plat/intel/soc/common/aarch64/platform_common.c \
-			plat/intel/soc/common/aarch64/plat_helpers.S
+			plat/intel/soc/common/aarch64/plat_helpers.S	\
+			plat/intel/soc/common/socfpga_delay_timer.c
 
 BL2_SOURCES     +=	\
 		common/desc_image_load.c				\
@@ -43,7 +44,6 @@
 		plat/intel/soc/stratix10/soc/s10_memory_controller.c	\
 		plat/intel/soc/stratix10/soc/s10_pinmux.c		\
                 plat/intel/soc/common/bl2_plat_mem_params_desc.c	\
-		plat/intel/soc/common/socfpga_delay_timer.c		\
 		plat/intel/soc/common/socfpga_image_load.c		\
 		plat/intel/soc/common/socfpga_storage.c			\
 		plat/intel/soc/common/soc/socfpga_emac.c		\