Implement save/restore_fp_state for RISC-V
authorAndrew Waterman <waterman@cs.berkeley.edu>
Tue, 23 Apr 2013 10:58:02 +0000 (03:58 -0700)
committerAndrew Waterman <waterman@cs.berkeley.edu>
Tue, 23 Apr 2013 10:58:02 +0000 (03:58 -0700)
kern/arch/riscv/arch.h
kern/arch/riscv/env.c
kern/arch/riscv/riscv.h
user/parlib/include/riscv/arch.h

index 9e46274..dce737d 100644 (file)
@@ -66,6 +66,12 @@ read_tsc_serialized(void)
 }
 
 static __inline uintptr_t
+enable_fp(void)
+{
+       return setpcr(PCR_SR, SR_EF);
+}
+
+static __inline uintptr_t
 enable_irq(void)
 {
        return setpcr(PCR_SR, SR_ET);
index 41228bd..da3032b 100644 (file)
@@ -7,12 +7,8 @@
 void
 save_fp_state(ancillary_state_t* silly)
 {
-       /* TODO: save FP state! */
-       return; // don't save FP state for now
-       uintptr_t sr = mfpcr(PCR_SR);
-       mtpcr(PCR_SR, sr | SR_EF);
-
-       asm("mffsr %0" : "=r"(silly->fsr));
+       uintptr_t sr = enable_fp();
+       uint32_t fsr = read_fsr();
 
        asm("fsd f0,%0" : "=m"(silly->fpr[0]));
        asm("fsd f1,%0" : "=m"(silly->fpr[1]));
@@ -47,17 +43,15 @@ save_fp_state(ancillary_state_t* silly)
        asm("fsd f30,%0" : "=m"(silly->fpr[30]));
        asm("fsd f31,%0" : "=m"(silly->fpr[31]));
 
+       silly->fsr = fsr;
        mtpcr(PCR_SR, sr);
 }
 
 void
 restore_fp_state(ancillary_state_t* silly)
 {
-       return; // don't restore FP state for now
-       uintptr_t sr = mfpcr(PCR_SR);
-       mtpcr(PCR_SR, sr | SR_EF);
-
-       asm("mtfsr %0" : : "r"(silly->fsr));
+       uintptr_t sr = enable_fp();
+       uint32_t fsr = silly->fsr;
 
        asm("fld f0,%0" : : "m"(silly->fpr[0]));
        asm("fld f1,%0" : : "m"(silly->fpr[1]));
@@ -92,12 +86,25 @@ restore_fp_state(ancillary_state_t* silly)
        asm("fld f30,%0" : : "m"(silly->fpr[30]));
        asm("fld f31,%0" : : "m"(silly->fpr[31]));
 
+       write_fsr(fsr);
        mtpcr(PCR_SR, sr);
 }
 
 void init_fp_state(void)
 {
-       /* TODO: implement me! */
+       uintptr_t sr = enable_fp();
+
+       asm("fcvt.d.w f0, x0; fcvt.d.w f1 ,x0; fcvt.d.w f2, x0; fcvt.d.w f3, x0;");
+       asm("fcvt.d.w f4, x0; fcvt.d.w f5, x0; fcvt.d.w f6, x0; fcvt.d.w f7, x0;");
+       asm("fcvt.d.w f8, x0; fcvt.d.w f9, x0; fcvt.d.w f10,x0; fcvt.d.w f11,x0;");
+       asm("fcvt.d.w f12,x0; fcvt.d.w f13,x0; fcvt.d.w f14,x0; fcvt.d.w f15,x0;");
+       asm("fcvt.d.w f16,x0; fcvt.d.w f17,x0; fcvt.d.w f18,x0; fcvt.d.w f19,x0;");
+       asm("fcvt.d.w f20,x0; fcvt.d.w f21,x0; fcvt.d.w f22,x0; fcvt.d.w f23,x0;");
+       asm("fcvt.d.w f24,x0; fcvt.d.w f25,x0; fcvt.d.w f26,x0; fcvt.d.w f27,x0;");
+       asm("fcvt.d.w f28,x0; fcvt.d.w f29,x0; fcvt.d.w f30,x0; fcvt.d.w f31,x0;");
+       asm("mtfsr x0");
+
+       mtpcr(PCR_SR, sr);
 }
 
 static int
index c23c962..d534909 100644 (file)
@@ -35,4 +35,18 @@ clear_ipi()
        mtpcr(PCR_CLR_IPI, 0);
 }
 
+static __inline uint32_t
+read_fsr(void)
+{
+       uint32_t fsr;
+       asm volatile ("mffsr %0" : "=r"(fsr));
+       return fsr;
+}
+
+static __inline void
+write_fsr(uint32_t fsr)
+{
+       asm volatile ("mtfsr %0" :: "r"(fsr));
+}
+
 #endif
index 44954bf..056aa8d 100644 (file)
@@ -2,6 +2,7 @@
 #define PARLIB_ARCH_H
 
 #include <ros/common.h>
+#include <ros/trapframe.h>
 
 #define internal_function
 
@@ -46,14 +47,52 @@ cpu_relax(void)
        asm volatile("li %0, 8; 1: addi %0, %0, -1; bnez %0, 1b" : "=r"(ctr) : : "memory");
 }
 
-static inline void save_fp_state(struct ancillary_state *silly)
+static inline void save_fp_state(struct ancillary_statesilly)
 {
-       /* TODO: (either inlined or in user/parlib/riscv/whatever.c */
+       uint32_t fsr = read_fsr();
+
+       asm("fsd fs0,%0" : "=m"(silly->fpr[0]));
+       asm("fsd fs1,%0" : "=m"(silly->fpr[1]));
+       asm("fsd fs2,%0" : "=m"(silly->fpr[2]));
+       asm("fsd fs3,%0" : "=m"(silly->fpr[3]));
+       asm("fsd fs4,%0" : "=m"(silly->fpr[4]));
+       asm("fsd fs5,%0" : "=m"(silly->fpr[5]));
+       asm("fsd fs6,%0" : "=m"(silly->fpr[6]));
+       asm("fsd fs7,%0" : "=m"(silly->fpr[7]));
+       asm("fsd fs8,%0" : "=m"(silly->fpr[8]));
+       asm("fsd fs9,%0" : "=m"(silly->fpr[9]));
+       asm("fsd fs10,%0" : "=m"(silly->fpr[10]));
+       asm("fsd fs11,%0" : "=m"(silly->fpr[11]));
+       asm("fsd fs12,%0" : "=m"(silly->fpr[12]));
+       asm("fsd fs13,%0" : "=m"(silly->fpr[13]));
+       asm("fsd fs14,%0" : "=m"(silly->fpr[14]));
+       asm("fsd fs15,%0" : "=m"(silly->fpr[15]));
+
+       silly->fsr = fsr;
 }
 
-static inline void restore_fp_state(struct ancillary_state *silly)
+static inline void restore_fp_state(struct ancillary_statesilly)
 {
-       /* TODO: (either inlined or in user/parlib/riscv/whatever.c */
+       uint32_t fsr = silly->fsr;
+
+       asm("fld fs0,%0" : : "m"(silly->fpr[0]));
+       asm("fld fs1,%0" : : "m"(silly->fpr[1]));
+       asm("fld fs2,%0" : : "m"(silly->fpr[2]));
+       asm("fld fs3,%0" : : "m"(silly->fpr[3]));
+       asm("fld fs4,%0" : : "m"(silly->fpr[4]));
+       asm("fld fs5,%0" : : "m"(silly->fpr[5]));
+       asm("fld fs6,%0" : : "m"(silly->fpr[6]));
+       asm("fld fs7,%0" : : "m"(silly->fpr[7]));
+       asm("fld fs8,%0" : : "m"(silly->fpr[8]));
+       asm("fld fs9,%0" : : "m"(silly->fpr[9]));
+       asm("fld fs10,%0" : : "m"(silly->fpr[10]));
+       asm("fld fs11,%0" : : "m"(silly->fpr[11]));
+       asm("fld fs12,%0" : : "m"(silly->fpr[12]));
+       asm("fld fs13,%0" : : "m"(silly->fpr[13]));
+       asm("fld fs14,%0" : : "m"(silly->fpr[14]));
+       asm("fld fs15,%0" : : "m"(silly->fpr[15]));
+
+       write_fsr(fsr);
 }
 
 #endif /* PARLIB_ARCH_H */