Fix x86 FPU bug

ChangeLog

+2006-09-13  Gilles Chanteperdrix  <gilles.chanteperdrix@laposte.net>
+
+	* include/asm-i386/bits/pod.h (xnarch_*_fpu): Restore root thread
+	fpu context consistently with ts bit value in cr0, fixes a bug
+	that occured when Xenomai preempted Linux in the middle of 
+	dup_task_struct().
+
 2006-03-16  Philippe Gerum  <rpm@xenomai.org>
 
  	* RELEASE: Xenomai 2.0.4 (Slick)

include/nucleus/asm-i386/system.h

@@ -74,6 +74,11 @@ typedef struct xnarchtcb {      /* Per-thread arch-dependent block */
     unsigned long *eipp;        /* Pointer to EIP backup area (&eip or &user->thread.eip) */
     union i387_union *fpup;     /* Pointer to the FPU backup area (&fpuenv or &user->thread.i387.f[x]save */
 
+    /* FPU context bits for root thread. */
+    unsigned is_root: 1;
+    unsigned cr0_ts: 1;
+    unsigned ts_usedfpu: 1;
+
 } xnarchtcb_t;
 
 typedef struct xnarch_fltinfo {
@@ -160,6 +165,8 @@ static inline void xnarch_leave_root (xnarchtcb_t *rootcb)
     __set_bit(cpuid,&rthal_cpu_realtime);
     /* Remember the preempted Linux task pointer. */
     rootcb->user_task = rootcb->active_task = rthal_current_host_task(cpuid);
+    rootcb->cr0_ts = (read_cr0() & 8) != 0;
+    rootcb->ts_usedfpu = (current->thread_info->status & TS_USEDFPU) != 0;
     /* So that xnarch_save_fpu() will operate on the right FPU area. */
     rootcb->fpup = &rootcb->user_task->thread.i387;
 }
@@ -336,6 +343,7 @@ static inline void xnarch_init_root_tcb (xnarchtcb_t *tcb,
     tcb->espp = &tcb->esp;
     tcb->eipp = &tcb->eip;
     tcb->fpup = NULL;
+    tcb->is_root = 1;
 }
 
 asmlinkage static void xnarch_thread_redirect (struct xnthread *self,
@@ -411,15 +419,28 @@ static inline void xnarch_save_fpu (xnarchtcb_t *tcb)
 {
     struct task_struct *task = tcb->user_task;
     
-    if(task)
-        {
-        if(!(task->thread_info->status & TS_USEDFPU))
-            return;
+    if (!tcb->is_root)
+	{
+        if (task)
+	    {	
+	    if(!(task->thread_info->status & TS_USEDFPU))
+		return;
+
+	    /* Tell Linux that we already saved the state of the FPU
+	       hardware of this task. */
+	    task->thread_info->status &= ~TS_USEDFPU;
+	    }
+	}
+    else
+	{
+	    /* Do not save root context FPU if cr0 bit ts is armed . */
+	    if (tcb->cr0_ts)
+		return;
+
+	    if (tcb->ts_usedfpu)
+		task->thread_info->status &= ~TS_USEDFPU;
+	}
 
-        /* Tell Linux that we already saved the state of the FPU hardware
-           of this task. */
-        task->thread_info->status &= ~TS_USEDFPU;
-        }
 
     clts();
     
@@ -434,18 +455,33 @@ static inline void xnarch_restore_fpu (xnarchtcb_t *tcb)
 {
     struct task_struct *task = tcb->user_task;
 
-    if (task)
-        {
-        if (!xnarch_fpu_init_p(task))
-            {
-            stts();
-            return;     /* Uninit fpu area -- do not restore. */
-            }
-
-        /* Tell Linux that this task has altered the state of the FPU
-           hardware. */
-        task->thread_info->status |= TS_USEDFPU;
-        }
+    if (!tcb->is_root)
+	{
+	if (task)
+	    {
+	    if (!xnarch_fpu_init_p(task))
+		{
+		stts();
+		return;	/* Uninit fpu area -- do not restore. */
+		}
+		    
+	    /* Tell Linux that this task has altered the state of
+	     * the FPU hardware. */
+	    task->thread_info->status |= TS_USEDFPU;
+	    }
+	}
+    else
+	{
+	/* Restore state of ts bit if armed. */
+	if (tcb->cr0_ts)
+	    {
+	    stts();
+	    return;
+	    }
+
+	if (tcb->ts_usedfpu)
+	    task->thread_info->status |= TS_USEDFPU;
+	}
 
     /* Restore the FPU hardware with valid fp registers from a
        user-space or kernel thread. */
@@ -462,8 +498,30 @@ static inline void xnarch_enable_fpu(xnarchtcb_t *tcb)
 {
     struct task_struct *task = tcb->user_task;
 
-    if (task && !(task->thread_info->status & TS_USEDFPU))
-        return;
+    if (!tcb->is_root)
+	{
+	if (task)
+	    {
+	    if (!xnarch_fpu_init_p(task))
+		return;
+
+	    /* If "task" switched while in Linux domain, its FPU
+	     * context may have been overriden, restore it. */
+	    if (!(task->thread_info->status & TS_USEDFPU))
+		{
+		xnarch_restore_fpu(tcb);
+		return;
+		}
+	    }
+	}
+    else
+	{
+	if (tcb->cr0_ts)
+	    return;
+
+	xnarch_restore_fpu(tcb);
+	return;
+	}
 
     clts();
 
@@ -505,6 +563,7 @@ static inline void xnarch_init_tcb (xnarchtcb_t *tcb)
     tcb->espp = &tcb->esp;
     tcb->eipp = &tcb->eip;
     tcb->fpup = &tcb->fpuenv;
+    tcb->is_root = 0;
     /* Must be followed by xnarch_init_thread(). */
 }