4 files changed, 106 insertions, 1 deletions

diff --git a/utils/hwstub/stub/asm/mips/system.S b/utils/hwstub/stub/asm/mips/system.S
index 97b0207ec9..2d89bde096 100644
--- a/utils/hwstub/stub/asm/mips/system.S
+++ b/utils/hwstub/stub/asm/mips/system.S
@@ -49,6 +49,33 @@ set_data_abort_jmp:
     sw ra, 40(v0)
     jr ra
     move v0, zero
+
+/* restore context on read/write error, performs the interrupt return */
+.global restore_data_abort_jmp
+restore_data_abort_jmp:
+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
+#ifdef CONFIG_JZ4760B
+    /* XBurst has a 3 interlock cycle delay, but we don't know if the interlock
+     * works with eret */
+    nop
+#else
+    ehb
+#endif
+    li v0, 1
+    eret
+    nop
 .set reorder
 
 #ifdef CONFIG_FLUSH_CACHES
diff --git a/utils/hwstub/stub/jz4760b/crt0.S b/utils/hwstub/stub/jz4760b/crt0.S
index 73dbe20428..94d95b3e73 100644
--- a/utils/hwstub/stub/jz4760b/crt0.S
+++ b/utils/hwstub/stub/jz4760b/crt0.S
@@ -51,7 +51,7 @@ reloc_loop:
 
     /* Tricky part: as explained earlier, tcsm0 is uncached so no need to commit
      * the dcache but we want to invalidate the icache ONLY AT THIS LOCATION.
-     * Indeed, if the invalidate the entire icache in the cache-as-ram case, we
+     * Indeed, if we invalidate the entire icache in the cache-as-ram case, we
      * will miserably crash */
     cache   ICHitInv, 0(t0)   /* invalidate virtual address in icache */
 
@@ -92,7 +92,71 @@ clear_bss_loop:
 stack_setup:
     la      sp, oc_stackend
 
+    /* the tcsm0 is usually accessed by its weird 0xf4000000 address but this
+     * address is not in the range available for EBASE (because EBASE[31:30]
+     * is hardwired to 0b10). Fortunately, the TCSM0 can be accessed by its
+     * physical address (0x132b0000) if we ungate the AHB1 */
+    la      t0, 0xb0000028 /* CPM_CLKGATE1 */
+    lw      t1, 0(t0)
+    li      t2, 0xffffff7e /* AHB1 */
+    and     t1, t2 /* clear AHB1 */
+    sw      t1, 0(t0)
+    /* keep interrupts disabled, use normal exception vectors (to use EBASE) */
+    li      t0, 0
+    mtc0    t0, C0_STATUS
+    /* set EBASE */
+    la      t0, irqbase
+    mtc0    t0, C0_EBASE
     /* jump to C code */
     la      t0, main
     jr      t0
     move    a0, k0
+
+die_blink:
+    /* die blinking */
+    la      a0, 0xb0010400
+    li      a1, 2
+    sw      a1, 0x48(a0) /* clear function (gpio or interrupt) */
+    sw      a1, 0x58(a0) /* clear select (gpio) */
+    sw      a1, 0x64(a0) /* set direction (out) */
+    sw      a1, 0x34(a0) /* set pull (disable) */
+    /* turn backlight on and off */
+    la      a0, 0xb0010414
+    li      a1, 2
+.blink_loop:
+    sw      a1, (a0)
+    la      v0, 10000000
+.wait:
+    bnez    v0, .wait
+    subu    v0, 1
+    sw      a1, 4(a0)
+    la      v0, 10000000
+.wait2:
+    bnez    v0, .wait2
+    subu    v0, 1
+    j       .blink_loop
+    nop
+
+/* restore_data_abort_jmp restores the context and returns from exception */
+    .extern restore_data_abort_jmp
+
+    .global tlb_refill_handler
+    .section .exception.tlb_refill,"ax",%progbits
+tlb_refill_handler:
+    la      k0, restore_data_abort_jmp
+    jr      k0
+    nop
+
+    .global cache_error_handler
+    .section .exception.cache_error,"ax",%progbits
+cache_error_handler:
+    la      k0, restore_data_abort_jmp
+    jr      k0
+    nop
+
+    .global general_exception_handler
+    .section .exception.general_exception,"ax",%progbits
+general_exception_handler:
+    la      k0, restore_data_abort_jmp
+    jr      k0
+    nop
diff --git a/utils/hwstub/stub/jz4760b/hwstub.lds b/utils/hwstub/stub/jz4760b/hwstub.lds
index 33aad51ebd..f0460284ca 100644
--- a/utils/hwstub/stub/jz4760b/hwstub.lds
+++ b/utils/hwstub/stub/jz4760b/hwstub.lds
@@ -20,10 +20,23 @@ SECTIONS
         *(.icode*)
         *(.data*)
         *(.rodata*)
+        /* exceptions needs to be on a 0x1000 boundary */
+        . = ALIGN(0x1000);
+        tcsm0_irqbase = .;
+        KEEP(*(.exception.tlb_refill))
+        . = tcsm0_irqbase + 0x100;
+        KEEP(*(.exception.cache_error))
+        . = tcsm0_irqbase + 0x180;
+        KEEP(*(.exception.general_exception))
         . = ALIGN(4);
         relocend = .;
     } > TCSM0
 
+    /* tcsm0_irqbase is the address in the 0xf400xxxx address space, but for
+     * EBASE, we want to the corresponding k1seg address, that maps to the
+     * physical address of TCSM0 */
+    irqbase = tcsm0_irqbase - TCSM0_ORIG + TCSM0_UNCACHED_ADDRESS;
+
     .bss (NOLOAD) :
     {
         bssbegin = .;
diff --git a/utils/hwstub/stub/jz4760b/target-config.h b/utils/hwstub/stub/jz4760b/target-config.h
index 681e17e6f6..5737e0bc87 100644
--- a/utils/hwstub/stub/jz4760b/target-config.h
+++ b/utils/hwstub/stub/jz4760b/target-config.h
@@ -1,6 +1,7 @@
 #define CONFIG_JZ4760B
 #define TCSM0_ORIG      0xf4000000
 #define TCSM0_SIZE      0x4000
+#define TCSM0_UNCACHED_ADDRESS  0xb32b0000
 #define CPU_MIPS
 #define STACK_SIZE      0x300
 #define DCACHE_SIZE     0x4000 /* 16 kB */