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