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/sem.h>
   8 #include <native/mutex.h>
   9 #include <native/cond.h>
  10 #include <native/alarm.h>
  11
  12 #include "applicfg.h"
  13 #include "can_driver.h"
  14 #include "timer.h"
  15
  16 #define TIMERLOOP_TASK_CREATED        1
  17
  18 TimerCallback_t exitall;
  19
  20 RT_MUTEX condition_mutex;
  21 RT_SEM CanFestival_mutex;
  22 RT_SEM control_task;
  23 RT_COND timer_set;
  24 RT_TASK timerloop_task;
  25
  26 RTIME last_time_read;
  27 RTIME last_occured_alarm;
  28 RTIME last_timeout_set;
  29
  30 int stop_timer = 0;
  31
  32 /**
  33  * Init Mutex, Semaphores and Condition variable
  34  */
  35 void TimerInit(void)
  36 {
  37         int ret = 0;
  38         char taskname[32];
  39
  40         mlockall(MCL_CURRENT | MCL_FUTURE);
  41
  42         snprintf(taskname, sizeof(taskname), "S1-%d", getpid());
  43         rt_sem_create(&CanFestival_mutex, taskname, 1, S_FIFO);
  44
  45         snprintf(taskname, sizeof(taskname), "S2-%d", getpid());
  46         rt_sem_create(&control_task, taskname, 0, S_FIFO);
  47
  48         snprintf(taskname, sizeof(taskname), "M1-%d", getpid());
  49         rt_mutex_create(&condition_mutex, taskname);
  50
  51         snprintf(taskname, sizeof(taskname), "C1-%d", getpid());
  52         rt_cond_create(&timer_set, taskname);
  53 }
  54
  55 /**
  56  * Stop Timer Task
  57  * @param exitfunction
  58  */
  59 void StopTimerLoop(TimerCallback_t exitfunction)
  60 {
  61         exitall = exitfunction;
  62         stop_timer = 1;
  63         rt_cond_signal(&timer_set);
  64 }
  65
  66 void cleanup_all(void)
  67 {
  68         rt_task_delete(&timerloop_task);
  69 }
  70
  71 /**
  72  * Clean all Semaphores, mutex, condition variable and main task
  73  */
  74 void TimerCleanup(void)
  75 {
  76         rt_sem_delete(&CanFestival_mutex);
  77         rt_mutex_delete(&condition_mutex);
  78         rt_cond_delete(&timer_set);
  79         rt_sem_delete(&control_task);
  80 }
  81
  82 /**
  83  * Take a semaphore
  84  */
  85 void EnterMutex(void)
  86 {
  87         rt_sem_p(&CanFestival_mutex, TM_INFINITE);
  88 }
  89
  90 /**
  91  * Signaling a semaphore
  92  */
  93 void LeaveMutex(void)
  94 {
  95         rt_sem_v(&CanFestival_mutex);
  96 }
  97
  98 static TimerCallback_t init_callback;
  99
 100 /**
 101  * Timer Task
 102  */
 103 void timerloop_task_proc(void *arg)
 104 {
 105         int ret = 0;
 106         // lock process in to RAM
 107         mlockall(MCL_CURRENT | MCL_FUTURE);
 108
 109         getElapsedTime();
 110         last_timeout_set = 0;
 111         last_occured_alarm = last_time_read;
 112
 113         /* trigger first alarm */
 114         SetAlarm(NULL, 0, init_callback, 0, 0);
 115         RTIME current_time;
 116         RTIME real_alarm;
 117         do{
 118
 119                 rt_mutex_acquire(&condition_mutex, TM_INFINITE);
 120                 if(last_timeout_set == TIMEVAL_MAX)
 121                 {
 122                         ret = rt_cond_wait(
 123                                 &timer_set,
 124                                 &condition_mutex,
 125                                 TM_INFINITE
 126                                 );              /* Then sleep until next message*/
 127                         rt_mutex_release(&condition_mutex);
 128                 }else{
 129                         current_time = rt_timer_read();
 130                         real_alarm = last_time_read + last_timeout_set;
 131                         ret = rt_cond_wait( /* sleep until next deadline */
 132                                 &timer_set,
 133                                 &condition_mutex,
 134                                 (real_alarm - current_time)); /* else alarm consider expired */
 135                         if(ret == -ETIMEDOUT){
 136                                 last_occured_alarm = real_alarm;
 137                                 rt_mutex_release(&condition_mutex);
 138                                 EnterMutex();
 139                                 TimeDispatch();
 140                                 LeaveMutex();
 141                         }else{
 142                                 rt_mutex_release(&condition_mutex);
 143                         }
 144                 }
 145         }while ((ret == 0 || ret == -EINTR || ret == -ETIMEDOUT) && !stop_timer);
 146
 147         if(exitall){
 148                 EnterMutex();
 149                 exitall(NULL,0);
 150                 LeaveMutex();
 151         }
 152
 153         rt_task_delete(&timerloop_task);
 154 }
 155
 156 /**
 157  * Create the Timer Task
 158  * @param _init_callback
 159  */
 160 void StartTimerLoop(TimerCallback_t _init_callback)
 161 {
 162         int ret = 0;
 163         stop_timer = 0;
 164         init_callback = _init_callback;
 165
 166         char taskname[32];
 167         snprintf(taskname, sizeof(taskname), "timerloop-%d", getpid());
 168
 169         /* create timerloop_task */
 170         ret = rt_task_create(&timerloop_task, taskname, 0, 50, 0);
 171         if (ret) {
 172                 printf("Failed to create timerloop_task, code %d\n",errno);
 173                 return;
 174         }
 175
 176         /* start timerloop_task */
 177         ret = rt_task_start(&timerloop_task,&timerloop_task_proc,NULL);
 178         if (ret) {
 179                 printf("Failed to start timerloop_task, code %u\n",errno);
 180                 goto error;
 181         }
 182
 183         return;
 184
 185 error:
 186         cleanup_all();
 187 }
 188
 189 /**
 190  * Create the CAN Receiver Task
 191  * @param fd0 CAN port
 192  * @param *ReceiveLoop_task CAN receiver task
 193  * @param *ReceiveLoop_task_proc CAN receiver function
 194  */
 195 void CreateReceiveTask(CAN_PORT fd0, TASK_HANDLE *ReceiveLoop_task, void* ReceiveLoop_task_proc)
 196 {
 197         int ret;
 198         static int id = 0;
 199         char taskname[32];
 200         snprintf(taskname, sizeof(taskname), "canloop%d-%d", id, getpid());
 201         id++;
 202
 203         /* create ReceiveLoop_task */
 204         ret = rt_task_create(ReceiveLoop_task,taskname,0,50,0);
 205         if (ret) {
 206                 printf("Failed to create ReceiveLoop_task number %d, code %d\n", id, errno);
 207                 return;
 208         }
 209         /* start ReceiveLoop_task */
 210         ret = rt_task_start(ReceiveLoop_task, ReceiveLoop_task_proc,(void*)fd0);
 211         if (ret) {
 212                 printf("Failed to start ReceiveLoop_task number %d, code %d\n", id, errno);
 213                 return;
 214         }
 215         rt_sem_v(&control_task);
 216 }
 217
 218 /**
 219  * Wait for the CAN Receiver Task end
 220  * @param *ReceiveLoop_task CAN receiver thread
 221  */
 222 void WaitReceiveTaskEnd(TASK_HANDLE *ReceiveLoop_task)
 223 {
 224         rt_task_delete(ReceiveLoop_task);
 225 }
 226
 227 /**
 228  * Set timer for the next wakeup
 229  * @param value
 230  */
 231 void setTimer(TIMEVAL value)
 232 {
 233         rt_mutex_acquire(&condition_mutex, TM_INFINITE);
 234         last_timeout_set = (value == TIMEVAL_MAX) ? TIMEVAL_MAX : value;
 235         rt_mutex_release(&condition_mutex);
 236         rt_cond_signal(&timer_set);
 237 }
 238
 239 /**
 240  * Get the elapsed time since the last alarm
 241  * @return a time in nanoseconds
 242  */
 243 TIMEVAL getElapsedTime(void)
 244 {
 245         RTIME res;
 246         rt_mutex_acquire(&condition_mutex, TM_INFINITE);
 247         last_time_read = rt_timer_read();
 248         res = last_time_read - last_occured_alarm;
 249         rt_mutex_release(&condition_mutex);
 250         return res;
 251 }