From dd82b939699fb0b298ccbfd66b84ef0d7a78a53b Mon Sep 17 00:00:00 2001 From: Lunaixsky Date: Wed, 25 Dec 2024 15:57:03 +0000 Subject: [PATCH] add el1 transfer at the boot stage in case higher EL is implemented fix up the ICC_SRE_ELx setting in case of presence of both EL2 and 3 --- lunaix-os/arch/aarch64/boot/init.c | 66 +++++++++++++++++++ lunaix-os/arch/aarch64/boot/start.S | 18 +++++ lunaix-os/arch/aarch64/includes/asm/aa64.h | 1 + .../arch/aarch64/includes/asm/aa64_msrs.h | 6 ++ .../arch/aarch64/includes/asm/aa64_spsr.h | 7 +- 5 files changed, 97 insertions(+), 1 deletion(-) diff --git a/lunaix-os/arch/aarch64/boot/init.c b/lunaix-os/arch/aarch64/boot/init.c index d10675e..42aba1f 100644 --- a/lunaix-os/arch/aarch64/boot/init.c +++ b/lunaix-os/arch/aarch64/boot/init.c @@ -1,5 +1,6 @@ #include #include +#include #include "init.h" @@ -84,6 +85,71 @@ extract_dtb_bootinfo(ptr_t dtb, struct boot_handoff* handoff) // TODO extract /memory, /reserved-memories from dtb } +static inline void +setup_gic_sysreg() +{ + int el; + bool has_el2; + u64_t pfr; + + pfr = read_sysreg(ID_AA64PFR0_EL1); + el = read_sysreg(CurrentEL) >> 2; + + // Arm A-Profile: D19.2.69, check for GIC sysreg avaliability + + if (!BITS_GET(pfr, BITFIELD(27, 24))) { + return; + } + + has_el2 = !!BITS_GET(pfr, BITFIELD(11, 8)); + + // GIC spec (v3,v4): 12.1.7, table 12-2 + // GIC spec (v3,v4): ICC_SRE_ELx accessing + // Arm A-Profile: R_PCDTX, PSTATE.EL reset value + + if (el == 3) { + sysreg_flagging(ICC_SRE_EL3, ICC_SRE_SRE, 0); + } + + if (has_el2) { + // el must be >= 2 + sysreg_flagging(ICC_SRE_EL2, ICC_SRE_SRE, 0); + } + + sysreg_flagging(ICC_SRE_EL1, ICC_SRE_SRE, 0); +} + +void +aarch64_pre_el1_init() +{ + setup_gic_sysreg(); +} + +bool +aarch64_prepare_el1_transfer() +{ + ptr_t spsr; + int el; + + el = read_sysreg(CurrentEL) >> 2; + + if (el == 1) { + return false; + } + + spsr = SPSR_AllInt | SPSR_I | SPSR_F | SPSR_SP; + spsr = BITS_SET(spsr, SPSR_EL, 1); + + if (el == 2) { + set_sysreg(SPSR_EL2, spsr); + } + else { + set_sysreg(SPSR_EL3, spsr); + } + + return true; +} + struct boot_handoff* aarch64_init(ptr_t dtb) { diff --git a/lunaix-os/arch/aarch64/boot/start.S b/lunaix-os/arch/aarch64/boot/start.S index 13878da..05787b6 100644 --- a/lunaix-os/arch/aarch64/boot/start.S +++ b/lunaix-os/arch/aarch64/boot/start.S @@ -1,3 +1,5 @@ +#include + .section .boot.data .align 4 stack_end: @@ -19,6 +21,22 @@ mov sp, x4 mov fp, xzr + mov x19, x0 + bl aarch64_pre_el1_init + + bl aarch64_prepare_el1_transfer + cbz x0, 1f + + /* transfer to EL1 if we are currently not in */ + adr x0, stack_top + msr SP_EL1, x0 /* EL1 stack */ + + adr x0, 1f + msr elr, x0 /* PC to resume at EL1, elr aliased to ELR_ELx */ + + eret + 1: + mov x0, x19 adr x4, aarch64_init bl x4 diff --git a/lunaix-os/arch/aarch64/includes/asm/aa64.h b/lunaix-os/arch/aarch64/includes/asm/aa64.h index d9e8d9a..d3e05b1 100644 --- a/lunaix-os/arch/aarch64/includes/asm/aa64.h +++ b/lunaix-os/arch/aarch64/includes/asm/aa64.h @@ -5,5 +5,6 @@ #include "aa64_mmu.h" #include "aa64_msrs.h" #include "aa64_sysinst.h" +#include "aa64_gic.h" #endif /* __LUNAIX_AA64_H */ diff --git a/lunaix-os/arch/aarch64/includes/asm/aa64_msrs.h b/lunaix-os/arch/aarch64/includes/asm/aa64_msrs.h index f1da5f2..546a241 100644 --- a/lunaix-os/arch/aarch64/includes/asm/aa64_msrs.h +++ b/lunaix-os/arch/aarch64/includes/asm/aa64_msrs.h @@ -11,12 +11,18 @@ #define CurrentEL __sr_encode(3, 0, 4, 2, 2) #define ELR_E1 __sr_encode(3, 0, 4, 0, 1) #define SPSel __sr_encode(3, 0, 4, 2, 0) + #define SPSR_EL1 __sr_encode(3, 0, 4, 0, 0) +#define SPSR_EL2 __sr_encode(3, 4, 4, 0, 0) +#define SPSR_EL3 __sr_encode(3, 6, 4, 0, 0) + #define DAIF_EL1 __sr_encode(3, 3, 4, 2, 1) #define ALLINT_EL1 __sr_encode(3, 0, 4, 3, 0) #define SP_EL0 __sr_encode(3, 0, 4, 1, 0) #define SP_EL1 __sr_encode(3, 4, 4, 1, 0) +#define ID_AA64PFR0_EL1 __sr_encode(3, 0, 0, 4, 0) + #ifndef __ASM__ #define read_sysreg(reg) \ ({ unsigned long _x; \ diff --git a/lunaix-os/arch/aarch64/includes/asm/aa64_spsr.h b/lunaix-os/arch/aarch64/includes/asm/aa64_spsr.h index 8b5b46f..04482b2 100644 --- a/lunaix-os/arch/aarch64/includes/asm/aa64_spsr.h +++ b/lunaix-os/arch/aarch64/includes/asm/aa64_spsr.h @@ -5,7 +5,12 @@ #include #define SPSR_EL BITFIELD(3, 2) -#define SPSR_SP BIT(0) + +#define SPSR_SP BITFLAG(0) +#define SPSR_I BITFLAG(7) +#define SPSR_F BITFLAG(6) +#define SPSR_I BITFLAG(7) +#define SPSR_AllInt BITFLAG(13) static inline bool spsr_from_el0(reg_t spsr) -- 2.27.0