bcm64: add experimental work in progress arm64 kernel for raspberry pi 3

1 files changed, 92 insertions, 0 deletions

diff --git a/sys/src/9/bcm64/fpu.c b/sys/src/9/bcm64/fpu.c
new file mode 100644
index 000000000..163678294
--- /dev/null
+++ b/sys/src/9/bcm64/fpu.c
@@ -0,0 +1,92 @@
+#include "u.h"
+#include "../port/lib.h"
+#include "mem.h"
+#include "dat.h"
+#include "fns.h"
+
+#include "ureg.h"
+#include "sysreg.h"
+
+/* libc */
+extern ulong getfcr(void);
+extern void setfcr(ulong fcr);
+extern ulong getfsr(void);
+extern void setfsr(ulong fsr);
+
+void
+fpuinit(void)
+{
+	fpoff();
+}
+
+void
+fpon(void)
+{
+	syswr(CPACR_EL1, 3<<20);
+}
+
+void
+fpoff(void)
+{
+	syswr(CPACR_EL1, 0<<20);
+}
+
+void
+fpinit(void)
+{
+	fpon();
+	setfcr(0);
+	setfsr(0);
+}
+
+void
+fpclear(void)
+{
+	fpoff();
+}
+
+void
+fpsave(FPsave *p)
+{
+	p->control = getfcr();
+	p->status = getfsr();
+	fpsaveregs(p->regs);
+	fpoff();
+}
+
+void
+fprestore(FPsave *p)
+{
+	fpon();
+	setfcr(p->control);
+	setfsr(p->status);
+	fploadregs(p->regs);
+}
+
+void
+mathtrap(Ureg*)
+{
+	int s;
+
+	if((up->fpstate & FPillegal) != 0){
+		postnote(up, 1, "sys: floating point in note handler", NDebug);
+		return;
+	}
+	switch(up->fpstate){
+	case FPinit:
+		s = splhi();
+		fpinit();
+		up->fpstate = FPactive;
+		splx(s);
+		break;
+	case FPinactive:
+		s = splhi();
+		fprestore(up->fpsave);
+		up->fpstate = FPactive;
+		splx(s);
+		break;
+	case FPactive:
+		postnote(up, 1, "sys: floating point error", NDebug);
+		break;
+	}
+}