[aarch64] Fix stack pointer misalignment

sp can become misaligned from 16 byte boundry because context frame
is not a multiple of 16 bytes.

arch/arm64/asm.S

@@ -32,7 +32,10 @@ FUNCTION(arm64_context_switch)
     push x22, x23
     push x20, x21
     push x18, x19
-    str  x30, [sp,#-8]!
+    mrs  x18, tpidr_el0
+    mrs  x19, tpidrro_el0
+    push x18, x19
+    push x30, xzr
 
     /* save old sp */
     mov  x15, sp
@@ -42,7 +45,10 @@ FUNCTION(arm64_context_switch)
     mov  sp, x1
 
     /* restore new frame */
-    ldr  x30, [sp], #8
+    pop  x30, xzr
+    pop  x18, x19
+    msr  tpidr_el0, x18
+    msr  tpidrro_el0, x19
     pop  x18, x19
     pop  x20, x21
     pop  x22, x23
@@ -114,7 +120,7 @@ FUNCTION(arm64_elX_to_el1)
     msr elr_el2, x4
     mov x4, #((0b1111 << 6) | (0b0101)) /* EL1h runlevel */
     msr spsr_el2, x4
-   
+
 
 
 .confEL1:

arch/arm64/start.S

@@ -39,7 +39,9 @@ arm_reset:
     mrs     tmp, sctlr_el1
     orr     tmp, tmp, #(1<<12) /* Enable icache */
     orr     tmp, tmp, #(1<<2)  /* Enable dcache/ucache */
-    bic     tmp, tmp, #(1<<3)  /* Disable Stack Alignment Check */ /* TODO: don't use unaligned stacks */
+    orr     tmp, tmp, #(1<<3)  /* Enable Stack Alignment Check EL1 */
+    orr     tmp, tmp, #(1<<4)  /* Enable Stack Alignment Check EL0 */
+    bic     tmp, tmp, #(1<<1)  /* Disable Alignment Checking for EL1 EL0 */
     msr     sctlr_el1, tmp
 
     /* set up the mmu according to mmu_initial_mappings */

arch/arm64/thread.c

@@ -32,6 +32,9 @@
 
 struct context_switch_frame {
     vaddr_t lr;
+    vaddr_t pad;                // Padding to keep frame size a multiple of
+    vaddr_t tpidr_el0;          //  sp alignment requirements (16 bytes)
+    vaddr_t tpidrro_el0;
     vaddr_t r18;
     vaddr_t r19;
     vaddr_t r20;