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