debugging stuff.

index 6cf2f6e..5cebcd4 100644
@@ -278,7 +278,7 @@ unsigned long long xnarch_get_cpu_time(void)
 
 EXPORT_SYMBOL(xnarch_get_cpu_time);
 
-#ifdef CONFIG_SMP
+#if defined(CONFIG_SMP) || XENO_DEBUG(NUCLEUS)
 void __xnlock_spin(xnlock_t *lock /*, */ XNLOCK_DBG_CONTEXT_ARGS)
 {
 	unsigned int spin_limit;

index e872711..b59e364 100644
@@ -91,7 +91,7 @@ static inline unsigned xnarch_current_cpu(void)
 	return rthal_processor_id();
 }
 
-#if defined(CONFIG_SMP) && XENO_DEBUG(NUCLEUS)
+#if XENO_DEBUG(NUCLEUS)
 
 typedef struct {
 

@@ -148,11 +148,14 @@ xnlock_dbg_spinning(xnlock_t *lock, int cpu, unsigned int *spin_limit,
 	if (--*spin_limit == 0) {
 		rthal_emergency_console();
 		printk(KERN_ERR "Xenomai: stuck on nucleus lock %p\n"
-				"         waiter = %s:%u (%s(), CPU #%d)\n"
-				"         owner  = %s:%u (%s(), CPU #%d)\n",
+				"	  waiter = %s:%u (%s(), CPU #%d)\n"
+				"	  owner	 = %s:%u (%s(), CPU #%d)\n",
 		       lock, file, line, function, cpu,
 		       lock->file, lock->line, lock->function, lock->cpu);
 		show_stack(NULL, NULL);
+#ifndef CONFIG_SMP
+		BUG();
+#endif
 	}
 }
 

@@ -319,7 +322,7 @@ static inline int xnarch_root_domain_p(void)
 	return rthal_current_domain == rthal_root_domain;
 }
 
-#ifdef CONFIG_SMP
+#if defined(CONFIG_SMP) || XENO_DEBUG(NUCLEUS)
 
 #define xnlock_get(lock)		__xnlock_get(lock  XNLOCK_DBG_CONTEXT)
 #define xnlock_get_irqsave(lock,x) \

@@ -395,7 +398,11 @@ static inline void xnlock_put_irqrestore(xnlock_t *lock, spl_t flags)
 
 static inline int xnarch_send_ipi(xnarch_cpumask_t cpumask)
 {
+#ifdef CONFIG_SMP
 	return rthal_send_ipi(RTHAL_SERVICE_IPI0, cpumask);
+#else /* !CONFIG_SMP */
+	return 0;
+#endif /* !CONFIG_SMP */
 }
 
 static inline int xnlock_is_owner(xnlock_t *lock)

@@ -403,7 +410,7 @@ static inline int xnlock_is_owner(xnlock_t *lock)
 	return atomic_read(&lock->owner) == xnarch_current_cpu();
 }
 
-#else /* !CONFIG_SMP */
+#else /* !(CONFIG_SMP || XENO_DEBUG(NUCLEUS) */
 
 #define xnlock_init(lock)		do { } while(0)
 #define xnlock_get(lock)		do { } while(0)

@@ -424,7 +431,7 @@ static inline int xnarch_send_ipi (xnarch_cpumask_t cpumask)
 	return 0;
 }
 
-#endif /* !CONFIG_SMP */
+#endif /* !(CONFIG_SMP || XENO_DEBUG(NUCLEUS) */
 
 #define xnlock_sync_irq(lock, x)			\
 	do {						\

index b84fa3c..13cde9f 100644
@@ -65,17 +65,17 @@ typedef struct xnqueue {
 
 	xnholder_t head;
 	int elems;
-#if defined(__KERNEL__) && XENO_DEBUG(QUEUES) && defined(CONFIG_SMP)
-	xnlock_t lock;
-#endif				/* __KERNEL__ && XENO_DEBUG(QUEUES) && CONFIG_SMP */
+#if defined(__KERNEL__) && XENO_DEBUG(QUEUES)
+	DECLARE_XNLOCK(lock);
+#endif /* __KERNEL__ && XENO_DEBUG(QUEUES) */
 
 } xnqueue_t;
 
-#if XENO_DEBUG(QUEUES) && defined(CONFIG_SMP)
+#if XENO_DEBUG(QUEUES)
 #define XNQUEUE_INITIALIZER(q) { { &(q).head, &(q).head }, 0, XNARCH_LOCK_UNLOCKED }
-#else /* !(XENO_DEBUG(QUEUES) && CONFIG_SMP) */
+#else /* !(XENO_DEBUG(QUEUES) */
 #define XNQUEUE_INITIALIZER(q) { { &(q).head, &(q).head }, 0 }
-#endif /* XENO_DEBUG(QUEUES) && CONFIG_SMP */
+#endif /* XENO_DEBUG(QUEUES) */
 
 #define DEFINE_XNQUEUE(q) xnqueue_t q = XNQUEUE_INITIALIZER(q)
 

@@ -83,9 +83,9 @@ static inline void initq(xnqueue_t *qslot)
 {
 	inith(&qslot->head);
 	qslot->elems = 0;
-#if defined(__KERNEL__) && XENO_DEBUG(QUEUES) && defined(CONFIG_SMP)
+#if defined(__KERNEL__) && XENO_DEBUG(QUEUES)
 	xnlock_init(&qslot->lock);
-#endif /* __KERNEL__ && XENO_DEBUG(QUEUES) && CONFIG_SMP */
+#endif /* __KERNEL__ && XENO_DEBUG(QUEUES) */
 }
 
 #if XENO_DEBUG(QUEUES)

index 97458f5..cfe3041 100644
@@ -404,7 +404,7 @@ static inline int xnintr_irq_detach(xnintr_t *intr)
 
 #else /* !CONFIG_XENO_OPT_SHIRQ */
 
-#ifdef CONFIG_SMP
+#if defined(CONFIG_SMP) || XENO_DEBUG(NUCLEUS)
 typedef struct xnintr_irq {
 
 	DECLARE_XNLOCK(lock);

@@ -412,7 +412,7 @@ typedef struct xnintr_irq {
 } ____cacheline_aligned_in_smp xnintr_irq_t;
 
 static xnintr_irq_t xnirqs[XNARCH_NR_IRQS];
-#endif /* CONFIG_SMP */
+#endif /* CONFIG_SMP || XENO_DEBUG(NUCLEUS) */
 
 static inline xnintr_t *xnintr_shirq_first(unsigned irq)
 {

index 4144ccd..bc7b241 100644
@@ -58,9 +58,9 @@ xnpod_t nkpod_struct;
 EXPORT_SYMBOL_GPL(nkpod_struct);
 
 DEFINE_XNLOCK(nklock);
-#ifdef CONFIG_SMP
+#if defined(CONFIG_SMP) || XENO_DEBUG(NUCLEUS)
 EXPORT_SYMBOL_GPL(nklock);
-#endif /* CONFIG_SMP */
+#endif /* CONFIG_SMP || XENO_DEBUG(NUCLEUS) */
 
 u_long nklatency = 0;
 EXPORT_SYMBOL_GPL(nklatency);

@@ -3087,7 +3087,7 @@ EXPORT_SYMBOL_GPL(xnpod_set_thread_tslice);
 #include <linux/proc_fs.h>
 #include <linux/ctype.h>
 
-#if defined(CONFIG_SMP) && XENO_DEBUG(NUCLEUS)
+#if defined(CONFIG_SMP) || XENO_DEBUG(NUCLEUS)
 
 xnlockinfo_t xnlock_stats[RTHAL_NR_CPUS];
 

@@ -3134,7 +3134,7 @@ static int lock_read_proc(char *page,
 }
 EXPORT_SYMBOL_GPL(xnlock_stats);
 
-#endif /* CONFIG_SMP && XENO_DEBUG(NUCLEUS) */
+#endif /* CONFIG_SMP || XENO_DEBUG(NUCLEUS) */
 
 static int latency_read_proc(char *page,
 			     char **start,

@@ -3217,17 +3217,17 @@ void xnpod_init_proc(void)
 	rthal_add_proc_leaf("version", &version_read_proc, NULL, NULL,
 			    rthal_proc_root);
 
-#if defined(CONFIG_SMP) && XENO_DEBUG(NUCLEUS)
+#if defined(CONFIG_SMP) || XENO_DEBUG(NUCLEUS)
 	rthal_add_proc_leaf("lock", &lock_read_proc, NULL, NULL,
 			    rthal_proc_root);
-#endif /* CONFIG_SMP && XENO_DEBUG(NUCLEUS) */
+#endif /* CONFIG_SMP || XENO_DEBUG(NUCLEUS) */
 }
 
 void xnpod_cleanup_proc(void)
 {
-#if defined(CONFIG_SMP) && XENO_DEBUG(NUCLEUS)
+#if defined(CONFIG_SMP) || XENO_DEBUG(NUCLEUS)
 	remove_proc_entry("lock", rthal_proc_root);
-#endif /* CONFIG_SMP && XENO_DEBUG(NUCLEUS) */
+#endif /* CONFIG_SMP || XENO_DEBUG(NUCLEUS) */
 	remove_proc_entry("version", rthal_proc_root);
 	remove_proc_entry("latency", rthal_proc_root);
 

index 5d2baf7..193674c 100644
@@ -64,9 +64,7 @@ typedef void siginfo_handler_t(int, siginfo_t *, void *);
 #define SIGRTMAX 64
 static struct sigaction actions[SIGRTMAX];
 static pse51_siginfo_t pse51_infos_pool[PSE51_SIGQUEUE_MAX];
-#ifdef CONFIG_SMP
-static xnlock_t pse51_infos_lock = XNARCH_LOCK_UNLOCKED;
-#endif
+DEFINE_XNLOCK(pse51_infos_lock);
 static xnpqueue_t pse51_infos_free_list;
 
 static pse51_siginfo_t *pse51_new_siginfo(int sig, int code, union sigval value)

index 7267e0d..f43e096 100644
@@ -37,4 +37,6 @@ xeno_bind_skin(unsigned skin_magic, const char *skin,
 	return muxid;
 }
 
+void xeno_fault_stack(void);
+
 #endif /* _XENO_ASM_GENERIC_BITS_BIND_H */

index 44017e8..0717ac1 100644
@@ -7,8 +7,10 @@
 #include <asm/xenomai/syscall.h>
 #include <asm-generic/xenomai/bits/bind.h>
 #include <asm-generic/xenomai/bits/current.h>
+#include <asm-generic/xenomai/stacksize.h>
 #include "sem_heap.h"
 
+static pthread_t xeno_main_tid;
 static xnsighandler *xnsig_handlers[32];
 
 void __attribute__((weak)) xeno_sigill_handler(int sig)

@@ -133,5 +135,17 @@ xeno_bind_skin_opt(unsigned skin_magic, const char *skin,
 
 	xeno_featinfo = finfo;
 
+	xeno_main_tid = pthread_self();
+	
 	return muxid;
 }
+
+void xeno_fault_stack(void)
+{
+	char stk[xeno_stacksize(1)];
+
+	if (pthread_self() != xeno_main_tid)
+		return;
+
+	stk[0] = stk[sizeof(stk) - 1] = 0xA5;
+}

index 341e4d8..e0b3bbd 100644
@@ -187,6 +187,8 @@ int rt_task_shadow(RT_TASK *task, const char *name, int prio, int mode)
 	RT_TASK *self;
 	int err;
 
+	xeno_fault_stack();
+
 #ifdef HAVE___THREAD
 	self = &__native_self;
 #else /* !HAVE___THREAD */

index 9115a58..dcc9373 100644
@@ -43,6 +43,8 @@ int __wrap_pthread_setschedparam(pthread_t thread,
 	int err, promoted;
 
 	if (thread == myself) {
+		xeno_fault_stack();
+
 #ifdef HAVE___THREAD
 		mode_buf = xeno_init_current_mode();
 #else /* !HAVE___THREAD */

index b5f36d2..4325a97 100644
@@ -172,6 +172,8 @@ u_long t_shadow(const char *name, /* Xenomai extension. */
 	struct psos_arg_bulk bulk;
 	int ret;
 
+	xeno_fault_stack();
+
 	pthread_setcanceltype(PTHREAD_CANCEL_ASYNCHRONOUS, NULL);
 	sigshadow_install_once();
 

index 69022c2..ed91a09 100644
@@ -23,11 +23,14 @@
 #include <signal.h>
 #include <errno.h>
 #include <limits.h>
+
 #include <asm/xenomai/system.h>
 #include <asm-generic/bits/sigshadow.h>
+#include <asm-generic/bits/current.h>
+#include <asm-generic/stacksize.h>
+#include <asm-generic/bits/bind.h>
 #include <nucleus/sched.h>
 #include <uitron/uitron.h>
-#include <asm-generic/bits/current.h>
 
 extern int __uitron_muxid;
 

@@ -149,7 +152,9 @@ ER shd_tsk(ID tskid, T_CTSK *pk_ctsk) /* Xenomai extension. */
 {
 	struct sched_param param;
 	int policy, err;
-	
+
+	xeno_fault_stack();
+
 	/* Make sure the POSIX library caches the right priority. */
 	policy = uitron_task_set_posix_priority(pk_ctsk->itskpri, &param);
 	pthread_setschedparam(pthread_self(), policy, &param);

index f82a072..9af785e 100644
@@ -1,13 +1,16 @@
-#include <sys/mman.h>
-#include <sys/time.h>
-#include <unistd.h>
 #include <stdlib.h>
 #include <math.h>
 #include <stdio.h>
 #include <string.h>
 #include <signal.h>
-#include <getopt.h>
 #include <time.h>
+
+#include <sys/mman.h>
+#include <sys/time.h>
+#include <unistd.h>
+
+#include <execinfo.h>
+
 #include <native/task.h>
 #include <native/timer.h>
 #include <native/sem.h>

@@ -20,6 +23,7 @@ RT_SEM display_sem;
 #define ONE_BILLION  1000000000
 #define TEN_MILLION    10000000
 
+unsigned max_relaxed;
 long minjitter, maxjitter, avgjitter;
 long gminjitter = TEN_MILLION, gmaxjitter = -TEN_MILLION, goverrun = 0;
 long long gavgjitter = 0;

@@ -32,6 +36,8 @@ int benchdev_no = 0;
 int benchdev = -1;
 int freeze_max = 0;
 int priority = T_HIPRIO;
+int stop_upon_switch = 0;
+sig_atomic_t sampling_relaxed = 0;
 
 #define USER_TASK       0
 #define KERNEL_TASK     1

@@ -72,6 +78,7 @@ void latency(void *cookie)
 	int err, count, nsamples, warmup = 1;
 	RTIME expected_tsc, period_tsc, start_ticks;
 	RT_TIMER_INFO timer_info;
+	unsigned old_relaxed = 0;
 
 	err = rt_timer_inquire(&timer_info);
 

@@ -109,8 +116,13 @@ void latency(void *cookie)
 			err = rt_task_wait_period(&ov);
 
 			dt = (long)(rt_timer_tsc() - expected_tsc);
-			if (dt > maxj)
+			if (dt > maxj) {
+				if (sampling_relaxed != old_relaxed)
+					max_relaxed += 
+						sampling_relaxed - old_relaxed;
 				maxj = dt;
+			}
+			old_relaxed = sampling_relaxed;
 			if (dt < minj)
 				minj = dt;
 			sumj += dt;

@@ -274,17 +286,17 @@ void display(void *cookie)
 				     (dt / 60) % 60, dt % 60,
 				     test_mode_names[test_mode],
 				     period_ns / 1000, priority);
-				printf("RTH|%12s|%12s|%12s|%8s|%12s|%12s\n",
-				       "-----lat min", "-----lat avg",
-				       "-----lat max", "-overrun",
-				       "----lat best", "---lat worst");
+				printf("RTH|%11s|%11s|%11s|%8s|%6s|%11s|%11s\n",
+				       "----lat min", "----lat avg",
+				       "----lat max", "-overrun", "---msw",
+				       "---lat best", "--lat worst");
 			}
-
-			printf("RTD|%12.3f|%12.3f|%12.3f|%8ld|%12.3f|%12.3f\n",
+			printf("RTD|%11.3f|%11.3f|%11.3f|%8ld|%6u|%11.3f|%11.3f\n",
 			       (double)minj / 1000,
 			       (double)avgj / 1000,
 			       (double)maxj / 1000,
 			       goverrun,
+			       max_relaxed,
 			       (double)gminj / 1000, (double)gmaxj / 1000);
 		}
 	}

@@ -396,12 +408,16 @@ void cleanup(void)
 		test_duration = actual_duration;
 
 	printf
-	    ("---|------------|------------|------------|--------|-------------------------\n"
-	     "RTS|%12.3f|%12.3f|%12.3f|%8ld|    %.2ld:%.2ld:%.2ld/%.2d:%.2d:%.2d\n",
+	    ("---|-----------|-----------|-----------|--------|------|-------------------------\n"
+	     "RTS|%11.3f|%11.3f|%11.3f|%8ld|%6u|    %.2ld:%.2ld:%.2ld/%.2d:%.2d:%.2d\n",
 	     (double)gminj / 1000, (double)gavgj / 1000, (double)gmaxj / 1000,
-	     goverrun, actual_duration / 3600, (actual_duration / 60) % 60,
+	     goverrun, max_relaxed, actual_duration / 3600, (actual_duration / 60) % 60,
 	     actual_duration % 60, test_duration / 3600,
 	     (test_duration / 60) % 60, test_duration % 60);
+	if (max_relaxed > 0)
+		printf(
+"Warning! some latency maxima may have been due to involuntary mode switches.\n"
+"Please contact [email protected]");
 
 	if (histogram_avg)
 		free(histogram_avg);

@@ -425,13 +441,32 @@ void faulthand(int sig)
 	kill(getpid(), sig);
 }
 
+void mode_sw(int sig)
+{
+	const char buffer[] = "Mode switch, aborting. Backtrace:\n";
+	static void *bt[200];
+	unsigned n;
+
+	if (!stop_upon_switch) {
+		++sampling_relaxed;
+		return;
+	}
+
+	write(STDERR_FILENO, buffer, sizeof(buffer));
+	n = backtrace(bt, sizeof(bt)/sizeof(bt[0]));
+	backtrace_symbols_fd(bt, n, STDERR_FILENO);
+
+	signal(sig, SIG_DFL);
+	kill(getpid(), sig);
+}
+
 int main(int argc, char **argv)
 {
 	int c, err;
 	char task_name[16];
 	int cpu = 0;
 
-	while ((c = getopt(argc, argv, "hp:l:T:qH:B:sD:t:fc:P:")) != EOF)
+	while ((c = getopt(argc, argv, "hp:l:T:qH:B:sD:t:fc:P:b")) != EOF)
 		switch (c) {
 		case 'h':
 

@@ -497,22 +532,29 @@ int main(int argc, char **argv)
 			priority = atoi(optarg);
 			break;
 
+		case 'b':
+			stop_upon_switch = 1;
+			break;
+
 		default:
 
-			fprintf(stderr, "usage: latency [options]\n"
-				"  [-h]                         # print histograms of min, avg, max latencies\n"
-				"  [-s]                         # print statistics of min, avg, max latencies\n"
-				"  [-H <histogram-size>]        # default = 200, increase if your last bucket is full\n"
-				"  [-B <bucket-size>]           # default = 1000ns, decrease for more resolution\n"
-				"  [-p <period_us>]             # sampling period\n"
-				"  [-l <data-lines per header>] # default=21, 0 to supress headers\n"
-				"  [-T <test_duration_seconds>] # default=0, so ^C to end\n"
-				"  [-q]                         # supresses RTD, RTH lines if -T is used\n"
-				"  [-D <testing_device_no>]     # number of testing device, default=0\n"
-				"  [-t <test_mode>]             # 0=user task (default), 1=kernel task, 2=timer IRQ\n"
-				"  [-f]                         # freeze trace for each new max latency\n"
-				"  [-c <cpu>]                   # pin measuring task down to given CPU\n"
-				"  [-P <priority>]              # task priority (test mode 0 and 1 only)\n");
+			fprintf(stderr, 
+"usage: latency [options]\n"
+"  [-h]                         # print histograms of min, avg, max latencies\n"
+"  [-s]                         # print statistics of min, avg, max latencies\n"
+"  [-H <histogram-size>]        # default = 200, increase if your last bucket is full\n"
+"  [-B <bucket-size>]           # default = 1000ns, decrease for more resolution\n"
+"  [-p <period_us>]             # sampling period\n"
+"  [-l <data-lines per header>] # default=21, 0 to supress headers\n"
+"  [-T <test_duration_seconds>] # default=0, so ^C to end\n"
+"  [-q]                         # supresses RTD, RTH lines if -T is used\n"
+"  [-D <testing_device_no>]     # number of testing device, default=0\n"
+"  [-t <test_mode>]             # 0=user task (default), 1=kernel task, 2=timer IRQ\n"
+"  [-f]                         # freeze trace for each new max latency\n"
+"  [-c <cpu>]                   # pin measuring task down to given CPU\n"
+"  [-P <priority>]              # task priority (test mode 0 and 1 only)\n"
+"  [-b]                         # break upon mode switch\n"
+);
 			exit(2);
 		}
 

@@ -548,6 +590,7 @@ int main(int argc, char **argv)
 	signal(SIGTERM, sighand);
 	signal(SIGHUP, sighand);
 	signal(SIGALRM, sighand);
+	signal(SIGXCPU, mode_sw);
 
 	if (freeze_max) {
 		/* If something goes wrong, we want to freeze the current

@@ -605,7 +648,7 @@ int main(int argc, char **argv)
 		snprintf(task_name, sizeof(task_name), "sampling-%d", getpid());
 		err =
 		    rt_task_create(&latency_task, task_name, 0, priority,
-				   T_FPU | cpu);
+				   T_FPU | cpu | T_WARNSW);
 
 		if (err) {
 			fprintf(stderr,