Module: xenomai-gch
Branch: for-arm64
Commit: 2ec7f94edbe1c21dac79ec4eb8b56cf571fb5f46
URL:    
http://git.xenomai.org/?p=xenomai-gch.git;a=commit;h=2ec7f94edbe1c21dac79ec4eb8b56cf571fb5f46

Author: Gilles Chanteperdrix <gilles.chanteperd...@xenomai.org>
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
Xenomai-git@xenomai.org
http://xenomai.org/mailman/listinfo/xenomai-git

Reply via email to