[U-Boot] [PATCH] semihosting: Improve ARMv7A support

[Andrey Smirnov]

Previous implementation of semihosting didn't account for cases where
doing an SVC call would clobber various data(most notable case would
be 'lr' and 'spsr' when doing SVC call in Supervisor mode). This patch
is an adaptation of the code from Newlib's Angel_SWI feature (can be
found in libc/sys/arm/swi.h) which hopefuly fixes the problem.

Tested with modified OpenOCD and custom Vybrid VF610 based board

Signed-off-by: Andrey Smirnov <andrew.smirnov at gmail.com>
---
 arch/arm/lib/semihosting.c | 30 ++++++++++++++++++++++++++----
 1 file changed, 26 insertions(+), 4 deletions(-)

diff --git a/arch/arm/lib/semihosting.c b/arch/arm/lib/semihosting.c
index c3e964e..5f22c2d 100644
--- a/arch/arm/lib/semihosting.c
+++ b/arch/arm/lib/semihosting.c
@@ -26,18 +26,40 @@
 /*
  * Call the handler
  */
+#if defined(CONFIG_ARM64)
 static noinline long smh_trap(unsigned int sysnum, void *addr)
 {
 	register long result asm("r0");
-#if defined(CONFIG_ARM64)
 	asm volatile ("hlt #0xf000" : "=r" (result) : "0"(sysnum), "r"(addr));
+	return result;
+}
 #else
-	/* Note - untested placeholder */
-	asm volatile ("svc #0x123456" : "=r" (result) : "0"(sysnum), "r"(addr));
+#if defined (CONFIG_SYS_THUMB_BUILD)
+#error "Support for semihosting in THUMB mode is not implemented"
 #endif
+/*
+ * Call the handler
+ */
+static noinline long smh_trap(unsigned int sysnum, void *addr)
+{
+	int result;
+
+	asm volatile ("mov r0, %1"	"\n\t"
+		      "mov r1, %2"	"\n\t"
+		      "svc #0x123456"	"\n\t"
+		      "mov %0, r0"	"\n\t"
+		      : "=r" (result)
+		      : "r" (sysnum), "r" (addr)
+		      : "r0", "r1", "r2", "r3", "ip", "lr", "memory", "cc");
+		      /* Clobbers r0 and r1, and lr if in supervisor
+			 mode Accordingly to page 13-77 of ARM DUI
+			 0040D other registers can also be clobbered.
+			 Some memory positions may also be changed by
+			 a system call, so they should not be kept in
+			 registers.  */
 	return result;
 }
-
+#endif
 /*
  * Open a file on the host. Mode is "r" or "rb" currently. Returns a file
  * descriptor or -1 on error.
--
2.1.4