restore: Restore FPU state

Since at moment we stick with sigreturn restore
we need to form a proper FPU frame and set a pointer
to it inside sigreturn frame.

For this sake we read the FPU image and here are two
cases are possible

 - no fpu data at all -- nothing to restore, simpliest
   case

 - xsave frame is present but the host cpu supports only
   fxsave instruction: we refuse to continue, since it means
   there are no ymm registers on the machine where we're trying
   to restore

 - fxsave frame is present but the host cpu has xsave feature:
   at moment we refuse to continue, requiring complete match
   between "checkpoint and restore hosts", but in real we could
   extend logic and form complete xsave frame from fxsave and
   continue processing

Signed-off-by: Cyrill Gorcunov <gorcunov@openvz.org>
Signed-off-by: Pavel Emelyanov <xemul@parallels.com>

cr-restore.c

@@ -54,6 +54,7 @@
 #include "net.h"
 #include "tty.h"
 #include "cpu.h"
+#include "fpu.h"
 
 #include "protobuf.h"
 #include "protobuf/sa.pb-c.h"
@@ -1574,6 +1575,133 @@ static int prep_sched_info(struct rst_sched_param *sp, ThreadCoreEntry *tc)
 	return 0;
 }
 
+static bool valid_xsave_frame(CoreEntry *core)
+{
+	struct xsave_struct *x = NULL;
+
+	if (core->thread_info->fpregs->n_st_space < ARRAY_SIZE(x->i387.st_space)) {
+		pr_err("Corruption in FPU st_space area "
+		       "(got %li but %li expected)\n",
+		       (long)core->thread_info->fpregs->n_st_space,
+		       (long)ARRAY_SIZE(x->i387.st_space));
+		return false;
+	}
+
+	if (core->thread_info->fpregs->n_xmm_space < ARRAY_SIZE(x->i387.xmm_space)) {
+		pr_err("Corruption in FPU xmm_space area "
+		       "(got %li but %li expected)\n",
+		       (long)core->thread_info->fpregs->n_st_space,
+		       (long)ARRAY_SIZE(x->i387.xmm_space));
+		return false;
+	}
+
+	if (cpu_has_feature(X86_FEATURE_XSAVE)) {
+		if (!core->thread_info->fpregs->xsave) {
+			pr_err("FPU xsave area is missing, "
+			       "but host cpu requires it\n");
+			return false;
+		}
+		if (core->thread_info->fpregs->xsave->n_ymmh_space < ARRAY_SIZE(x->ymmh.ymmh_space)) {
+			pr_err("Corruption in FPU ymmh_space area "
+			       "(got %li but %li expected)\n",
+			       (long)core->thread_info->fpregs->xsave->n_ymmh_space,
+			       (long)ARRAY_SIZE(x->ymmh.ymmh_space));
+			return false;
+		}
+	} else {
+		if (core->thread_info->fpregs->xsave) {
+			pr_err("FPU xsave area present, "
+			       "but host cpu doesn't support it\n");
+			return false;
+		}
+		return true;
+	}
+
+	return true;
+}
+
+static void show_rt_xsave_frame(struct xsave_struct *x)
+{
+	struct fpx_sw_bytes *fpx = (void *)&x->i387.sw_reserved;
+	struct xsave_hdr_struct *xsave_hdr = &x->xsave_hdr;
+	struct i387_fxsave_struct *i387 = &x->i387;
+
+	pr_debug("xsave runtime structure\n");
+	pr_debug("-----------------------\n");
+
+	pr_debug("cwd:%x swd:%x twd:%x fop:%x mxcsr:%x mxcsr_mask:%x\n",
+		 (int)i387->cwd, (int)i387->swd, (int)i387->twd,
+		 (int)i387->fop, (int)i387->mxcsr, (int)i387->mxcsr_mask);
+
+	pr_debug("magic1:%x extended_size:%x xstate_bv:%lx xstate_size:%x\n",
+		 fpx->magic1, fpx->extended_size, fpx->xstate_bv, fpx->xstate_size);
+
+	pr_debug("xstate_bv: %lx\n", xsave_hdr->xstate_bv);
+
+	pr_debug("-----------------------\n");
+}
+
+static int sigreturn_prep_xsave_frame(struct thread_restore_args *args, CoreEntry *core)
+{
+	struct xsave_struct *x = &args->xsave;
+
+	/*
+	 * If no FPU information provided -- we're restoring
+	 * old image which has no FPU support, or the dump simply
+	 * has no FPU support at all.
+	 */
+	if (!core->thread_info->fpregs) {
+		args->has_fpu = false;
+		return 0;
+	}
+
+	if (!valid_xsave_frame(core))
+		return -1;
+
+	args->has_fpu = true;
+
+#define assign_reg(dst, src, e)		do { dst.e = (__typeof__(dst.e))src->e; } while (0)
+#define assign_array(dst, src, e)	memcpy(dst.e, (src)->e, sizeof(dst.e))
+
+	assign_reg(x->i387, core->thread_info->fpregs, cwd);
+	assign_reg(x->i387, core->thread_info->fpregs, swd);
+	assign_reg(x->i387, core->thread_info->fpregs, twd);
+	assign_reg(x->i387, core->thread_info->fpregs, fop);
+	assign_reg(x->i387, core->thread_info->fpregs, rip);
+	assign_reg(x->i387, core->thread_info->fpregs, rdp);
+	assign_reg(x->i387, core->thread_info->fpregs, mxcsr);
+	assign_reg(x->i387, core->thread_info->fpregs, mxcsr_mask);
+
+	assign_array(x->i387, core->thread_info->fpregs, st_space);
+	assign_array(x->i387, core->thread_info->fpregs, xmm_space);
+
+	if (cpu_has_feature(X86_FEATURE_XSAVE)) {
+		struct fpx_sw_bytes *fpx_sw = (void *)&x->i387.sw_reserved;
+		void *magic2;
+
+		x->xsave_hdr.xstate_bv	= XSTATE_FP | XSTATE_SSE | XSTATE_YMM;
+		assign_array(x->ymmh, core->thread_info->fpregs->xsave, ymmh_space);
+
+		fpx_sw->magic1		= FP_XSTATE_MAGIC1;
+		fpx_sw->xstate_bv	= XSTATE_FP | XSTATE_SSE | XSTATE_YMM;
+		fpx_sw->xstate_size	= sizeof(struct xsave_struct);
+		fpx_sw->extended_size	= sizeof(struct xsave_struct) + FP_XSTATE_MAGIC2_SIZE;
+
+		/*
+		 * This should be at the end of xsave frame.
+		 */
+		magic2 = args->__pad + sizeof(struct xsave_struct);
+		*(u32 *)magic2 = FP_XSTATE_MAGIC2;
+	}
+
+	show_rt_xsave_frame(x);
+
+#undef assign_reg
+#undef assign_array
+
+	return 0;
+}
+
 static int sigreturn_restore(pid_t pid, CoreEntry *core)
 {
 	long restore_task_vma_len;
@@ -1756,6 +1884,9 @@ static int sigreturn_restore(pid_t pid, CoreEntry *core)
 			goto err;
 	}
 
+	if (sigreturn_prep_xsave_frame(&task_args->t, core))
+		goto err;
+
 	/* No longer need it */
 	core_entry__free_unpacked(core, NULL);
 
@@ -1829,6 +1960,9 @@ static int sigreturn_restore(pid_t pid, CoreEntry *core)
 				goto err;
 		}
 
+		if (sigreturn_prep_xsave_frame(&thread_args[i], core))
+			goto err;
+
 		core_entry__free_unpacked(core, NULL);
 
 		pr_info("Thread %4d stack %8p heap %8p rt_sigframe %8p\n",

pie/restorer.c

@@ -141,6 +141,22 @@ static void restore_sched_info(struct rst_sched_param *p)
 	sys_sched_setscheduler(0, p->policy, &parm);
 }
 
+static int restore_fpu(struct rt_sigframe *sigframe, struct thread_restore_args *args)
+{
+	if (args->has_fpu) {
+		unsigned long addr = (unsigned long)(void *)&args->xsave;
+
+		if ((addr % 64ul) == 0ul) {
+			sigframe->uc.uc_mcontext.fpstate = &args->xsave;
+		} else {
+			pr_err("Unaligned address passed: %lx\n", addr);
+			return -1;
+		}
+	}
+
+	return 0;
+}
+
 static int restore_gpregs(struct rt_sigframe *f, UserX86RegsEntry *r)
 {
 	long ret;
@@ -204,6 +220,8 @@ static int restore_thread_common(struct rt_sigframe *sigframe,
 		sigframe->uc.uc_sigmask.sig[0] = args->blk_sigset;
 
 	restore_sched_info(&args->sp);
+	if (restore_fpu(sigframe, args))
+		return -1;
 
 	return restore_gpregs(sigframe, &args->gpregs);
 }