From 0f81fb03f2b03f93a64baa99381060b071f4058a Mon Sep 17 00:00:00 2001
From: konrad <konrad@serenityos.org>
Date: Sat, 7 Jan 2023 21:21:22 +0100
Subject: [PATCH] Kernel: Introduce stages in Aarch64 CPU initialization phase

Dropping to each exception level is now more explicit.
---
 Kernel/Arch/aarch64/ASM_wrapper.h  |  2 +-
 Kernel/Arch/aarch64/CPU.h          |  4 ++-
 Kernel/Arch/aarch64/Exceptions.cpp | 44 +++++++++++++++++++-----------
 Kernel/Arch/aarch64/Processor.cpp  | 11 +-------
 4 files changed, 33 insertions(+), 28 deletions(-)

diff --git a/Kernel/Arch/aarch64/ASM_wrapper.h b/Kernel/Arch/aarch64/ASM_wrapper.h
index f083f63f1b..7710cbb695 100644
--- a/Kernel/Arch/aarch64/ASM_wrapper.h
+++ b/Kernel/Arch/aarch64/ASM_wrapper.h
@@ -68,7 +68,7 @@ inline void wait_cycles(int n)
     }
 }
 
-inline void el1_vector_table_install(void* vector_table)
+inline void load_el1_vector_table(void* vector_table)
 {
     asm("msr VBAR_EL1, %[value]" ::[value] "r"(vector_table));
 }
diff --git a/Kernel/Arch/aarch64/CPU.h b/Kernel/Arch/aarch64/CPU.h
index 61a89b042c..ead124eaf4 100644
--- a/Kernel/Arch/aarch64/CPU.h
+++ b/Kernel/Arch/aarch64/CPU.h
@@ -6,9 +6,11 @@
 
 #pragma once
 
+#include <AK/Types.h>
+
 namespace Kernel {
 
-void drop_to_exception_level_1();
+void initialize_exceptions(u32 cpu);
 void init_page_tables();
 
 }
diff --git a/Kernel/Arch/aarch64/Exceptions.cpp b/Kernel/Arch/aarch64/Exceptions.cpp
index d3e402557e..2ef9a974f3 100644
--- a/Kernel/Arch/aarch64/Exceptions.cpp
+++ b/Kernel/Arch/aarch64/Exceptions.cpp
@@ -9,9 +9,11 @@
 #include <Kernel/Arch/aarch64/Registers.h>
 #include <Kernel/Panic.h>
 
+extern "C" uintptr_t vector_table_el1;
+
 namespace Kernel {
 
-static void drop_to_el2()
+static void drop_el3_to_el2()
 {
     Aarch64::SCR_EL3 secure_configuration_register_el3 = {};
 
@@ -40,7 +42,7 @@ static void drop_to_el2()
     Aarch64::Asm::enter_el2_from_el3();
 }
 
-static void drop_to_el1()
+static void drop_el2_to_el1()
 {
     Aarch64::HCR_EL2 hypervisor_configuration_register_el2 = {};
     hypervisor_configuration_register_el2.RW = 1; // EL1 to use 64-bit mode
@@ -64,7 +66,7 @@ static void drop_to_el1()
     Aarch64::Asm::enter_el1_from_el2();
 }
 
-static void set_up_el1()
+static void setup_el1()
 {
     Aarch64::SCTLR_EL1 system_control_register_el1 = Aarch64::SCTLR_EL1::reset_value();
 
@@ -78,24 +80,34 @@ static void set_up_el1()
     system_control_register_el1.A = 1;    // Enable memory access alignment check
 
     Aarch64::SCTLR_EL1::write(system_control_register_el1);
+
+    Aarch64::Asm::load_el1_vector_table(&vector_table_el1);
 }
 
-void drop_to_exception_level_1()
+void initialize_exceptions(u32 cpu)
 {
-    switch (Aarch64::Asm::get_current_exception_level()) {
-    case Aarch64::Asm::ExceptionLevel::EL3:
-        drop_to_el2();
-        [[fallthrough]];
-    case Aarch64::Asm::ExceptionLevel::EL2:
-        drop_to_el1();
-        [[fallthrough]];
-    case Aarch64::Asm::ExceptionLevel::EL1:
-        set_up_el1();
-        break;
-    default: {
-        PANIC("CPU booted in unsupported exception mode!");
+    auto base_exception_level = Aarch64::Asm::get_current_exception_level();
+
+    if (base_exception_level > Aarch64::Asm::ExceptionLevel::EL3) {
+        PANIC("CPU[{}]: Started in unknown EL{}", cpu, static_cast<u8>(base_exception_level));
+    } else if (base_exception_level < Aarch64::Asm::ExceptionLevel::EL1) {
+        PANIC("CPU[{}]: Started in unsupported EL{}", cpu, static_cast<u8>(base_exception_level));
+    } else {
+        dbgln("CPU[{}]: Started in EL{}", cpu, static_cast<u8>(base_exception_level));
     }
+
+    if (base_exception_level > Aarch64::Asm::ExceptionLevel::EL2) {
+        drop_el3_to_el2();
+        dbgln("CPU[{}]: Dropped to EL2", cpu);
     }
+
+    if (base_exception_level > Aarch64::Asm::ExceptionLevel::EL1) {
+        drop_el2_to_el1();
+        dbgln("CPU[{}]: Dropped to EL1", cpu);
+    }
+
+    setup_el1();
+    dbgln("CPU[{}]: Set up EL1", cpu);
 }
 
 }
diff --git a/Kernel/Arch/aarch64/Processor.cpp b/Kernel/Arch/aarch64/Processor.cpp
index 840f5fdd3b..6160ae4625 100644
--- a/Kernel/Arch/aarch64/Processor.cpp
+++ b/Kernel/Arch/aarch64/Processor.cpp
@@ -18,8 +18,6 @@
 #include <Kernel/Thread.h>
 #include <Kernel/Time/TimeManagement.h>
 
-extern "C" uintptr_t vector_table_el1;
-
 namespace Kernel {
 
 extern "C" void thread_context_first_enter(void);
@@ -33,14 +31,7 @@ void Processor::initialize(u32 cpu)
 {
     VERIFY(g_current_processor == nullptr);
 
-    auto current_exception_level = static_cast<u64>(Aarch64::Asm::get_current_exception_level());
-    dbgln("CPU{} started in: EL{}", cpu, current_exception_level);
-
-    dbgln("Drop CPU{} to EL1", cpu);
-    drop_to_exception_level_1();
-
-    // Load EL1 vector table
-    Aarch64::Asm::el1_vector_table_install(&vector_table_el1);
+    initialize_exceptions(cpu);
 
     g_current_processor = this;
 }