4 files changed, 61 insertions, 3 deletions

diff --git a/arch/arm/lib/bootm.c b/arch/arm/lib/bootm.c
index c56285738a2..f30a483ed8b 100644
--- a/arch/arm/lib/bootm.c
+++ b/arch/arm/lib/bootm.c
@@ -12,6 +12,7 @@
  */
 
 #include <common.h>
+#include <bootm.h>
 #include <bootstage.h>
 #include <command.h>
 #include <cpu_func.h>
@@ -378,9 +379,10 @@ static void boot_jump_linux(struct bootm_headers *images, int flag)
  * DIFFERENCE: Instead of calling prep and go at the end
  * they are called if subcommand is equal 0.
  */
-int do_bootm_linux(int flag, int argc, char *const argv[],
-		   struct bootm_headers *images)
+int do_bootm_linux(int flag, struct bootm_info *bmi)
 {
+	struct bootm_headers *images = bmi->images;
+
 	/* No need for those on ARM */
 	if (flag & BOOTM_STATE_OS_BD_T || flag & BOOTM_STATE_OS_CMDLINE)
 		return -1;
diff --git a/arch/arm/lib/interrupts.c b/arch/arm/lib/interrupts.c
index 6dc27d1d589..9961472f69f 100644
--- a/arch/arm/lib/interrupts.c
+++ b/arch/arm/lib/interrupts.c
@@ -22,6 +22,7 @@
 #include <cpu_func.h>
 #include <efi_loader.h>
 #include <irq_func.h>
+#include <semihosting.h>
 #include <asm/global_data.h>
 #include <asm/proc-armv/ptrace.h>
 #include <asm/ptrace.h>
@@ -135,6 +136,32 @@ static inline void fixup_pc(struct pt_regs *regs, int offset)
 	regs->ARM_pc = pc | (regs->ARM_pc & PCMASK);
 }
 
+/*
+ * 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)
+{
+	if (regs->ARM_cpsr & T_BIT) {
+		u16 *insn = (u16 *)(regs->ARM_pc - 2);
+
+		if (*insn != SMH_T32_SVC)
+			return false;
+	} else {
+		u32 *insn = (u32 *)(regs->ARM_pc - 4);
+
+		if (*insn != SMH_A32_SVC)
+			return false;
+	}
+
+	/* Avoid future semihosting calls */
+	disable_semihosting();
+
+	/* Just pretend the call failed */
+	regs->ARM_r0 = -1;
+	return true;
+}
+
 void do_undefined_instruction (struct pt_regs *pt_regs)
 {
 	efi_restore_gd();
@@ -147,6 +174,10 @@ void do_undefined_instruction (struct pt_regs *pt_regs)
 
 void do_software_interrupt (struct pt_regs *pt_regs)
 {
+	if (CONFIG_IS_ENABLED(SEMIHOSTING_FALLBACK) &&
+	    smh_emulate_trap(pt_regs))
+		return;
+
 	efi_restore_gd();
 	printf ("software interrupt\n");
 	fixup_pc(pt_regs, -4);
diff --git a/arch/arm/lib/semihosting.S b/arch/arm/lib/semihosting.S
index 393aade94a5..6e1691a832c 100644
--- a/arch/arm/lib/semihosting.S
+++ b/arch/arm/lib/semihosting.S
@@ -18,11 +18,17 @@ ENTRY(smh_trap)
 #elif defined(CONFIG_SYS_THUMB_BUILD)
 	svc	#0xab
 #else
+#if CONFIG_SYS_ARM_ARCH < 7
+	/* Before the ARMv7 exception model, svc (swi) clobbers lr */
+	mov	r2, lr
+#endif
 	svc	#0x123456
 #endif
 
 #if defined(CONFIG_ARM64)
 	ret
+#elif CONFIG_SYS_ARM_ARCH < 7
+	bx	r2
 #else
 	bx	lr
 #endif
diff --git a/arch/arm/lib/vectors.S b/arch/arm/lib/vectors.S
index 7cf7d1636f5..843f9b9c281 100644
--- a/arch/arm/lib/vectors.S
+++ b/arch/arm/lib/vectors.S
@@ -240,6 +240,18 @@ IRQ_STACK_START_IN:
 	movs	pc, lr		@ jump to next instruction & switch modes.
 	.endm
 
+	.macro get_bad_stack_swi
+	sub	r13, r13, #4	@ space on current stack for scratch reg.
+	str	r0, [r13]	@ save R0's value.
+	ldr	r0, IRQ_STACK_START_IN		@ get data regions start
+	str	lr, [r0]	@ save caller lr in position 0 of saved stack
+	mrs	lr, spsr	@ get the spsr
+	str	lr, [r0, #4]	@ save spsr in position 1 of saved stack
+	ldr	lr, [r0]	@ restore lr
+	ldr	r0, [r13]	@ restore r0
+	add	r13, r13, #4	@ pop stack entry
+	.endm
+
 	.macro get_irq_stack			@ setup IRQ stack
 	ldr	sp, IRQ_STACK_START
 	.endm
@@ -260,9 +272,16 @@ undefined_instruction:
 
 	.align	5
 software_interrupt:
-	get_bad_stack
+	get_bad_stack_swi
 	bad_save_user_regs
 	bl	do_software_interrupt
+#if CONFIG_IS_ENABLED(SEMIHOSTING_FALLBACK)
+	ldmia	sp, {r0 - lr}^			@ Calling r0 - lr
+	mov	r0, r0
+	ldr	lr, [sp, #S_PC]			@ Get PC
+	add	sp, sp, #S_FRAME_SIZE
+	movs	pc, lr		@ return & move spsr_svc into cpsr
+#endif
 
 	.align	5
 prefetch_abort: