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