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