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 <[email protected]> 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 [email protected] https://mail.gna.org/listinfo/xenomai-git
