diff options
Diffstat (limited to 'arch/arm/lib')
| -rw-r--r-- | arch/arm/lib/interrupts.c | 31 | ||||
| -rw-r--r-- | arch/arm/lib/semihosting.S | 6 | ||||
| -rw-r--r-- | arch/arm/lib/vectors.S | 21 |
3 files changed, 57 insertions, 1 deletions
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: |