6 #include <string.h> |
6 #include <string.h> |
7 #include <time.h> |
7 #include <time.h> |
8 #include <signal.h> |
8 #include <signal.h> |
9 #include <stdlib.h> |
9 #include <stdlib.h> |
10 #include <sys/mman.h> |
10 #include <sys/mman.h> |
|
11 #include <sys/fcntl.h> |
11 |
12 |
12 #include <native/task.h> |
13 #include <native/task.h> |
13 #include <native/timer.h> |
14 #include <native/timer.h> |
14 #include <native/mutex.h> |
15 #include <native/mutex.h> |
15 #include <native/sem.h> |
16 #include <native/sem.h> |
|
17 #include <native/pipe.h> |
16 |
18 |
17 unsigned int PLC_state = 0; |
19 unsigned int PLC_state = 0; |
18 #define PLC_STATE_TASK_CREATED 1 |
20 #define PLC_STATE_TASK_CREATED 1 |
19 #define PLC_STATE_PYTHON_MUTEX_CREATED 2 |
21 #define PLC_STATE_PYTHON_MUTEX_CREATED 2 |
20 #define PLC_STATE_PYTHON_WAIT_SEM_CREATED 4 |
22 #define PLC_STATE_PYTHON_WAIT_SEM_CREATED 4 |
21 #define PLC_STATE_DEBUG_MUTEX_CREATED 8 |
23 #define PLC_STATE_DEBUG_MUTEX_CREATED 8 |
22 #define PLC_STATE_DEBUG_WAIT_SEM_CREATED 16 |
24 #define PLC_STATE_DEBUG_FILE_OPENED 16 |
|
25 #define PLC_STATE_DEBUG_PIPE_CREATED 32 |
|
26 |
|
27 #define WAITDEBUG_PIPE_DEVICE "/dev/rtp0" |
|
28 #define WAITDEBUG_PIPE_MINOR 0 |
|
29 #define WAITDEBUG_PIPE_SIZE 500 |
23 |
30 |
24 /* provided by POUS.C */ |
31 /* provided by POUS.C */ |
25 extern int common_ticktime__; |
32 extern int common_ticktime__; |
26 |
33 |
27 long AtomicCompareExchange(long* atomicvar,long compared, long exchange) |
34 long AtomicCompareExchange(long* atomicvar,long compared, long exchange) |
35 CURRENT_TIME->tv_sec = current_time / 1000000000; |
42 CURRENT_TIME->tv_sec = current_time / 1000000000; |
36 CURRENT_TIME->tv_nsec = current_time % 1000000000; |
43 CURRENT_TIME->tv_nsec = current_time % 1000000000; |
37 } |
44 } |
38 |
45 |
39 RT_TASK PLC_task; |
46 RT_TASK PLC_task; |
40 RT_TASK WaitDebug_task; |
47 RT_PIPE WaitDebug_pipe; |
|
48 RT_TASK SuspendDebug_task; |
|
49 RT_TASK ResumeDebug_task; |
41 RT_TASK WaitPythonCommand_task; |
50 RT_TASK WaitPythonCommand_task; |
42 RT_TASK UnLockPython_task; |
51 RT_TASK UnLockPython_task; |
43 RT_TASK LockPython_task; |
52 RT_TASK LockPython_task; |
44 int PLC_shutdown = 0; |
53 int PLC_shutdown = 0; |
|
54 |
|
55 int WaitDebug_pipe_fd = -1; |
45 |
56 |
46 void PLC_SetTimer(long long next, long long period) |
57 void PLC_SetTimer(long long next, long long period) |
47 { |
58 { |
48 RTIME current_time = rt_timer_read(); |
59 RTIME current_time = rt_timer_read(); |
49 rt_task_set_periodic(&PLC_task, current_time + next, rt_timer_ns2ticks(period)); |
60 rt_task_set_periodic(&PLC_task, current_time + next, rt_timer_ns2ticks(period)); |
63 |
74 |
64 static int __debug_tick; |
75 static int __debug_tick; |
65 |
76 |
66 RT_SEM python_wait_sem; |
77 RT_SEM python_wait_sem; |
67 RT_MUTEX python_mutex; |
78 RT_MUTEX python_mutex; |
68 RT_SEM debug_wait_sem; |
|
69 RT_MUTEX debug_mutex; |
79 RT_MUTEX debug_mutex; |
70 |
80 |
71 void PLC_cleanup_all(void) |
81 void PLC_cleanup_all(void) |
72 { |
82 { |
73 if (PLC_state & PLC_STATE_TASK_CREATED) { |
83 if (PLC_state & PLC_STATE_TASK_CREATED) { |
74 rt_task_delete(&PLC_task); |
84 rt_task_delete(&PLC_task); |
75 PLC_state &= ~PLC_STATE_TASK_CREATED; |
85 PLC_state &= ~PLC_STATE_TASK_CREATED; |
76 } |
86 } |
77 |
87 |
78 if (PLC_state & PLC_STATE_PYTHON_WAIT_SEM_CREATED) { |
88 if (PLC_state & PLC_STATE_PYTHON_WAIT_SEM_CREATED) { |
|
89 rt_sem_v(&python_wait_sem); |
79 rt_sem_delete(&python_wait_sem); |
90 rt_sem_delete(&python_wait_sem); |
80 PLC_state &= ~ PLC_STATE_PYTHON_WAIT_SEM_CREATED; |
91 PLC_state &= ~ PLC_STATE_PYTHON_WAIT_SEM_CREATED; |
81 } |
92 } |
82 |
93 |
83 if (PLC_state & PLC_STATE_PYTHON_MUTEX_CREATED) { |
94 if (PLC_state & PLC_STATE_PYTHON_MUTEX_CREATED) { |
84 rt_mutex_delete(&python_mutex); |
95 rt_mutex_delete(&python_mutex); |
85 PLC_state &= ~ PLC_STATE_PYTHON_MUTEX_CREATED; |
96 PLC_state &= ~ PLC_STATE_PYTHON_MUTEX_CREATED; |
86 } |
97 } |
87 |
98 |
88 if (PLC_state & PLC_STATE_DEBUG_WAIT_SEM_CREATED) { |
99 if (PLC_state & PLC_STATE_DEBUG_PIPE_CREATED) { |
89 rt_sem_delete(&debug_wait_sem); |
100 rt_pipe_delete(&WaitDebug_pipe); |
90 PLC_state &= ~ PLC_STATE_DEBUG_WAIT_SEM_CREATED; |
101 PLC_state &= ~PLC_STATE_DEBUG_PIPE_CREATED; |
|
102 } |
|
103 |
|
104 if (PLC_state & PLC_STATE_DEBUG_FILE_OPENED) { |
|
105 close(WaitDebug_pipe_fd); |
|
106 PLC_state &= ~PLC_STATE_DEBUG_FILE_OPENED; |
91 } |
107 } |
92 |
108 |
93 if (PLC_state & PLC_STATE_DEBUG_MUTEX_CREATED) { |
109 if (PLC_state & PLC_STATE_DEBUG_MUTEX_CREATED) { |
94 rt_mutex_delete(&debug_mutex); |
110 rt_mutex_delete(&debug_mutex); |
95 PLC_state &= ~ PLC_STATE_DEBUG_MUTEX_CREATED; |
111 PLC_state &= ~ PLC_STATE_DEBUG_MUTEX_CREATED; |
139 /* create python_mutex */ |
153 /* create python_mutex */ |
140 ret = rt_mutex_create(&python_mutex, "python_mutex"); |
154 ret = rt_mutex_create(&python_mutex, "python_mutex"); |
141 if (ret) goto error; |
155 if (ret) goto error; |
142 PLC_state |= PLC_STATE_PYTHON_MUTEX_CREATED; |
156 PLC_state |= PLC_STATE_PYTHON_MUTEX_CREATED; |
143 |
157 |
144 /* create debug_wait_sem */ |
158 /* create WaitDebug_pipe */ |
145 ret = rt_sem_create(&debug_wait_sem, "debug_wait_sem", 0, S_FIFO); |
159 ret = rt_pipe_create(&WaitDebug_pipe, "WaitDebug_pipe", WAITDEBUG_PIPE_MINOR, |
146 if (ret) goto error; |
160 WAITDEBUG_PIPE_SIZE * sizeof(char)); |
147 PLC_state |= PLC_STATE_DEBUG_WAIT_SEM_CREATED; |
161 if (ret) goto error; |
|
162 PLC_state |= PLC_STATE_DEBUG_PIPE_CREATED; |
|
163 |
|
164 /* open WaitDebug_pipe*/ |
|
165 WaitDebug_pipe_fd = open(WAITDEBUG_PIPE_DEVICE, O_RDWR); |
|
166 if (WaitDebug_pipe_fd == -1) { |
|
167 ret = -EBADF; |
|
168 goto error; |
|
169 } |
|
170 PLC_state |= PLC_STATE_DEBUG_FILE_OPENED; |
148 |
171 |
149 /* create debug_mutex */ |
172 /* create debug_mutex */ |
150 ret = rt_mutex_create(&debug_mutex, "debug_mutex"); |
173 ret = rt_mutex_create(&debug_mutex, "debug_mutex"); |
151 if (ret) goto error; |
174 if (ret) goto error; |
152 PLC_state |= PLC_STATE_DEBUG_MUTEX_CREATED; |
175 PLC_state |= PLC_STATE_DEBUG_MUTEX_CREATED; |
182 |
205 |
183 extern int __tick; |
206 extern int __tick; |
184 /* from plc_debugger.c */ |
207 /* from plc_debugger.c */ |
185 int WaitDebugData() |
208 int WaitDebugData() |
186 { |
209 { |
187 rt_task_shadow(&WaitDebug_task, "WaitDebug_task", 0, 0); |
210 char message; |
188 /* Wait signal from PLC thread */ |
211 /* Wait signal from PLC thread */ |
189 rt_sem_p(&debug_wait_sem, TM_INFINITE); |
212 if (PLC_state & PLC_STATE_DEBUG_FILE_OPENED) |
|
213 read(WaitDebug_pipe_fd, &message, sizeof(char)); |
190 return __debug_tick; |
214 return __debug_tick; |
191 } |
215 } |
192 |
216 |
193 /* Called by PLC thread when debug_publish finished |
217 /* Called by PLC thread when debug_publish finished |
194 * This is supposed to unlock debugger thread in WaitDebugData*/ |
218 * This is supposed to unlock debugger thread in WaitDebugData*/ |
195 void InitiateDebugTransfer() |
219 void InitiateDebugTransfer() |
196 { |
220 { |
|
221 char message = 1; |
197 /* remember tick */ |
222 /* remember tick */ |
198 __debug_tick = __tick; |
223 __debug_tick = __tick; |
199 /* signal debugger thread it can read data */ |
224 /* signal debugger thread it can read data */ |
200 rt_sem_v(&debug_wait_sem); |
225 if (PLC_state & PLC_STATE_DEBUG_PIPE_CREATED) |
|
226 rt_pipe_write(&WaitDebug_pipe, &message, sizeof(char), P_NORMAL); |
201 } |
227 } |
202 |
228 |
203 void suspendDebug(void) |
229 void suspendDebug(void) |
204 { |
230 { |
205 __DEBUG = 0; |
231 __DEBUG = 0; |
206 /* Prevent PLC to enter debug code */ |
232 if (PLC_state & PLC_STATE_DEBUG_MUTEX_CREATED) { |
207 rt_mutex_acquire(&debug_mutex, TM_INFINITE); |
233 rt_task_shadow(&SuspendDebug_task, "SuspendDebug_task", 0, 0); |
|
234 /* Prevent PLC to enter debug code */ |
|
235 rt_mutex_acquire(&debug_mutex, TM_INFINITE); |
|
236 } |
208 } |
237 } |
209 |
238 |
210 void resumeDebug(void) |
239 void resumeDebug(void) |
211 { |
240 { |
212 __DEBUG = 1; |
241 __DEBUG = 1; |
213 /* Let PLC enter debug code */ |
242 if (PLC_state & PLC_STATE_DEBUG_MUTEX_CREATED) { |
214 rt_mutex_release(&debug_mutex); |
243 rt_task_shadow(&ResumeDebug_task, "ResumeDebug_task", 0, 0); |
|
244 /* Let PLC enter debug code */ |
|
245 rt_mutex_release(&debug_mutex); |
|
246 } |
215 } |
247 } |
216 |
248 |
217 /* from plc_python.c */ |
249 /* from plc_python.c */ |
218 int WaitPythonCommands(void) |
250 int WaitPythonCommands(void) |
219 { |
251 { |
220 rt_task_shadow(&WaitPythonCommand_task, "WaitPythonCommand_task", 0, 0); |
|
221 /* Wait signal from PLC thread */ |
252 /* Wait signal from PLC thread */ |
222 rt_sem_p(&python_wait_sem, TM_INFINITE); |
253 if (PLC_state & PLC_STATE_PYTHON_WAIT_SEM_CREATED) { |
|
254 rt_task_shadow(&WaitPythonCommand_task, "WaitPythonCommand_task", 0, 0); |
|
255 rt_sem_p(&python_wait_sem, TM_INFINITE); |
|
256 } |
223 } |
257 } |
224 |
258 |
225 /* Called by PLC thread on each new python command*/ |
259 /* Called by PLC thread on each new python command*/ |
226 void UnBlockPythonCommands(void) |
260 void UnBlockPythonCommands(void) |
227 { |
261 { |
234 return rt_mutex_acquire(&python_mutex, TM_NONBLOCK) == 0; |
268 return rt_mutex_acquire(&python_mutex, TM_NONBLOCK) == 0; |
235 } |
269 } |
236 |
270 |
237 void UnLockPython(void) |
271 void UnLockPython(void) |
238 { |
272 { |
239 rt_task_shadow(&UnLockPython_task, "UnLockPython_task", 0, 0); |
273 if (PLC_state & PLC_STATE_PYTHON_MUTEX_CREATED) { |
240 rt_mutex_release(&python_mutex); |
274 rt_task_shadow(&UnLockPython_task, "UnLockPython_task", 0, 0); |
|
275 rt_mutex_release(&python_mutex); |
|
276 } |
241 } |
277 } |
242 |
278 |
243 void LockPython(void) |
279 void LockPython(void) |
244 { |
280 { |
245 rt_task_shadow(&LockPython_task, "LockPython_task", 0, 0); |
281 if (PLC_state & PLC_STATE_PYTHON_MUTEX_CREATED) { |
246 rt_mutex_acquire(&python_mutex, TM_INFINITE); |
282 rt_task_shadow(&LockPython_task, "LockPython_task", 0, 0); |
247 } |
283 rt_mutex_acquire(&python_mutex, TM_INFINITE); |
|
284 } |
|
285 } |