From 5c190beb045dec8180b367ac89489709e457eafd Mon Sep 17 00:00:00 2001
From: Tomoki Shirasawa <sira@popeye.cc.u-tokyo.ac.jp>
Date: Tue, 5 Jul 2016 15:26:00 +0900
Subject: [PATCH] save fpregs when to call sighandler refs #50

---
 arch/x86/kernel/cpu.c     | 15 +++++++++++
 arch/x86/kernel/syscall.c | 54 ++++++++++++++++++++++++++++++++++++---
 2 files changed, 65 insertions(+), 4 deletions(-)

diff --git a/arch/x86/kernel/cpu.c b/arch/x86/kernel/cpu.c
index 3e2ddd18..d86aecbd 100644
--- a/arch/x86/kernel/cpu.c
+++ b/arch/x86/kernel/cpu.c
@@ -181,6 +181,7 @@ static void init_idt(void)
 }
 
 static int xsave_available = 0;
+static int xsave_size = 0;
 
 void init_fpu(void)
 {
@@ -224,6 +225,14 @@ void init_fpu(void)
 		xsetbv(0, reg);
 		dkprintf("init_fpu(): AVX init: XCR0 = 0x%016lX\n", reg);
 	}
+	if(xsave_available){
+		unsigned long eax;
+		unsigned long ebx;
+		unsigned long ecx;
+		unsigned long edx;
+		asm volatile("cpuid" : "=a"(eax),"=b"(ebx),"=c"(ecx),"=d"(edx) : "a" (0x0d), "c" (0x00));
+		xsave_size = ecx;
+	}
 
 	/* TODO: set MSR_IA32_XSS to enable xsaves/xrstors */
 
@@ -234,6 +243,12 @@ void init_fpu(void)
 	asm volatile("finit");
 }
 
+int
+get_xsave_size()
+{
+	return xsave_size;
+}
+
 void reload_gdt(struct x86_desc_ptr *gdt_ptr)
 {
 	asm volatile("pushq %1\n"
diff --git a/arch/x86/kernel/syscall.c b/arch/x86/kernel/syscall.c
index 44993442..782e282b 100644
--- a/arch/x86/kernel/syscall.c
+++ b/arch/x86/kernel/syscall.c
@@ -38,6 +38,7 @@ void set_signal(int sig, void *regs0, siginfo_t *info);
 void check_signal(unsigned long rc, void *regs0, int num);
 extern unsigned long do_fork(int, unsigned long, unsigned long, unsigned long,
 	unsigned long, unsigned long, unsigned long);
+extern int get_xsave_size();
 
 //#define DEBUG_PRINT_SC
 
@@ -219,6 +220,7 @@ SYSCALL_DECLARE(rt_sigreturn)
 	struct x86_user_context *regs;
 	struct sigsp ksigsp;
 	struct sigsp *sigsp;
+	int xsavesize = get_xsave_size();
 
 	asm ("movq %%gs:(%1),%0"
 			: "=r"(regs)
@@ -265,6 +267,24 @@ SYSCALL_DECLARE(rt_sigreturn)
 		check_signal(0, regs, 0);
 		check_need_resched();
 	}
+
+	if(ksigsp.fpregs && xsavesize){
+		void *fpregs = kmalloc(xsavesize + 64, IHK_MC_AP_NOWAIT);
+
+		if(fpregs){
+			unsigned int low = 0x7;
+			unsigned int high = 0;
+			struct xsave_struct *kfpregs;
+
+			kfpregs = (void *)((((unsigned long)fpregs) + 63) & ~63);
+
+			if(copy_from_user(kfpregs, ksigsp.fpregs, xsavesize))
+				return -EFAULT;
+			asm volatile("xrstor %0" : : "m"(*kfpregs), "a"(low), "d"(high) : "memory");
+			kfree(fpregs);
+		}
+	}
+
 	return sigsp->sigrc;
 }
 
@@ -707,6 +727,8 @@ do_signal(unsigned long rc, void *regs0, struct thread *thread, struct sig_pendi
 		unsigned long *usp; /* user stack */
 		struct sigsp ksigsp;
 		struct sigsp *sigsp;
+		int xsavesize = get_xsave_size();
+		unsigned long fpregs;
 
 		if((k->sa.sa_flags & SA_ONSTACK) &&
 		   !(thread->sigstack.ss_flags & SS_DISABLE) &&
@@ -719,7 +741,8 @@ do_signal(unsigned long rc, void *regs0, struct thread *thread, struct sig_pendi
 		else{
 			usp = (unsigned long *)regs->gpr.rsp;
 		}
-		sigsp = ((struct sigsp *)usp) - 1;
+		fpregs = (unsigned long)usp - xsavesize;
+		sigsp = ((struct sigsp *)fpregs) - 1;
 		sigsp = (struct sigsp *)((unsigned long)sigsp & 0xfffffffffffffff0UL);
 		memset(&ksigsp, '\0', sizeof ksigsp);
 
@@ -751,6 +774,32 @@ do_signal(unsigned long rc, void *regs0, struct thread *thread, struct sig_pendi
 		ksigsp.restart = isrestart(num, rc, sig, k->sa.sa_flags & SA_RESTART);
 		if(num != 0 && rc == -EINTR && sig == SIGCHLD)
 			ksigsp.restart = 1;
+		if(xsavesize){
+			unsigned int low = 0x7;
+			unsigned int high = 0;
+			void *_kfpregs = kmalloc(xsavesize + 64, IHK_MC_AP_NOWAIT);
+			struct xsave_struct *kfpregs;
+
+			if(!_kfpregs){
+				kfree(pending);
+				kfree(_kfpregs);
+				kprintf("do_signal,no space available\n");
+				terminate(0, sig);
+				return;
+			}
+			kfpregs = (void *)((((unsigned long)_kfpregs) + 63) & ~63);
+			memset(kfpregs, '\0', xsavesize);
+			asm volatile("xsave %0" : : "m"(*kfpregs), "a"(low), "d"(high) : "memory");
+			if(copy_to_user((void *)fpregs, kfpregs, xsavesize)){
+				kfree(pending);
+				kfree(_kfpregs);
+				kprintf("do_signal,write_process_vm failed\n");
+				terminate(0, sig);
+				return;
+			}
+			ksigsp.fpregs = (void *)fpregs;
+			kfree(_kfpregs);
+		}
 		memcpy(&ksigsp.info, &pending->info, sizeof(siginfo_t));
 
 		if(copy_to_user(sigsp, &ksigsp, sizeof ksigsp)){
@@ -761,9 +810,6 @@ do_signal(unsigned long rc, void *regs0, struct thread *thread, struct sig_pendi
 			return;
 		}
 
-
-
-
 		usp = (unsigned long *)sigsp;
 		usp--;
 		*usp = (unsigned long)k->sa.sa_restorer;