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 }
+