diff --git a/plat/intel/soc/common/include/socfpga_sip_svc.h b/plat/intel/soc/common/include/socfpga_sip_svc.h
index 33e7e1b..6bb41f3 100644
--- a/plat/intel/soc/common/include/socfpga_sip_svc.h
+++ b/plat/intel/soc/common/include/socfpga_sip_svc.h
@@ -30,7 +30,7 @@
 #define INTEL_SIP_SMC_RSU_RETRY_COUNTER			0xC200000F
 
 /* FPGA config helpers */
-#define INTEL_SIP_SMC_FPGA_CONFIG_ADDR			0x1000
+#define INTEL_SIP_SMC_FPGA_CONFIG_ADDR			0x400000
 #define INTEL_SIP_SMC_FPGA_CONFIG_SIZE			16777216
 
 /* SMC function IDs for SiP Service queries */
diff --git a/plat/intel/soc/common/socfpga_sip_svc.c b/plat/intel/soc/common/socfpga_sip_svc.c
index 421fa44..4a09526 100644
--- a/plat/intel/soc/common/socfpga_sip_svc.c
+++ b/plat/intel/soc/common/socfpga_sip_svc.c
@@ -18,13 +18,14 @@
 /* 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;
+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;
 
 struct fpga_config_info {
 	uint32_t addr;
@@ -68,7 +69,8 @@
 			buffer->size_written +=
 				buffer->size - buffer->size_written;
 			buffer->subblocks_sent++;
-			mailbox_send_cmd_async(0x4,
+			mailbox_send_cmd_async(
+				send_id++ % MBOX_MAX_JOB_ID,
 				MBOX_RECONFIG_DATA,
 				args, 3, 0);
 			current_buffer++;
@@ -78,7 +80,8 @@
 			args[1] = buffer->addr + buffer->size_written;
 			args[2] = bytes_per_block;
 			buffer->size_written += bytes_per_block;
-			mailbox_send_cmd_async(0x4,
+			mailbox_send_cmd_async(
+				send_id++ % MBOX_MAX_JOB_ID,
 				MBOX_RECONFIG_DATA,
 				args, 3, 0);
 			buffer->subblocks_sent++;
@@ -134,6 +137,8 @@
 	return -1;
 }
 
+int mailbox_poll_response(int job_id, int urgent, uint32_t *response);
+
 int intel_fpga_config_completed_write(uint32_t *completed_addr,
 					uint32_t *count)
 {
@@ -142,30 +147,29 @@
 	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;
+	while (*count < 3) {
 
-	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(
+			rcv_id % MBOX_MAX_JOB_ID, resp);
 
-	resp_len = mailbox_read_response(0x4, resp);
+		if (resp_len < 0)
+			break;
 
-	while (resp_len >= 0 && *count < 3) {
 		max_blocks++;
+		rcv_id++;
+
 		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) {
+			mailbox_clear_response();
 			return INTEL_SIP_SMC_STATUS_ERROR;
 		}
 
@@ -197,7 +201,11 @@
 	uint32_t response[3];
 	int status = 0;
 
+	mailbox_clear_response();
+
-	status = mailbox_send_cmd(2, MBOX_RECONFIG, 0, 0, 0,
+	mailbox_send_cmd(1, MBOX_CMD_CANCEL, 0, 0, 0, response);
+
+	status = mailbox_send_cmd(1, MBOX_RECONFIG, 0, 0, 0,
 			response);
 
 	if (status < 0)
@@ -217,7 +225,10 @@
 
 	blocks_submitted = 0;
 	current_block = 0;
+	read_block = 0;
 	current_buffer = 0;
+	send_id = 0;
+	rcv_id = 0;
 
 	return 0;
 }
@@ -323,6 +334,7 @@
 			SMC_RET4(handle, status, 0, 0, 0);
 			break;
 		default:
+			mailbox_clear_response();
 			SMC_RET1(handle, INTEL_SIP_SMC_STATUS_ERROR);
 		}
 		break;