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>

arch/arm/mach-msm/scm.c

@@ -174,6 +174,7 @@ 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;
+	do {
 		asm volatile(
 			__asmeq("%0", "r0")
 			__asmeq("%1", "r0")
@@ -183,6 +184,8 @@ static u32 smc(u32 cmd_addr)
 			: "=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) {
+	if (ret < 0)
 		ret = scm_remap_error(ret);
-			break;
-		}
-	} while (ret == SCM_INTERRUPTED);
 
 	return ret;
 }
@@ -274,14 +273,18 @@ u32 scm_get_version(void)
 
 	r0 = 0x1 << 8;
 	r1 = (u32)&context_id;
+	do {
 		asm volatile(
-		__asmeq("%0", "r1")
-		__asmeq("%1", "r0")
-		__asmeq("%2", "r1")
+			__asmeq("%0", "r0")
+			__asmeq("%1", "r1")
+			__asmeq("%2", "r0")
+			__asmeq("%3", "r1")
 			"smc	#0	@ switch to secure world\n"
-		: "=r" (r1)
+			: "=r" (r0), "=r" (r1)
 			: "r" (r0), "r" (r1)
 			: "r2", "r3");
+	} while (r0 == SCM_INTERRUPTED);
+
 	version = r1;
 	mutex_unlock(&scm_lock);