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]:
  63
  64 #include <cassert>
  65 #include <cstdio>
  66 #include <cstring>
  67
  68 #include "fpu_state.h"
  69 #include "mem.h"
  70 #include "processor.h"
  71 #include "static_assert.h"
  72 #include "trap_state.h"
  73
  74 PUBLIC static inline
  75 Mword
  76 Fpu::fpsid_read()
  77 {
  78   Mword v;
  79   asm volatile("mrc p10, 7, %0, cr0, cr0" : "=r" (v));
  80   return v;
  81 }
  82
  83 PUBLIC static inline
  84 Mword
  85 Fpu::mvfr0()
  86 {
  87   Mword v;
  88   asm volatile("mrc p10, 7, %0, cr7, cr0" : "=r" (v));
  89   return v;
  90 }
  91
  92 PUBLIC static inline
  93 Mword
  94 Fpu::mvfr1()
  95 {
  96   Mword v;
  97   asm volatile("mrc p10, 7, %0, cr6, cr0" : "=r" (v));
  98   return v;
  99 }
 100
 101
 102 PRIVATE static inline
 103 void
 104 Fpu::fpexc(Mword v)
 105 {
 106   asm volatile("mcr p10, 7, %0, cr8, cr0" : : "r" (v));
 107 }
 108
 109 PUBLIC static inline
 110 Mword
 111 Fpu::fpexc()
 112 {
 113   Mword v;
 114   asm volatile("mrc p10, 7, %0, cr8, cr0" : "=r" (v));
 115   return v;
 116 }
 117
 118 PUBLIC static inline
 119 Mword
 120 Fpu::fpinst()
 121 {
 122   Mword i;
 123   asm volatile("mcr p10, 7, %0, cr9,  cr0" : "=r" (i));
 124   return i;
 125 }
 126
 127 PUBLIC static inline
 128 Mword
 129 Fpu::fpinst2()
 130 {
 131   Mword i;
 132   asm volatile("mcr p10, 7, %0, cr10,  cr0" : "=r" (i));
 133   return i;
 134 }
 135
 136 PUBLIC static inline
 137 bool
 138 Fpu::exc_pending()
 139 {
 140   return fpexc() & FPEXC_EX;
 141 }
 142
 143 IMPLEMENT
 144 void
 145 Fpu::enable()
 146 {
 147   fpexc((fpexc() | FPEXC_EN) & ~FPEXC_EX);
 148 }
 149
 150 IMPLEMENT
 151 void
 152 Fpu::disable()
 153 {
 154   fpexc(fpexc() & ~FPEXC_EN);
 155 }
 156
 157 PUBLIC static inline
 158 bool
 159 Fpu::emulate_insns(Mword opcode, Trap_state *ts, unsigned cpu)
 160 {
 161   unsigned reg = (opcode >> 16) & 0xf;
 162   unsigned rt  = (opcode >> 12) & 0xf;
 163   Mword fpsid = Fpu::fpu(cpu).fpsid();
 164   switch (reg)
 165     {
 166     case 0: // FPSID
 167       ts->r[rt] = fpsid;
 168       break;
 169     case 6: // MVFR1
 170       if (Fpu::fpsid_arch_version(fpsid) < 2)
 171         return false;
 172       ts->r[rt] = Fpu::mvfr1();
 173       break;
 174     case 7: // MVFR0
 175       if (Fpu::fpsid_arch_version(fpsid) < 2)
 176         return false;
 177       ts->r[rt] = Fpu::mvfr0();
 178       break;
 179     default:
 180       break;
 181     }
 182
 183   if (ts->psr & Proc::Status_thumb)
 184     ts->pc += 2;
 185
 186   return true;
 187 }
 188
 189
 190
 191 IMPLEMENT
 192 void
 193 Fpu::init(unsigned cpu)
 194 {
 195   asm volatile(" mcr  p15, 0, %0, c1, c0, 2   \n" : : "r"(0x00f00000));
 196   Mem::dsb();
 197
 198   Mword s = fpsid_read();
 199
 200   _fpu.cpu(cpu)._fpsid = s;
 201
 202   printf("FPU%d: Arch: %s(%lx), Part: %s(%lx), r: %lx, v: %lx, i: %lx, t: %s, p: %s\n",
 203          cpu, fpsid_arch_version(s) == 1
 204            ? "VFPv2"
 205            : (fpsid_arch_version(s) == 3 ? "VFPv3" : "Unkn"),
 206          fpsid_arch_version(s),
 207          fpsid_part_number(s) == 0x20
 208            ? "VFP11"
 209            : (fpsid_part_number(s) == 0x30 ?  "VFPv3" : "Unkn"),
 210          fpsid_part_number(s),
 211          fpsid_rev(s), fpsid_variant(s), fpsid_implementer(s),
 212          fpsid_hw_sw(s) ? "soft" : "hard",
 213          fpsid_precision(s) ? "sngl" : "dbl/sngl");
 214
 215   disable();
 216
 217   set_owner(cpu, 0);
 218 }
 219
 220 IMPLEMENT inline NEEDS ["fpu_state.h", "mem.h", "static_assert.h", <cstring>]
 221 void
 222 Fpu::init_state (Fpu_state *s)
 223 {
 224   Fpu_regs *fpu_regs = reinterpret_cast<Fpu_regs *>(s->state_buffer());
 225   static_assert(!(sizeof (*fpu_regs) % sizeof(Mword)),
 226                 "Non-mword size of Fpu_regs");
 227   Mem::memset_mwords(fpu_regs, 0, sizeof (*fpu_regs) / sizeof(Mword));
 228 }
 229
 230 IMPLEMENT
 231 void
 232 Fpu::save_state(Fpu_state *s)
 233 {
 234   assert(s->state_buffer());
 235   Fpu_regs *fpu_regs = reinterpret_cast<Fpu_regs *>(s->state_buffer());
 236
 237   asm volatile ("stc p11, cr0, [%0], #32*4     \n"
 238                 : : "r" (fpu_regs->state));
 239   asm volatile ("mrc p10, 7, %0, cr8,  cr0, 0  \n"
 240                 "mrc p10, 7, %1, cr1,  cr0, 0  \n"
 241                 : "=r" (fpu_regs->fpexc),
 242                   "=r" (fpu_regs->fpscr));
 243 }
 244
 245 IMPLEMENT
 246 void
 247 Fpu::restore_state (Fpu_state *s)
 248 {
 249   assert (s->state_buffer());
 250   Fpu_regs *fpu_regs = reinterpret_cast<Fpu_regs *>(s->state_buffer());
 251
 252   asm volatile ("ldc p11, cr0, [%0], #32*4     \n"
 253                 : : "r" (fpu_regs->state));
 254   asm volatile ("mcr p10, 7, %0, cr8,  cr0, 0  \n"
 255                 "mcr p10, 7, %1, cr1,  cr0, 0  \n"
 256                 :
 257                 : "r" (fpu_regs->fpexc | FPEXC_EN),
 258                   "r" (fpu_regs->fpscr));
 259
 260 #if 0
 261   asm volatile("mcr p10, 7, %2, cr9,  cr0, 0  \n"
 262                "mcr p10, 7, %3, cr10, cr0, 0  \n"
 263                :
 264                : "r" (fpu_regs->fpinst),
 265                  "r" (fpu_regs->fpinst2));
 266 #endif
 267 }
 268
 269 IMPLEMENT inline
 270 unsigned
 271 Fpu::state_size()
 272 { return sizeof (Fpu_regs); }
 273
 274 IMPLEMENT inline
 275 unsigned
 276 Fpu::state_align()
 277 { return 4; }
 278
 279 PUBLIC static
 280 bool
 281 Fpu::is_enabled()
 282 {
 283   return fpexc() & FPEXC_EN;
 284 }
 285
 286 PUBLIC static inline NEEDS["trap_state.h"]
 287 void
 288 Fpu::save_user_exception_state(Trap_state *ts, Exception_state_user *esu)
 289 {
 290   if ((ts->error_code & 0x01f00000) == 0x01100000)
 291     {
 292       esu->fpexc = Fpu::fpexc();
 293       if (ts->error_code == 0x03100000)
 294         {
 295           esu->fpinst  = Fpu::fpinst();
 296           esu->fpinst2 = Fpu::fpinst2();
 297         }
 298     }
 299 }