Module: xenomai-gch Branch: for-arm64 Commit: 2ec7f94edbe1c21dac79ec4eb8b56cf571fb5f46 URL: http://git.xenomai.org/?p=xenomai-gch.git;a=commit;h=2ec7f94edbe1c21dac79ec4eb8b56cf571fb5f46
Author: Gilles Chanteperdrix <[email protected]> Date: Thu Oct 29 00:26:57 2015 +0100 arm64: attempt to fix fpu --- kernel/cobalt/arch/arm64/thread.c | 60 ++++++++++++++++++------------------- 1 file changed, 30 insertions(+), 30 deletions(-) diff --git a/kernel/cobalt/arch/arm64/thread.c b/kernel/cobalt/arch/arm64/thread.c index 2238751..6e2ab38 100644 --- a/kernel/cobalt/arch/arm64/thread.c +++ b/kernel/cobalt/arch/arm64/thread.c @@ -36,12 +36,14 @@ #ifdef CONFIG_XENO_ARCH_FPU +DECLARE_PER_CPU(struct fpsimd_state *, fpsimd_last_state); + #define FPSIMD_EN (0x3 << 20) static inline unsigned long get_cpacr(void) { unsigned long result; - __asm__ __volatile__("mrs %0, cpacr_el1": "=r"(result)); + __asm__ ("mrs %0, cpacr_el1": "=r"(result)); return result; } @@ -55,19 +57,23 @@ static inline void set_cpacr(long val) static void enable_fpsimd(void) { - unsigned long cpacr = get_cpacr(); - cpacr |= FPSIMD_EN; - set_cpacr(cpacr); + set_cpacr(get_cpacr() | FPSIMD_EN); } int xnarch_fault_fpu_p(struct ipipe_trap_data *d) { - return (d->exception == IPIPE_TRAP_FPU_ACC); + return 0; } static inline struct fpsimd_state *get_fpu_owner(struct xnarchtcb *tcb) { - return &(tcb->core.tsp->fpsimd_state); + struct fpsimd_state *st; + + st = __this_cpu_read(fpsimd_last_state); + if (!st || st->cpu != smp_processor_id()) + return NULL; + + return st; } void xnarch_leave_root(struct xnthread *root) @@ -76,52 +82,46 @@ void xnarch_leave_root(struct xnthread *root) rootcb->fpup = get_fpu_owner(rootcb); } -void xnarch_save_fpu(struct xnthread *thread) -{ - struct xnarchtcb *tcb = &(thread->tcb); - if (xnarch_fpu_ptr(tcb)) - fpsimd_save_state(tcb->fpup); -} - void xnarch_switch_fpu(struct xnthread *from, struct xnthread *to) { struct fpsimd_state *const from_fpup = from ? from->tcb.fpup : NULL; struct fpsimd_state *const to_fpup = to->tcb.fpup; + struct xnarchtcb *to_tcb = xnthread_archtcb(to); + struct task_struct *to_ts = to_tcb->core.host_task; enable_fpsimd(); if (from_fpup == to_fpup) return; - if (from_fpup) - fpsimd_save_state(from_fpup); + if (from_fpup) { + struct xnarchtcb *from_tcb = xnthread_archtcb(from); + struct task_struct *from_ts = from_tcb->core.host_task; - fpsimd_load_state(to_fpup); + if (!xnthread_test_state(from, XNROOT) + || !test_ti_thread_flag(task_thread_info(from_ts), + TIF_FOREIGN_FPSTATE)) + fpsimd_save_state(from_fpup); + } + + if (xnthread_test_state(to, XNROOT)) { + set_ti_thread_flag(task_thread_info(to_ts), + TIF_FOREIGN_FPSTATE); + this_cpu_write(fpsimd_last_state, NULL); + } else + fpsimd_load_state(to_fpup); } int xnarch_handle_fpu_fault(struct xnthread *from, struct xnthread *to, struct ipipe_trap_data *d) { - spl_t s; - - /* FPU should already be enabled for XNFPU tasks. */ - if (xnthread_test_state(to, XNFPU)) - BUG(); - - xnlock_get_irqsave(&nklock, s); - xnthread_set_state(to, XNFPU); - xnlock_put_irqrestore(&nklock, s); - - xnarch_switch_fpu(from, to); - - return 1; + return 0; } void xnarch_init_shadow_tcb(struct xnthread *thread) { struct xnarchtcb *tcb = xnthread_archtcb(thread); tcb->fpup = &(tcb->core.host_task->thread.fpsimd_state); - xnthread_clear_state(thread, XNFPU); } #endif /* CONFIG_XENO_ARCH_FPU */ _______________________________________________ Xenomai-git mailing list [email protected] http://xenomai.org/mailman/listinfo/xenomai-git
