From a10ba643448642f2bf4ddcb2921bad6ca8a19f5b Mon Sep 17 00:00:00 2001
From: fanbingxin <fanbingxin.me@gmail.com>
Date: Sun, 11 Jul 2021 21:23:25 +0800
Subject: [PATCH] add fpu support (#31)

---
 app/phy/game.go  |  6 +++---
 bochsrc          |  4 ++--
 kernel/thread.go | 15 ++++++++++++++-
 kernel/trap.s    | 10 ++++++++++
 magefile.go      |  2 +-
 sys/sys.go       |  3 +++
 sys/sys.s        |  5 +++++
 7 files changed, 38 insertions(+), 7 deletions(-)

diff --git a/app/phy/game.go b/app/phy/game.go
index fdd91de..3e1c4ce 100644
--- a/app/phy/game.go
+++ b/app/phy/game.go
@@ -62,8 +62,8 @@ func addBox(space *cp.Space) {
 	body.SetAngularVelocity(v)
 	body.SetAngle(math.Atan2(pos.Y, pos.X))
 
-	//shape := space.AddShape(cp.NewPolyShape(body, 4, verts, cp.NewTransformIdentity(), 0))
-	shape := space.AddShape(cp.NewCircle(body, float64(size), cp.Vector{0, 0}))
+	shape := space.AddShape(cp.NewPolyShape(body, 4, verts, cp.NewTransformIdentity(), 0))
+	// shape := space.AddShape(cp.NewCircle(body, float64(size), cp.Vector{0, 0}))
 	shape.SetElasticity(0)
 	shape.SetFriction(0.7)
 }
@@ -95,7 +95,7 @@ func (g *Game) Init() {
 	planetBody = g.space.AddBody(cp.NewKinematicBody())
 	planetBody.SetAngularVelocity(0.2)
 
-	N := 1
+	N := 50
 	for i := 0; i < N; i++ {
 		addBox(g.space)
 	}
diff --git a/bochsrc b/bochsrc
index c9d586a..dd07fa0 100644
--- a/bochsrc
+++ b/bochsrc
@@ -5,8 +5,8 @@ vga: extension=vbe
 #floppya: image="images/floppy.img", status=inserted
 #floppyb: image="images/b.img", status=inserted
 #ata0-master: type=disk, path=os.img, cylinders=20, heads=16, spt=63
-ata0-master: type=disk, path=os.img
-boot: c
+ata0-master: type=cdrom, path=eggos.iso, status=inserted
+boot: cdrom
 
 com1: enabled=1,mode=file,dev="serial"
 mouse: enabled=0
diff --git a/kernel/thread.go b/kernel/thread.go
index 676d03b..f91d220 100644
--- a/kernel/thread.go
+++ b/kernel/thread.go
@@ -68,11 +68,14 @@ type TrapFrame struct {
 }
 
 type Thread struct {
-	// position of tf must be synced with trap.s
+	// position of tf and fpstate must be synced with trap.s
 	stack  uintptr
 	tf     *TrapFrame
 	kstack uintptr
 
+	// the state of fpu
+	fpstate uintptr
+
 	sigstack stackt
 	sigset   sigset
 
@@ -106,6 +109,7 @@ func allocThread() *Thread {
 	t.sigstack.ss_size = mm.PGSIZE
 	t.state = INITING
 	t.kstack = mm.Mmap(0, _THREAD_STACK_SIZE) + _THREAD_STACK_SIZE
+	t.fpstate = mm.Alloc()
 	return t
 }
 
@@ -160,6 +164,9 @@ func thread0_init() {
 	sp -= unsafe.Sizeof(TrapFrame{})
 	tf := (*TrapFrame)(unsafe.Pointer(sp))
 
+	// Because trapret restore fpstate
+	// we need a valid fpstate here
+	sys.Fxsave(t.fpstate)
 	tf.DS = _UDATA_IDX<<3 | _RPL_USER
 	tf.ES = _UDATA_IDX<<3 | _RPL_USER
 	tf.FS = _KTLS_IDX<<3 | _RPL_USER
@@ -225,6 +232,12 @@ func clone(pc, usp uintptr) int {
 	sp -= unsafe.Sizeof(TrapFrame{})
 	tf := (*TrapFrame)(unsafe.Pointer(sp))
 	*tf = *my.tf
+
+	// copy fpstate
+	fpsrc := (*[512]byte)(unsafe.Pointer(my.fpstate))
+	fpdst := (*[512]byte)(unsafe.Pointer(chld.fpstate))
+	*fpdst = *fpsrc
+
 	tf.SP = usp
 	tf.IP = pc
 	tf.AX = 0
diff --git a/kernel/trap.s b/kernel/trap.s
index ebf9394..408f3de 100644
--- a/kernel/trap.s
+++ b/kernel/trap.s
@@ -7,6 +7,7 @@ TEXT ·idt_init(SB), NOSPLIT, $0
 
 #define m_stack 0
 #define m_tf 4
+#define m_fpstate 12
 
 TEXT alltraps(SB), NOSPLIT, $0
 	PUSHAL
@@ -14,10 +15,15 @@ TEXT alltraps(SB), NOSPLIT, $0
 	PUSHW ES
 	PUSHW FS
 	PUSHW GS
+	
 
 	MOVL 0(FS), CX    // CX store mythread
 	MOVL SP, m_tf(CX)
 
+	// save FPU
+	MOVL m_fpstate(CX), DX
+	FXSAVE (DX)
+
 	// call trap(tf)
 	PUSHL SP
 	CALL  ·dotrap(SB)
@@ -28,6 +34,10 @@ TEXT ·trapret(SB), NOSPLIT, $0
 	MOVL 0(FS), CX    // CX store mythread
 	MOVL $0, m_tf(CX) // clear tf
 
+	// restore FPU
+	MOVL m_fpstate(CX), DX
+	FXRSTOR (DX)
+	
 	POPW GS
 	POPW FS
 	POPW ES
diff --git a/magefile.go b/magefile.go
index 6832c58..dcf9e29 100644
--- a/magefile.go
+++ b/magefile.go
@@ -26,7 +26,7 @@ var (
 )
 
 var (
-	GOTAGS    = "nes"
+	GOTAGS    = "nes phy"
 	GOGCFLAGS = ""
 )
 
diff --git a/sys/sys.go b/sys/sys.go
index e291803..384631d 100644
--- a/sys/sys.go
+++ b/sys/sys.go
@@ -61,3 +61,6 @@ func FuncPC(f interface{}) uintptr {
 
 //go:nosplit
 func Mfence()
+
+//go:nosplit
+func Fxsave(addr uintptr)
diff --git a/sys/sys.s b/sys/sys.s
index 45584aa..3545341 100644
--- a/sys/sys.s
+++ b/sys/sys.s
@@ -54,3 +54,8 @@ TEXT ·Flags(SB), NOSPLIT, $0-4
 TEXT ·Mfence(SB), NOSPLIT, $0
 	MFENCE
 	RET
+
+TEXT ·Fxsave(SB), NOSPLIT, $0-4
+	MOVL addr+0(FP), AX
+	FXSAVE (AX)
+	RET