/*
 * 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
