kernel/fiasco/src/kern/arm/fpu-arm.cpp

   1 INTERFACE [arm && fpu]:
   2
   3 EXTENSION class Fpu
   4 {
   5 public:
   6   struct Exception_state_user
   7   {
   8     Mword fpexc;
   9     Mword fpinst;
  10     Mword fpinst2;
  11   };
  12
  13   Mword fpsid() const { return _fpsid; }
  14
  15   enum
  16   {
  17     FPEXC_EN  = 1 << 30,
  18     FPEXC_EX  = 1 << 31,
  19   };
  20
  21   struct Fpu_regs
  22   {
  23     Mword fpexc, fpscr;
  24     Mword state[32 * 4]; // 4*32 bytes for each FP-reg
  25   };
  26
  27   static Mword fpsid_rev(Mword v)          { return v & 0xf; }
  28   static Mword fpsid_variant(Mword v)      { return (v >> 4) & 0xf; }
  29   static Mword fpsid_part_number(Mword v)  { return (v >> 8) & 0xff; }
  30   static Mword fpsid_arch_version(Mword v) { return (v >> 16) & 0xf; }
  31   static Mword fpsid_precision(Mword v)    { return (v >> 20) & 1; }
  32   static Mword fpsid_format(Mword v)       { return (v >> 21) & 3; }
  33   static Mword fpsid_hw_sw(Mword v)        { return (v >> 23) & 1; }
  34   static Mword fpsid_implementer(Mword v)  { return v >> 24; }
  35
  36 private:
  37   Mword _fpsid;
  38 };
  39
  40 // ------------------------------------------------------------------------
  41 INTERFACE [arm && !fpu]:
  42
  43 EXTENSION class Fpu
  44 {
  45 public:
  46   struct Exception_state_user
  47   {
  48   };
  49 };
  50
  51 // ------------------------------------------------------------------------
  52 IMPLEMENTATION [arm && !fpu]:
  53
  54 #include "trap_state.h"
  55
  56 PUBLIC static inline NEEDS["trap_state.h"]
  57 void
  58 Fpu::save_user_exception_state(Trap_state *, Exception_state_user *)
  59 {}
  60
  61 // ------------------------------------------------------------------------
  62 IMPLEMENTATION [arm && fpu && !armv6plus]:
  63
  64 PRIVATE static inline
  65 void
  66 Fpu::copro_enable()
  67 {}
  68
  69 // ------------------------------------------------------------------------
  70 IMPLEMENTATION [arm && fpu && armv6plus]:
  71
  72 PRIVATE static inline
  73 void
  74 Fpu::copro_enable()
  75 {
  76   asm volatile("mrc  p15, 0, %0, c1, c0, 2\n"
  77                "orr  %0, %0, %1           \n"
  78                "mcr  p15, 0, %0, c1, c0, 2\n"
  79                : : "r" (0), "I" (0x00f00000));
  80   Mem::isb();
  81 }
  82
  83 // ------------------------------------------------------------------------
  84 IMPLEMENTATION [arm && fpu]:
  85
  86 #include <cassert>
  87 #include <cstdio>
  88 #include <cstring>
  89
  90 #include "fpu_state.h"
  91 #include "mem.h"
  92 #include "processor.h"
  93 #include "static_assert.h"
  94 #include "trap_state.h"
  95
  96 PUBLIC static inline
  97 Mword
  98 Fpu::fpsid_read()
  99 {
 100   Mword v;
 101   asm volatile("mrc p10, 7, %0, cr0, cr0" : "=r" (v));
 102   return v;
 103 }
 104
 105 PUBLIC static inline
 106 Mword
 107 Fpu::mvfr0()
 108 {
 109   Mword v;
 110   asm volatile("mrc p10, 7, %0, cr7, cr0" : "=r" (v));
 111   return v;
 112 }
 113
 114 PUBLIC static inline
 115 Mword
 116 Fpu::mvfr1()
 117 {
 118   Mword v;
 119   asm volatile("mrc p10, 7, %0, cr6, cr0" : "=r" (v));
 120   return v;
 121 }
 122
 123
 124 PRIVATE static inline
 125 void
 126 Fpu::fpexc(Mword v)
 127 {
 128   asm volatile("mcr p10, 7, %0, cr8, cr0" : : "r" (v));
 129 }
 130
 131 PUBLIC static inline
 132 Mword
 133 Fpu::fpexc()
 134 {
 135   Mword v;
 136   asm volatile("mrc p10, 7, %0, cr8, cr0" : "=r" (v));
 137   return v;
 138 }
 139
 140 PUBLIC static inline
 141 Mword
 142 Fpu::fpinst()
 143 {
 144   Mword i;
 145   asm volatile("mcr p10, 7, %0, cr9,  cr0" : "=r" (i));
 146   return i;
 147 }
 148
 149 PUBLIC static inline
 150 Mword
 151 Fpu::fpinst2()
 152 {
 153   Mword i;
 154   asm volatile("mcr p10, 7, %0, cr10,  cr0" : "=r" (i));
 155   return i;
 156 }
 157
 158 PUBLIC static inline
 159 bool
 160 Fpu::exc_pending()
 161 {
 162   return fpexc() & FPEXC_EX;
 163 }
 164
 165 IMPLEMENT
 166 void
 167 Fpu::enable()
 168 {
 169   fpexc((fpexc() | FPEXC_EN) & ~FPEXC_EX);
 170 }
 171
 172 IMPLEMENT
 173 void
 174 Fpu::disable()
 175 {
 176   fpexc(fpexc() & ~FPEXC_EN);
 177 }
 178
 179 PUBLIC static inline
 180 int
 181 Fpu::is_emu_insn(Mword opcode)
 182 {
 183   return (opcode & 0x0ff00f90) == 0x0ef00a10;
 184 }
 185
 186 PUBLIC static inline
 187 bool
 188 Fpu::emulate_insns(Mword opcode, Trap_state *ts)
 189 {
 190   unsigned reg = (opcode >> 16) & 0xf;
 191   unsigned rt  = (opcode >> 12) & 0xf;
 192   Mword fpsid = Fpu::fpu.current().fpsid();
 193   switch (reg)
 194     {
 195     case 0: // FPSID
 196       ts->r[rt] = fpsid;
 197       break;
 198     case 6: // MVFR1
 199       if (Fpu::fpsid_arch_version(fpsid) < 2)
 200         return false;
 201       ts->r[rt] = Fpu::mvfr1();
 202       break;
 203     case 7: // MVFR0
 204       if (Fpu::fpsid_arch_version(fpsid) < 2)
 205         return false;
 206       ts->r[rt] = Fpu::mvfr0();
 207       break;
 208     default:
 209       break;
 210     }
 211
 212   if (ts->psr & Proc::Status_thumb)
 213     ts->pc += 2;
 214
 215   return true;
 216 }
 217
 218
 219
 220 IMPLEMENT
 221 void
 222 Fpu::init(unsigned cpu)
 223 {
 224   copro_enable();
 225
 226   Mword s = fpsid_read();
 227
 228   fpu.cpu(cpu)._fpsid = s;
 229
 230   printf("FPU%d: Arch: %s(%lx), Part: %s(%lx), r: %lx, v: %lx, i: %lx, t: %s, p: %s\n",
 231          cpu, fpsid_arch_version(s) == 1
 232            ? "VFPv2"
 233            : (fpsid_arch_version(s) == 3 ? "VFPv3" : "Unkn"),
 234          fpsid_arch_version(s),
 235          fpsid_part_number(s) == 0x20
 236            ? "VFP11"
 237            : (fpsid_part_number(s) == 0x30 ?  "VFPv3" : "Unkn"),
 238          fpsid_part_number(s),
 239          fpsid_rev(s), fpsid_variant(s), fpsid_implementer(s),
 240          fpsid_hw_sw(s) ? "soft" : "hard",
 241          fpsid_precision(s) ? "sngl" : "dbl/sngl");
 242
 243   disable();
 244
 245   fpu.cpu(cpu).set_owner(0);
 246 }
 247
 248 IMPLEMENT inline NEEDS ["fpu_state.h", "mem.h", "static_assert.h", <cstring>]
 249 void
 250 Fpu::init_state (Fpu_state *s)
 251 {
 252   Fpu_regs *fpu_regs = reinterpret_cast<Fpu_regs *>(s->state_buffer());
 253   static_assert(!(sizeof (*fpu_regs) % sizeof(Mword)),
 254                 "Non-mword size of Fpu_regs");
 255   Mem::memset_mwords(fpu_regs, 0, sizeof (*fpu_regs) / sizeof(Mword));
 256 }
 257
 258 IMPLEMENT
 259 void
 260 Fpu::save_state(Fpu_state *s)
 261 {
 262   assert(s->state_buffer());
 263   Fpu_regs *fpu_regs = reinterpret_cast<Fpu_regs *>(s->state_buffer());
 264
 265   asm volatile ("stc p11, cr0, [%0], #32*4     \n"
 266                 : : "r" (fpu_regs->state));
 267   asm volatile ("mrc p10, 7, %0, cr8,  cr0, 0  \n"
 268                 "mrc p10, 7, %1, cr1,  cr0, 0  \n"
 269                 : "=r" (fpu_regs->fpexc),
 270                   "=r" (fpu_regs->fpscr));
 271 }
 272
 273 IMPLEMENT
 274 void
 275 Fpu::restore_state(Fpu_state *s)
 276 {
 277   assert (s->state_buffer());
 278   Fpu_regs *fpu_regs = reinterpret_cast<Fpu_regs *>(s->state_buffer());
 279
 280   asm volatile ("ldc p11, cr0, [%0], #32*4     \n"
 281                 : : "r" (fpu_regs->state));
 282   asm volatile ("mcr p10, 7, %0, cr8,  cr0, 0  \n"
 283                 "mcr p10, 7, %1, cr1,  cr0, 0  \n"
 284                 :
 285                 : "r" (fpu_regs->fpexc | FPEXC_EN),
 286                   "r" (fpu_regs->fpscr));
 287
 288 #if 0
 289   asm volatile("mcr p10, 7, %2, cr9,  cr0, 0  \n"
 290                "mcr p10, 7, %3, cr10, cr0, 0  \n"
 291                :
 292                : "r" (fpu_regs->fpinst),
 293                  "r" (fpu_regs->fpinst2));
 294 #endif
 295 }
 296
 297 IMPLEMENT inline
 298 unsigned
 299 Fpu::state_size()
 300 { return sizeof (Fpu_regs); }
 301
 302 IMPLEMENT inline
 303 unsigned
 304 Fpu::state_align()
 305 { return 4; }
 306
 307 PUBLIC static
 308 bool
 309 Fpu::is_enabled()
 310 {
 311   return fpexc() & FPEXC_EN;
 312 }
 313
 314 PUBLIC static inline NEEDS["trap_state.h"]
 315 void
 316 Fpu::save_user_exception_state(Trap_state *ts, Exception_state_user *esu)
 317 {
 318   if ((ts->error_code & 0x01f00000) == 0x01100000)
 319     {
 320       esu->fpexc = Fpu::fpexc();
 321       if (ts->error_code == 0x03100000)
 322         {
 323           esu->fpinst  = Fpu::fpinst();
 324           esu->fpinst2 = Fpu::fpinst2();
 325         }
 326     }
 327 }