com32/lib/sys/x86_init_fpu.c

   1 /*
   2  * x86_has_fpu.c
   3  *
   4  * Test for an x86 FPU, and do any necessary setup.
   5  */
   6
   7 #include <inttypes.h>
   8 #include <sys/fpu.h>
   9
  10 static inline uint64_t get_cr0(void)
  11 {
  12   uint32_t v;
  13   asm("movl %%cr0,%0" : "=r" (v));
  14   return v;
  15 }
  16
  17 static inline void set_cr0(uint32_t v)
  18 {
  19   asm volatile("movl %0,%%cr0" : : "r" (v));
  20 }
  21
  22 #define CR0_PE  0x00000001
  23 #define CR0_MP  0x00000002
  24 #define CR0_EM  0x00000004
  25 #define CR0_TS  0x00000008
  26 #define CR0_ET  0x00000010
  27 #define CR0_NE  0x00000020
  28 #define CR0_WP  0x00010000
  29 #define CR0_AM  0x00040000
  30 #define CR0_NW  0x20000000
  31 #define CR0_CD  0x40000000
  32 #define CR0_PG  0x80000000
  33
  34 int x86_init_fpu(void)
  35 {
  36   uint32_t cr0;
  37   uint16_t fsw = 0xffff;
  38   uint16_t fcw = 0xffff;
  39
  40   cr0 = get_cr0();
  41   cr0 &= ~(CR0_EM|CR0_TS);
  42   cr0 |= CR0_MP;
  43   set_cr0(cr0);
  44
  45   asm volatile("fninit");
  46   asm volatile("fnstsw %0" : "+m" (fsw));
  47   if (fsw != 0)
  48     return -1;
  49
  50   asm volatile("fnstcw %0" : "+m" (fcw));
  51   if ((fcw & 0x103f) != 0x3f)
  52     return -1;
  53
  54   /* Techically, this could be a 386 with a 287.  We could add a check
  55      for that here... */
  56
  57   return 0;
  58 }