hwstub: implement read/write data abort recovery

Change-Id: I1625873b6864584c40984723d82548ad242ee08e

11 files changed, 304 insertions, 39 deletions

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__ */