--- a/configure	Tue Apr 29 13:54:23 2008 +0200
+++ b/configure	Fri May 02 17:30:37 2008 +0200
@@ -92,6 +92,10 @@
 	XENO_CONFIG=/usr/xenomai/bin/xeno-config
 fi
 
+if [ "$RTAI_CONFIG" = "" ]; then
+	RTAI_CONFIG=/usr/realtime/bin/rtai-config
+fi
+
 ###########################################################################
 #                          ARGUMENTS PARSING                              #
 ###########################################################################
@@ -426,7 +430,14 @@
 if [ "$SUB_TIMERS_DRIVER" = "xeno" ]; then
 	RT_LIB_DIR=`$XENO_CONFIG --library-dir`\ -Wl,-rpath\ `$XENO_CONFIG --library-dir`
     SUB_EXE_CFLAGS=$SUB_EXE_CFLAGS\ `$XENO_CONFIG --xeno-ldflags`\ -L$RT_LIB_DIR\ -lnative\ -lrtdm
-	SUB_PROG_CFLAGS=$SUB_PROG_CFLAGS\ `$XENO_CONFIG --xeno-cflags`
+	SUB_PROG_CFLAGS=$SUB_PROG_CFLAGS\ -DUSE_XENO\ `$XENO_CONFIG --xeno-cflags`
+    RTCAN_SOCKET=1
+fi
+
+if [ "$SUB_TIMERS_DRIVER" = "rtai" ]; then
+	RT_LIB_DIR=`$RTAI_CONFIG --library-dir`\ -Wl,-rpath\ `$RTAI_CONFIG --library-dir`
+    SUB_EXE_CFLAGS=$SUB_EXE_CFLAGS\ `$RTAI_CONFIG --lxrt-ldflags`\ -L$RT_LIB_DIR\ -llxrt\ -lrtdm
+	SUB_PROG_CFLAGS=$SUB_PROG_CFLAGS\ -DUSE_RTAI\ `$RTAI_CONFIG --lxrt-cflags`
     RTCAN_SOCKET=1
 fi
 

--- a/doc/manual/en/manual.tex	Tue Apr 29 13:54:23 2008 +0200
+++ b/doc/manual/en/manual.tex	Fri May 02 17:30:37 2008 +0200
@@ -191,6 +191,7 @@
 ./drivers/unix Linux and Cygwin OS interface
 ./drivers/win32 Native Win32 OS interface
 ./drivers/timers_xeno Xenomai timers/threads (Linux only)
+./drivers/timers_rtai Rtai timers/threads (Linux only)
 ./drivers/timers_kernel Linux kernel timer/threads
 ./drivers/timers_unix Posix timers/threads (Linux, Cygwin)
 ./drivers/can_virtual_kernel Fake CAN network (kernel space)
@@ -328,17 +329,19 @@
 
 \subsubsection{Real -Time Linux node}
 
-
+With Xenomai :
 \begin{verbatim}
 	./configure --timers=xeno
 \end{verbatim}
-
-
+With Rtai :
+\begin{verbatim}
+	./configure --timers=rtai
+\end{verbatim}
 
 To do a \canopen node running on PC -Linux, you need :
 
 \begin{enumerate}
-\item A working Linux distribution patched with XENOMAI 2.1 or greater. 
+\item A working Linux distribution patched with XENOMAI (2.1 or greater) or RTAI (3.6). 
 \item One or more Peak system PC CAN interface and the last Peak Real Time
 Linux driver installed. 
 \end{enumerate}
@@ -419,7 +422,7 @@
 Virtual CAN interface use Unix pipes to emulate a virtual CAN network.
 Each message issued from a node is repeat to all other nodes. Currently
 only works with nodes running in the same process, and does not support
-work with Xenomai.
+work with Xenomai or Rtai.
 
 
 \subsection{Testing your CanFestival installation}

--- a/drivers/can_peak_linux/can_peak_linux.c	Tue Apr 29 13:54:23 2008 +0200
+++ b/drivers/can_peak_linux/can_peak_linux.c	Fri May 02 17:30:37 2008 +0200
@@ -24,6 +24,7 @@
 #include <string.h>
 #include <errno.h>
 #include <fcntl.h>
+#include <stdlib.h>
 
 /* driver pcan pci for Peak board */
 //#include "libpcan.h"
@@ -58,7 +59,7 @@
     m->data[data] = peakMsg.DATA[data];         	/* data bytes, up to 8 */
 
 #if defined DEBUG_MSG_CONSOLE_ON
-  printf("in : ");
+  MSG("in : ");
   print_message(m);
 #endif
   
@@ -82,7 +83,7 @@
   	peakMsg.DATA[data] = m->data[data];         	/* data bytes, up to 8 */
   
 #if defined DEBUG_MSG_CONSOLE_ON
-  printf("out : ");
+  MSG("out : ");
   print_message(m);
 #endif
   if((errno = CAN_Write(fd0, & peakMsg))) {
@@ -121,7 +122,6 @@
   HANDLE fd0 = NULL;
   char busname[64];
   char* pEnd;
-  int i;  
   int baudrate;
   
   if(strtol(board->busname, &pEnd,0) >= 0)

--- a/drivers/can_peak_win32/can_peak_win32.c	Tue Apr 29 13:54:23 2008 +0200
+++ b/drivers/can_peak_win32/can_peak_win32.c	Fri May 02 17:30:37 2008 +0200
@@ -135,7 +135,7 @@
 			for (data = 0; data < peakMsg.LEN; data++)
 				m->data[data] = peakMsg.DATA[data];	/* data bytes, up to 8 */
 #if defined DEBUG_MSG_CONSOLE_ON
-			printf("in : ");
+			MSG("in : ");
 			print_message(m);
 #endif
 		}else{
@@ -202,7 +202,7 @@
 	}
 	while (errno != CAN_ERR_OK);
 #if defined DEBUG_MSG_CONSOLE_ON
-	printf("out : ");
+	MSG("out : ");
 	print_message(m);
 #endif
 	return 0;

--- a/drivers/can_socket/can_socket.c	Tue Apr 29 13:54:23 2008 +0200
+++ b/drivers/can_socket/can_socket.c	Fri May 02 17:30:37 2008 +0200
@@ -82,7 +82,7 @@
   memcpy (m->data, frame.data, 8);
 
 #if defined DEBUG_MSG_CONSOLE_ON
-  printf("in : ");
+  MSG("in : ");
   print_message(m);
 #endif
   return 0;
@@ -106,7 +106,7 @@
     memcpy (frame.data, m->data, 8);
 
 #if defined DEBUG_MSG_CONSOLE_ON
-  printf("out : ");
+  MSG("out : ");
   print_message(m);
 #endif
   res = CAN_SEND (*(int *) fd0, &frame, sizeof (frame), 0);

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/drivers/timers_rtai/.cvsignore	Fri May 02 17:30:37 2008 +0200
@@ -0,0 +1,1 @@
+Makefile

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/drivers/timers_rtai/Makefile.in	Fri May 02 17:30:37 2008 +0200
@@ -0,0 +1,60 @@
+#! gmake
+
+#
+# Copyright (C) 2006 Laurent Bessard
+# 
+# This file is part of canfestival, a library implementing the canopen
+# stack
+# 
+# This library is free software; you can redistribute it and/or
+# modify it under the terms of the GNU Lesser General Public
+# License as published by the Free Software Foundation; either
+# version 2.1 of the License, or (at your option) any later version.
+# 
+# This library is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+# Lesser General Public License for more details.
+# 
+# You should have received a copy of the GNU Lesser General Public
+# License along with this library; if not, write to the Free Software
+# Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+# 
+
+CC = SUB_CC
+OPT_CFLAGS = -O2
+CFLAGS = SUB_OPT_CFLAGS
+PROG_CFLAGS = SUB_PROG_CFLAGS
+OS_NAME = SUB_OS_NAME
+ARCH_NAME = SUB_ARCH_NAME
+PREFIX = SUB_PREFIX
+TARGET = SUB_TARGET
+CAN_DRIVER = SUB_CAN_DRIVER
+TIMERS_DRIVER = SUB_TIMERS_DRIVER
+
+INCLUDES = -I../../include -I../../include/$(TARGET) -I../../include/$(CAN_DRIVER) -I../../include/$(TIMERS_DRIVER)
+
+OBJS = $(TIMERS_DRIVER).o
+
+SRC_HFILES = ../../include/$(TIMERS_DRIVER)/timerscfg.h
+
+TARGET_HFILES = $(PREFIX)/include/canfestival/timerscfg.h
+
+all: driver
+
+driver: $(OBJS)
+
+%o: %c
+	$(CC) $(CFLAGS) $(PROG_CFLAGS) ${PROGDEFINES} $(INCLUDES) -o $@ -c $<
+
+install:
+	mkdir -p $(PREFIX)/include/canfestival
+	cp $(SRC_HFILES) $(PREFIX)/include/canfestival
+
+uninstall:
+	rm -f $(TARGET_HFILES)
+
+clean:
+	rm -f $(OBJS)
+
+mrproper: clean

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/drivers/timers_rtai/timers_rtai.c	Fri May 02 17:30:37 2008 +0200
@@ -0,0 +1,237 @@
+#include <stdlib.h>
+#include <unistd.h>
+#include <sys/mman.h>
+
+#include <sys/poll.h>
+#include <rtai_lxrt.h>
+
+#include <rtai_sem.h>
+#include <pthread.h>
+#include <errno.h>
+
+#include "applicfg.h"
+#include "can_driver.h"
+#include "timer.h"
+
+#define TIMERLOOP_TASK_CREATED        1
+
+TimerCallback_t exitall;
+
+SEM *CanFestival_mutex;
+SEM *condition_mutex;
+SEM *control_task; 
+CND *timer_set;
+
+// realtime task structures
+RT_TASK *timerloop_task;
+RT_TASK *Main_Task;
+
+// linux threads id's
+static pthread_t timerloop_thr;
+ 
+RTIME last_time_read;
+RTIME last_occured_alarm;
+RTIME last_timeout_set;
+
+int stop_timer = 0;
+
+void TimerInit(void)
+{
+    /* Init Main Task */
+    if (!(Main_Task = rt_thread_init(rt_get_name(0), 0, 0, SCHED_FIFO, 1))) {
+            printf("CANNOT INIT MAIN TASK\n");
+            exit(1);
+    }
+  	
+  	/* Init Mutex */
+	CanFestival_mutex = rt_sem_init(rt_get_name(0), 1);
+	condition_mutex = rt_typed_sem_init(rt_get_name(0), 1, RES_SEM);
+	timer_set = rt_cond_init(rt_get_name(0));
+	control_task = rt_sem_init(rt_get_name(0), 0);
+	/* Set timer mode and start timer */
+	rt_set_oneshot_mode();
+	start_rt_timer(0);
+}
+
+/**
+ * Stop Timer Task
+ * @param exitfunction
+ */
+void StopTimerLoop(TimerCallback_t exitfunction)
+{
+	exitall = exitfunction;
+	stop_timer = 1;
+	rt_cond_signal(timer_set);
+}
+
+/**
+ * Clean all Semaphores and main task
+ */
+void TimerCleanup(void)
+{
+	/* Stop timer */
+	stop_rt_timer();
+
+	/* Delete all mutex and the main task */
+	rt_sem_delete(CanFestival_mutex);
+	rt_sem_delete(condition_mutex);
+	rt_sem_delete(timer_set);
+	rt_sem_delete(control_task);
+	rt_thread_delete(Main_Task);
+}
+
+/**
+ * Take a semaphore
+ */
+void EnterMutex(void)
+{
+	rt_sem_wait(CanFestival_mutex);
+}
+
+/**
+ * Signaling a semaphore
+ */
+void LeaveMutex(void)
+{
+	rt_sem_signal(CanFestival_mutex);
+}
+
+static TimerCallback_t init_callback;
+
+/**
+ * Timer Task
+ */
+void timerloop_task_proc(void *arg)
+{
+	int ret = 0;
+  	// lock process in to RAM
+  	mlockall(MCL_CURRENT | MCL_FUTURE);
+	timerloop_task = rt_thread_init(rt_get_name(0), 0, 0, SCHED_FIFO, 1);
+	rt_make_hard_real_time();
+
+	getElapsedTime();
+	last_timeout_set = 0;
+	last_occured_alarm = last_time_read;
+
+	/* trigger first alarm */
+	SetAlarm(NULL, 0, init_callback, 0, 0);
+	
+	do{
+		RTIME real_alarm;
+		rt_sem_wait(condition_mutex);
+		if(last_timeout_set == TIMEVAL_MAX)
+		{
+			ret = rt_cond_wait(
+				timer_set,
+				condition_mutex);		/* Then sleep until next message*/
+ 
+			rt_sem_signal(condition_mutex);
+		}else{
+			real_alarm = last_time_read + last_timeout_set;
+			ret = rt_cond_wait_until(
+				timer_set,
+				condition_mutex,
+				real_alarm); /* Else, sleep until next deadline */
+			if(ret == SEM_TIMOUT){
+				last_occured_alarm = real_alarm;
+				rt_sem_signal(condition_mutex);
+				EnterMutex();
+				TimeDispatch();
+				LeaveMutex();
+			}else{ 
+				rt_sem_signal(condition_mutex);
+			}
+		}
+	}while ( ret != SEM_ERR && !stop_timer);
+	if(exitall){
+		EnterMutex();
+		exitall(NULL,0);
+		LeaveMutex();
+	}
+	rt_make_soft_real_time();
+	rt_thread_delete(timerloop_task);
+}
+
+/**
+ * Create the Timer Task
+ * @param _init_callback
+ */
+void StartTimerLoop(TimerCallback_t _init_callback)
+{
+	stop_timer = 0;	
+	init_callback = _init_callback;
+	
+	/* start timerloop_task ( do nothing and get blocked ) */
+	timerloop_thr = rt_thread_create(timerloop_task_proc, NULL, 0);
+}
+
+/* We assume that ReceiveLoop_task_proc is always the same */
+static void (*rtai_ReceiveLoop_task_proc)(CAN_PORT) = NULL;
+
+/**
+ * Enter in realtime and start the CAN receiver loop
+ * @param port
+ */
+void rtai_canReceiveLoop(CAN_PORT port)
+{
+	RT_TASK *current_task;
+	mlockall(MCL_CURRENT | MCL_FUTURE);
+	current_task = rt_thread_init(rt_get_name(0), 0, 0, SCHED_FIFO, 1);
+	rt_make_hard_real_time();
+
+	rt_sem_signal(control_task);
+
+	/* Call original receive loop with port struct as a param */
+	rtai_ReceiveLoop_task_proc(port);
+
+	rt_make_soft_real_time();
+	rt_thread_delete(current_task);
+}
+
+/**
+ * Create the CAN Receiver Task
+ * @param fd0 CAN port
+ * @param *ReceiveLoop_thread CAN receiver thread
+ * @param *ReceiveLoop_task_proc CAN receiver task
+ */
+void CreateReceiveTask(CAN_PORT fd0, TASK_HANDLE *ReceiveLoop_thread, void* ReceiveLoop_task_proc)
+{	
+	rtai_ReceiveLoop_task_proc = ReceiveLoop_task_proc;
+	*ReceiveLoop_thread = rt_thread_create(rtai_canReceiveLoop, (void*)fd0, 0); 
+	rt_sem_wait(control_task);
+}
+
+/**
+ * Wait for the CAN Receiver Task end
+ * @param *ReceiveLoop_thread CAN receiver thread
+ */
+void WaitReceiveTaskEnd(TASK_HANDLE *ReceiveLoop_thread)
+{
+	rt_thread_join(*ReceiveLoop_thread);
+}
+
+/**
+ * Set timer for the next wakeup
+ * @param value
+ */
+void setTimer(TIMEVAL value)
+{
+	rt_sem_wait(condition_mutex);
+	last_timeout_set = (value == TIMEVAL_MAX) ? TIMEVAL_MAX : value;
+	rt_sem_signal(condition_mutex);
+	rt_cond_signal(timer_set);
+}
+
+/**
+ * Get the elapsed time since the last alarm
+ * @return a time in nanoseconds
+ */
+TIMEVAL getElapsedTime(void)
+{
+	RTIME res;
+	rt_sem_wait(condition_mutex);
+	last_time_read = rt_get_time();
+	res = last_time_read - last_occured_alarm;
+	rt_sem_signal(condition_mutex);
+	return res;
+}

--- a/drivers/timers_unix/timers_unix.c	Tue Apr 29 13:54:23 2008 +0200
+++ b/drivers/timers_unix/timers_unix.c	Fri May 02 17:30:37 2008 +0200
@@ -17,6 +17,11 @@
 
 timer_t timer;
 
+void TimerCleanup(void)
+{
+	/* only used in realtime apps */
+}
+
 void EnterMutex(void)
 {
 	pthread_mutex_lock(&CanFestival_mutex); 
@@ -36,7 +41,7 @@
 //	printf("getCurrentTime() return=%u\n", p.tv_usec);
 }
 
-void initTimer(void)
+void TimerInit(void)
 {
 	struct sigevent sigev;
 
@@ -52,16 +57,16 @@
 	timer_create (CLOCK_REALTIME, &sigev, &timer);
 }
 
-void StopTimerLoop(void)
+void StopTimerLoop(TimerCallback_t exitfunction)
 {
 	EnterMutex();
 	timer_delete (timer);
+	exitfunction(NULL,0);
 	LeaveMutex();
 }
 
 void StartTimerLoop(TimerCallback_t init_callback)
 {
-	initTimer();
 	EnterMutex();
 	// At first, TimeDispatch will call init_callback.
 	SetAlarm(NULL, 0, init_callback, 0, 0);

--- a/drivers/timers_xeno/timers_xeno.c	Tue Apr 29 13:54:23 2008 +0200
+++ b/drivers/timers_xeno/timers_xeno.c	Fri May 02 17:30:37 2008 +0200
@@ -4,7 +4,9 @@
 
 #include <native/task.h>
 #include <native/timer.h>
+#include <native/sem.h>
 #include <native/mutex.h>
+#include <native/cond.h>
 #include <native/alarm.h>
 
 #include "applicfg.h"
@@ -13,74 +15,168 @@
 
 #define TIMERLOOP_TASK_CREATED        1
 
-RT_MUTEX CanFestival_mutex;
+TimerCallback_t exitall;
+
+RT_MUTEX condition_mutex;
+RT_SEM CanFestival_mutex;
+RT_SEM control_task; 
+RT_COND timer_set;	
 RT_TASK timerloop_task;
+ 
 RTIME last_time_read;
 RTIME last_occured_alarm;
-RTIME last_alarm_set;
-
-char stop_timer=0;
+RTIME last_timeout_set;
+
+int stop_timer = 0;
+
+/**
+ * Init Mutex, Semaphores and Condition variable
+ */
+void TimerInit(void)
+{
+  	int ret = 0;
+  	char taskname[32];
+
+  	mlockall(MCL_CURRENT | MCL_FUTURE);
+
+  	snprintf(taskname, sizeof(taskname), "S1-%d", getpid());
+	rt_sem_create(&CanFestival_mutex, taskname, 1, S_FIFO);
+
+  	snprintf(taskname, sizeof(taskname), "S2-%d", getpid());
+  	rt_sem_create(&control_task, taskname, 0, S_FIFO);
+  	  	
+  	snprintf(taskname, sizeof(taskname), "M1-%d", getpid());
+  	rt_mutex_create(&condition_mutex, taskname);
+  	
+  	snprintf(taskname, sizeof(taskname), "C1-%d", getpid());
+  	rt_cond_create(&timer_set, taskname);
+}
+
+/**
+ * Stop Timer Task
+ * @param exitfunction
+ */
+void StopTimerLoop(TimerCallback_t exitfunction)
+{
+	exitall = exitfunction;
+	stop_timer = 1;
+	rt_cond_signal(&timer_set);
+}
 
 void cleanup_all(void)
 {
 	rt_task_delete(&timerloop_task);
 }
-void StopTimerLoop(void)
-{
-	stop_timer = 1;
-	rt_task_unblock(&timerloop_task);
-}
-
-
+
+/**
+ * Clean all Semaphores, mutex, condition variable and main task
+ */
+void TimerCleanup(void)
+{
+	rt_sem_delete(&CanFestival_mutex);
+	rt_mutex_delete(&condition_mutex);
+	rt_cond_delete(&timer_set);
+	rt_sem_delete(&control_task);
+}
+
+/**
+ * Take a semaphore
+ */
 void EnterMutex(void)
 {
-	rt_mutex_lock(&CanFestival_mutex, TM_INFINITE); 
-}
-
+	rt_sem_p(&CanFestival_mutex, TM_INFINITE);
+}
+
+/**
+ * Signaling a semaphore
+ */
 void LeaveMutex(void)
 {
-	rt_mutex_unlock(&CanFestival_mutex);
-}
-
+	rt_sem_v(&CanFestival_mutex);
+}
+
+static TimerCallback_t init_callback;
+
+/**
+ * Timer Task
+ */
 void timerloop_task_proc(void *arg)
 {
-	int ret;
+	int ret = 0;
+  	// lock process in to RAM
+  	mlockall(MCL_CURRENT | MCL_FUTURE);
+
+	getElapsedTime();
+	last_timeout_set = 0;
+	last_occured_alarm = last_time_read;
+	
+	/* trigger first alarm */
+	SetAlarm(NULL, 0, init_callback, 0, 0);
+	RTIME current_time;
+	RTIME real_alarm;
 	do{
-		do{
-			last_occured_alarm = last_alarm_set;
-			EnterMutex();
-			TimeDispatch();
-			LeaveMutex();
-			while ((ret = rt_task_sleep_until(last_alarm_set)) == -EINTR);
-		}while (ret == 0);
-	}while (!stop_timer);
-	printf("End of TimerLoop, code %d\n",ret);
-}
-
-void StartTimerLoop(TimerCallback_t init_callback)
-{
-	int ret;
-	stop_timer = 0;
+		
+		rt_mutex_acquire(&condition_mutex, TM_INFINITE);
+		if(last_timeout_set == TIMEVAL_MAX)
+		{
+			ret = rt_cond_wait(
+				&timer_set,
+				&condition_mutex,
+				TM_INFINITE
+				);		/* Then sleep until next message*/
+			rt_mutex_release(&condition_mutex);
+		}else{
+			current_time = rt_timer_read();
+			real_alarm = last_time_read + last_timeout_set;
+			ret = rt_cond_wait( /* sleep until next deadline */
+				&timer_set,
+				&condition_mutex,
+				(real_alarm - current_time)); /* else alarm consider expired */   
+			if(ret = -ETIMEDOUT){
+				last_occured_alarm = real_alarm;
+				rt_mutex_release(&condition_mutex);
+				EnterMutex();
+				TimeDispatch();
+				LeaveMutex();
+			}else{ 
+				rt_mutex_release(&condition_mutex);
+			}
+		}
+	}while ((ret == 0 || ret == -EINTR || ret == -ETIMEDOUT) && !stop_timer);
+	
+	if(exitall){
+		EnterMutex();
+		exitall(NULL,0);
+		LeaveMutex();
+	}
+	
+	rt_task_delete(&timerloop_task);
+}
+
+/**
+ * Create the Timer Task
+ * @param _init_callback
+ */
+void StartTimerLoop(TimerCallback_t _init_callback)
+{
+	int ret = 0;
+	stop_timer = 0;	
+	init_callback = _init_callback;
+	
 	char taskname[32];
 	snprintf(taskname, sizeof(taskname), "timerloop-%d", getpid());
 
-	mlockall(MCL_CURRENT | MCL_FUTURE);
-
-	//create timerloop_task
+	/* create timerloop_task */
 	ret = rt_task_create(&timerloop_task, taskname, 0, 50, 0);
 	if (ret) {
 		printf("Failed to create timerloop_task, code %d\n",errno);
 		return;
 	}
  	
-	getElapsedTime();
-	last_alarm_set = last_time_read;
-	last_occured_alarm = last_alarm_set;
-	SetAlarm(NULL, 0, init_callback, 0, 0);
-	// start timerloop_task
+	/* start timerloop_task */
 	ret = rt_task_start(&timerloop_task,&timerloop_task_proc,NULL);
 	if (ret) {
-		printf("Failed to start timerloop_task, code %d\n",errno);
+		printf("Failed to start timerloop_task, code %u\n",errno);
 		goto error;
 	}
 	
@@ -90,43 +186,66 @@
 	cleanup_all();
 }
 
+/**
+ * Create the CAN Receiver Task
+ * @param fd0 CAN port
+ * @param *ReceiveLoop_task CAN receiver task
+ * @param *ReceiveLoop_task_proc CAN receiver function
+ */
 void CreateReceiveTask(CAN_PORT fd0, TASK_HANDLE *ReceiveLoop_task, void* ReceiveLoop_task_proc)
-{
+{	
 	int ret;
 	static int id = 0;
 	char taskname[32];
 	snprintf(taskname, sizeof(taskname), "canloop%d-%d", id, getpid());
 	id++;
 
-	mlockall(MCL_CURRENT | MCL_FUTURE);
-
-	//create timerloop_task
+	/* create ReceiveLoop_task */
 	ret = rt_task_create(ReceiveLoop_task,taskname,0,50,0);
 	if (ret) {
 		printf("Failed to create ReceiveLoop_task number %d, code %d\n", id, errno);
 		return;
 	}
-	// start timerloop_task
-	ret = rt_task_start(ReceiveLoop_task,ReceiveLoop_task_proc,(void*)fd0);
+	/* start ReceiveLoop_task */
+	ret = rt_task_start(ReceiveLoop_task, ReceiveLoop_task_proc,(void*)fd0);
 	if (ret) {
 		printf("Failed to start ReceiveLoop_task number %d, code %d\n", id, errno);
 		return;
 	}
-}
-
-void WaitReceiveTaskEnd(TASK_HANDLE *Thread)
-{
-	rt_task_delete(Thread);
-}
-
+	rt_sem_v(&control_task);
+}
+
+/**
+ * Wait for the CAN Receiver Task end
+ * @param *ReceiveLoop_task CAN receiver thread
+ */
+void WaitReceiveTaskEnd(TASK_HANDLE *ReceiveLoop_task)
+{
+	rt_task_delete(ReceiveLoop_task);
+}
+
+/**
+ * Set timer for the next wakeup
+ * @param value
+ */
 void setTimer(TIMEVAL value)
 {
-	last_alarm_set = (value == TIMEVAL_MAX) ? TIMEVAL_MAX : last_time_read + value;
-	rt_task_unblock(&timerloop_task);
-}
-
+	rt_mutex_acquire(&condition_mutex, TM_INFINITE);
+	last_timeout_set = (value == TIMEVAL_MAX) ? TIMEVAL_MAX : value;
+	rt_mutex_release(&condition_mutex);
+	rt_cond_signal(&timer_set);
+}
+
+/**
+ * Get the elapsed time since the last alarm
+ * @return a time in nanoseconds
+ */
 TIMEVAL getElapsedTime(void)
 {
-	last_time_read = rt_timer_ticks2ns(rt_timer_read());
-	return last_time_read - last_occured_alarm;
-}
+	RTIME res;
+	rt_mutex_acquire(&condition_mutex, TM_INFINITE);
+	last_time_read = rt_timer_read();
+	res = last_time_read - last_occured_alarm;
+	rt_mutex_release(&condition_mutex);
+	return res;
+}

--- a/drivers/unix/unix.c	Tue Apr 29 13:54:23 2008 +0200
+++ b/drivers/unix/unix.c	Fri May 02 17:30:37 2008 +0200
@@ -190,12 +190,8 @@
 		canports[i].used = 1;
 		canports[i].fd = fd0;
 		canports[i].d = d;
-	
+		d->canHandle = (CAN_PORT)&canports[i];		
 		CreateReceiveTask(&(canports[i]), &canports[i].receiveTask, &canReceiveLoop);
-		
-		EnterMutex();
-		d->canHandle = (CAN_PORT)&canports[i];
-		LeaveMutex();
 		return (CAN_PORT)&canports[i];
 	}else{
         	MSG("CanOpen : Cannot open board {busname='%s',baudrate='%s'}\n",board->busname, board->baudrate);
@@ -211,12 +207,10 @@
 int canClose(CO_Data * d)
 {
 	UNS8 res;
-
-	EnterMutex();
+	
 	((CANPort*)d->canHandle)->used = 0;
 	CANPort* tmp = (CANPort*)d->canHandle;
 	d->canHandle = NULL;
-	LeaveMutex();
 	
 	// close CAN port
 	res = DLL_CALL(canClose)(tmp->fd);

--- a/examples/DS401_Master/Makefile.in	Tue Apr 29 13:54:23 2008 +0200
+++ b/examples/DS401_Master/Makefile.in	Fri May 02 17:30:37 2008 +0200
@@ -41,10 +41,6 @@
 
 OBJS = $(MASTER_OBJS) ../../src/libcanfestival.a ../../drivers/$(TARGET)/libcanfestival_$(TARGET).a
 
-ifeq ($(TIMERS_DRIVER),timers_xeno)
-	PROGDEFINES = -DUSE_XENO
-endif
-
 all: DS401_Master
 
 ../../drivers/$(TARGET)/libcanfestival_$(TARGET).a:

--- a/examples/DS401_Master/TestMasterMicroMod.c	Tue Apr 29 13:54:23 2008 +0200
+++ b/examples/DS401_Master/TestMasterMicroMod.c	Fri May 02 17:30:37 2008 +0200
@@ -82,7 +82,7 @@
 			RW);  /* UNS8 checkAccess */
 }
 
-static init_step = 0;
+static int init_step = 0;
 
 /*Froward declaration*/
 static void ConfigureSlaveNode(CO_Data* d, UNS8 nodeId);
@@ -363,7 +363,7 @@
 }
 #endif
 
-void help()
+void help(void)
 {
   printf("**************************************************************\n");
   printf("*  TestMasterMicroMod                                        *\n");
@@ -409,6 +409,15 @@
 	}
 }
 
+/***************************  EXIT  *****************************************/
+void Exit(CO_Data* d, UNS32 id)
+{
+	masterSendNMTstateChange(&TestMaster_Data, 0x02, NMT_Reset_Node);
+
+    //Stop master
+	setState(&TestMaster_Data, Stopped);
+}
+
 /****************************************************************************/
 /***************************  MAIN  *****************************************/
 /****************************************************************************/
@@ -466,6 +475,7 @@
   /* install signal handler for manual break */
 	signal(SIGTERM, catch_signal);
 	signal(SIGINT, catch_signal);
+	TimerInit();
 #endif
 
 #ifndef NOT_USE_DYNAMIC_LOADING
@@ -499,12 +509,13 @@
 	setState(&TestMaster_Data, Stopped);
 	
 	// Stop timer thread
-	StopTimerLoop();
+	StopTimerLoop(&Exit);
 	
 fail_master:
 	if(MasterBoard.baudrate) canClose(&TestMaster_Data);	
 
-  return 0;
-}
-
-
+	TimerCleanup();
+  	return 0;
+}
+
+

--- a/examples/DS401_Slave_Gui/Makefile.in	Tue Apr 29 13:54:23 2008 +0200
+++ b/examples/DS401_Slave_Gui/Makefile.in	Fri May 02 17:30:37 2008 +0200
@@ -46,10 +46,6 @@
 
 OBJS = ObjDict.o ../../src/libcanfestival.a ../../drivers/$(TARGET)/libcanfestival_$(TARGET).a
 
-ifeq ($(TIMERS_DRIVER),timers_xeno)
-	PROGDEFINES = -DUSE_XENO
-endif
-
 all: DS401_Slave_Gui
 
 ../../drivers/$(TARGET)/libcanfestival_$(TARGET).a:

--- a/examples/DS401_Slave_Gui/TestSlaveGui.cpp	Tue Apr 29 13:54:23 2008 +0200
+++ b/examples/DS401_Slave_Gui/TestSlaveGui.cpp	Fri May 02 17:30:37 2008 +0200
@@ -60,6 +60,12 @@
   setState (&ObjDict_Data, Initialisation);
 }
 
+/***************************  EXIT  *****************************************/
+void Exit(CO_Data* d, UNS32 id)
+{
+  	setState (&ObjDict_Data, Stopped);
+	canClose (&ObjDict_Data);
+}
 //****************************************************************************
 //***************************  MAIN  *****************************************
 //****************************************************************************
@@ -69,6 +75,7 @@
   printf ("Bus name: %s        Freq: %s       Driver: %s\n",
 	  SlaveBoard.busname, SlaveBoard.baudrate, LibraryPath);
 
+  TimerInit();
 #ifndef NOT_USE_DYNAMIC_LOADING
   if (LoadCanDriver (LibraryPath) == NULL)
     *textLog << wxT ("Unable to load library\n");
@@ -99,12 +106,7 @@
 void
 stop_slave ()
 {
-  EnterMutex ();
-  setState (&ObjDict_Data, Stopped);
-  LeaveMutex ();
-
-  StopTimerLoop ();
-  canClose (&ObjDict_Data);
-
+  StopTimerLoop (&Exit);
+  TimerCleanup();
   return;
 }

--- a/examples/SillySlave/Makefile.in	Tue Apr 29 13:54:23 2008 +0200
+++ b/examples/SillySlave/Makefile.in	Fri May 02 17:30:37 2008 +0200
@@ -41,10 +41,6 @@
 
 OBJS = $(SILLYSLAVE_OBJS) ../../src/libcanfestival.a ../../drivers/$(TARGET)/libcanfestival_$(TARGET).a -lcanlib
 
-ifeq ($(TIMERS_DRIVER),timers_xeno)
-	PROGDEFINES = -DUSE_XENO
-endif
-
 all: SillySlave
 
 ../../drivers/$(TARGET)/libcanfestival_$(TARGET).a:

--- a/examples/SillySlave/slave.c	Tue Apr 29 13:54:23 2008 +0200
+++ b/examples/SillySlave/slave.c	Fri May 02 17:30:37 2008 +0200
@@ -44,6 +44,12 @@
     setState(&SillySlave_Data, Initialisation);
 }
 
+void Exit(CO_Data* d, UNS32 id)
+{
+	/* Stop slave */
+    setState(&SillySlave_Data, Stopped);
+}
+
 INTEGER8 InitCANdevice( UNS8 bus, UNS32 baudrate, UNS8 node )
 { 
 char busName[2];
@@ -83,11 +89,8 @@
 	pause();
 	printf("\nFinishing.\n");
 	
-	/* Stop slave */
-    setState(&SillySlave_Data, Stopped);
-	
 	/* Stop timer thread */
-	StopTimerLoop();
+	StopTimerLoop(&Exit);
     return 0;
 }
 

--- a/examples/TestMasterMicroMod/Makefile.in	Tue Apr 29 13:54:23 2008 +0200
+++ b/examples/TestMasterMicroMod/Makefile.in	Fri May 02 17:30:37 2008 +0200
@@ -49,10 +49,6 @@
 	EXE_CFLAGS =
 endif
 
-ifeq ($(TIMERS_DRIVER),timers_xeno)
-	PROGDEFINES = -DUSE_XENO
-endif
-
 all: $(TESTMASTERMICROMOD)
 
 ../../drivers/$(TARGET)/libcanfestival_$(TARGET).a:

--- a/examples/TestMasterMicroMod/TestMasterMicroMod.c	Tue Apr 29 13:54:23 2008 +0200
+++ b/examples/TestMasterMicroMod/TestMasterMicroMod.c	Fri May 02 17:30:37 2008 +0200
@@ -82,7 +82,7 @@
 			RW);  /* UNS8 checkAccess */
 }
 
-static init_step = 0;
+static int init_step = 0;
 
 /*Froward declaration*/
 static void ConfigureSlaveNode(CO_Data* d, UNS8 nodeId);
@@ -346,7 +346,7 @@
 }
 #endif
 
-void help()
+void help(void)
 {
   printf("**************************************************************\n");
   printf("*  TestMasterMicroMod                                        *\n");
@@ -392,6 +392,14 @@
 	}
 }
 
+/***************************  EXIT  *****************************************/
+void Exit(CO_Data* d, UNS32 id)
+{
+	masterSendNMTstateChange(&TestMaster_Data, 0x02, NMT_Reset_Node);
+    
+    //Stop master
+	setState(&TestMaster_Data, Stopped);
+}
 /****************************************************************************/
 /***************************  MAIN  *****************************************/
 /****************************************************************************/
@@ -449,6 +457,7 @@
   /* install signal handler for manual break */
 	signal(SIGTERM, catch_signal);
 	signal(SIGINT, catch_signal);
+	TimerInit();
 #endif
 
 #ifndef NOT_USE_DYNAMIC_LOADING
@@ -475,18 +484,13 @@
 	pause();
 	eprintf("Finishing.\n");
 	
-	// Reset the slave node for next use (will stop emitting heartbeat)
-	masterSendNMTstateChange (&TestMaster_Data, slavenodeid, NMT_Reset_Node);
-	
-	// Stop master
-	setState(&TestMaster_Data, Stopped);
-	
 	// Stop timer thread
-	StopTimerLoop();
+	StopTimerLoop(&Exit);
 	
 fail_master:
 	if(MasterBoard.baudrate) canClose(&TestMaster_Data);	
 
+  TimerCleanup();
   return 0;
 }
 

--- a/examples/TestMasterSlave/Makefile.in	Tue Apr 29 13:54:23 2008 +0200
+++ b/examples/TestMasterSlave/Makefile.in	Fri May 02 17:30:37 2008 +0200
@@ -54,6 +54,10 @@
 	PROGDEFINES = -DUSE_XENO
 endif
 
+ifeq ($(TIMERS_DRIVER),timers_rtai)
+	PROGDEFINES = -DUSE_RTAI
+endif
+
 all: $(TESTMASTERSLAVE)
 
 ../../drivers/$(TARGET)/libcanfestival_$(TARGET).a:

--- a/examples/TestMasterSlave/Master.c	Tue Apr 29 13:54:23 2008 +0200
+++ b/examples/TestMasterSlave/Master.c	Fri May 02 17:30:37 2008 +0200
@@ -66,7 +66,7 @@
 }
 
 // Step counts number of times ConfigureSlaveNode is called
-static init_step = 0;
+static int init_step = 0;
 
 /*Froward declaration*/
 static void ConfigureSlaveNode(CO_Data* d, UNS8 nodeId);

--- a/examples/TestMasterSlave/TestMasterSlave.c	Tue Apr 29 13:54:23 2008 +0200
+++ b/examples/TestMasterSlave/TestMasterSlave.c	Fri May 02 17:30:37 2008 +0200
@@ -61,7 +61,7 @@
 }
 #endif
 
-void help()
+void help(void)
 {
   printf("**************************************************************\n");
   printf("*  TestMasterSlave                                           *\n");
@@ -113,6 +113,15 @@
 	}
 }
 
+/***************************  EXIT  *****************************************/
+void Exit(CO_Data* d, UNS32 id)
+{
+	masterSendNMTstateChange(&TestMaster_Data, 0x02, NMT_Reset_Node);    
+    
+    //Stop master
+	setState(&TestMaster_Data, Stopped);
+}
+
 /****************************************************************************/
 /***************************  MAIN  *****************************************/
 /****************************************************************************/
@@ -177,6 +186,7 @@
   /* install signal handler for manual break */
 	signal(SIGTERM, catch_signal);
 	signal(SIGINT, catch_signal);
+	TimerInit();
 #endif
 
 #ifndef NOT_USE_DYNAMIC_LOADING
@@ -224,27 +234,16 @@
 	StartTimerLoop(&InitNodes);
 
 	// wait Ctrl-C
-	
 	pause();
 
-	eprintf("Finishing.\n");
-    EnterMutex();
-	masterSendNMTstateChange (&TestMaster_Data, 0x02, NMT_Reset_Node);
-    LeaveMutex();
-
-	eprintf("reset\n");
-	// Stop master
-    EnterMutex();
-	setState(&TestMaster_Data, Stopped);
-    LeaveMutex();
-	
 	// Stop timer thread
-	StopTimerLoop();
+	StopTimerLoop(&Exit);
 	
 	// Close CAN devices (and can threads)
+	if(strcmp(MasterBoard.baudrate, "none")) canClose(&TestMaster_Data);	
+fail_master:
 	if(strcmp(SlaveBoard.baudrate, "none")) canClose(&TestSlave_Data);
-fail_master:
-	if(strcmp(MasterBoard.baudrate, "none")) canClose(&TestMaster_Data);	
 fail_slave:
+	TimerCleanup();
 	return 0;
 }

--- a/examples/TestMasterSlave/TestMasterSlave.h	Tue Apr 29 13:54:23 2008 +0200
+++ b/examples/TestMasterSlave/TestMasterSlave.h	Fri May 02 17:30:37 2008 +0200
@@ -22,6 +22,8 @@
 #ifdef USE_XENO
 //#define eprintf(...) if(0){}
 #define eprintf(...)
+#elif defined USE_RTAI
+#define eprintf(...)
 #else
 #define eprintf(...) printf (__VA_ARGS__)
 #endif

--- a/examples/TestMasterSlaveLSS/Makefile.in	Tue Apr 29 13:54:23 2008 +0200
+++ b/examples/TestMasterSlaveLSS/Makefile.in	Fri May 02 17:30:37 2008 +0200
@@ -50,9 +50,9 @@
 	EXE_CFLAGS =
 endif
 
-ifeq ($(TIMERS_DRIVER),timers_xeno)
-	PROGDEFINES = -DUSE_XENO
-endif
+#ifeq ($(TIMERS_DRIVER),timers_xeno)
+#	PROGDEFINES = -DUSE_XENO
+#endif
 
 all: $(TESTMASTERSLAVELSS)
 ../../drivers/$(TARGET)/libcanfestival_$(TARGET).a:

--- a/examples/TestMasterSlaveLSS/TestMasterSlaveLSS.c	Tue Apr 29 13:54:23 2008 +0200
+++ b/examples/TestMasterSlaveLSS/TestMasterSlaveLSS.c	Fri May 02 17:30:37 2008 +0200
@@ -57,7 +57,7 @@
 }
 #endif
 
-void help()
+void help(void)
 {
   printf("**************************************************************\n");
   printf("*  TestMasterSlaveLSS                                        *\n");
@@ -122,6 +122,19 @@
 	}
 }
 
+/***************************  EXIT  *****************************************/
+void Exit(CO_Data* d, UNS32 id)
+{
+	eprintf("Finishing.\n");
+	masterSendNMTstateChange (&TestMaster_Data, 0x00, NMT_Stop_Node);
+
+	eprintf("reset\n");
+
+	// Stop master
+	setState(&TestMaster_Data, Stopped);
+
+}
+
 /****************************************************************************/
 /***************************  MAIN  *****************************************/
 /****************************************************************************/
@@ -202,6 +215,7 @@
   /* install signal handler for manual break */
 	signal(SIGTERM, catch_signal);
 	signal(SIGINT, catch_signal);
+	TimerInit();
 #endif
 
 #ifndef NOT_USE_DYNAMIC_LOADING
@@ -274,19 +288,8 @@
 	
 	pause();
 
-	eprintf("Finishing.\n");
-    EnterMutex();
-	masterSendNMTstateChange (&TestMaster_Data, 0x00, NMT_Stop_Node);
-    LeaveMutex();
-
-	eprintf("reset\n");
-	// Stop master
-    EnterMutex();
-	setState(&TestMaster_Data, Stopped);
-    LeaveMutex();
-	
 	// Stop timer thread
-	StopTimerLoop();
+	StopTimerLoop(&Exit);
 	
 	// Close CAN devices (and can threads)
 	if(strcmp(MasterBoard.baudrate, "none")) canClose(&TestMaster_Data);	
@@ -295,5 +298,6 @@
 fail_slaveB:
 	if(strcmp(SlaveBoardA.baudrate, "none")) canClose(&TestSlaveA_Data);
 fail_slaveA:
+	TimerCleanup();
 	return 0;
 }

--- a/include/timers_driver.h	Tue Apr 29 13:54:23 2008 +0200
+++ b/include/timers_driver.h	Fri May 02 17:30:37 2008 +0200
@@ -33,8 +33,10 @@
 void WaitReceiveTaskEnd(TASK_HANDLE*);
 
 // For use from application
-void StartTimerLoop(TimerCallback_t init_callback);
-void StopTimerLoop(void);
+void TimerInit(void);
+void TimerCleanup(void);
+void StartTimerLoop(TimerCallback_t);
+void StopTimerLoop(TimerCallback_t);
 void CreateReceiveTask(CAN_PORT , TASK_HANDLE* , void* );
 
 #endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/include/timers_rtai/timerscfg.h	Fri May 02 17:30:37 2008 +0200
@@ -0,0 +1,38 @@
+/*
+This file is part of CanFestival, a library implementing CanOpen Stack. 
+
+Copyright (C): Edouard TISSERANT and Francis DUPIN
+
+See COPYING file for copyrights details.
+
+This library is free software; you can redistribute it and/or
+modify it under the terms of the GNU Lesser General Public
+License as published by the Free Software Foundation; either
+version 2.1 of the License, or (at your option) any later version.
+
+This library is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+Lesser General Public License for more details.
+
+You should have received a copy of the GNU Lesser General Public
+License along with this library; if not, write to the Free Software
+Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+*/
+
+#ifndef __TIMERSCFG_H__
+#define __TIMERSCFG_H__
+
+#include <pthread.h>
+#include <rtai_lxrt.h>
+
+// Time unit : RTAI's timers count, 64bit signed 
+#define TIMEVAL RTIME
+#define TIMEVAL_MAX ((long long)(~0ULL>>1))
+
+#define MS_TO_TIMEVAL(ms) nano2count((RTIME)ms*1000000)
+#define US_TO_TIMEVAL(us) nano2count((RTIME)us*1000)
+
+#define TASK_HANDLE pthread_t
+
+#endif

--- a/include/timers_xeno/timerscfg.h	Tue Apr 29 13:54:23 2008 +0200
+++ b/include/timers_xeno/timerscfg.h	Fri May 02 17:30:37 2008 +0200
@@ -26,14 +26,16 @@
 #include <native/task.h>
 #include <native/timer.h>
 #include <native/mutex.h>
+#include <native/cond.h>
+#include <native/sem.h>
 #include <native/alarm.h>
 
 // Time unit : ns
 // Time resolution : 64bit (~584 years)
 #define TIMEVAL RTIME
 #define TIMEVAL_MAX ~(RTIME)0
-#define MS_TO_TIMEVAL(ms) (RTIME)ms*1000000
-#define US_TO_TIMEVAL(us) (RTIME)us*1000
+#define MS_TO_TIMEVAL(ms)  rt_timer_ns2ticks((RTIME)ms*1000000)
+#define US_TO_TIMEVAL(us)  rt_timer_ns2ticks((RTIME)us*1000)
 
 #define TASK_HANDLE RT_TASK
 

--- a/include/unix/applicfg.h	Tue Apr 29 13:54:23 2008 +0200
+++ b/include/unix/applicfg.h	Fri May 02 17:30:37 2008 +0200
@@ -76,11 +76,15 @@
 
 /* Definition of error and warning macros */
 /* -------------------------------------- */
-#ifndef __KERNEL__
+#ifdef __KERNEL__
+#	define MSG(...) printk (__VA_ARGS__)
+#elif defined USE_RTAI
+#	define MSG(...) /*rt_printk (__VA_ARGS__)*/
+#elif defined USE_XENO
+#	define MSG(...)
+#else
 #	include <stdio.h>
 #	define MSG(...) printf (__VA_ARGS__)
-#else
-#	define MSG(...) printk (__VA_ARGS__)
 #endif
 
 /* Definition of MSG_ERR */