5e: added FPA support

6 files changed, 166 insertions, 2 deletions

diff --git a/sys/src/cmd/5e/arm.c b/sys/src/cmd/5e/arm.c
index 507c46361..bacdab853 100644
--- a/sys/src/cmd/5e/arm.c
+++ b/sys/src/cmd/5e/arm.c
@@ -19,7 +19,7 @@ enum {
 	fH = 1<<5,
 };
 
-static void
+void
 invalid(u32int instr)
 {
 	suicide("undefined instruction %8ux @ %8ux", instr, P->R[15] - 4);
@@ -467,6 +467,12 @@ step(void)
 		syscall();
 	else if((instr & (7<<25)) == (4 << 25))
 		block(instr);
+	else if((instr & 0x0E000F00) == 0x0C000100)
+		fpatransfer(instr);
+	else if((instr & 0x0E000F10) == 0x0E000100)
+		fpaoperation(instr);
+	else if((instr & 0x0E000F10) == 0x0E000110)
+		fparegtransfer(instr);
 	else
 		invalid(instr);
 }
diff --git a/sys/src/cmd/5e/dat.h b/sys/src/cmd/5e/dat.h
index 153b99ee3..d904c8a7b 100644
--- a/sys/src/cmd/5e/dat.h
+++ b/sys/src/cmd/5e/dat.h
@@ -32,6 +32,9 @@ struct Process {
 	Segment *S[SEGNUM];	/* memory */
 	u32int R[16];		/* general purpose registers / PC (R15) */
 	u32int CPSR;		/* status register */
+	
+	u32int FPSR;
+	long double F[8];
 
 	char errbuf[ERRMAX];
 	Fd *fd;			/* bitmap of OCEXEC files */
diff --git a/sys/src/cmd/5e/fns.h b/sys/src/cmd/5e/fns.h
index e3966136f..2d28c522e 100644
--- a/sys/src/cmd/5e/fns.h
+++ b/sys/src/cmd/5e/fns.h
@@ -30,3 +30,8 @@ Process *findproc(int);
 void donote(char *, ulong);
 void addnote(char *);
 void dump(void);
+void resetfpa(void);
+void invalid(u32int);
+void fpatransfer(u32int);
+void fpaoperation(u32int);
+void fparegtransfer(u32int);
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);
+	}
+}
diff --git a/sys/src/cmd/5e/mkfile b/sys/src/cmd/5e/mkfile
index 496fa8346..6eab4fb5b 100644
--- a/sys/src/cmd/5e/mkfile
+++ b/sys/src/cmd/5e/mkfile
@@ -1,7 +1,7 @@
 </$objtype/mkfile
 
 TARG=5e
-OFILES=5e.$O seg.$O proc.$O util.$O arm.$O sys.$O fs.$O
+OFILES=5e.$O seg.$O proc.$O util.$O arm.$O sys.$O fs.$O fpa.$O
 HFILES=dat.h fns.h
 BIN=/$objtype/bin
 
diff --git a/sys/src/cmd/5e/proc.c b/sys/src/cmd/5e/proc.c
index 2639ea696..85768df56 100644
--- a/sys/src/cmd/5e/proc.c
+++ b/sys/src/cmd/5e/proc.c
@@ -22,6 +22,7 @@ initproc(void)
 	plist.next = P;
 	P->prev = &plist;
 	P->next = &plist;
+	resetfpa();
 }
 
 void
@@ -202,6 +203,7 @@ loadtext(char *file, int argc, char **argv)
 	close(fd);
 	fdclear(P->fd);
 	initstack(argc, argv);
+	resetfpa();
 	return 0;
 }