Module: xenomai-gch
Branch: 2.6-fpu_trace
Commit: b684dda6bb5d70e51b6561e031f94a0221ce9102
URL:
http://git.xenomai.org/?p=xenomai-gch.git;a=commit;h=b684dda6bb5d70e51b6561e031f94a0221ce9102
Author: Gilles Chanteperdrix <gilles.chanteperd...@xenomai.org>
Date: Tue May 15 00:48:41 2012 +0200
fpu tracer
---
include/asm-arm/bits/pod.h | 16 +-
include/asm-arm/fp_dump.h | 89 +++++++
include/asm-generic/fp_dump.h | 152 ++++++++++++
include/asm-x86/bits/pod.h | 63 +++++-
include/asm-x86/fp_dump.h | 96 ++++++++
include/nucleus/fpu_trace.h | 47 ++++
ksrc/nucleus/Kconfig | 7 +
ksrc/nucleus/Makefile | 2 +
ksrc/nucleus/fpu_trace.c | 542 +++++++++++++++++++++++++++++++++++++++++
ksrc/nucleus/pod.c | 3 +
ksrc/nucleus/shadow.c | 3 +
11 files changed, 1011 insertions(+), 9 deletions(-)
diff --git a/include/asm-arm/bits/pod.h b/include/asm-arm/bits/pod.h
index 53a692e..c512b42 100644
--- a/include/asm-arm/bits/pod.h
+++ b/include/asm-arm/bits/pod.h
@@ -141,6 +141,7 @@ static inline void xnarch_enable_fpu(xnarchtcb_t *tcb)
*/
if (likely(!tcb->is_root)) {
rthal_enable_fpu();
+ fp_trace_xeno_enable(0, NULL);
/* No exception should be pending, since it should have caused
a trap earlier.
*/
@@ -149,8 +150,10 @@ static inline void xnarch_enable_fpu(xnarchtcb_t *tcb)
unsigned cpu;
#ifndef CONFIG_SMP
if (likely(!(fpexc & RTHAL_VFP_ANY_EXC)
- && !(rthal_vfp_fmrx(FPSCR) & FPSCR_IXE)))
+ && !(rthal_vfp_fmrx(FPSCR) & FPSCR_IXE))) {
+ fp_trace_xeno_enable(1, NULL);
return;
+ }
/*
If current process has pending exceptions it is
illegal to restore the FPEXC register with them, we must
@@ -166,11 +169,13 @@ static inline void xnarch_enable_fpu(xnarchtcb_t *tcb)
systems for still unknown reasons.
*/
rthal_save_fpu(tcb->fpup, fpexc);
+ fp_trace_xeno_enable(2, &tcb->fpup->vfpstate);
cpu = rthal_processor_id();
vfp_current_hw_state[cpu] = NULL;
rthal_disable_fpu();
- }
+ } else
+ fp_trace_xeno_enable(3, NULL);
#else /* !CONFIG_VFP */
if (!tcb->user_task)
rthal_enable_fpu();
@@ -196,8 +201,11 @@ static inline void xnarch_save_fpu(xnarchtcb_t * tcb)
{
#ifdef CONFIG_XENO_HW_FPU
#ifdef CONFIG_VFP
- if (tcb->fpup)
+ if (tcb->fpup) {
rthal_save_fpu(tcb->fpup, rthal_enable_fpu());
+ fp_trace_xeno_save(0, &tcb->fpup->vfpstate);
+ } else
+ fp_trace_xeno_save(1, NULL);
#else /* !CONFIG_VFP */
if (tcb->fpup) {
rthal_save_fpu(tcb->fpup);
@@ -218,6 +226,7 @@ static inline void xnarch_restore_fpu(xnarchtcb_t * tcb)
if (likely(!tcb->is_root)) {
rthal_enable_fpu();
rthal_restore_fpu(tcb->fpup);
+ fp_trace_xeno_restore(0, &tcb->fpup->vfpstate);
} else {
/* We are restoring the Linux current thread which does not own the FPU
context, so the FPU must be disabled, so that a fault will occur if
@@ -232,6 +241,7 @@ static inline void xnarch_restore_fpu(xnarchtcb_t * tcb)
unsigned cpu = rthal_processor_id();
vfp_current_hw_state[cpu] = NULL;
rthal_disable_fpu();
+ fp_trace_xeno_restore(1, NULL);
}
#else /* !CONFIG_VFP */
if (tcb->fpup) {
diff --git a/include/asm-arm/fp_dump.h b/include/asm-arm/fp_dump.h
new file mode 100644
index 0000000..53c59e4
--- /dev/null
+++ b/include/asm-arm/fp_dump.h
@@ -0,0 +1,89 @@
+#ifndef FP_DUMP_ARM_H
+#define FP_DUMP_ARM_H
+
+#include <asm/fpstate.h>
+#include <asm/xenomai/hal.h>
+#include <asm-generic/xenomai/fp_dump.h>
+
+#define DECLARE_FPENV(var) \
+ fpenv_t var ## _buf; \
+ fpenv_t *var = &var ## _buf
+
+typedef struct fpreg {
+ unsigned long fracl;
+ unsigned long frach: 20;
+ unsigned short exp: 11; /* signed, but not using 2's complement. */
+ unsigned short sign: 1;
+} fpreg_t;
+
+typedef struct vfp_hard_struct fpenv_t;
+
+#define fp_enabled() (rthal_vfp_fmrx(FPEXC) & FPEXC_EN)
+
+static inline void fpenv_save(fpenv_t *fpuenv)
+{
+ rthal_vfp_save((union vfp_state *)fpuenv, rthal_vfp_fmrx(FPEXC));
+}
+
+static inline void fpenv_dump(void *cookie, fpenv_t *fpenv)
+{
+ unsigned i;
+
+ fp_print(cookie, "FPU state:\n");
+ fp_print(cookie, "fpexc: 0x%08x\n", fpenv->fpexc);
+ for(i = 0; i < ARRAY_SIZE(fpenv->fpregs); i++) {
+ fpreg_t *reg = (fpreg_t *)&fpenv->fpregs[i];
+ fp_conv_t fp = {
+ .significand = ((1ULL << 63)
+ + (((unsigned long long)reg->frach) << 43)
+ + (((unsigned long long)reg->fracl) << 11)),
+ .exp = (int)(reg->exp - 1023),
+ .pow10 = 0,
+ };
+
+ fp_print(cookie, "D%2u: %Lu, ", i, fpenv->fpregs[i]);
+
+ switch(reg->exp) {
+ case 0:
+ /* zero or denormalized. */
+ if(!reg->frach && !reg->fracl) {
+ fp_print(cookie, "0");
+ break;
+ }
+
+ fp_print(cookie, "(DEN) ");
+ fp.significand <<= 1;
+ fp.exp--;
+ /* Fall through normalized. */
+
+ default:
+ /* normalized. */
+ fp_disp(cookie, &fp, reg->sign);
+ break;
+
+ case 2047:
+ /* infinities or nans. */
+ if(!reg->frach && !reg->fracl)
+ fp_print(cookie, "%c inf.",
+ reg->sign ? '-' : '+');
+ else
+ fp_print(cookie, "%cnan.",
+ (reg->frach & (1UL << 31) ? 'Q' : 'S'
));
+ break;
+ }
+ fp_print(cookie, " %c", i % 2 ? '\n' : ' ');
+ }
+}
+
+static inline int fpenv_cmp(fpenv_t *env1p, fpenv_t *env2p)
+{
+ unsigned i;
+
+ /* Only compare the values of the 32 registers; do not take
status/control
+ registers value into account. */
+ for(i = 0; i < ARRAY_SIZE(env1p->fpregs); i++)
+ if(memcmp(&env1p->fpregs[i], &env2p->fpregs[i],
sizeof(fpreg_t)))
+ return 1;
+ return 0;
+}
+#endif /* FP_DUMP_ARM_H */
diff --git a/include/asm-generic/fp_dump.h b/include/asm-generic/fp_dump.h
new file mode 100644
index 0000000..6826b47
--- /dev/null
+++ b/include/asm-generic/fp_dump.h
@@ -0,0 +1,152 @@
+#ifndef FP_DUMP_INNER_H
+#define FP_DUMP_INNER_H
+
+#include <linux/kernel.h> /* For printk */
+#include <linux/bitops.h> /* For fls */
+#include <asm/div64.h> /* For do_div */
+
+#ifndef fp_print
+#define fp_print(cookie, fmt, ...) printk(fmt , ##args)
+#endif /* fp_print */
+
+/* Float in course of conversion from base 2 to base 10.
+ f == significand * 2 ^ (exp - 63) * 10 ^ pow10. */
+typedef struct fp_conv {
+ unsigned long long significand;
+ int pow10;
+ int exp;
+} fp_conv_t;
+
+static inline unsigned __attribute__((const)) fp_sq(unsigned x)
+{
+ return x * x;
+}
+
+static inline unsigned __attribute__((const)) fp_pow3(unsigned x)
+{
+ return x * fp_sq(x);
+}
+
+static inline unsigned __attribute__((const)) fp_pow6(unsigned x)
+{
+ return fp_sq(fp_pow3(x));
+}
+
+static inline unsigned __attribute__((const)) fp_pow9(unsigned x)
+{
+ return fp_pow3(fp_pow3(x));
+}
+
+#define fp_flnzul(word) (fls(word) - 1)
+
+static inline unsigned long fp_flnzull(unsigned long long ull)
+{
+ unsigned long h = (ull >> 32);
+ if(h)
+ return 32 + fp_flnzul(h);
+ return fp_flnzul((unsigned long) ull);
+}
+
+static inline unsigned long upow(unsigned long x, unsigned long n)
+{
+ switch(n) {
+ case 1: return x;
+ case 2: return fp_sq(x);
+ case 3: return fp_pow3(x);
+ case 6: return fp_pow6(x);
+ case 9: return fp_pow9(x);
+ }
+
+ return -1;
+}
+
+/* Divide significand by a power of two and multiply by a power of 10. */
+static inline void shiftmul(fp_conv_t *fp, unsigned shift, unsigned pow10)
+{
+ /* Avoid overflow. */
+ if((fp->significand & (1ULL << 63))) {
+ fp->significand >>= 1;
+ fp->exp++;
+ }
+
+ fp->significand >>= shift - pow10 -1;
+ fp->significand *= upow(5, pow10);
+ fp->exp += shift - 1;
+ fp->pow10 -= pow10;
+}
+
+/* Divide significand by a power of ten and multiply by a power of 2. */
+static inline void divshift(fp_conv_t *fp, unsigned pow10, unsigned shift)
+{
+ do_div(fp->significand, upow(5, pow10));
+ fp->significand <<= shift - pow10 -1;
+ fp->exp -= shift - 1;
+ fp->pow10 += pow10;
+
+ /* Maintain accuracy. */
+ if(!(fp->significand & (1ULL << 63))) {
+ fp->significand <<= 1;
+ fp->exp--;
+ }
+}
+
+static inline void fp_disp(void *cookie, fp_conv_t *fp, int neg)
+{
+ /* Normalize. */
+ if(fp->significand && !(fp->significand & (1ULL << 63))) {
+ unsigned shift = 63 - fp_flnzull(fp->significand);
+ fp->significand <<= shift;
+ fp->exp -= shift;
+ }
+
+ /* Convert power of 2 exponent to power of 10,
+ bring exponent between 0 and 63 for integers,
+ between 0 and 29 for non integers. */
+ while(fp->exp <= 0)
+ shiftmul(fp, 30, 9);
+ while(fp->exp > 63 || (fp->pow10 && fp->exp >= 29))
+ divshift(fp, 9, 30);
+
+ /* For non-integer, bring integer part between 0 and 10. */
+ if(fp->pow10) {
+ if(fp->exp >= 19)
+ divshift(fp, 6, 20);
+ if(fp->exp >= 9)
+ divshift(fp, 3, 10);
+ if(fp->exp >= 6)
+ divshift(fp, 2, 7);
+ if(fp->exp > 3)
+ divshift(fp, 1, 4);
+ }
+
+ {
+ unsigned long long tmp, i;
+ unsigned long f, rem;
+ /* Integer part. */
+ i = fp->significand >> (63 - fp->exp);
+
+ /* Fractionary part, left-aligned. */
+ tmp = (fp->significand << (fp->exp + 1));
+
+ /* Convert to base 10, to 9 places. */
+ tmp >>= 21;
+ tmp *= fp_pow9(5);
+
+ /* Round to 32 bits. */
+ f = tmp >> 34;
+ rem = (tmp >> 2);
+ if(rem > (1U << 31) ||
+ (rem == (1U << 31) && (f % 2)) /* Round to even. */)
+ ++f;
+
+
+ /* Display. */
+ fp_print(cookie, "%c%llu", neg ? '-' : ' ', i);
+ if (f)
+ fp_print(cookie, ".%09lu", f);
+ if(fp->pow10)
+ fp_print(cookie, "E%+d", fp->pow10);
+ }
+}
+
+#endif /* FP_DUMP_INNER_H */
diff --git a/include/asm-x86/bits/pod.h b/include/asm-x86/bits/pod.h
index 90e36f0..d5e8ebe 100644
--- a/include/asm-x86/bits/pod.h
+++ b/include/asm-x86/bits/pod.h
@@ -271,28 +271,41 @@ static inline void __save_i387(x86_fpustate *fpup)
static inline void xnarch_save_fpu(xnarchtcb_t *tcb)
{
struct task_struct *task = tcb->user_task;
+ unsigned branch;
if (!tcb->is_root) {
if (task) {
/* fpu not used or already saved by __switch_to. */
- if (wrap_test_fpu_used(task) == 0)
+ if (wrap_test_fpu_used(task) == 0) {
+ fp_trace_xeno_save(0, NULL);
return;
+ }
/* Tell Linux that we already saved the state
* of the FPU hardware of this task. */
wrap_clear_fpu_used(task);
- }
+ branch = 1;
+ } else
+ branch = 2;
} else {
- if (tcb->cr0_ts ||
- (tcb->ts_usedfpu && wrap_test_fpu_used(task) == 0))
+ if (tcb->cr0_ts) {
+ fp_trace_xeno_save(3, NULL);
+ return;
+ }
+
+ if(tcb->ts_usedfpu && wrap_test_fpu_used(task) == 0) {
+ fp_trace_xeno_save(4, NULL);
return;
+ }
wrap_clear_fpu_used(task);
+ branch = 5;
}
clts();
__save_i387(tcb->fpup);
+ fp_trace_xeno_save(branch, &tcb->fpup->fxsave);
}
static inline void __restore_i387(x86_fpustate *fpup)
@@ -332,10 +345,12 @@ static inline void __restore_i387(x86_fpustate *fpup)
static inline void xnarch_restore_fpu(xnarchtcb_t * tcb)
{
struct task_struct *task = tcb->user_task;
+ unsigned branch;
if (!tcb->is_root) {
if (task) {
if (!xnarch_fpu_init_p(task)) {
+ fp_trace_xeno_restore(0, NULL);
stts();
return; /* Uninit fpu area -- do not restore. */
}
@@ -343,10 +358,13 @@ static inline void xnarch_restore_fpu(xnarchtcb_t * tcb)
/* Tell Linux that this task has altered the state of
* the FPU hardware. */
wrap_set_fpu_used(task);
- }
+ branch = 1;
+ } else
+ branch = 2;
} else {
/* Restore state of FPU only if TS bit in cr0 was clear. */
if (tcb->cr0_ts) {
+ fp_trace_xeno_restore(3, NULL);
wrap_clear_fpu_used(task);
stts();
return;
@@ -358,25 +376,35 @@ static inline void xnarch_restore_fpu(xnarchtcb_t * tcb)
it since we are switching to root, where fpu can be
in lazy state. */
stts();
+ fp_trace_xeno_restore(4, NULL);
return;
}
+ branch = 5;
}
/* Restore the FPU hardware with valid fp registers from a
user-space or kernel thread. */
clts();
+<<<<<<< HEAD:include/asm-x86/bits/pod.h
__restore_i387(tcb->fpup);
+=======
+ fp_trace_xeno_restore(branch, &tcb->fpup->fxsave);
+ __restore_i387_checking(&tcb->fpup->fxsave);
+>>>>>>> fpu tracer:include/asm-x86/bits/pod_64.h
}
static inline void xnarch_enable_fpu(xnarchtcb_t *tcb)
{
struct task_struct *task = tcb->user_task;
+ unsigned branch;
if (!tcb->is_root) {
if (task) {
- if (!xnarch_fpu_init_p(task))
+ if (!xnarch_fpu_init_p(task)) {
+ fp_trace_xeno_enable(0, NULL);
return;
+<<<<<<< HEAD:include/asm-x86/bits/pod.h
/*
* We used to test here if __switch_to had not
* saved current fpu state, but this can not
@@ -391,8 +419,31 @@ static inline void xnarch_enable_fpu(xnarchtcb_t *tcb)
return;
/* The comment in the non-root case applies here too. */
+=======
+ }
+ branch = 1;
+
+ /* We used to test here if __switch_to had not saved
+ current fpu state, but this can not happen, since
+ xnarch_enable_fpu may only be called when switching
+ back to a user-space task after one or several
+ switches to non-fpu kernel-space real-time tasks, so
+ xnarch_switch_to never uses __switch_to. */
+ } else
+ branch = 2;
+ } else {
+ if (tcb->cr0_ts) {
+ fp_trace_xeno_enable(3, NULL);
+ return;
+ }
+
+ /* The comment in the non-root case applies here too. */
+ branch = 4;
+ }
+>>>>>>> fpu tracer:include/asm-x86/bits/pod_64.h
clts();
+ fp_trace_xeno_enable(branch, NULL);
}
#else /* !CONFIG_XENO_HW_FPU */
diff --git a/include/asm-x86/fp_dump.h b/include/asm-x86/fp_dump.h
new file mode 100644
index 0000000..bd02993
--- /dev/null
+++ b/include/asm-x86/fp_dump.h
@@ -0,0 +1,96 @@
+#ifndef FP_DUMP_H
+#define FP_DUMP_H
+
+#include <asm/processor.h>
+#include <asm/fpu-internal.h>
+#include <asm/xenomai/hal.h>
+#include <asm-generic/xenomai/fp_dump.h>
+
+#define DECLARE_FPENV(var) \
+ fpenv_t var ## _buf; \
+ fpenv_t *var = (fpenv_t *)(((unsigned long)(&var ## _buf) + 15) & ~15)
+
+typedef struct fpreg {
+ unsigned long long significand;
+ unsigned short exp : 15;
+ unsigned short sign : 1;
+} fpreg_t;
+
+typedef struct {
+ struct i387_fxsave_struct u;
+ char align[15];
+} fpenv_t;
+
+#define fp_enabled() ((read_cr0() & X86_CR0_TS) == 0)
+
+static inline void fpenv_save(fpenv_t *fpuenv)
+{
+#ifdef __i386__
+ asm volatile ("fnop; fwait; fxsave %0; fwait" : "=m"(fpuenv->u));
+#else
+ asm volatile ("fnop; fwait; fxsaveq %0; fwait" : "=m"(fpuenv->u));
+#endif
+}
+
+static inline void fpenv_dump(void *cookie, fpenv_t *fpuenv)
+{
+ struct i387_fxsave_struct *fpenv = &fpuenv->u;
+ unsigned top, i;
+ char *stack;
+
+ top = (fpenv->swd >> 11) & 0x7;
+
+ fp_print(cookie, "FPU state:\n");
+ fp_print(cookie, "cwd: 0x%04x, swd: 0x%04x\n", fpenv->cwd, fpenv->swd);
+ fp_print(cookie, "FPU registers stack, top at %u\n", top);
+
+ stack = (char *)&fpenv->st_space[0];
+
+ for (i = 7; i <= 7; i--) {
+ fpreg_t *reg = (fpreg_t *)(stack + 16 * ((i - top) % 8));
+ unsigned tag = !!(fpenv->twd & (1 << i));
+ fp_conv_t fp = {
+ .significand = reg->significand,
+ .exp = (int)(reg->exp - 0x3FFF),
+ .pow10 = 0,
+ };
+
+ fp_print(cookie, "ST%u/MM%u: ", i, i);
+ switch (tag) {
+ case 0: /* Empty */
+ fp_print(cookie, "Empty");
+ break;
+ case 1: /* Not empty */
+ switch (reg->exp) {
+ unsigned s;
+ case 0x7fff: /* Special */
+ if (reg->significand == 0) {
+ s = reg->sign;
+ fp_print(cookie, "%c inf.", s ? '-' :
'+');
+ } else {
+ s = !!(reg->significand & (1ULL << 63));
+ fp_print(cookie, "%cnan.", s ? 'Q' :
'S');
+ }
+ break;
+
+ case 0:
+ if (reg->significand == 0) {
+ fp_print(cookie, "0");
+ break;
+ }
+
+ /* Fall through wanted */
+ default:
+ s = !!(reg->significand & (1ULL << 63));
+ if (s == 0)
+ fp_print(cookie, "(DEN) ");
+ fp_disp(cookie, &fp, reg->sign);
+ break;
+ }
+ }
+ fp_print(cookie, " %c", i % 2 ? ' ' : '\n');
+ }
+}
+
+
+#endif /* FP_DUMP_H */
diff --git a/include/nucleus/fpu_trace.h b/include/nucleus/fpu_trace.h
new file mode 100644
index 0000000..712dc70
--- /dev/null
+++ b/include/nucleus/fpu_trace.h
@@ -0,0 +1,47 @@
+#ifndef FPU_TRACE_H
+#define FPU_TRACE_H
+
+#ifdef CONFIG_FPU_TRACE
+
+struct task_struct;
+struct xnthread;
+
+void fp_trace_switch(struct task_struct *from, struct task_struct *to);
+
+void fp_trace_save(void *to);
+
+void fp_trace_restore(void *from);
+
+void fp_trace_xeno_switch(struct xnthread *from, struct xnthread *to);
+
+void fp_trace_xeno_save(unsigned branch, void *to);
+
+void fp_trace_xeno_restore(unsigned branch, void *from);
+
+void fp_trace_xeno_enable(unsigned branch, void *from);
+
+void fp_trace_printf(const char *fmt, ...);
+
+void fp_trace_freeze(void);
+
+#else /* !CONFIG_FPU_TRACE */
+
+#define fp_trace_switch(f, t) do { } while (0)
+
+#define fp_trace_save(t) do { } while (0)
+
+#define fp_trace_restore(f) do { } while (0)
+
+#define fp_trace_xeno_switch(f, t) do { } while (0)
+
+#define fp_trace_xeno_save(b, t) do { } while (0)
+
+#define fp_trace_xeno_restore(b, t) do { } while (0)
+
+#define fp_trace_xeno_enable(b, f) do { } while (0)
+
+#define fp_trace_freeze() do { } while (0)
+
+#endif /* !CONFIG_FPU_TRACE */
+
+#endif /* FPU_TRACE_H */
diff --git a/ksrc/nucleus/Kconfig b/ksrc/nucleus/Kconfig
index 2f2f6f6..5d6775e 100644
--- a/ksrc/nucleus/Kconfig
+++ b/ksrc/nucleus/Kconfig
@@ -357,6 +357,13 @@ config XENO_OPT_WATCHDOG_TIMEOUT
Watchdog timeout value (in seconds).
+config FPU_TRACE
+ bool "FPU tracing"
+ depends on XENO_OPT_DEBUG
+ help
+
+ Enable FPU tracer
+
config XENO_OPT_SHIRQ
bool "Shared interrupts"
help
diff --git a/ksrc/nucleus/Makefile b/ksrc/nucleus/Makefile
index 70ee9b8..bd71109 100644
--- a/ksrc/nucleus/Makefile
+++ b/ksrc/nucleus/Makefile
@@ -18,6 +18,8 @@ xeno_nucleus-$(CONFIG_XENO_OPT_MAP) += map.o
xeno_nucleus-$(CONFIG_XENO_OPT_SELECT) += select.o
xeno_nucleus-$(CONFIG_PROC_FS) += vfile.o
+xeno_nucleus-$(CONFIG_FPU_TRACE) += fpu_trace.o
+
# CAUTION: this module shall appear last, so that dependencies may
# exist on initcalls defined by other object files.
xeno_nucleus-y += module.o
diff --git a/ksrc/nucleus/fpu_trace.c b/ksrc/nucleus/fpu_trace.c
new file mode 100644
index 0000000..ca97814
--- /dev/null
+++ b/ksrc/nucleus/fpu_trace.c
@@ -0,0 +1,542 @@
+#include <linux/module.h>
+#include <linux/seq_file.h>
+#include <linux/proc_fs.h>
+#include <linux/percpu.h>
+#include <nucleus/thread.h>
+#include <nucleus/fpu_trace.h>
+
+#define fp_print(cookie, format, args...) \
+ seq_printf((struct seq_file *)(cookie), format , ##args)
+
+#include <asm/xenomai/fp_dump.h>
+
+#ifndef SZ_128M
+#define SZ_128M 0x08000000
+#endif /* SZ_128M */
+#define STORAGE_SIZE SZ_128M
+
+#define FP_TRACE_LTHR 0
+#define FP_TRACE_LFPS 1
+#define FP_TRACE_LFPR 2
+#define FP_TRACE_XTHR 3
+#define FP_TRACE_XFPS 4
+#define FP_TRACE_XFPR 5
+#define FP_TRACE_XFPEN 6
+#define FP_TRACE_STOP 7
+#define FP_TRACE_TEXT 8
+
+struct fp_trace_pt {
+ unsigned short type;
+ unsigned short branch;
+};
+
+struct fp_trace_switch {
+ char from[64];
+ char to[64];
+};
+
+struct fp_trace_text {
+ char msg[128];
+};
+
+struct tracer {
+ char *storage;
+ unsigned begin;
+ unsigned end;
+ unsigned points;
+ unsigned starting;
+ unsigned frozen;
+ unsigned bug;
+};
+
+struct tracer_it {
+ struct tracer *tracer;
+ unsigned pos;
+};
+
+static unsigned size_mask, ready, run = 0;
+static DEFINE_PER_CPU(struct tracer, percpu_tracer);
+
+static void fp_trace_push(unsigned type,
+ unsigned branch, void *buf, unsigned len);
+
+static unsigned
+fp_trace_print_task(char *buf, size_t size,
+ xnthread_t *thread, struct task_struct *user);
+
+void fp_trace_switch(struct task_struct *from, struct task_struct *to)
+{
+ struct fp_trace_switch buf;
+
+ if (!run)
+ return;
+ fp_trace_print_task(&buf.from[0], sizeof(buf.from), NULL, from);
+ fp_trace_print_task(&buf.to[0], sizeof(buf.to), NULL, to);
+ fp_trace_push(FP_TRACE_LTHR, 0, &buf, sizeof(buf));
+}
+
+void fp_trace_save(void *to)
+{
+ fpenv_t *ctxt = (fpenv_t *)to;
+ if (!run)
+ return;
+ fp_trace_push(FP_TRACE_LFPS, 0, ctxt, sizeof(*ctxt));
+}
+
+void fp_trace_restore(void *from)
+{
+ fpenv_t *ctxt = (fpenv_t *)from;
+ if (!run)
+ return;
+ fp_trace_push(FP_TRACE_LFPR, 0, ctxt, sizeof(*ctxt));
+}
+
+void fp_trace_xeno_switch(xnthread_t *from, xnthread_t *to)
+{
+ struct fp_trace_switch buf;
+
+ if (!run)
+ return;
+
+ fp_trace_print_task(&buf.from[0],
+ sizeof(buf.from), from, xnthread_user_task(from));
+ fp_trace_print_task(&buf.to[0],
+ sizeof(buf.to), to, xnthread_user_task(to));
+ fp_trace_push(FP_TRACE_XTHR, 0, &buf, sizeof(buf));
+}
+
+void fp_trace_xeno_save(unsigned branch, void *to)
+{
+ DECLARE_FPENV(ctxt);
+
+ if (!run)
+ return;
+
+ if (to == NULL && fp_enabled())
+ fpenv_save(ctxt);
+ else
+ ctxt = (fpenv_t *)to;
+
+ if (ctxt)
+ fp_trace_push(FP_TRACE_XFPS,
+ branch | (1 << 15), ctxt, sizeof(*ctxt));
+ else
+ fp_trace_push(FP_TRACE_XFPS, branch, NULL, 0);
+}
+
+void fp_trace_xeno_restore(unsigned branch, void *from)
+{
+ DECLARE_FPENV(ctxt);
+
+ if (!run)
+ return;
+
+ if (from == NULL && fp_enabled())
+ fpenv_save(ctxt);
+ else
+ ctxt = (fpenv_t *)from;
+
+ if (ctxt)
+ fp_trace_push(FP_TRACE_XFPR,
+ branch | (1 << 15), ctxt, sizeof(*ctxt));
+ else
+ fp_trace_push(FP_TRACE_XFPR, branch, NULL, 0);
+}
+
+void fp_trace_xeno_enable(unsigned branch, void *from)
+{
+ DECLARE_FPENV(ctxt);
+
+ if (!run)
+ return;
+
+ if (from == NULL && fp_enabled())
+ fpenv_save(ctxt);
+ else
+ ctxt = (fpenv_t *)from;
+
+ if (ctxt)
+ fp_trace_push(FP_TRACE_XFPEN,
+ branch | (1 << 15), ctxt, sizeof(*ctxt));
+ else
+ fp_trace_push(FP_TRACE_XFPEN, branch, NULL, 0);
+}
+
+void fp_trace_freeze(void)
+{
+ struct tracer *t;
+
+ preempt_disable();
+ t = &get_cpu_var(percpu_tracer);
+
+ if (fp_enabled()) {
+ DECLARE_FPENV(ctxt);
+ fpenv_save(ctxt);
+ fp_trace_push(FP_TRACE_STOP, 1 << 15, ctxt, sizeof(*ctxt));
+ } else
+ fp_trace_push(FP_TRACE_STOP, 0, NULL, 0);
+ t->frozen = 1;
+ preempt_enable();
+}
+
+void fp_trace_printf(const char *fmt, ...)
+{
+ struct fp_trace_text txt;
+ va_list ap;
+
+ va_start(ap, fmt);
+ vsnprintf(txt.msg, sizeof(txt.msg), fmt, ap);
+ va_end(ap);
+
+ fp_trace_push(FP_TRACE_TEXT, 0, &txt, sizeof(txt));
+}
+
+static unsigned fp_trace_next_entry(struct tracer *t, unsigned cur)
+{
+ struct fp_trace_pt *pt = (struct fp_trace_pt *)(t->storage + cur);
+ if (cur % 4) {
+ t->bug = 1;
+ show_stack(NULL, NULL);
+ BUG();
+ }
+ cur += sizeof(*pt);
+ switch(pt->type) {
+ case FP_TRACE_LTHR:
+ case FP_TRACE_XTHR:
+ cur += sizeof(struct fp_trace_switch);
+ break;
+ case FP_TRACE_LFPS:
+ case FP_TRACE_LFPR:
+ cur += sizeof(fpenv_t);
+ break;
+ case FP_TRACE_XFPS:
+ case FP_TRACE_XFPR:
+ case FP_TRACE_XFPEN:
+ if (pt->branch & (1 << 15))
+ cur += sizeof(fpenv_t);
+ break;
+ case FP_TRACE_STOP:
+ if (pt->branch & (1 << 15))
+ cur += sizeof(fpenv_t);
+ break;
+ case FP_TRACE_TEXT:
+ cur += sizeof(struct fp_trace_text);
+ break;
+ default:
+ printk("next_entry(0x%08x/0x%08x): invalid type %d\n",
+ cur, size_mask, pt->type);
+ t->bug = 1;
+ BUG();
+ }
+ if (cur % 4) {
+ t->bug = 1;
+ show_stack(NULL, NULL);
+ BUG();
+ }
+ return cur & size_mask;
+}
+
+static void fp_trace_push(unsigned type,
+ unsigned branch, void *buf, unsigned len)
+{
+ struct fp_trace_pt *pt;
+ struct tracer *t;
+ unsigned long flags;
+
+ local_irq_save_hw(flags);
+ t = &get_cpu_var(percpu_tracer);
+ if (t->frozen || ready == 0 || t->bug)
+ goto unlock;
+
+ pt = (struct fp_trace_pt *)(t->storage + t->end);
+ if (unlikely(t->starting))
+ t->starting = 0;
+ else
+ while (((t->begin - t->end) & size_mask) < sizeof(*t) + len) {
+ t->begin = fp_trace_next_entry(t, t->begin);
+ --t->points;
+ }
+
+ BUG_ON(type > FP_TRACE_TEXT);
+ pt->type = type;
+ pt->branch = branch;
+ t->end = (t->end + sizeof(*pt)) & size_mask;
+ if (!len)
+ goto inc_points;
+ if (t->end + len <= size_mask + 1)
+ memcpy(t->storage + t->end, buf, len);
+ else {
+ unsigned cut = size_mask + 1 - t->end;
+ memcpy(t->storage + t->end, buf, cut);
+ memcpy(t->storage, buf + cut, len - cut);
+ }
+ t->end = (t->end + len) & size_mask;
+#if 0
+ if (fp_trace_next_entry(t, (char *)pt - t->storage) != t->end) {
+ printk("Trace, type: %d, entry: %x, len: %x, next: %x,"
+ " end: %x\n", pt->type, (char *)pt - t->storage, len,
+ fp_trace_next_entry(t, (char *)pt - t->storage), t->end);
+ t->bug = 1;
+ BUG();
+ }
+#endif
+ inc_points:
+ ++t->points;
+ unlock:
+ local_irq_restore_hw(flags);
+}
+
+static unsigned
+fp_trace_print_task(char *buf, size_t size,
+ xnthread_t *thread, struct task_struct *user)
+{
+ if (!user)
+ return snprintf(buf, size,
+ "RTK: %s(%p)", xnthread_name(thread), thread);
+
+ if (thread && !xnthread_test_state(thread, XNROOT))
+ return snprintf(buf, size,
+ "RTUP: %s[%d]", user->comm, user->pid);
+
+ thread = xnshadow_thread(user);
+ if (thread)
+ return snprintf(buf, size,
+ "RTUS: %s[%d]", user->comm, user->pid);
+
+ return snprintf(buf, size, "NRT: %s[%d]", user->comm, user->pid);
+}
+
+static void *fp_trace_seq_start(struct seq_file *seq, loff_t *pos)
+{
+ struct tracer_it *it = seq->private;
+ struct tracer *t = it->tracer;
+ unsigned i;
+
+ if (!t->frozen || *pos >= t->points)
+ return NULL;
+
+ for (i = 0, it->pos = t->begin; i < *pos; i++)
+ it->pos = fp_trace_next_entry(t, it->pos);
+
+ return t->storage + it->pos;
+}
+
+static void *fp_trace_seq_next(struct seq_file *seq, void *v, loff_t *pos)
+{
+ struct tracer_it *it = seq->private;
+ struct tracer *t = it->tracer;
+
+ if(++*pos >= t->points)
+ return NULL;
+
+ it->pos = fp_trace_next_entry(t, it->pos);
+
+ return t->storage + it->pos;
+}
+
+static void fp_trace_seq_stop(struct seq_file *seq, void *v)
+{
+}
+
+static void cpy_buf(struct tracer *t, void *dest, unsigned len, void *src)
+{
+ unsigned cur = ((char *)src - t->storage) & size_mask;
+
+ if (cur + len <= size_mask + 1)
+ memcpy(dest, src, len);
+ else {
+ unsigned cut = size_mask - cur;
+ memcpy(dest, src, cut);
+ memcpy(dest + cut, t->storage, len - cut);
+ }
+}
+
+static int fp_trace_seq_show(struct seq_file *seq, void *v)
+{
+ struct tracer_it *it = seq->private;
+ struct tracer *t = it->tracer;
+ struct fp_trace_pt *pt = v;
+ struct fp_trace_switch swtch;
+ struct fp_trace_text text;
+ fpenv_t ctxt;
+
+ switch(pt->type) {
+ case FP_TRACE_LTHR:
+ cpy_buf(t, &swtch, sizeof(swtch), pt + 1);
+ seq_printf(seq, "Linux switch from %s to %s\n",
+ swtch.from, swtch.to);
+ break;
+
+ case FP_TRACE_LFPS:
+ seq_printf(seq, "Linux saving fpu\n");
+ cpy_buf(t, &ctxt, sizeof(ctxt), pt + 1);
+ fpenv_dump(seq, &ctxt);
+ break;
+
+ case FP_TRACE_LFPR:
+ seq_printf(seq, "Linux restoring fpu\n");
+ cpy_buf(t, &ctxt, sizeof(ctxt), pt + 1);
+ fpenv_dump(seq, &ctxt);
+ break;
+
+ case FP_TRACE_XTHR:
+ cpy_buf(t, &swtch, sizeof(swtch), pt + 1);
+ seq_printf(seq, "Xeno switch from %s to %s\n",
+ swtch.from, swtch.to);
+ break;
+
+ case FP_TRACE_XFPS:
+ seq_printf(seq, "Xeno saving fpu (branch %d)\n",
+ pt->branch & ~(1 << 15));
+ if (pt->branch & (1 << 15)) {
+ cpy_buf(t, &ctxt, sizeof(ctxt), pt + 1);
+ fpenv_dump(seq, &ctxt);
+ }
+ break;
+
+ case FP_TRACE_XFPR:
+ seq_printf(seq, "Xeno restoring fpu (branch %d)\n",
+ pt->branch & ~(1 << 15));
+ if (pt->branch & (1 << 15)) {
+ cpy_buf(t, &ctxt, sizeof(ctxt), pt + 1);
+ fpenv_dump(seq, &ctxt);
+ }
+ break;
+
+ case FP_TRACE_XFPEN:
+ seq_printf(seq, "Xeno enabling fpu (branch %d)\n",
+ pt->branch & ~(1 << 15));
+ if (pt->branch & (1 << 15)) {
+ cpy_buf(t, &ctxt, sizeof(ctxt), pt + 1);
+ fpenv_dump(seq, &ctxt);
+ }
+ break;
+
+ case FP_TRACE_STOP:
+ seq_printf(seq, "Capture stopped\n");
+ if (pt->branch & (1 << 15)) {
+ cpy_buf(t, &ctxt, sizeof(ctxt), pt + 1);
+ fpenv_dump(seq, &ctxt);
+ }
+ break;
+
+ case FP_TRACE_TEXT:
+ cpy_buf(t, &text, sizeof(text), pt + 1);
+ seq_printf(seq, "%s\n", text.msg);
+ break;
+
+ default:
+ printk("type: 0x%04x, cur: 0x%016lx\n",
+ pt->type, (char *) pt - t->storage);
+ return -EINVAL;
+ }
+
+ return 0;
+}
+
+static struct seq_operations fp_trace_seq_op = {
+ .start = &fp_trace_seq_start,
+ .next = &fp_trace_seq_next,
+ .stop = &fp_trace_seq_stop,
+ .show = &fp_trace_seq_show
+};
+
+static int fp_trace_seq_open(struct inode *inode, struct file *file)
+{
+ struct seq_file *seq;
+ struct tracer_it *it;
+ struct tracer *t;
+ int rc, i;
+
+ if (!ready)
+ return -ENOENT;
+
+ for (i = 0; i < num_online_cpus(); i++) {
+ t = &per_cpu(percpu_tracer, i);
+ if (t->frozen)
+ goto found;
+ }
+
+ return -ENOENT;
+ found:
+ it = kmalloc(sizeof(*it), GFP_KERNEL);
+ if (!it)
+ return -ENOSPC;
+ it->tracer = t;
+
+ rc = seq_open(file, &fp_trace_seq_op);
+ if (rc < 0) {
+ kfree(it);
+ return rc;
+ }
+
+ seq = file->private_data;
+ seq->private = it;
+
+ return rc;
+}
+
+static int fp_trace_seq_release(struct inode *inode, struct file *file)
+{
+ struct seq_file *seq = file->private_data;
+ struct trace_it *it;
+
+ if (!seq)
+ return 0;
+
+ it = seq->private;
+ if (it)
+ kfree(it);
+
+ return seq_release(inode, file);
+}
+
+static struct file_operations fp_trace_seq_operations = {
+ .owner = THIS_MODULE,
+ .open = fp_trace_seq_open,
+ .read = seq_read,
+ .llseek = seq_lseek,
+ .release = fp_trace_seq_release,
+};
+
+extern struct proc_dir_entry *rthal_proc_root;
+
+int __init fp_trace_init(void)
+{
+ struct proc_dir_entry *entry;
+ unsigned i, d, cpus;
+ char *glob_storage;
+
+ glob_storage = vmalloc(STORAGE_SIZE);
+ if (glob_storage == 0)
+ return -ENOSPC;
+
+ cpus = num_online_cpus();
+ d = 1 << fp_flnzul(cpus);
+ if (cpus != d)
+ d <<= 1;
+ for (i = 0; i < cpus; i++) {
+ struct tracer *t = &per_cpu(percpu_tracer, i);
+
+ t->storage = glob_storage + i * (STORAGE_SIZE / d);
+ t->begin = 0;
+ t->end = 0;
+ t->points = 0;
+ t->starting = 1;
+ t->frozen = 0;
+ t->bug = 0;
+ }
+
+ entry = create_proc_entry("fp_trace_dump", 0, NULL);
+ if (!entry)
+ return -EINVAL;
+
+ entry->proc_fops = &fp_trace_seq_operations;
+ wrap_proc_dir_entry_owner(entry);
+
+ size_mask = STORAGE_SIZE / d - 1;
+ run = ready = 1;
+ return 0;
+}
+
+module_init(fp_trace_init);
diff --git a/ksrc/nucleus/pod.c b/ksrc/nucleus/pod.c
index f977db3..77321cd 100644
--- a/ksrc/nucleus/pod.c
+++ b/ksrc/nucleus/pod.c
@@ -44,6 +44,7 @@
#include <nucleus/stat.h>
#include <nucleus/assert.h>
#include <nucleus/select.h>
+#include <nucleus/fpu_trace.h>
#include <asm/xenomai/bits/pod.h>
/*
@@ -2086,6 +2087,7 @@ EXPORT_SYMBOL_GPL(xnpod_welcome_thread);
static inline void xnpod_switch_to(xnsched_t *sched,
xnthread_t *prev, xnthread_t *next)
{
+ fp_trace_xeno_switch(prev, next);
#ifdef CONFIG_XENO_HW_UNLOCKED_SWITCH
sched->last = prev;
__setbits(sched->status, XNINSW);
@@ -2571,6 +2573,7 @@ int xnpod_trap_fault(xnarch_fltinfo_t *fltinfo)
if (xnarch_fault_fpu_p(fltinfo)) {
if (__xnpod_fault_init_fpu(thread))
return 1;
+ fp_trace_freeze();
print_symbol("invalid use of FPU in Xenomai context at %s\n",
xnarch_fault_pc(fltinfo));
}
diff --git a/ksrc/nucleus/shadow.c b/ksrc/nucleus/shadow.c
index 7c4e032..be0843a 100644
--- a/ksrc/nucleus/shadow.c
+++ b/ksrc/nucleus/shadow.c
@@ -53,6 +53,8 @@
#include <nucleus/stat.h>
#include <nucleus/sys_ppd.h>
#include <nucleus/vdso.h>
+#include <nucleus/fpu_trace.h>
+
#include <asm/xenomai/features.h>
#include <asm/xenomai/syscall.h>
#include <asm/xenomai/bits/shadow.h>
@@ -2041,6 +2043,7 @@ static int xnshadow_sys_trace(struct pt_regs *regs)
break;
case __xntrace_op_user_freeze:
+ fp_trace_freeze();
err = xnarch_trace_user_freeze(__xn_reg_arg2(regs),
__xn_reg_arg3(regs));
break;
_______________________________________________
Xenomai-git mailing list
Xenomai-git@xenomai.org
http://www.xenomai.org/mailman/listinfo/xenomai-git
