sys/src/9/bcm64/fpu.c

   1 #include "u.h"
   2 #include "../port/lib.h"
   3 #include "mem.h"
   4 #include "dat.h"
   5 #include "fns.h"
   6
   7 #include "ureg.h"
   8 #include "sysreg.h"
   9
  10 /* libc */
  11 extern ulong getfcr(void);
  12 extern void setfcr(ulong fcr);
  13 extern ulong getfsr(void);
  14 extern void setfsr(ulong fsr);
  15
  16 void
  17 fpuinit(void)
  18 {
  19         fpoff();
  20 }
  21
  22 void
  23 fpon(void)
  24 {
  25         syswr(CPACR_EL1, 3<<20);
  26 }
  27
  28 void
  29 fpoff(void)
  30 {
  31         syswr(CPACR_EL1, 0<<20);
  32 }
  33
  34 void
  35 fpinit(void)
  36 {
  37         fpon();
  38         setfcr(0);
  39         setfsr(0);
  40 }
  41
  42 void
  43 fpclear(void)
  44 {
  45         fpoff();
  46 }
  47
  48 void
  49 fpsave(FPsave *p)
  50 {
  51         p->control = getfcr();
  52         p->status = getfsr();
  53         fpsaveregs(p->regs);
  54         fpoff();
  55 }
  56
  57 void
  58 fprestore(FPsave *p)
  59 {
  60         fpon();
  61         setfcr(p->control);
  62         setfsr(p->status);
  63         fploadregs(p->regs);
  64 }
  65
  66 void
  67 mathtrap(Ureg*)
  68 {
  69         int s;
  70
  71         if((up->fpstate & FPillegal) != 0){
  72                 postnote(up, 1, "sys: floating point in note handler", NDebug);
  73                 return;
  74         }
  75         switch(up->fpstate){
  76         case FPinit:
  77                 s = splhi();
  78                 fpinit();
  79                 up->fpstate = FPactive;
  80                 splx(s);
  81                 break;
  82         case FPinactive:
  83                 s = splhi();
  84                 fprestore(up->fpsave);
  85                 up->fpstate = FPactive;
  86                 splx(s);
  87                 break;
  88         case FPactive:
  89                 postnote(up, 1, "sys: floating point error", NDebug);
  90                 break;
  91         }
  92 }