3 files changed, 111 insertions, 17 deletions

diff --git a/arch/arm/include/asm/ucontext.h b/arch/arm/include/asm/ucontext.h
index bf65e9f4525..47f023aa849 100644
--- a/arch/arm/include/asm/ucontext.h
+++ b/arch/arm/include/asm/ucontext.h
@@ -59,23 +59,22 @@ struct iwmmxt_sigframe {
 #endif /* CONFIG_IWMMXT */
 
 #ifdef CONFIG_VFP
-#if __LINUX_ARM_ARCH__ < 6
-/* For ARM pre-v6, we use fstmiax and fldmiax.  This adds one extra
- * word after the registers, and a word of padding at the end for
- * alignment.  */
 #define VFP_MAGIC		0x56465001
-#define VFP_STORAGE_SIZE	152
-#else
-#define VFP_MAGIC		0x56465002
-#define VFP_STORAGE_SIZE	144
-#endif
 
 struct vfp_sigframe
 {
 	unsigned long		magic;
 	unsigned long		size;
-	union vfp_state		storage;
-};
+	struct user_vfp		ufp;
+	struct user_vfp_exc	ufp_exc;
+} __attribute__((__aligned__(8)));
+
+/*
+ *  8 byte for magic and size, 264 byte for ufp, 12 bytes for ufp_exc,
+ *  4 bytes padding.
+ */
+#define VFP_STORAGE_SIZE	sizeof(struct vfp_sigframe)
+
 #endif /* CONFIG_VFP */
 
 /*
@@ -91,7 +90,7 @@ struct aux_sigframe {
 #ifdef CONFIG_IWMMXT
 	struct iwmmxt_sigframe	iwmmxt;
 #endif
-#if 0 && defined CONFIG_VFP /* Not yet saved.  */
+#ifdef CONFIG_VFP
 	struct vfp_sigframe	vfp;
 #endif
 	/* Something that isn't a valid magic number for any coprocessor.  */
diff --git a/arch/arm/include/asm/user.h b/arch/arm/include/asm/user.h
index df95e050f9d..05ac4b06876 100644
--- a/arch/arm/include/asm/user.h
+++ b/arch/arm/include/asm/user.h
@@ -83,11 +83,21 @@ struct user{
 
 /*
  * User specific VFP registers. If only VFPv2 is present, registers 16 to 31
- * are ignored by the ptrace system call.
+ * are ignored by the ptrace system call and the signal handler.
  */
 struct user_vfp {
 	unsigned long long fpregs[32];
 	unsigned long fpscr;
 };
 
+/*
+ * VFP exception registers exposed to user space during signal delivery.
+ * Fields not relavant to the current VFP architecture are ignored.
+ */
+struct user_vfp_exc {
+	unsigned long	fpexc;
+	unsigned long	fpinst;
+	unsigned long	fpinst2;
+};
+
 #endif /* _ARM_USER_H */
diff --git a/arch/arm/kernel/signal.c b/arch/arm/kernel/signal.c
index e7714f367eb..907d5a620bc 100644
--- a/arch/arm/kernel/signal.c
+++ b/arch/arm/kernel/signal.c
@@ -18,6 +18,7 @@
 #include <asm/cacheflush.h>
 #include <asm/ucontext.h>
 #include <asm/unistd.h>
+#include <asm/vfp.h>
 
 #include "ptrace.h"
 #include "signal.h"
@@ -175,6 +176,90 @@ static int restore_iwmmxt_context(struct iwmmxt_sigframe *frame)
 
 #endif
 
+#ifdef CONFIG_VFP
+
+static int preserve_vfp_context(struct vfp_sigframe __user *frame)
+{
+	struct thread_info *thread = current_thread_info();
+	struct vfp_hard_struct *h = &thread->vfpstate.hard;
+	const unsigned long magic = VFP_MAGIC;
+	const unsigned long size = VFP_STORAGE_SIZE;
+	int err = 0;
+
+	vfp_sync_hwstate(thread);
+	__put_user_error(magic, &frame->magic, err);
+	__put_user_error(size, &frame->size, err);
+
+	/*
+	 * Copy the floating point registers. There can be unused
+	 * registers see asm/hwcap.h for details.
+	 */
+	err |= __copy_to_user(&frame->ufp.fpregs, &h->fpregs,
+			      sizeof(h->fpregs));
+	/*
+	 * Copy the status and control register.
+	 */
+	__put_user_error(h->fpscr, &frame->ufp.fpscr, err);
+
+	/*
+	 * Copy the exception registers.
+	 */
+	__put_user_error(h->fpexc, &frame->ufp_exc.fpexc, err);
+	__put_user_error(h->fpinst, &frame->ufp_exc.fpinst, err);
+	__put_user_error(h->fpinst2, &frame->ufp_exc.fpinst2, err);
+
+	return err ? -EFAULT : 0;
+}
+
+static int restore_vfp_context(struct vfp_sigframe __user *frame)
+{
+	struct thread_info *thread = current_thread_info();
+	struct vfp_hard_struct *h = &thread->vfpstate.hard;
+	unsigned long magic;
+	unsigned long size;
+	unsigned long fpexc;
+	int err = 0;
+
+	__get_user_error(magic, &frame->magic, err);
+	__get_user_error(size, &frame->size, err);
+
+	if (err)
+		return -EFAULT;
+	if (magic != VFP_MAGIC || size != VFP_STORAGE_SIZE)
+		return -EINVAL;
+
+	/*
+	 * Copy the floating point registers. There can be unused
+	 * registers see asm/hwcap.h for details.
+	 */
+	err |= __copy_from_user(&h->fpregs, &frame->ufp.fpregs,
+				sizeof(h->fpregs));
+	/*
+	 * Copy the status and control register.
+	 */
+	__get_user_error(h->fpscr, &frame->ufp.fpscr, err);
+
+	/*
+	 * Sanitise and restore the exception registers.
+	 */
+	__get_user_error(fpexc, &frame->ufp_exc.fpexc, err);
+	/* Ensure the VFP is enabled. */
+	fpexc |= FPEXC_EN;
+	/* Ensure FPINST2 is invalid and the exception flag is cleared. */
+	fpexc &= ~(FPEXC_EX | FPEXC_FP2V);
+	h->fpexc = fpexc;
+
+	__get_user_error(h->fpinst, &frame->ufp_exc.fpinst, err);
+	__get_user_error(h->fpinst2, &frame->ufp_exc.fpinst2, err);
+
+	if (!err)
+		vfp_flush_hwstate(thread);
+
+	return err ? -EFAULT : 0;
+}
+
+#endif
+
 /*
  * Do a signal return; undo the signal stack.  These are aligned to 64-bit.
  */
@@ -233,8 +318,8 @@ static int restore_sigframe(struct pt_regs *regs, struct sigframe __user *sf)
 		err |= restore_iwmmxt_context(&aux->iwmmxt);
 #endif
 #ifdef CONFIG_VFP
-//	if (err == 0)
-//		err |= vfp_restore_state(&sf->aux.vfp);
+	if (err == 0)
+		err |= restore_vfp_context(&aux->vfp);
 #endif
 
 	return err;
@@ -348,8 +433,8 @@ setup_sigframe(struct sigframe __user *sf, struct pt_regs *regs, sigset_t *set)
 		err |= preserve_iwmmxt_context(&aux->iwmmxt);
 #endif
 #ifdef CONFIG_VFP
-//	if (err == 0)
-//		err |= vfp_save_state(&sf->aux.vfp);
+	if (err == 0)
+		err |= preserve_vfp_context(&aux->vfp);
 #endif
 	__put_user_error(0, &aux->end_magic, err);