[PATCH] ia64: IA32 ptrace bug-fixes

The changes done in the patch include:
1) Support for xmm registers.
   At present xmm registers are not saved/restored during
   ptrace and gdb wont show them. Patch adds new ptrace
   options (IA32_PTRACE_GETFPXREGS and IA32_PTRACE_SETFPXREGS,
   used by gdb to get/set fp+xmm state).
2) Bug fix in getting 'tag' field of fpstate
      (fsr>>16 in place of fsr>>32)
3) Bug fix in calculating fp TOS
   (it is a 3 bit field in fsr. Using (fsr>>11) & 7 in place
    of (fsr>>11) & 3)
Also, I had to add new structures in ia32.h, corresponding to
the way gdb is expecting the data. Gdb uses structures
defined in sys/user.h

arch/ia64/ia32/sys_ia32.c

@@ -2898,57 +2898,152 @@ get_fpreg (int regno, struct _fpreg_ia32 *reg, struct pt_regs *ptp, struct switc
 }
 
 static int
-save_ia32_fpstate (struct task_struct *tsk, struct _fpstate_ia32 *save)
+save_ia32_fpstate (struct task_struct *tsk, struct ia32_user_i387_struct *save)
 {
 	struct switch_stack *swp;
 	struct pt_regs *ptp;
 	int i, tos;
 
 	if (!access_ok(VERIFY_WRITE, save, sizeof(*save)))
-		return -EIO;
-	__put_user(tsk->thread.fcr, &save->cw);
-	__put_user(tsk->thread.fsr, &save->sw);
-	__put_user(tsk->thread.fsr >> 32, &save->tag);
-	__put_user(tsk->thread.fir, &save->ipoff);
-	__put_user(__USER_CS, &save->cssel);
-	__put_user(tsk->thread.fdr, &save->dataoff);
-	__put_user(__USER_DS, &save->datasel);
+		return -EFAULT;
+
+	__put_user(tsk->thread.fcr & 0xffff, &save->cwd);
+	__put_user(tsk->thread.fsr & 0xffff, &save->swd);
+	__put_user((tsk->thread.fsr>>16) & 0xffff, &save->twd);
+	__put_user(tsk->thread.fir, &save->fip);
+	__put_user((tsk->thread.fir>>32) & 0xffff, &save->fcs);
+	__put_user(tsk->thread.fdr, &save->foo);
+	__put_user((tsk->thread.fdr>>32) & 0xffff, &save->fos);
+
 	/*
 	 *  Stack frames start with 16-bytes of temp space
 	 */
 	swp = (struct switch_stack *)(tsk->thread.ksp + 16);
 	ptp = ia64_task_regs(tsk);
-	tos = (tsk->thread.fsr >> 11) & 3;
+	tos = (tsk->thread.fsr >> 11) & 7;
 	for (i = 0; i < 8; i++)
-		put_fpreg(i, &save->_st[i], ptp, swp, tos);
+		put_fpreg(i, (struct _fpreg_ia32 *)&save->st_space[4*i], ptp, swp, tos);
 	return 0;
 }
 
 static int
-restore_ia32_fpstate (struct task_struct *tsk, struct _fpstate_ia32 *save)
+restore_ia32_fpstate (struct task_struct *tsk, struct ia32_user_i387_struct *save)
 {
 	struct switch_stack *swp;
 	struct pt_regs *ptp;
-	int i, tos, ret;
-	int fsrlo, fsrhi;
+	int i, tos;
+	unsigned int fsrlo, fsrhi, num32;
 
 	if (!access_ok(VERIFY_READ, save, sizeof(*save)))
-		return(-EIO);
-	ret = __get_user(tsk->thread.fcr, (unsigned int *)&save->cw);
-	ret |= __get_user(fsrlo, (unsigned int *)&save->sw);
-	ret |= __get_user(fsrhi, (unsigned int *)&save->tag);
-	tsk->thread.fsr = ((long)fsrhi << 32) | (long)fsrlo;
-	ret |= __get_user(tsk->thread.fir, (unsigned int *)&save->ipoff);
-	ret |= __get_user(tsk->thread.fdr, (unsigned int *)&save->dataoff);
+		return(-EFAULT);
+
+	__get_user(num32, (unsigned int *)&save->cwd);
+	tsk->thread.fcr = (tsk->thread.fcr & (~0x1f3f)) | (num32 & 0x1f3f);
+	__get_user(fsrlo, (unsigned int *)&save->swd);
+	__get_user(fsrhi, (unsigned int *)&save->twd);
+	num32 = (fsrhi << 16) | fsrlo;
+	tsk->thread.fsr = (tsk->thread.fsr & (~0xffffffff)) | num32;
+	__get_user(num32, (unsigned int *)&save->fip);
+	tsk->thread.fir = (tsk->thread.fir & (~0xffffffff)) | num32;
+	__get_user(num32, (unsigned int *)&save->foo);
+	tsk->thread.fdr = (tsk->thread.fdr & (~0xffffffff)) | num32;
+
 	/*
 	 *  Stack frames start with 16-bytes of temp space
 	 */
 	swp = (struct switch_stack *)(tsk->thread.ksp + 16);
 	ptp = ia64_task_regs(tsk);
-	tos = (tsk->thread.fsr >> 11) & 3;
+	tos = (tsk->thread.fsr >> 11) & 7;
 	for (i = 0; i < 8; i++)
-		get_fpreg(i, &save->_st[i], ptp, swp, tos);
-	return ret ? -EFAULT : 0;
+		get_fpreg(i, (struct _fpreg_ia32 *)&save->st_space[4*i], ptp, swp, tos);
+	return 0;
+}
+
+static int
+save_ia32_fpxstate (struct task_struct *tsk, struct ia32_user_fxsr_struct *save)
+{
+	struct switch_stack *swp;
+	struct pt_regs *ptp;
+	int i, tos;
+	unsigned long mxcsr=0;
+	unsigned long num128[2];
+
+	if (!access_ok(VERIFY_WRITE, save, sizeof(*save)))
+		return -EFAULT;
+
+	__put_user(tsk->thread.fcr & 0xffff, &save->cwd);
+	__put_user(tsk->thread.fsr & 0xffff, &save->swd);
+	__put_user((tsk->thread.fsr>>16) & 0xffff, &save->twd);
+	__put_user(tsk->thread.fir, &save->fip);
+	__put_user((tsk->thread.fir>>32) & 0xffff, &save->fcs);
+	__put_user(tsk->thread.fdr, &save->foo);
+	__put_user((tsk->thread.fdr>>32) & 0xffff, &save->fos);
+
+        /*
+         *  Stack frames start with 16-bytes of temp space
+         */
+        swp = (struct switch_stack *)(tsk->thread.ksp + 16);
+        ptp = ia64_task_regs(tsk);
+	tos = (tsk->thread.fsr >> 11) & 7;
+        for (i = 0; i < 8; i++)
+		put_fpreg(i, (struct _fpxreg_ia32 *)&save->st_space[4*i], ptp, swp, tos);
+
+	mxcsr = ((tsk->thread.fcr>>32) & 0xff80) | ((tsk->thread.fsr>>32) & 0x3f);
+	__put_user(mxcsr & 0xffff, &save->mxcsr);
+	for (i = 0; i < 8; i++) {
+		memcpy(&(num128[0]), &(swp->f16) + i*2, sizeof(unsigned long));
+		memcpy(&(num128[1]), &(swp->f17) + i*2, sizeof(unsigned long));
+		copy_to_user(&save->xmm_space[0] + 4*i, num128, sizeof(struct _xmmreg_ia32));
+	}
+	return 0;
+}
+
+static int
+restore_ia32_fpxstate (struct task_struct *tsk, struct ia32_user_fxsr_struct *save)
+{
+	struct switch_stack *swp;
+	struct pt_regs *ptp;
+	int i, tos;
+	unsigned int fsrlo, fsrhi, num32;
+	int mxcsr;
+	unsigned long num64;
+	unsigned long num128[2];
+
+	if (!access_ok(VERIFY_READ, save, sizeof(*save)))
+		return(-EFAULT);
+
+	__get_user(num32, (unsigned int *)&save->cwd);
+	tsk->thread.fcr = (tsk->thread.fcr & (~0x1f3f)) | (num32 & 0x1f3f);
+	__get_user(fsrlo, (unsigned int *)&save->swd);
+	__get_user(fsrhi, (unsigned int *)&save->twd);
+	num32 = (fsrhi << 16) | fsrlo;
+	tsk->thread.fsr = (tsk->thread.fsr & (~0xffffffff)) | num32;
+	__get_user(num32, (unsigned int *)&save->fip);
+	tsk->thread.fir = (tsk->thread.fir & (~0xffffffff)) | num32;
+	__get_user(num32, (unsigned int *)&save->foo);
+	tsk->thread.fdr = (tsk->thread.fdr & (~0xffffffff)) | num32;
+
+	/*
+	 *  Stack frames start with 16-bytes of temp space
+	 */
+	swp = (struct switch_stack *)(tsk->thread.ksp + 16);
+	ptp = ia64_task_regs(tsk);
+	tos = (tsk->thread.fsr >> 11) & 7;
+	for (i = 0; i < 8; i++)
+	get_fpreg(i, (struct _fpxreg_ia32 *)&save->st_space[4*i], ptp, swp, tos);
+
+	__get_user(mxcsr, (unsigned int *)&save->mxcsr);
+	num64 = mxcsr & 0xff10;
+	tsk->thread.fcr = (tsk->thread.fcr & (~0xff1000000000)) | (num64<<32);
+	num64 = mxcsr & 0x3f;
+	tsk->thread.fsr = (tsk->thread.fsr & (~0x3f00000000)) | (num64<<32);
+
+	for (i = 0; i < 8; i++) {
+		copy_from_user(num128, &save->xmm_space[0] + 4*i, sizeof(struct _xmmreg_ia32));
+		memcpy(&(swp->f16) + i*2, &(num128[0]), sizeof(unsigned long));
+		memcpy(&(swp->f17) + i*2, &(num128[1]), sizeof(unsigned long));
+	}
+	return 0;
 }
 
 extern asmlinkage long sys_ptrace (long, pid_t, unsigned long, unsigned long, long, long, long,
@@ -3060,11 +3155,19 @@ sys32_ptrace (int request, pid_t pid, unsigned int addr, unsigned int data,
 		break;
 
 	      case IA32_PTRACE_GETFPREGS:
-		ret = save_ia32_fpstate(child, (struct _fpstate_ia32 *) A(data));
+		ret = save_ia32_fpstate(child, (struct ia32_user_i387_struct *) A(data));
+		break;
+
+	      case IA32_PTRACE_GETFPXREGS:
+		ret = save_ia32_fpxstate(child, (struct ia32_user_fxsr_struct *) A(data));
 		break;
 
 	      case IA32_PTRACE_SETFPREGS:
-		ret = restore_ia32_fpstate(child, (struct _fpstate_ia32 *) A(data));
+		ret = restore_ia32_fpstate(child, (struct ia32_user_i387_struct *) A(data));
+		break;
+
+	      case IA32_PTRACE_SETFPXREGS:
+		ret = restore_ia32_fpxstate(child, (struct ia32_user_fxsr_struct *) A(data));
 		break;
 
 	      case PTRACE_SYSCALL:	/* continue, stop after next syscall */

include/asm-ia64/ia32.h

@@ -130,6 +130,44 @@ struct sigcontext_ia32 {
        unsigned int cr2;
 };
 
+/* user.h */
+/*
+ * IA32 (Pentium III/4) FXSR, SSE support
+ *
+ * Provide support for the GDB 5.0+ PTRACE_{GET|SET}FPXREGS requests for
+ * interacting with the FXSR-format floating point environment.  Floating
+ * point data can be accessed in the regular format in the usual manner,
+ * and both the standard and SIMD floating point data can be accessed via
+ * the new ptrace requests.  In either case, changes to the FPU environment
+ * will be reflected in the task's state as expected.
+ */
+struct ia32_user_i387_struct {
+	int	cwd;
+	int	swd;
+	int	twd;
+	int	fip;
+	int	fcs;
+	int	foo;
+	int	fos;
+	int	st_space[20];	/* 8*10 bytes for each FP-reg = 80 bytes */
+};
+
+struct ia32_user_fxsr_struct {
+	unsigned short	cwd;
+	unsigned short	swd;
+	unsigned short	twd;
+	unsigned short	fop;
+	int	fip;
+	int	fcs;
+	int	foo;
+	int	fos;
+	int	mxcsr;
+	int	reserved;
+	int	st_space[32];	/* 8*16 bytes for each FP-reg = 128 bytes */
+	int	xmm_space[32];	/* 8*16 bytes for each XMM-reg = 128 bytes */
+	int	padding[56];
+};
+
 /* signal.h */
 #define _IA32_NSIG	       64
 #define _IA32_NSIG_BPW	       32
@@ -462,6 +500,8 @@ typedef elf_fpreg_t elf_fpregset_t[ELF_NFPREG];
 #define IA32_PTRACE_SETREGS	13
 #define IA32_PTRACE_GETFPREGS	14
 #define IA32_PTRACE_SETFPREGS	15
+#define IA32_PTRACE_GETFPXREGS	18
+#define IA32_PTRACE_SETFPXREGS	19
 
 #define ia32_start_thread(regs,new_ip,new_sp) do {				\
 	set_fs(USER_DS);							\