etisserant@0: #include <stdlib.h>
Edouard@807: 
etisserant@47: #include <unistd.h>
Edouard@807: #include <stdint.h>
Edouard@807: #include <sys/time.h>
Edouard@807: #include <pthread.h>
Edouard@807: #include <signal.h>
Edouard@807: #include <sys/timerfd.h>
Edouard@807: #include <time.h>
Edouard@807: 
Edouard@807: #include <applicfg.h>
Edouard@807: #include <timers.h>
Edouard@807: 
Edouard@807: static pthread_mutex_t CanFestival_mutex = PTHREAD_MUTEX_INITIALIZER;
Edouard@807: 
Edouard@807: static pthread_mutex_t timer_mutex = PTHREAD_MUTEX_INITIALIZER;
Edouard@807: int tfd;
Edouard@807: static int timer_created = 0;
Edouard@807: struct timespec last_occured_alarm;
Edouard@807: struct timespec next_alarm;
Edouard@807: struct timespec time_ref;
Edouard@807: 
Edouard@807: static pthread_t timer_thread; 
greg@454: int stop_timer = 0;
greg@454: 
Edouard@807: /* Subtract the struct timespec values X and Y,
Edouard@807:    storing the result in RESULT.
Edouard@807:    Return 1 if the difference is negative, otherwise 0. */
Edouard@807: int
Edouard@807: timespec_subtract (struct timespec *result, struct timespec *x, struct timespec *y)
Edouard@807: {
Edouard@807:   /* Perform the carry for the later subtraction by updating y. */
Edouard@807:   if (x->tv_nsec < y->tv_nsec) {
Edouard@807:     int seconds = (y->tv_nsec - x->tv_nsec) / 1000000000L + 1;
Edouard@807:     y->tv_nsec -= 1000000000L * seconds;
Edouard@807:     y->tv_sec += seconds;
Edouard@807:   }
Edouard@807:   if (x->tv_nsec - y->tv_nsec > 1000000000L) {
Edouard@807:     int seconds = (x->tv_nsec - y->tv_nsec) / 1000000000L;
Edouard@807:     y->tv_nsec += 1000000000L * seconds;
Edouard@807:     y->tv_sec -= seconds;
Edouard@807:   }
Edouard@807: 
Edouard@807:   /* Compute the time remaining to wait.
Edouard@807:      tv_nsec is certainly positive. */
Edouard@807:   result->tv_sec = x->tv_sec - y->tv_sec;
Edouard@807:   result->tv_nsec = x->tv_nsec - y->tv_nsec;
Edouard@807: 
Edouard@807:   /* Return 1 if result is negative. */
Edouard@807:   return x->tv_sec < y->tv_sec;
Edouard@807: }
Edouard@807: 
Edouard@807: void timespec_add (struct timespec *result, struct timespec *x, struct timespec *y)
Edouard@807: {
Edouard@807:   result->tv_sec = x->tv_sec + y->tv_sec;
Edouard@807:   result->tv_nsec = x->tv_nsec + y->tv_nsec;
Edouard@807: 
Edouard@807:   while (result->tv_nsec > 1000000000L) {
Edouard@807:       result->tv_sec ++;
Edouard@807:       result->tv_nsec -= 1000000000L;
Edouard@807:   }
Edouard@807: }
Edouard@807: 
Edouard@807: void TimerCleanup(void)
Edouard@807: {
Edouard@807: }
Edouard@807: 
Edouard@807: void EnterTimerMutex(void)
Edouard@807: {
Edouard@807: 	if(pthread_mutex_lock(&timer_mutex)) {
Edouard@807: 		perror("pthread_mutex_lock(timer_mutex) failed\n");
Edouard@807: 	}
Edouard@807: }
Edouard@807: 
Edouard@807: void LeaveTimerMutex(void)
Edouard@807: {
Edouard@807: 	if(pthread_mutex_unlock(&timer_mutex)) {
Edouard@807: 		perror("pthread_mutex_unlock(timer_mutex) failed\n");
Edouard@807: 	}
Edouard@807: }
Edouard@807: 
Edouard@807: void EnterMutex(void)
Edouard@807: {
Edouard@807: 	if(pthread_mutex_lock(&CanFestival_mutex)) {
Edouard@807: 		perror("pthread_mutex_lock(CanFestival_mutex) failed\n");
Edouard@807: 	}
Edouard@807: }
Edouard@807: 
Edouard@807: void LeaveMutex(void)
Edouard@807: {
Edouard@807: 	if(pthread_mutex_unlock(&CanFestival_mutex)) {
Edouard@807: 		perror("pthread_mutex_unlock(CanFestival_mutex) failed\n");
Edouard@807: 	}
Edouard@807: }
Edouard@807: 
greg@454: void TimerInit(void)
greg@454: {
Edouard@807:     /* Initialize absolute time references */
Edouard@807:     if(clock_gettime(CLOCK_MONOTONIC, &time_ref)){
Edouard@807:         perror("clock_gettime(time_ref)");
Edouard@807:     }
Edouard@807:     next_alarm = last_occured_alarm = time_ref;
Edouard@807: }
Edouard@807: 
greg@454: void StopTimerLoop(TimerCallback_t exitfunction)
greg@454: {
Edouard@807: 
greg@454: 	stop_timer = 1;
Edouard@807: 
Edouard@807:     pthread_cancel(timer_thread);
Edouard@807:     pthread_join(timer_thread, NULL);
Edouard@807: 
Edouard@807: 	EnterMutex();
Edouard@807: 	exitfunction(NULL,0);
Edouard@807: 	LeaveMutex();
Edouard@807: }
Edouard@807: 
Edouard@807: void* xenomai_timerLoop(void* unused)
Edouard@807: {
Edouard@807:     struct sched_param param = { .sched_priority = 80 };
Edouard@807:     int ret;
Edouard@807: 
Edouard@807:     if (ret = pthread_setname_np(pthread_self(), "canfestival_timer")) {
Edouard@807:         fprintf(stderr, "pthread_setname_np(): %s\n",
Edouard@807:                 strerror(-ret));
Edouard@807:         goto exit_timerLoop;
Edouard@807:     }
Edouard@807:     
Edouard@807:     if (ret = pthread_setschedparam(pthread_self(), SCHED_FIFO, &param)) {
Edouard@807:         fprintf(stderr, "pthread_setschedparam(): %s\n",
Edouard@807:                 strerror(-ret));
Edouard@807:         goto exit_timerLoop;
Edouard@807:     }
Edouard@807: 
Edouard@807:     EnterTimerMutex();
Edouard@807: 
Edouard@807:     tfd = timerfd_create(CLOCK_MONOTONIC, 0);
Edouard@807:     if(tfd == -1) {
Edouard@807: 		perror("timer_create() failed\n");
Edouard@807:         goto exit_timerLoop_timermutex;
Edouard@807:     }
Edouard@807:     timer_created = 1;
Edouard@807: 
Edouard@807:     while (!stop_timer)
Edouard@807:     {
Edouard@807:         uint64_t ticks;
Edouard@807: 
Edouard@807:         LeaveTimerMutex();
Edouard@807: 
Edouard@807:         EnterMutex();
Edouard@807:         TimeDispatch();
Edouard@807:         LeaveMutex();
Edouard@807: 
Edouard@807:         /*  wait next timer occurence */
Edouard@807: 
Edouard@807:         ret = read(tfd, &ticks, sizeof(ticks));
Edouard@807:         if (ret < 0) {
Edouard@807:             perror("timerfd read()\n");
Edouard@807:             break;
Edouard@807:         }
Edouard@807: 
Edouard@807:         EnterTimerMutex();
Edouard@807: 
Edouard@807:         last_occured_alarm = next_alarm;
Edouard@807:     }
Edouard@807: 
Edouard@807:     close(tfd);
Edouard@807: 
Edouard@807:     timer_created = 0;
Edouard@807: 
Edouard@807: exit_timerLoop_timermutex:
Edouard@807:     LeaveTimerMutex();
Edouard@807: 
Edouard@807: exit_timerLoop:
Edouard@807:     return NULL;
Edouard@807: }
Edouard@807: 
Edouard@807: 
Edouard@807: void StartTimerLoop(TimerCallback_t init_callback)
Edouard@807: {
Edouard@807:     int ret;
Edouard@807: 
Edouard@807: 	stop_timer = 0;	
Edouard@807: 
Edouard@807: 	EnterMutex();
Edouard@807: 	// At first, TimeDispatch will call init_callback.
greg@454: 	SetAlarm(NULL, 0, init_callback, 0, 0);
Edouard@807: 	LeaveMutex();
Edouard@807: 
Edouard@807:     ret = pthread_create(&timer_thread, NULL, &xenomai_timerLoop, NULL);
Edouard@807:     if (ret) {
Edouard@807:         fprintf(stderr, "StartTimerLoop pthread_create(): %s\n",
Edouard@807:                 strerror(-ret));
Edouard@807:     }
Edouard@807: }
Edouard@807: 
Edouard@807: static void (*unixtimer_ReceiveLoop_task_proc)(CAN_PORT) = NULL;
Edouard@807: 
Edouard@807: void* xenomai_canReceiveLoop(void* port)
Edouard@807: {
Edouard@807:     int ret;
Edouard@807:     struct sched_param param = { .sched_priority = 82 };
Edouard@807:     pthread_setname_np(pthread_self(), "canReceiveLoop");
Edouard@807:     pthread_setschedparam(pthread_self(), SCHED_FIFO, &param);
Edouard@807: 
Edouard@807:     unixtimer_ReceiveLoop_task_proc((CAN_PORT)port);
Edouard@807: 
Edouard@807:     return NULL;
Edouard@807: }
Edouard@807: 
Edouard@807: void CreateReceiveTask(CAN_PORT port, TASK_HANDLE* Thread, void* ReceiveLoopPtr)
Edouard@807: {
Edouard@807:     unixtimer_ReceiveLoop_task_proc = ReceiveLoopPtr;
Edouard@807: 
Edouard@807: 	if(pthread_create(Thread, NULL, xenomai_canReceiveLoop, (void*)port)) {
Edouard@807: 		perror("CreateReceiveTask pthread_create()");
Edouard@807: 	}
Edouard@807: }
Edouard@807: 
Edouard@807: void WaitReceiveTaskEnd(TASK_HANDLE *Thread)
Edouard@807: {
Edouard@807: 	if(pthread_cancel(*Thread)) {
Edouard@807: 		perror("pthread_cancel()");
Edouard@807: 	}
Edouard@807: 	if(pthread_join(*Thread, NULL)) {
Edouard@807: 		perror("pthread_join()");
Edouard@807: 	}
Edouard@807: }
Edouard@807: 
Edouard@807: #define maxval(a,b) ((a>b)?a:b)
etisserant@0: void setTimer(TIMEVAL value)
etisserant@47: {
Edouard@807:     struct itimerspec timerValues;
Edouard@807: 
Edouard@807:     EnterTimerMutex();
Edouard@807: 
Edouard@807:     if(timer_created){
Edouard@807:         long tv_nsec = (maxval(value,1)%1000000000LL);
Edouard@807:         time_t tv_sec = value/1000000000LL;
Edouard@807:         timerValues.it_value.tv_sec = tv_sec;
Edouard@807:         timerValues.it_value.tv_nsec = tv_nsec;
Edouard@807:         timerValues.it_interval.tv_sec = 0;
Edouard@807:         timerValues.it_interval.tv_nsec = 0;
Edouard@807: 
Edouard@807:         /* keep track of when should alarm occur*/
Edouard@807:         timespec_add(&next_alarm, &time_ref, &timerValues.it_value);
Edouard@807:         timerfd_settime (tfd, 0, &timerValues, NULL);
Edouard@807:     }
Edouard@807: 
Edouard@807:     LeaveTimerMutex();
Edouard@807: }
Edouard@807: 
Edouard@807: 
etisserant@0: TIMEVAL getElapsedTime(void)
etisserant@0: {
Edouard@807:     struct itimerspec outTimerValues;
Edouard@807:     struct timespec ts;
Edouard@807:     struct timespec last_occured_alarm_copy;
Edouard@807: 
Edouard@807:     TIMEVAL res = 0;
Edouard@807: 
Edouard@807:     EnterTimerMutex();
Edouard@807: 
Edouard@807:     if(timer_created){
Edouard@807:         if(clock_gettime(CLOCK_MONOTONIC, &time_ref)){
Edouard@807:             perror("clock_gettime(ts)");
Edouard@807: 
Edouard@807:         }
Edouard@807:         /* timespec_substract modifies second operand */
Edouard@807:         last_occured_alarm_copy = last_occured_alarm;
Edouard@807: 
Edouard@807:         timespec_subtract(&ts, &time_ref, &last_occured_alarm_copy);
Edouard@807: 
Edouard@807:         /* TIMEVAL is nano seconds */
Edouard@807:         res = (ts.tv_sec * 1000000000L) + ts.tv_nsec;
Edouard@807:     }
Edouard@807: 
Edouard@807:     LeaveTimerMutex();
Edouard@807: 
greg@454: 	return res;
greg@454: }
Edouard@807: