trusty: Retry std_calls on SM_ERR_BUSY

If the trusty spinlock is held, or if the strex fails for another
reason, trusty returns SM_ERR_BUSY. Add retry code to handle this.

Without this retry code, std_calls can fail. If the previous smc
call had returned SM_ERR_INTERRUPTED, this failure would cause
the driver to get out of sync with trusty. All later calls would
then fail with SM_ERR_INTERLEAVED_SMC.

Change-Id: I9f72bb7ce9af9e1ef0a38aeff62fb83ea51377cf
Signed-off-by: Arve Hjønnevåg <arve@android.com>
(cherry picked from commit 84925a939f207ccbc0a2536a20f49de5040bb907)

1 files changed, 64 insertions, 14 deletions

diff --git a/drivers/trusty/trusty.c b/drivers/trusty/trusty.c
index c3c9c8fc1b4d..209e7c209e07 100644
--- a/drivers/trusty/trusty.c
+++ b/drivers/trusty/trusty.c
@@ -13,6 +13,7 @@
  */
 
 #include <asm/compiler.h>
+#include <linux/delay.h>
 #include <linux/of.h>
 #include <linux/of_platform.h>
 #include <linux/platform_device.h>
@@ -93,6 +94,67 @@ s64 trusty_fast_call64(struct device *dev, u64 smcnr, u64 a0, u64 a1, u64 a2)
 }
 #endif
 
+static ulong trusty_std_call_inner(struct device *dev, ulong smcnr,
+				   ulong a0, ulong a1, ulong a2)
+{
+	ulong ret;
+	int retry = 5;
+
+	dev_dbg(dev, "%s(0x%lx 0x%lx 0x%lx 0x%lx)\n",
+		__func__, smcnr, a0, a1, a2);
+	while (true) {
+		ret = smc(smcnr, a0, a1, a2);
+		if ((int)ret != SM_ERR_BUSY || !retry)
+			break;
+
+		dev_dbg(dev, "%s(0x%lx 0x%lx 0x%lx 0x%lx) returned busy, retry\n",
+			__func__, smcnr, a0, a1, a2);
+		retry--;
+	}
+
+	return ret;
+}
+
+static ulong trusty_std_call_helper(struct device *dev, ulong smcnr,
+				    ulong a0, ulong a1, ulong a2)
+{
+	ulong ret;
+	int sleep_time = 1;
+	struct trusty_state *s = platform_get_drvdata(to_platform_device(dev));
+
+	while (true) {
+		local_irq_disable();
+		atomic_notifier_call_chain(&s->notifier, TRUSTY_CALL_PREPARE,
+					   NULL);
+		ret = trusty_std_call_inner(dev, smcnr, a0, a1, a2);
+		atomic_notifier_call_chain(&s->notifier, TRUSTY_CALL_RETURNED,
+					   NULL);
+		local_irq_enable();
+
+		if ((int)ret != SM_ERR_BUSY)
+			break;
+
+		if (sleep_time == 256)
+			dev_warn(dev, "%s(0x%lx 0x%lx 0x%lx 0x%lx) returned busy\n",
+				 __func__, smcnr, a0, a1, a2);
+		dev_dbg(dev, "%s(0x%lx 0x%lx 0x%lx 0x%lx) returned busy, wait %d ms\n",
+			__func__, smcnr, a0, a1, a2, sleep_time);
+
+		msleep(sleep_time);
+		if (sleep_time < 1000)
+			sleep_time <<= 1;
+
+		dev_dbg(dev, "%s(0x%lx 0x%lx 0x%lx 0x%lx) retry\n",
+			__func__, smcnr, a0, a1, a2);
+	}
+
+	if (sleep_time > 256)
+		dev_warn(dev, "%s(0x%lx 0x%lx 0x%lx 0x%lx) busy cleared\n",
+			 __func__, smcnr, a0, a1, a2);
+
+	return ret;
+}
+
 s32 trusty_std_call32(struct device *dev, u32 smcnr, u32 a0, u32 a1, u32 a2)
 {
 	int ret;
@@ -103,30 +165,18 @@ s32 trusty_std_call32(struct device *dev, u32 smcnr, u32 a0, u32 a1, u32 a2)
 
 	mutex_lock(&s->smc_lock);
 
-	local_irq_disable();
 	dev_dbg(dev, "%s(0x%x 0x%x 0x%x 0x%x) started\n",
 		__func__, smcnr, a0, a1, a2);
 
-	atomic_notifier_call_chain(&s->notifier, TRUSTY_CALL_PREPARE, NULL);
-	ret = smc(smcnr, a0, a1, a2);
-	atomic_notifier_call_chain(&s->notifier, TRUSTY_CALL_RETURNED, NULL);
-
+	ret = trusty_std_call_helper(dev, smcnr, a0, a1, a2);
 	while (ret == SM_ERR_INTERRUPTED) {
 		dev_dbg(dev, "%s(0x%x 0x%x 0x%x 0x%x) interrupted\n",
 			__func__, smcnr, a0, a1, a2);
-		local_irq_enable();
-		local_irq_disable();
-		atomic_notifier_call_chain(&s->notifier, TRUSTY_CALL_PREPARE,
-					   NULL);
-		ret = smc(SMC_SC_RESTART_LAST, 0, 0, 0);
-		atomic_notifier_call_chain(&s->notifier, TRUSTY_CALL_RETURNED,
-					   NULL);
+		ret = trusty_std_call_helper(dev, SMC_SC_RESTART_LAST, 0, 0, 0);
 	}
 	dev_dbg(dev, "%s(0x%x 0x%x 0x%x 0x%x) returned 0x%x\n",
 		__func__, smcnr, a0, a1, a2, ret);
 
-	local_irq_enable();
-
 	mutex_unlock(&s->smc_lock);
 
 	return ret;