2021-05-08 12:54:07 +00:00
|
|
|
/* SPDX-License-Identifier: MIT */
|
|
|
|
|
2021-09-15 13:09:07 +00:00
|
|
|
#include "gxf.h"
|
2021-09-21 11:29:19 +00:00
|
|
|
#include "cpu_regs.h"
|
|
|
|
#include "exception.h"
|
2021-05-08 12:54:07 +00:00
|
|
|
|
|
|
|
#define genter .long 0x00201420
|
|
|
|
#define gexit .long 0x00201400
|
|
|
|
|
2021-09-15 13:09:07 +00:00
|
|
|
.extern gl1_stack
|
|
|
|
.extern gl2_stack
|
|
|
|
|
2021-05-08 12:54:07 +00:00
|
|
|
.global gxf_init
|
|
|
|
.type gxf_init, @function
|
|
|
|
gxf_init:
|
|
|
|
str x30, [sp, #-16]!
|
|
|
|
mov x0, 1
|
|
|
|
msr SYS_IMP_APL_SPRR_CONFIG_EL1, x0
|
|
|
|
isb
|
|
|
|
msr SYS_IMP_APL_GXF_CONFIG_EL1, x0
|
|
|
|
isb
|
|
|
|
ldr x0, =_gxf_setup
|
|
|
|
msr SYS_IMP_APL_GXF_ENTER_EL1, x0
|
|
|
|
isb
|
|
|
|
genter
|
|
|
|
msr SYS_IMP_APL_GXF_CONFIG_EL1, xzr
|
|
|
|
isb
|
|
|
|
msr SYS_IMP_APL_SPRR_CONFIG_EL1, xzr
|
|
|
|
isb
|
|
|
|
ldr x30, [sp], #16
|
|
|
|
ret
|
|
|
|
|
|
|
|
.globl gxf_enter
|
|
|
|
.type gxf_enter, @function
|
|
|
|
gxf_enter:
|
|
|
|
genter
|
2021-05-27 12:11:14 +00:00
|
|
|
ret
|
2021-05-08 12:54:07 +00:00
|
|
|
|
|
|
|
_gxf_setup:
|
2021-09-15 13:09:07 +00:00
|
|
|
mrs x2, TPIDR_EL1
|
|
|
|
mrs x4, CurrentEL
|
|
|
|
cmp x4, #8
|
|
|
|
bne 1f
|
|
|
|
mrs x2, TPIDR_EL2
|
|
|
|
1:
|
|
|
|
add x2, x2, 1
|
|
|
|
|
|
|
|
ldr x0, =gl2_stack
|
|
|
|
ldr x1, =GL_STACK_SIZE
|
|
|
|
mul x1, x1, x2
|
|
|
|
add x0, x0, x1
|
|
|
|
|
2021-05-08 12:54:07 +00:00
|
|
|
mov sp, x0
|
|
|
|
ldr x1, =_gxf_vectors
|
|
|
|
ldr x2, =_gxf_exc_sync
|
|
|
|
ldr x3, =_gxf_entry
|
|
|
|
msr SYS_IMP_APL_VBAR_GL1, x1
|
|
|
|
msr SYS_IMP_APL_GXF_ABORT_EL1, x2
|
|
|
|
msr SYS_IMP_APL_GXF_ENTER_EL1, x3
|
|
|
|
|
|
|
|
mrs x4, CurrentEL
|
|
|
|
cmp x4, #8
|
|
|
|
bne 1f
|
|
|
|
|
2021-09-15 13:09:07 +00:00
|
|
|
mrs x2, TPIDR_EL2
|
|
|
|
ldr x0, =gl1_stack
|
|
|
|
ldr x1, =GL_STACK_SIZE
|
|
|
|
mul x1, x1, x2
|
|
|
|
add x0, x0, x1
|
|
|
|
|
2021-05-08 12:54:07 +00:00
|
|
|
msr SYS_IMP_APL_SP_GL12, x0
|
|
|
|
msr SYS_IMP_APL_VBAR_GL12, x1
|
|
|
|
msr SYS_IMP_APL_GXF_ABORT_EL12, x2
|
|
|
|
msr SYS_IMP_APL_GXF_ENTER_EL12, x3
|
|
|
|
|
2021-09-15 13:09:07 +00:00
|
|
|
1:
|
2021-05-08 12:54:07 +00:00
|
|
|
isb
|
|
|
|
gexit
|
|
|
|
|
|
|
|
_gxf_entry:
|
|
|
|
stp x29, x30, [sp, #-16]!
|
|
|
|
stp x23, x24, [sp, #-16]!
|
|
|
|
stp x21, x22, [sp, #-16]!
|
|
|
|
stp x19, x20, [sp, #-16]!
|
|
|
|
|
|
|
|
// these registers would be overwritten by each exception happening in GL1/2
|
|
|
|
// but we need them to gexit correctly again
|
|
|
|
mrs x20, SYS_IMP_APL_SPSR_GL1
|
|
|
|
mrs x21, SYS_IMP_APL_ASPSR_GL1
|
|
|
|
mrs x22, SYS_IMP_APL_ESR_GL1
|
|
|
|
mrs x23, SYS_IMP_APL_ELR_GL1
|
|
|
|
mrs x24, SYS_IMP_APL_FAR_GL1
|
|
|
|
|
|
|
|
mov x5, x0
|
|
|
|
mov x0, x1
|
|
|
|
mov x1, x2
|
|
|
|
mov x2, x3
|
|
|
|
mov x3, x4
|
|
|
|
|
|
|
|
blr x5
|
|
|
|
|
|
|
|
msr SYS_IMP_APL_SPSR_GL1, x20
|
|
|
|
msr SYS_IMP_APL_ASPSR_GL1, x21
|
|
|
|
msr SYS_IMP_APL_ESR_GL1, x22
|
|
|
|
msr SYS_IMP_APL_ELR_GL1, x23
|
|
|
|
msr SYS_IMP_APL_FAR_GL1, x24
|
|
|
|
|
|
|
|
ldp x19, x20, [sp], #16
|
|
|
|
ldp x21, x22, [sp], #16
|
|
|
|
ldp x23, x24, [sp], #16
|
|
|
|
ldp x29, x30, [sp], #16
|
|
|
|
|
|
|
|
isb
|
|
|
|
gexit
|
|
|
|
|
|
|
|
.align 11
|
|
|
|
_gxf_vectors:
|
|
|
|
mov x9, '0'
|
|
|
|
b _gxf_exc_unk
|
|
|
|
.align 7
|
|
|
|
mov x9, '1'
|
|
|
|
b _gxf_exc_unk
|
|
|
|
.align 7
|
|
|
|
mov x9, '2'
|
|
|
|
b _gxf_exc_unk
|
|
|
|
.align 7
|
|
|
|
mov x9, '3'
|
|
|
|
b _gxf_exc_unk
|
|
|
|
.align 7
|
|
|
|
b _gxf_exc_sync
|
|
|
|
.align 7
|
|
|
|
mov x9, '5'
|
|
|
|
b _gxf_exc_unk
|
|
|
|
.align 7
|
|
|
|
mov x9, '6'
|
|
|
|
b _gxf_exc_unk
|
|
|
|
.align 7
|
|
|
|
b _gxf_serr
|
|
|
|
.align 7
|
|
|
|
b _gxf_exc_sync
|
|
|
|
.align 7
|
|
|
|
mov x9, '9'
|
|
|
|
b _gxf_exc_unk
|
|
|
|
.align 7
|
|
|
|
mov x9, 'a'
|
|
|
|
b _gxf_exc_unk
|
|
|
|
.align 7
|
|
|
|
b _gxf_serr
|
|
|
|
.align 7
|
|
|
|
mov x9, 'c'
|
|
|
|
b _gxf_exc_unk
|
|
|
|
.align 7
|
|
|
|
mov x9, 'd'
|
|
|
|
b _gxf_exc_unk
|
|
|
|
.align 7
|
|
|
|
mov x9, 'e'
|
|
|
|
b _gxf_exc_unk
|
|
|
|
.align 7
|
|
|
|
mov x9, 'f'
|
|
|
|
b _gxf_exc_unk
|
|
|
|
.align 7
|
|
|
|
|
|
|
|
_gxf_exc_sync:
|
2021-05-27 12:11:14 +00:00
|
|
|
msr pan, #0
|
2021-09-21 11:29:19 +00:00
|
|
|
sub sp, sp, #(SIZEOF_EXC_INFO - 32 * 8)
|
2021-05-08 12:54:07 +00:00
|
|
|
str x30, [sp, #-16]!
|
|
|
|
bl _gxf_exc_entry
|
|
|
|
bl exc_sync
|
|
|
|
b _gxf_exc_return
|
|
|
|
|
|
|
|
_gxf_serr:
|
2021-05-27 12:11:14 +00:00
|
|
|
msr pan, #0
|
2021-09-21 11:29:19 +00:00
|
|
|
sub sp, sp, #(SIZEOF_EXC_INFO - 32 * 8)
|
2021-05-08 12:54:07 +00:00
|
|
|
str x30, [sp, #-16]!
|
|
|
|
bl _gxf_exc_entry
|
|
|
|
bl exc_serr
|
|
|
|
b _gxf_exc_return
|
|
|
|
|
|
|
|
_gxf_exc_entry:
|
|
|
|
stp x28, x29, [sp, #-16]!
|
|
|
|
stp x26, x27, [sp, #-16]!
|
|
|
|
stp x24, x25, [sp, #-16]!
|
|
|
|
stp x22, x23, [sp, #-16]!
|
|
|
|
stp x20, x21, [sp, #-16]!
|
|
|
|
stp x18, x19, [sp, #-16]!
|
|
|
|
stp x16, x17, [sp, #-16]!
|
|
|
|
stp x14, x15, [sp, #-16]!
|
|
|
|
stp x12, x13, [sp, #-16]!
|
|
|
|
stp x10, x11, [sp, #-16]!
|
|
|
|
stp x8, x9, [sp, #-16]!
|
|
|
|
stp x6, x7, [sp, #-16]!
|
|
|
|
stp x4, x5, [sp, #-16]!
|
|
|
|
stp x2, x3, [sp, #-16]!
|
|
|
|
stp x0, x1, [sp, #-16]!
|
|
|
|
|
|
|
|
mov x0, sp
|
|
|
|
|
|
|
|
mrs x1, SYS_IMP_APL_SPSR_GL1
|
|
|
|
msr SPSR_EL1, x1
|
|
|
|
mrs x1, SYS_IMP_APL_ELR_GL1
|
|
|
|
msr ELR_EL1, x1
|
|
|
|
mrs x1, SYS_IMP_APL_ESR_GL1
|
|
|
|
msr ESR_EL1, x1
|
|
|
|
mrs x1, SYS_IMP_APL_FAR_GL1
|
|
|
|
msr FAR_EL1, x1
|
|
|
|
|
|
|
|
ret
|
|
|
|
|
|
|
|
_gxf_exc_return:
|
|
|
|
mrs x0, SPSR_EL1
|
|
|
|
msr SYS_IMP_APL_SPSR_GL1, x0
|
|
|
|
mrs x0, ELR_EL1
|
|
|
|
msr SYS_IMP_APL_ELR_GL1, x0
|
|
|
|
|
|
|
|
ldp x0, x1, [sp], #16
|
|
|
|
ldp x2, x3, [sp], #16
|
|
|
|
ldp x4, x5, [sp], #16
|
|
|
|
ldp x6, x7, [sp], #16
|
|
|
|
ldp x8, x9, [sp], #16
|
|
|
|
ldp x10, x11, [sp], #16
|
|
|
|
ldp x12, x13, [sp], #16
|
|
|
|
ldp x14, x15, [sp], #16
|
|
|
|
ldp x16, x17, [sp], #16
|
|
|
|
ldp x18, x19, [sp], #16
|
|
|
|
ldp x20, x21, [sp], #16
|
|
|
|
ldp x22, x23, [sp], #16
|
|
|
|
ldp x24, x25, [sp], #16
|
|
|
|
ldp x26, x27, [sp], #16
|
|
|
|
ldp x28, x29, [sp], #16
|
|
|
|
ldr x30, [sp], #16
|
2021-09-21 11:29:19 +00:00
|
|
|
|
|
|
|
add sp, sp, #(SIZEOF_EXC_INFO - 32 * 8)
|
|
|
|
|
2021-05-08 12:54:07 +00:00
|
|
|
isb
|
|
|
|
|
|
|
|
gexit
|
|
|
|
|
|
|
|
_gxf_exc_unk:
|
2021-05-27 12:11:14 +00:00
|
|
|
msr pan, #0
|
2021-05-08 12:54:07 +00:00
|
|
|
mov w0, 0xd /* '\r', clang compat */
|
|
|
|
bl debug_putc
|
|
|
|
mov w0, '\n'
|
|
|
|
bl debug_putc
|
|
|
|
mov w0, '!'
|
|
|
|
bl debug_putc
|
|
|
|
mov w0, 'G'
|
|
|
|
bl debug_putc
|
|
|
|
mov w0, 'L'
|
|
|
|
bl debug_putc
|
|
|
|
mov w0, 'E'
|
|
|
|
bl debug_putc
|
|
|
|
mov w0, 'X'
|
|
|
|
bl debug_putc
|
|
|
|
mov w0, 'C'
|
|
|
|
bl debug_putc
|
|
|
|
mov w0, ':'
|
|
|
|
bl debug_putc
|
|
|
|
mov w0, w9
|
|
|
|
bl debug_putc
|
|
|
|
mov w0, '!'
|
|
|
|
bl debug_putc
|
|
|
|
mov w0, 0xd /* '\r', clang compat */
|
|
|
|
bl debug_putc
|
|
|
|
mov w0, '\n'
|
|
|
|
bl debug_putc
|
|
|
|
b reboot
|