[Xenomai-git] Dmitriy Cherkasov : cobalt/arm64: FPU code cleanup

2017-07-27 Thread git repository hosting
Module: xenomai-3
Branch: next
Commit: a62c774495ac864cecd5912f80c1f37c99c85f4e
URL:
http://git.xenomai.org/?p=xenomai-3.git;a=commit;h=a62c774495ac864cecd5912f80c1f37c99c85f4e

Author: Dmitriy Cherkasov 
Date:   Fri Oct  2 12:54:37 2015 -0700

cobalt/arm64: FPU code cleanup

---

 kernel/cobalt/arch/arm64/thread.c |   89 -
 1 file changed, 39 insertions(+), 50 deletions(-)

diff --git a/kernel/cobalt/arch/arm64/thread.c 
b/kernel/cobalt/arch/arm64/thread.c
index db369be..35dbd72 100644
--- a/kernel/cobalt/arch/arm64/thread.c
+++ b/kernel/cobalt/arch/arm64/thread.c
@@ -36,31 +36,44 @@
 
 #if defined(CONFIG_XENO_ARCH_FPU)
 
-static void enable_fpsimd(void) {
-   __asm__ __volatile__("mrs x1, cpacr_el1\n\
-   orr x1, x1, #(0x3 << 20)\n\
-   msr cpacr_el1, x1\n\
-   isb" : : : "x1", "memory", "cc");
+#define FPSIMD_EN (0x3 << 20)
+
+static inline unsigned long get_cpacr(void)
+{
+   unsigned long result;
+   __asm__ __volatile__("mrs %0, cpacr_el1": "=r"(result));
+   return result;
+}
+
+static inline void set_cpacr(long val)
+{
+   __asm__ __volatile__ (
+   "msr cpacr_el1, %0\n\t"
+   "isb"
+   : /* */ : "r"(val));
 }
 
-static void disable_fpsimd(void) {
-   __asm__ __volatile__("mrs x1, cpacr_el1\n\
-   and x1, x1, #~(0x3 << 20)\n\
-   msr cpacr_el1, x1\n\
-   isb" : : : "x1", "memory", "cc");
+static void enable_fpsimd(void)
+{
+   unsigned long cpacr = get_cpacr();
+   cpacr |= FPSIMD_EN;
+   set_cpacr(cpacr);
+}
+
+static void disable_fpsimd(void)
+{
+   unsigned long cpacr = get_cpacr();
+   cpacr &= ~FPSIMD_EN;
+   set_cpacr(cpacr);
 }
 
 int xnarch_fault_fpu_p(struct ipipe_trap_data *d)
 {
-   /* check if this is an FPU access trap to be handled by Xenomai */
-   if(d->exception == IPIPE_TRAP_FPU_ACC){
-   return 1;
-   }
-   /* FPU already enabled, propagate fault to kernel */
-   return 0;
+   return (d->exception == IPIPE_TRAP_FPU_ACC);
 }
 
-static inline struct fpsimd_state *get_fpu_owner(struct xnarchtcb *tcb) {
+static inline struct fpsimd_state *get_fpu_owner(struct xnarchtcb *tcb)
+{
return &(tcb->core.tsp->fpsimd_state);
 }
 
@@ -68,7 +81,6 @@ void xnarch_leave_root(struct xnthread *root)
 {
struct xnarchtcb *rootcb = xnthread_archtcb(root);
rootcb->fpup = get_fpu_owner(rootcb);
-   disable_fpsimd();
 }
 
 void xnarch_save_fpu(struct xnthread *thread)
@@ -83,29 +95,15 @@ 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;
 
-   /*
-* This only gets called if XNFPU flag is set, or if migrating to Linux.
-* In both cases, this means turn on FPU and switch.
-*/
enable_fpsimd();
 
-   if (xnthread_test_state(to, XNROOT) == 0) {
-   if (from_fpup == to_fpup)
-   return;
-
-   if (from_fpup)
-   fpsimd_save_state(from_fpup);
-
-   fpsimd_load_state(to_fpup);
-   }
-   else {
-   /* Going to Linux. */
-   if (from_fpup)
-   fpsimd_save_state(from_fpup);
+   if (from_fpup == to_fpup)
+   return;
 
-   fpsimd_load_state(to_fpup);
-   }
+   if (from_fpup)
+   fpsimd_save_state(from_fpup);
 
+   fpsimd_load_state(to_fpup);
 }
 
 int xnarch_handle_fpu_fault(struct xnthread *from, 
@@ -113,9 +111,9 @@ int xnarch_handle_fpu_fault(struct xnthread *from,
 {
spl_t s;
 
+   /* FPU should already be enabled for XNFPU tasks. */
if (xnthread_test_state(to, XNFPU))
-   /* FPU is already enabled, probably an exception */
-  return 0;
+   BUG();
 
xnlock_get_irqsave(, s);
xnthread_set_state(to, XNFPU);
@@ -124,20 +122,13 @@ int xnarch_handle_fpu_fault(struct xnthread *from,
xnarch_switch_fpu(from, to);
 
return 1;
-
 }
 
 void xnarch_init_shadow_tcb(struct xnthread *thread)
 {
-   spl_t s;
struct xnarchtcb *tcb = xnthread_archtcb(thread);
-
tcb->fpup = &(tcb->core.host_task->thread.fpsimd_state);
-
-   xnlock_get_irqsave(, s);
xnthread_clear_state(thread, XNFPU);
-   xnlock_put_irqrestore(, s);
-
 }
 #endif /* CONFIG_XENO_ARCH_FPU */
 
@@ -210,16 +201,14 @@ void xnarch_switch_to(struct xnthread *out, struct 
xnthread *in)
xnarch_tls_thread_switch(in_tcb->core.tip->task);
xnarch_contextidr_thread_switch(in_tcb->core.tip->task);
 
-   /* check if we need to switch FPU on return to Linux */
-   if (xnthread_test_state(in, XNROOT) == 1)
-   

[Xenomai-git] Dmitriy Cherkasov : cobalt/arm64: FPU code cleanup

2017-06-03 Thread git repository hosting
Module: xenomai-3
Branch: next
Commit: 26d8310032390ed87ead2a8093a28d02cc98b6bd
URL:
http://git.xenomai.org/?p=xenomai-3.git;a=commit;h=26d8310032390ed87ead2a8093a28d02cc98b6bd

Author: Dmitriy Cherkasov 
Date:   Fri Oct  2 12:54:37 2015 -0700

cobalt/arm64: FPU code cleanup

---

 kernel/cobalt/arch/arm64/thread.c |   89 -
 1 file changed, 39 insertions(+), 50 deletions(-)

diff --git a/kernel/cobalt/arch/arm64/thread.c 
b/kernel/cobalt/arch/arm64/thread.c
index db369be..35dbd72 100644
--- a/kernel/cobalt/arch/arm64/thread.c
+++ b/kernel/cobalt/arch/arm64/thread.c
@@ -36,31 +36,44 @@
 
 #if defined(CONFIG_XENO_ARCH_FPU)
 
-static void enable_fpsimd(void) {
-   __asm__ __volatile__("mrs x1, cpacr_el1\n\
-   orr x1, x1, #(0x3 << 20)\n\
-   msr cpacr_el1, x1\n\
-   isb" : : : "x1", "memory", "cc");
+#define FPSIMD_EN (0x3 << 20)
+
+static inline unsigned long get_cpacr(void)
+{
+   unsigned long result;
+   __asm__ __volatile__("mrs %0, cpacr_el1": "=r"(result));
+   return result;
+}
+
+static inline void set_cpacr(long val)
+{
+   __asm__ __volatile__ (
+   "msr cpacr_el1, %0\n\t"
+   "isb"
+   : /* */ : "r"(val));
 }
 
-static void disable_fpsimd(void) {
-   __asm__ __volatile__("mrs x1, cpacr_el1\n\
-   and x1, x1, #~(0x3 << 20)\n\
-   msr cpacr_el1, x1\n\
-   isb" : : : "x1", "memory", "cc");
+static void enable_fpsimd(void)
+{
+   unsigned long cpacr = get_cpacr();
+   cpacr |= FPSIMD_EN;
+   set_cpacr(cpacr);
+}
+
+static void disable_fpsimd(void)
+{
+   unsigned long cpacr = get_cpacr();
+   cpacr &= ~FPSIMD_EN;
+   set_cpacr(cpacr);
 }
 
 int xnarch_fault_fpu_p(struct ipipe_trap_data *d)
 {
-   /* check if this is an FPU access trap to be handled by Xenomai */
-   if(d->exception == IPIPE_TRAP_FPU_ACC){
-   return 1;
-   }
-   /* FPU already enabled, propagate fault to kernel */
-   return 0;
+   return (d->exception == IPIPE_TRAP_FPU_ACC);
 }
 
-static inline struct fpsimd_state *get_fpu_owner(struct xnarchtcb *tcb) {
+static inline struct fpsimd_state *get_fpu_owner(struct xnarchtcb *tcb)
+{
return &(tcb->core.tsp->fpsimd_state);
 }
 
@@ -68,7 +81,6 @@ void xnarch_leave_root(struct xnthread *root)
 {
struct xnarchtcb *rootcb = xnthread_archtcb(root);
rootcb->fpup = get_fpu_owner(rootcb);
-   disable_fpsimd();
 }
 
 void xnarch_save_fpu(struct xnthread *thread)
@@ -83,29 +95,15 @@ 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;
 
-   /*
-* This only gets called if XNFPU flag is set, or if migrating to Linux.
-* In both cases, this means turn on FPU and switch.
-*/
enable_fpsimd();
 
-   if (xnthread_test_state(to, XNROOT) == 0) {
-   if (from_fpup == to_fpup)
-   return;
-
-   if (from_fpup)
-   fpsimd_save_state(from_fpup);
-
-   fpsimd_load_state(to_fpup);
-   }
-   else {
-   /* Going to Linux. */
-   if (from_fpup)
-   fpsimd_save_state(from_fpup);
+   if (from_fpup == to_fpup)
+   return;
 
-   fpsimd_load_state(to_fpup);
-   }
+   if (from_fpup)
+   fpsimd_save_state(from_fpup);
 
+   fpsimd_load_state(to_fpup);
 }
 
 int xnarch_handle_fpu_fault(struct xnthread *from, 
@@ -113,9 +111,9 @@ int xnarch_handle_fpu_fault(struct xnthread *from,
 {
spl_t s;
 
+   /* FPU should already be enabled for XNFPU tasks. */
if (xnthread_test_state(to, XNFPU))
-   /* FPU is already enabled, probably an exception */
-  return 0;
+   BUG();
 
xnlock_get_irqsave(, s);
xnthread_set_state(to, XNFPU);
@@ -124,20 +122,13 @@ int xnarch_handle_fpu_fault(struct xnthread *from,
xnarch_switch_fpu(from, to);
 
return 1;
-
 }
 
 void xnarch_init_shadow_tcb(struct xnthread *thread)
 {
-   spl_t s;
struct xnarchtcb *tcb = xnthread_archtcb(thread);
-
tcb->fpup = &(tcb->core.host_task->thread.fpsimd_state);
-
-   xnlock_get_irqsave(, s);
xnthread_clear_state(thread, XNFPU);
-   xnlock_put_irqrestore(, s);
-
 }
 #endif /* CONFIG_XENO_ARCH_FPU */
 
@@ -210,16 +201,14 @@ void xnarch_switch_to(struct xnthread *out, struct 
xnthread *in)
xnarch_tls_thread_switch(in_tcb->core.tip->task);
xnarch_contextidr_thread_switch(in_tcb->core.tip->task);
 
-   /* check if we need to switch FPU on return to Linux */
-   if (xnthread_test_state(in, XNROOT) == 1)
-   

[Xenomai-git] Dmitriy Cherkasov : cobalt/arm64: FPU code cleanup

2017-05-21 Thread git repository hosting
Module: xenomai-3
Branch: next
Commit: 283f9cf0111f1208123ad758401a94039d9e1bd0
URL:
http://git.xenomai.org/?p=xenomai-3.git;a=commit;h=283f9cf0111f1208123ad758401a94039d9e1bd0

Author: Dmitriy Cherkasov 
Date:   Fri Oct  2 12:54:37 2015 -0700

cobalt/arm64: FPU code cleanup

---

 kernel/cobalt/arch/arm64/thread.c |   89 -
 1 file changed, 39 insertions(+), 50 deletions(-)

diff --git a/kernel/cobalt/arch/arm64/thread.c 
b/kernel/cobalt/arch/arm64/thread.c
index db369be..35dbd72 100644
--- a/kernel/cobalt/arch/arm64/thread.c
+++ b/kernel/cobalt/arch/arm64/thread.c
@@ -36,31 +36,44 @@
 
 #if defined(CONFIG_XENO_ARCH_FPU)
 
-static void enable_fpsimd(void) {
-   __asm__ __volatile__("mrs x1, cpacr_el1\n\
-   orr x1, x1, #(0x3 << 20)\n\
-   msr cpacr_el1, x1\n\
-   isb" : : : "x1", "memory", "cc");
+#define FPSIMD_EN (0x3 << 20)
+
+static inline unsigned long get_cpacr(void)
+{
+   unsigned long result;
+   __asm__ __volatile__("mrs %0, cpacr_el1": "=r"(result));
+   return result;
+}
+
+static inline void set_cpacr(long val)
+{
+   __asm__ __volatile__ (
+   "msr cpacr_el1, %0\n\t"
+   "isb"
+   : /* */ : "r"(val));
 }
 
-static void disable_fpsimd(void) {
-   __asm__ __volatile__("mrs x1, cpacr_el1\n\
-   and x1, x1, #~(0x3 << 20)\n\
-   msr cpacr_el1, x1\n\
-   isb" : : : "x1", "memory", "cc");
+static void enable_fpsimd(void)
+{
+   unsigned long cpacr = get_cpacr();
+   cpacr |= FPSIMD_EN;
+   set_cpacr(cpacr);
+}
+
+static void disable_fpsimd(void)
+{
+   unsigned long cpacr = get_cpacr();
+   cpacr &= ~FPSIMD_EN;
+   set_cpacr(cpacr);
 }
 
 int xnarch_fault_fpu_p(struct ipipe_trap_data *d)
 {
-   /* check if this is an FPU access trap to be handled by Xenomai */
-   if(d->exception == IPIPE_TRAP_FPU_ACC){
-   return 1;
-   }
-   /* FPU already enabled, propagate fault to kernel */
-   return 0;
+   return (d->exception == IPIPE_TRAP_FPU_ACC);
 }
 
-static inline struct fpsimd_state *get_fpu_owner(struct xnarchtcb *tcb) {
+static inline struct fpsimd_state *get_fpu_owner(struct xnarchtcb *tcb)
+{
return &(tcb->core.tsp->fpsimd_state);
 }
 
@@ -68,7 +81,6 @@ void xnarch_leave_root(struct xnthread *root)
 {
struct xnarchtcb *rootcb = xnthread_archtcb(root);
rootcb->fpup = get_fpu_owner(rootcb);
-   disable_fpsimd();
 }
 
 void xnarch_save_fpu(struct xnthread *thread)
@@ -83,29 +95,15 @@ 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;
 
-   /*
-* This only gets called if XNFPU flag is set, or if migrating to Linux.
-* In both cases, this means turn on FPU and switch.
-*/
enable_fpsimd();
 
-   if (xnthread_test_state(to, XNROOT) == 0) {
-   if (from_fpup == to_fpup)
-   return;
-
-   if (from_fpup)
-   fpsimd_save_state(from_fpup);
-
-   fpsimd_load_state(to_fpup);
-   }
-   else {
-   /* Going to Linux. */
-   if (from_fpup)
-   fpsimd_save_state(from_fpup);
+   if (from_fpup == to_fpup)
+   return;
 
-   fpsimd_load_state(to_fpup);
-   }
+   if (from_fpup)
+   fpsimd_save_state(from_fpup);
 
+   fpsimd_load_state(to_fpup);
 }
 
 int xnarch_handle_fpu_fault(struct xnthread *from, 
@@ -113,9 +111,9 @@ int xnarch_handle_fpu_fault(struct xnthread *from,
 {
spl_t s;
 
+   /* FPU should already be enabled for XNFPU tasks. */
if (xnthread_test_state(to, XNFPU))
-   /* FPU is already enabled, probably an exception */
-  return 0;
+   BUG();
 
xnlock_get_irqsave(, s);
xnthread_set_state(to, XNFPU);
@@ -124,20 +122,13 @@ int xnarch_handle_fpu_fault(struct xnthread *from,
xnarch_switch_fpu(from, to);
 
return 1;
-
 }
 
 void xnarch_init_shadow_tcb(struct xnthread *thread)
 {
-   spl_t s;
struct xnarchtcb *tcb = xnthread_archtcb(thread);
-
tcb->fpup = &(tcb->core.host_task->thread.fpsimd_state);
-
-   xnlock_get_irqsave(, s);
xnthread_clear_state(thread, XNFPU);
-   xnlock_put_irqrestore(, s);
-
 }
 #endif /* CONFIG_XENO_ARCH_FPU */
 
@@ -210,16 +201,14 @@ void xnarch_switch_to(struct xnthread *out, struct 
xnthread *in)
xnarch_tls_thread_switch(in_tcb->core.tip->task);
xnarch_contextidr_thread_switch(in_tcb->core.tip->task);
 
-   /* check if we need to switch FPU on return to Linux */
-   if (xnthread_test_state(in, XNROOT) == 1)
-   

[Xenomai-git] Dmitriy Cherkasov : cobalt/arm64: FPU code cleanup

2017-05-14 Thread git repository hosting
Module: xenomai-3
Branch: next
Commit: fbe2f25a04dd37930aadce507f816c374e83
URL:
http://git.xenomai.org/?p=xenomai-3.git;a=commit;h=fbe2f25a04dd37930aadce507f816c374e83

Author: Dmitriy Cherkasov 
Date:   Fri Oct  2 12:54:37 2015 -0700

cobalt/arm64: FPU code cleanup

---

 kernel/cobalt/arch/arm64/thread.c |   89 -
 1 file changed, 39 insertions(+), 50 deletions(-)

diff --git a/kernel/cobalt/arch/arm64/thread.c 
b/kernel/cobalt/arch/arm64/thread.c
index db369be..35dbd72 100644
--- a/kernel/cobalt/arch/arm64/thread.c
+++ b/kernel/cobalt/arch/arm64/thread.c
@@ -36,31 +36,44 @@
 
 #if defined(CONFIG_XENO_ARCH_FPU)
 
-static void enable_fpsimd(void) {
-   __asm__ __volatile__("mrs x1, cpacr_el1\n\
-   orr x1, x1, #(0x3 << 20)\n\
-   msr cpacr_el1, x1\n\
-   isb" : : : "x1", "memory", "cc");
+#define FPSIMD_EN (0x3 << 20)
+
+static inline unsigned long get_cpacr(void)
+{
+   unsigned long result;
+   __asm__ __volatile__("mrs %0, cpacr_el1": "=r"(result));
+   return result;
+}
+
+static inline void set_cpacr(long val)
+{
+   __asm__ __volatile__ (
+   "msr cpacr_el1, %0\n\t"
+   "isb"
+   : /* */ : "r"(val));
 }
 
-static void disable_fpsimd(void) {
-   __asm__ __volatile__("mrs x1, cpacr_el1\n\
-   and x1, x1, #~(0x3 << 20)\n\
-   msr cpacr_el1, x1\n\
-   isb" : : : "x1", "memory", "cc");
+static void enable_fpsimd(void)
+{
+   unsigned long cpacr = get_cpacr();
+   cpacr |= FPSIMD_EN;
+   set_cpacr(cpacr);
+}
+
+static void disable_fpsimd(void)
+{
+   unsigned long cpacr = get_cpacr();
+   cpacr &= ~FPSIMD_EN;
+   set_cpacr(cpacr);
 }
 
 int xnarch_fault_fpu_p(struct ipipe_trap_data *d)
 {
-   /* check if this is an FPU access trap to be handled by Xenomai */
-   if(d->exception == IPIPE_TRAP_FPU_ACC){
-   return 1;
-   }
-   /* FPU already enabled, propagate fault to kernel */
-   return 0;
+   return (d->exception == IPIPE_TRAP_FPU_ACC);
 }
 
-static inline struct fpsimd_state *get_fpu_owner(struct xnarchtcb *tcb) {
+static inline struct fpsimd_state *get_fpu_owner(struct xnarchtcb *tcb)
+{
return &(tcb->core.tsp->fpsimd_state);
 }
 
@@ -68,7 +81,6 @@ void xnarch_leave_root(struct xnthread *root)
 {
struct xnarchtcb *rootcb = xnthread_archtcb(root);
rootcb->fpup = get_fpu_owner(rootcb);
-   disable_fpsimd();
 }
 
 void xnarch_save_fpu(struct xnthread *thread)
@@ -83,29 +95,15 @@ 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;
 
-   /*
-* This only gets called if XNFPU flag is set, or if migrating to Linux.
-* In both cases, this means turn on FPU and switch.
-*/
enable_fpsimd();
 
-   if (xnthread_test_state(to, XNROOT) == 0) {
-   if (from_fpup == to_fpup)
-   return;
-
-   if (from_fpup)
-   fpsimd_save_state(from_fpup);
-
-   fpsimd_load_state(to_fpup);
-   }
-   else {
-   /* Going to Linux. */
-   if (from_fpup)
-   fpsimd_save_state(from_fpup);
+   if (from_fpup == to_fpup)
+   return;
 
-   fpsimd_load_state(to_fpup);
-   }
+   if (from_fpup)
+   fpsimd_save_state(from_fpup);
 
+   fpsimd_load_state(to_fpup);
 }
 
 int xnarch_handle_fpu_fault(struct xnthread *from, 
@@ -113,9 +111,9 @@ int xnarch_handle_fpu_fault(struct xnthread *from,
 {
spl_t s;
 
+   /* FPU should already be enabled for XNFPU tasks. */
if (xnthread_test_state(to, XNFPU))
-   /* FPU is already enabled, probably an exception */
-  return 0;
+   BUG();
 
xnlock_get_irqsave(, s);
xnthread_set_state(to, XNFPU);
@@ -124,20 +122,13 @@ int xnarch_handle_fpu_fault(struct xnthread *from,
xnarch_switch_fpu(from, to);
 
return 1;
-
 }
 
 void xnarch_init_shadow_tcb(struct xnthread *thread)
 {
-   spl_t s;
struct xnarchtcb *tcb = xnthread_archtcb(thread);
-
tcb->fpup = &(tcb->core.host_task->thread.fpsimd_state);
-
-   xnlock_get_irqsave(, s);
xnthread_clear_state(thread, XNFPU);
-   xnlock_put_irqrestore(, s);
-
 }
 #endif /* CONFIG_XENO_ARCH_FPU */
 
@@ -210,16 +201,14 @@ void xnarch_switch_to(struct xnthread *out, struct 
xnthread *in)
xnarch_tls_thread_switch(in_tcb->core.tip->task);
xnarch_contextidr_thread_switch(in_tcb->core.tip->task);
 
-   /* check if we need to switch FPU on return to Linux */
-   if (xnthread_test_state(in, XNROOT) == 1)
-   

[Xenomai-git] Dmitriy Cherkasov : cobalt/arm64: FPU code cleanup

2017-05-13 Thread git repository hosting
Module: xenomai-3
Branch: next
Commit: 60bab837e981641bfc4788137448e81ec9a345d0
URL:
http://git.xenomai.org/?p=xenomai-3.git;a=commit;h=60bab837e981641bfc4788137448e81ec9a345d0

Author: Dmitriy Cherkasov 
Date:   Fri Oct  2 12:54:37 2015 -0700

cobalt/arm64: FPU code cleanup

---

 kernel/cobalt/arch/arm64/thread.c |   89 -
 1 file changed, 39 insertions(+), 50 deletions(-)

diff --git a/kernel/cobalt/arch/arm64/thread.c 
b/kernel/cobalt/arch/arm64/thread.c
index db369be..35dbd72 100644
--- a/kernel/cobalt/arch/arm64/thread.c
+++ b/kernel/cobalt/arch/arm64/thread.c
@@ -36,31 +36,44 @@
 
 #if defined(CONFIG_XENO_ARCH_FPU)
 
-static void enable_fpsimd(void) {
-   __asm__ __volatile__("mrs x1, cpacr_el1\n\
-   orr x1, x1, #(0x3 << 20)\n\
-   msr cpacr_el1, x1\n\
-   isb" : : : "x1", "memory", "cc");
+#define FPSIMD_EN (0x3 << 20)
+
+static inline unsigned long get_cpacr(void)
+{
+   unsigned long result;
+   __asm__ __volatile__("mrs %0, cpacr_el1": "=r"(result));
+   return result;
+}
+
+static inline void set_cpacr(long val)
+{
+   __asm__ __volatile__ (
+   "msr cpacr_el1, %0\n\t"
+   "isb"
+   : /* */ : "r"(val));
 }
 
-static void disable_fpsimd(void) {
-   __asm__ __volatile__("mrs x1, cpacr_el1\n\
-   and x1, x1, #~(0x3 << 20)\n\
-   msr cpacr_el1, x1\n\
-   isb" : : : "x1", "memory", "cc");
+static void enable_fpsimd(void)
+{
+   unsigned long cpacr = get_cpacr();
+   cpacr |= FPSIMD_EN;
+   set_cpacr(cpacr);
+}
+
+static void disable_fpsimd(void)
+{
+   unsigned long cpacr = get_cpacr();
+   cpacr &= ~FPSIMD_EN;
+   set_cpacr(cpacr);
 }
 
 int xnarch_fault_fpu_p(struct ipipe_trap_data *d)
 {
-   /* check if this is an FPU access trap to be handled by Xenomai */
-   if(d->exception == IPIPE_TRAP_FPU_ACC){
-   return 1;
-   }
-   /* FPU already enabled, propagate fault to kernel */
-   return 0;
+   return (d->exception == IPIPE_TRAP_FPU_ACC);
 }
 
-static inline struct fpsimd_state *get_fpu_owner(struct xnarchtcb *tcb) {
+static inline struct fpsimd_state *get_fpu_owner(struct xnarchtcb *tcb)
+{
return &(tcb->core.tsp->fpsimd_state);
 }
 
@@ -68,7 +81,6 @@ void xnarch_leave_root(struct xnthread *root)
 {
struct xnarchtcb *rootcb = xnthread_archtcb(root);
rootcb->fpup = get_fpu_owner(rootcb);
-   disable_fpsimd();
 }
 
 void xnarch_save_fpu(struct xnthread *thread)
@@ -83,29 +95,15 @@ 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;
 
-   /*
-* This only gets called if XNFPU flag is set, or if migrating to Linux.
-* In both cases, this means turn on FPU and switch.
-*/
enable_fpsimd();
 
-   if (xnthread_test_state(to, XNROOT) == 0) {
-   if (from_fpup == to_fpup)
-   return;
-
-   if (from_fpup)
-   fpsimd_save_state(from_fpup);
-
-   fpsimd_load_state(to_fpup);
-   }
-   else {
-   /* Going to Linux. */
-   if (from_fpup)
-   fpsimd_save_state(from_fpup);
+   if (from_fpup == to_fpup)
+   return;
 
-   fpsimd_load_state(to_fpup);
-   }
+   if (from_fpup)
+   fpsimd_save_state(from_fpup);
 
+   fpsimd_load_state(to_fpup);
 }
 
 int xnarch_handle_fpu_fault(struct xnthread *from, 
@@ -113,9 +111,9 @@ int xnarch_handle_fpu_fault(struct xnthread *from,
 {
spl_t s;
 
+   /* FPU should already be enabled for XNFPU tasks. */
if (xnthread_test_state(to, XNFPU))
-   /* FPU is already enabled, probably an exception */
-  return 0;
+   BUG();
 
xnlock_get_irqsave(, s);
xnthread_set_state(to, XNFPU);
@@ -124,20 +122,13 @@ int xnarch_handle_fpu_fault(struct xnthread *from,
xnarch_switch_fpu(from, to);
 
return 1;
-
 }
 
 void xnarch_init_shadow_tcb(struct xnthread *thread)
 {
-   spl_t s;
struct xnarchtcb *tcb = xnthread_archtcb(thread);
-
tcb->fpup = &(tcb->core.host_task->thread.fpsimd_state);
-
-   xnlock_get_irqsave(, s);
xnthread_clear_state(thread, XNFPU);
-   xnlock_put_irqrestore(, s);
-
 }
 #endif /* CONFIG_XENO_ARCH_FPU */
 
@@ -210,16 +201,14 @@ void xnarch_switch_to(struct xnthread *out, struct 
xnthread *in)
xnarch_tls_thread_switch(in_tcb->core.tip->task);
xnarch_contextidr_thread_switch(in_tcb->core.tip->task);
 
-   /* check if we need to switch FPU on return to Linux */
-   if (xnthread_test_state(in, XNROOT) == 1)
-   

[Xenomai-git] Dmitriy Cherkasov : cobalt/arm64: FPU code cleanup

2017-04-17 Thread git repository hosting
Module: xenomai-3
Branch: next
Commit: 3c556d3599e7e96d94f65a13582961a4ee67333d
URL:
http://git.xenomai.org/?p=xenomai-3.git;a=commit;h=3c556d3599e7e96d94f65a13582961a4ee67333d

Author: Dmitriy Cherkasov 
Date:   Fri Oct  2 12:54:37 2015 -0700

cobalt/arm64: FPU code cleanup

---

 kernel/cobalt/arch/arm64/thread.c |   89 -
 1 file changed, 39 insertions(+), 50 deletions(-)

diff --git a/kernel/cobalt/arch/arm64/thread.c 
b/kernel/cobalt/arch/arm64/thread.c
index db369be..35dbd72 100644
--- a/kernel/cobalt/arch/arm64/thread.c
+++ b/kernel/cobalt/arch/arm64/thread.c
@@ -36,31 +36,44 @@
 
 #if defined(CONFIG_XENO_ARCH_FPU)
 
-static void enable_fpsimd(void) {
-   __asm__ __volatile__("mrs x1, cpacr_el1\n\
-   orr x1, x1, #(0x3 << 20)\n\
-   msr cpacr_el1, x1\n\
-   isb" : : : "x1", "memory", "cc");
+#define FPSIMD_EN (0x3 << 20)
+
+static inline unsigned long get_cpacr(void)
+{
+   unsigned long result;
+   __asm__ __volatile__("mrs %0, cpacr_el1": "=r"(result));
+   return result;
+}
+
+static inline void set_cpacr(long val)
+{
+   __asm__ __volatile__ (
+   "msr cpacr_el1, %0\n\t"
+   "isb"
+   : /* */ : "r"(val));
 }
 
-static void disable_fpsimd(void) {
-   __asm__ __volatile__("mrs x1, cpacr_el1\n\
-   and x1, x1, #~(0x3 << 20)\n\
-   msr cpacr_el1, x1\n\
-   isb" : : : "x1", "memory", "cc");
+static void enable_fpsimd(void)
+{
+   unsigned long cpacr = get_cpacr();
+   cpacr |= FPSIMD_EN;
+   set_cpacr(cpacr);
+}
+
+static void disable_fpsimd(void)
+{
+   unsigned long cpacr = get_cpacr();
+   cpacr &= ~FPSIMD_EN;
+   set_cpacr(cpacr);
 }
 
 int xnarch_fault_fpu_p(struct ipipe_trap_data *d)
 {
-   /* check if this is an FPU access trap to be handled by Xenomai */
-   if(d->exception == IPIPE_TRAP_FPU_ACC){
-   return 1;
-   }
-   /* FPU already enabled, propagate fault to kernel */
-   return 0;
+   return (d->exception == IPIPE_TRAP_FPU_ACC);
 }
 
-static inline struct fpsimd_state *get_fpu_owner(struct xnarchtcb *tcb) {
+static inline struct fpsimd_state *get_fpu_owner(struct xnarchtcb *tcb)
+{
return &(tcb->core.tsp->fpsimd_state);
 }
 
@@ -68,7 +81,6 @@ void xnarch_leave_root(struct xnthread *root)
 {
struct xnarchtcb *rootcb = xnthread_archtcb(root);
rootcb->fpup = get_fpu_owner(rootcb);
-   disable_fpsimd();
 }
 
 void xnarch_save_fpu(struct xnthread *thread)
@@ -83,29 +95,15 @@ 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;
 
-   /*
-* This only gets called if XNFPU flag is set, or if migrating to Linux.
-* In both cases, this means turn on FPU and switch.
-*/
enable_fpsimd();
 
-   if (xnthread_test_state(to, XNROOT) == 0) {
-   if (from_fpup == to_fpup)
-   return;
-
-   if (from_fpup)
-   fpsimd_save_state(from_fpup);
-
-   fpsimd_load_state(to_fpup);
-   }
-   else {
-   /* Going to Linux. */
-   if (from_fpup)
-   fpsimd_save_state(from_fpup);
+   if (from_fpup == to_fpup)
+   return;
 
-   fpsimd_load_state(to_fpup);
-   }
+   if (from_fpup)
+   fpsimd_save_state(from_fpup);
 
+   fpsimd_load_state(to_fpup);
 }
 
 int xnarch_handle_fpu_fault(struct xnthread *from, 
@@ -113,9 +111,9 @@ int xnarch_handle_fpu_fault(struct xnthread *from,
 {
spl_t s;
 
+   /* FPU should already be enabled for XNFPU tasks. */
if (xnthread_test_state(to, XNFPU))
-   /* FPU is already enabled, probably an exception */
-  return 0;
+   BUG();
 
xnlock_get_irqsave(, s);
xnthread_set_state(to, XNFPU);
@@ -124,20 +122,13 @@ int xnarch_handle_fpu_fault(struct xnthread *from,
xnarch_switch_fpu(from, to);
 
return 1;
-
 }
 
 void xnarch_init_shadow_tcb(struct xnthread *thread)
 {
-   spl_t s;
struct xnarchtcb *tcb = xnthread_archtcb(thread);
-
tcb->fpup = &(tcb->core.host_task->thread.fpsimd_state);
-
-   xnlock_get_irqsave(, s);
xnthread_clear_state(thread, XNFPU);
-   xnlock_put_irqrestore(, s);
-
 }
 #endif /* CONFIG_XENO_ARCH_FPU */
 
@@ -210,16 +201,14 @@ void xnarch_switch_to(struct xnthread *out, struct 
xnthread *in)
xnarch_tls_thread_switch(in_tcb->core.tip->task);
xnarch_contextidr_thread_switch(in_tcb->core.tip->task);
 
-   /* check if we need to switch FPU on return to Linux */
-   if (xnthread_test_state(in, XNROOT) == 1)
-   

[Xenomai-git] Dmitriy Cherkasov : cobalt/arm64: FPU code cleanup

2017-03-15 Thread git repository hosting
Module: xenomai-3
Branch: next
Commit: 1b1d05c28e55bb672644a6438503398fe1aa2348
URL:
http://git.xenomai.org/?p=xenomai-3.git;a=commit;h=1b1d05c28e55bb672644a6438503398fe1aa2348

Author: Dmitriy Cherkasov 
Date:   Fri Oct  2 12:54:37 2015 -0700

cobalt/arm64: FPU code cleanup

---

 kernel/cobalt/arch/arm64/thread.c |   89 -
 1 file changed, 39 insertions(+), 50 deletions(-)

diff --git a/kernel/cobalt/arch/arm64/thread.c 
b/kernel/cobalt/arch/arm64/thread.c
index db369be..35dbd72 100644
--- a/kernel/cobalt/arch/arm64/thread.c
+++ b/kernel/cobalt/arch/arm64/thread.c
@@ -36,31 +36,44 @@
 
 #if defined(CONFIG_XENO_ARCH_FPU)
 
-static void enable_fpsimd(void) {
-   __asm__ __volatile__("mrs x1, cpacr_el1\n\
-   orr x1, x1, #(0x3 << 20)\n\
-   msr cpacr_el1, x1\n\
-   isb" : : : "x1", "memory", "cc");
+#define FPSIMD_EN (0x3 << 20)
+
+static inline unsigned long get_cpacr(void)
+{
+   unsigned long result;
+   __asm__ __volatile__("mrs %0, cpacr_el1": "=r"(result));
+   return result;
+}
+
+static inline void set_cpacr(long val)
+{
+   __asm__ __volatile__ (
+   "msr cpacr_el1, %0\n\t"
+   "isb"
+   : /* */ : "r"(val));
 }
 
-static void disable_fpsimd(void) {
-   __asm__ __volatile__("mrs x1, cpacr_el1\n\
-   and x1, x1, #~(0x3 << 20)\n\
-   msr cpacr_el1, x1\n\
-   isb" : : : "x1", "memory", "cc");
+static void enable_fpsimd(void)
+{
+   unsigned long cpacr = get_cpacr();
+   cpacr |= FPSIMD_EN;
+   set_cpacr(cpacr);
+}
+
+static void disable_fpsimd(void)
+{
+   unsigned long cpacr = get_cpacr();
+   cpacr &= ~FPSIMD_EN;
+   set_cpacr(cpacr);
 }
 
 int xnarch_fault_fpu_p(struct ipipe_trap_data *d)
 {
-   /* check if this is an FPU access trap to be handled by Xenomai */
-   if(d->exception == IPIPE_TRAP_FPU_ACC){
-   return 1;
-   }
-   /* FPU already enabled, propagate fault to kernel */
-   return 0;
+   return (d->exception == IPIPE_TRAP_FPU_ACC);
 }
 
-static inline struct fpsimd_state *get_fpu_owner(struct xnarchtcb *tcb) {
+static inline struct fpsimd_state *get_fpu_owner(struct xnarchtcb *tcb)
+{
return &(tcb->core.tsp->fpsimd_state);
 }
 
@@ -68,7 +81,6 @@ void xnarch_leave_root(struct xnthread *root)
 {
struct xnarchtcb *rootcb = xnthread_archtcb(root);
rootcb->fpup = get_fpu_owner(rootcb);
-   disable_fpsimd();
 }
 
 void xnarch_save_fpu(struct xnthread *thread)
@@ -83,29 +95,15 @@ 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;
 
-   /*
-* This only gets called if XNFPU flag is set, or if migrating to Linux.
-* In both cases, this means turn on FPU and switch.
-*/
enable_fpsimd();
 
-   if (xnthread_test_state(to, XNROOT) == 0) {
-   if (from_fpup == to_fpup)
-   return;
-
-   if (from_fpup)
-   fpsimd_save_state(from_fpup);
-
-   fpsimd_load_state(to_fpup);
-   }
-   else {
-   /* Going to Linux. */
-   if (from_fpup)
-   fpsimd_save_state(from_fpup);
+   if (from_fpup == to_fpup)
+   return;
 
-   fpsimd_load_state(to_fpup);
-   }
+   if (from_fpup)
+   fpsimd_save_state(from_fpup);
 
+   fpsimd_load_state(to_fpup);
 }
 
 int xnarch_handle_fpu_fault(struct xnthread *from, 
@@ -113,9 +111,9 @@ int xnarch_handle_fpu_fault(struct xnthread *from,
 {
spl_t s;
 
+   /* FPU should already be enabled for XNFPU tasks. */
if (xnthread_test_state(to, XNFPU))
-   /* FPU is already enabled, probably an exception */
-  return 0;
+   BUG();
 
xnlock_get_irqsave(, s);
xnthread_set_state(to, XNFPU);
@@ -124,20 +122,13 @@ int xnarch_handle_fpu_fault(struct xnthread *from,
xnarch_switch_fpu(from, to);
 
return 1;
-
 }
 
 void xnarch_init_shadow_tcb(struct xnthread *thread)
 {
-   spl_t s;
struct xnarchtcb *tcb = xnthread_archtcb(thread);
-
tcb->fpup = &(tcb->core.host_task->thread.fpsimd_state);
-
-   xnlock_get_irqsave(, s);
xnthread_clear_state(thread, XNFPU);
-   xnlock_put_irqrestore(, s);
-
 }
 #endif /* CONFIG_XENO_ARCH_FPU */
 
@@ -210,16 +201,14 @@ void xnarch_switch_to(struct xnthread *out, struct 
xnthread *in)
xnarch_tls_thread_switch(in_tcb->core.tip->task);
xnarch_contextidr_thread_switch(in_tcb->core.tip->task);
 
-   /* check if we need to switch FPU on return to Linux */
-   if (xnthread_test_state(in, XNROOT) == 1)
-   

[Xenomai-git] Dmitriy Cherkasov : cobalt/arm64: FPU code cleanup

2017-03-13 Thread git repository hosting
Module: xenomai-3
Branch: next
Commit: 12191a1f540e2fec5ba4219755836d5fdae8772c
URL:
http://git.xenomai.org/?p=xenomai-3.git;a=commit;h=12191a1f540e2fec5ba4219755836d5fdae8772c

Author: Dmitriy Cherkasov 
Date:   Fri Oct  2 12:54:37 2015 -0700

cobalt/arm64: FPU code cleanup

---

 kernel/cobalt/arch/arm64/thread.c |   89 -
 1 file changed, 39 insertions(+), 50 deletions(-)

diff --git a/kernel/cobalt/arch/arm64/thread.c 
b/kernel/cobalt/arch/arm64/thread.c
index db369be..35dbd72 100644
--- a/kernel/cobalt/arch/arm64/thread.c
+++ b/kernel/cobalt/arch/arm64/thread.c
@@ -36,31 +36,44 @@
 
 #if defined(CONFIG_XENO_ARCH_FPU)
 
-static void enable_fpsimd(void) {
-   __asm__ __volatile__("mrs x1, cpacr_el1\n\
-   orr x1, x1, #(0x3 << 20)\n\
-   msr cpacr_el1, x1\n\
-   isb" : : : "x1", "memory", "cc");
+#define FPSIMD_EN (0x3 << 20)
+
+static inline unsigned long get_cpacr(void)
+{
+   unsigned long result;
+   __asm__ __volatile__("mrs %0, cpacr_el1": "=r"(result));
+   return result;
+}
+
+static inline void set_cpacr(long val)
+{
+   __asm__ __volatile__ (
+   "msr cpacr_el1, %0\n\t"
+   "isb"
+   : /* */ : "r"(val));
 }
 
-static void disable_fpsimd(void) {
-   __asm__ __volatile__("mrs x1, cpacr_el1\n\
-   and x1, x1, #~(0x3 << 20)\n\
-   msr cpacr_el1, x1\n\
-   isb" : : : "x1", "memory", "cc");
+static void enable_fpsimd(void)
+{
+   unsigned long cpacr = get_cpacr();
+   cpacr |= FPSIMD_EN;
+   set_cpacr(cpacr);
+}
+
+static void disable_fpsimd(void)
+{
+   unsigned long cpacr = get_cpacr();
+   cpacr &= ~FPSIMD_EN;
+   set_cpacr(cpacr);
 }
 
 int xnarch_fault_fpu_p(struct ipipe_trap_data *d)
 {
-   /* check if this is an FPU access trap to be handled by Xenomai */
-   if(d->exception == IPIPE_TRAP_FPU_ACC){
-   return 1;
-   }
-   /* FPU already enabled, propagate fault to kernel */
-   return 0;
+   return (d->exception == IPIPE_TRAP_FPU_ACC);
 }
 
-static inline struct fpsimd_state *get_fpu_owner(struct xnarchtcb *tcb) {
+static inline struct fpsimd_state *get_fpu_owner(struct xnarchtcb *tcb)
+{
return &(tcb->core.tsp->fpsimd_state);
 }
 
@@ -68,7 +81,6 @@ void xnarch_leave_root(struct xnthread *root)
 {
struct xnarchtcb *rootcb = xnthread_archtcb(root);
rootcb->fpup = get_fpu_owner(rootcb);
-   disable_fpsimd();
 }
 
 void xnarch_save_fpu(struct xnthread *thread)
@@ -83,29 +95,15 @@ 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;
 
-   /*
-* This only gets called if XNFPU flag is set, or if migrating to Linux.
-* In both cases, this means turn on FPU and switch.
-*/
enable_fpsimd();
 
-   if (xnthread_test_state(to, XNROOT) == 0) {
-   if (from_fpup == to_fpup)
-   return;
-
-   if (from_fpup)
-   fpsimd_save_state(from_fpup);
-
-   fpsimd_load_state(to_fpup);
-   }
-   else {
-   /* Going to Linux. */
-   if (from_fpup)
-   fpsimd_save_state(from_fpup);
+   if (from_fpup == to_fpup)
+   return;
 
-   fpsimd_load_state(to_fpup);
-   }
+   if (from_fpup)
+   fpsimd_save_state(from_fpup);
 
+   fpsimd_load_state(to_fpup);
 }
 
 int xnarch_handle_fpu_fault(struct xnthread *from, 
@@ -113,9 +111,9 @@ int xnarch_handle_fpu_fault(struct xnthread *from,
 {
spl_t s;
 
+   /* FPU should already be enabled for XNFPU tasks. */
if (xnthread_test_state(to, XNFPU))
-   /* FPU is already enabled, probably an exception */
-  return 0;
+   BUG();
 
xnlock_get_irqsave(, s);
xnthread_set_state(to, XNFPU);
@@ -124,20 +122,13 @@ int xnarch_handle_fpu_fault(struct xnthread *from,
xnarch_switch_fpu(from, to);
 
return 1;
-
 }
 
 void xnarch_init_shadow_tcb(struct xnthread *thread)
 {
-   spl_t s;
struct xnarchtcb *tcb = xnthread_archtcb(thread);
-
tcb->fpup = &(tcb->core.host_task->thread.fpsimd_state);
-
-   xnlock_get_irqsave(, s);
xnthread_clear_state(thread, XNFPU);
-   xnlock_put_irqrestore(, s);
-
 }
 #endif /* CONFIG_XENO_ARCH_FPU */
 
@@ -210,16 +201,14 @@ void xnarch_switch_to(struct xnthread *out, struct 
xnthread *in)
xnarch_tls_thread_switch(in_tcb->core.tip->task);
xnarch_contextidr_thread_switch(in_tcb->core.tip->task);
 
-   /* check if we need to switch FPU on return to Linux */
-   if (xnthread_test_state(in, XNROOT) == 1)
-   

[Xenomai-git] Dmitriy Cherkasov : cobalt/arm64: FPU code cleanup

2017-03-05 Thread git repository hosting
Module: xenomai-3
Branch: next
Commit: aeab7bf9bfdf974385b98e166b208a1aec87d502
URL:
http://git.xenomai.org/?p=xenomai-3.git;a=commit;h=aeab7bf9bfdf974385b98e166b208a1aec87d502

Author: Dmitriy Cherkasov 
Date:   Fri Oct  2 12:54:37 2015 -0700

cobalt/arm64: FPU code cleanup

---

 kernel/cobalt/arch/arm64/thread.c |   89 -
 1 file changed, 39 insertions(+), 50 deletions(-)

diff --git a/kernel/cobalt/arch/arm64/thread.c 
b/kernel/cobalt/arch/arm64/thread.c
index db369be..35dbd72 100644
--- a/kernel/cobalt/arch/arm64/thread.c
+++ b/kernel/cobalt/arch/arm64/thread.c
@@ -36,31 +36,44 @@
 
 #if defined(CONFIG_XENO_ARCH_FPU)
 
-static void enable_fpsimd(void) {
-   __asm__ __volatile__("mrs x1, cpacr_el1\n\
-   orr x1, x1, #(0x3 << 20)\n\
-   msr cpacr_el1, x1\n\
-   isb" : : : "x1", "memory", "cc");
+#define FPSIMD_EN (0x3 << 20)
+
+static inline unsigned long get_cpacr(void)
+{
+   unsigned long result;
+   __asm__ __volatile__("mrs %0, cpacr_el1": "=r"(result));
+   return result;
+}
+
+static inline void set_cpacr(long val)
+{
+   __asm__ __volatile__ (
+   "msr cpacr_el1, %0\n\t"
+   "isb"
+   : /* */ : "r"(val));
 }
 
-static void disable_fpsimd(void) {
-   __asm__ __volatile__("mrs x1, cpacr_el1\n\
-   and x1, x1, #~(0x3 << 20)\n\
-   msr cpacr_el1, x1\n\
-   isb" : : : "x1", "memory", "cc");
+static void enable_fpsimd(void)
+{
+   unsigned long cpacr = get_cpacr();
+   cpacr |= FPSIMD_EN;
+   set_cpacr(cpacr);
+}
+
+static void disable_fpsimd(void)
+{
+   unsigned long cpacr = get_cpacr();
+   cpacr &= ~FPSIMD_EN;
+   set_cpacr(cpacr);
 }
 
 int xnarch_fault_fpu_p(struct ipipe_trap_data *d)
 {
-   /* check if this is an FPU access trap to be handled by Xenomai */
-   if(d->exception == IPIPE_TRAP_FPU_ACC){
-   return 1;
-   }
-   /* FPU already enabled, propagate fault to kernel */
-   return 0;
+   return (d->exception == IPIPE_TRAP_FPU_ACC);
 }
 
-static inline struct fpsimd_state *get_fpu_owner(struct xnarchtcb *tcb) {
+static inline struct fpsimd_state *get_fpu_owner(struct xnarchtcb *tcb)
+{
return &(tcb->core.tsp->fpsimd_state);
 }
 
@@ -68,7 +81,6 @@ void xnarch_leave_root(struct xnthread *root)
 {
struct xnarchtcb *rootcb = xnthread_archtcb(root);
rootcb->fpup = get_fpu_owner(rootcb);
-   disable_fpsimd();
 }
 
 void xnarch_save_fpu(struct xnthread *thread)
@@ -83,29 +95,15 @@ 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;
 
-   /*
-* This only gets called if XNFPU flag is set, or if migrating to Linux.
-* In both cases, this means turn on FPU and switch.
-*/
enable_fpsimd();
 
-   if (xnthread_test_state(to, XNROOT) == 0) {
-   if (from_fpup == to_fpup)
-   return;
-
-   if (from_fpup)
-   fpsimd_save_state(from_fpup);
-
-   fpsimd_load_state(to_fpup);
-   }
-   else {
-   /* Going to Linux. */
-   if (from_fpup)
-   fpsimd_save_state(from_fpup);
+   if (from_fpup == to_fpup)
+   return;
 
-   fpsimd_load_state(to_fpup);
-   }
+   if (from_fpup)
+   fpsimd_save_state(from_fpup);
 
+   fpsimd_load_state(to_fpup);
 }
 
 int xnarch_handle_fpu_fault(struct xnthread *from, 
@@ -113,9 +111,9 @@ int xnarch_handle_fpu_fault(struct xnthread *from,
 {
spl_t s;
 
+   /* FPU should already be enabled for XNFPU tasks. */
if (xnthread_test_state(to, XNFPU))
-   /* FPU is already enabled, probably an exception */
-  return 0;
+   BUG();
 
xnlock_get_irqsave(, s);
xnthread_set_state(to, XNFPU);
@@ -124,20 +122,13 @@ int xnarch_handle_fpu_fault(struct xnthread *from,
xnarch_switch_fpu(from, to);
 
return 1;
-
 }
 
 void xnarch_init_shadow_tcb(struct xnthread *thread)
 {
-   spl_t s;
struct xnarchtcb *tcb = xnthread_archtcb(thread);
-
tcb->fpup = &(tcb->core.host_task->thread.fpsimd_state);
-
-   xnlock_get_irqsave(, s);
xnthread_clear_state(thread, XNFPU);
-   xnlock_put_irqrestore(, s);
-
 }
 #endif /* CONFIG_XENO_ARCH_FPU */
 
@@ -210,16 +201,14 @@ void xnarch_switch_to(struct xnthread *out, struct 
xnthread *in)
xnarch_tls_thread_switch(in_tcb->core.tip->task);
xnarch_contextidr_thread_switch(in_tcb->core.tip->task);
 
-   /* check if we need to switch FPU on return to Linux */
-   if (xnthread_test_state(in, XNROOT) == 1)
-   

[Xenomai-git] Dmitriy Cherkasov : cobalt/arm64: FPU code cleanup

2017-02-15 Thread git repository hosting
Module: xenomai-3
Branch: next
Commit: e6e8acf9804f6fb1d71da51648277f9938495cd6
URL:
http://git.xenomai.org/?p=xenomai-3.git;a=commit;h=e6e8acf9804f6fb1d71da51648277f9938495cd6

Author: Dmitriy Cherkasov 
Date:   Fri Oct  2 12:54:37 2015 -0700

cobalt/arm64: FPU code cleanup

---

 kernel/cobalt/arch/arm64/thread.c |   89 -
 1 file changed, 39 insertions(+), 50 deletions(-)

diff --git a/kernel/cobalt/arch/arm64/thread.c 
b/kernel/cobalt/arch/arm64/thread.c
index db369be..35dbd72 100644
--- a/kernel/cobalt/arch/arm64/thread.c
+++ b/kernel/cobalt/arch/arm64/thread.c
@@ -36,31 +36,44 @@
 
 #if defined(CONFIG_XENO_ARCH_FPU)
 
-static void enable_fpsimd(void) {
-   __asm__ __volatile__("mrs x1, cpacr_el1\n\
-   orr x1, x1, #(0x3 << 20)\n\
-   msr cpacr_el1, x1\n\
-   isb" : : : "x1", "memory", "cc");
+#define FPSIMD_EN (0x3 << 20)
+
+static inline unsigned long get_cpacr(void)
+{
+   unsigned long result;
+   __asm__ __volatile__("mrs %0, cpacr_el1": "=r"(result));
+   return result;
+}
+
+static inline void set_cpacr(long val)
+{
+   __asm__ __volatile__ (
+   "msr cpacr_el1, %0\n\t"
+   "isb"
+   : /* */ : "r"(val));
 }
 
-static void disable_fpsimd(void) {
-   __asm__ __volatile__("mrs x1, cpacr_el1\n\
-   and x1, x1, #~(0x3 << 20)\n\
-   msr cpacr_el1, x1\n\
-   isb" : : : "x1", "memory", "cc");
+static void enable_fpsimd(void)
+{
+   unsigned long cpacr = get_cpacr();
+   cpacr |= FPSIMD_EN;
+   set_cpacr(cpacr);
+}
+
+static void disable_fpsimd(void)
+{
+   unsigned long cpacr = get_cpacr();
+   cpacr &= ~FPSIMD_EN;
+   set_cpacr(cpacr);
 }
 
 int xnarch_fault_fpu_p(struct ipipe_trap_data *d)
 {
-   /* check if this is an FPU access trap to be handled by Xenomai */
-   if(d->exception == IPIPE_TRAP_FPU_ACC){
-   return 1;
-   }
-   /* FPU already enabled, propagate fault to kernel */
-   return 0;
+   return (d->exception == IPIPE_TRAP_FPU_ACC);
 }
 
-static inline struct fpsimd_state *get_fpu_owner(struct xnarchtcb *tcb) {
+static inline struct fpsimd_state *get_fpu_owner(struct xnarchtcb *tcb)
+{
return &(tcb->core.tsp->fpsimd_state);
 }
 
@@ -68,7 +81,6 @@ void xnarch_leave_root(struct xnthread *root)
 {
struct xnarchtcb *rootcb = xnthread_archtcb(root);
rootcb->fpup = get_fpu_owner(rootcb);
-   disable_fpsimd();
 }
 
 void xnarch_save_fpu(struct xnthread *thread)
@@ -83,29 +95,15 @@ 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;
 
-   /*
-* This only gets called if XNFPU flag is set, or if migrating to Linux.
-* In both cases, this means turn on FPU and switch.
-*/
enable_fpsimd();
 
-   if (xnthread_test_state(to, XNROOT) == 0) {
-   if (from_fpup == to_fpup)
-   return;
-
-   if (from_fpup)
-   fpsimd_save_state(from_fpup);
-
-   fpsimd_load_state(to_fpup);
-   }
-   else {
-   /* Going to Linux. */
-   if (from_fpup)
-   fpsimd_save_state(from_fpup);
+   if (from_fpup == to_fpup)
+   return;
 
-   fpsimd_load_state(to_fpup);
-   }
+   if (from_fpup)
+   fpsimd_save_state(from_fpup);
 
+   fpsimd_load_state(to_fpup);
 }
 
 int xnarch_handle_fpu_fault(struct xnthread *from, 
@@ -113,9 +111,9 @@ int xnarch_handle_fpu_fault(struct xnthread *from,
 {
spl_t s;
 
+   /* FPU should already be enabled for XNFPU tasks. */
if (xnthread_test_state(to, XNFPU))
-   /* FPU is already enabled, probably an exception */
-  return 0;
+   BUG();
 
xnlock_get_irqsave(, s);
xnthread_set_state(to, XNFPU);
@@ -124,20 +122,13 @@ int xnarch_handle_fpu_fault(struct xnthread *from,
xnarch_switch_fpu(from, to);
 
return 1;
-
 }
 
 void xnarch_init_shadow_tcb(struct xnthread *thread)
 {
-   spl_t s;
struct xnarchtcb *tcb = xnthread_archtcb(thread);
-
tcb->fpup = &(tcb->core.host_task->thread.fpsimd_state);
-
-   xnlock_get_irqsave(, s);
xnthread_clear_state(thread, XNFPU);
-   xnlock_put_irqrestore(, s);
-
 }
 #endif /* CONFIG_XENO_ARCH_FPU */
 
@@ -210,16 +201,14 @@ void xnarch_switch_to(struct xnthread *out, struct 
xnthread *in)
xnarch_tls_thread_switch(in_tcb->core.tip->task);
xnarch_contextidr_thread_switch(in_tcb->core.tip->task);
 
-   /* check if we need to switch FPU on return to Linux */
-   if (xnthread_test_state(in, XNROOT) == 1)
-   

[Xenomai-git] Dmitriy Cherkasov : cobalt/arm64: FPU code cleanup

2017-01-26 Thread git repository hosting
Module: xenomai-3
Branch: next
Commit: 803335f8c02d50830930599d200e1d4d552c1ba7
URL:
http://git.xenomai.org/?p=xenomai-3.git;a=commit;h=803335f8c02d50830930599d200e1d4d552c1ba7

Author: Dmitriy Cherkasov 
Date:   Fri Oct  2 12:54:37 2015 -0700

cobalt/arm64: FPU code cleanup

---

 kernel/cobalt/arch/arm64/thread.c |   89 -
 1 file changed, 39 insertions(+), 50 deletions(-)

diff --git a/kernel/cobalt/arch/arm64/thread.c 
b/kernel/cobalt/arch/arm64/thread.c
index db369be..35dbd72 100644
--- a/kernel/cobalt/arch/arm64/thread.c
+++ b/kernel/cobalt/arch/arm64/thread.c
@@ -36,31 +36,44 @@
 
 #if defined(CONFIG_XENO_ARCH_FPU)
 
-static void enable_fpsimd(void) {
-   __asm__ __volatile__("mrs x1, cpacr_el1\n\
-   orr x1, x1, #(0x3 << 20)\n\
-   msr cpacr_el1, x1\n\
-   isb" : : : "x1", "memory", "cc");
+#define FPSIMD_EN (0x3 << 20)
+
+static inline unsigned long get_cpacr(void)
+{
+   unsigned long result;
+   __asm__ __volatile__("mrs %0, cpacr_el1": "=r"(result));
+   return result;
+}
+
+static inline void set_cpacr(long val)
+{
+   __asm__ __volatile__ (
+   "msr cpacr_el1, %0\n\t"
+   "isb"
+   : /* */ : "r"(val));
 }
 
-static void disable_fpsimd(void) {
-   __asm__ __volatile__("mrs x1, cpacr_el1\n\
-   and x1, x1, #~(0x3 << 20)\n\
-   msr cpacr_el1, x1\n\
-   isb" : : : "x1", "memory", "cc");
+static void enable_fpsimd(void)
+{
+   unsigned long cpacr = get_cpacr();
+   cpacr |= FPSIMD_EN;
+   set_cpacr(cpacr);
+}
+
+static void disable_fpsimd(void)
+{
+   unsigned long cpacr = get_cpacr();
+   cpacr &= ~FPSIMD_EN;
+   set_cpacr(cpacr);
 }
 
 int xnarch_fault_fpu_p(struct ipipe_trap_data *d)
 {
-   /* check if this is an FPU access trap to be handled by Xenomai */
-   if(d->exception == IPIPE_TRAP_FPU_ACC){
-   return 1;
-   }
-   /* FPU already enabled, propagate fault to kernel */
-   return 0;
+   return (d->exception == IPIPE_TRAP_FPU_ACC);
 }
 
-static inline struct fpsimd_state *get_fpu_owner(struct xnarchtcb *tcb) {
+static inline struct fpsimd_state *get_fpu_owner(struct xnarchtcb *tcb)
+{
return &(tcb->core.tsp->fpsimd_state);
 }
 
@@ -68,7 +81,6 @@ void xnarch_leave_root(struct xnthread *root)
 {
struct xnarchtcb *rootcb = xnthread_archtcb(root);
rootcb->fpup = get_fpu_owner(rootcb);
-   disable_fpsimd();
 }
 
 void xnarch_save_fpu(struct xnthread *thread)
@@ -83,29 +95,15 @@ 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;
 
-   /*
-* This only gets called if XNFPU flag is set, or if migrating to Linux.
-* In both cases, this means turn on FPU and switch.
-*/
enable_fpsimd();
 
-   if (xnthread_test_state(to, XNROOT) == 0) {
-   if (from_fpup == to_fpup)
-   return;
-
-   if (from_fpup)
-   fpsimd_save_state(from_fpup);
-
-   fpsimd_load_state(to_fpup);
-   }
-   else {
-   /* Going to Linux. */
-   if (from_fpup)
-   fpsimd_save_state(from_fpup);
+   if (from_fpup == to_fpup)
+   return;
 
-   fpsimd_load_state(to_fpup);
-   }
+   if (from_fpup)
+   fpsimd_save_state(from_fpup);
 
+   fpsimd_load_state(to_fpup);
 }
 
 int xnarch_handle_fpu_fault(struct xnthread *from, 
@@ -113,9 +111,9 @@ int xnarch_handle_fpu_fault(struct xnthread *from,
 {
spl_t s;
 
+   /* FPU should already be enabled for XNFPU tasks. */
if (xnthread_test_state(to, XNFPU))
-   /* FPU is already enabled, probably an exception */
-  return 0;
+   BUG();
 
xnlock_get_irqsave(, s);
xnthread_set_state(to, XNFPU);
@@ -124,20 +122,13 @@ int xnarch_handle_fpu_fault(struct xnthread *from,
xnarch_switch_fpu(from, to);
 
return 1;
-
 }
 
 void xnarch_init_shadow_tcb(struct xnthread *thread)
 {
-   spl_t s;
struct xnarchtcb *tcb = xnthread_archtcb(thread);
-
tcb->fpup = &(tcb->core.host_task->thread.fpsimd_state);
-
-   xnlock_get_irqsave(, s);
xnthread_clear_state(thread, XNFPU);
-   xnlock_put_irqrestore(, s);
-
 }
 #endif /* CONFIG_XENO_ARCH_FPU */
 
@@ -210,16 +201,14 @@ void xnarch_switch_to(struct xnthread *out, struct 
xnthread *in)
xnarch_tls_thread_switch(in_tcb->core.tip->task);
xnarch_contextidr_thread_switch(in_tcb->core.tip->task);
 
-   /* check if we need to switch FPU on return to Linux */
-   if (xnthread_test_state(in, XNROOT) == 1)
-   

[Xenomai-git] Dmitriy Cherkasov : cobalt/arm64: FPU code cleanup

2016-12-09 Thread git repository hosting
Module: xenomai-3
Branch: next
Commit: 1c5f5583772012cc7cea9dc34d4ab7b753d1e787
URL:
http://git.xenomai.org/?p=xenomai-3.git;a=commit;h=1c5f5583772012cc7cea9dc34d4ab7b753d1e787

Author: Dmitriy Cherkasov 
Date:   Fri Oct  2 12:54:37 2015 -0700

cobalt/arm64: FPU code cleanup

---

 kernel/cobalt/arch/arm64/thread.c |   89 -
 1 file changed, 39 insertions(+), 50 deletions(-)

diff --git a/kernel/cobalt/arch/arm64/thread.c 
b/kernel/cobalt/arch/arm64/thread.c
index db369be..35dbd72 100644
--- a/kernel/cobalt/arch/arm64/thread.c
+++ b/kernel/cobalt/arch/arm64/thread.c
@@ -36,31 +36,44 @@
 
 #if defined(CONFIG_XENO_ARCH_FPU)
 
-static void enable_fpsimd(void) {
-   __asm__ __volatile__("mrs x1, cpacr_el1\n\
-   orr x1, x1, #(0x3 << 20)\n\
-   msr cpacr_el1, x1\n\
-   isb" : : : "x1", "memory", "cc");
+#define FPSIMD_EN (0x3 << 20)
+
+static inline unsigned long get_cpacr(void)
+{
+   unsigned long result;
+   __asm__ __volatile__("mrs %0, cpacr_el1": "=r"(result));
+   return result;
+}
+
+static inline void set_cpacr(long val)
+{
+   __asm__ __volatile__ (
+   "msr cpacr_el1, %0\n\t"
+   "isb"
+   : /* */ : "r"(val));
 }
 
-static void disable_fpsimd(void) {
-   __asm__ __volatile__("mrs x1, cpacr_el1\n\
-   and x1, x1, #~(0x3 << 20)\n\
-   msr cpacr_el1, x1\n\
-   isb" : : : "x1", "memory", "cc");
+static void enable_fpsimd(void)
+{
+   unsigned long cpacr = get_cpacr();
+   cpacr |= FPSIMD_EN;
+   set_cpacr(cpacr);
+}
+
+static void disable_fpsimd(void)
+{
+   unsigned long cpacr = get_cpacr();
+   cpacr &= ~FPSIMD_EN;
+   set_cpacr(cpacr);
 }
 
 int xnarch_fault_fpu_p(struct ipipe_trap_data *d)
 {
-   /* check if this is an FPU access trap to be handled by Xenomai */
-   if(d->exception == IPIPE_TRAP_FPU_ACC){
-   return 1;
-   }
-   /* FPU already enabled, propagate fault to kernel */
-   return 0;
+   return (d->exception == IPIPE_TRAP_FPU_ACC);
 }
 
-static inline struct fpsimd_state *get_fpu_owner(struct xnarchtcb *tcb) {
+static inline struct fpsimd_state *get_fpu_owner(struct xnarchtcb *tcb)
+{
return &(tcb->core.tsp->fpsimd_state);
 }
 
@@ -68,7 +81,6 @@ void xnarch_leave_root(struct xnthread *root)
 {
struct xnarchtcb *rootcb = xnthread_archtcb(root);
rootcb->fpup = get_fpu_owner(rootcb);
-   disable_fpsimd();
 }
 
 void xnarch_save_fpu(struct xnthread *thread)
@@ -83,29 +95,15 @@ 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;
 
-   /*
-* This only gets called if XNFPU flag is set, or if migrating to Linux.
-* In both cases, this means turn on FPU and switch.
-*/
enable_fpsimd();
 
-   if (xnthread_test_state(to, XNROOT) == 0) {
-   if (from_fpup == to_fpup)
-   return;
-
-   if (from_fpup)
-   fpsimd_save_state(from_fpup);
-
-   fpsimd_load_state(to_fpup);
-   }
-   else {
-   /* Going to Linux. */
-   if (from_fpup)
-   fpsimd_save_state(from_fpup);
+   if (from_fpup == to_fpup)
+   return;
 
-   fpsimd_load_state(to_fpup);
-   }
+   if (from_fpup)
+   fpsimd_save_state(from_fpup);
 
+   fpsimd_load_state(to_fpup);
 }
 
 int xnarch_handle_fpu_fault(struct xnthread *from, 
@@ -113,9 +111,9 @@ int xnarch_handle_fpu_fault(struct xnthread *from,
 {
spl_t s;
 
+   /* FPU should already be enabled for XNFPU tasks. */
if (xnthread_test_state(to, XNFPU))
-   /* FPU is already enabled, probably an exception */
-  return 0;
+   BUG();
 
xnlock_get_irqsave(, s);
xnthread_set_state(to, XNFPU);
@@ -124,20 +122,13 @@ int xnarch_handle_fpu_fault(struct xnthread *from,
xnarch_switch_fpu(from, to);
 
return 1;
-
 }
 
 void xnarch_init_shadow_tcb(struct xnthread *thread)
 {
-   spl_t s;
struct xnarchtcb *tcb = xnthread_archtcb(thread);
-
tcb->fpup = &(tcb->core.host_task->thread.fpsimd_state);
-
-   xnlock_get_irqsave(, s);
xnthread_clear_state(thread, XNFPU);
-   xnlock_put_irqrestore(, s);
-
 }
 #endif /* CONFIG_XENO_ARCH_FPU */
 
@@ -210,16 +201,14 @@ void xnarch_switch_to(struct xnthread *out, struct 
xnthread *in)
xnarch_tls_thread_switch(in_tcb->core.tip->task);
xnarch_contextidr_thread_switch(in_tcb->core.tip->task);
 
-   /* check if we need to switch FPU on return to Linux */
-   if (xnthread_test_state(in, XNROOT) == 1)
-   

[Xenomai-git] Dmitriy Cherkasov : cobalt/arm64: FPU code cleanup

2016-11-28 Thread git repository hosting
Module: xenomai-3
Branch: next
Commit: 6a4a4de3fae86cd44e0f4a51df321d285d9a6c0f
URL:
http://git.xenomai.org/?p=xenomai-3.git;a=commit;h=6a4a4de3fae86cd44e0f4a51df321d285d9a6c0f

Author: Dmitriy Cherkasov 
Date:   Fri Oct  2 12:54:37 2015 -0700

cobalt/arm64: FPU code cleanup

---

 kernel/cobalt/arch/arm64/thread.c |   89 -
 1 file changed, 39 insertions(+), 50 deletions(-)

diff --git a/kernel/cobalt/arch/arm64/thread.c 
b/kernel/cobalt/arch/arm64/thread.c
index db369be..35dbd72 100644
--- a/kernel/cobalt/arch/arm64/thread.c
+++ b/kernel/cobalt/arch/arm64/thread.c
@@ -36,31 +36,44 @@
 
 #if defined(CONFIG_XENO_ARCH_FPU)
 
-static void enable_fpsimd(void) {
-   __asm__ __volatile__("mrs x1, cpacr_el1\n\
-   orr x1, x1, #(0x3 << 20)\n\
-   msr cpacr_el1, x1\n\
-   isb" : : : "x1", "memory", "cc");
+#define FPSIMD_EN (0x3 << 20)
+
+static inline unsigned long get_cpacr(void)
+{
+   unsigned long result;
+   __asm__ __volatile__("mrs %0, cpacr_el1": "=r"(result));
+   return result;
+}
+
+static inline void set_cpacr(long val)
+{
+   __asm__ __volatile__ (
+   "msr cpacr_el1, %0\n\t"
+   "isb"
+   : /* */ : "r"(val));
 }
 
-static void disable_fpsimd(void) {
-   __asm__ __volatile__("mrs x1, cpacr_el1\n\
-   and x1, x1, #~(0x3 << 20)\n\
-   msr cpacr_el1, x1\n\
-   isb" : : : "x1", "memory", "cc");
+static void enable_fpsimd(void)
+{
+   unsigned long cpacr = get_cpacr();
+   cpacr |= FPSIMD_EN;
+   set_cpacr(cpacr);
+}
+
+static void disable_fpsimd(void)
+{
+   unsigned long cpacr = get_cpacr();
+   cpacr &= ~FPSIMD_EN;
+   set_cpacr(cpacr);
 }
 
 int xnarch_fault_fpu_p(struct ipipe_trap_data *d)
 {
-   /* check if this is an FPU access trap to be handled by Xenomai */
-   if(d->exception == IPIPE_TRAP_FPU_ACC){
-   return 1;
-   }
-   /* FPU already enabled, propagate fault to kernel */
-   return 0;
+   return (d->exception == IPIPE_TRAP_FPU_ACC);
 }
 
-static inline struct fpsimd_state *get_fpu_owner(struct xnarchtcb *tcb) {
+static inline struct fpsimd_state *get_fpu_owner(struct xnarchtcb *tcb)
+{
return &(tcb->core.tsp->fpsimd_state);
 }
 
@@ -68,7 +81,6 @@ void xnarch_leave_root(struct xnthread *root)
 {
struct xnarchtcb *rootcb = xnthread_archtcb(root);
rootcb->fpup = get_fpu_owner(rootcb);
-   disable_fpsimd();
 }
 
 void xnarch_save_fpu(struct xnthread *thread)
@@ -83,29 +95,15 @@ 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;
 
-   /*
-* This only gets called if XNFPU flag is set, or if migrating to Linux.
-* In both cases, this means turn on FPU and switch.
-*/
enable_fpsimd();
 
-   if (xnthread_test_state(to, XNROOT) == 0) {
-   if (from_fpup == to_fpup)
-   return;
-
-   if (from_fpup)
-   fpsimd_save_state(from_fpup);
-
-   fpsimd_load_state(to_fpup);
-   }
-   else {
-   /* Going to Linux. */
-   if (from_fpup)
-   fpsimd_save_state(from_fpup);
+   if (from_fpup == to_fpup)
+   return;
 
-   fpsimd_load_state(to_fpup);
-   }
+   if (from_fpup)
+   fpsimd_save_state(from_fpup);
 
+   fpsimd_load_state(to_fpup);
 }
 
 int xnarch_handle_fpu_fault(struct xnthread *from, 
@@ -113,9 +111,9 @@ int xnarch_handle_fpu_fault(struct xnthread *from,
 {
spl_t s;
 
+   /* FPU should already be enabled for XNFPU tasks. */
if (xnthread_test_state(to, XNFPU))
-   /* FPU is already enabled, probably an exception */
-  return 0;
+   BUG();
 
xnlock_get_irqsave(, s);
xnthread_set_state(to, XNFPU);
@@ -124,20 +122,13 @@ int xnarch_handle_fpu_fault(struct xnthread *from,
xnarch_switch_fpu(from, to);
 
return 1;
-
 }
 
 void xnarch_init_shadow_tcb(struct xnthread *thread)
 {
-   spl_t s;
struct xnarchtcb *tcb = xnthread_archtcb(thread);
-
tcb->fpup = &(tcb->core.host_task->thread.fpsimd_state);
-
-   xnlock_get_irqsave(, s);
xnthread_clear_state(thread, XNFPU);
-   xnlock_put_irqrestore(, s);
-
 }
 #endif /* CONFIG_XENO_ARCH_FPU */
 
@@ -210,16 +201,14 @@ void xnarch_switch_to(struct xnthread *out, struct 
xnthread *in)
xnarch_tls_thread_switch(in_tcb->core.tip->task);
xnarch_contextidr_thread_switch(in_tcb->core.tip->task);
 
-   /* check if we need to switch FPU on return to Linux */
-   if (xnthread_test_state(in, XNROOT) == 1)
-   

[Xenomai-git] Dmitriy Cherkasov : cobalt/arm64: FPU code cleanup

2016-11-21 Thread git repository hosting
Module: xenomai-3
Branch: next
Commit: bec4dce9927e822be8c32f5534a6e87334d16c6e
URL:
http://git.xenomai.org/?p=xenomai-3.git;a=commit;h=bec4dce9927e822be8c32f5534a6e87334d16c6e

Author: Dmitriy Cherkasov 
Date:   Fri Oct  2 12:54:37 2015 -0700

cobalt/arm64: FPU code cleanup

---

 kernel/cobalt/arch/arm64/thread.c |   89 -
 1 file changed, 39 insertions(+), 50 deletions(-)

diff --git a/kernel/cobalt/arch/arm64/thread.c 
b/kernel/cobalt/arch/arm64/thread.c
index db369be..35dbd72 100644
--- a/kernel/cobalt/arch/arm64/thread.c
+++ b/kernel/cobalt/arch/arm64/thread.c
@@ -36,31 +36,44 @@
 
 #if defined(CONFIG_XENO_ARCH_FPU)
 
-static void enable_fpsimd(void) {
-   __asm__ __volatile__("mrs x1, cpacr_el1\n\
-   orr x1, x1, #(0x3 << 20)\n\
-   msr cpacr_el1, x1\n\
-   isb" : : : "x1", "memory", "cc");
+#define FPSIMD_EN (0x3 << 20)
+
+static inline unsigned long get_cpacr(void)
+{
+   unsigned long result;
+   __asm__ __volatile__("mrs %0, cpacr_el1": "=r"(result));
+   return result;
+}
+
+static inline void set_cpacr(long val)
+{
+   __asm__ __volatile__ (
+   "msr cpacr_el1, %0\n\t"
+   "isb"
+   : /* */ : "r"(val));
 }
 
-static void disable_fpsimd(void) {
-   __asm__ __volatile__("mrs x1, cpacr_el1\n\
-   and x1, x1, #~(0x3 << 20)\n\
-   msr cpacr_el1, x1\n\
-   isb" : : : "x1", "memory", "cc");
+static void enable_fpsimd(void)
+{
+   unsigned long cpacr = get_cpacr();
+   cpacr |= FPSIMD_EN;
+   set_cpacr(cpacr);
+}
+
+static void disable_fpsimd(void)
+{
+   unsigned long cpacr = get_cpacr();
+   cpacr &= ~FPSIMD_EN;
+   set_cpacr(cpacr);
 }
 
 int xnarch_fault_fpu_p(struct ipipe_trap_data *d)
 {
-   /* check if this is an FPU access trap to be handled by Xenomai */
-   if(d->exception == IPIPE_TRAP_FPU_ACC){
-   return 1;
-   }
-   /* FPU already enabled, propagate fault to kernel */
-   return 0;
+   return (d->exception == IPIPE_TRAP_FPU_ACC);
 }
 
-static inline struct fpsimd_state *get_fpu_owner(struct xnarchtcb *tcb) {
+static inline struct fpsimd_state *get_fpu_owner(struct xnarchtcb *tcb)
+{
return &(tcb->core.tsp->fpsimd_state);
 }
 
@@ -68,7 +81,6 @@ void xnarch_leave_root(struct xnthread *root)
 {
struct xnarchtcb *rootcb = xnthread_archtcb(root);
rootcb->fpup = get_fpu_owner(rootcb);
-   disable_fpsimd();
 }
 
 void xnarch_save_fpu(struct xnthread *thread)
@@ -83,29 +95,15 @@ 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;
 
-   /*
-* This only gets called if XNFPU flag is set, or if migrating to Linux.
-* In both cases, this means turn on FPU and switch.
-*/
enable_fpsimd();
 
-   if (xnthread_test_state(to, XNROOT) == 0) {
-   if (from_fpup == to_fpup)
-   return;
-
-   if (from_fpup)
-   fpsimd_save_state(from_fpup);
-
-   fpsimd_load_state(to_fpup);
-   }
-   else {
-   /* Going to Linux. */
-   if (from_fpup)
-   fpsimd_save_state(from_fpup);
+   if (from_fpup == to_fpup)
+   return;
 
-   fpsimd_load_state(to_fpup);
-   }
+   if (from_fpup)
+   fpsimd_save_state(from_fpup);
 
+   fpsimd_load_state(to_fpup);
 }
 
 int xnarch_handle_fpu_fault(struct xnthread *from, 
@@ -113,9 +111,9 @@ int xnarch_handle_fpu_fault(struct xnthread *from,
 {
spl_t s;
 
+   /* FPU should already be enabled for XNFPU tasks. */
if (xnthread_test_state(to, XNFPU))
-   /* FPU is already enabled, probably an exception */
-  return 0;
+   BUG();
 
xnlock_get_irqsave(, s);
xnthread_set_state(to, XNFPU);
@@ -124,20 +122,13 @@ int xnarch_handle_fpu_fault(struct xnthread *from,
xnarch_switch_fpu(from, to);
 
return 1;
-
 }
 
 void xnarch_init_shadow_tcb(struct xnthread *thread)
 {
-   spl_t s;
struct xnarchtcb *tcb = xnthread_archtcb(thread);
-
tcb->fpup = &(tcb->core.host_task->thread.fpsimd_state);
-
-   xnlock_get_irqsave(, s);
xnthread_clear_state(thread, XNFPU);
-   xnlock_put_irqrestore(, s);
-
 }
 #endif /* CONFIG_XENO_ARCH_FPU */
 
@@ -210,16 +201,14 @@ void xnarch_switch_to(struct xnthread *out, struct 
xnthread *in)
xnarch_tls_thread_switch(in_tcb->core.tip->task);
xnarch_contextidr_thread_switch(in_tcb->core.tip->task);
 
-   /* check if we need to switch FPU on return to Linux */
-   if (xnthread_test_state(in, XNROOT) == 1)
-   

[Xenomai-git] Dmitriy Cherkasov : cobalt/arm64: FPU code cleanup

2016-11-15 Thread git repository hosting
Module: xenomai-3
Branch: next
Commit: c11a2e2f21650bb21c04e8fbfd100ed1c6167d95
URL:
http://git.xenomai.org/?p=xenomai-3.git;a=commit;h=c11a2e2f21650bb21c04e8fbfd100ed1c6167d95

Author: Dmitriy Cherkasov 
Date:   Fri Oct  2 12:54:37 2015 -0700

cobalt/arm64: FPU code cleanup

---

 kernel/cobalt/arch/arm64/thread.c |   89 -
 1 file changed, 39 insertions(+), 50 deletions(-)

diff --git a/kernel/cobalt/arch/arm64/thread.c 
b/kernel/cobalt/arch/arm64/thread.c
index db369be..35dbd72 100644
--- a/kernel/cobalt/arch/arm64/thread.c
+++ b/kernel/cobalt/arch/arm64/thread.c
@@ -36,31 +36,44 @@
 
 #if defined(CONFIG_XENO_ARCH_FPU)
 
-static void enable_fpsimd(void) {
-   __asm__ __volatile__("mrs x1, cpacr_el1\n\
-   orr x1, x1, #(0x3 << 20)\n\
-   msr cpacr_el1, x1\n\
-   isb" : : : "x1", "memory", "cc");
+#define FPSIMD_EN (0x3 << 20)
+
+static inline unsigned long get_cpacr(void)
+{
+   unsigned long result;
+   __asm__ __volatile__("mrs %0, cpacr_el1": "=r"(result));
+   return result;
+}
+
+static inline void set_cpacr(long val)
+{
+   __asm__ __volatile__ (
+   "msr cpacr_el1, %0\n\t"
+   "isb"
+   : /* */ : "r"(val));
 }
 
-static void disable_fpsimd(void) {
-   __asm__ __volatile__("mrs x1, cpacr_el1\n\
-   and x1, x1, #~(0x3 << 20)\n\
-   msr cpacr_el1, x1\n\
-   isb" : : : "x1", "memory", "cc");
+static void enable_fpsimd(void)
+{
+   unsigned long cpacr = get_cpacr();
+   cpacr |= FPSIMD_EN;
+   set_cpacr(cpacr);
+}
+
+static void disable_fpsimd(void)
+{
+   unsigned long cpacr = get_cpacr();
+   cpacr &= ~FPSIMD_EN;
+   set_cpacr(cpacr);
 }
 
 int xnarch_fault_fpu_p(struct ipipe_trap_data *d)
 {
-   /* check if this is an FPU access trap to be handled by Xenomai */
-   if(d->exception == IPIPE_TRAP_FPU_ACC){
-   return 1;
-   }
-   /* FPU already enabled, propagate fault to kernel */
-   return 0;
+   return (d->exception == IPIPE_TRAP_FPU_ACC);
 }
 
-static inline struct fpsimd_state *get_fpu_owner(struct xnarchtcb *tcb) {
+static inline struct fpsimd_state *get_fpu_owner(struct xnarchtcb *tcb)
+{
return &(tcb->core.tsp->fpsimd_state);
 }
 
@@ -68,7 +81,6 @@ void xnarch_leave_root(struct xnthread *root)
 {
struct xnarchtcb *rootcb = xnthread_archtcb(root);
rootcb->fpup = get_fpu_owner(rootcb);
-   disable_fpsimd();
 }
 
 void xnarch_save_fpu(struct xnthread *thread)
@@ -83,29 +95,15 @@ 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;
 
-   /*
-* This only gets called if XNFPU flag is set, or if migrating to Linux.
-* In both cases, this means turn on FPU and switch.
-*/
enable_fpsimd();
 
-   if (xnthread_test_state(to, XNROOT) == 0) {
-   if (from_fpup == to_fpup)
-   return;
-
-   if (from_fpup)
-   fpsimd_save_state(from_fpup);
-
-   fpsimd_load_state(to_fpup);
-   }
-   else {
-   /* Going to Linux. */
-   if (from_fpup)
-   fpsimd_save_state(from_fpup);
+   if (from_fpup == to_fpup)
+   return;
 
-   fpsimd_load_state(to_fpup);
-   }
+   if (from_fpup)
+   fpsimd_save_state(from_fpup);
 
+   fpsimd_load_state(to_fpup);
 }
 
 int xnarch_handle_fpu_fault(struct xnthread *from, 
@@ -113,9 +111,9 @@ int xnarch_handle_fpu_fault(struct xnthread *from,
 {
spl_t s;
 
+   /* FPU should already be enabled for XNFPU tasks. */
if (xnthread_test_state(to, XNFPU))
-   /* FPU is already enabled, probably an exception */
-  return 0;
+   BUG();
 
xnlock_get_irqsave(, s);
xnthread_set_state(to, XNFPU);
@@ -124,20 +122,13 @@ int xnarch_handle_fpu_fault(struct xnthread *from,
xnarch_switch_fpu(from, to);
 
return 1;
-
 }
 
 void xnarch_init_shadow_tcb(struct xnthread *thread)
 {
-   spl_t s;
struct xnarchtcb *tcb = xnthread_archtcb(thread);
-
tcb->fpup = &(tcb->core.host_task->thread.fpsimd_state);
-
-   xnlock_get_irqsave(, s);
xnthread_clear_state(thread, XNFPU);
-   xnlock_put_irqrestore(, s);
-
 }
 #endif /* CONFIG_XENO_ARCH_FPU */
 
@@ -210,16 +201,14 @@ void xnarch_switch_to(struct xnthread *out, struct 
xnthread *in)
xnarch_tls_thread_switch(in_tcb->core.tip->task);
xnarch_contextidr_thread_switch(in_tcb->core.tip->task);
 
-   /* check if we need to switch FPU on return to Linux */
-   if (xnthread_test_state(in, XNROOT) == 1)
-   

[Xenomai-git] Dmitriy Cherkasov : cobalt/arm64: FPU code cleanup

2016-10-17 Thread git repository hosting
Module: xenomai-3
Branch: next
Commit: b432ec42055abb5a57736fbfe8d72854ecc06a82
URL:
http://git.xenomai.org/?p=xenomai-3.git;a=commit;h=b432ec42055abb5a57736fbfe8d72854ecc06a82

Author: Dmitriy Cherkasov 
Date:   Fri Oct  2 12:54:37 2015 -0700

cobalt/arm64: FPU code cleanup

---

 kernel/cobalt/arch/arm64/thread.c |   89 -
 1 file changed, 39 insertions(+), 50 deletions(-)

diff --git a/kernel/cobalt/arch/arm64/thread.c 
b/kernel/cobalt/arch/arm64/thread.c
index db369be..35dbd72 100644
--- a/kernel/cobalt/arch/arm64/thread.c
+++ b/kernel/cobalt/arch/arm64/thread.c
@@ -36,31 +36,44 @@
 
 #if defined(CONFIG_XENO_ARCH_FPU)
 
-static void enable_fpsimd(void) {
-   __asm__ __volatile__("mrs x1, cpacr_el1\n\
-   orr x1, x1, #(0x3 << 20)\n\
-   msr cpacr_el1, x1\n\
-   isb" : : : "x1", "memory", "cc");
+#define FPSIMD_EN (0x3 << 20)
+
+static inline unsigned long get_cpacr(void)
+{
+   unsigned long result;
+   __asm__ __volatile__("mrs %0, cpacr_el1": "=r"(result));
+   return result;
+}
+
+static inline void set_cpacr(long val)
+{
+   __asm__ __volatile__ (
+   "msr cpacr_el1, %0\n\t"
+   "isb"
+   : /* */ : "r"(val));
 }
 
-static void disable_fpsimd(void) {
-   __asm__ __volatile__("mrs x1, cpacr_el1\n\
-   and x1, x1, #~(0x3 << 20)\n\
-   msr cpacr_el1, x1\n\
-   isb" : : : "x1", "memory", "cc");
+static void enable_fpsimd(void)
+{
+   unsigned long cpacr = get_cpacr();
+   cpacr |= FPSIMD_EN;
+   set_cpacr(cpacr);
+}
+
+static void disable_fpsimd(void)
+{
+   unsigned long cpacr = get_cpacr();
+   cpacr &= ~FPSIMD_EN;
+   set_cpacr(cpacr);
 }
 
 int xnarch_fault_fpu_p(struct ipipe_trap_data *d)
 {
-   /* check if this is an FPU access trap to be handled by Xenomai */
-   if(d->exception == IPIPE_TRAP_FPU_ACC){
-   return 1;
-   }
-   /* FPU already enabled, propagate fault to kernel */
-   return 0;
+   return (d->exception == IPIPE_TRAP_FPU_ACC);
 }
 
-static inline struct fpsimd_state *get_fpu_owner(struct xnarchtcb *tcb) {
+static inline struct fpsimd_state *get_fpu_owner(struct xnarchtcb *tcb)
+{
return &(tcb->core.tsp->fpsimd_state);
 }
 
@@ -68,7 +81,6 @@ void xnarch_leave_root(struct xnthread *root)
 {
struct xnarchtcb *rootcb = xnthread_archtcb(root);
rootcb->fpup = get_fpu_owner(rootcb);
-   disable_fpsimd();
 }
 
 void xnarch_save_fpu(struct xnthread *thread)
@@ -83,29 +95,15 @@ 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;
 
-   /*
-* This only gets called if XNFPU flag is set, or if migrating to Linux.
-* In both cases, this means turn on FPU and switch.
-*/
enable_fpsimd();
 
-   if (xnthread_test_state(to, XNROOT) == 0) {
-   if (from_fpup == to_fpup)
-   return;
-
-   if (from_fpup)
-   fpsimd_save_state(from_fpup);
-
-   fpsimd_load_state(to_fpup);
-   }
-   else {
-   /* Going to Linux. */
-   if (from_fpup)
-   fpsimd_save_state(from_fpup);
+   if (from_fpup == to_fpup)
+   return;
 
-   fpsimd_load_state(to_fpup);
-   }
+   if (from_fpup)
+   fpsimd_save_state(from_fpup);
 
+   fpsimd_load_state(to_fpup);
 }
 
 int xnarch_handle_fpu_fault(struct xnthread *from, 
@@ -113,9 +111,9 @@ int xnarch_handle_fpu_fault(struct xnthread *from,
 {
spl_t s;
 
+   /* FPU should already be enabled for XNFPU tasks. */
if (xnthread_test_state(to, XNFPU))
-   /* FPU is already enabled, probably an exception */
-  return 0;
+   BUG();
 
xnlock_get_irqsave(, s);
xnthread_set_state(to, XNFPU);
@@ -124,20 +122,13 @@ int xnarch_handle_fpu_fault(struct xnthread *from,
xnarch_switch_fpu(from, to);
 
return 1;
-
 }
 
 void xnarch_init_shadow_tcb(struct xnthread *thread)
 {
-   spl_t s;
struct xnarchtcb *tcb = xnthread_archtcb(thread);
-
tcb->fpup = &(tcb->core.host_task->thread.fpsimd_state);
-
-   xnlock_get_irqsave(, s);
xnthread_clear_state(thread, XNFPU);
-   xnlock_put_irqrestore(, s);
-
 }
 #endif /* CONFIG_XENO_ARCH_FPU */
 
@@ -210,16 +201,14 @@ void xnarch_switch_to(struct xnthread *out, struct 
xnthread *in)
xnarch_tls_thread_switch(in_tcb->core.tip->task);
xnarch_contextidr_thread_switch(in_tcb->core.tip->task);
 
-   /* check if we need to switch FPU on return to Linux */
-   if (xnthread_test_state(in, XNROOT) == 1)
-   

[Xenomai-git] Dmitriy Cherkasov : cobalt/arm64: FPU code cleanup

2016-09-22 Thread git repository hosting
Module: xenomai-3
Branch: next
Commit: b17b2ed0448055fb13c3008cebb5f4acb023b2fb
URL:
http://git.xenomai.org/?p=xenomai-3.git;a=commit;h=b17b2ed0448055fb13c3008cebb5f4acb023b2fb

Author: Dmitriy Cherkasov 
Date:   Fri Oct  2 12:54:37 2015 -0700

cobalt/arm64: FPU code cleanup

---

 kernel/cobalt/arch/arm64/thread.c |   89 -
 1 file changed, 39 insertions(+), 50 deletions(-)

diff --git a/kernel/cobalt/arch/arm64/thread.c 
b/kernel/cobalt/arch/arm64/thread.c
index db369be..35dbd72 100644
--- a/kernel/cobalt/arch/arm64/thread.c
+++ b/kernel/cobalt/arch/arm64/thread.c
@@ -36,31 +36,44 @@
 
 #if defined(CONFIG_XENO_ARCH_FPU)
 
-static void enable_fpsimd(void) {
-   __asm__ __volatile__("mrs x1, cpacr_el1\n\
-   orr x1, x1, #(0x3 << 20)\n\
-   msr cpacr_el1, x1\n\
-   isb" : : : "x1", "memory", "cc");
+#define FPSIMD_EN (0x3 << 20)
+
+static inline unsigned long get_cpacr(void)
+{
+   unsigned long result;
+   __asm__ __volatile__("mrs %0, cpacr_el1": "=r"(result));
+   return result;
+}
+
+static inline void set_cpacr(long val)
+{
+   __asm__ __volatile__ (
+   "msr cpacr_el1, %0\n\t"
+   "isb"
+   : /* */ : "r"(val));
 }
 
-static void disable_fpsimd(void) {
-   __asm__ __volatile__("mrs x1, cpacr_el1\n\
-   and x1, x1, #~(0x3 << 20)\n\
-   msr cpacr_el1, x1\n\
-   isb" : : : "x1", "memory", "cc");
+static void enable_fpsimd(void)
+{
+   unsigned long cpacr = get_cpacr();
+   cpacr |= FPSIMD_EN;
+   set_cpacr(cpacr);
+}
+
+static void disable_fpsimd(void)
+{
+   unsigned long cpacr = get_cpacr();
+   cpacr &= ~FPSIMD_EN;
+   set_cpacr(cpacr);
 }
 
 int xnarch_fault_fpu_p(struct ipipe_trap_data *d)
 {
-   /* check if this is an FPU access trap to be handled by Xenomai */
-   if(d->exception == IPIPE_TRAP_FPU_ACC){
-   return 1;
-   }
-   /* FPU already enabled, propagate fault to kernel */
-   return 0;
+   return (d->exception == IPIPE_TRAP_FPU_ACC);
 }
 
-static inline struct fpsimd_state *get_fpu_owner(struct xnarchtcb *tcb) {
+static inline struct fpsimd_state *get_fpu_owner(struct xnarchtcb *tcb)
+{
return &(tcb->core.tsp->fpsimd_state);
 }
 
@@ -68,7 +81,6 @@ void xnarch_leave_root(struct xnthread *root)
 {
struct xnarchtcb *rootcb = xnthread_archtcb(root);
rootcb->fpup = get_fpu_owner(rootcb);
-   disable_fpsimd();
 }
 
 void xnarch_save_fpu(struct xnthread *thread)
@@ -83,29 +95,15 @@ 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;
 
-   /*
-* This only gets called if XNFPU flag is set, or if migrating to Linux.
-* In both cases, this means turn on FPU and switch.
-*/
enable_fpsimd();
 
-   if (xnthread_test_state(to, XNROOT) == 0) {
-   if (from_fpup == to_fpup)
-   return;
-
-   if (from_fpup)
-   fpsimd_save_state(from_fpup);
-
-   fpsimd_load_state(to_fpup);
-   }
-   else {
-   /* Going to Linux. */
-   if (from_fpup)
-   fpsimd_save_state(from_fpup);
+   if (from_fpup == to_fpup)
+   return;
 
-   fpsimd_load_state(to_fpup);
-   }
+   if (from_fpup)
+   fpsimd_save_state(from_fpup);
 
+   fpsimd_load_state(to_fpup);
 }
 
 int xnarch_handle_fpu_fault(struct xnthread *from, 
@@ -113,9 +111,9 @@ int xnarch_handle_fpu_fault(struct xnthread *from,
 {
spl_t s;
 
+   /* FPU should already be enabled for XNFPU tasks. */
if (xnthread_test_state(to, XNFPU))
-   /* FPU is already enabled, probably an exception */
-  return 0;
+   BUG();
 
xnlock_get_irqsave(, s);
xnthread_set_state(to, XNFPU);
@@ -124,20 +122,13 @@ int xnarch_handle_fpu_fault(struct xnthread *from,
xnarch_switch_fpu(from, to);
 
return 1;
-
 }
 
 void xnarch_init_shadow_tcb(struct xnthread *thread)
 {
-   spl_t s;
struct xnarchtcb *tcb = xnthread_archtcb(thread);
-
tcb->fpup = &(tcb->core.host_task->thread.fpsimd_state);
-
-   xnlock_get_irqsave(, s);
xnthread_clear_state(thread, XNFPU);
-   xnlock_put_irqrestore(, s);
-
 }
 #endif /* CONFIG_XENO_ARCH_FPU */
 
@@ -210,16 +201,14 @@ void xnarch_switch_to(struct xnthread *out, struct 
xnthread *in)
xnarch_tls_thread_switch(in_tcb->core.tip->task);
xnarch_contextidr_thread_switch(in_tcb->core.tip->task);
 
-   /* check if we need to switch FPU on return to Linux */
-   if (xnthread_test_state(in, XNROOT) == 1)
-   

[Xenomai-git] Dmitriy Cherkasov : cobalt/arm64: FPU code cleanup

2015-11-01 Thread git repository hosting
Module: xenomai-3
Branch: next
Commit: 6245e0842f74e49723ef1c32be5629cc42b7fc9a
URL:
http://git.xenomai.org/?p=xenomai-3.git;a=commit;h=6245e0842f74e49723ef1c32be5629cc42b7fc9a

Author: Dmitriy Cherkasov 
Date:   Fri Oct  2 12:54:37 2015 -0700

cobalt/arm64: FPU code cleanup

---

 kernel/cobalt/arch/arm64/thread.c |   89 -
 1 file changed, 39 insertions(+), 50 deletions(-)

diff --git a/kernel/cobalt/arch/arm64/thread.c 
b/kernel/cobalt/arch/arm64/thread.c
index db369be..35dbd72 100644
--- a/kernel/cobalt/arch/arm64/thread.c
+++ b/kernel/cobalt/arch/arm64/thread.c
@@ -36,31 +36,44 @@
 
 #if defined(CONFIG_XENO_ARCH_FPU)
 
-static void enable_fpsimd(void) {
-   __asm__ __volatile__("mrs x1, cpacr_el1\n\
-   orr x1, x1, #(0x3 << 20)\n\
-   msr cpacr_el1, x1\n\
-   isb" : : : "x1", "memory", "cc");
+#define FPSIMD_EN (0x3 << 20)
+
+static inline unsigned long get_cpacr(void)
+{
+   unsigned long result;
+   __asm__ __volatile__("mrs %0, cpacr_el1": "=r"(result));
+   return result;
+}
+
+static inline void set_cpacr(long val)
+{
+   __asm__ __volatile__ (
+   "msr cpacr_el1, %0\n\t"
+   "isb"
+   : /* */ : "r"(val));
 }
 
-static void disable_fpsimd(void) {
-   __asm__ __volatile__("mrs x1, cpacr_el1\n\
-   and x1, x1, #~(0x3 << 20)\n\
-   msr cpacr_el1, x1\n\
-   isb" : : : "x1", "memory", "cc");
+static void enable_fpsimd(void)
+{
+   unsigned long cpacr = get_cpacr();
+   cpacr |= FPSIMD_EN;
+   set_cpacr(cpacr);
+}
+
+static void disable_fpsimd(void)
+{
+   unsigned long cpacr = get_cpacr();
+   cpacr &= ~FPSIMD_EN;
+   set_cpacr(cpacr);
 }
 
 int xnarch_fault_fpu_p(struct ipipe_trap_data *d)
 {
-   /* check if this is an FPU access trap to be handled by Xenomai */
-   if(d->exception == IPIPE_TRAP_FPU_ACC){
-   return 1;
-   }
-   /* FPU already enabled, propagate fault to kernel */
-   return 0;
+   return (d->exception == IPIPE_TRAP_FPU_ACC);
 }
 
-static inline struct fpsimd_state *get_fpu_owner(struct xnarchtcb *tcb) {
+static inline struct fpsimd_state *get_fpu_owner(struct xnarchtcb *tcb)
+{
return &(tcb->core.tsp->fpsimd_state);
 }
 
@@ -68,7 +81,6 @@ void xnarch_leave_root(struct xnthread *root)
 {
struct xnarchtcb *rootcb = xnthread_archtcb(root);
rootcb->fpup = get_fpu_owner(rootcb);
-   disable_fpsimd();
 }
 
 void xnarch_save_fpu(struct xnthread *thread)
@@ -83,29 +95,15 @@ 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;
 
-   /*
-* This only gets called if XNFPU flag is set, or if migrating to Linux.
-* In both cases, this means turn on FPU and switch.
-*/
enable_fpsimd();
 
-   if (xnthread_test_state(to, XNROOT) == 0) {
-   if (from_fpup == to_fpup)
-   return;
-
-   if (from_fpup)
-   fpsimd_save_state(from_fpup);
-
-   fpsimd_load_state(to_fpup);
-   }
-   else {
-   /* Going to Linux. */
-   if (from_fpup)
-   fpsimd_save_state(from_fpup);
+   if (from_fpup == to_fpup)
+   return;
 
-   fpsimd_load_state(to_fpup);
-   }
+   if (from_fpup)
+   fpsimd_save_state(from_fpup);
 
+   fpsimd_load_state(to_fpup);
 }
 
 int xnarch_handle_fpu_fault(struct xnthread *from, 
@@ -113,9 +111,9 @@ int xnarch_handle_fpu_fault(struct xnthread *from,
 {
spl_t s;
 
+   /* FPU should already be enabled for XNFPU tasks. */
if (xnthread_test_state(to, XNFPU))
-   /* FPU is already enabled, probably an exception */
-  return 0;
+   BUG();
 
xnlock_get_irqsave(, s);
xnthread_set_state(to, XNFPU);
@@ -124,20 +122,13 @@ int xnarch_handle_fpu_fault(struct xnthread *from,
xnarch_switch_fpu(from, to);
 
return 1;
-
 }
 
 void xnarch_init_shadow_tcb(struct xnthread *thread)
 {
-   spl_t s;
struct xnarchtcb *tcb = xnthread_archtcb(thread);
-
tcb->fpup = &(tcb->core.host_task->thread.fpsimd_state);
-
-   xnlock_get_irqsave(, s);
xnthread_clear_state(thread, XNFPU);
-   xnlock_put_irqrestore(, s);
-
 }
 #endif /* CONFIG_XENO_ARCH_FPU */
 
@@ -210,16 +201,14 @@ void xnarch_switch_to(struct xnthread *out, struct 
xnthread *in)
xnarch_tls_thread_switch(in_tcb->core.tip->task);
xnarch_contextidr_thread_switch(in_tcb->core.tip->task);
 
-   /* check if we need to switch FPU on return to Linux */
-   if (xnthread_test_state(in, XNROOT) == 1)
-   

[Xenomai-git] Dmitriy Cherkasov : cobalt/arm64: FPU code cleanup

2015-10-21 Thread git repository hosting
Module: xenomai-3
Branch: arm64
Commit: f84c083ed6486ae792af13481edc6c2076d82296
URL:
http://git.xenomai.org/?p=xenomai-3.git;a=commit;h=f84c083ed6486ae792af13481edc6c2076d82296

Author: Dmitriy Cherkasov 
Date:   Fri Oct  2 12:54:37 2015 -0700

cobalt/arm64: FPU code cleanup

---

 kernel/cobalt/arch/arm64/thread.c |   89 -
 1 file changed, 39 insertions(+), 50 deletions(-)

diff --git a/kernel/cobalt/arch/arm64/thread.c 
b/kernel/cobalt/arch/arm64/thread.c
index db369be..35dbd72 100644
--- a/kernel/cobalt/arch/arm64/thread.c
+++ b/kernel/cobalt/arch/arm64/thread.c
@@ -36,31 +36,44 @@
 
 #if defined(CONFIG_XENO_ARCH_FPU)
 
-static void enable_fpsimd(void) {
-   __asm__ __volatile__("mrs x1, cpacr_el1\n\
-   orr x1, x1, #(0x3 << 20)\n\
-   msr cpacr_el1, x1\n\
-   isb" : : : "x1", "memory", "cc");
+#define FPSIMD_EN (0x3 << 20)
+
+static inline unsigned long get_cpacr(void)
+{
+   unsigned long result;
+   __asm__ __volatile__("mrs %0, cpacr_el1": "=r"(result));
+   return result;
+}
+
+static inline void set_cpacr(long val)
+{
+   __asm__ __volatile__ (
+   "msr cpacr_el1, %0\n\t"
+   "isb"
+   : /* */ : "r"(val));
 }
 
-static void disable_fpsimd(void) {
-   __asm__ __volatile__("mrs x1, cpacr_el1\n\
-   and x1, x1, #~(0x3 << 20)\n\
-   msr cpacr_el1, x1\n\
-   isb" : : : "x1", "memory", "cc");
+static void enable_fpsimd(void)
+{
+   unsigned long cpacr = get_cpacr();
+   cpacr |= FPSIMD_EN;
+   set_cpacr(cpacr);
+}
+
+static void disable_fpsimd(void)
+{
+   unsigned long cpacr = get_cpacr();
+   cpacr &= ~FPSIMD_EN;
+   set_cpacr(cpacr);
 }
 
 int xnarch_fault_fpu_p(struct ipipe_trap_data *d)
 {
-   /* check if this is an FPU access trap to be handled by Xenomai */
-   if(d->exception == IPIPE_TRAP_FPU_ACC){
-   return 1;
-   }
-   /* FPU already enabled, propagate fault to kernel */
-   return 0;
+   return (d->exception == IPIPE_TRAP_FPU_ACC);
 }
 
-static inline struct fpsimd_state *get_fpu_owner(struct xnarchtcb *tcb) {
+static inline struct fpsimd_state *get_fpu_owner(struct xnarchtcb *tcb)
+{
return &(tcb->core.tsp->fpsimd_state);
 }
 
@@ -68,7 +81,6 @@ void xnarch_leave_root(struct xnthread *root)
 {
struct xnarchtcb *rootcb = xnthread_archtcb(root);
rootcb->fpup = get_fpu_owner(rootcb);
-   disable_fpsimd();
 }
 
 void xnarch_save_fpu(struct xnthread *thread)
@@ -83,29 +95,15 @@ 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;
 
-   /*
-* This only gets called if XNFPU flag is set, or if migrating to Linux.
-* In both cases, this means turn on FPU and switch.
-*/
enable_fpsimd();
 
-   if (xnthread_test_state(to, XNROOT) == 0) {
-   if (from_fpup == to_fpup)
-   return;
-
-   if (from_fpup)
-   fpsimd_save_state(from_fpup);
-
-   fpsimd_load_state(to_fpup);
-   }
-   else {
-   /* Going to Linux. */
-   if (from_fpup)
-   fpsimd_save_state(from_fpup);
+   if (from_fpup == to_fpup)
+   return;
 
-   fpsimd_load_state(to_fpup);
-   }
+   if (from_fpup)
+   fpsimd_save_state(from_fpup);
 
+   fpsimd_load_state(to_fpup);
 }
 
 int xnarch_handle_fpu_fault(struct xnthread *from, 
@@ -113,9 +111,9 @@ int xnarch_handle_fpu_fault(struct xnthread *from,
 {
spl_t s;
 
+   /* FPU should already be enabled for XNFPU tasks. */
if (xnthread_test_state(to, XNFPU))
-   /* FPU is already enabled, probably an exception */
-  return 0;
+   BUG();
 
xnlock_get_irqsave(, s);
xnthread_set_state(to, XNFPU);
@@ -124,20 +122,13 @@ int xnarch_handle_fpu_fault(struct xnthread *from,
xnarch_switch_fpu(from, to);
 
return 1;
-
 }
 
 void xnarch_init_shadow_tcb(struct xnthread *thread)
 {
-   spl_t s;
struct xnarchtcb *tcb = xnthread_archtcb(thread);
-
tcb->fpup = &(tcb->core.host_task->thread.fpsimd_state);
-
-   xnlock_get_irqsave(, s);
xnthread_clear_state(thread, XNFPU);
-   xnlock_put_irqrestore(, s);
-
 }
 #endif /* CONFIG_XENO_ARCH_FPU */
 
@@ -210,16 +201,14 @@ void xnarch_switch_to(struct xnthread *out, struct 
xnthread *in)
xnarch_tls_thread_switch(in_tcb->core.tip->task);
xnarch_contextidr_thread_switch(in_tcb->core.tip->task);
 
-   /* check if we need to switch FPU on return to Linux */
-   if (xnthread_test_state(in, XNROOT) == 1)
-   

[Xenomai-git] Dmitriy Cherkasov : cobalt/arm64: FPU code cleanup

2015-10-17 Thread git repository hosting
Module: xenomai-3
Branch: arm64
Commit: dbaf7a78feb68dc2ae630c48dbad270a6cfac759
URL:
http://git.xenomai.org/?p=xenomai-3.git;a=commit;h=dbaf7a78feb68dc2ae630c48dbad270a6cfac759

Author: Dmitriy Cherkasov 
Date:   Fri Oct  2 12:54:37 2015 -0700

cobalt/arm64: FPU code cleanup

---

 kernel/cobalt/arch/arm64/thread.c |   89 -
 1 file changed, 39 insertions(+), 50 deletions(-)

diff --git a/kernel/cobalt/arch/arm64/thread.c 
b/kernel/cobalt/arch/arm64/thread.c
index db369be..35dbd72 100644
--- a/kernel/cobalt/arch/arm64/thread.c
+++ b/kernel/cobalt/arch/arm64/thread.c
@@ -36,31 +36,44 @@
 
 #if defined(CONFIG_XENO_ARCH_FPU)
 
-static void enable_fpsimd(void) {
-   __asm__ __volatile__("mrs x1, cpacr_el1\n\
-   orr x1, x1, #(0x3 << 20)\n\
-   msr cpacr_el1, x1\n\
-   isb" : : : "x1", "memory", "cc");
+#define FPSIMD_EN (0x3 << 20)
+
+static inline unsigned long get_cpacr(void)
+{
+   unsigned long result;
+   __asm__ __volatile__("mrs %0, cpacr_el1": "=r"(result));
+   return result;
+}
+
+static inline void set_cpacr(long val)
+{
+   __asm__ __volatile__ (
+   "msr cpacr_el1, %0\n\t"
+   "isb"
+   : /* */ : "r"(val));
 }
 
-static void disable_fpsimd(void) {
-   __asm__ __volatile__("mrs x1, cpacr_el1\n\
-   and x1, x1, #~(0x3 << 20)\n\
-   msr cpacr_el1, x1\n\
-   isb" : : : "x1", "memory", "cc");
+static void enable_fpsimd(void)
+{
+   unsigned long cpacr = get_cpacr();
+   cpacr |= FPSIMD_EN;
+   set_cpacr(cpacr);
+}
+
+static void disable_fpsimd(void)
+{
+   unsigned long cpacr = get_cpacr();
+   cpacr &= ~FPSIMD_EN;
+   set_cpacr(cpacr);
 }
 
 int xnarch_fault_fpu_p(struct ipipe_trap_data *d)
 {
-   /* check if this is an FPU access trap to be handled by Xenomai */
-   if(d->exception == IPIPE_TRAP_FPU_ACC){
-   return 1;
-   }
-   /* FPU already enabled, propagate fault to kernel */
-   return 0;
+   return (d->exception == IPIPE_TRAP_FPU_ACC);
 }
 
-static inline struct fpsimd_state *get_fpu_owner(struct xnarchtcb *tcb) {
+static inline struct fpsimd_state *get_fpu_owner(struct xnarchtcb *tcb)
+{
return &(tcb->core.tsp->fpsimd_state);
 }
 
@@ -68,7 +81,6 @@ void xnarch_leave_root(struct xnthread *root)
 {
struct xnarchtcb *rootcb = xnthread_archtcb(root);
rootcb->fpup = get_fpu_owner(rootcb);
-   disable_fpsimd();
 }
 
 void xnarch_save_fpu(struct xnthread *thread)
@@ -83,29 +95,15 @@ 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;
 
-   /*
-* This only gets called if XNFPU flag is set, or if migrating to Linux.
-* In both cases, this means turn on FPU and switch.
-*/
enable_fpsimd();
 
-   if (xnthread_test_state(to, XNROOT) == 0) {
-   if (from_fpup == to_fpup)
-   return;
-
-   if (from_fpup)
-   fpsimd_save_state(from_fpup);
-
-   fpsimd_load_state(to_fpup);
-   }
-   else {
-   /* Going to Linux. */
-   if (from_fpup)
-   fpsimd_save_state(from_fpup);
+   if (from_fpup == to_fpup)
+   return;
 
-   fpsimd_load_state(to_fpup);
-   }
+   if (from_fpup)
+   fpsimd_save_state(from_fpup);
 
+   fpsimd_load_state(to_fpup);
 }
 
 int xnarch_handle_fpu_fault(struct xnthread *from, 
@@ -113,9 +111,9 @@ int xnarch_handle_fpu_fault(struct xnthread *from,
 {
spl_t s;
 
+   /* FPU should already be enabled for XNFPU tasks. */
if (xnthread_test_state(to, XNFPU))
-   /* FPU is already enabled, probably an exception */
-  return 0;
+   BUG();
 
xnlock_get_irqsave(, s);
xnthread_set_state(to, XNFPU);
@@ -124,20 +122,13 @@ int xnarch_handle_fpu_fault(struct xnthread *from,
xnarch_switch_fpu(from, to);
 
return 1;
-
 }
 
 void xnarch_init_shadow_tcb(struct xnthread *thread)
 {
-   spl_t s;
struct xnarchtcb *tcb = xnthread_archtcb(thread);
-
tcb->fpup = &(tcb->core.host_task->thread.fpsimd_state);
-
-   xnlock_get_irqsave(, s);
xnthread_clear_state(thread, XNFPU);
-   xnlock_put_irqrestore(, s);
-
 }
 #endif /* CONFIG_XENO_ARCH_FPU */
 
@@ -210,16 +201,14 @@ void xnarch_switch_to(struct xnthread *out, struct 
xnthread *in)
xnarch_tls_thread_switch(in_tcb->core.tip->task);
xnarch_contextidr_thread_switch(in_tcb->core.tip->task);
 
-   /* check if we need to switch FPU on return to Linux */
-   if (xnthread_test_state(in, XNROOT) == 1)
-