aarch64: implement exception entry and exit

generate exception frame

Signed-off-by: Jani Kokkonen <jani.kokkonen@huawei.com>
Signed-off-by: Claudio Fontana <claudio.fontana@huawei.com>

arch/aarch64/entry.S

@@ -5,8 +5,6 @@
  * BSD license as described in the LICENSE file in the top-level directory.
  */
 
-/* XXX TODO nothing to see here, move along XXX */
-
 #include "macros.S"
 
 /* the exception vectors area must be page aligned (we adrp directly).
@@ -27,7 +25,6 @@
 .macro vector_entry label idx
 /* every entry is at 2^7 bits distance */
 .align 7
-        mov     x19, #\idx
         b       \label
 .endm
 
@@ -58,14 +55,65 @@ exception_vectors:
         vector_entry entry_invalid 14
         vector_entry entry_invalid 15
 
+/* keep in sync with the struct in exceptions.hh */
+.macro push_state_to_exception_frame
+        sub     sp, sp, #48 // make space for align2, align1+ESR, PSTATE, PC, SP
+        push_pair x28, x29
+        push_pair x26, x27
+        push_pair x24, x25
+        push_pair x22, x23
+        push_pair x20, x21
+        push_pair x18, x19
+        push_pair x16, x17
+        push_pair x14, x15
+        push_pair x12, x13
+        push_pair x10, x11
+        push_pair x8, x9
+        push_pair x6, x7
+        push_pair x4, x5
+        push_pair x2, x3
+        push_pair x0, x1
+        add     x1, sp, #288         // x1 := old SP (48 + 16 * 15 = 288)
+        mrs     x2, elr_el1
+        mrs     x3, spsr_el1
+        stp     x30, x1, [sp, #240]  // store lr, old SP
+        stp     x2, x3, [sp, #256]   // store elr_el1, spsr_el1
+.endm /* push_state_to_exception_frame */
+
+.macro pop_state_from_exception_frame
+        ldp     x21, x22, [sp, #256] // load elr_el1, spsr_el1
+        pop_pair x0, x1
+        pop_pair x2, x3
+        pop_pair x4, x5
+        pop_pair x6, x7
+        pop_pair x8, x9
+        msr     elr_el1, x21         // set loaded elr and spsr
+        msr     spsr_el1, x22
+        pop_pair x10, x11
+        pop_pair x12, x13
+        pop_pair x14, x15
+        pop_pair x16, x17
+        pop_pair x18, x19
+        pop_pair x20, x21
+        pop_pair x22, x23
+        pop_pair x24, x25
+        pop_pair x26, x27
+        pop_pair x28, x29
+        ldr     x30, [sp], #48
+.endm /* pop_state_to_exception_frame */
+
+.equ ESR_EC_SHIFT,26        // Exception Class field bit position in ESR
+.equ ESR_EC_DATA_ABORT,0x25 // Exception Class Data Abort value
+.equ ESR_ISS_LEN,24         // Instruction-Specific Syndrome field len in ESR
+
 .global entry_invalid
 entry_invalid:
         mrs     x20, elr_el1       // Exception Link Register -> X20
         mrs     x21, spsr_el1      // Saved PSTATE -> X21
         mrs     x22, esr_el1       // Exception Syndrome Register -> X22
 
-        ubfm    x23, x22, #26, #31 // Exception Class -> X23
-        ubfm    x24, x22, #0, #24  // Instruction-Specific Syndrome -> X24
+        ubfm    x23, x22, #ESR_EC_SHIFT, #31 // Exception Class -> X23
+        ubfm    x24, x22, #0, #ESR_ISS_LEN   // Instruction-Specific Syndrome -> X24
         ubfm    x25, x22, #25, #25 // Instruction Length -> X25
 
 1:      wfi
@@ -73,52 +121,22 @@ entry_invalid:
 
 .global entry_sync
 entry_sync:
+        b       entry_invalid
+
 .global entry_irq
 entry_irq:
+        push_state_to_exception_frame
+        mov     x0, sp
+        bl      interrupt // extern "C"
+        pop_state_from_exception_frame
+        eret
+
 .global entry_fiq
 entry_fiq:
 .global entry_serror
 entry_serror:
         b       entry_invalid
 
-        /*
-        .macro	exception_entry name, handler, has_error_code
-	push_pair x28, x29
-	push_pair x26, x27
-	push_pair x24, x25
-	push_pair x22, x23
-	push_pair x20, x21
-
-	push_pair x18, x19
-	push_pair x16, x17
-	push_pair x14, x15
-	push_pair x12, x13
-	push_pair x10, x11
-
-	push_pair x8, x9
-	push_pair x6, x7
-	push_pair x4, x5
-	push_pair x2, x3
-	push_pair x0, x1
-
-	push_pair x29, x30
-        ...
-        .endm
-
-.macro exception_error_entry name, handler
-	exception_entry \name, \handler, 1
-.endm
-
-.macro exception_noerror_entry name, handler
-	exception_entry \name, \handler, 0
-.endm
-
-.cfi_sections .eh_frame,  .debug_frame
-
-.text
-
-*/
-
 .global thread_main
 thread_main:
         .type thread_main, @function

arch/aarch64/exceptions.hh

@@ -22,13 +22,13 @@
 struct exception_frame {
     u64 regs[31];
     u64 sp;
-    u64 pc;
-    u64 pstate;
+    u64 elr;
+    u64 spsr;
     u32 esr;
     u32 align1;
     u64 align2; /* align to 16 */
 
-    void *get_pc(void) { return (void *)pc; }
+    void *get_pc(void) { return (void *)elr; }
     unsigned int get_error(void) { return esr; }
 };
 

arch/aarch64/signal.cc

@@ -42,7 +42,7 @@ void build_signal_frame(exception_frame* ef,
     frame->state = *ef;
     frame->si = si;
     frame->sa = sa;
-    ef->pc = reinterpret_cast<ulong>(call_signal_handler_thunk);
+    ef->elr = reinterpret_cast<ulong>(call_signal_handler_thunk);
     ef->sp = reinterpret_cast<ulong>(sp);
 }