Merge changes from topic "mailbox-fixes" into integration

* changes:
  intel: Fix SMC SIP service
  intel: Introduce mailbox response length handling
  intel: Fix mailbox config return status
  intel: Mailbox driver logic fixes
  plat: intel: Fix FPGA manager on reconfiguration
  plat: intel: Fix mailbox send_cmd issue
  intel: Modify mailbox's get_config_status

plat/intel/soc/common/include/socfpga_mailbox.h

@@ -11,6 +11,7 @@
 
 #define MBOX_OFFSET			0xffa30000
 
+#define MBOX_MAX_JOB_ID			0xf
 #define MBOX_ATF_CLIENT_ID		0x1
 #define MBOX_JOB_ID			0x1
 
@@ -66,6 +67,9 @@
 #define MBOX_CMD_GET_IDCODE		16
 #define MBOX_CMD_QSPI_SET_CS		52
 
+/* Mailbox CANCEL command */
+#define MBOX_CMD_CANCEL			0x3
+
 /* Mailbox REBOOT commands */
 #define MBOX_CMD_REBOOT_HPS		71
 
@@ -75,16 +79,27 @@
 #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		(U(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
+#define RECONFIG_STATUS_STATE				0
+#define RECONFIG_STATUS_PIN_STATUS			2
+#define RECONFIG_STATUS_SOFTFUNC_STATUS			3
+#define PIN_STATUS_NSTATUS				(U(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_IDLE				0x00000000
+#define MBOX_CFGSTAT_STATE_CONFIG			0x10000000
+#define MBOX_CFGSTAT_STATE_FAILACK			0x08000000
+#define MBOX_CFGSTAT_STATE_ERROR_INVALID		0xf0000001
+#define MBOX_CFGSTAT_STATE_ERROR_CORRUPT		0xf0000002
+#define MBOX_CFGSTAT_STATE_ERROR_AUTH			0xf0000003
+#define MBOX_CFGSTAT_STATE_ERROR_CORE_IO		0xf0000004
+#define MBOX_CFGSTAT_STATE_ERROR_HARDWARE		0xf0000005
+#define MBOX_CFGSTAT_STATE_ERROR_FAKE			0xf0000006
+#define MBOX_CFGSTAT_STATE_ERROR_BOOT_INFO		0xf0000007
+#define MBOX_CFGSTAT_STATE_ERROR_QSPI_ERROR		0xf0000008
 
 /* Mailbox reconfiguration commands */
+#define MBOX_CONFIG_STATUS	4
 #define MBOX_RECONFIG		6
 #define MBOX_RECONFIG_DATA	8
 #define MBOX_RECONFIG_STATUS	9
@@ -96,11 +111,14 @@ 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, 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 mailbox_read_response(int job_id, uint32_t *response, int resp_len);
 int mailbox_get_qspi_clock(void);
 void mailbox_reset_cold(void);
+void mailbox_clear_response(void);
+
+uint32_t intel_mailbox_get_config_status(uint32_t cmd);
 
 #endif /* SOCFPGA_MBOX_H */

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 */

plat/intel/soc/common/soc/socfpga_mailbox.c

@@ -6,8 +6,10 @@
 
 #include <lib/mmio.h>
 #include <common/debug.h>
+#include <drivers/delay_timer.h>
 
 #include "socfpga_mailbox.h"
+#include "socfpga_sip_svc.h"
 
 static int fill_mailbox_circular_buffer(uint32_t header_cmd, uint32_t *args,
 					int len)
@@ -17,12 +19,6 @@ static int fill_mailbox_circular_buffer(uint32_t header_cmd, uint32_t *args,
 
 	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);
 
@@ -39,28 +35,21 @@ static int fill_mailbox_circular_buffer(uint32_t header_cmd, uint32_t *args,
 	return 0;
 }
 
-int mailbox_read_response(int job_id, uint32_t *response)
+int mailbox_read_response(int job_id, uint32_t *response, int resp_len)
 {
 	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);
+	if (mmio_read_32(MBOX_OFFSET + MBOX_DOORBELL_FROM_SDM))
+		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) {
+	if (rout != rin) {
 		resp = mmio_read_32(MBOX_OFFSET +
 				    MBOX_RESP_BUFFER + ((rout++)*4));
 
@@ -84,8 +73,9 @@ int mailbox_read_response(int job_id, uint32_t *response)
 			resp = mmio_read_32(MBOX_OFFSET +
 						MBOX_RESP_BUFFER +
 						(rout)*4);
-			if (response) {
+			if (response && resp_len) {
 				*(response + total_resp_len) = resp;
+				resp_len--;
 				total_resp_len++;
 			}
 			rout++;
@@ -99,25 +89,25 @@ int mailbox_read_response(int job_id, uint32_t *response)
 }
 
 
-int mailbox_poll_response(int job_id, int urgent, uint32_t *response)
+int mailbox_poll_response(int job_id, int urgent, uint32_t *response,
+				int resp_len)
 {
-	int timeout = 80000;
+	int timeout = 0xFFFFFF;
 	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) {
+			!(mmio_read_32(MBOX_OFFSET +
+				MBOX_DOORBELL_FROM_SDM) & 1)) {
 			timeout--;
 		}
 
-		if (mmio_read_32(MBOX_OFFSET + MBOX_DOORBELL_FROM_SDM) != 1) {
+		if (!timeout) {
 			INFO("Timed out waiting for SDM");
 			return MBOX_TIMEOUT;
 		}
@@ -125,6 +115,7 @@ int mailbox_poll_response(int job_id, int urgent, uint32_t *response)
 		mmio_write_32(MBOX_OFFSET + MBOX_DOORBELL_FROM_SDM, 0);
 
 		if (urgent & 1) {
+			mdelay(5);
 			if ((mmio_read_32(MBOX_OFFSET + MBOX_STATUS) &
 				MBOX_STATUS_UA_MASK) ^
 				(urgent & MBOX_STATUS_UA_MASK)) {
@@ -158,13 +149,13 @@ int mailbox_poll_response(int job_id, int urgent, uint32_t *response)
 			response_length = MBOX_RESP_LEN(resp);
 
 			while (response_length) {
-
 				response_length--;
 				resp = mmio_read_32(MBOX_OFFSET +
 							MBOX_RESP_BUFFER +
 							(rout)*4);
-				if (response) {
+				if (response && resp_len) {
 					*(response + total_resp_len) = resp;
+					resp_len--;
 					total_resp_len++;
 				}
 				rout++;
@@ -176,7 +167,7 @@ int mailbox_poll_response(int job_id, int urgent, uint32_t *response)
 	}
 }
 
-void mailbox_send_cmd_async(int job_id, unsigned int cmd, uint32_t *args,
+int mailbox_send_cmd_async(int job_id, unsigned int cmd, uint32_t *args,
 			  int len, int urgent)
 {
 	if (urgent)
@@ -187,29 +178,44 @@ void mailbox_send_cmd_async(int job_id, unsigned int cmd, uint32_t *args,
 					MBOX_CMD_LEN_CMD(len) |
 					MBOX_INDIRECT |
 					cmd, args, len);
+
+	mmio_write_32(MBOX_OFFSET + MBOX_DOORBELL_TO_SDM, 1);
+
+	return 0;
 }
 
 int mailbox_send_cmd(int job_id, unsigned int cmd, uint32_t *args,
-			  int len, int urgent, uint32_t *response)
+			int len, int urgent, uint32_t *response, int resp_len)
 {
-	int status;
+	int status = 0;
 
 	if (urgent) {
 		urgent |= mmio_read_32(MBOX_OFFSET + MBOX_STATUS) &
 					MBOX_STATUS_UA_MASK;
 		mmio_write_32(MBOX_OFFSET + MBOX_URG, cmd);
-		status = 0;
-	} else {
+	}
+
+	else {
 		status = fill_mailbox_circular_buffer(
 			MBOX_CLIENT_ID_CMD(MBOX_ATF_CLIENT_ID) |
 			MBOX_JOB_ID_CMD(job_id) |
+			MBOX_CMD_LEN_CMD(len) |
 			cmd, args, len);
 	}
 
 	if (status)
 		return status;
 
-	return mailbox_poll_response(job_id, urgent, response);
+	mmio_write_32(MBOX_OFFSET + MBOX_DOORBELL_TO_SDM, 1);
+	status = mailbox_poll_response(job_id, urgent, response, resp_len);
+
+	return status;
+}
+
+void mailbox_clear_response(void)
+{
+	mmio_write_32(MBOX_OFFSET + MBOX_ROUT,
+		mmio_read_32(MBOX_OFFSET + MBOX_RIN));
 }
 
 void mailbox_set_int(int interrupt)
@@ -223,24 +229,25 @@ void mailbox_set_int(int 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);
+	mailbox_send_cmd(MBOX_JOB_ID, MBOX_CMD_QSPI_OPEN, 0, 0, 0, NULL, 0);
 }
 
 void mailbox_set_qspi_direct(void)
 {
-	mailbox_send_cmd(MBOX_JOB_ID, MBOX_CMD_QSPI_DIRECT, 0, 0, 0, 0);
+	mailbox_send_cmd(MBOX_JOB_ID, MBOX_CMD_QSPI_DIRECT, 0, 0, 0, NULL, 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);
+	mailbox_send_cmd(MBOX_JOB_ID, MBOX_CMD_QSPI_CLOSE, 0, 0, 0, NULL, 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);
+	return mailbox_send_cmd(MBOX_JOB_ID, MBOX_CMD_QSPI_DIRECT, 0, 0, 0,
+				NULL, 0);
 }
 
 void mailbox_qspi_set_cs(int device_select)
@@ -251,13 +258,13 @@ void mailbox_qspi_set_cs(int device_select)
 	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);
+				1, 0, NULL, 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);
+	mailbox_send_cmd(MBOX_JOB_ID, MBOX_CMD_REBOOT_HPS, 0, 0, 0, NULL, 0);
 }
 
 int mailbox_init(void)
@@ -268,13 +275,44 @@ int mailbox_init(void)
 			MBOX_INT_FLAG_UAE);
 	mmio_write_32(MBOX_OFFSET + MBOX_URG, 0);
 	mmio_write_32(MBOX_OFFSET + MBOX_DOORBELL_FROM_SDM, 0);
-	status = mailbox_send_cmd(0, MBOX_CMD_RESTART, 0, 0, 1, 0);
+
+	status = mailbox_send_cmd(0, MBOX_CMD_RESTART, 0, 0, 1, NULL, 0);
 
 	if (status)
 		return status;
 
-	mailbox_set_int(MBOX_INT_FLAG_COE | MBOX_INT_FLAG_RIE);
+	mailbox_set_int(MBOX_INT_FLAG_COE | MBOX_INT_FLAG_RIE |
+			MBOX_INT_FLAG_UAE);
 
 	return 0;
 }
 
+uint32_t intel_mailbox_get_config_status(uint32_t cmd)
+{
+	uint32_t status, res;
+	uint32_t response[6];
+
+	status = mailbox_send_cmd(1, cmd, NULL, 0, 0, response,
+		sizeof(response) / sizeof(response[0]));
+
+	if (status < 0)
+		return status;
+
+	res = response[RECONFIG_STATUS_STATE];
+	if (res && res != MBOX_CFGSTAT_STATE_CONFIG)
+		return res;
+
+	res = response[RECONFIG_STATUS_PIN_STATUS];
+	if (!(res & PIN_STATUS_NSTATUS))
+		return MBOX_CFGSTAT_STATE_ERROR_HARDWARE;
+
+	res = response[RECONFIG_STATUS_SOFTFUNC_STATUS];
+	if (res & SOFTFUNC_STATUS_SEU_ERROR)
+		return MBOX_CFGSTAT_STATE_ERROR_HARDWARE;
+
+	if ((res & SOFTFUNC_STATUS_CONF_DONE) &&
+		(res & SOFTFUNC_STATUS_INIT_DONE))
+		return 0;
+
+	return MBOX_CFGSTAT_STATE_CONFIG;
+}

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;
@@ -55,79 +56,54 @@ uint64_t socfpga_sip_handler(uint32_t smc_fid,
 
 struct fpga_config_info fpga_config_buffers[FPGA_CONFIG_BUFFER_SIZE];
 
-static void intel_fpga_sdm_write_buffer(struct fpga_config_info *buffer)
+static int 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[0] = (1<<8);
+		args[1] = buffer->addr + buffer->size_written;
+		if (buffer->size - buffer->size_written <= bytes_per_block) {
 			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;
+		} else
 			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++;
-		}
+
+		buffer->size_written += args[2];
+		mailbox_send_cmd_async(
+			send_id++ % MBOX_MAX_JOB_ID,
+			MBOX_RECONFIG_DATA,
+			args, 3, 0);
+
+		buffer->subblocks_sent++;
 		max_blocks--;
 	}
+
+	return !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]);
-
+	for (int i = 0; i < FPGA_CONFIG_BUFFER_SIZE; i++)
+		if (intel_fpga_sdm_write_buffer(
+			&fpga_config_buffers[current_buffer]))
+			break;
 	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;
+	uint32_t ret = intel_mailbox_get_config_status(MBOX_RECONFIG_STATUS);
 
-	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;
+	if (ret) {
+		if (ret == MBOX_CFGSTAT_STATE_CONFIG)
+			return INTEL_SIP_SMC_STATUS_BUSY;
+		else
+			return INTEL_SIP_SMC_STATUS_ERROR;
+	}
 
-	return INTEL_SIP_SMC_STATUS_ERROR;
+	return INTEL_SIP_SMC_STATUS_OK;
 }
 
 static int mark_last_buffer_xfer_completed(uint32_t *buffer_addr_completed)
@@ -152,15 +128,6 @@ static int mark_last_buffer_xfer_completed(uint32_t *buffer_addr_completed)
 	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)
 {
@@ -169,30 +136,29 @@ int intel_fpga_config_completed_write(uint32_t *completed_addr,
 	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, sizeof(resp) / sizeof(resp[0]));
 
-	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;
 		}
 
@@ -224,8 +190,12 @@ 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);
+	mailbox_clear_response();
+
+	mailbox_send_cmd(1, MBOX_CMD_CANCEL, 0, 0, 0, NULL, 0);
+
+	status = mailbox_send_cmd(1, MBOX_RECONFIG, 0, 0, 0,
+			response, sizeof(response) / sizeof(response[0]));
 
 	if (status < 0)
 		return status;
@@ -244,47 +214,60 @@ int intel_fpga_config_start(uint32_t config_type)
 
 	blocks_submitted = 0;
 	current_block = 0;
+	read_block = 0;
 	current_buffer = 0;
+	send_id = 0;
+	rcv_id = 0;
 
 	return 0;
 }
 
+static bool is_fpga_config_buffer_full(void)
+{
+	for (int i = 0; i < FPGA_CONFIG_BUFFER_SIZE; i++)
+		if (!fpga_config_buffers[i].write_requested)
+			return false;
+	return true;
+}
+
+static bool is_address_in_ddr_range(uint64_t addr)
+{
+	if (addr >= DRAM_BASE && addr <= DRAM_BASE + DRAM_SIZE)
+		return true;
+
+	return false;
+}
 
 uint32_t intel_fpga_config_write(uint64_t mem, uint64_t size)
 {
-	int i = 0;
-	uint32_t status = INTEL_SIP_SMC_STATUS_OK;
+	int i;
 
-	if (mem < DRAM_BASE || mem > DRAM_BASE + DRAM_SIZE)
-		status = INTEL_SIP_SMC_STATUS_REJECTED;
+	intel_fpga_sdm_write_all();
 
-	if (mem + size > DRAM_BASE + DRAM_SIZE)
-		status = INTEL_SIP_SMC_STATUS_REJECTED;
+	if (!is_address_in_ddr_range(mem) ||
+		!is_address_in_ddr_range(mem + size) ||
+		is_fpga_config_buffer_full())
+		return 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 =
+		int j = (i + current_buffer) % FPGA_CONFIG_BUFFER_SIZE;
+
+		if (!fpga_config_buffers[j].write_requested) {
+			fpga_config_buffers[j].addr = mem;
+			fpga_config_buffers[j].size = size;
+			fpga_config_buffers[j].size_written = 0;
+			fpga_config_buffers[j].write_requested = 1;
+			fpga_config_buffers[j].block_number =
 				blocks_submitted++;
-			fpga_config_buffers[i].subblocks_sent = 0;
+			fpga_config_buffers[j].subblocks_sent = 0;
 			break;
 		}
 	}
 
+	if (is_fpga_config_buffer_full())
+		return INTEL_SIP_SMC_STATUS_BUSY;
 
-	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;
+	return INTEL_SIP_SMC_STATUS_OK;
 }
 
 /*
@@ -350,6 +333,7 @@ uintptr_t sip_smc_handler(uint32_t smc_fid,
 			SMC_RET4(handle, status, 0, 0, 0);
 			break;
 		default:
+			mailbox_clear_response();
 			SMC_RET1(handle, INTEL_SIP_SMC_STATUS_ERROR);
 		}
 		break;