diff --git a/plat/intel/soc/common/include/socfpga_mailbox.h b/plat/intel/soc/common/include/socfpga_mailbox.h
index 27f8277..687022c 100644
--- a/plat/intel/soc/common/include/socfpga_mailbox.h
+++ b/plat/intel/soc/common/include/socfpga_mailbox.h
@@ -148,18 +148,18 @@
 
 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);
+			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);
+			unsigned int *resp_len);
+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);
 
-int intel_mailbox_get_config_status(uint32_t cmd);
+int intel_mailbox_get_config_status(uint32_t cmd, bool init_done);
 int intel_mailbox_is_fpga_not_ready(void);
 
 int mailbox_rsu_get_spt_offset(uint32_t *resp_buf, uint32_t resp_buf_len);
diff --git a/plat/intel/soc/common/soc/socfpga_mailbox.c b/plat/intel/soc/common/soc/socfpga_mailbox.c
index e445b60..306eea26 100644
--- a/plat/intel/soc/common/soc/socfpga_mailbox.c
+++ b/plat/intel/soc/common/soc/socfpga_mailbox.c
@@ -127,7 +127,7 @@
 }
 
 int mailbox_read_response(unsigned int *job_id, uint32_t *response,
-				unsigned int resp_len)
+				unsigned int *resp_len)
 {
 	uint32_t rin;
 	uint32_t rout;
@@ -156,9 +156,9 @@
 
 		ret_resp_len = MBOX_RESP_LEN(resp_data);
 
-		if (ret_resp_len != 0U) {
-			ret_resp_len = iterate_resp(ret_resp_len, response,
-						    resp_len);
+		if (iterate_resp(ret_resp_len, response, resp_len)
+			!= MBOX_RET_OK) {
+			return MBOX_TIMEOUT;
 		}
 
 		if (MBOX_RESP_ERR(resp_data) > 0U) {
@@ -166,14 +166,14 @@
 			return -MBOX_RESP_ERR(resp_data);
 		}
 
-		return ret_resp_len;
+		return MBOX_RET_OK;
 	}
 	return MBOX_NO_RESPONSE;
 }
 
 
 int mailbox_poll_response(uint32_t job_id, uint32_t urgent, uint32_t *response,
-				unsigned int resp_len)
+				unsigned int *resp_len)
 {
 	unsigned int timeout = 40U;
 	unsigned int sdm_loop = 255U;
@@ -229,10 +229,9 @@
 
 			ret_resp_len = MBOX_RESP_LEN(resp_data);
 
-			if (ret_resp_len != 0U) {
-				ret_resp_len = iterate_resp(ret_resp_len,
-							    response,
-							    resp_len);
+			if (iterate_resp(ret_resp_len, response, resp_len)
+				!= MBOX_RET_OK) {
+				return MBOX_TIMEOUT;
 			}
 
 			if (MBOX_RESP_ERR(resp_data) > 0U) {
@@ -240,7 +239,7 @@
 				return -MBOX_RESP_ERR(resp_data);
 			}
 
-			return ret_resp_len;
+			return MBOX_RET_OK;
 		}
 
 	sdm_loop--;
@@ -250,8 +249,8 @@
 	return MBOX_TIMEOUT;
 }
 
-unsigned int iterate_resp(uint32_t mbox_resp_len, uint32_t *resp_buf,
-			unsigned int resp_len)
+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;
@@ -263,10 +262,11 @@
 		mbox_resp_len--;
 		resp_data = mmio_read_32(MBOX_ENTRY_TO_ADDR(RESP, (rout)++));
 
-		if ((resp_buf != NULL) && (resp_len != 0U)) {
+		if ((resp_buf != NULL) && (resp_len != NULL)
+			&& (*resp_len != 0U)) {
 			*(resp_buf + total_resp_len)
 					= resp_data;
-			resp_len--;
+			*resp_len = *resp_len - 1;
 			total_resp_len++;
 		}
 		rout %= MBOX_RESP_BUFFER_SIZE;
@@ -287,7 +287,11 @@
 			return MBOX_TIMEOUT;
 		}
 	}
-	return total_resp_len;
+
+	if (resp_len)
+		*resp_len = total_resp_len;
+
+	return MBOX_RET_OK;
 }
 
 int mailbox_send_cmd_async(uint32_t *job_id, uint32_t cmd, uint32_t *args,
@@ -312,7 +316,7 @@
 
 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)
+			unsigned int *resp_len)
 {
 	int status = 0;
 
@@ -358,20 +362,20 @@
 {
 	mailbox_set_int(MBOX_INT_FLAG_COE | MBOX_INT_FLAG_RIE);
 	mailbox_send_cmd(MBOX_JOB_ID, MBOX_CMD_QSPI_OPEN, NULL, 0U,
-				CMD_CASUAL, NULL, 0U);
+				CMD_CASUAL, NULL, NULL);
 }
 
 void mailbox_set_qspi_direct(void)
 {
 	mailbox_send_cmd(MBOX_JOB_ID, MBOX_CMD_QSPI_DIRECT, NULL, 0U,
-				CMD_CASUAL, NULL, 0U);
+				CMD_CASUAL, NULL, NULL);
 }
 
 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, NULL, 0U,
-				CMD_CASUAL, NULL, 0U);
+				CMD_CASUAL, NULL, NULL);
 }
 
 void mailbox_qspi_set_cs(uint32_t device_select)
@@ -382,21 +386,21 @@
 	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,
-				1U, CMD_CASUAL, NULL, 0U);
+				1U, CMD_CASUAL, NULL, NULL);
 }
 
 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, NULL, 0U,
-				CMD_CASUAL, NULL, 0U);
+				CMD_CASUAL, NULL, NULL);
 }
 
 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, 0U, CMD_CASUAL, resp_buf,
-				resp_buf_len);
+				&resp_buf_len);
 }
 
 struct rsu_status_info {
@@ -418,7 +422,7 @@
 
 	ret = mailbox_send_cmd(MBOX_JOB_ID, MBOX_RSU_STATUS, NULL, 0U,
 				CMD_CASUAL, resp_buf,
-				resp_buf_len);
+				&resp_buf_len);
 
 	if (ret < 0) {
 		return ret;
@@ -437,14 +441,14 @@
 {
 	return mailbox_send_cmd(MBOX_JOB_ID, MBOX_RSU_UPDATE,
 				flash_offset, 2U,
-				CMD_CASUAL, NULL, 0U);
+				CMD_CASUAL, NULL, NULL);
 }
 
 int mailbox_hps_stage_notify(uint32_t execution_stage)
 {
 	return mailbox_send_cmd(MBOX_JOB_ID, MBOX_HPS_STAGE_NOTIFY,
 				&execution_stage, 1U, CMD_CASUAL,
-				NULL, 0U);
+				NULL, NULL);
 }
 
 int mailbox_init(void)
@@ -457,7 +461,7 @@
 	mmio_write_32(MBOX_OFFSET + MBOX_DOORBELL_FROM_SDM, 0U);
 
 	status = mailbox_send_cmd(0U, MBOX_CMD_RESTART, NULL, 0U,
-					CMD_URGENT, NULL, 0U);
+					CMD_URGENT, NULL, NULL);
 
 	if (status != 0) {
 		return status;
@@ -469,13 +473,14 @@
 	return MBOX_RET_OK;
 }
 
-int intel_mailbox_get_config_status(uint32_t cmd)
+int intel_mailbox_get_config_status(uint32_t cmd, bool init_done)
 {
 	int status;
 	uint32_t res, response[6];
+	unsigned int resp_len = ARRAY_SIZE(response);
 
 	status = mailbox_send_cmd(MBOX_JOB_ID, cmd, NULL, 0U, CMD_CASUAL,
-				response, ARRAY_SIZE(response));
+				response, &resp_len);
 
 	if (status < 0) {
 		return status;
@@ -496,20 +501,22 @@
 		return MBOX_CFGSTAT_STATE_ERROR_HARDWARE;
 	}
 
-	if ((res & SOFTFUNC_STATUS_CONF_DONE) != 0U &&
-		(res & SOFTFUNC_STATUS_INIT_DONE) != 0U) {
-		return MBOX_RET_OK;
-	}
+	if ((res & SOFTFUNC_STATUS_CONF_DONE) == 0U)
+		return MBOX_CFGSTAT_STATE_CONFIG;
 
-	return MBOX_CFGSTAT_STATE_CONFIG;
+	if (init_done && (res & SOFTFUNC_STATUS_INIT_DONE) == 0U)
+		return MBOX_CFGSTAT_STATE_CONFIG;
+
+	return MBOX_RET_OK;
 }
 
 int intel_mailbox_is_fpga_not_ready(void)
 {
-	int ret = intel_mailbox_get_config_status(MBOX_RECONFIG_STATUS);
+	int ret = intel_mailbox_get_config_status(MBOX_RECONFIG_STATUS, true);
 
 	if ((ret != MBOX_RET_OK) && (ret != MBOX_CFGSTAT_STATE_CONFIG)) {
-		ret = intel_mailbox_get_config_status(MBOX_CONFIG_STATUS);
+		ret = intel_mailbox_get_config_status(MBOX_CONFIG_STATUS,
+							false);
 	}
 
 	return ret;
diff --git a/plat/intel/soc/common/socfpga_sip_svc.c b/plat/intel/soc/common/socfpga_sip_svc.c
index a9e3ce3..d53e8de 100644
--- a/plat/intel/soc/common/socfpga_sip_svc.c
+++ b/plat/intel/soc/common/socfpga_sip_svc.c
@@ -83,9 +83,9 @@
 	uint32_t ret;
 
 	if (query_type == 1)
-		ret = intel_mailbox_get_config_status(MBOX_CONFIG_STATUS);
+		ret = intel_mailbox_get_config_status(MBOX_CONFIG_STATUS, false);
 	else
-		ret = intel_mailbox_get_config_status(MBOX_RECONFIG_STATUS);
+		ret = intel_mailbox_get_config_status(MBOX_RECONFIG_STATUS, true);
 
 	if (ret) {
 		if (ret == MBOX_CFGSTAT_STATE_CONFIG)
@@ -128,16 +128,16 @@
 static int intel_fpga_config_completed_write(uint32_t *completed_addr,
 					uint32_t *count, uint32_t *job_id)
 {
-	uint32_t status = INTEL_SIP_SMC_STATUS_OK;
-	*count = 0;
-	int resp_len = 0;
 	uint32_t resp[5];
+	unsigned int resp_len = ARRAY_SIZE(resp);
+	int status = INTEL_SIP_SMC_STATUS_OK;
 	int all_completed = 1;
+	*count = 0;
 
 	while (*count < 3) {
 
-		resp_len = mailbox_read_response(job_id,
-				resp, ARRAY_SIZE(resp));
+		status = mailbox_read_response(job_id,
+				resp, &resp_len);
 
 		if (resp_len < 0)
 			break;
@@ -183,17 +183,21 @@
 
 static int intel_fpga_config_start(uint32_t config_type)
 {
+	uint32_t argument = 0x1;
 	uint32_t response[3];
 	int status = 0;
+	unsigned int size = 0;
+	unsigned int resp_len = ARRAY_SIZE(response);
 
 	is_partial_reconfig = config_type;
 
 	mailbox_clear_response();
 
-	mailbox_send_cmd(1U, MBOX_CMD_CANCEL, NULL, 0U, CMD_CASUAL, NULL, 0U);
+	mailbox_send_cmd(MBOX_JOB_ID, MBOX_CMD_CANCEL, NULL, 0U,
+			CMD_CASUAL, NULL, NULL);
 
-	status = mailbox_send_cmd(1U, MBOX_RECONFIG, NULL, 0U, CMD_CASUAL,
-			response, ARRAY_SIZE(response));
+	status = mailbox_send_cmd(MBOX_JOB_ID, MBOX_RECONFIG, &argument, size,
+			CMD_CASUAL, response, &resp_len);
 
 	if (status < 0)
 		return status;
@@ -387,10 +391,11 @@
 }
 
 /* Mailbox services */
-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)
+static uint32_t intel_mbox_send_cmd(uint32_t cmd, uint32_t *args,
+				unsigned int len,
+				uint32_t urgent, uint32_t *response,
+				unsigned int resp_len, int *mbox_status,
+				unsigned int *len_in_resp)
 {
 	*len_in_resp = 0;
 	*mbox_status = 0;
@@ -399,7 +404,7 @@
 		return INTEL_SIP_SMC_STATUS_REJECTED;
 
 	int status = mailbox_send_cmd(MBOX_JOB_ID, cmd, args, len, urgent,
-				      response, resp_len);
+				      response, &resp_len);
 
 	if (status < 0) {
 		*mbox_status = -status;
@@ -407,7 +412,7 @@
 	}
 
 	*mbox_status = 0;
-	*len_in_resp = status;
+	*len_in_resp = resp_len;
 	return INTEL_SIP_SMC_STATUS_OK;
 }
 
@@ -428,9 +433,9 @@
 	uint32_t status = INTEL_SIP_SMC_STATUS_OK;
 	uint32_t completed_addr[3];
 	uint64_t rsu_respbuf[9];
+	int mbox_status;
+	unsigned int len_in_resp;
 	u_register_t x5, x6;
-	int mbox_status, len_in_resp;
-
 
 	switch (smc_fid) {
 	case SIP_SVC_UID: