1 files changed, 205 insertions, 0 deletions
diff --git a/os/mpc/fp.s b/os/mpc/fp.s
new file mode 100644
index 00000000..e37bb63a
--- /dev/null
+++ b/os/mpc/fp.s
@@ -0,0 +1,205 @@
+/*
+ * support for floating-point hardware
+ */
+
+#include "mem.h"
+
+/* on some models mtmsr doesn't synchronise enough (eg, 603e) */
+#define	MSRSYNC	SYNC; ISYNC
+
+#define	FPON(X, Y)\
+	MOVW	MSR, X;\
+	OR	$FPE, X, Y;\
+	SYNC;\
+	ISYNC;\
+	MOVW	Y, MSR;\
+	MSRSYNC
+
+#define	FPOFF(X,Y)\
+	MOVW	MSR, X;\
+	RLWNM	$0, X, $~FPE, Y;\
+	SYNC;\
+	ISYNC;\
+	MOVW	Y, MSR;\
+	MSRSYNC
+
+#define	FPPREV(X)\
+	SYNC;\
+	ISYNC;\
+	MOVW	X, MSR;\
+	MSRSYNC
+
+TEXT	kfpinit(SB), $0
+	MOVFL	$0,FPSCR(7)
+	MOVFL	$0xD,FPSCR(6)	/* VE, OE, ZE */
+	MOVFL	$0, FPSCR(5)
+	MOVFL	$0, FPSCR(3)
+	MOVFL	$0, FPSCR(2)
+	MOVFL	$0, FPSCR(1)
+	MOVFL	$0, FPSCR(0)
+
+	FMOVD	$4503601774854144.0, F27
+	FMOVD	$0.5, F29
+	FSUB	F29, F29, F28
+	FADD	F29, F29, F30
+	FADD	F30, F30, F31
+	FMOVD	F28, F0
+	FMOVD	F28, F1
+	FMOVD	F28, F2
+	FMOVD	F28, F3
+	FMOVD	F28, F4
+	FMOVD	F28, F5
+	FMOVD	F28, F6
+	FMOVD	F28, F7
+	FMOVD	F28, F8
+	FMOVD	F28, F9
+	FMOVD	F28, F10
+	FMOVD	F28, F11
+	FMOVD	F28, F12
+	FMOVD	F28, F13
+	FMOVD	F28, F14
+	FMOVD	F28, F15
+	FMOVD	F28, F16
+	FMOVD	F28, F17
+	FMOVD	F28, F18
+	FMOVD	F28, F19
+	FMOVD	F28, F20
+	FMOVD	F28, F21
+	FMOVD	F28, F22
+	FMOVD	F28, F23
+	FMOVD	F28, F24
+	FMOVD	F28, F25
+	FMOVD	F28, F26
+	RETURN
+
+TEXT	getfpscr(SB), $8
+	FPON(R4, R5)
+	MOVFL	FPSCR, F3
+	FMOVD	F3, -8(SP)
+	MOVW	-4(SP), R3
+	FPPREV(R4)
+	RETURN
+
+TEXT	fpsave(SB), $0
+	FPON(R4, R4)
+
+	FMOVD	F0,0(R3)
+	FMOVD	F1,8(R3)
+	FMOVD	F2,16(R3)
+	FMOVD	F3,24(R3)
+	FMOVD	F4,32(R3)
+	FMOVD	F5,40(R3)
+	FMOVD	F6,48(R3)
+	FMOVD	F7,56(R3)
+	FMOVD	F8,64(R3)
+	FMOVD	F9,72(R3)
+	FMOVD	F10,80(R3)
+	FMOVD	F11,88(R3)
+	FMOVD	F12,96(R3)
+	FMOVD	F13,104(R3)
+	FMOVD	F14,112(R3)
+	FMOVD	F15,120(R3)
+	FMOVD	F16,128(R3)
+	FMOVD	F17,136(R3)
+	FMOVD	F18,144(R3)
+	FMOVD	F19,152(R3)
+	FMOVD	F20,160(R3)
+	FMOVD	F21,168(R3)
+	FMOVD	F22,176(R3)
+	FMOVD	F23,184(R3)
+	FMOVD	F24,192(R3)
+	FMOVD	F25,200(R3)
+	FMOVD	F26,208(R3)
+	FMOVD	F27,216(R3)
+	FMOVD	F28,224(R3)
+	FMOVD	F29,232(R3)
+	FMOVD	F30,240(R3)
+	FMOVD	F31,248(R3)
+
+	MOVFL	FPSCR, F0
+	FMOVD	F0, 256(R3)
+	MOVFL	$0,FPSCR(7)
+	MOVFL	$0xD,FPSCR(6)	/* VE, OE, ZE */
+	MOVFL	$0, FPSCR(5)
+	MOVFL	$0, FPSCR(4)
+	MOVFL	$0, FPSCR(3)
+	MOVFL	$0, FPSCR(2)
+	MOVFL	$0, FPSCR(1)
+	MOVFL	$0, FPSCR(0)
+
+	FPOFF(R4, R4)
+	RETURN
+
+TEXT	fprestore(SB), $0
+	FPON(R4, R4)
+
+	FMOVD	256(R3), F0
+	MOVFL	F0, FPSCR
+	FMOVD	0(R3), F0
+	FMOVD	8(R3), F1
+	FMOVD	16(R3), F2
+	FMOVD	24(R3), F3
+	FMOVD	32(R3), F4
+	FMOVD	40(R3), F5
+	FMOVD	48(R3), F6
+	FMOVD	56(R3), F7
+	FMOVD	64(R3), F8
+	FMOVD	72(R3), F9
+	FMOVD	80(R3), F10
+	FMOVD	88(R3), F11
+	FMOVD	96(R3), F12
+	FMOVD	104(R3), F13
+	FMOVD	112(R3), F14
+	FMOVD	120(R3), F15
+	FMOVD	128(R3), F16
+	FMOVD	136(R3), F17
+	FMOVD	144(R3), F18
+	FMOVD	152(R3), F19
+	FMOVD	160(R3), F20
+	FMOVD	168(R3), F21
+	FMOVD	176(R3), F22
+	FMOVD	184(R3), F23
+	FMOVD	192(R3), F24
+	FMOVD	200(R3), F25
+	FMOVD	208(R3), F26
+	FMOVD	216(R3), F27
+	FMOVD	224(R3), F28
+	FMOVD	232(R3), F29
+	FMOVD	240(R3), F30
+	FMOVD	248(R3), F31
+
+	RETURN
+
+TEXT	clrfptrap(SB), $0
+	FPON(R4, R5)
+	MOVFL	$0, FPSCR(5)
+	MOVFL	$0, FPSCR(3)
+	MOVFL	$0, FPSCR(2)
+	MOVFL	$0, FPSCR(1)
+	MOVFL	$0, FPSCR(0)
+	FPPREV(R4)
+	RETURN
+
+TEXT	fpinit(SB), $0
+	FPON(R4, R5)
+	BL	kfpinit(SB)
+	RETURN
+
+TEXT	fpoff(SB), $0
+	FPOFF(R4, R5)
+	RETURN
+
+
+TEXT	FPsave(SB), 1, $0
+	FPON(R4, R5)
+	MOVFL	FPSCR, F0
+	FMOVD	F0, 0(R3)
+	FPPREV(R4)
+	RETURN
+
+TEXT	FPrestore(SB), 1, $0
+	FPON(R4, R5)
+	FMOVD	0(R3), F0
+	MOVFL	F0, FPSCR
+	FPPREV(R4)
+	RETURN