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