5e: added FPA support

1 files changed, 148 insertions, 0 deletions

diff --git a/sys/src/cmd/5e/fpa.c b/sys/src/cmd/5e/fpa.c
new file mode 100644
index 000000000..c220de881
--- /dev/null
+++ b/sys/src/cmd/5e/fpa.c
@@ -0,0 +1,148 @@
+#include <u.h>
+#include <libc.h>
+#include <thread.h>
+#include <bio.h>
+#include <mach.h>
+#include "dat.h"
+#include "fns.h"
+
+void
+resetfpa(void)
+{
+	int i;
+	
+	P->FPSR = 0x81000000;
+	for(i = 0; i < 8; i++)
+		P->F[i] = 0;
+}
+
+void
+fpatransfer(u32int instr)
+{
+	enum {
+		fP = 1<<24,
+		fU = 1<<23,
+		fT1 = 1<<22,
+		fW = 1<<21,
+		fL = 1<<20,
+		fT0 = 1<<15,
+	};
+
+	long double *Fd;
+	u32int *Rn, addr;
+	int off;
+	void *targ;
+	Segment *seg;
+
+	Rn = P->R + ((instr >> 16) & 15);
+	Fd = P->F + ((instr >> 12) & 7);
+	if(Rn == P->R + 15)
+		invalid(instr);
+	off = (instr & 255) * 4;
+	if(!(instr  & fU))
+		off = -off;
+	addr = *Rn;
+	if(instr & fP)
+		addr += off;
+	targ = vaddr(addr, 8, &seg);
+	switch(instr & (fT0 | fT1 | fL)) {
+	case 0: *(float *) targ = *Fd; break;
+	case fL: *Fd = *(float *) targ; break;
+	case fT0: *(double *) targ = *Fd; break;
+	case fT0 | fL: *Fd = *(double *) targ; break;
+	default: invalid(instr);
+	}
+	segunlock(seg);
+	if(!(instr & fP))
+		addr += off;
+	if(instr & fW)
+		*Rn = addr;
+}
+
+static long double
+fpasecop(u32int instr)
+{
+	switch(instr & 15) {
+	case 8: return 0.0; break;
+	case 9: return 1.0; break;
+	case 10: return 2.0; break;
+	case 11: return 3.0; break;
+	case 12: return 4.0; break;
+	case 13: return 5.0; break;
+	case 14: return 0.5; break;
+	case 15: return 10.0; break;
+	}
+	return P->F[instr & 7];
+}
+
+void
+fpaoperation(u32int instr)
+{
+	long double *Fn, *Fd, op, op2, res;
+	int prec, opc;
+	
+	Fn = P->F + ((instr >> 16) & 7);
+	Fd = P->F + ((instr >> 12) & 7);
+	op2 = fpasecop(instr);
+	op = *Fn;
+	prec = ((instr >> 7) & 1) | ((instr >> 18) & 2);
+	opc = ((instr >> 20) & 15) | ((instr >> 11) & 16);
+	switch(opc) {
+	case 0: res = op + op2; break;
+	case 1: res = op * op2; break;
+	case 2: res = op - op2; break;
+	case 3: res = op2 - op; break;
+	case 4: res = op / op2; break;
+	case 5: res = op2 / op; break;
+	case 16: res = op2; break;
+	case 17: res = - op2; break;
+	case 18: res = fabs(op2); break;
+	case 19: res = (vlong) op2; break;
+	case 20: res = sqrt(op2); break;
+	default: sysfatal("unimplemented FPA operation %#x @ %8ux", opc, P->R[15] - 4);
+	}
+	switch(prec) {
+	case 0: *Fd = (float) res; break;
+	case 1: *Fd = (double) res; break;
+	case 2: *Fd = res; break;
+	default: invalid(instr);
+	}
+}
+
+void
+fparegtransfer(u32int instr)
+{
+	u32int *Rd;
+	long tmp;
+	long double *Fn, op, op2;
+	
+	Rd = P->R + ((instr >> 12) & 15);
+	Fn = P->F + ((instr >> 16) & 7);
+	op = fpasecop(instr);
+	if(Rd == P->R + 15) {
+		op2 = *Fn;
+		switch((instr >> 21) & 7) {
+		case 4: break;
+		case 5: op = - op; break;
+		default: invalid(instr);
+		}
+		if(op2 < op)
+			P->CPSR = (P->CPSR & ~FLAGS) | flN;
+		else if(op2 >= op) {
+			P->CPSR = (P->CPSR & ~FLAGS) | flC;
+			if(op2 == op)
+				P->CPSR |= flZ;
+		} else
+			P->CPSR = (P->CPSR & ~FLAGS) | flV;
+		return;
+	}
+	if(instr & (1<<3))
+		invalid(instr);
+	switch((instr >> 20) & 15) {
+	case 0: *Fn = *(long *) Rd; break;
+	case 1: tmp = op; *Rd = tmp; break;
+	case 2: P->FPSR = *Rd; break;
+	case 3: *Rd = P->FPSR; break;
+	default: invalid(instr);
+	}
+}