Line data Source code
1 : #include "Scheduler.h"
2 :
3 : #include <algorithm>
4 : #include <errno.h>
5 : #include <poll.h>
6 : #include <stdio.h>
7 : #include <stdlib.h>
8 : #include <sys/mman.h>
9 : #include <sys/time.h>
10 : #include <unistd.h>
11 :
12 : #include <AP_HAL/AP_HAL.h>
13 : #include <AP_Math/AP_Math.h>
14 : #include <AP_Vehicle/AP_Vehicle_Type.h>
15 :
16 : #include "RCInput.h"
17 : #include "SPIUARTDriver.h"
18 : #include "Storage.h"
19 : #include "UARTDriver.h"
20 : #include "Util.h"
21 :
22 : using namespace Linux;
23 :
24 : extern const AP_HAL::HAL& hal;
25 :
26 : #define APM_LINUX_MAX_PRIORITY 20
27 : #define APM_LINUX_TIMER_PRIORITY 15
28 : #define APM_LINUX_UART_PRIORITY 14
29 : #define APM_LINUX_NET_PRIORITY 14
30 : #define APM_LINUX_RCIN_PRIORITY 13
31 : #define APM_LINUX_MAIN_PRIORITY 12
32 : #define APM_LINUX_IO_PRIORITY 10
33 : #define APM_LINUX_SCRIPTING_PRIORITY 1
34 :
35 : #define APM_LINUX_TIMER_RATE 1000
36 : #define APM_LINUX_UART_RATE 100
37 :
38 : #ifndef APM_LINUX_RCIN_RATE
39 : #define APM_LINUX_RCIN_RATE 100
40 : #endif // APM_LINUX_RCIN_RATE
41 :
42 : #ifndef APM_LINUX_IO_RATE
43 : #define APM_LINUX_IO_RATE 50
44 : #endif // APM_LINUX_IO_RATE
45 :
46 : #define SCHED_THREAD(name_, UPPER_NAME_) \
47 : { \
48 : .name = "ap-" #name_, \
49 : .thread = &_##name_##_thread, \
50 : .policy = SCHED_FIFO, \
51 : .prio = APM_LINUX_##UPPER_NAME_##_PRIORITY, \
52 : .rate = APM_LINUX_##UPPER_NAME_##_RATE, \
53 : }
54 :
55 48 : Scheduler::Scheduler()
56 : {
57 48 : CPU_ZERO(&_cpu_affinity);
58 48 : }
59 :
60 :
61 0 : void Scheduler::init_realtime()
62 : {
63 : #if APM_BUILD_TYPE(APM_BUILD_Replay)
64 : // we don't run Replay in real-time...
65 : return;
66 : #endif
67 : #if APM_BUILD_TYPE(APM_BUILD_UNKNOWN)
68 : // we opportunistically run examples/tools in realtime
69 0 : if (geteuid() != 0) {
70 0 : fprintf(stderr, "WARNING: not running as root. Will not use realtime scheduling\n");
71 0 : return;
72 : }
73 : #endif
74 :
75 0 : mlockall(MCL_CURRENT|MCL_FUTURE);
76 :
77 0 : struct sched_param param = { .sched_priority = APM_LINUX_MAIN_PRIORITY };
78 0 : if (pthread_setschedparam(pthread_self(), SCHED_FIFO, ¶m) == -1) {
79 0 : AP_HAL::panic("Scheduler: failed to set scheduling parameters: %s",
80 0 : strerror(errno));
81 : }
82 : }
83 :
84 0 : void Scheduler::init_cpu_affinity()
85 : {
86 0 : if (!CPU_COUNT(&_cpu_affinity)) {
87 0 : return;
88 : }
89 :
90 0 : if (sched_setaffinity(0, sizeof(_cpu_affinity), &_cpu_affinity) != 0) {
91 0 : AP_HAL::panic("Failed to set affinity for main process: %m");
92 : }
93 : }
94 :
95 0 : void Scheduler::init()
96 : {
97 : int ret;
98 : const struct sched_table {
99 : const char *name;
100 : SchedulerThread *thread;
101 : int policy;
102 : int prio;
103 : uint32_t rate;
104 0 : } sched_table[] = {
105 0 : SCHED_THREAD(timer, TIMER),
106 0 : SCHED_THREAD(uart, UART),
107 0 : SCHED_THREAD(rcin, RCIN),
108 0 : SCHED_THREAD(io, IO),
109 0 : };
110 :
111 0 : _main_ctx = pthread_self();
112 :
113 0 : init_realtime();
114 0 : init_cpu_affinity();
115 :
116 : /* set barrier to N + 1 threads: worker threads + main */
117 0 : unsigned n_threads = ARRAY_SIZE(sched_table) + 1;
118 0 : ret = pthread_barrier_init(&_initialized_barrier, nullptr, n_threads);
119 0 : if (ret) {
120 0 : AP_HAL::panic("Scheduler: Failed to initialise barrier object: %s",
121 : strerror(ret));
122 : }
123 :
124 0 : for (size_t i = 0; i < ARRAY_SIZE(sched_table); i++) {
125 0 : const struct sched_table *t = &sched_table[i];
126 :
127 0 : t->thread->set_rate(t->rate);
128 0 : t->thread->set_stack_size(1024 * 1024);
129 0 : t->thread->start(t->name, t->policy, t->prio);
130 : }
131 :
132 : #if defined(DEBUG_STACK) && DEBUG_STACK
133 : register_timer_process(FUNCTOR_BIND_MEMBER(&Scheduler::_debug_stack, void));
134 : #endif
135 0 : }
136 :
137 0 : void Scheduler::_debug_stack()
138 : {
139 0 : uint64_t now = AP_HAL::millis64();
140 :
141 0 : if (now - _last_stack_debug_msec > 5000) {
142 0 : fprintf(stderr, "Stack Usage:\n"
143 : "\ttimer = %zu\n"
144 : "\tio = %zu\n"
145 : "\trcin = %zu\n"
146 : "\tuart = %zu\n",
147 : _timer_thread.get_stack_usage(),
148 : _io_thread.get_stack_usage(),
149 : _rcin_thread.get_stack_usage(),
150 : _uart_thread.get_stack_usage());
151 0 : _last_stack_debug_msec = now;
152 : }
153 0 : }
154 :
155 2008 : void Scheduler::microsleep(uint32_t usec)
156 : {
157 : struct timespec ts;
158 2008 : ts.tv_sec = 0;
159 2008 : ts.tv_nsec = usec*1000UL;
160 2008 : while (nanosleep(&ts, &ts) == -1 && errno == EINTR) ;
161 2008 : }
162 :
163 200 : void Scheduler::delay(uint16_t ms)
164 : {
165 200 : if (_stopped_clock_usec) {
166 0 : return;
167 : }
168 :
169 200 : if (ms == 0) {
170 0 : return;
171 : }
172 :
173 200 : uint64_t now = AP_HAL::micros64();
174 200 : uint64_t end = now + 1000UL * ms + 1U;
175 : do {
176 : // this yields the CPU to other apps
177 1999 : microsleep(MIN(1000UL, end-now));
178 1999 : if (in_main_thread() && _min_delay_cb_ms <= ms) {
179 0 : call_delay_cb();
180 : }
181 1999 : now = AP_HAL::micros64();
182 1999 : } while (now < end);
183 : }
184 :
185 0 : void Scheduler::delay_microseconds(uint16_t us)
186 : {
187 0 : if (_stopped_clock_usec) {
188 0 : return;
189 : }
190 0 : microsleep(us);
191 : }
192 :
193 0 : void Scheduler::register_timer_process(AP_HAL::MemberProc proc)
194 : {
195 0 : for (uint8_t i = 0; i < _num_timer_procs; i++) {
196 0 : if (_timer_proc[i] == proc) {
197 0 : return;
198 : }
199 : }
200 :
201 0 : if (_num_timer_procs >= LINUX_SCHEDULER_MAX_TIMER_PROCS) {
202 0 : hal.console->printf("Out of timer processes\n");
203 0 : return;
204 : }
205 :
206 0 : _timer_proc[_num_timer_procs] = proc;
207 0 : _num_timer_procs++;
208 : }
209 :
210 1 : void Scheduler::register_io_process(AP_HAL::MemberProc proc)
211 : {
212 1 : for (uint8_t i = 0; i < _num_io_procs; i++) {
213 0 : if (_io_proc[i] == proc) {
214 0 : return;
215 : }
216 : }
217 :
218 1 : if (_num_io_procs < LINUX_SCHEDULER_MAX_IO_PROCS) {
219 1 : _io_proc[_num_io_procs] = proc;
220 1 : _num_io_procs++;
221 : } else {
222 0 : hal.console->printf("Out of IO processes\n");
223 : }
224 : }
225 :
226 0 : void Scheduler::register_timer_failsafe(AP_HAL::Proc failsafe, uint32_t period_us)
227 : {
228 0 : _failsafe = failsafe;
229 0 : }
230 :
231 0 : void Scheduler::_timer_task()
232 : {
233 : int i;
234 :
235 0 : if (_in_timer_proc) {
236 0 : return;
237 : }
238 0 : _in_timer_proc = true;
239 :
240 : // now call the timer based drivers
241 0 : for (i = 0; i < _num_timer_procs; i++) {
242 0 : if (_timer_proc[i]) {
243 0 : _timer_proc[i]();
244 : }
245 : }
246 :
247 : // and the failsafe, if one is setup
248 0 : if (_failsafe != nullptr) {
249 0 : _failsafe();
250 : }
251 :
252 0 : _in_timer_proc = false;
253 : }
254 :
255 18 : void Scheduler::_run_io(void)
256 : {
257 18 : _io_semaphore.take_blocking();
258 :
259 : // now call the IO based drivers
260 18 : for (int i = 0; i < _num_io_procs; i++) {
261 0 : if (_io_proc[i]) {
262 0 : _io_proc[i]();
263 : }
264 : }
265 :
266 18 : _io_semaphore.give();
267 18 : }
268 :
269 : /*
270 : run timers for all UARTs
271 : */
272 0 : void Scheduler::_run_uarts()
273 : {
274 : // process any pending serial bytes
275 0 : for (uint8_t i=0;i<hal.num_serial; i++) {
276 0 : hal.serial(i)->_timer_tick();
277 : }
278 0 : }
279 :
280 0 : void Scheduler::_rcin_task()
281 : {
282 0 : RCInput::from(hal.rcin)->_timer_tick();
283 0 : }
284 :
285 0 : void Scheduler::_uart_task()
286 : {
287 0 : _run_uarts();
288 0 : }
289 :
290 0 : void Scheduler::_io_task()
291 : {
292 : // process any pending storage writes
293 0 : hal.storage->_timer_tick();
294 :
295 : // run registered IO processes
296 0 : _run_io();
297 0 : }
298 :
299 2233 : bool Scheduler::in_main_thread() const
300 : {
301 2233 : return pthread_equal(pthread_self(), _main_ctx);
302 : }
303 :
304 0 : void Scheduler::_wait_all_threads()
305 : {
306 0 : int r = pthread_barrier_wait(&_initialized_barrier);
307 0 : if (r == PTHREAD_BARRIER_SERIAL_THREAD) {
308 0 : pthread_barrier_destroy(&_initialized_barrier);
309 : }
310 0 : }
311 :
312 0 : void Scheduler::set_system_initialized()
313 : {
314 0 : if (_initialized) {
315 0 : AP_HAL::panic("PANIC: scheduler::set_system_initialized called more than once");
316 : }
317 :
318 0 : _initialized = true;
319 :
320 0 : _wait_all_threads();
321 0 : }
322 :
323 0 : void Scheduler::reboot(bool hold_in_bootloader)
324 : {
325 0 : exit(1);
326 : }
327 :
328 : #if APM_BUILD_TYPE(APM_BUILD_Replay) || APM_BUILD_TYPE(APM_BUILD_UNKNOWN)
329 18 : void Scheduler::stop_clock(uint64_t time_usec)
330 : {
331 18 : if (time_usec < _stopped_clock_usec) {
332 0 : ::fprintf(stderr, "Warning: setting time backwards from (%" PRIu64 ") to (%" PRIu64 ")\n", _stopped_clock_usec, time_usec);
333 0 : return;
334 : }
335 :
336 18 : _stopped_clock_usec = time_usec;
337 18 : _run_io();
338 : }
339 : #else
340 : void Scheduler::stop_clock(uint64_t time_usec)
341 : {
342 : // stop_clock() is not called outside of Replay, but we can't
343 : // guard it in the header because of the vehicle-dependent-library
344 : // checks in waf.
345 : }
346 : #endif
347 :
348 0 : bool Scheduler::SchedulerThread::_run()
349 : {
350 0 : _sched._wait_all_threads();
351 :
352 0 : return PeriodicThread::_run();
353 : }
354 :
355 0 : void Scheduler::teardown()
356 : {
357 0 : _timer_thread.stop();
358 0 : _io_thread.stop();
359 0 : _rcin_thread.stop();
360 0 : _uart_thread.stop();
361 :
362 0 : _timer_thread.join();
363 0 : _io_thread.join();
364 0 : _rcin_thread.join();
365 0 : _uart_thread.join();
366 0 : }
367 :
368 : // calculates an integer to be used as the priority for a newly-created thread
369 0 : uint8_t Scheduler::calculate_thread_priority(priority_base base, int8_t priority) const
370 : {
371 0 : uint8_t thread_priority = APM_LINUX_IO_PRIORITY;
372 : static const struct {
373 : priority_base base;
374 : uint8_t p;
375 : } priority_map[] = {
376 : { PRIORITY_BOOST, APM_LINUX_MAIN_PRIORITY},
377 : { PRIORITY_MAIN, APM_LINUX_MAIN_PRIORITY},
378 : { PRIORITY_SPI, AP_LINUX_SENSORS_SCHED_PRIO},
379 : { PRIORITY_I2C, AP_LINUX_SENSORS_SCHED_PRIO},
380 : { PRIORITY_CAN, APM_LINUX_TIMER_PRIORITY},
381 : { PRIORITY_TIMER, APM_LINUX_TIMER_PRIORITY},
382 : { PRIORITY_RCIN, APM_LINUX_RCIN_PRIORITY},
383 : { PRIORITY_IO, APM_LINUX_IO_PRIORITY},
384 : { PRIORITY_UART, APM_LINUX_UART_PRIORITY},
385 : { PRIORITY_STORAGE, APM_LINUX_IO_PRIORITY},
386 : { PRIORITY_SCRIPTING, APM_LINUX_SCRIPTING_PRIORITY},
387 : { PRIORITY_NET, APM_LINUX_NET_PRIORITY},
388 : };
389 0 : for (uint8_t i=0; i<ARRAY_SIZE(priority_map); i++) {
390 0 : if (priority_map[i].base == base) {
391 0 : thread_priority = constrain_int16(priority_map[i].p + priority, 1, APM_LINUX_MAX_PRIORITY);
392 0 : break;
393 : }
394 : }
395 :
396 0 : return thread_priority;
397 : }
398 :
399 : /*
400 : create a new thread
401 : */
402 0 : bool Scheduler::thread_create(AP_HAL::MemberProc proc, const char *name, uint32_t stack_size, priority_base base, int8_t priority)
403 : {
404 0 : Thread *thread = NEW_NOTHROW Thread{(Thread::task_t)proc};
405 0 : if (!thread) {
406 0 : return false;
407 : }
408 :
409 0 : const uint8_t thread_priority = calculate_thread_priority(base, priority);
410 :
411 : // Add 256k to HAL-independent requested stack size
412 0 : thread->set_stack_size(256 * 1024 + stack_size);
413 :
414 : /*
415 : * We should probably store the thread handlers and join() when exiting,
416 : * but let's the thread manage itself for now.
417 : */
418 0 : thread->set_auto_free(true);
419 :
420 0 : if (!thread->start(name, SCHED_FIFO, thread_priority)) {
421 0 : delete thread;
422 0 : return false;
423 : }
424 :
425 0 : return true;
426 : }
|
