From ca2232343b4a9d919710d96ee84e16e948869992 Mon Sep 17 00:00:00 2001
From: "H. Peter Anvin" <hpa@zytor.com>
Date: Mon, 28 Aug 2006 02:40:02 -0700
Subject: [PATCH] Add routine to initialize and test for the FPU.

---
 com32/lib/Makefile           |  2 +-
 com32/lib/sys/x86_init_fpu.c | 58 ++++++++++++++++++++++++++++++++++++++++++++
 2 files changed, 59 insertions(+), 1 deletion(-)
 create mode 100644 com32/lib/sys/x86_init_fpu.c

diff --git a/com32/lib/Makefile b/com32/lib/Makefile
index a3aaa51..5b6e057 100644
--- a/com32/lib/Makefile
+++ b/com32/lib/Makefile
@@ -50,7 +50,7 @@ LIBOBJS = \
 	libpng/pngrtran.o libpng/pngwtran.o libpng/pngmem.o		\
 	libpng/pngerror.o libpng/pngpread.o				\
 	\
-	math/pow.o
+	sys/x86_init_fpu.o math/pow.o math/strtod.o
 
 BINDIR   = /usr/bin
 LIBDIR   = /usr/lib
diff --git a/com32/lib/sys/x86_init_fpu.c b/com32/lib/sys/x86_init_fpu.c
new file mode 100644
index 0000000..250e52e
--- /dev/null
+++ b/com32/lib/sys/x86_init_fpu.c
@@ -0,0 +1,58 @@
+/*
+ * x86_has_fpu.c
+ *
+ * Test for an x86 FPU, and do any necessary setup.
+ */
+
+#include <inttypes.h>
+#include <sys/fpu.h>
+
+static inline uint64_t get_cr0(void)
+{
+  uint32_t v;
+  asm("movl %%cr0,%0" : "=r" (v));
+  return v;
+}
+
+static inline void set_cr0(uint32_t v)
+{
+  asm volatile("movl %0,%%cr0" : : "r" (v));
+}
+
+#define CR0_PE	0x00000001
+#define CR0_MP  0x00000002
+#define CR0_EM  0x00000004
+#define CR0_TS  0x00000008
+#define CR0_ET  0x00000010
+#define CR0_NE  0x00000020
+#define CR0_WP  0x00010000
+#define CR0_AM  0x00040000
+#define CR0_NW  0x20000000
+#define CR0_CD  0x40000000
+#define CR0_PG  0x80000000
+
+int x86_init_fpu(void)
+{
+  uint32_t cr0;
+  uint16_t fsw = 0xffff;
+  uint16_t fcw = 0xffff;
+
+  cr0 = get_cr0();
+  cr0 &= ~(CR0_EM|CR0_TS);
+  cr0 |= CR0_MP;
+  set_cr0(cr0);
+
+  asm volatile("fninit");
+  asm volatile("fnstsw %0" : "+m" (fsw));
+  if (fsw != 0)
+    return -1;
+  
+  asm volatile("fnstcw %0" : "+m" (fcw));
+  if ((fcw & 0x103f) != 0x3f)
+    return -1;
+
+  /* Techically, this could be a 386 with a 287.  We could add a check
+     for that here... */
+
+  return 0;
+}
-- 
2.7.4