Module: xenomai-2.5
Branch: master
Commit: d692877ab5f73a2303e1ad1291a6cf7a12b8e286
URL:    
http://git.xenomai.org/?p=xenomai-2.5.git;a=commit;h=d692877ab5f73a2303e1ad1291a6cf7a12b8e286

Author: Gilles Chanteperdrix <gilles.chanteperd...@xenomai.org>
Date:   Tue May  4 01:35:19 2010 +0200

arm: handle correctly primary mode VFP exceptions

---

 include/asm-arm/bits/pod.h |   16 +++-
 include/asm-arm/hal.h      |    5 +-
 include/asm-arm/system.h   |  231 +++++++++++++++++++++++---------------------
 ksrc/arch/arm/switch.S     |   12 +-
 4 files changed, 147 insertions(+), 117 deletions(-)

diff --git a/include/asm-arm/bits/pod.h b/include/asm-arm/bits/pod.h
index f8965f7..9304250 100644
--- a/include/asm-arm/bits/pod.h
+++ b/include/asm-arm/bits/pod.h
@@ -142,8 +142,20 @@ static inline void xnarch_enable_fpu(xnarchtcb_t *tcb)
           pick the correct FPU context.
        */
        if (likely(!tcb->is_root)
-           || (tcb->fpup && tcb->fpup == rthal_task_fpenv(tcb->user_task)))
-               rthal_enable_fpu();
+           || (tcb->fpup && tcb->fpup == rthal_task_fpenv(tcb->user_task))) {
+               unsigned fpexc = rthal_enable_fpu();
+               if (likely(!(fpexc & RTHAL_VFP_ANY_EXC)
+                          && !(rthal_vfp_fmrx(FPSCR) & FPSCR_IXE)))
+                       return;
+
+               /* If current process has pending exceptions it is
+                  illegal to restore the FPEXC register with them, we must
+                  save the fpu state and disable them, to get linux
+                  fpu fault handler take care of them correctly. */
+               rthal_save_fpu(tcb->fpup, fpexc);
+               last_VFP_context[smp_processor_id()] = NULL;
+               rthal_disable_fpu();
+       }
 #else /* !CONFIG_VFP */
        if (!tcb->user_task)
                rthal_enable_fpu();
diff --git a/include/asm-arm/hal.h b/include/asm-arm/hal.h
index 1d731aa..1787094 100644
--- a/include/asm-arm/hal.h
+++ b/include/asm-arm/hal.h
@@ -249,10 +249,13 @@ extern union vfp_state *last_VFP_context[NR_CPUS];
 #define rthal_disable_fpu() \
     rthal_vfp_fmxr(FPEXC, rthal_vfp_fmrx(FPEXC) & ~FPEXC_EN)
 
+#define RTHAL_VFP_ANY_EXC \
+       (FPEXC_EX|FPEXC_DEX|FPEXC_FP2V|FPEXC_VV|FPEXC_TRAP_MASK)
+
 #define rthal_enable_fpu()                                     \
     ({                                                         \
        unsigned _fpexc = rthal_vfp_fmrx(FPEXC) | FPEXC_EN;     \
-       rthal_vfp_fmxr(FPEXC, _fpexc);                          \
+       rthal_vfp_fmxr(FPEXC, _fpexc & ~RTHAL_VFP_ANY_EXC);     \
        _fpexc;                                                 \
     })
 
diff --git a/include/asm-arm/system.h b/include/asm-arm/system.h
index ebc1b7d..3cbf43b 100644
--- a/include/asm-arm/system.h
+++ b/include/asm-arm/system.h
@@ -3,7 +3,7 @@
  *
  * ARM port
  *   Copyright (C) 2005 Stelian Pop
- *   
+ *
  * Xenomai is free software; you can redistribute it and/or modify it
  * under the terms of the GNU General Public License as published by
  * the Free Software Foundation; either version 2 of the License, or
@@ -11,7 +11,7 @@
  *
  * Xenomai is distributed in the hope that it will be useful, but
  * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.         See the GNU
  * General Public License for more details.
  *
  * You should have received a copy of the GNU General Public License
@@ -29,66 +29,66 @@
 #include <asm-generic/xenomai/system.h>
 #include <asm/xenomai/syscall.h>
 
-#define XNARCH_THREAD_STACKSZ   4096
+#define XNARCH_THREAD_STACKSZ  4096
 
-#define xnarch_stack_size(tcb)  ((tcb)->stacksize)
+#define xnarch_stack_size(tcb) ((tcb)->stacksize)
 #define xnarch_stack_base(tcb) ((tcb)->stackbase)
 #define xnarch_stack_end(tcb)  ((caddr_t)(tcb)->stackbase - (tcb)->stacksize)
-#define xnarch_user_task(tcb)   ((tcb)->user_task)
-#define xnarch_user_pid(tcb)    ((tcb)->user_task->pid)
+#define xnarch_user_task(tcb)  ((tcb)->user_task)
+#define xnarch_user_pid(tcb)   ((tcb)->user_task->pid)
 
 struct xnthread;
 struct task_struct;
 
 typedef struct xnarchtcb {  /* Per-thread arch-dependent block */
 
-    /* Kernel mode side */
+       /* Kernel mode side */
 
 #ifdef CONFIG_XENO_HW_FPU
-    rthal_fpenv_t fpuenv;
-    rthal_fpenv_t *fpup;       /* Pointer to the FPU backup area */
-    struct task_struct *user_fpu_owner;
-    /* Pointer the the FPU owner in userspace:
-       - NULL for RT K threads,
-       - last_task_used_math for Linux US threads (only current or NULL when 
MP)
-       - current for RT US threads.
-    */
-    unsigned is_root;
-#define xnarch_fpu_ptr(tcb)     ((tcb)->fpup)
+       rthal_fpenv_t fpuenv;
+       rthal_fpenv_t *fpup;    /* Pointer to the FPU backup area */
+       struct task_struct *user_fpu_owner;
+       /* Pointer the the FPU owner in userspace:
+          - NULL for RT K threads,
+          - last_task_used_math for Linux US threads (only current or NULL 
when MP)
+          - current for RT US threads.
+       */
+       unsigned is_root;
+#define xnarch_fpu_ptr(tcb)    ((tcb)->fpup)
 #else /* !CONFIG_XENO_HW_FPU */
-#define xnarch_fpu_ptr(tcb)     NULL
+#define xnarch_fpu_ptr(tcb)    NULL
 #endif /* CONFIG_XENO_HW_FPU */
 
-    unsigned stacksize;         /* Aligned size of stack (bytes) */
-    unsigned long *stackbase;   /* Stack space */
+       unsigned stacksize;         /* Aligned size of stack (bytes) */
+       unsigned long *stackbase;   /* Stack space */
 
-    /* User mode side */
-    struct task_struct *user_task;      /* Shadowed user-space task */
-    struct task_struct *active_task;    /* Active user-space task */
-    struct mm_struct *mm;
-    struct mm_struct *active_mm;
-    struct thread_info ti;              /* Holds kernel-based thread info */
-    struct thread_info *tip;            /* Pointer to the active thread info 
(ti or user->thread_info). */
+       /* User mode side */
+       struct task_struct *user_task;      /* Shadowed user-space task */
+       struct task_struct *active_task;    /* Active user-space task */
+       struct mm_struct *mm;
+       struct mm_struct *active_mm;
+       struct thread_info ti;              /* Holds kernel-based thread info */
+       struct thread_info *tip;            /* Pointer to the active thread 
info (ti or user->thread_info). */
 
-    /* Init block */
-    struct xnthread *self;
-    int imask;
-    const char *name;
-    void (*entry)(void *cookie);
-    void *cookie;
+       /* Init block */
+       struct xnthread *self;
+       int imask;
+       const char *name;
+       void (*entry)(void *cookie);
+       void *cookie;
 
 } xnarchtcb_t;
 
 typedef struct xnarch_fltinfo {
 
-    unsigned exception;
-    struct pt_regs *regs;
+       unsigned exception;
+       struct pt_regs *regs;
 
 } xnarch_fltinfo_t;
 
-#define xnarch_fault_trap(fi)   ((fi)->exception)
-#define xnarch_fault_code(fi)   (0)
-#define xnarch_fault_pc(fi)     ((fi)->regs->ARM_pc - (thumb_mode((fi)->regs) 
? 2 : 4)) /* XXX ? */
+#define xnarch_fault_trap(fi)  ((fi)->exception)
+#define xnarch_fault_code(fi)  (0)
+#define xnarch_fault_pc(fi)    ((fi)->regs->ARM_pc - (thumb_mode((fi)->regs) ? 
2 : 4)) /* XXX ? */
 #ifndef CONFIG_XENO_HW_FPU
 /* It is normal on ARM for user-space support running with a kernel compiled
    with FPU support to make FPU faults, even on the context of real-time 
threads
@@ -97,90 +97,105 @@ typedef struct xnarch_fltinfo {
 #else /* CONFIG_XENO_HW_FPU */
 static inline int xnarch_fault_fpu_p(struct xnarch_fltinfo *fi)
 {
-    /* This function does the same thing to decode the faulting instruct as
-       "call_fpe" in arch/arm/entry-armv.S */
-    static unsigned copro_to_exc[16] = {
-       IPIPE_TRAP_UNDEFINSTR,
-       /* FPE */
-       IPIPE_TRAP_FPU, IPIPE_TRAP_FPU,
-       IPIPE_TRAP_UNDEFINSTR,
+       /* This function does the same thing to decode the faulting instruct as
+          "call_fpe" in arch/arm/entry-armv.S */
+       static unsigned copro_to_exc[16] = {
+               IPIPE_TRAP_UNDEFINSTR,
+               /* FPE */
+               IPIPE_TRAP_FPU, IPIPE_TRAP_FPU,
+               IPIPE_TRAP_UNDEFINSTR,
 #ifdef CONFIG_CRUNCH
-       IPIPE_TRAP_FPU, IPIPE_TRAP_FPU, IPIPE_TRAP_FPU,
+               IPIPE_TRAP_FPU, IPIPE_TRAP_FPU, IPIPE_TRAP_FPU,
 #else /* !CONFIG_CRUNCH */
-       IPIPE_TRAP_UNDEFINSTR, IPIPE_TRAP_UNDEFINSTR, IPIPE_TRAP_UNDEFINSTR,
+               IPIPE_TRAP_UNDEFINSTR, IPIPE_TRAP_UNDEFINSTR, 
IPIPE_TRAP_UNDEFINSTR,
 #endif /* !CONFIG_CRUNCH */
-       IPIPE_TRAP_UNDEFINSTR, IPIPE_TRAP_UNDEFINSTR, IPIPE_TRAP_UNDEFINSTR,
+               IPIPE_TRAP_UNDEFINSTR, IPIPE_TRAP_UNDEFINSTR, 
IPIPE_TRAP_UNDEFINSTR,
 #ifdef CONFIG_VFP
-       IPIPE_TRAP_VFP, IPIPE_TRAP_VFP,
+               IPIPE_TRAP_VFP, IPIPE_TRAP_VFP,
 #else /* !CONFIG_VFP */
-       IPIPE_TRAP_UNDEFINSTR, IPIPE_TRAP_UNDEFINSTR,
+               IPIPE_TRAP_UNDEFINSTR, IPIPE_TRAP_UNDEFINSTR,
 #endif /* !CONFIG_VFP */
-       IPIPE_TRAP_UNDEFINSTR, IPIPE_TRAP_UNDEFINSTR,
-       IPIPE_TRAP_UNDEFINSTR, IPIPE_TRAP_UNDEFINSTR,
-    };
-    unsigned instr, exc, cp;
-    char *pc;
-
-    if (fi->exception == IPIPE_TRAP_FPU || fi->exception == IPIPE_TRAP_VFP)
-       return 1;
-
-    /* When an FPU fault occurs in user-mode, it will be properly resolved
-       before __ipipe_dispatch_event is called. */
-    if (fi->exception != IPIPE_TRAP_UNDEFINSTR || xnarch_fault_um(fi))
-       return 0;
-
-    pc = (char *) xnarch_fault_pc(fi);
-    if (unlikely(thumb_mode(fi->regs))) {
-       unsigned short thumbh, thumbl;
-
-       thumbh = *(unsigned short *) pc;
-       thumbl = *((unsigned short *) pc + 1);
-       
-       if ((thumbh & 0x0000f800) < 0x0000e800)
-           return 0;
-       instr = (thumbh << 16) | thumbl;
+               IPIPE_TRAP_UNDEFINSTR, IPIPE_TRAP_UNDEFINSTR,
+               IPIPE_TRAP_UNDEFINSTR, IPIPE_TRAP_UNDEFINSTR,
+       };
+       unsigned instr, exc, cp;
+       char *pc;
 
-#ifdef CONFIG_NEON
-       if ((instr & 0xef000000) == 0xef000000
-           || (instr & 0xff100000) == 0xf9000000) {
-               fi->exception = IPIPE_TRAP_VFP;
+       if (fi->exception == IPIPE_TRAP_FPU)
                return 1;
-       }
+
+#ifdef CONFIG_VFP
+       if (fi->exception == IPIPE_TRAP_VFP)
+               goto trap_vfp;
 #endif
-    } else {
-       instr = *(unsigned *) pc;
+
+       /* When an FPU fault occurs in user-mode, it will be properly resolved
+          before __ipipe_dispatch_event is called. */
+       if (fi->exception != IPIPE_TRAP_UNDEFINSTR || xnarch_fault_um(fi))
+               return 0;
+
+       pc = (char *) xnarch_fault_pc(fi);
+       if (unlikely(thumb_mode(fi->regs))) {
+               unsigned short thumbh, thumbl;
+
+               thumbh = *(unsigned short *) pc;
+               thumbl = *((unsigned short *) pc + 1);
+
+               if ((thumbh & 0x0000f800) < 0x0000e800)
+                       return 0;
+               instr = (thumbh << 16) | thumbl;
 
 #ifdef CONFIG_NEON
-       if ((instr & 0xfe000000) == 0xf2000000
-           || (instr & 0xff100000) == 0xf4000000) {
-               fi->exception = IPIPE_TRAP_VFP;
-               return 1;
-       }
+               if ((instr & 0xef000000) == 0xef000000
+                   || (instr & 0xff100000) == 0xf9000000)
+                       goto trap_vfp;
 #endif
-    }
+       } else {
+               instr = *(unsigned *) pc;
 
-    if ((instr & 0x0c000000) != 0x0c000000)
-       return 0;
+#ifdef CONFIG_NEON
+               if ((instr & 0xfe000000) == 0xf2000000
+                   || (instr & 0xff100000) == 0xf4000000)
+                       goto trap_vfp;
+#endif
+       }
 
-    cp = (instr & 0x00000f00) >> 8;
+       if ((instr & 0x0c000000) != 0x0c000000)
+               return 0;
+
+       cp = (instr & 0x00000f00) >> 8;
 #ifdef CONFIG_IWMMXT
-    /* We need something equivalent to _TIF_USING_IWMMXT for Xenomai kernel
-       threads */
-    if (cp <= 1) {
-       fi->exception = IPIPE_TRAP_FPU;
-       return 1;
-    }
+       /* We need something equivalent to _TIF_USING_IWMMXT for Xenomai kernel
+          threads */
+       if (cp <= 1) {
+               fi->exception = IPIPE_TRAP_FPU;
+               return 1;
+       }
 #endif
 
-    fi->exception = exc = copro_to_exc[cp];
-    return exc != IPIPE_TRAP_UNDEFINSTR;
+       exc = copro_to_exc[cp];
+#ifdef CONFIG_VFP
+       if (exc == IPIPE_TRAP_VFP) {
+         trap_vfp:
+               /* If an exception is pending, the VFP fault is not really an
+                  "FPU unavailable" fault, so we return undefinstr in that
+                  case, the nucleus will let linux handle the fault. */
+               if (rthal_vfp_fmrx(FPEXC) & (FPEXC_EX|FPEXC_DEX)
+                   || rthal_vfp_fmrx(FPSCR) & FPSCR_IXE)
+                       exc = IPIPE_TRAP_UNDEFINSTR;
+               else
+                       exc = IPIPE_TRAP_VFP;
+       }
+#endif
+       fi->exception = exc;
+       return exc != IPIPE_TRAP_UNDEFINSTR;
 }
 #endif /* CONFIG_XENO_HW_FPU */
 /* The following predicates are only usable over a regular Linux stack
    context. */
-#define xnarch_fault_pf_p(fi)   ((fi)->exception == IPIPE_TRAP_ACCESS)
-#define xnarch_fault_bp_p(fi)   ((current->ptrace & PT_PTRACED) && \
-                                ((fi)->exception == IPIPE_TRAP_BREAK))
+#define xnarch_fault_pf_p(fi)  ((fi)->exception == IPIPE_TRAP_ACCESS)
+#define xnarch_fault_bp_p(fi)  ((current->ptrace & PT_PTRACED) &&      \
+                                ((fi)->exception == IPIPE_TRAP_BREAK))
 
 #define xnarch_fault_notify(fi) (!xnarch_fault_bp_p(fi))
 
@@ -190,18 +205,18 @@ extern "C" {
 
 static inline void *xnarch_alloc_host_mem (u_long bytes)
 {
-    if (bytes > 128*1024)
-       return vmalloc(bytes);
+       if (bytes > 128*1024)
+               return vmalloc(bytes);
 
-    return kmalloc(bytes,GFP_KERNEL);
+       return kmalloc(bytes,GFP_KERNEL);
 }
 
 static inline void xnarch_free_host_mem (void *chunk, u_long bytes)
 {
-    if (bytes > 128*1024)
-       vfree(chunk);
-    else
-       kfree(chunk);
+       if (bytes > 128*1024)
+               vfree(chunk);
+       else
+               kfree(chunk);
 }
 
 static inline void *xnarch_alloc_stack_mem(u_long bytes)
diff --git a/ksrc/arch/arm/switch.S b/ksrc/arch/arm/switch.S
index e30c198..20a5b34 100644
--- a/ksrc/arch/arm/switch.S
+++ b/ksrc/arch/arm/switch.S
@@ -45,7 +45,7 @@
     .text
 
 #ifdef CONFIG_VFP
-/* Copied from vfp_save_state in arch/arm/vfp/vfphw.S 
+/* Copied from vfp_save_state in arch/arm/vfp/vfphw.S
  * r0 = pointer to union vfp_state, r1 = fpexc
  */
 ENTRY(rthal_vfp_save)
@@ -63,7 +63,7 @@ ENTRY(rthal_vfp_save)
 
 /* Copied from no_old_VFP_process in arch/arm/vfp/vfphw.S
  * r0 = pointer to union vfp_state
- */    
+ */
 ENTRY(rthal_vfp_load)
     VFPFLDMIA  r0, r2                  @ reload the working registers while
                                        @ FPEXC is in a safe state
@@ -78,7 +78,7 @@ ENTRY(rthal_vfp_load)
     VFPFMXR    FPSCR, r2               @ restore status
     mov                pc, lr
 #endif
-       
+
 /*
  * Copied from __switch_to, arch/arm/kernel/entry-armv.S.
  * Right now it is identical, but who knows what the
@@ -91,7 +91,7 @@ ENTRY(rthal_vfp_load)
  *      CONFIG_HAS_TLS_REG
  *      CONFIG_VFP
  *
- *  Calling args: 
+ *  Calling args:
  * r0 = previous task_struct, r1 = previous thread_info, r2 = next thread_info
  */
 ENTRY(rthal_thread_switch)
@@ -108,7 +108,7 @@ ENTRY(rthal_thread_switch)
     strex   r5, r4, [ip]                    @ Clear exclusive monitor
 #endif
 #endif
-#if 0  
+#if 0
 #if defined(CONFIG_CPU_XSCALE) && !defined(CONFIG_IWMMXT)
     mra     r4, r5, acc0
     stmia   ip, {r4, r5}
@@ -148,7 +148,7 @@ ENTRY(rthal_thread_switch)
 #endif
 #endif
     add            r4, r2, #TI_CPU_SAVE
-#if 0  
+#if 0
 #if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 18) && defined(CONFIG_IWMMXT)
     mov            r5, r0
     mov     r0, r2


_______________________________________________
Xenomai-git mailing list
Xenomai-git@gna.org
https://mail.gna.org/listinfo/xenomai-git

Reply via email to