INTERFACE [arm && fpu]:
+#include <cxx/bitfield>
+
EXTENSION class Fpu
{
public:
Mword fpinst2;
};
- Mword fpsid() const { return _fpsid; }
-
enum
{
FPEXC_EN = 1 << 30,
struct Fpu_regs
{
Mword fpexc, fpscr;
- Mword state[32 * 4]; // 4*32 bytes for each FP-reg
+ Mword state[64];
+ };
+
+ struct Fpsid
+ {
+ Mword v;
+
+ Fpsid() = default;
+ explicit Fpsid(Mword v) : v(v) {}
+
+ CXX_BITFIELD_MEMBER(0, 3, rev, v);
+ CXX_BITFIELD_MEMBER(4, 7, variant, v);
+ CXX_BITFIELD_MEMBER(8, 15, part_number, v);
+ CXX_BITFIELD_MEMBER(16, 19, arch_version, v);
+ CXX_BITFIELD_MEMBER(20, 20, precision, v);
+ CXX_BITFIELD_MEMBER(21, 22, format, v);
+ CXX_BITFIELD_MEMBER(23, 23, hw_sw, v);
+ CXX_BITFIELD_MEMBER(24, 31, implementer, v);
};
- static Mword fpsid_rev(Mword v) { return v & 0xf; }
- static Mword fpsid_variant(Mword v) { return (v >> 4) & 0xf; }
- static Mword fpsid_part_number(Mword v) { return (v >> 8) & 0xff; }
- static Mword fpsid_arch_version(Mword v) { return (v >> 16) & 0xf; }
- static Mword fpsid_precision(Mword v) { return (v >> 20) & 1; }
- static Mword fpsid_format(Mword v) { return (v >> 21) & 3; }
- static Mword fpsid_hw_sw(Mword v) { return (v >> 23) & 1; }
- static Mword fpsid_implementer(Mword v) { return v >> 24; }
+ Fpsid fpsid() const { return _fpsid; }
private:
- Mword _fpsid;
+ Fpsid _fpsid;
+ static bool save_32r;
};
// ------------------------------------------------------------------------
Fpu::save_user_exception_state(Trap_state *, Exception_state_user *)
{}
+// ------------------------------------------------------------------------
+IMPLEMENTATION [arm && fpu && !armv6plus]:
+
+PRIVATE static inline
+void
+Fpu::copro_enable()
+{}
+
+// ------------------------------------------------------------------------
+IMPLEMENTATION [arm && fpu && armv6plus]:
+
+PRIVATE static inline
+void
+Fpu::copro_enable()
+{
+ asm volatile("mrc p15, 0, %0, c1, c0, 2\n"
+ "orr %0, %0, %1 \n"
+ "mcr p15, 0, %0, c1, c0, 2\n"
+ : : "r" (0), "I" (0x00f00000));
+ Mem::isb();
+}
+
// ------------------------------------------------------------------------
IMPLEMENTATION [arm && fpu]:
#include "static_assert.h"
#include "trap_state.h"
+bool Fpu::save_32r;
+
PUBLIC static inline
Mword
Fpu::fpsid_read()
fpexc(fpexc() & ~FPEXC_EN);
}
+PUBLIC static inline
+int
+Fpu::is_emu_insn(Mword opcode)
+{
+ return (opcode & 0x0ff00f90) == 0x0ef00a10;
+}
+
PUBLIC static inline
bool
-Fpu::emulate_insns(Mword opcode, Trap_state *ts, unsigned cpu)
+Fpu::emulate_insns(Mword opcode, Trap_state *ts)
{
unsigned reg = (opcode >> 16) & 0xf;
unsigned rt = (opcode >> 12) & 0xf;
- Mword fpsid = Fpu::fpu(cpu).fpsid();
+ Fpsid fpsid = Fpu::fpu.current().fpsid();
switch (reg)
{
case 0: // FPSID
- ts->r[rt] = fpsid;
+ ts->r[rt] = fpsid.v;
break;
case 6: // MVFR1
- if (Fpu::fpsid_arch_version(fpsid) < 2)
+ if (fpsid.arch_version() < 2)
return false;
ts->r[rt] = Fpu::mvfr1();
break;
case 7: // MVFR0
- if (Fpu::fpsid_arch_version(fpsid) < 2)
+ if (fpsid.arch_version() < 2)
return false;
ts->r[rt] = Fpu::mvfr0();
break;
return true;
}
+PRIVATE static
+void
+Fpu::show(Cpu_number cpu)
+{
+ const Fpsid s = fpu.cpu(cpu)._fpsid;
+ unsigned arch = s.arch_version();
+ printf("FPU%d: Arch: %s(%x), Part: %s(%x), r: %x, v: %x, i: %x, t: %s, p: %s\n",
+ cxx::int_value<Cpu_number>(cpu),
+ arch == 1 ? "VFPv2"
+ : (arch == 3 ? "VFPv3"
+ : (arch == 4 ? "VFPv4"
+ : "Unkn")),
+ arch,
+ (int)s.part_number() == 0x20
+ ? "VFP11"
+ : (s.part_number() == 0x30 ? "VFPv3" : "Unkn"),
+ (int)s.part_number(),
+ (int)s.rev(), (int)s.variant(), (int)s.implementer(),
+ (int)s.hw_sw() ? "soft" : "hard",
+ (int)s.precision() ? "sngl" : "dbl/sngl");
+}
IMPLEMENT
void
-Fpu::init(unsigned cpu)
+Fpu::init(Cpu_number cpu, bool resume)
{
- asm volatile(" mcr p15, 0, %0, c1, c0, 2 \n" : : "r"(0x00f00000));
- Mem::dsb();
+ copro_enable();
- Mword s = fpsid_read();
+ Fpu &f = fpu.cpu(cpu);
+ f._fpsid = Fpsid(fpsid_read());
+ if (cpu == Cpu_number::boot_cpu() && f._fpsid.arch_version() > 1)
+ save_32r = (mvfr0() & 0xf) == 2;
- _fpu.cpu(cpu)._fpsid = s;
-
- printf("FPU%d: Arch: %s(%lx), Part: %s(%lx), r: %lx, v: %lx, i: %lx, t: %s, p: %s\n",
- cpu, fpsid_arch_version(s) == 1
- ? "VFPv2"
- : (fpsid_arch_version(s) == 3 ? "VFPv3" : "Unkn"),
- fpsid_arch_version(s),
- fpsid_part_number(s) == 0x20
- ? "VFP11"
- : (fpsid_part_number(s) == 0x30 ? "VFPv3" : "Unkn"),
- fpsid_part_number(s),
- fpsid_rev(s), fpsid_variant(s), fpsid_implementer(s),
- fpsid_hw_sw(s) ? "soft" : "hard",
- fpsid_precision(s) ? "sngl" : "dbl/sngl");
+ if (!resume)
+ show(cpu);
disable();
- set_owner(cpu, 0);
+ f.set_owner(0);
}
-IMPLEMENT inline NEEDS ["fpu_state.h", "mem.h", "static_assert.h", <cstring>]
+IMPLEMENT inline NEEDS ["fpu_state.h", "mem.h", "static_assert.h"]
void
-Fpu::init_state (Fpu_state *s)
+Fpu::init_state(Fpu_state *s)
{
Fpu_regs *fpu_regs = reinterpret_cast<Fpu_regs *>(s->state_buffer());
static_assert(!(sizeof (*fpu_regs) % sizeof(Mword)),
Mem::memset_mwords(fpu_regs, 0, sizeof (*fpu_regs) / sizeof(Mword));
}
+PRIVATE static inline
+void
+Fpu::save_fpu_regs(Fpu_regs *r)
+{
+ Mword tmp;
+ asm volatile("stc p11, cr0, [%0], #128 \n"
+ "cmp %2, #0 \n"
+ "stcnel p11, cr0, [%0], #128 \n"
+ : "=r" (tmp) : "0" (r->state), "r" (save_32r));
+}
+
+PRIVATE static inline
+void
+Fpu::restore_fpu_regs(Fpu_regs *r)
+{
+ Mword tmp;
+ asm volatile("ldc p11, cr0, [%0], #128 \n"
+ "cmp %2, #0 \n"
+ "ldcnel p11, cr0, [%0], #128 \n"
+ : "=r" (tmp) : "0" (r->state), "r" (save_32r));
+}
+
IMPLEMENT
void
Fpu::save_state(Fpu_state *s)
{
- assert(s->state_buffer());
Fpu_regs *fpu_regs = reinterpret_cast<Fpu_regs *>(s->state_buffer());
- asm volatile ("stc p11, cr0, [%0], #32*4 \n"
- : : "r" (fpu_regs->state));
+ assert(fpu_regs);
+
+ save_fpu_regs(fpu_regs);
+
asm volatile ("mrc p10, 7, %0, cr8, cr0, 0 \n"
"mrc p10, 7, %1, cr1, cr0, 0 \n"
: "=r" (fpu_regs->fpexc),
IMPLEMENT
void
-Fpu::restore_state (Fpu_state *s)
+Fpu::restore_state(Fpu_state *s)
{
- assert (s->state_buffer());
Fpu_regs *fpu_regs = reinterpret_cast<Fpu_regs *>(s->state_buffer());
- asm volatile ("ldc p11, cr0, [%0], #32*4 \n"
- : : "r" (fpu_regs->state));
+ assert(fpu_regs);
+
+ restore_fpu_regs(fpu_regs);
+
asm volatile ("mcr p10, 7, %0, cr8, cr0, 0 \n"
"mcr p10, 7, %1, cr1, cr0, 0 \n"
:
: "r" (fpu_regs->fpexc | FPEXC_EN),
"r" (fpu_regs->fpscr));
-
-#if 0
- asm volatile("mcr p10, 7, %2, cr9, cr0, 0 \n"
- "mcr p10, 7, %3, cr10, cr0, 0 \n"
- :
- : "r" (fpu_regs->fpinst),
- "r" (fpu_regs->fpinst2));
-#endif
}
IMPLEMENT inline
projects / l4.git / blobdiff