Add nonsecure EL1

Signed-off-by: Greg Bellows <greg.bellows@linaro.org>

9 files changed, 113 insertions, 30 deletions

diff --git a/.gdbinit64 b/.gdbinit64
index 0aa6c69..36651a5 100644
--- a/.gdbinit64
+++ b/.gdbinit64
@@ -16,6 +16,6 @@ end
 
 define load_el1ns
 file aarch64/el1_ns/el1_nsec.elf
-add-symbol-file aarch64/el1_ns/el1_nsec.elf &_EL1_S_TEXT_BASE
+add-symbol-file aarch64/el1_ns/el1_nsec.elf &_EL1_NS_TEXT_BASE
 end
 
diff --git a/aarch64/el1_common/el1.c b/aarch64/el1_common/el1.c
index e6cbdf0..204a21d 100644
--- a/aarch64/el1_common/el1.c
+++ b/aarch64/el1_common/el1.c
@@ -3,6 +3,7 @@
 #include "common_defs.h"
 #include "el1_loader.h"
 #include "string.h"
+#include "el1.h"
 
 typedef union {
     struct {
@@ -25,6 +26,19 @@ typedef union {
 
 typedef union {
     struct {
+        uint32_t ifsc : 6;
+        uint32_t res0_0 : 1;
+        uint32_t s1ptw : 1;
+        uint32_t res0_1 : 1;
+        uint32_t ea : 1;
+        uint32_t fnv  : 1;
+        uint32_t res0_2 : 3;
+    };
+    uint32_t raw;
+} armv8_inst_abort_iss_t;
+
+typedef union {
+    struct {
         uint64_t type : 2;
         uint64_t attridx : 3;
         uint64_t ns : 1;
@@ -65,7 +79,7 @@ uint64_t el1_allocate_pa() {
 
 void el1_map_va(uintptr_t addr)
 {
-    uint64_t pa = EL3_PGTBL_BASE;
+    uint64_t pa = EL1_S_PGTBL_BASE;
     uint32_t i;
     armv8_4k_tbl_pte_t *pte;
     armv8_4k_pg_pte_t *l3pte;
@@ -92,6 +106,7 @@ void el1_map_va(uintptr_t addr)
 void el1_handle_exception(uint64_t ec, uint64_t iss, uint64_t addr)
 {
     armv8_data_abort_iss_t dai = {.raw = iss};
+//    armv8_inst_abort_iss_t iai = {.raw = iss};
     switch (ec) {
     case 0x17:      /* SMC from aarch64 */
         switch (iss) {
@@ -112,6 +127,15 @@ void el1_handle_exception(uint64_t ec, uint64_t iss, uint64_t addr)
             printf("Unrecognized AArch64 SMC opcode: iss = %d\n", iss);
         }
         break;
+    case 0x20:
+        printf("Instruction abort at lower level: address = %0lx\n",
+               addr);
+        break;
+    case 0x21:
+        printf("Instruction abort at current level (EL3): address = %0lx\n",
+               addr);
+        el1_map_va(addr);
+        break;
     case 0x24:
         printf("Data abort (%s) at lower level: address = %0lx\n",
                dai.wnr ? "write" : "read", addr);
@@ -130,14 +154,19 @@ void el1_handle_exception(uint64_t ec, uint64_t iss, uint64_t addr)
 
 #define __smc(_op)      \
     asm volatile (      \
-        "smc #0 \n"  \
+        "smc #2 \n"  \
     )
 
 void el1_start()
 {
-    printf("Entered el1_start\n");
+    int i = 0;
+
+    while (1) {
+        printf("%d: Entered %s el1_start\n", i, SECURE_STATE);
 
-    __smc(SMC_YIELD);
+        __smc(SMC_YIELD);
+        i++;
+    }
 
     return;
 }
diff --git a/aarch64/el1_common/el1_init.S b/aarch64/el1_common/el1_init.S
index 252dacd..fcc059a 100644
--- a/aarch64/el1_common/el1_init.S
+++ b/aarch64/el1_common/el1_init.S
@@ -1,4 +1,5 @@
 #include "el1.h"
+#include "common_defs.h"
 
 .section .init
 .align 12
diff --git a/aarch64/el1_ns/el1.h b/aarch64/el1_ns/el1.h
index a9e7ff3..394bb28 100644
--- a/aarch64/el1_ns/el1.h
+++ b/aarch64/el1_ns/el1.h
@@ -15,4 +15,5 @@
 #define EL1_PGTBL_BASE EL1_NS_PGTBL_BASE
 #define EL1_PGTBL_SIZE EL1_NS_PGTBL_SIZE
 
+#define SECURE_STATE "non-secure"
 #endif
diff --git a/aarch64/el1_s/el1.h b/aarch64/el1_s/el1.h
index 493f9e4..9c086cc 100644
--- a/aarch64/el1_s/el1.h
+++ b/aarch64/el1_s/el1.h
@@ -15,4 +15,5 @@
 #define EL1_PGTBL_BASE EL1_S_PGTBL_BASE
 #define EL1_PGTBL_SIZE EL1_S_PGTBL_SIZE
 
+#define SECURE_STATE "secure"
 #endif
diff --git a/aarch64/el3/el3.c b/aarch64/el3/el3.c
index 7d7e005..7d93b9c 100644
--- a/aarch64/el3/el3.c
+++ b/aarch64/el3/el3.c
@@ -10,10 +10,32 @@
 typedef struct state_buf {
     uintptr_t elr_el3;
     uintptr_t spsr_el3;
-    uintptr_t sp_el0;
-    uintptr_t sp_el1;
     uintptr_t elr_el1;
+    uintptr_t spsr_el1;
     uintptr_t esr_el1;
+    uintptr_t sp_el1;
+    uintptr_t spsr_abt;
+    uintptr_t spsr_und;
+    uintptr_t spsr_irq;
+    uintptr_t spsr_fiq;
+    uintptr_t sctlr_el1;
+    uintptr_t actlr_el1;
+    uintptr_t cpacr_el1;
+    uintptr_t csselr_el1;
+    uintptr_t ttbr0_el1;
+    uintptr_t ttbr1_el1;
+    uintptr_t tcr_el1;
+    uintptr_t vbar_el1;
+    uintptr_t mair_el1;
+    uintptr_t amair_el1;
+    uintptr_t tpidr_el0;
+    uintptr_t tpidr_el1;
+    uintptr_t tpidrro_el0;
+    uintptr_t contextidr_el1;
+    uintptr_t par_el1;
+    uintptr_t far_el1;
+    uintptr_t afsr0_el1;
+    uintptr_t afsr1_el1;
     uintptr_t x[27];        /* X4:X30 */
 } state_buf;
 
diff --git a/aarch64/el3/el3_exception.S b/aarch64/el3/el3_exception.S
index 80388c0..44bf7d5 100644
--- a/aarch64/el3/el3_exception.S
+++ b/aarch64/el3/el3_exception.S
@@ -1,3 +1,5 @@
+#include "common_defs.h"
+
 .section .vectors
 .align 12                   // Align to vector table size (0x800)
 .globl el3_vectors
diff --git a/aarch64/el3/el3_monitor_asm.S b/aarch64/el3/el3_monitor_asm.S
index a2a70c2..cbc79dd 100644
--- a/aarch64/el3/el3_monitor_asm.S
+++ b/aarch64/el3/el3_monitor_asm.S
@@ -9,7 +9,7 @@ monitor_init:
     adr x3, nsec_state
     stp x1, x4, [x3], #16        /* NS.ELR_EL3 = nsec_base */
     mov x0, #0
-    mov x1, #31
+    mov x1, #26+27
 zero_loop:
     sub x1, x1, #1
     str x0, [x2], #8
@@ -19,19 +19,38 @@ zero_loop:
     bl monitor_restore_state
     eret
 
+.macro SAVE_SYS_PAIR reg1, reg2
+    mrs x1, \reg1
+    mrs x2, \reg2
+    stp x1, x2, [x0], #16
+.endm
+
+.macro RESTORE_SYS_PAIR reg1, reg2
+    ldp x1, x2, [x0], #16
+    msr \reg1, x1
+    msr \reg2, x2
+.endm
+
+
 .globl monitor_save_state
 monitor_save_state:
     stp x0, x1, [sp, #-16]!     /* Save the input regs x0:x3 */
     stp x2, x3, [sp, #-16]!
-    mrs x1, elr_el3
-    mrs x2, spsr_el3
-    stp x1, x2, [x0], #16
-    mrs x1, sp_el0
-    mrs x2, sp_el1
-    stp x1, x2, [x0], #16
-    mrs x1, elr_el1
-    mrs x2, esr_el1
-    stp x1, x2, [x0], #16
+    SAVE_SYS_PAIR elr_el3, spsr_el3
+    SAVE_SYS_PAIR elr_el1, spsr_el1
+    SAVE_SYS_PAIR esr_el1, sp_el1
+//    SAVE_SYS_PAIR spsr_abt, spsr_und
+//    SAVE_SYS_PAIR spsr_irq, spsr_fiq
+    SAVE_SYS_PAIR sctlr_el1, actlr_el1
+    SAVE_SYS_PAIR cpacr_el1, csselr_el1
+    SAVE_SYS_PAIR ttbr0_el1, ttbr1_el1
+    SAVE_SYS_PAIR tcr_el1, vbar_el1
+    SAVE_SYS_PAIR mair_el1, amair_el1
+    SAVE_SYS_PAIR tpidr_el0, tpidr_el1
+    SAVE_SYS_PAIR tpidrro_el0, contextidr_el1
+//    SAVE_SYS_PAIR par_el1, far_el1
+//    SAVE_SYS_PAIR afsr0_el1, afsr1_el1
+
     stp x4, x5, [x0], #16
     stp x6, x7, [x0], #16
     stp x8, x9, [x0], #16
@@ -54,15 +73,21 @@ monitor_save_state:
 monitor_restore_state:
     str x30, [sp, #-8]!         /* Save the LR onto the stack for later */
     stp x0, x1, [sp, #-16]!
-    ldp x1, x2, [x0], #16
-    msr elr_el3, x1
-    msr spsr_el3, x2
-    ldp x1, x2, [x0], #16
-    msr sp_el0, x1
-    msr sp_el1, x2
-    ldp x1, x2, [x0], #16
-    msr elr_el1, x1
-    msr esr_el1, x2
+    RESTORE_SYS_PAIR elr_el3, spsr_el3
+    RESTORE_SYS_PAIR elr_el1, spsr_el1
+    RESTORE_SYS_PAIR esr_el1, sp_el1
+//    RESTORE_SYS_PAIR spsr_abt, spsr_und
+//    RESTORE_SYS_PAIR spsr_irq, spsr_fiq
+    RESTORE_SYS_PAIR sctlr_el1, actlr_el1
+    RESTORE_SYS_PAIR cpacr_el1, csselr_el1
+    RESTORE_SYS_PAIR ttbr0_el1, ttbr1_el1
+    RESTORE_SYS_PAIR tcr_el1, vbar_el1
+    RESTORE_SYS_PAIR mair_el1, amair_el1
+    RESTORE_SYS_PAIR tpidr_el0, tpidr_el1
+    RESTORE_SYS_PAIR tpidrro_el0, contextidr_el1
+//    RESTORE_SYS_PAIR par_el1, far_el1
+//    RESTORE_SYS_PAIR afsr0_el1, afsr1_el1
+
     ldp x4, x5, [x0], #16
     ldp x6, x7, [x0], #16
     ldp x8, x9, [x0], #16
@@ -83,6 +108,7 @@ monitor_restore_state:
 
 .globl monitor_switch
 monitor_switch:
+    str x30, [sp, #-8]!
     stp x0, x1, [sp, #-16]!     /* Save the input regs x0:x3 */
     mrs x1, scr_el3
     and x1, x1, #0x1
@@ -102,6 +128,7 @@ switch_state:
     eor x1, x1, #0x1
     msr scr_el3, x1
     ldp x0, x1, [sp], #16
-    eret
+    ldr x30, [sp], #8
+    ret
 
 .end
diff --git a/platform/virt/platform.h b/platform/virt/platform.h
index 7dc154a..827605b 100644
--- a/platform/virt/platform.h
+++ b/platform/virt/platform.h
@@ -20,8 +20,8 @@
 #define EL3_RAM_SIZE (512*1024)
 #define EL1_S_RAM_BASE (RAM_BASE+0x1000000)
 #define EL1_S_RAM_SIZE (512*1024)
-#define EL1_NS_RAM_BASE (RAM_BASE+0x8000000)
-#define EL1_NS_RAM_SIZE (RAM_SIZE/2)
+#define EL1_NS_RAM_BASE (RAM_BASE+0x4000000)
+#define EL1_NS_RAM_SIZE (512*1024)
 #define VA_SIZE 48
 #define TnSZ (64-VA_SIZE)
 
@@ -36,7 +36,7 @@
 #define EL1_NS_PGTBL_SIZE 0x40000
 #define EL1_NS_PGTBL_BASE EL1_NS_RAM_BASE+EL1_NS_RAM_SIZE-EL1_NS_PGTBL_SIZE
 #define EL1_NS_STACK_SIZE 0x40000
-#define EL1_NS_STACK_BASE 0xCF00000000
+#define EL1_NS_STACK_BASE 0xFFFFF000
 
 #define UART0_BASE 0x09000000