arm64: Catch non-emulated semihosting calls

If a debugger is not attached to U-Boot, semihosting calls will raise a
synchronous abort exception. Try to catch this and disable semihosting
so we can e.g. use another uart if one is available. In the immediate
case, we return an error, since it is not always possible to check for
semihosting beforehand (debug uart, user-initiated load command, etc.)

We handle all possible semihosting instructions, which is probably
overkill. However, we do need to keep track of what instruction set
we're using so that we don't suppress an actual error.

A future enhancement could try to determine semihosting capability by
inspecting the processor state.  There's an example of this at [1] for
RISC-V. The equivalent for ARM would inspect the monitor modei
enable/select bits of the DSCR. However, as the article notes, an
exception handler is still helpful in order to catch disconnected
debuggers.

[1] https://tomverbeure.github.io/2021/12/30/Semihosting-on-RISCV.html#avoiding-hangs-when-a-debugger-is-not-connected

Signed-off-by: Sean Anderson <sean.anderson@seco.com>

1 files changed, 47 insertions, 0 deletions

diff --git a/arch/arm/lib/interrupts_64.c b/arch/arm/lib/interrupts_64.c
index 049beeca7e..2e091415a4 100644
--- a/arch/arm/lib/interrupts_64.c
+++ b/arch/arm/lib/interrupts_64.c
@@ -5,11 +5,13 @@
  */
 
 #include <common.h>
+#include <asm/esr.h>
 #include <asm/global_data.h>
 #include <asm/ptrace.h>
 #include <irq_func.h>
 #include <linux/compiler.h>
 #include <efi_loader.h>
+#include <semihosting.h>
 
 DECLARE_GLOBAL_DATA_PTR;
 
@@ -64,6 +66,48 @@ void show_regs(struct pt_regs *regs)
 }
 
 /*
+ * Try to "emulate" a semihosting call in the event that we don't have a
+ * debugger attached.
+ */
+static bool smh_emulate_trap(struct pt_regs *regs)
+{
+	int size;
+
+	if (ESR_ELx_EC(regs->esr) != ESR_ELx_EC_UNKNOWN)
+		return false;
+
+	if (regs->spsr & PSR_MODE32_BIT) {
+		if (regs->spsr & PSR_AA32_T_BIT) {
+			u16 *insn = (u16 *)ALIGN_DOWN(regs->elr, 2);
+
+			if (*insn != SMH_T32_SVC && *insn != SMH_T32_HLT)
+				return false;
+			size = 2;
+		} else {
+			u32 *insn = (u32 *)ALIGN_DOWN(regs->elr, 4);
+
+			if (*insn != SMH_A32_SVC && *insn != SMH_A32_HLT)
+				return false;
+			size = 4;
+		}
+	} else {
+		u32 *insn = (u32 *)ALIGN_DOWN(regs->elr, 4);
+
+		if (*insn != SMH_A64_HLT)
+			return false;
+		size = 4;
+	}
+
+	/* Avoid future semihosting calls */
+	disable_semihosting();
+
+	/* Just pretend the call failed */
+	regs->regs[0] = -1;
+	regs->elr += size;
+	return true;
+}
+
+/*
  * do_bad_sync handles the impossible case in the Synchronous Abort vector.
  */
 void do_bad_sync(struct pt_regs *pt_regs)
@@ -117,6 +161,9 @@ void do_bad_error(struct pt_regs *pt_regs)
  */
 void do_sync(struct pt_regs *pt_regs)
 {
+	if (CONFIG_IS_ENABLED(SEMIHOSTING_FALLBACK) &&
+	    smh_emulate_trap(pt_regs))
+		return;
 	efi_restore_gd();
 	printf("\"Synchronous Abort\" handler, esr 0x%08lx\n", pt_regs->esr);
 	show_regs(pt_regs);