nico@215: nico@215:
nico@215:00001 #include <stdlib.h> nico@215: 00002 #include <unistd.h> nico@215: 00003 #include <sys/mman.h> nico@215: 00004 nico@215: 00005 #include <native/task.h> nico@215: 00006 #include <native/timer.h> nico@215: 00007 #include <native/mutex.h> nico@215: 00008 #include <native/alarm.h> nico@215: 00009 nico@215: 00010 #include "applicfg.h" nico@215: 00011 #include "can_driver.h" nico@215: 00012 #include "timer.h" nico@215: 00013 etisserant@240: 00014 #define TIMERLOOP_TASK_CREATED 1 nico@215: 00015 etisserant@240: 00016 RT_MUTEX CanFestival_mutex; etisserant@240: 00017 RT_TASK timerloop_task; etisserant@240: 00018 RTIME last_time_read; etisserant@240: 00019 RTIME last_occured_alarm; etisserant@240: 00020 RTIME last_alarm_set; nico@215: 00021 etisserant@240: 00022 char stop_timer=0; nico@215: 00023 etisserant@240: 00024 void cleanup_all(void) nico@215: 00025 { etisserant@240: 00026 rt_task_delete(&timerloop_task); nico@215: 00027 } etisserant@240: 00028 void StopTimerLoop(void) nico@215: 00029 { etisserant@240: 00030 stop_timer = 1; etisserant@240: 00031 rt_task_unblock(&timerloop_task); nico@215: 00032 } nico@215: 00033 nico@215: 00034 etisserant@240: 00035 void EnterMutex(void) nico@215: 00036 { etisserant@240: 00037 rt_mutex_lock(&CanFestival_mutex, TM_INFINITE); nico@215: 00038 } nico@215: 00039 etisserant@240: 00040 void LeaveMutex(void) nico@215: 00041 { etisserant@240: 00042 rt_mutex_unlock(&CanFestival_mutex); nico@215: 00043 } nico@215: 00044 etisserant@240: 00045 void timerloop_task_proc(void *arg) nico@215: 00046 { nico@215: 00047 int ret; nico@215: 00048 do{ nico@215: 00049 do{ etisserant@240: 00050 last_occured_alarm = last_alarm_set; etisserant@240: 00051 EnterMutex(); etisserant@240: 00052 TimeDispatch(); etisserant@240: 00053 LeaveMutex(); etisserant@240: 00054 while ((ret = rt_task_sleep_until(last_alarm_set)) == -EINTR); nico@215: 00055 }while (ret == 0); etisserant@240: 00056 }while (!stop_timer); nico@215: 00057 printf("End of TimerLoop, code %d\n",ret); nico@215: 00058 } nico@215: 00059 etisserant@240: 00060 void StartTimerLoop(TimerCallback_t init_callback) nico@215: 00061 { nico@215: 00062 int ret; etisserant@240: 00063 stop_timer = 0; nico@215: 00064 char taskname[32]; nico@215: 00065 snprintf(taskname, sizeof(taskname), "timerloop-%d", getpid()); nico@215: 00066 nico@215: 00067 mlockall(MCL_CURRENT | MCL_FUTURE); nico@215: 00068 nico@215: 00069 //create timerloop_task etisserant@240: 00070 ret = rt_task_create(&timerloop_task, taskname, 0, 50, 0); nico@215: 00071 if (ret) { nico@215: 00072 printf("Failed to create timerloop_task, code %d\n",errno); nico@215: 00073 return; nico@215: 00074 } nico@215: 00075 etisserant@240: 00076 getElapsedTime(); etisserant@240: 00077 last_alarm_set = last_time_read; etisserant@240: 00078 last_occured_alarm = last_alarm_set; etisserant@240: 00079 SetAlarm(NULL, 0, init_callback, 0, 0); nico@215: 00080 // start timerloop_task etisserant@240: 00081 ret = rt_task_start(&timerloop_task,&timerloop_task_proc,NULL); nico@215: 00082 if (ret) { nico@215: 00083 printf("Failed to start timerloop_task, code %d\n",errno); nico@215: 00084 goto error; nico@215: 00085 } nico@215: 00086 nico@215: 00087 return; nico@215: 00088 nico@215: 00089 error: etisserant@240: 00090 cleanup_all(); nico@215: 00091 } nico@215: 00092 etisserant@240: 00093 void CreateReceiveTask(CAN_PORT fd0, TASK_HANDLE *ReceiveLoop_task, void* ReceiveLoop_task_proc) nico@215: 00094 { nico@215: 00095 int ret; nico@215: 00096 static int id = 0; nico@215: 00097 char taskname[32]; nico@215: 00098 snprintf(taskname, sizeof(taskname), "canloop%d-%d", id, getpid()); nico@215: 00099 id++; nico@215: 00100 nico@215: 00101 mlockall(MCL_CURRENT | MCL_FUTURE); nico@215: 00102 nico@215: 00103 //create timerloop_task nico@215: 00104 ret = rt_task_create(ReceiveLoop_task,taskname,0,50,0); nico@215: 00105 if (ret) { nico@215: 00106 printf("Failed to create ReceiveLoop_task number %d, code %d\n", id, errno); nico@215: 00107 return; nico@215: 00108 } nico@215: 00109 // start timerloop_task nico@215: 00110 ret = rt_task_start(ReceiveLoop_task,ReceiveLoop_task_proc,(void*)fd0); nico@215: 00111 if (ret) { nico@215: 00112 printf("Failed to start ReceiveLoop_task number %d, code %d\n", id, errno); nico@215: 00113 return; nico@215: 00114 } nico@215: 00115 } nico@215: 00116 etisserant@240: 00117 void WaitReceiveTaskEnd(TASK_HANDLE *Thread) nico@215: 00118 { nico@215: 00119 rt_task_delete(Thread); nico@215: 00120 } nico@215: 00121 etisserant@240: 00122 void setTimer(TIMEVAL value) nico@215: 00123 { etisserant@240: 00124 last_alarm_set = (value == TIMEVAL_MAX) ? TIMEVAL_MAX : last_time_read + value; etisserant@240: 00125 rt_task_unblock(&timerloop_task); nico@215: 00126 } nico@215: 00127 etisserant@240: 00128 TIMEVAL getElapsedTime(void) nico@215: 00129 { etisserant@240: 00130 last_time_read = rt_timer_ticks2ns(rt_timer_read()); etisserant@240: 00131 return last_time_read - last_occured_alarm; nico@215: 00132 } etisserant@240: