diff --git a/plat/intel/soc/common/include/socfpga_mailbox.h b/plat/intel/soc/common/include/socfpga_mailbox.h
index 710ecf0..75323fd 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 */
@@ -81,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 */
@@ -138,11 +140,11 @@
 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 mailbox_send_cmd(uint32_t 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 mailbox_send_cmd_async(uint32_t *job_id, unsigned int cmd, uint32_t *args,
 				int len, int indirect);
-int mailbox_read_response(int job_id, uint32_t *response, int resp_len);
+int mailbox_read_response(uint32_t *job_id, uint32_t *response, 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 329f511..92adfa3 100644
--- a/plat/intel/soc/common/include/socfpga_sip_svc.h
+++ b/plat/intel/soc/common/include/socfpga_sip_svc.h
@@ -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 4f02b69..5935777 100644
--- a/plat/intel/soc/common/soc/socfpga_mailbox.c
+++ b/plat/intel/soc/common/soc/socfpga_mailbox.c
@@ -14,10 +14,16 @@
 static int fill_mailbox_circular_buffer(uint32_t header_cmd, uint32_t *args,
 					int len)
 {
-	uint32_t cmd_free_offset;
+	uint32_t sdm_read_offset, cmd_free_offset;
 	int i;
 
 	cmd_free_offset = mmio_read_32(MBOX_OFFSET + MBOX_CIN);
+	sdm_read_offset = mmio_read_32(MBOX_OFFSET + MBOX_COUT);
+
+	if ((cmd_free_offset < sdm_read_offset) &&
+		(cmd_free_offset + len > sdm_read_offset)) {
+		return MBOX_BUFFER_FULL;
+	}
 
 	mmio_write_32(MBOX_OFFSET + MBOX_CMD_BUFFER + (cmd_free_offset++ * 4),
 			header_cmd);
@@ -35,7 +41,7 @@
 	return MBOX_RET_OK;
 }
 
-int mailbox_read_response(int job_id, uint32_t *response, int resp_len)
+int mailbox_read_response(uint32_t *job_id, uint32_t *response, int resp_len)
 {
 	int rin = 0;
 	int rout = 0;
@@ -57,11 +63,13 @@
 		rout %= MBOX_RESP_BUFFER_SIZE;
 		mmio_write_32(MBOX_OFFSET + MBOX_ROUT, rout);
 
-		if (MBOX_RESP_CLIENT_ID(resp_data) != MBOX_ATF_CLIENT_ID ||
-		   MBOX_RESP_JOB_ID(resp_data) != job_id) {
+
+		if (MBOX_RESP_CLIENT_ID(resp_data) != MBOX_ATF_CLIENT_ID) {
 			return MBOX_WRONG_ID;
 		}
 
+		*job_id = MBOX_RESP_JOB_ID(resp_data);
+
 		if (MBOX_RESP_ERR(resp_data) > 0) {
 			INFO("Error in response: %x\n", resp_data);
 			return -resp_data;
@@ -90,7 +98,7 @@
 }
 
 
-int mailbox_poll_response(int job_id, int urgent, uint32_t *response,
+int mailbox_poll_response(uint32_t job_id, int urgent, uint32_t *response,
 				int resp_len)
 {
 	int timeout = 0xFFFFFF;
@@ -170,21 +178,28 @@
 	}
 }
 
-int mailbox_send_cmd_async(int job_id, unsigned int cmd, uint32_t *args,
+int mailbox_send_cmd_async(uint32_t *job_id, unsigned int cmd, uint32_t *args,
 			  int len, int indirect)
 {
-	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);
+	int status;
+
+	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 + 1) % MBOX_MAX_IND_JOB_ID;
 
 	return MBOX_RET_OK;
 }
 
-int mailbox_send_cmd(int job_id, unsigned int cmd, uint32_t *args,
+int mailbox_send_cmd(uint32_t job_id, unsigned int cmd, uint32_t *args,
 			int len, int urgent, uint32_t *response, int resp_len)
 {
 	int status = 0;
diff --git a/plat/intel/soc/common/socfpga_sip_svc.c b/plat/intel/soc/common/socfpga_sip_svc.c
index 8588e93..d5789f2 100644
--- a/plat/intel/soc/common/socfpga_sip_svc.c
+++ b/plat/intel/soc/common/socfpga_sip_svc.c
@@ -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,9 +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,
-					CMD_INDIRECT);
+		mailbox_send_cmd_async(&send_id, MBOX_RECONFIG_DATA, args,
+					3, CMD_INDIRECT);
 
 		buffer->subblocks_sent++;
 		max_blocks--;
@@ -142,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;
@@ -152,14 +136,13 @@
 
 	while (*count < 3) {
 
-		resp_len = mailbox_read_response(rcv_id % MBOX_MAX_JOB_ID,
+		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)
@@ -231,8 +214,6 @@
 	current_block = 0;
 	read_block = 0;
 	current_buffer = 0;
-	send_id = 0;
-	rcv_id = 0;
 
 	/* full reconfiguration */
 	if (!is_partial_reconfig) {
@@ -251,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;
@@ -440,11 +421,10 @@
 			 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;
 
@@ -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: