Kernel/aarch64: Implement storing and restoring floating-point registers

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()

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 };