[rpi3]64bit platform support

arch/arm64/asm.S

@@ -83,5 +83,60 @@ FUNCTION(arm64_el3_to_el1)
 
     eret
 
+FUNCTION(arm64_elX_to_el1)
+    mrs x0, CurrentEL
+
+    cmp x0, #(0b01 << 2)
+    bne .notEL1
+    /* Already in EL1 */
+    ret 
+
+.notEL1:
+    cmp x0, #(0b10 << 2)
+    beq .inEL2
+
+
+    /* set EL2 to 64bit */
+    mrs x0, scr_el3
+    orr x0, x0, #(1<<10)
+    msr scr_el3, x0
+
+
+    adr x0, .Ltarget
+    msr elr_el3, x0
+
+    mov x0, #((0b1111 << 6) | (0b0101)) /* EL1h runlevel */
+    msr spsr_el3, x0
+    b   .confEL1
+
+.inEL2:
+    adr x0, .Ltarget
+    msr elr_el2, x0
+    mov x0, #((0b1111 << 6) | (0b0101)) /* EL1h runlevel */
+    msr spsr_el2, x0
+   
+
+
+.confEL1:
+    /* disable EL2 coprocessor traps */
+    mov x0, #0x33ff
+    msr cptr_el2, x0
+
+    /* set EL1 to 64bit */
+    mov x0, #(1<<31)
+    msr hcr_el2, x0
+
+    /* disable EL1 FPU traps */
+    mov x0, #(0b11<<20)
+    msr cpacr_el1, x0
+
+    /* set up the EL1 bounce interrupt */
+    mov x0, sp
+    msr sp_el1, x0
+
+    isb
+    eret
+
+
 .Ltarget:
     ret