Line data Source code
1 : #include <AP_HAL/AP_HAL.h>
2 :
3 : #include "AP_HAL_SITL.h"
4 : #include <AP_HAL_SITL/I2CDevice.h>
5 : #include "Scheduler.h"
6 : #include "UARTDriver.h"
7 : #include <sys/time.h>
8 : #include <fenv.h>
9 : #include <AP_BoardConfig/AP_BoardConfig.h>
10 : #if defined (__clang__) || (defined (__APPLE__) && defined (__MACH__)) || defined (__OpenBSD__)
11 : #include <stdlib.h>
12 : #else
13 : #include <malloc.h>
14 : #endif
15 : #include <AP_RCProtocol/AP_RCProtocol.h>
16 : #ifdef UBSAN_ENABLED
17 : #include <fcntl.h>
18 : #include <sanitizer/asan_interface.h>
19 : #endif
20 :
21 : using namespace HALSITL;
22 :
23 : extern const AP_HAL::HAL& hal;
24 :
25 : #ifndef SITL_STACK_CHECKING_ENABLED
26 : //#define SITL_STACK_CHECKING_ENABLED !defined(__CYGWIN__) && !defined(__CYGWIN64__)
27 : // stack checking is disabled until the memory corruption issues are
28 : // fixed with pthread_attr_setstack. These may be due to
29 : // changes in the way guard pages are handled.
30 : #define SITL_STACK_CHECKING_ENABLED 0
31 : #endif
32 :
33 : AP_HAL::Proc Scheduler::_failsafe = nullptr;
34 :
35 : AP_HAL::MemberProc Scheduler::_timer_proc[SITL_SCHEDULER_MAX_TIMER_PROCS] = {nullptr};
36 : uint8_t Scheduler::_num_timer_procs = 0;
37 : bool Scheduler::_in_timer_proc = false;
38 :
39 : AP_HAL::MemberProc Scheduler::_io_proc[SITL_SCHEDULER_MAX_TIMER_PROCS] = {nullptr};
40 : uint8_t Scheduler::_num_io_procs = 0;
41 : bool Scheduler::_in_io_proc = false;
42 : bool Scheduler::_should_exit = false;
43 :
44 : bool Scheduler::_in_semaphore_take_wait = false;
45 :
46 : Scheduler::thread_attr *Scheduler::threads;
47 : HAL_Semaphore Scheduler::_thread_sem;
48 :
49 2145 : Scheduler::Scheduler(SITL_State *sitlState) :
50 2145 : _sitlState(sitlState),
51 2145 : _stopped_clock_usec(0)
52 : {
53 2145 : }
54 :
55 : #ifdef UBSAN_ENABLED
56 : /*
57 : catch ubsan errors and append to a log file
58 : */
59 : extern "C" {
60 : void __ubsan_get_current_report_data(const char **OutIssueKind,
61 : const char **OutMessage,
62 : const char **OutFilename, unsigned *OutLine,
63 : unsigned *OutCol, char **OutMemoryAddr);
64 :
65 : void __ubsan_on_report();
66 : void __ubsan_on_report()
67 : {
68 : static int fd = -1;
69 : if (fd == -1) {
70 : const char *ubsan_log_path = getenv("UBSAN_LOG_PATH");
71 : if (ubsan_log_path == nullptr) {
72 : ubsan_log_path = "ubsan.log";
73 : }
74 : if (ubsan_log_path != nullptr) {
75 : fd = open(ubsan_log_path, O_APPEND|O_CREAT|O_WRONLY, 0644);
76 : }
77 : }
78 : if (fd != -1) {
79 : const char *OutIssueKind = nullptr;
80 : const char *OutMessage = nullptr;
81 : const char *OutFilename = nullptr;
82 : unsigned OutLine=0;
83 : unsigned OutCol=0;
84 : char *OutMemoryAddr=nullptr;
85 : __ubsan_get_current_report_data(&OutIssueKind, &OutMessage, &OutFilename,
86 : &OutLine, &OutCol, &OutMemoryAddr);
87 : dprintf(fd, "ubsan error: %s:%u:%u %s:%s\n",
88 : OutFilename, OutLine, OutCol,
89 : OutIssueKind, OutMessage);
90 : }
91 : }
92 : }
93 : #endif
94 :
95 2094 : void Scheduler::init()
96 : {
97 2094 : _main_ctx = pthread_self();
98 2094 : }
99 :
100 5713279994 : bool Scheduler::in_main_thread() const
101 : {
102 5713279994 : if (!_in_timer_proc && !_in_io_proc && pthread_self() == _main_ctx) {
103 4221205145 : return true;
104 : }
105 1492074849 : return false;
106 : }
107 :
108 : /*
109 : * semaphore_wait_hack_required - possibly move time input step
110 : * forward even if we are currently pretending to be the IO or timer
111 : * threads.
112 : *
113 : * Without this, if another thread has taken a semaphore (e.g. the
114 : * Object Avoidance thread), and an "IO process" tries to take that
115 : * semaphore with a timeout specified, then we end up not advancing
116 : * time (due to the logic in SITL_State::wait_clock) and thus taking
117 : * the semaphore never times out - meaning we essentially deadlock.
118 : */
119 109438397 : bool Scheduler::semaphore_wait_hack_required() const
120 : {
121 109438397 : if (pthread_self() != _main_ctx) {
122 : // only the main thread ever moves stuff forwards
123 109438862 : return false;
124 : }
125 :
126 0 : return _in_semaphore_take_wait;
127 : }
128 :
129 189696991 : void Scheduler::delay_microseconds(uint16_t usec)
130 : {
131 189696991 : if (_sitlState->_sitl == nullptr) {
132 : // this allows examples to run
133 2000 : hal.scheduler->stop_clock(AP_HAL::micros64()+usec);
134 2000 : return;
135 : }
136 189694991 : uint64_t start = AP_HAL::micros64();
137 : do {
138 337688757 : uint64_t dtime = AP_HAL::micros64() - start;
139 337606070 : if (dtime >= usec) {
140 189677286 : break;
141 : }
142 147928784 : _sitlState->wait_clock(start + usec);
143 148013669 : } while (true);
144 : }
145 :
146 1916382 : void Scheduler::delay(uint16_t ms)
147 : {
148 1916382 : uint32_t start = AP_HAL::millis();
149 1916381 : uint32_t now = start;
150 : do {
151 10535173 : delay_microseconds(1000);
152 10535141 : if (_min_delay_cb_ms <= (ms - (now - start))) {
153 8299869 : if (in_main_thread()) {
154 2619247 : call_delay_cb();
155 : }
156 : }
157 10535126 : now = AP_HAL::millis();
158 10534935 : } while (now - start < ms);
159 1916143 : }
160 :
161 14638 : void Scheduler::register_timer_process(AP_HAL::MemberProc proc)
162 : {
163 58537 : for (uint8_t i = 0; i < _num_timer_procs; i++) {
164 43899 : if (_timer_proc[i] == proc) {
165 0 : return;
166 : }
167 : }
168 :
169 14638 : if (_num_timer_procs < SITL_SCHEDULER_MAX_TIMER_PROCS) {
170 14638 : _timer_proc[_num_timer_procs] = proc;
171 14638 : _num_timer_procs++;
172 : }
173 : }
174 :
175 11336 : void Scheduler::register_io_process(AP_HAL::MemberProc proc)
176 : {
177 36746 : for (uint8_t i = 0; i < _num_io_procs; i++) {
178 25410 : if (_io_proc[i] == proc) {
179 0 : return;
180 : }
181 : }
182 :
183 11336 : if (_num_io_procs < SITL_SCHEDULER_MAX_TIMER_PROCS) {
184 11336 : _io_proc[_num_io_procs] = proc;
185 11336 : _num_io_procs++;
186 : }
187 : }
188 :
189 2059 : void Scheduler::register_timer_failsafe(AP_HAL::Proc failsafe, uint32_t period_us)
190 : {
191 2059 : _failsafe = failsafe;
192 2059 : }
193 :
194 2086 : void Scheduler::set_system_initialized() {
195 2086 : if (_initialized) {
196 0 : AP_HAL::panic(
197 : "PANIC: scheduler system initialized called more than once");
198 : }
199 2086 : int exceptions = FE_OVERFLOW | FE_DIVBYZERO;
200 : #ifndef __i386__
201 : // i386 with gcc doesn't work with FE_INVALID
202 2086 : exceptions |= FE_INVALID;
203 : #endif
204 : #if !defined(HAL_BUILD_AP_PERIPH)
205 2086 : if (_sitlState->_sitl == nullptr || _sitlState->_sitl->float_exception) {
206 2086 : feenableexcept(exceptions);
207 : } else {
208 0 : feclearexcept(exceptions);
209 : }
210 : #else
211 : feclearexcept(exceptions);
212 : #endif
213 2086 : _initialized = true;
214 2086 : }
215 :
216 187268324 : void Scheduler::sitl_end_atomic() {
217 187268324 : if (_nested_atomic_ctr == 0) {
218 0 : hal.serial(0)->printf("NESTED ATOMIC ERROR\n");
219 : } else {
220 187268324 : _nested_atomic_ctr--;
221 : }
222 187268324 : }
223 :
224 1639 : void Scheduler::reboot(bool hold_in_bootloader)
225 : {
226 1639 : HAL_SITL::actually_reboot();
227 0 : abort();
228 : }
229 :
230 187268325 : void Scheduler::_run_timer_procs()
231 : {
232 187268325 : if (_in_timer_proc) {
233 : // the timer calls took longer than the period of the
234 : // timer. This is bad, and may indicate a serious
235 : // driver failure. We can't just call the drivers
236 : // again, as we could run out of stack. So we only
237 : // call the _failsafe call. It's job is to detect if
238 : // the drivers or the main loop are indeed dead and to
239 : // activate whatever failsafe it thinks may help if
240 : // need be. We assume the failsafe code can't
241 : // block. If it does then we will recurse and die when
242 : // we run out of stack
243 0 : if (_failsafe != nullptr) {
244 0 : _failsafe();
245 : }
246 0 : return;
247 : }
248 187268325 : _in_timer_proc = true;
249 :
250 : // now call the timer based drivers
251 1489327529 : for (int i = 0; i < _num_timer_procs; i++) {
252 1302059204 : if (_timer_proc[i]) {
253 1302059204 : _timer_proc[i]();
254 : }
255 : }
256 :
257 : // and the failsafe, if one is setup
258 187268325 : if (_failsafe != nullptr) {
259 183880867 : _failsafe();
260 : }
261 :
262 187268325 : _in_timer_proc = false;
263 : }
264 :
265 243357862 : void Scheduler::_run_io_procs()
266 : {
267 243357862 : if (_in_io_proc) {
268 0 : return;
269 : }
270 243357862 : _in_io_proc = true;
271 :
272 : // now call the IO based drivers
273 1555468512 : for (int i = 0; i < _num_io_procs; i++) {
274 1312110650 : if (_io_proc[i]) {
275 1312110650 : _io_proc[i]();
276 : }
277 : }
278 :
279 243357862 : _in_io_proc = false;
280 :
281 2676936472 : for (uint8_t i=0; i<hal.num_serial; i++) {
282 2433578611 : hal.serial(i)->_timer_tick();
283 : }
284 243357861 : hal.storage->_timer_tick();
285 :
286 : // in lieu of a thread-per-bus:
287 243357861 : ((HALSITL::I2CDeviceManager*)(hal.i2c_mgr))->_timer_tick();
288 :
289 : #if SITL_STACK_CHECKING_ENABLED
290 : check_thread_stacks();
291 : #endif
292 :
293 : #if AP_RCPROTOCOL_ENABLED
294 243357861 : AP::RC().update();
295 : #endif
296 : }
297 :
298 : /*
299 : set simulation timestamp
300 : */
301 187279594 : void Scheduler::stop_clock(uint64_t time_usec)
302 : {
303 187279594 : _stopped_clock_usec = time_usec;
304 187279594 : if (_sitlState->_sitl != nullptr && time_usec - _last_io_run > 10000) {
305 14404915 : _last_io_run = time_usec;
306 14404915 : _run_io_procs();
307 : }
308 187279594 : }
309 :
310 : /*
311 : trampoline for thread create
312 : */
313 2428 : void *Scheduler::thread_create_trampoline(void *ctx)
314 : {
315 2428 : struct thread_attr *a = (struct thread_attr *)ctx;
316 2428 : a->thread = pthread_self();
317 2428 : a->f[0]();
318 :
319 0 : WITH_SEMAPHORE(_thread_sem);
320 0 : if (threads == a) {
321 0 : threads = a->next;
322 : } else {
323 0 : for (struct thread_attr *p=threads; p->next; p=p->next) {
324 0 : if (p->next == a) {
325 0 : p->next = p->next->next;
326 0 : break;
327 : }
328 : }
329 : }
330 0 : free(a->stack);
331 0 : free(a->f);
332 0 : delete a;
333 0 : return nullptr;
334 0 : }
335 :
336 : #ifndef PTHREAD_STACK_MIN
337 : #define PTHREAD_STACK_MIN 16384U
338 : #endif
339 :
340 : /*
341 : create a new thread
342 : */
343 2428 : bool Scheduler::thread_create(AP_HAL::MemberProc proc, const char *name, uint32_t stack_size, priority_base base, int8_t priority)
344 : {
345 2428 : WITH_SEMAPHORE(_thread_sem);
346 :
347 : // even an empty thread takes 2500 bytes on Linux, so always add 2300, giving us 200 bytes
348 : // safety margin
349 2428 : stack_size += 2300;
350 :
351 2428 : pthread_t thread {};
352 2428 : const uint32_t alloc_stack = MAX(size_t(PTHREAD_STACK_MIN),stack_size);
353 :
354 2428 : struct thread_attr *a = NEW_NOTHROW struct thread_attr;
355 2428 : if (!a) {
356 0 : return false;
357 : }
358 : // take a copy of the MemberProc, it is freed after thread exits
359 2428 : a->f = (AP_HAL::MemberProc *)malloc(sizeof(proc));
360 2428 : if (!a->f) {
361 0 : goto failed;
362 : }
363 2428 : if (posix_memalign(&a->stack, 4096, alloc_stack) != 0) {
364 0 : goto failed;
365 : }
366 2428 : if (!a->stack) {
367 0 : goto failed;
368 : }
369 2428 : memset(a->stack, stackfill, alloc_stack);
370 2428 : a->stack_min = (const uint8_t *)((((uint8_t *)a->stack) + alloc_stack) - stack_size);
371 :
372 2428 : a->stack_size = stack_size;
373 2428 : a->f[0] = proc;
374 2428 : a->name = name;
375 :
376 2428 : if (pthread_attr_init(&a->attr) != 0) {
377 0 : goto failed;
378 : }
379 : #if SITL_STACK_CHECKING_ENABLED
380 : if (pthread_attr_setstack(&a->attr, a->stack, alloc_stack) != 0) {
381 : AP_HAL::panic("Failed to set stack of size %u for thread %s", alloc_stack, name);
382 : }
383 : #endif
384 2428 : if (pthread_create(&thread, &a->attr, thread_create_trampoline, a) != 0) {
385 0 : goto failed;
386 : }
387 :
388 : #if !defined(__APPLE__) && !defined(__OpenBSD__)
389 2428 : pthread_setname_np(thread, name);
390 : #endif
391 :
392 2428 : a->next = threads;
393 2428 : threads = a;
394 2428 : return true;
395 :
396 0 : failed:
397 0 : if (a->stack) {
398 0 : free(a->stack);
399 : }
400 0 : if (a->f) {
401 0 : free(a->f);
402 : }
403 0 : delete a;
404 0 : return false;
405 2428 : }
406 :
407 : /*
408 : check for stack overflow
409 : */
410 0 : void Scheduler::check_thread_stacks(void)
411 : {
412 0 : WITH_SEMAPHORE(_thread_sem);
413 0 : for (struct thread_attr *p=threads; p; p=p->next) {
414 0 : const uint8_t ncheck = 8;
415 0 : for (uint8_t i=0; i<ncheck; i++) {
416 0 : if (p->stack_min[i] != stackfill) {
417 0 : AP_HAL::panic("stack overflow in thread %s", p->name);
418 : }
419 : }
420 : }
421 0 : }
422 :
423 : // get the name of the current thread, or nullptr if not known
424 0 : const char *Scheduler::get_current_thread_name(void) const
425 : {
426 0 : const pthread_t self = pthread_self();
427 0 : for (struct thread_attr *a=threads; a; a=a->next) {
428 0 : if (a->thread == self) {
429 0 : return a->name;
430 : }
431 : }
432 0 : return nullptr;
433 : }
|
