diff --git a/Kernel/Arch/aarch64/Processor.cpp b/Kernel/Arch/aarch64/Processor.cpp index df80acb4d7..6e1b36b2d5 100644 --- a/Kernel/Arch/aarch64/Processor.cpp +++ b/Kernel/Arch/aarch64/Processor.cpp @@ -27,6 +27,53 @@ extern "C" void context_first_init(Thread* from_thread, Thread* to_thread) __att extern "C" void enter_thread_context(Thread* from_thread, Thread* to_thread) __attribute__((used)); Processor* g_current_processor; +READONLY_AFTER_INIT FPUState Processor::s_clean_fpu_state; + +static void store_fpu_state(FPUState* fpu_state) +{ + asm volatile( + "mov x0, %[fpu_state]\n" + "stp q0, q1, [x0, #(0 * 16)]\n" + "stp q2, q3, [x0, #(2 * 16)]\n" + "stp q4, q5, [x0, #(4 * 16)]\n" + "stp q6, q7, [x0, #(6 * 16)]\n" + "stp q8, q9, [x0, #(8 * 16)]\n" + "stp q10, q11, [x0, #(10 * 16)]\n" + "stp q12, q13, [x0, #(12 * 16)]\n" + "stp q14, q15, [x0, #(14 * 16)]\n" + "stp q16, q17, [x0, #(16 * 16)]\n" + "stp q18, q19, [x0, #(18 * 16)]\n" + "stp q20, q21, [x0, #(20 * 16)]\n" + "stp q22, q23, [x0, #(22 * 16)]\n" + "stp q24, q25, [x0, #(24 * 16)]\n" + "stp q26, q27, [x0, #(26 * 16)]\n" + "stp q28, q29, [x0, #(28 * 16)]\n" + "stp q30, q31, [x0, #(30 * 16)]\n" + "\n" ::[fpu_state] "r"(fpu_state)); +} + +static void load_fpu_state(FPUState* fpu_state) +{ + asm volatile( + "mov x0, %[fpu_state]\n" + "ldp q0, q1, [x0, #(0 * 16)]\n" + "ldp q2, q3, [x0, #(2 * 16)]\n" + "ldp q4, q5, [x0, #(4 * 16)]\n" + "ldp q6, q7, [x0, #(6 * 16)]\n" + "ldp q8, q9, [x0, #(8 * 16)]\n" + "ldp q10, q11, [x0, #(10 * 16)]\n" + "ldp q12, q13, [x0, #(12 * 16)]\n" + "ldp q14, q15, [x0, #(14 * 16)]\n" + "ldp q16, q17, [x0, #(16 * 16)]\n" + "ldp q18, q19, [x0, #(18 * 16)]\n" + "ldp q20, q21, [x0, #(20 * 16)]\n" + "ldp q22, q23, [x0, #(22 * 16)]\n" + "ldp q24, q25, [x0, #(24 * 16)]\n" + "ldp q26, q27, [x0, #(26 * 16)]\n" + "ldp q28, q29, [x0, #(28 * 16)]\n" + "ldp q30, q31, [x0, #(30 * 16)]\n" + "\n" ::[fpu_state] "r"(fpu_state)); +} void Processor::install(u32 cpu) { @@ -46,6 +93,8 @@ void Processor::initialize() dmesgln("CPU[{}]: Virtual address bit width: {}", m_cpu, m_virtual_address_bit_width); if (!has_feature(CPUFeature::RNG)) dmesgln("CPU[{}]: {} not detected, randomness will be poor", m_cpu, cpu_feature_to_description(CPUFeature::RNG)); + + store_fpu_state(&s_clean_fpu_state); } [[noreturn]] void Processor::halt() @@ -397,6 +446,8 @@ extern "C" void enter_thread_context(Thread* from_thread, Thread* to_thread) Processor::set_current_thread(*to_thread); + store_fpu_state(&from_thread->fpu_state()); + auto& from_regs = from_thread->regs(); auto& to_regs = to_thread->regs(); if (from_regs.ttbr0_el1 != to_regs.ttbr0_el1) @@ -407,6 +458,8 @@ extern "C" void enter_thread_context(Thread* from_thread, Thread* to_thread) auto in_critical = to_thread->saved_critical(); VERIFY(in_critical > 0); Processor::restore_critical(in_critical); + + load_fpu_state(&to_thread->fpu_state()); } StringView Processor::platform_string() diff --git a/Kernel/Arch/aarch64/Processor.h b/Kernel/Arch/aarch64/Processor.h index 68db2412d1..57b55152bf 100644 --- a/Kernel/Arch/aarch64/Processor.h +++ b/Kernel/Arch/aarch64/Processor.h @@ -213,8 +213,6 @@ public: ALWAYS_INLINE static FPUState const& clean_fpu_state() { - static FPUState s_clean_fpu_state {}; - dbgln("FIXME: Processor: Actually return correct FPUState."); return s_clean_fpu_state; } @@ -293,6 +291,8 @@ private: Thread* m_idle_thread; u32 m_in_critical { 0 }; + static FPUState s_clean_fpu_state; + // FIXME: Once there is code in place to differentiate IRQs from synchronous exceptions (syscalls), // this member should be incremented. Also this member shouldn't be a FlatPtr. FlatPtr m_in_irq { 0 };