[PATCH 5/6] ARM: semihosting: add DEBUG_LL implementation

[Ahmad Fatoum <a.fatoum@pengutronix.de>]

Both OpenOCD and QEMU have a semihosting implementation that provides a
console suitable for low level debugging. Add a DEBUG_LL implementation
that is usable when QEMU is called with

  --semihosting-config chardev=serial0

or following OpenOCD is executed:

  arm semihosting enable

Signed-off-by: Ahmad Fatoum <a.fatoum@pengutronix.de>
---
 arch/arm/include/asm/debug_ll.h    |  4 ++++
 arch/arm/include/asm/semihosting.h | 32 ++++++++++++++++++++++++++++++
 common/Kconfig.debug_ll            | 13 ++++++++++++
 drivers/firmware/Kconfig           |  1 +
 include/debug_ll/semihosting.h     | 15 ++++++++++++++
 5 files changed, 65 insertions(+)
 create mode 100644 include/debug_ll/semihosting.h

diff --git a/arch/arm/include/asm/debug_ll.h b/arch/arm/include/asm/debug_ll.h
index 999f1cb831a8..bac0b2106936 100644
--- a/arch/arm/include/asm/debug_ll.h
+++ b/arch/arm/include/asm/debug_ll.h
@@ -35,6 +35,10 @@
 #include <mach/layerscape/debug_ll.h>
 #endif
 
+#ifdef CONFIG_DEBUG_SEMIHOSTING
+#include <debug_ll/semihosting.h>
+#endif
+
 #ifdef CONFIG_DEBUG_QEMU_ARM64_VIRT
 #define DEBUG_LL_UART_ADDR		0x9000000
 #include <debug_ll/pl011.h>
diff --git a/arch/arm/include/asm/semihosting.h b/arch/arm/include/asm/semihosting.h
index c18aa1a5fef5..2219e858d040 100644
--- a/arch/arm/include/asm/semihosting.h
+++ b/arch/arm/include/asm/semihosting.h
@@ -4,5 +4,37 @@
 #define __ASM_ARM_SEMIHOSTING_H
 
 #include <asm-generic/semihosting.h>
+#include <asm/unified.h>
+
+static inline void semihosting_putc(void *unused, int c)
+{
+	/* We may not be relocated yet here, so we push
+	 * to stack and take address of that
+	 */
+#ifdef CONFIG_CPU_64
+	asm volatile (
+		"stp %0, xzr, [sp, #-16]!\n"
+		"mov x1, sp\n"
+		"mov x0, #0x03\n"
+	 	"hlt #0xf000\n"
+		"add sp, sp, #16\n"
+		: /* No outputs */
+		: "r" ((long)c)
+		: "x0", "x1", "x2", "x3", "x12", "memory"
+	);
+#else
+	asm volatile (
+		"push {%0}\n"
+		"mov r1, sp\n"
+		"mov r0, #0x03\n"
+	 ARM(	"bkpt #0x123456\n")
+	 THUMB(	"bkpt #0xAB\n")
+		"pop {%0}\n"
+		: /* No outputs */
+		: "r" (c)
+		: "r0", "r1", "r2", "r3", "r12", "memory"
+	);
+#endif
+}
 
 #endif
diff --git a/common/Kconfig.debug_ll b/common/Kconfig.debug_ll
index 472048177f8b..3f6f3e7c3bdb 100644
--- a/common/Kconfig.debug_ll
+++ b/common/Kconfig.debug_ll
@@ -337,6 +337,19 @@ config DEBUG_QEMU_ARM64_VIRT
 	bool "QEMU ARM64 Virt PL011 console"
 	depends on ARCH_ARM64_VIRT
 
+config DEBUG_SEMIHOSTING
+	bool "Semihosting console"
+	depends on SEMIHOSTING
+	help
+	  Semihosting enables code to use the I/O facilities on a
+	  host debugger/emulator through a simple supervisor call.
+	  The host debugger or emulator must have semihosting enabled
+	  for the supervisor call to be trapped, otherwise barebox
+	  will crash.
+
+	  Say Y here if you want low-level debugging support
+	  using semihosting writec.
+
 endchoice
 
 config DEBUG_LL_NS16550
diff --git a/drivers/firmware/Kconfig b/drivers/firmware/Kconfig
index 89c8bdeda3f0..264f7b2a5a65 100644
--- a/drivers/firmware/Kconfig
+++ b/drivers/firmware/Kconfig
@@ -3,6 +3,7 @@ menu "Firmware Drivers"
 
 config SEMIHOSTING
 	bool
+	select HAS_DEBUG_LL
 
 config FIRMWARE_ALTERA_SERIAL
 	bool "Altera SPI programming"
diff --git a/include/debug_ll/semihosting.h b/include/debug_ll/semihosting.h
new file mode 100644
index 000000000000..125d42ee88de
--- /dev/null
+++ b/include/debug_ll/semihosting.h
@@ -0,0 +1,15 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+#ifndef __DEBUG_LL_SEMIHOSTING_H
+#define __DEBUG_LL_SEMIHOSTING_H
+
+#include <asm/semihosting.h>
+#include <linux/stddef.h>
+
+#ifdef CONFIG_DEBUG_SEMIHOSTING
+static inline void PUTC_LL(char c)
+{
+	semihosting_putc(NULL, c);
+}
+#endif
+
+#endif /* __DEBUG_LL_ARM_SEMIHOSTING_H */
-- 
2.39.2