drivers/timers_xeno/timers_xeno.c

   1 #include <stdlib.h>
   2 #include <unistd.h>
   3 #include <sys/mman.h>
   4
   5 #include <native/task.h>
   6 #include <native/timer.h>
   7 #include <native/mutex.h>
   8 #include <native/alarm.h>
   9
  10 #include "applicfg.h"
  11 #include "can_driver.h"
  12 #include "timer.h"
  13
  14 #define TIMERLOOP_TASK_CREATED        1
  15
  16 RT_MUTEX CanFestival_mutex;
  17 RT_TASK timerloop_task;
  18 RTIME last_time_read;
  19 RTIME last_occured_alarm;
  20 RTIME last_alarm_set;
  21
  22 char stop_timer=0;
  23
  24 void cleanup_all(void)
  25 {
  26         rt_task_delete(&timerloop_task);
  27 }
  28 void StopTimerLoop(void)
  29 {
  30         stop_timer = 1;
  31         rt_task_unblock(&timerloop_task);
  32 }
  33
  34
  35 void EnterMutex(void)
  36 {
  37         rt_mutex_lock(&CanFestival_mutex, TM_INFINITE);
  38 }
  39
  40 void LeaveMutex(void)
  41 {
  42         rt_mutex_unlock(&CanFestival_mutex);
  43 }
  44
  45 void timerloop_task_proc(void *arg)
  46 {
  47         int ret;
  48         do{
  49                 do{
  50                         last_occured_alarm = last_alarm_set;
  51                         EnterMutex();
  52                         TimeDispatch();
  53                         LeaveMutex();
  54                         while ((ret = rt_task_sleep_until(last_alarm_set)) == -EINTR);
  55                 }while (ret == 0);
  56         }while (!stop_timer);
  57         printf("End of TimerLoop, code %d\n",ret);
  58 }
  59
  60 void StartTimerLoop(TimerCallback_t init_callback)
  61 {
  62         int ret;
  63         stop_timer = 0;
  64         char taskname[32];
  65         snprintf(taskname, sizeof(taskname), "timerloop-%d", getpid());
  66
  67         mlockall(MCL_CURRENT | MCL_FUTURE);
  68
  69         //create timerloop_task
  70         ret = rt_task_create(&timerloop_task, taskname, 0, 50, 0);
  71         if (ret) {
  72                 printf("Failed to create timerloop_task, code %d\n",errno);
  73                 return;
  74         }
  75
  76         getElapsedTime();
  77         last_alarm_set = last_time_read;
  78         last_occured_alarm = last_alarm_set;
  79         SetAlarm(NULL, 0, init_callback, 0, 0);
  80         // start timerloop_task
  81         ret = rt_task_start(&timerloop_task,&timerloop_task_proc,NULL);
  82         if (ret) {
  83                 printf("Failed to start timerloop_task, code %d\n",errno);
  84                 goto error;
  85         }
  86
  87         return;
  88
  89 error:
  90         cleanup_all();
  91 }
  92
  93 void CreateReceiveTask(CAN_PORT fd0, TASK_HANDLE *ReceiveLoop_task, void* ReceiveLoop_task_proc)
  94 {
  95         int ret;
  96         static int id = 0;
  97         char taskname[32];
  98         snprintf(taskname, sizeof(taskname), "canloop%d-%d", id, getpid());
  99         id++;
 100
 101         mlockall(MCL_CURRENT | MCL_FUTURE);
 102
 103         //create timerloop_task
 104         ret = rt_task_create(ReceiveLoop_task,taskname,0,50,0);
 105         if (ret) {
 106                 printf("Failed to create ReceiveLoop_task number %d, code %d\n", id, errno);
 107                 return;
 108         }
 109         // start timerloop_task
 110         ret = rt_task_start(ReceiveLoop_task,ReceiveLoop_task_proc,(void*)fd0);
 111         if (ret) {
 112                 printf("Failed to start ReceiveLoop_task number %d, code %d\n", id, errno);
 113                 return;
 114         }
 115 }
 116
 117 void WaitReceiveTaskEnd(TASK_HANDLE *Thread)
 118 {
 119         rt_task_delete(Thread);
 120 }
 121
 122 void setTimer(TIMEVAL value)
 123 {
 124         last_alarm_set = (value == TIMEVAL_MAX) ? TIMEVAL_MAX : last_time_read + value;
 125         rt_task_unblock(&timerloop_task);
 126 }
 127
 128 TIMEVAL getElapsedTime(void)
 129 {
 130         last_time_read = rt_timer_ticks2ns(rt_timer_read());
 131         return last_time_read - last_occured_alarm;
 132 }