#include "runtime.h"
-// returns number of words that the fp instruction is occupying
+void abort(void);
+
+static void
+fabort(void)
+{
+ if (1) {
+ printf("Unsupported floating point instruction\n");
+ abort();
+ }
+}
+
+static uint32 doabort = 0;
+static uint32 trace = 0;
+
+#define DOUBLE_EXPBIAS 1023
+#define SINGLE_EXPBIAS 127
+
+static const int8* opnames[] = {
+ // binary
+ "adf",
+ "muf",
+ "suf",
+ "rsf",
+ "dvf",
+ "rdf",
+ "pow",
+ "rpw",
+ "rmf",
+ "fml",
+ "fdv",
+ "frd",
+ "pol",
+ "UNDEFINED",
+ "UNDEFINED",
+ "UNDEFINED",
+
+ // unary
+ "mvf",
+ "mnf",
+ "abs",
+ "rnd",
+ "sqt",
+ "log",
+ "lgn",
+ "exp",
+ "sin",
+ "cos",
+ "tan",
+ "asn",
+ "acs",
+ "atn",
+ "urd",
+ "nrm"
+};
+
+static const int8* fpconst[] = {
+ "0.0", "1.0", "2.0", "3.0", "4.0", "5.0", "0.5", "10.0",
+};
+
+static const uint64 fpdconst[] = {
+ 0x0000000000000000ll,
+ 0x3ff0000000000000ll,
+ 0x4000000000000000ll,
+ 0x4008000000000000ll,
+ 0x4010000000000000ll,
+ 0x4014000000000000ll,
+ 0x3fe0000000000000ll,
+ 0x4024000000000000ll
+};
+
+static const int8* fpprec[] = {
+ "s", "d", "e", "?"
+};
+
static uint32
-isfltinstr(uint32 *pc)
+precision(uint32 i)
+{
+ switch (i&0x00080080) {
+ case 0:
+ return 0;
+ case 0x80:
+ return 1;
+ default:
+ fabort();
+ }
+ return 0;
+}
+
+static uint64
+frhs(uint32 rhs)
+{
+ if (rhs & 0x8) {
+ return fpdconst[rhs&0x7];
+ } else {
+ return m->freg[rhs&0x7];
+ }
+}
+
+static int32
+fexp(uint64 f)
+{
+ return (int32)((uint32)(f >> 52) & 0x7ff) - DOUBLE_EXPBIAS;
+}
+
+static uint32
+fsign(uint64 f)
+{
+ return (uint32)(f >> 63) & 0x1;
+}
+
+static uint64
+fmantissa(uint64 f)
+{
+ return f &0x000fffffffffffffll;
+}
+
+static void
+fprint()
{
uint32 i;
- uint32 c;
+ for (i = 0; i < 8; i++) {
+ printf("\tf%d:\t%X\n", i, m->freg[i]);
+ }
+}
+
+static uint32
+d2s(uint64 d)
+{
+ return (d>>32 & 0x80000000) | //sign
+ ((uint32)(fexp(d) + SINGLE_EXPBIAS) & 0xff) << 23 | // exponent
+ (d >> 29 & 0x7fffff); // mantissa
+}
+
+static uint64
+s2d(uint32 s)
+{
+ return (uint64)(s & 0x80000000) << 63 | // sign
+ (uint64)((s >> 23 &0xff) + (DOUBLE_EXPBIAS - SINGLE_EXPBIAS)) << 52 | // exponent
+ (uint64)(s & 0x7fffff) << 29; // mantissa
+}
+
+// cdp, data processing instructions
+static void
+dataprocess(uint32* pc)
+{
+ uint32 i, opcode, unary, dest, lhs, rhs, prec;
+ uint32 high;
+ uint64 fraw0, fraw1, exp, sign;
+ uint64 fd, f0, f1;
+
+ i = *pc;
+
+ // data processing
+ opcode = i>>20 & 15;
+ unary = i>>15 & 1;
+
+ dest = i>>12 & 7;
+ lhs = i>>16 & 7;
+ rhs = i & 15;
+
+ prec = precision(i);
+ if (unary) {
+ switch (opcode) {
+ case 0: // mvf
+ m->freg[dest] = frhs(rhs);
+ goto ret;
+ default:
+ goto undef;
+ }
+ } else {
+ switch (opcode) {
+ case 1: // muf
+ fraw0 = m->freg[lhs];
+ fraw1 = frhs(rhs);
+ f0 = fraw0>>21 & 0xffffffff | 0x80000000;
+ f1 = fraw1>>21 & 0xffffffff | 0x80000000;
+ fd = f0*f1;
+ high = fd >> 63;
+ if (high)
+ fd = fd >> 11 & 0x000fffffffffffffll;
+ else
+ fd = fd >> 10 & 0x000fffffffffffffll;
+ exp = (uint64)(fexp(fraw0) + fexp(fraw1) + !!high + DOUBLE_EXPBIAS) & 0x7ff;
+ sign = fraw0 >> 63 ^ fraw1 >> 63;
+ fd = sign << 63 | exp <<52 | fd;
+ m->freg[dest] = fd;
+ goto ret;
+ default:
+ goto undef;
+ }
+ }
+
+undef:
+ doabort = 1;
+
+ret:
+ if (trace || doabort) {
+ printf(" %p %x\t%s%s\tf%d, ", pc, *pc, opnames[opcode | unary<<4],
+ fpprec[prec], dest);
+ if (!unary)
+ printf("f%d, ", lhs);
+ if (rhs & 0x8)
+ printf("#%s\n", fpconst[rhs&0x7]);
+ else
+ printf("f%d\n", rhs&0x7);
+ }
+ if (doabort)
+ fabort();
+}
+
+#define CPSR 14
+#define FLAGS_N (1 << 31)
+#define FLAGS_Z (1 << 30)
+#define FLAGS_C (1 << 29)
+
+// cmf, compare floating point
+static void
+compare(uint32 *pc, uint32 *regs) {
+ uint32 i, flags, lhs, rhs, sign0, sign1;
+ uint32 f0, f1, mant0, mant1;
+ int32 exp0, exp1;
+
+ i = *pc;
+ flags = 0;
+ lhs = i>>16 & 0x7;
+ rhs = i & 0xf;
+
+ f0 = m->freg[lhs];
+ f1 = frhs(rhs);
+ if (f0 == f1) {
+ flags = FLAGS_Z | FLAGS_C;
+ goto ret;
+ }
+
+ sign0 = fsign(f0);
+ sign1 = fsign(f1);
+ if (sign0 == 1 && sign1 == 0) {
+ flags = FLAGS_N;
+ goto ret;
+ }
+ if (sign0 == 0 && sign1 == 1) {
+ flags = FLAGS_C;
+ goto ret;
+ }
+
+ if (sign0 == 0) {
+ exp0 = fexp(f0);
+ exp1 = fexp(f1);
+ mant0 = fmantissa(f0);
+ mant1 = fmantissa(f1);
+ } else {
+ exp0 = fexp(f1);
+ exp1 = fexp(f0);
+ mant0 = fmantissa(f1);
+ mant1 = fmantissa(f0);
+ }
+
+ if (exp0 > exp1) {
+ flags = FLAGS_C;
+ } else if (exp0 < exp1) {
+ flags = FLAGS_N;
+ } else {
+ if (mant0 > mant1)
+ flags = FLAGS_C;
+ else
+ flags = FLAGS_N;
+ }
+
+ret:
+ if (trace) {
+ printf(" %p %x\tcmf\tf%d, ", pc, *pc, lhs);
+ if (rhs & 0x8)
+ printf("#%s\n", fpconst[rhs&0x7]);
+ else
+ printf("f%d\n", rhs&0x7);
+ }
+ regs[CPSR] = regs[CPSR] & 0x0fffffff | flags;
+}
+
+// ldf/stf, load/store floating
+static void
+loadstore(uint32 *pc, uint32 *regs)
+{
+ uint32 i, isload, coproc, ud, wb, tlen, p, reg, freg, offset;
+ uint32 addr;
+
+ i = *pc;
+ coproc = i>>8&0xf;
+ isload = i>>20&1;
+ p = i>>24&1;
+ ud = i>>23&1;
+ tlen = i>>(22 - 1)&1 | i>>15&1;
+ wb = i>>21&1;
+ reg = i>>16 &0xf;
+ freg = i>>12 &0x7;
+ offset = (i&0xff) << 2;
+
+ if (coproc != 1 || p != 1 || wb != 0 || tlen > 1)
+ goto undef;
+ if (reg > 13)
+ goto undef;
+
+ if (ud)
+ addr = regs[reg] + offset;
+ else
+ addr = regs[reg] - offset;
+
+ if (isload)
+ if (tlen)
+ m->freg[freg] = *((uint64*)addr);
+ else
+ m->freg[freg] = s2d(*((uint32*)addr));
+ else
+ if (tlen)
+ *((uint64*)addr) = m->freg[freg];
+ else
+ *((uint32*)addr) = d2s(m->freg[freg]);
+ goto ret;
+
+undef:
+ doabort = 1;
+
+ret:
+ if (trace || doabort) {
+ if (isload)
+ printf(" %p %x\tldf", pc, *pc);
+ else
+ printf(" %p %x\tstf", pc, *pc);
+ printf("%s\t\tf%d, %s%d(r%d)", fpprec[tlen], freg, ud ? "" : "-", offset, reg);
+ printf("\t\t// %p", regs[reg] + (ud ? offset : -offset));
+ if (coproc != 1 || p != 1 || wb != 0)
+ printf(" coproc: %d pre: %d wb %d", coproc, p, wb);
+ printf("\n");
+ fprint();
+ }
+ if (doabort)
+ fabort();
+}
+
+static void
+loadconst(uint32 *pc, uint32 *regs)
+{
+ uint32 offset;
+ uint32 *addr;
+
+ if (*pc & 0xfffff000 != 0xe59fb838 ||
+ *(pc+1) != 0xe08bb00c ||
+ *(pc+2) & 0xffff8fff != 0xed9b0100)
+ goto undef;
+
+ offset = *pc & 0xfff;
+ addr = (uint32*)((uint8*)pc + offset + 8);
+//printf("DEBUG: addr %p *addr %x final %p\n", addr, *addr, *addr + regs[12]);
+ regs[11] = *addr + regs[12];
+ loadstore(pc + 2, regs);
+ goto ret;
+
+undef:
+ doabort = 1;
+
+ret:
+ if (trace || doabort) {
+ printf(" %p coproc const %x %x %x\n", pc, *pc, *(pc+1), *(pc+2));
+ }
+ if (doabort)
+ fabort();
+}
+
+
+// returns number of words that the fp instruction is occupying, 0 if next instruction isn't float.
+// TODO(kaib): insert sanity checks for coproc 1
+static uint32
+stepflt(uint32 *pc, uint32 *regs)
+{
+ uint32 i, c;
i = *pc;
c = i >> 25 & 7;
switch(c) {
case 6: // 110
-//printf(" %p coproc multi: %x\n", pc, i);
+ loadstore(pc, regs);
return 1;
case 7: // 111
if (i>>24 & 1) return 0; // ignore swi
-//printf(" %p coproc %x\n", pc, i);
+
+ if (i>>4 & 1) { //data transfer
+ if ((i&0x00f0ff00) != 0x0090f100) {
+ printf(" %p %x\n", pc, i);
+ fabort();
+ }
+ compare(pc, regs);
+ } else {
+ dataprocess(pc);
+ }
return 1;
}
c = ((*pc & 0x0f000000) >> 16) |
((*(pc + 1) & 0x0f000000) >> 20) |
((*(pc + 2) & 0x0f000000) >> 24);
- if(c == 0x50d) {
-//printf(" %p coproc const %x\n", pc, i);
+ if(c == 0x50d) { // 0101 0000 1101
+ loadconst(pc, regs);
return 3;
}
-//printf(" %p %x\n", pc, i);
return 0;
}
_sfloat2(uint32 *lr, uint32 r0)
{
uint32 skip;
+ uint32 cpsr;
-//printf("softfloat: pre %p\n", lr);
- while(skip = isfltinstr(lr))
+ while(skip = stepflt(lr, &r0)) {
lr += skip;
-//printf(" post: %p\n", lr);
+ }
return lr;
}
-