feat: support save and restore fp regs for loongarch (#14)

* feat: support save and restore fp regs for loongarch

* fix: Fix format

Author: MF-B

polyhal/Cargo.toml

@@ -13,6 +13,7 @@ repository = { workspace = true }
 trap = []
 boot = []
 logger = []
+fp_simd = []
 
 graphic = []
 

polyhal/src/components/kcontext/loongarch64.rs

@@ -1,3 +1,4 @@
+use core::mem::offset_of;
 use core::{
     arch::naked_asm,
     ops::{Index, IndexMut},
@@ -47,6 +48,155 @@ macro_rules! restore_callee_regs {
     };
 }
 
+#[cfg(feature = "fp_simd")]
+macro_rules! save_fp_regs {
+    () => {
+        "
+            // LoongArch64 specific floating point macros
+            fst.d $f0,  $a0, 0*8
+            fst.d $f1,  $a0, 1*8
+            fst.d $f2,  $a0, 2*8
+            fst.d $f3,  $a0, 3*8
+            fst.d $f4,  $a0, 4*8
+            fst.d $f5,  $a0, 5*8
+            fst.d $f6,  $a0, 6*8
+            fst.d $f7,  $a0, 7*8
+            fst.d $f8,  $a0, 8*8
+            fst.d $f9,  $a0, 9*8
+            fst.d $f10, $a0, 10*8
+            fst.d $f11, $a0, 11*8
+            fst.d $f12, $a0, 12*8
+            fst.d $f13, $a0, 13*8
+            fst.d $f14, $a0, 14*8
+            fst.d $f15, $a0, 15*8
+            fst.d $f16, $a0, 16*8
+            fst.d $f17, $a0, 17*8
+            fst.d $f18, $a0, 18*8
+            fst.d $f19, $a0, 19*8
+            fst.d $f20, $a0, 20*8
+            fst.d $f21, $a0, 21*8
+            fst.d $f22, $a0, 22*8
+            fst.d $f23, $a0, 23*8
+            fst.d $f24, $a0, 24*8
+            fst.d $f25, $a0, 25*8
+            fst.d $f26, $a0, 26*8
+            fst.d $f27, $a0, 27*8
+            fst.d $f28, $a0, 28*8
+            fst.d $f29, $a0, 29*8
+            fst.d $f30, $a0, 30*8
+            fst.d $f31, $a0, 31*8
+
+            addi.d $t8, $a0, 32*8
+
+            // SAVE_FCC
+            movcf2gr    $t0, $fcc0
+            move        $t1, $t0
+            movcf2gr    $t0, $fcc1
+            bstrins.d   $t1, $t0, 15, 8
+            movcf2gr    $t0, $fcc2
+            bstrins.d   $t1, $t0, 23, 16
+            movcf2gr    $t0, $fcc3
+            bstrins.d   $t1, $t0, 31, 24
+            movcf2gr    $t0, $fcc4
+            bstrins.d   $t1, $t0, 39, 32
+            movcf2gr    $t0, $fcc5
+            bstrins.d   $t1, $t0, 47, 40
+            movcf2gr    $t0, $fcc6
+            bstrins.d   $t1, $t0, 55, 48
+            movcf2gr    $t0, $fcc7
+            bstrins.d   $t1, $t0, 63, 56
+            st.d        $t1, $t8, 0
+
+            addi.d $t8, $a0, 33*8
+
+            // SAVE_FCSR
+            movfcsr2gr  $t0, $fcsr0
+            st.w        $t0, $t8, 0
+        "
+    };
+}
+
+#[cfg(feature = "fp_simd")]
+macro_rules! restore_fp_regs {
+    () => {
+        "
+            // LoongArch64 specific floating point macros
+            fld.d $f0,  $a0, 0*8
+            fld.d $f1,  $a0, 1*8
+            fld.d $f2,  $a0, 2*8
+            fld.d $f3,  $a0, 3*8
+            fld.d $f4,  $a0, 4*8
+            fld.d $f5,  $a0, 5*8
+            fld.d $f6,  $a0, 6*8
+            fld.d $f7,  $a0, 7*8
+            fld.d $f8,  $a0, 8*8
+            fld.d $f9,  $a0, 9*8
+            fld.d $f10, $a0, 10*8
+            fld.d $f11, $a0, 11*8
+            fld.d $f12, $a0, 12*8
+            fld.d $f13, $a0, 13*8
+            fld.d $f14, $a0, 14*8
+            fld.d $f15, $a0, 15*8
+            fld.d $f16, $a0, 16*8
+            fld.d $f17, $a0, 17*8
+            fld.d $f18, $a0, 18*8
+            fld.d $f19, $a0, 19*8
+            fld.d $f20, $a0, 20*8
+            fld.d $f21, $a0, 21*8
+            fld.d $f22, $a0, 22*8
+            fld.d $f23, $a0, 23*8
+            fld.d $f24, $a0, 24*8
+            fld.d $f25, $a0, 25*8
+            fld.d $f26, $a0, 26*8
+            fld.d $f27, $a0, 27*8
+            fld.d $f28, $a0, 28*8
+            fld.d $f29, $a0, 29*8
+            fld.d $f30, $a0, 30*8
+            fld.d $f31, $a0, 31*8
+
+            addi.d $t8, $a0, 32*8
+
+            // RESTORE_FCC
+            ld.d        $t0, $t8, 0
+            bstrpick.d  $t1, $t0, 7, 0
+            movgr2cf    $fcc0, $t1
+            bstrpick.d  $t1, $t0, 15, 8
+            movgr2cf    $fcc1, $t1
+            bstrpick.d  $t1, $t0, 23, 16
+            movgr2cf    $fcc2, $t1
+            bstrpick.d  $t1, $t0, 31, 24
+            movgr2cf    $fcc3, $t1
+            bstrpick.d  $t1, $t0, 39, 32
+            movgr2cf    $fcc4, $t1
+            bstrpick.d  $t1, $t0, 47, 40
+            movgr2cf    $fcc5, $t1
+            bstrpick.d  $t1, $t0, 55, 48
+            movgr2cf    $fcc6, $t1
+            bstrpick.d  $t1, $t0, 63, 56
+            movgr2cf    $fcc7, $t1
+
+            addi.d $t8, $a0, 33*8
+
+            // RESTORE_FCSR
+            ld.w        $t0, $t8, 0
+            movgr2fcsr  $fcsr0, $t0
+        "
+    };
+}
+
+/// Floating-point registers of LoongArch64.
+#[cfg(feature = "fp_simd")]
+#[repr(C)]
+#[derive(Debug, Default, Clone, Copy)]
+pub struct FpStatus {
+    /// Floating-point registers (f0-f31)
+    pub fp: [u64; 32],
+    /// Floating-point Condition Code register
+    pub fcc: [u8; 8],
+    /// Floating-point Control and Status register
+    pub fcsr: usize,
+}
+
 /// Kernel Context
 ///
 /// Kernel Context is used to switch context between kernel task.
@@ -61,6 +211,9 @@ pub struct KContext {
     _sregs: [usize; 10],
     /// Kernel Program Counter, Will return to this address.
     kpc: usize,
+    #[cfg(feature = "fp_simd")]
+    /// Floating Point Status
+    fp_status: FpStatus,
 }
 
 impl KContext {
@@ -71,6 +224,8 @@ impl KContext {
             ktp: 0,
             _sregs: [0; 10],
             kpc: 0,
+            #[cfg(feature = "fp_simd")]
+            fp_status: FpStatus::default(),
         }
     }
 }
@@ -180,6 +335,29 @@ unsafe extern "C" fn context_switch_pt_impl(
     )
 }
 
+#[cfg(feature = "fp_simd")]
+#[naked]
+pub unsafe extern "C" fn save_fp_regs(from: *mut KContext) {
+    naked_asm!(
+        // Save floating point registers.
+        "addi.d    $a0, $a0, {fp_offset}",
+        save_fp_regs!(),
+        "ret",
+        fp_offset = const offset_of!(KContext, fp_status)
+    )
+}
+
+#[cfg(feature = "fp_simd")]
+#[naked]
+pub unsafe extern "C" fn restore_fp_regs(to: *const KContext) {
+    naked_asm!(
+        "addi.d    $a0, $a0, {fp_offset}",
+        restore_fp_regs!(),
+        "ret",
+        fp_offset = const offset_of!(KContext, fp_status)
+    )
+}
+
 #[naked]
 pub extern "C" fn read_current_tp() -> usize {
     unsafe {

polyhal/src/components/kcontext/mod.rs

@@ -20,3 +20,7 @@ pub enum KContextArgs {
 }
 
 pub_use_arch!(context_switch, context_switch_pt);
+
+#[cfg(feature = "fp_simd")]
+#[cfg(target_arch = "loongarch64")]
+pub_use_arch!(save_fp_regs, restore_fp_regs);