From 2cdfc43f10e3d755018ea508b64b209608d71864 Mon Sep 17 00:00:00 2001 From: Amaury Pouly Date: Sat, 20 Sep 2014 14:29:12 +0200 Subject: hwstub: implement read/write data abort recovery Change-Id: I1625873b6864584c40984723d82548ad242ee08e --- utils/hwstub/stub/SOURCES | 2 + utils/hwstub/stub/asm/arm/system.S | 57 +++++++++++++++++++++++++ utils/hwstub/stub/asm/mips/atomic_rw.S | 2 +- utils/hwstub/stub/asm/mips/system.S | 51 +++++++++++++++++++++++ utils/hwstub/stub/atj213x/crt0.S | 76 ++++++++++++++++++++++++++++++---- utils/hwstub/stub/atj213x/hwstub.lds | 66 +++++++++++++++++++---------- utils/hwstub/stub/main.c | 60 ++++++++++++++++++++++++--- utils/hwstub/stub/rk27xx/crt0.S | 2 +- utils/hwstub/stub/stmp/crt0.S | 24 ++++++++++- utils/hwstub/stub/stmp/hwstub.lds | 1 + utils/hwstub/stub/system.h | 2 + 11 files changed, 304 insertions(+), 39 deletions(-) create mode 100644 utils/hwstub/stub/asm/arm/system.S create mode 100644 utils/hwstub/stub/asm/mips/system.S diff --git a/utils/hwstub/stub/SOURCES b/utils/hwstub/stub/SOURCES index c91580c966..1ade167ca3 100644 --- a/utils/hwstub/stub/SOURCES +++ b/utils/hwstub/stub/SOURCES @@ -3,10 +3,12 @@ asm/arm/memcpy.S asm/arm/memmove.S asm/arm/memset.S asm/arm/atomic_rw.S +asm/arm/system.S #elif defined(CPU_MIPS) asm/mips/memcpy.S asm/mips/memset.S asm/mips/atomic_rw.S +asm/mips/system.S #else #error "Unimplemented ISA" #endif diff --git a/utils/hwstub/stub/asm/arm/system.S b/utils/hwstub/stub/asm/arm/system.S new file mode 100644 index 0000000000..df6b5a2e81 --- /dev/null +++ b/utils/hwstub/stub/asm/arm/system.S @@ -0,0 +1,57 @@ +/*************************************************************************** + * __________ __ ___. + * Open \______ \ ____ ____ | | _\_ |__ _______ ___ + * Source | _// _ \_/ ___\| |/ /| __ \ / _ \ \/ / + * Jukebox | | ( <_> ) \___| < | \_\ ( <_> > < < + * Firmware |____|_ /\____/ \___ >__|_ \|___ /\____/__/\_ \ + * \/ \/ \/ \/ \/ + * $Id$ + * + * Copyright (C) 2014 by Amaury Pouly + * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. + * + * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY + * KIND, either express or implied. + * + ****************************************************************************/ + +/* Handling of data abort: + * the code can register a "longjmp" buffer to restore the context in case of + * fault */ +.data +data_abort_jmp_ctx_ptr: +/* buffer contains in order: cpsr,r4-r11,sp,lr,pc */ +.skip 48 /* = 4 * (cpsr + 11 registers) */ + +.text +/* Prototype: int set_data_abort_jmp() + * Return: 1 in case of data abort, 0 otherwise */ +.global set_data_abort_jmp +set_data_abort_jmp: + mrs r2, cpsr + ldr r1, =data_abort_jmp_ctx_ptr + mov r0, #0 + stmia r1, {r2,r4-r11,sp,lr,pc} /* see PC note below */ + bx lr + mov r0, #1 /* <-- PC points here in stmia */ + bx lr + +.global data_abort_handler +data_abort_handler: + /* restore everything from context */ + ldr r1, =data_abort_jmp_ctx_ptr + /* NOTE: we need to restore sp_sys and lr_sys, for this we need the + * LDM Rn, {}^ + * variant, but we cannot restore PC from it because ^ has a different + * meaning and won't restore user/sys registers. On top of that, the + * non-PC ^ variant cannot do the register writeback, so on the PC restore, + * we reload all registers once again to avoid manually offseting the base + * register, it will trash sp_abt and lr_abr but those are unused anyway + * because we do not save the abort address and we don't use an abort stack */ + ldmia r1, {r0,r4-r11,sp,lr}^ /* this variant cannot have writeback (r1!) */ + msr spsr, r0 + ldmia r1, {r0,r4-r11,sp,lr,pc}^ /* reload some registers but we don't care */ diff --git a/utils/hwstub/stub/asm/mips/atomic_rw.S b/utils/hwstub/stub/asm/mips/atomic_rw.S index 47c4213a5d..db766af857 100644 --- a/utils/hwstub/stub/asm/mips/atomic_rw.S +++ b/utils/hwstub/stub/asm/mips/atomic_rw.S @@ -20,7 +20,7 @@ #include "mips.h" .set noreorder - .section .text, "ax", %progbits + .section .icode, "ax", %progbits .global target_read8 .type target_read8, %function .global target_read16 diff --git a/utils/hwstub/stub/asm/mips/system.S b/utils/hwstub/stub/asm/mips/system.S new file mode 100644 index 0000000000..ab134aed48 --- /dev/null +++ b/utils/hwstub/stub/asm/mips/system.S @@ -0,0 +1,51 @@ +/*************************************************************************** + * __________ __ ___. + * Open \______ \ ____ ____ | | _\_ |__ _______ ___ + * Source | _// _ \_/ ___\| |/ /| __ \ / _ \ \/ / + * Jukebox | | ( <_> ) \___| < | \_\ ( <_> > < < + * Firmware |____|_ /\____/ \___ >__|_ \|___ /\____/__/\_ \ + * \/ \/ \/ \/ \/ + * + * Copyright (C) 2015 by Marcin Bukat + * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. + * + * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY + * KIND, either express or implied. + * + ****************************************************************************/ +#include "mips.h" + +/* Handling of data abort: + * the code can register a "longjmp" buffer to restore the context in case of + * fault */ +.data +.global data_abort_jmp_ctx_ptr +data_abort_jmp_ctx_ptr: +/* buffer contains in order: s0-s7, sp, s8, ra */ +.skip 44 /* = 4 * (9 callee saved registers + sp + ra) */ + +.set noreorder +.section .icode, "ax", %progbits +/* Prototype: int set_data_abort_jmp() + * Return: 1 in case of data abort, 0 otherwise */ +.global set_data_abort_jmp +set_data_abort_jmp: + la v0, data_abort_jmp_ctx_ptr + sw s0, 0(v0) + sw s1, 4(v0) + sw s2, 8(v0) + sw s3, 12(v0) + sw s4, 16(v0) + sw s5, 20(v0) + sw s6, 24(v0) + sw s7, 28(v0) + sw sp, 32(v0) + sw s8, 36(v0) + sw ra, 40(v0) + jr ra + move v0, zero +.set reorder diff --git a/utils/hwstub/stub/atj213x/crt0.S b/utils/hwstub/stub/atj213x/crt0.S index d19640c32c..93cc57fc5e 100644 --- a/utils/hwstub/stub/atj213x/crt0.S +++ b/utils/hwstub/stub/atj213x/crt0.S @@ -107,7 +107,7 @@ core_irq_setup: mtc0 t1, C0_CAUSE # DC=1, IV=1 mtc0 zero,C0_INTCTL # VS = 0 - # clear bss +clear_bss: la t0, bssbegin la t1, bssend beq t0, t1, stack_setup @@ -119,7 +119,6 @@ clear_bss_loop: addiu t0, 4 stack_setup: - # setup stack la k0, irqstackend la sp, stackend la t0, stackbegin @@ -135,8 +134,72 @@ stack_munge_loop: jr.hb t0 ei - .set at - .set reorder + .extern data_abort_jmp_ctx_ptr + .global tlb_refill_handler + .section .exception.tlb_refill,"ax",%progbits + +tlb_refill_handler: + la k1, data_abort_jmp_ctx_ptr + lw s0, 0(k1) + lw s1, 4(k1) + lw s2, 8(k1) + lw s3, 12(k1) + lw s4, 16(k1) + lw s5, 20(k1) + lw s6, 24(k1) + lw s7, 28(k1) + lw sp, 32(k1) + lw s8, 36(k1) + lw k1, 40(k1) + mtc0 k1, C0_EPC + ehb + li v0, 1 + eret + nop + + .global cache_error_handler + .section .exception.cache_error,"ax",%progbits + +cache_error_handler: + la k1, data_abort_jmp_ctx_ptr + lw s0, 0(k1) + lw s1, 4(k1) + lw s2, 8(k1) + lw s3, 12(k1) + lw s4, 16(k1) + lw s5, 20(k1) + lw s6, 24(k1) + lw s7, 28(k1) + lw sp, 32(k1) + lw s8, 36(k1) + lw k1, 40(k1) + mtc0 k1, C0_EPC + ehb + li v0, 1 + eret + nop + + .global general_exception_handler + .section .exception.general_exception,"ax",%progbits + +general_exception_handler: + la k1, data_abort_jmp_ctx_ptr + lw s0, 0(k1) + lw s1, 4(k1) + lw s2, 8(k1) + lw s3, 12(k1) + lw s4, 16(k1) + lw s5, 20(k1) + lw s6, 24(k1) + lw s7, 28(k1) + lw sp, 32(k1) + lw s8, 36(k1) + lw k1, 40(k1) + mtc0 k1, C0_EPC + ehb + li v0, 1 + eret + nop /* s0-s7 not saved as this are callee saved registers * CO_STATUS is not saved as nested interrupts are not supported @@ -149,10 +212,7 @@ stack_munge_loop: .extern INT_UDC .global irq_handler - .set mips32r2 - .set noreorder - .set noat - .section .irq_vector,"ax",%progbits + .section .exception.irq,"ax",%progbits irq_handler: move k1, sp diff --git a/utils/hwstub/stub/atj213x/hwstub.lds b/utils/hwstub/stub/atj213x/hwstub.lds index 7f5c9fae18..06912fb56f 100644 --- a/utils/hwstub/stub/atj213x/hwstub.lds +++ b/utils/hwstub/stub/atj213x/hwstub.lds @@ -13,28 +13,59 @@ SECTIONS { .init.text : { - _irqbase = .; relocstart = .; oc_codestart = .; - *(.init.text*) + KEEP(*(.init.text*)) } > IRAM - .exception.text (IRAM_ORIG + 0x200) : + .icode : { - *(.irq_vector*) - } > IRAM - - .text : - { - *(.text*) *(.icode*) - } > IRAM + } > IRAM .data : { *(.rodata*) *(.data*) *(.rel.dyn) + } > IRAM + + .bss (NOLOAD) : + { + bssbegin = .; + *(.sbss*) + *(.bss*) + *(COMMON) + *(.scommon*) + . = ALIGN(4); + bssend = .; + } > IRAM + + .exception.tlb_refill (IRAM_ORIG + 0x1000) : + { + _irqbase = .; + KEEP(*(.exception.tlb_refill)) + } > IRAM + + .exception.cache_error (IRAM_ORIG + 0x1100) : + { + KEEP(*(.exception.cache_error)) + } > IRAM + + .exception.general_exception (IRAM_ORIG + 0x1180) : + { + KEEP(*(.exception.general_exception)) + } > IRAM + + .exception.irq (IRAM_ORIG + 0x1200) : + { + KEEP(*(.exception.irq)) + } > IRAM + + .text : + { + *(.text*) + . = ALIGN(16); relocend = .; } > IRAM @@ -42,26 +73,17 @@ SECTIONS { . = ALIGN(4); stackbegin = .; + oc_codeend = .; oc_stackstart = .; . += 0x2000; stackend = .; + irqstackbegin = .; . += 0x400; irqstackend = .; oc_stackend = .; - } > IRAM - - .bss (NOLOAD) : - { - bssbegin = .; - *(.sbss*) - *(.bss*) - *(COMMON) - *(.scommon*) - bssend = .; - oc_codeend = .; oc_bufferstart = .; - } > IRAM + } > IRAM .end IRAM_ORIG+IRAM_SIZE (NOLOAD) : { diff --git a/utils/hwstub/stub/main.c b/utils/hwstub/stub/main.c index 3604cfdae0..ee93ad4b63 100644 --- a/utils/hwstub/stub/main.c +++ b/utils/hwstub/stub/main.c @@ -26,6 +26,7 @@ #include "usb_drv.h" #include "memory.h" #include "target.h" +#include "system.h" extern unsigned char oc_codestart[]; extern unsigned char oc_codeend[]; @@ -420,18 +421,41 @@ static void handle_read(struct usb_ctrlrequest *req) { if(id != last_id) return usb_drv_stall(EP_CONTROL, true, true); + if(req->bRequest == HWSTUB_READ2_ATOMIC) { - if(!read_atomic(usb_buffer, (void *)last_addr, req->wLength)) + if(set_data_abort_jmp() == 0) + { + if(!read_atomic(usb_buffer, (void *)last_addr, req->wLength)) + return usb_drv_stall(EP_CONTROL, true, true); + } + else + { + logf("trapped read data abort in [0x%x,0x%x]\n", last_addr, + last_addr + req->wLength); return usb_drv_stall(EP_CONTROL, true, true); + } } else - memcpy(usb_buffer, (void *)last_addr, req->wLength); - asm volatile("nop" : : : "memory"); + { + if(set_data_abort_jmp() == 0) + { + memcpy(usb_buffer, (void *)last_addr, req->wLength); + asm volatile("nop" : : : "memory"); + } + else + { + logf("trapped read data abort in [0x%x,0x%x]\n", last_addr, + last_addr + req->wLength); + return usb_drv_stall(EP_CONTROL, true, true); + } + + } + usb_drv_send(EP_CONTROL, usb_buffer, req->wLength); usb_drv_recv(EP_CONTROL, NULL, 0); } -}; +} static bool write_atomic(void *dst, void *src, size_t sz) { @@ -452,13 +476,37 @@ static void handle_write(struct usb_ctrlrequest *req) int sz_hdr = sizeof(struct hwstub_write_req_t); if(size < sz_hdr) return usb_drv_stall(EP_CONTROL, true, true); + if(req->bRequest == HWSTUB_WRITE_ATOMIC) { - if(!write_atomic((void *)write->dAddress, usb_buffer + sz_hdr, size - sz_hdr)) + if(set_data_abort_jmp() == 0) + { + if(!write_atomic((void *)write->dAddress, + usb_buffer + sz_hdr, size - sz_hdr)) + return usb_drv_stall(EP_CONTROL, true, true); + } + else + { + logf("trapped write data abort in [0x%x,0x%x]\n", write->dAddress, + write->dAddress + size - sz_hdr); return usb_drv_stall(EP_CONTROL, true, true); + } } else - memcpy((void *)write->dAddress, usb_buffer + sz_hdr, size - sz_hdr); + { + if(set_data_abort_jmp() == 0) + { + memcpy((void *)write->dAddress, + usb_buffer + sz_hdr, size - sz_hdr); + } + else + { + logf("trapped write data abort in [0x%x,0x%x]\n", write->dAddress, + write->dAddress + size - sz_hdr); + return usb_drv_stall(EP_CONTROL, true, true); + } + } + usb_drv_send(EP_CONTROL, NULL, 0); } diff --git a/utils/hwstub/stub/rk27xx/crt0.S b/utils/hwstub/stub/rk27xx/crt0.S index 0c702eca91..953a7f315a 100644 --- a/utils/hwstub/stub/rk27xx/crt0.S +++ b/utils/hwstub/stub/rk27xx/crt0.S @@ -31,7 +31,7 @@ ldr pc, =start ldr pc, =start ldr pc, =start - ldr pc, =start + ldr pc, =data_abort_handler ldr pc, =start ldr pc, =irq_handler ldr pc, =start diff --git a/utils/hwstub/stub/stmp/crt0.S b/utils/hwstub/stub/stmp/crt0.S index 38b6f18ac5..5d8705497c 100644 --- a/utils/hwstub/stub/stmp/crt0.S +++ b/utils/hwstub/stub/stmp/crt0.S @@ -1,10 +1,29 @@ +.section .vectors,"ax",%progbits +.code 32 + /* most handlers are in DRAM which is too far away for a relative jump */ + b start + b start + b start + b start + b data_abort_handler + b start + b start + b start + .section .text,"ax",%progbits .code 32 .align 0x04 .global start start: + /* Save running address */ sub r7, pc, #8 /* Copy running address */ - msr cpsr_c, #0xd3 /* enter supervisor mode, disable IRQ/FIQ */ + /* adjust the offset between start and beginning of the binary */ + ldr r0, =_copystart + ldr r1, =start + add r7, r0 + sub r7, r1 + /* enter supervisor mode, disable IRQ/FIQ */ + msr cpsr_c, #0xd3 /* The stub could be located at a virtual address so killing the MMU at * this point would be mere suicide. We assume that the remap location * is identically mapped and kill the MMU after the copy */ @@ -37,6 +56,9 @@ remap: cmp r3, r2 strhi r4, [r2], #4 bhi 1b + /* NOTE: we don't need an abort stack */ + /* Switch to sys mode */ + msr cpsr_c, #0xdf /* jump to C code */ ldr sp, =oc_stackend b main diff --git a/utils/hwstub/stub/stmp/hwstub.lds b/utils/hwstub/stub/stmp/hwstub.lds index 8e3f4e68ba..7b93d50863 100644 --- a/utils/hwstub/stub/stmp/hwstub.lds +++ b/utils/hwstub/stub/stmp/hwstub.lds @@ -38,6 +38,7 @@ SECTIONS { _copystart = .; oc_codestart = .; + *(.vectors); *(.text*) *(.icode*) *(.data*) diff --git a/utils/hwstub/stub/system.h b/utils/hwstub/stub/system.h index e5aea12051..09c86debfe 100644 --- a/utils/hwstub/stub/system.h +++ b/utils/hwstub/stub/system.h @@ -114,5 +114,7 @@ static inline int disable_interrupt_save(int mask) return cpsr; } +int set_data_abort_jmp(void); + #endif /* __HWSTUB_SYSTEM__ */ -- cgit