# HG changeset patch
# User Edouard Tisserant
# Date 1548334180 -3600
# Node ID 46027bb24429a6f8d0b49e0ebf1fc25aa6d0535f
# Parent  b9f1fcda7d30b75426c9e7dc5b7a7ef5fc238756
Rewrite of timer_xeno.c to use POSIX API instead of Alchemy.

diff -r b9f1fcda7d30 -r 46027bb24429 drivers/can_socket/can_socket.c
--- a/drivers/can_socket/can_socket.c	Wed May 09 13:16:00 2018 +0200
+++ b/drivers/can_socket/can_socket.c	Thu Jan 24 13:49:40 2019 +0100
@@ -54,16 +54,33 @@
 UNS8
 canReceive_driver (CAN_HANDLE fd0, Message * m)
 {
-  int res;
+  int res = 0;
   struct can_frame frame;
 
-  res = recv (*(int *) fd0, &frame, sizeof (frame), 0);
-  if (res <= 0)
-    {
-      fprintf (stderr, "Recv failed: %s\n", strerror (errno));
-      return 1;
-    }
-
+  while (res <= 0) {
+    res = recv (*(int *) fd0, &frame, sizeof (frame), 0);
+    if (res < 0)
+    {
+        if (errno == EBADF)
+        {
+            /* this will exit recieve loop */
+            return 1;
+        }
+        else if (errno == EINTR) continue; /* retry immediatly */
+        else
+        {
+            /* print error and retry after 20ms, arbitrary */
+            fprintf (stderr, "Recv failed: %s\n", strerror (errno));
+            usleep(20000);
+            continue;
+        }
+    }
+    else if (res == 0)
+    {
+        /* this will exit recieve loop */
+        return 1;
+    }
+  } 
   m->cob_id = frame.can_id & CAN_EFF_MASK;
   m->len = frame.can_dlc;
   if (frame.can_id & CAN_RTR_FLAG)
diff -r b9f1fcda7d30 -r 46027bb24429 drivers/timers_xeno/timers_xeno.c
--- a/drivers/timers_xeno/timers_xeno.c	Wed May 09 13:16:00 2018 +0200
+++ b/drivers/timers_xeno/timers_xeno.c	Thu Jan 24 13:49:40 2019 +0100
@@ -1,253 +1,282 @@
 #include <stdlib.h>
+
 #include <unistd.h>
-#include <sys/mman.h>
-
-#include <alchemy/task.h>
-#include <alchemy/timer.h>
-#include <alchemy/sem.h>
-#include <alchemy/mutex.h>
-#include <alchemy/cond.h>
-#include <alchemy/alarm.h>
-
-#include "applicfg.h"
-#include "can_driver.h"
-#include "timers.h"
-
-#define TIMERLOOP_TASK_CREATED        1
-
-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_timeout_set;
-
+#include <stdint.h>
+#include <sys/time.h>
+#include <pthread.h>
+#include <signal.h>
+#include <sys/timerfd.h>
+#include <time.h>
+
+#include <applicfg.h>
+#include <timers.h>
+
+static pthread_mutex_t CanFestival_mutex = PTHREAD_MUTEX_INITIALIZER;
+
+static pthread_mutex_t timer_mutex = PTHREAD_MUTEX_INITIALIZER;
+int tfd;
+static int timer_created = 0;
+struct timespec last_occured_alarm;
+struct timespec next_alarm;
+struct timespec time_ref;
+
+static pthread_t timer_thread; 
 int stop_timer = 0;
 
-/**
- * Init Mutex, Semaphores and Condition variable
- */
+/* Subtract the struct timespec values X and Y,
+   storing the result in RESULT.
+   Return 1 if the difference is negative, otherwise 0. */
+int
+timespec_subtract (struct timespec *result, struct timespec *x, struct timespec *y)
+{
+  /* Perform the carry for the later subtraction by updating y. */
+  if (x->tv_nsec < y->tv_nsec) {
+    int seconds = (y->tv_nsec - x->tv_nsec) / 1000000000L + 1;
+    y->tv_nsec -= 1000000000L * seconds;
+    y->tv_sec += seconds;
+  }
+  if (x->tv_nsec - y->tv_nsec > 1000000000L) {
+    int seconds = (x->tv_nsec - y->tv_nsec) / 1000000000L;
+    y->tv_nsec += 1000000000L * seconds;
+    y->tv_sec -= seconds;
+  }
+
+  /* Compute the time remaining to wait.
+     tv_nsec is certainly positive. */
+  result->tv_sec = x->tv_sec - y->tv_sec;
+  result->tv_nsec = x->tv_nsec - y->tv_nsec;
+
+  /* Return 1 if result is negative. */
+  return x->tv_sec < y->tv_sec;
+}
+
+void timespec_add (struct timespec *result, struct timespec *x, struct timespec *y)
+{
+  result->tv_sec = x->tv_sec + y->tv_sec;
+  result->tv_nsec = x->tv_nsec + y->tv_nsec;
+
+  while (result->tv_nsec > 1000000000L) {
+      result->tv_sec ++;
+      result->tv_nsec -= 1000000000L;
+  }
+}
+
+void TimerCleanup(void)
+{
+}
+
+void EnterTimerMutex(void)
+{
+	if(pthread_mutex_lock(&timer_mutex)) {
+		perror("pthread_mutex_lock(timer_mutex) failed\n");
+	}
+}
+
+void LeaveTimerMutex(void)
+{
+	if(pthread_mutex_unlock(&timer_mutex)) {
+		perror("pthread_mutex_unlock(timer_mutex) failed\n");
+	}
+}
+
+void EnterMutex(void)
+{
+	if(pthread_mutex_lock(&CanFestival_mutex)) {
+		perror("pthread_mutex_lock(CanFestival_mutex) failed\n");
+	}
+}
+
+void LeaveMutex(void)
+{
+	if(pthread_mutex_unlock(&CanFestival_mutex)) {
+		perror("pthread_mutex_unlock(CanFestival_mutex) failed\n");
+	}
+}
+
 void TimerInit(void)
 {
-  	int ret = 0;
-  	char taskname[32];
-
-	// lock process in to RAM
-  	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
- */
+    /* Initialize absolute time references */
+    if(clock_gettime(CLOCK_MONOTONIC, &time_ref)){
+        perror("clock_gettime(time_ref)");
+    }
+    next_alarm = last_occured_alarm = time_ref;
+}
+
 void StopTimerLoop(TimerCallback_t exitfunction)
 {
-	exitall = exitfunction;
+
 	stop_timer = 1;
-	rt_cond_signal(&timer_set);
-}
-
-
-/**
- * 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);
-	rt_task_unblock(&timerloop_task);
-	if (rt_task_join(&timerloop_task) != 0){
-		printf("Failed to join with Timerloop task\n");
-	}
-	rt_task_delete(&timerloop_task);
-}
-
-/**
- * Take a semaphore
- */
-void EnterMutex(void)
-{
-	rt_sem_p(&CanFestival_mutex, TM_INFINITE);
-}
-
-/**
- * Signaling a semaphore
- */
-void LeaveMutex(void)
-{
-	rt_sem_v(&CanFestival_mutex);
-}
-
-static TimerCallback_t init_callback;
-
-/**
- * Timer Task
- */
-void timerloop_task_proc(void *arg)
-{
-	int ret = 0;
-
-	getElapsedTime();
-	last_timeout_set = 0;
-	last_occured_alarm = last_time_read;
-	
-	/* trigger first alarm */
+
+    pthread_cancel(timer_thread);
+    pthread_join(timer_thread, NULL);
+
+	EnterMutex();
+	exitfunction(NULL,0);
+	LeaveMutex();
+}
+
+void* xenomai_timerLoop(void* unused)
+{
+    struct sched_param param = { .sched_priority = 80 };
+    int ret;
+
+    if (ret = pthread_setname_np(pthread_self(), "canfestival_timer")) {
+        fprintf(stderr, "pthread_setname_np(): %s\n",
+                strerror(-ret));
+        goto exit_timerLoop;
+    }
+    
+    if (ret = pthread_setschedparam(pthread_self(), SCHED_FIFO, &param)) {
+        fprintf(stderr, "pthread_setschedparam(): %s\n",
+                strerror(-ret));
+        goto exit_timerLoop;
+    }
+
+    EnterTimerMutex();
+
+    tfd = timerfd_create(CLOCK_MONOTONIC, 0);
+    if(tfd == -1) {
+		perror("timer_create() failed\n");
+        goto exit_timerLoop_timermutex;
+    }
+    timer_created = 1;
+
+    while (!stop_timer)
+    {
+        uint64_t ticks;
+
+        LeaveTimerMutex();
+
+        EnterMutex();
+        TimeDispatch();
+        LeaveMutex();
+
+        /*  wait next timer occurence */
+
+        ret = read(tfd, &ticks, sizeof(ticks));
+        if (ret < 0) {
+            perror("timerfd read()\n");
+            break;
+        }
+
+        EnterTimerMutex();
+
+        last_occured_alarm = next_alarm;
+    }
+
+    close(tfd);
+
+    timer_created = 0;
+
+exit_timerLoop_timermutex:
+    LeaveTimerMutex();
+
+exit_timerLoop:
+    return NULL;
+}
+
+
+void StartTimerLoop(TimerCallback_t init_callback)
+{
+    int ret;
+
+	stop_timer = 0;	
+
+	EnterMutex();
+	// At first, TimeDispatch will call init_callback.
 	SetAlarm(NULL, 0, init_callback, 0, 0);
-	RTIME current_time;
-	RTIME real_alarm;
-	do{
-		
-		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();
-	}
-}
-
-/**
- * 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());
-
-	/* create timerloop_task */
-	ret = rt_task_create(&timerloop_task, taskname, 0, 50, T_JOINABLE);
-	if (ret) {
-		printf("Failed to create timerloop_task, code %d\n",errno);
-		return;
-	}
- 	
-	/* start timerloop_task */
-	ret = rt_task_start(&timerloop_task,&timerloop_task_proc,NULL);
-	if (ret) {
-		printf("Failed to start timerloop_task, code %u\n",errno);
-		goto error;
-	}
-	
-	return;
-	
-error:
-	rt_task_delete(&timerloop_task);
-}
-
-/**
- * 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++;
-
-	/* create ReceiveLoop_task */
-	ret = rt_task_create(ReceiveLoop_task,taskname,0,50,T_JOINABLE);
-	if (ret) {
-		printf("Failed to create ReceiveLoop_task number %d, code %d\n", id, errno);
-		return;
-	}
-	/* 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;
-	}
-	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_unblock(ReceiveLoop_task);
-	if (rt_task_join(ReceiveLoop_task) != 0){
-		printf("Failed to join with Receive task\n");
-	}
-	rt_task_delete(ReceiveLoop_task);
-}
-
-/**
- * Set timer for the next wakeup
- * @param value
- */
+	LeaveMutex();
+
+    ret = pthread_create(&timer_thread, NULL, &xenomai_timerLoop, NULL);
+    if (ret) {
+        fprintf(stderr, "StartTimerLoop pthread_create(): %s\n",
+                strerror(-ret));
+    }
+}
+
+static void (*unixtimer_ReceiveLoop_task_proc)(CAN_PORT) = NULL;
+
+void* xenomai_canReceiveLoop(void* port)
+{
+    int ret;
+    struct sched_param param = { .sched_priority = 82 };
+    pthread_setname_np(pthread_self(), "canReceiveLoop");
+    pthread_setschedparam(pthread_self(), SCHED_FIFO, &param);
+
+    unixtimer_ReceiveLoop_task_proc((CAN_PORT)port);
+
+    return NULL;
+}
+
+void CreateReceiveTask(CAN_PORT port, TASK_HANDLE* Thread, void* ReceiveLoopPtr)
+{
+    unixtimer_ReceiveLoop_task_proc = ReceiveLoopPtr;
+
+	if(pthread_create(Thread, NULL, xenomai_canReceiveLoop, (void*)port)) {
+		perror("CreateReceiveTask pthread_create()");
+	}
+}
+
+void WaitReceiveTaskEnd(TASK_HANDLE *Thread)
+{
+	if(pthread_cancel(*Thread)) {
+		perror("pthread_cancel()");
+	}
+	if(pthread_join(*Thread, NULL)) {
+		perror("pthread_join()");
+	}
+}
+
+#define maxval(a,b) ((a>b)?a:b)
 void setTimer(TIMEVAL value)
 {
-	rt_mutex_acquire(&condition_mutex, TM_INFINITE);
-	last_timeout_set = value;
-	rt_mutex_release(&condition_mutex);
-	rt_cond_signal(&timer_set);
-}
-
-/**
- * Get the elapsed time since the last alarm
- * @return a time in nanoseconds
- */
+    struct itimerspec timerValues;
+
+    EnterTimerMutex();
+
+    if(timer_created){
+        long tv_nsec = (maxval(value,1)%1000000000LL);
+        time_t tv_sec = value/1000000000LL;
+        timerValues.it_value.tv_sec = tv_sec;
+        timerValues.it_value.tv_nsec = tv_nsec;
+        timerValues.it_interval.tv_sec = 0;
+        timerValues.it_interval.tv_nsec = 0;
+
+        /* keep track of when should alarm occur*/
+        timespec_add(&next_alarm, &time_ref, &timerValues.it_value);
+        timerfd_settime (tfd, 0, &timerValues, NULL);
+    }
+
+    LeaveTimerMutex();
+}
+
+
 TIMEVAL getElapsedTime(void)
 {
-	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);
+    struct itimerspec outTimerValues;
+    struct timespec ts;
+    struct timespec last_occured_alarm_copy;
+
+    TIMEVAL res = 0;
+
+    EnterTimerMutex();
+
+    if(timer_created){
+        if(clock_gettime(CLOCK_MONOTONIC, &time_ref)){
+            perror("clock_gettime(ts)");
+
+        }
+        /* timespec_substract modifies second operand */
+        last_occured_alarm_copy = last_occured_alarm;
+
+        timespec_subtract(&ts, &time_ref, &last_occured_alarm_copy);
+
+        /* TIMEVAL is nano seconds */
+        res = (ts.tv_sec * 1000000000L) + ts.tv_nsec;
+    }
+
+    LeaveTimerMutex();
+
 	return res;
 }
+
diff -r b9f1fcda7d30 -r 46027bb24429 include/timers_xeno/timerscfg.h
--- a/include/timers_xeno/timerscfg.h	Wed May 09 13:16:00 2018 +0200
+++ b/include/timers_xeno/timerscfg.h	Thu Jan 24 13:49:40 2019 +0100
@@ -23,20 +23,15 @@
 #ifndef __TIMERSCFG_H__
 #define __TIMERSCFG_H__
 
-#include <alchemy/task.h>
-#include <alchemy/timer.h>
-#include <alchemy/mutex.h>
-#include <alchemy/cond.h>
-#include <alchemy/sem.h>
-#include <alchemy/alarm.h>
+#include <pthread.h>
 
 // Time unit : ns
 // Time resolution : 64bit (~584 years)
-#define TIMEVAL RTIME
-#define TIMEVAL_MAX ~(RTIME)0
-#define MS_TO_TIMEVAL(ms)  rt_timer_ns2ticks((RTIME)ms*1000000)
-#define US_TO_TIMEVAL(us)  rt_timer_ns2ticks((RTIME)us*1000)
+#define TIMEVAL unsigned long long
+#define TIMEVAL_MAX ~(TIMEVAL)0
+#define MS_TO_TIMEVAL(ms) ms*1000000LL
+#define US_TO_TIMEVAL(us) us*1000LL
 
-#define TASK_HANDLE RT_TASK
+#define TASK_HANDLE pthread_t
 
 #endif
diff -r b9f1fcda7d30 -r 46027bb24429 include/unix/applicfg.h
--- a/include/unix/applicfg.h	Wed May 09 13:16:00 2018 +0200
+++ b/include/unix/applicfg.h	Thu Jan 24 13:49:40 2019 +0100
@@ -78,10 +78,6 @@
 /* -------------------------------------- */
 #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__)