msm: scm: Check for interruption immediately

When we're interrupted on the secure side, we should just issue
another smc instruction again instead of replaying the arguments
to smc. Fix it.

Signed-off-by: Stephen Boyd <sboyd@codeaurora.org>
Signed-off-by: David Brown <davidb@codeaurora.org>

1 files changed, 27 insertions, 24 deletions

diff --git a/arch/arm/mach-msm/scm.c b/arch/arm/mach-msm/scm.c
index 5eddf549717a..cfa808dd4897 100644
--- a/arch/arm/mach-msm/scm.c
+++ b/arch/arm/mach-msm/scm.c
@@ -174,15 +174,18 @@ static u32 smc(u32 cmd_addr)
         register u32 r0 asm("r0") = 1;
         register u32 r1 asm("r1") = (u32)&context_id;
         register u32 r2 asm("r2") = cmd_addr;
-        asm volatile(
-                __asmeq("%0", "r0")
-                __asmeq("%1", "r0")
-                __asmeq("%2", "r1")
-                __asmeq("%3", "r2")
-                "smc    #0      @ switch to secure world\n"
-                : "=r" (r0)
-                : "r" (r0), "r" (r1), "r" (r2)
-                : "r3");
+        do {
+                asm volatile(
+                        __asmeq("%0", "r0")
+                        __asmeq("%1", "r0")
+                        __asmeq("%2", "r1")
+                        __asmeq("%3", "r2")
+                        "smc    #0      @ switch to secure world\n"
+                        : "=r" (r0)
+                        : "r" (r0), "r" (r1), "r" (r2)
+                        : "r3");
+        } while (r0 == SCM_INTERRUPTED);
+
         return r0;
 }
 
@@ -197,13 +200,9 @@ static int __scm_call(const struct scm_command *cmd)
          * side in the buffer.
          */
         flush_cache_all();
-        do {
-                ret = smc(cmd_addr);
-                if (ret < 0) {
-                        ret = scm_remap_error(ret);
-                        break;
-                }
-        } while (ret == SCM_INTERRUPTED);
+        ret = smc(cmd_addr);
+        if (ret < 0)
+                ret = scm_remap_error(ret);
 
         return ret;
 }
@@ -274,14 +273,18 @@ u32 scm_get_version(void)
 
         r0 = 0x1 << 8;
         r1 = (u32)&context_id;
-        asm volatile(
-                __asmeq("%0", "r1")
-                __asmeq("%1", "r0")
-                __asmeq("%2", "r1")
-                "smc    #0      @ switch to secure world\n"
-                : "=r" (r1)
-                : "r" (r0), "r" (r1)
-                : "r2", "r3");
+        do {
+                asm volatile(
+                        __asmeq("%0", "r0")
+                        __asmeq("%1", "r1")
+                        __asmeq("%2", "r0")
+                        __asmeq("%3", "r1")
+                        "smc    #0      @ switch to secure world\n"
+                        : "=r" (r0), "=r" (r1)
+                        : "r" (r0), "r" (r1)
+                        : "r2", "r3");
+        } while (r0 == SCM_INTERRUPTED);
+
         version = r1;
         mutex_unlock(&scm_lock);