1 #include <stdlib.h> |
1 #include <stdlib.h> |
|
2 |
2 #include <unistd.h> |
3 #include <unistd.h> |
3 #include <sys/mman.h> |
4 #include <stdint.h> |
4 |
5 #include <sys/time.h> |
5 #include <alchemy/task.h> |
6 #include <pthread.h> |
6 #include <alchemy/timer.h> |
7 #include <signal.h> |
7 #include <alchemy/sem.h> |
8 #include <sys/timerfd.h> |
8 #include <alchemy/mutex.h> |
9 #include <time.h> |
9 #include <alchemy/cond.h> |
10 |
10 #include <alchemy/alarm.h> |
11 #include <applicfg.h> |
11 |
12 #include <timers.h> |
12 #include "applicfg.h" |
13 |
13 #include "can_driver.h" |
14 static pthread_mutex_t CanFestival_mutex = PTHREAD_MUTEX_INITIALIZER; |
14 #include "timers.h" |
15 |
15 |
16 static pthread_mutex_t timer_mutex = PTHREAD_MUTEX_INITIALIZER; |
16 #define TIMERLOOP_TASK_CREATED 1 |
17 int tfd; |
17 |
18 static int timer_created = 0; |
18 TimerCallback_t exitall; |
19 struct timespec last_occured_alarm; |
19 |
20 struct timespec next_alarm; |
20 RT_MUTEX condition_mutex; |
21 struct timespec time_ref; |
21 RT_SEM CanFestival_mutex; |
22 |
22 RT_SEM control_task; |
23 static pthread_t timer_thread; |
23 RT_COND timer_set; |
|
24 RT_TASK timerloop_task; |
|
25 |
|
26 RTIME last_time_read; |
|
27 RTIME last_occured_alarm; |
|
28 RTIME last_timeout_set; |
|
29 |
|
30 int stop_timer = 0; |
24 int stop_timer = 0; |
31 |
25 |
32 /** |
26 /* Subtract the struct timespec values X and Y, |
33 * Init Mutex, Semaphores and Condition variable |
27 storing the result in RESULT. |
34 */ |
28 Return 1 if the difference is negative, otherwise 0. */ |
|
29 int |
|
30 timespec_subtract (struct timespec *result, struct timespec *x, struct timespec *y) |
|
31 { |
|
32 /* Perform the carry for the later subtraction by updating y. */ |
|
33 if (x->tv_nsec < y->tv_nsec) { |
|
34 int seconds = (y->tv_nsec - x->tv_nsec) / 1000000000L + 1; |
|
35 y->tv_nsec -= 1000000000L * seconds; |
|
36 y->tv_sec += seconds; |
|
37 } |
|
38 if (x->tv_nsec - y->tv_nsec > 1000000000L) { |
|
39 int seconds = (x->tv_nsec - y->tv_nsec) / 1000000000L; |
|
40 y->tv_nsec += 1000000000L * seconds; |
|
41 y->tv_sec -= seconds; |
|
42 } |
|
43 |
|
44 /* Compute the time remaining to wait. |
|
45 tv_nsec is certainly positive. */ |
|
46 result->tv_sec = x->tv_sec - y->tv_sec; |
|
47 result->tv_nsec = x->tv_nsec - y->tv_nsec; |
|
48 |
|
49 /* Return 1 if result is negative. */ |
|
50 return x->tv_sec < y->tv_sec; |
|
51 } |
|
52 |
|
53 void timespec_add (struct timespec *result, struct timespec *x, struct timespec *y) |
|
54 { |
|
55 result->tv_sec = x->tv_sec + y->tv_sec; |
|
56 result->tv_nsec = x->tv_nsec + y->tv_nsec; |
|
57 |
|
58 while (result->tv_nsec > 1000000000L) { |
|
59 result->tv_sec ++; |
|
60 result->tv_nsec -= 1000000000L; |
|
61 } |
|
62 } |
|
63 |
|
64 void TimerCleanup(void) |
|
65 { |
|
66 } |
|
67 |
|
68 void EnterTimerMutex(void) |
|
69 { |
|
70 if(pthread_mutex_lock(&timer_mutex)) { |
|
71 perror("pthread_mutex_lock(timer_mutex) failed\n"); |
|
72 } |
|
73 } |
|
74 |
|
75 void LeaveTimerMutex(void) |
|
76 { |
|
77 if(pthread_mutex_unlock(&timer_mutex)) { |
|
78 perror("pthread_mutex_unlock(timer_mutex) failed\n"); |
|
79 } |
|
80 } |
|
81 |
|
82 void EnterMutex(void) |
|
83 { |
|
84 if(pthread_mutex_lock(&CanFestival_mutex)) { |
|
85 perror("pthread_mutex_lock(CanFestival_mutex) failed\n"); |
|
86 } |
|
87 } |
|
88 |
|
89 void LeaveMutex(void) |
|
90 { |
|
91 if(pthread_mutex_unlock(&CanFestival_mutex)) { |
|
92 perror("pthread_mutex_unlock(CanFestival_mutex) failed\n"); |
|
93 } |
|
94 } |
|
95 |
35 void TimerInit(void) |
96 void TimerInit(void) |
36 { |
97 { |
37 int ret = 0; |
98 /* Initialize absolute time references */ |
38 char taskname[32]; |
99 if(clock_gettime(CLOCK_MONOTONIC, &time_ref)){ |
39 |
100 perror("clock_gettime(time_ref)"); |
40 // lock process in to RAM |
101 } |
41 mlockall(MCL_CURRENT | MCL_FUTURE); |
102 next_alarm = last_occured_alarm = time_ref; |
42 |
103 } |
43 snprintf(taskname, sizeof(taskname), "S1-%d", getpid()); |
104 |
44 rt_sem_create(&CanFestival_mutex, taskname, 1, S_FIFO); |
|
45 |
|
46 snprintf(taskname, sizeof(taskname), "S2-%d", getpid()); |
|
47 rt_sem_create(&control_task, taskname, 0, S_FIFO); |
|
48 |
|
49 snprintf(taskname, sizeof(taskname), "M1-%d", getpid()); |
|
50 rt_mutex_create(&condition_mutex, taskname); |
|
51 |
|
52 snprintf(taskname, sizeof(taskname), "C1-%d", getpid()); |
|
53 rt_cond_create(&timer_set, taskname); |
|
54 } |
|
55 |
|
56 /** |
|
57 * Stop Timer Task |
|
58 * @param exitfunction |
|
59 */ |
|
60 void StopTimerLoop(TimerCallback_t exitfunction) |
105 void StopTimerLoop(TimerCallback_t exitfunction) |
61 { |
106 { |
62 exitall = exitfunction; |
107 |
63 stop_timer = 1; |
108 stop_timer = 1; |
64 rt_cond_signal(&timer_set); |
109 |
65 } |
110 pthread_cancel(timer_thread); |
66 |
111 pthread_join(timer_thread, NULL); |
67 |
112 |
68 /** |
113 EnterMutex(); |
69 * Clean all Semaphores, mutex, condition variable and main task |
114 exitfunction(NULL,0); |
70 */ |
115 LeaveMutex(); |
71 void TimerCleanup(void) |
116 } |
72 { |
117 |
73 rt_sem_delete(&CanFestival_mutex); |
118 void* xenomai_timerLoop(void* unused) |
74 rt_mutex_delete(&condition_mutex); |
119 { |
75 rt_cond_delete(&timer_set); |
120 struct sched_param param = { .sched_priority = 80 }; |
76 rt_sem_delete(&control_task); |
121 int ret; |
77 rt_task_unblock(&timerloop_task); |
122 |
78 if (rt_task_join(&timerloop_task) != 0){ |
123 if (ret = pthread_setname_np(pthread_self(), "canfestival_timer")) { |
79 printf("Failed to join with Timerloop task\n"); |
124 fprintf(stderr, "pthread_setname_np(): %s\n", |
80 } |
125 strerror(-ret)); |
81 rt_task_delete(&timerloop_task); |
126 goto exit_timerLoop; |
82 } |
127 } |
83 |
128 |
84 /** |
129 if (ret = pthread_setschedparam(pthread_self(), SCHED_FIFO, ¶m)) { |
85 * Take a semaphore |
130 fprintf(stderr, "pthread_setschedparam(): %s\n", |
86 */ |
131 strerror(-ret)); |
87 void EnterMutex(void) |
132 goto exit_timerLoop; |
88 { |
133 } |
89 rt_sem_p(&CanFestival_mutex, TM_INFINITE); |
134 |
90 } |
135 EnterTimerMutex(); |
91 |
136 |
92 /** |
137 tfd = timerfd_create(CLOCK_MONOTONIC, 0); |
93 * Signaling a semaphore |
138 if(tfd == -1) { |
94 */ |
139 perror("timer_create() failed\n"); |
95 void LeaveMutex(void) |
140 goto exit_timerLoop_timermutex; |
96 { |
141 } |
97 rt_sem_v(&CanFestival_mutex); |
142 timer_created = 1; |
98 } |
143 |
99 |
144 while (!stop_timer) |
100 static TimerCallback_t init_callback; |
145 { |
101 |
146 uint64_t ticks; |
102 /** |
147 |
103 * Timer Task |
148 LeaveTimerMutex(); |
104 */ |
149 |
105 void timerloop_task_proc(void *arg) |
150 EnterMutex(); |
106 { |
151 TimeDispatch(); |
107 int ret = 0; |
152 LeaveMutex(); |
108 |
153 |
109 getElapsedTime(); |
154 /* wait next timer occurence */ |
110 last_timeout_set = 0; |
155 |
111 last_occured_alarm = last_time_read; |
156 ret = read(tfd, &ticks, sizeof(ticks)); |
112 |
157 if (ret < 0) { |
113 /* trigger first alarm */ |
158 perror("timerfd read()\n"); |
|
159 break; |
|
160 } |
|
161 |
|
162 EnterTimerMutex(); |
|
163 |
|
164 last_occured_alarm = next_alarm; |
|
165 } |
|
166 |
|
167 close(tfd); |
|
168 |
|
169 timer_created = 0; |
|
170 |
|
171 exit_timerLoop_timermutex: |
|
172 LeaveTimerMutex(); |
|
173 |
|
174 exit_timerLoop: |
|
175 return NULL; |
|
176 } |
|
177 |
|
178 |
|
179 void StartTimerLoop(TimerCallback_t init_callback) |
|
180 { |
|
181 int ret; |
|
182 |
|
183 stop_timer = 0; |
|
184 |
|
185 EnterMutex(); |
|
186 // At first, TimeDispatch will call init_callback. |
114 SetAlarm(NULL, 0, init_callback, 0, 0); |
187 SetAlarm(NULL, 0, init_callback, 0, 0); |
115 RTIME current_time; |
188 LeaveMutex(); |
116 RTIME real_alarm; |
189 |
117 do{ |
190 ret = pthread_create(&timer_thread, NULL, &xenomai_timerLoop, NULL); |
118 |
191 if (ret) { |
119 rt_mutex_acquire(&condition_mutex, TM_INFINITE); |
192 fprintf(stderr, "StartTimerLoop pthread_create(): %s\n", |
120 if(last_timeout_set == TIMEVAL_MAX) |
193 strerror(-ret)); |
121 { |
194 } |
122 ret = rt_cond_wait( |
195 } |
123 &timer_set, |
196 |
124 &condition_mutex, |
197 static void (*unixtimer_ReceiveLoop_task_proc)(CAN_PORT) = NULL; |
125 TM_INFINITE |
198 |
126 ); /* Then sleep until next message*/ |
199 void* xenomai_canReceiveLoop(void* port) |
127 rt_mutex_release(&condition_mutex); |
200 { |
128 }else{ |
201 int ret; |
129 current_time = rt_timer_read(); |
202 struct sched_param param = { .sched_priority = 82 }; |
130 real_alarm = last_time_read + last_timeout_set; |
203 pthread_setname_np(pthread_self(), "canReceiveLoop"); |
131 ret = rt_cond_wait( /* sleep until next deadline */ |
204 pthread_setschedparam(pthread_self(), SCHED_FIFO, ¶m); |
132 &timer_set, |
205 |
133 &condition_mutex, |
206 unixtimer_ReceiveLoop_task_proc((CAN_PORT)port); |
134 (real_alarm - current_time)); /* else alarm consider expired */ |
207 |
135 if(ret == -ETIMEDOUT){ |
208 return NULL; |
136 last_occured_alarm = real_alarm; |
209 } |
137 rt_mutex_release(&condition_mutex); |
210 |
138 EnterMutex(); |
211 void CreateReceiveTask(CAN_PORT port, TASK_HANDLE* Thread, void* ReceiveLoopPtr) |
139 TimeDispatch(); |
212 { |
140 LeaveMutex(); |
213 unixtimer_ReceiveLoop_task_proc = ReceiveLoopPtr; |
141 }else{ |
214 |
142 rt_mutex_release(&condition_mutex); |
215 if(pthread_create(Thread, NULL, xenomai_canReceiveLoop, (void*)port)) { |
143 } |
216 perror("CreateReceiveTask pthread_create()"); |
144 } |
217 } |
145 }while ((ret == 0 || ret == -EINTR || ret == -ETIMEDOUT) && !stop_timer); |
218 } |
146 |
219 |
147 if(exitall){ |
220 void WaitReceiveTaskEnd(TASK_HANDLE *Thread) |
148 EnterMutex(); |
221 { |
149 exitall(NULL,0); |
222 if(pthread_cancel(*Thread)) { |
150 LeaveMutex(); |
223 perror("pthread_cancel()"); |
151 } |
224 } |
152 } |
225 if(pthread_join(*Thread, NULL)) { |
153 |
226 perror("pthread_join()"); |
154 /** |
227 } |
155 * Create the Timer Task |
228 } |
156 * @param _init_callback |
229 |
157 */ |
230 #define maxval(a,b) ((a>b)?a:b) |
158 void StartTimerLoop(TimerCallback_t _init_callback) |
|
159 { |
|
160 int ret = 0; |
|
161 stop_timer = 0; |
|
162 init_callback = _init_callback; |
|
163 |
|
164 char taskname[32]; |
|
165 snprintf(taskname, sizeof(taskname), "timerloop-%d", getpid()); |
|
166 |
|
167 /* create timerloop_task */ |
|
168 ret = rt_task_create(&timerloop_task, taskname, 0, 50, T_JOINABLE); |
|
169 if (ret) { |
|
170 printf("Failed to create timerloop_task, code %d\n",errno); |
|
171 return; |
|
172 } |
|
173 |
|
174 /* start timerloop_task */ |
|
175 ret = rt_task_start(&timerloop_task,&timerloop_task_proc,NULL); |
|
176 if (ret) { |
|
177 printf("Failed to start timerloop_task, code %u\n",errno); |
|
178 goto error; |
|
179 } |
|
180 |
|
181 return; |
|
182 |
|
183 error: |
|
184 rt_task_delete(&timerloop_task); |
|
185 } |
|
186 |
|
187 /** |
|
188 * Create the CAN Receiver Task |
|
189 * @param fd0 CAN port |
|
190 * @param *ReceiveLoop_task CAN receiver task |
|
191 * @param *ReceiveLoop_task_proc CAN receiver function |
|
192 */ |
|
193 void CreateReceiveTask(CAN_PORT fd0, TASK_HANDLE *ReceiveLoop_task, void* ReceiveLoop_task_proc) |
|
194 { |
|
195 int ret; |
|
196 static int id = 0; |
|
197 char taskname[32]; |
|
198 snprintf(taskname, sizeof(taskname), "canloop%d-%d", id, getpid()); |
|
199 id++; |
|
200 |
|
201 /* create ReceiveLoop_task */ |
|
202 ret = rt_task_create(ReceiveLoop_task,taskname,0,50,T_JOINABLE); |
|
203 if (ret) { |
|
204 printf("Failed to create ReceiveLoop_task number %d, code %d\n", id, errno); |
|
205 return; |
|
206 } |
|
207 /* start ReceiveLoop_task */ |
|
208 ret = rt_task_start(ReceiveLoop_task, ReceiveLoop_task_proc,(void*)fd0); |
|
209 if (ret) { |
|
210 printf("Failed to start ReceiveLoop_task number %d, code %d\n", id, errno); |
|
211 return; |
|
212 } |
|
213 rt_sem_v(&control_task); |
|
214 } |
|
215 |
|
216 /** |
|
217 * Wait for the CAN Receiver Task end |
|
218 * @param *ReceiveLoop_task CAN receiver thread |
|
219 */ |
|
220 void WaitReceiveTaskEnd(TASK_HANDLE *ReceiveLoop_task) |
|
221 { |
|
222 rt_task_unblock(ReceiveLoop_task); |
|
223 if (rt_task_join(ReceiveLoop_task) != 0){ |
|
224 printf("Failed to join with Receive task\n"); |
|
225 } |
|
226 rt_task_delete(ReceiveLoop_task); |
|
227 } |
|
228 |
|
229 /** |
|
230 * Set timer for the next wakeup |
|
231 * @param value |
|
232 */ |
|
233 void setTimer(TIMEVAL value) |
231 void setTimer(TIMEVAL value) |
234 { |
232 { |
235 rt_mutex_acquire(&condition_mutex, TM_INFINITE); |
233 struct itimerspec timerValues; |
236 last_timeout_set = value; |
234 |
237 rt_mutex_release(&condition_mutex); |
235 EnterTimerMutex(); |
238 rt_cond_signal(&timer_set); |
236 |
239 } |
237 if(timer_created){ |
240 |
238 long tv_nsec = (maxval(value,1)%1000000000LL); |
241 /** |
239 time_t tv_sec = value/1000000000LL; |
242 * Get the elapsed time since the last alarm |
240 timerValues.it_value.tv_sec = tv_sec; |
243 * @return a time in nanoseconds |
241 timerValues.it_value.tv_nsec = tv_nsec; |
244 */ |
242 timerValues.it_interval.tv_sec = 0; |
|
243 timerValues.it_interval.tv_nsec = 0; |
|
244 |
|
245 /* keep track of when should alarm occur*/ |
|
246 timespec_add(&next_alarm, &time_ref, &timerValues.it_value); |
|
247 timerfd_settime (tfd, 0, &timerValues, NULL); |
|
248 } |
|
249 |
|
250 LeaveTimerMutex(); |
|
251 } |
|
252 |
|
253 |
245 TIMEVAL getElapsedTime(void) |
254 TIMEVAL getElapsedTime(void) |
246 { |
255 { |
247 RTIME res; |
256 struct itimerspec outTimerValues; |
248 rt_mutex_acquire(&condition_mutex, TM_INFINITE); |
257 struct timespec ts; |
249 last_time_read = rt_timer_read(); |
258 struct timespec last_occured_alarm_copy; |
250 res = last_time_read - last_occured_alarm; |
259 |
251 rt_mutex_release(&condition_mutex); |
260 TIMEVAL res = 0; |
|
261 |
|
262 EnterTimerMutex(); |
|
263 |
|
264 if(timer_created){ |
|
265 if(clock_gettime(CLOCK_MONOTONIC, &time_ref)){ |
|
266 perror("clock_gettime(ts)"); |
|
267 |
|
268 } |
|
269 /* timespec_substract modifies second operand */ |
|
270 last_occured_alarm_copy = last_occured_alarm; |
|
271 |
|
272 timespec_subtract(&ts, &time_ref, &last_occured_alarm_copy); |
|
273 |
|
274 /* TIMEVAL is nano seconds */ |
|
275 res = (ts.tv_sec * 1000000000L) + ts.tv_nsec; |
|
276 } |
|
277 |
|
278 LeaveTimerMutex(); |
|
279 |
252 return res; |
280 return res; |
253 } |
281 } |
|
282 |