1 files changed, 64 insertions, 16 deletions

diff --git a/arch/arm/lib/interrupts_64.c b/arch/arm/lib/interrupts_64.c
index c653e67db2..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,12 +66,55 @@ 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, unsigned int esr)
+void do_bad_sync(struct pt_regs *pt_regs)
 {
 	efi_restore_gd();
-	printf("Bad mode in \"Synchronous Abort\" handler, esr 0x%08x\n", esr);
+	printf("Bad mode in \"Synchronous Abort\" handler, esr 0x%08lx\n",
+	       pt_regs->esr);
 	show_regs(pt_regs);
 	show_efi_loaded_images(pt_regs);
 	panic("Resetting CPU ...\n");
@@ -78,10 +123,10 @@ void do_bad_sync(struct pt_regs *pt_regs, unsigned int esr)
 /*
  * do_bad_irq handles the impossible case in the Irq vector.
  */
-void do_bad_irq(struct pt_regs *pt_regs, unsigned int esr)
+void do_bad_irq(struct pt_regs *pt_regs)
 {
 	efi_restore_gd();
-	printf("Bad mode in \"Irq\" handler, esr 0x%08x\n", esr);
+	printf("Bad mode in \"Irq\" handler, esr 0x%08lx\n", pt_regs->esr);
 	show_regs(pt_regs);
 	show_efi_loaded_images(pt_regs);
 	panic("Resetting CPU ...\n");
@@ -90,10 +135,10 @@ void do_bad_irq(struct pt_regs *pt_regs, unsigned int esr)
 /*
  * do_bad_fiq handles the impossible case in the Fiq vector.
  */
-void do_bad_fiq(struct pt_regs *pt_regs, unsigned int esr)
+void do_bad_fiq(struct pt_regs *pt_regs)
 {
 	efi_restore_gd();
-	printf("Bad mode in \"Fiq\" handler, esr 0x%08x\n", esr);
+	printf("Bad mode in \"Fiq\" handler, esr 0x%08lx\n", pt_regs->esr);
 	show_regs(pt_regs);
 	show_efi_loaded_images(pt_regs);
 	panic("Resetting CPU ...\n");
@@ -102,10 +147,10 @@ void do_bad_fiq(struct pt_regs *pt_regs, unsigned int esr)
 /*
  * do_bad_error handles the impossible case in the Error vector.
  */
-void do_bad_error(struct pt_regs *pt_regs, unsigned int esr)
+void do_bad_error(struct pt_regs *pt_regs)
 {
 	efi_restore_gd();
-	printf("Bad mode in \"Error\" handler, esr 0x%08x\n", esr);
+	printf("Bad mode in \"Error\" handler, esr 0x%08lx\n", pt_regs->esr);
 	show_regs(pt_regs);
 	show_efi_loaded_images(pt_regs);
 	panic("Resetting CPU ...\n");
@@ -114,10 +159,13 @@ void do_bad_error(struct pt_regs *pt_regs, unsigned int esr)
 /*
  * do_sync handles the Synchronous Abort exception.
  */
-void do_sync(struct pt_regs *pt_regs, unsigned int esr)
+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%08x\n", esr);
+	printf("\"Synchronous Abort\" handler, esr 0x%08lx\n", pt_regs->esr);
 	show_regs(pt_regs);
 	show_efi_loaded_images(pt_regs);
 	panic("Resetting CPU ...\n");
@@ -126,10 +174,10 @@ void do_sync(struct pt_regs *pt_regs, unsigned int esr)
 /*
  * do_irq handles the Irq exception.
  */
-void do_irq(struct pt_regs *pt_regs, unsigned int esr)
+void do_irq(struct pt_regs *pt_regs)
 {
 	efi_restore_gd();
-	printf("\"Irq\" handler, esr 0x%08x\n", esr);
+	printf("\"Irq\" handler, esr 0x%08lx\n", pt_regs->esr);
 	show_regs(pt_regs);
 	show_efi_loaded_images(pt_regs);
 	panic("Resetting CPU ...\n");
@@ -138,10 +186,10 @@ void do_irq(struct pt_regs *pt_regs, unsigned int esr)
 /*
  * do_fiq handles the Fiq exception.
  */
-void do_fiq(struct pt_regs *pt_regs, unsigned int esr)
+void do_fiq(struct pt_regs *pt_regs)
 {
 	efi_restore_gd();
-	printf("\"Fiq\" handler, esr 0x%08x\n", esr);
+	printf("\"Fiq\" handler, esr 0x%08lx\n", pt_regs->esr);
 	show_regs(pt_regs);
 	show_efi_loaded_images(pt_regs);
 	panic("Resetting CPU ...\n");
@@ -153,10 +201,10 @@ void do_fiq(struct pt_regs *pt_regs, unsigned int esr)
  * it is defined with weak attribute and can be redefined
  * in processor specific code.
  */
-void __weak do_error(struct pt_regs *pt_regs, unsigned int esr)
+void __weak do_error(struct pt_regs *pt_regs)
 {
 	efi_restore_gd();
-	printf("\"Error\" handler, esr 0x%08x\n", esr);
+	printf("\"Error\" handler, esr 0x%08lx\n", pt_regs->esr);
 	show_regs(pt_regs);
 	show_efi_loaded_images(pt_regs);
 	panic("Resetting CPU ...\n");