From 3a76c3b26eeb995604db24afc98f1689a7514db0 Mon Sep 17 00:00:00 2001
From: Vadim Kaushan <admin@disasm.info>
Date: Fri, 29 Mar 2019 16:05:07 +0300
Subject: [PATCH] Initialize FPU when available

---
 src/lib.rs | 12 ++++++++++--
 1 file changed, 10 insertions(+), 2 deletions(-)

diff --git a/src/lib.rs b/src/lib.rs
index d227481..b8d1e2b 100644
--- a/src/lib.rs
+++ b/src/lib.rs
@@ -208,7 +208,7 @@ extern crate r0;
 
 pub use macros::{entry, pre_init};
 
-use riscv::register::{mstatus, mtvec};
+use riscv::register::{mstatus, mtvec, misa, fcsr};
 
 #[export_name = "error: riscv-rt appears more than once in the dependency graph"]
 #[doc(hidden)]
@@ -251,7 +251,15 @@ pub unsafe extern "C" fn start_rust() -> ! {
     r0::zero_bss(&mut _sbss, &mut _ebss);
     r0::init_data(&mut _sdata, &mut _edata, &_sidata);
 
-    // TODO: Enable FPU when available
+    // Initialize FPU when available
+    if let Some(isa) = misa::read() {
+        if isa.has_extension('F') || isa.has_extension('D') {
+            fcsr::clear_flags();
+            fcsr::set_rounding_mode(fcsr::RoundingMode::RoundToNearestEven);
+            mstatus::set_fs(mstatus::FS::Dirty);
+            // TODO: fill registers with zeros
+        }
+    }
 
     // Set mtvec to _start_trap
     mtvec::write(&_start_trap as *const _ as usize, mtvec::TrapMode::Direct);