1 files changed, 120 insertions, 52 deletions

diff --git a/arch/arm/mach-socfpga/mailbox_s10.c b/arch/arm/mach-socfpga/mailbox_s10.c
index f30e7f80a2..18d44924e6 100644
--- a/arch/arm/mach-socfpga/mailbox_s10.c
+++ b/arch/arm/mach-socfpga/mailbox_s10.c
@@ -29,54 +29,107 @@ DECLARE_GLOBAL_DATA_PTR;
 static __always_inline int mbox_polling_resp(u32 rout)
 {
 	u32 rin;
-	unsigned long i = ~0;
+	unsigned long i = 2000;
 
 	while (i) {
 		rin = MBOX_READL(MBOX_RIN);
 		if (rout != rin)
 			return 0;
 
+		udelay(1000);
 		i--;
 	}
 
 	return -ETIMEDOUT;
 }
 
+static __always_inline int mbox_is_cmdbuf_full(u32 cin)
+{
+	return (((cin + 1) % MBOX_CMD_BUFFER_SIZE) == MBOX_READL(MBOX_COUT));
+}
+
+static __always_inline int mbox_is_cmdbuf_empty(u32 cin)
+{
+	return (((MBOX_READL(MBOX_COUT) + 1) % MBOX_CMD_BUFFER_SIZE) == cin);
+}
+
+static __always_inline int mbox_wait_for_cmdbuf_empty(u32 cin)
+{
+	int timeout = 2000;
+
+	while (timeout) {
+		if (mbox_is_cmdbuf_empty(cin))
+			return 0;
+		udelay(1000);
+		timeout--;
+	}
+
+	return -ETIMEDOUT;
+}
+
+static __always_inline int mbox_write_cmd_buffer(u32 *cin, u32 data,
+						 int *is_cmdbuf_overflow)
+{
+	int timeout = 1000;
+
+	while (timeout) {
+		if (mbox_is_cmdbuf_full(*cin)) {
+			if (is_cmdbuf_overflow &&
+			    *is_cmdbuf_overflow == 0) {
+				/* Trigger SDM doorbell */
+				MBOX_WRITEL(1, MBOX_DOORBELL_TO_SDM);
+				*is_cmdbuf_overflow = 1;
+			}
+			udelay(1000);
+		} else {
+			/* write header to circular buffer */
+			MBOX_WRITE_CMD_BUF(data, (*cin)++);
+			*cin %= MBOX_CMD_BUFFER_SIZE;
+			MBOX_WRITEL(*cin, MBOX_CIN);
+			break;
+		}
+		timeout--;
+	}
+
+	if (!timeout)
+		return -ETIMEDOUT;
+
+	/* Wait for the SDM to drain the FIFO command buffer */
+	if (is_cmdbuf_overflow && *is_cmdbuf_overflow)
+		return mbox_wait_for_cmdbuf_empty(*cin);
+
+	return 0;
+}
+
 /* Check for available slot and write to circular buffer.
  * It also update command valid offset (cin) register.
  */
 static __always_inline int mbox_fill_cmd_circular_buff(u32 header, u32 len,
 						       u32 *arg)
 {
-	u32 cin;
-	u32 cout;
-	u32 i;
+	int i, ret;
+	int is_cmdbuf_overflow = 0;
+	u32 cin = MBOX_READL(MBOX_CIN) % MBOX_CMD_BUFFER_SIZE;
 
-	cin = MBOX_READL(MBOX_CIN) % MBOX_CMD_BUFFER_SIZE;
-	cout = MBOX_READL(MBOX_COUT) % MBOX_CMD_BUFFER_SIZE;
-
-	/* if command buffer is full or not enough free space
-	 * to fit the data. Note, len is in u32 unit.
-	 */
-	if (((cin + 1) % MBOX_CMD_BUFFER_SIZE) == cout ||
-	    ((MBOX_CMD_BUFFER_SIZE - cin + cout - 1) %
-	     MBOX_CMD_BUFFER_SIZE) < (len + 1))
-		return -ENOMEM;
-
-	/* write header to circular buffer */
-	MBOX_WRITE_CMD_BUF(header, cin++);
-	/* wrapping around when it reach the buffer size */
-	cin %= MBOX_CMD_BUFFER_SIZE;
+	ret = mbox_write_cmd_buffer(&cin, header, &is_cmdbuf_overflow);
+	if (ret)
+		return ret;
 
 	/* write arguments */
 	for (i = 0; i < len; i++) {
-		MBOX_WRITE_CMD_BUF(arg[i], cin++);
-		/* wrapping around when it reach the buffer size */
-		cin %= MBOX_CMD_BUFFER_SIZE;
+		is_cmdbuf_overflow = 0;
+		ret = mbox_write_cmd_buffer(&cin, arg[i], &is_cmdbuf_overflow);
+		if (ret)
+			return ret;
 	}
 
-	/* write command valid offset */
-	MBOX_WRITEL(cin, MBOX_CIN);
+	/* If SDM doorbell is not triggered after the last data is
+	 * written into mailbox FIFO command buffer, trigger the
+	 * SDM doorbell again to ensure SDM able to read the remaining
+	 * data.
+	 */
+	if (!is_cmdbuf_overflow)
+		MBOX_WRITEL(1, MBOX_DOORBELL_TO_SDM);
 
 	return 0;
 }
@@ -89,10 +142,6 @@ static __always_inline int mbox_prepare_cmd_only(u8 id, u32 cmd,
 	u32 header;
 	int ret;
 
-	/* Total length is command + argument length */
-	if ((len + 1) > MBOX_CMD_BUFFER_SIZE)
-		return -EINVAL;
-
 	if (cmd > MBOX_MAX_CMD_INDEX)
 		return -EINVAL;
 
@@ -109,11 +158,7 @@ static __always_inline int mbox_send_cmd_only_common(u8 id, u32 cmd,
 						     u8 is_indirect, u32 len,
 						     u32 *arg)
 {
-	int ret = mbox_prepare_cmd_only(id, cmd, is_indirect, len, arg);
-	/* write doorbell */
-	MBOX_WRITEL(1, MBOX_DOORBELL_TO_SDM);
-
-	return ret;
+	return mbox_prepare_cmd_only(id, cmd, is_indirect, len, arg);
 }
 
 /* Return number of responses received in buffer */
@@ -166,21 +211,24 @@ static __always_inline int mbox_send_cmd_common(u8 id, u32 cmd, u8 is_indirect,
 		status = MBOX_READL(MBOX_STATUS) & MBOX_STATUS_UA_MSK;
 		/* Write urgent command to urgent register */
 		MBOX_WRITEL(cmd, MBOX_URG);
+		/* write doorbell */
+		MBOX_WRITEL(1, MBOX_DOORBELL_TO_SDM);
 	} else {
 		ret = mbox_prepare_cmd_only(id, cmd, is_indirect, len, arg);
 		if (ret)
 			return ret;
 	}
 
-	/* write doorbell */
-	MBOX_WRITEL(1, MBOX_DOORBELL_TO_SDM);
-
 	while (1) {
-		ret = ~0;
+		ret = 1000;
 
 		/* Wait for doorbell from SDM */
-		while (!MBOX_READL(MBOX_DOORBELL_FROM_SDM) && ret--)
-			;
+		do {
+			if (MBOX_READL(MBOX_DOORBELL_FROM_SDM))
+				break;
+			udelay(1000);
+		} while (--ret);
+
 		if (!ret)
 			return -ETIMEDOUT;
 
@@ -216,9 +264,7 @@ static __always_inline int mbox_send_cmd_common(u8 id, u32 cmd, u8 is_indirect,
 			if ((MBOX_RESP_CLIENT_GET(resp) ==
 			     MBOX_CLIENT_ID_UBOOT) &&
 			    (MBOX_RESP_ID_GET(resp) == id)) {
-				ret = MBOX_RESP_ERR_GET(resp);
-				if (ret)
-					return ret;
+				int resp_err = MBOX_RESP_ERR_GET(resp);
 
 				if (resp_buf_len) {
 					buf_len = *resp_buf_len;
@@ -247,14 +293,36 @@ static __always_inline int mbox_send_cmd_common(u8 id, u32 cmd, u8 is_indirect,
 						buf_len--;
 					}
 				}
-				return ret;
+				return resp_err;
 			}
 		}
-	};
+	}
 
 	return -EIO;
 }
 
+static __always_inline int mbox_send_cmd_common_retry(u8 id, u32 cmd,
+						      u8 is_indirect,
+						      u32 len, u32 *arg,
+						      u8 urgent,
+						      u32 *resp_buf_len,
+						      u32 *resp_buf)
+{
+	int ret;
+	int i;
+
+	for (i = 0; i < 3; i++) {
+		ret = mbox_send_cmd_common(id, cmd, is_indirect, len, arg,
+					   urgent, resp_buf_len, resp_buf);
+		if (ret == MBOX_RESP_TIMEOUT || ret == MBOX_RESP_DEVICE_BUSY)
+			udelay(2000); /* wait for 2ms before resend */
+		else
+			break;
+	}
+
+	return ret;
+}
+
 int mbox_init(void)
 {
 	int ret;
@@ -349,10 +417,10 @@ static __always_inline int mbox_get_fpga_config_status_common(u32 cmd)
 	int ret;
 
 	reconfig_status_resp_len = RECONFIG_STATUS_RESPONSE_LEN;
-	ret = mbox_send_cmd_common(MBOX_ID_UBOOT, cmd,
-				   MBOX_CMD_DIRECT, 0, NULL, 0,
-				   &reconfig_status_resp_len,
-				   reconfig_status_resp);
+	ret = mbox_send_cmd_common_retry(MBOX_ID_UBOOT, cmd,
+					 MBOX_CMD_DIRECT, 0, NULL, 0,
+					 &reconfig_status_resp_len,
+					 reconfig_status_resp);
 
 	if (ret)
 		return ret;
@@ -392,16 +460,16 @@ int __secure mbox_get_fpga_config_status_psci(u32 cmd)
 int mbox_send_cmd(u8 id, u32 cmd, u8 is_indirect, u32 len, u32 *arg,
 		  u8 urgent, u32 *resp_buf_len, u32 *resp_buf)
 {
-	return mbox_send_cmd_common(id, cmd, is_indirect, len, arg, urgent,
-			       resp_buf_len, resp_buf);
+	return mbox_send_cmd_common_retry(id, cmd, is_indirect, len, arg,
+					  urgent, resp_buf_len, resp_buf);
 }
 
 int __secure mbox_send_cmd_psci(u8 id, u32 cmd, u8 is_indirect, u32 len,
 				u32 *arg, u8 urgent, u32 *resp_buf_len,
 				u32 *resp_buf)
 {
-	return mbox_send_cmd_common(id, cmd, is_indirect, len, arg, urgent,
-			       resp_buf_len, resp_buf);
+	return mbox_send_cmd_common_retry(id, cmd, is_indirect, len, arg,
+					  urgent, resp_buf_len, resp_buf);
 }
 
 int mbox_send_cmd_only(u8 id, u32 cmd, u8 is_indirect, u32 len, u32 *arg)