hwstub: implement read/write data abort recovery

Change-Id: I1625873b6864584c40984723d82548ad242ee08e

2 files changed, 112 insertions, 30 deletions

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) :
   {