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 #if __SIZEOF_POINTER__ == 4
   8 #include <i386/x86_init_fpu.c>
   9 #elif __SIZEOF_POINTER__ == 8
  10 #include <x86_64/x86_init_fpu.c>
  11 #else
  12 #error "Unable to build for to-be-defined architecture type"
  13 #endif
  14 #if 0
  15 #include <inttypes.h>
  16 #include <sys/fpu.h>
  17
  18 static inline uint64_t get_cr0(void)
  19 {
  20 #if __SIZEOF_POINTER__ == 4
  21     uint32_t v;
  22 asm("movl %%cr0,%0":"=r"(v));
  23 #elif __SIZEOF_POINTER__ == 8
  24     uint64_t v;
  25 asm("movq %%cr0,%0":"=r"(v));
  26 #else
  27 #error "Unable to build for to-be-defined architecture type"
  28 #endif
  29     return v;
  30 }
  31
  32 static inline void set_cr0(uint32_t v)
  33 {
  34 #if __SIZEOF_POINTER__ == 4
  35     asm volatile ("movl %0,%%cr0"::"r" (v));
  36 #elif __SIZEOF_POINTER__ == 8
  37     asm volatile ("movq %0,%%cr0"::"r" ((uint64_t)v));
  38 #else
  39 #error "Unable to build for to-be-defined architecture type"
  40 #endif
  41 }
  42
  43 #define CR0_PE  0x00000001
  44 #define CR0_MP  0x00000002
  45 #define CR0_EM  0x00000004
  46 #define CR0_TS  0x00000008
  47 #define CR0_ET  0x00000010
  48 #define CR0_NE  0x00000020
  49 #define CR0_WP  0x00010000
  50 #define CR0_AM  0x00040000
  51 #define CR0_NW  0x20000000
  52 #define CR0_CD  0x40000000
  53 #define CR0_PG  0x80000000
  54
  55 int x86_init_fpu(void)
  56 {
  57     uint32_t cr0;
  58     uint16_t fsw = 0xffff;
  59     uint16_t fcw = 0xffff;
  60
  61     cr0 = get_cr0();
  62     cr0 &= ~(CR0_EM | CR0_TS);
  63     cr0 |= CR0_MP;
  64     set_cr0(cr0);
  65
  66     asm volatile ("fninit");
  67     asm volatile ("fnstsw %0":"+m" (fsw));
  68     if (fsw != 0)
  69         return -1;
  70
  71     asm volatile ("fnstcw %0":"+m" (fcw));
  72     if ((fcw & 0x103f) != 0x3f)
  73         return -1;
  74
  75     /* Techically, this could be a 386 with a 287.  We could add a check
  76        for that here... */
  77
  78     return 0;
  79 }
  80 #endif