--- /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;
+}