From 90e33f688ab8de54173caca1e8d9a7357def5df8 Mon Sep 17 00:00:00 2001
From: pca006132 <john.lck40@gmail.com>
Date: Fri, 3 Jul 2020 16:02:34 +0800
Subject: [PATCH] FPU: moved enable function to zc706

---
 libcortex_a9/src/fpu.rs | 14 ++++++++++++++
 libcortex_a9/src/lib.rs |  2 ++
 2 files changed, 16 insertions(+)
 create mode 100644 libcortex_a9/src/fpu.rs

diff --git a/libcortex_a9/src/fpu.rs b/libcortex_a9/src/fpu.rs
new file mode 100644
index 0000000..2a987be
--- /dev/null
+++ b/libcortex_a9/src/fpu.rs
@@ -0,0 +1,14 @@
+/// Enable FPU in the current core.
+pub fn enable_fpu() {
+    unsafe {
+        llvm_asm!("
+            mrc p15, 0, r1, c1, c0, 2
+            orr r1, r1, (0b1111<<20)
+            mcr p15, 0, r1, c1, c0, 2
+
+            vmrs r1, fpexc
+            orr r1, r1, (1<<30)
+            vmsr fpexc, r1
+            ":::"r1");
+    }
+}
diff --git a/libcortex_a9/src/lib.rs b/libcortex_a9/src/lib.rs
index dc6e930..52cb03c 100644
--- a/libcortex_a9/src/lib.rs
+++ b/libcortex_a9/src/lib.rs
@@ -11,6 +11,8 @@ pub mod mmu;
 pub mod mutex;
 pub mod sync_channel;
 mod uncached;
+mod fpu;
 pub use uncached::UncachedSlice;
+pub use fpu::enable_fpu;
 
 global_asm!(include_str!("exceptions.s"));