Line data Source code
1 : /*
2 : * Copyright (C) 2016 Intel Corporation. All rights reserved.
3 : *
4 : * This file is free software: you can redistribute it and/or modify it
5 : * under the terms of the GNU General Public License as published by the
6 : * Free Software Foundation, either version 3 of the License, or
7 : * (at your option) any later version.
8 : *
9 : * This file is distributed in the hope that it will be useful, but
10 : * WITHOUT ANY WARRANTY; without even the implied warranty of
11 : * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
12 : * See the GNU General Public License for more details.
13 : *
14 : * You should have received a copy of the GNU General Public License along
15 : * with this program. If not, see <http://www.gnu.org/licenses/>.
16 : */
17 : #include "Thread.h"
18 :
19 : #include <alloca.h>
20 : #include <limits.h>
21 : #include <sys/types.h>
22 : #include <stdio.h>
23 : #include <unistd.h>
24 : #include <utility>
25 :
26 : #include <AP_HAL/AP_HAL.h>
27 : #include <AP_Math/AP_Math.h>
28 : #include "Scheduler.h"
29 :
30 : #define STACK_POISON 0xBEBACAFE
31 :
32 : extern const AP_HAL::HAL &hal;
33 :
34 : namespace Linux {
35 :
36 :
37 4 : void *Thread::_run_trampoline(void *arg)
38 : {
39 4 : Thread *thread = static_cast<Thread *>(arg);
40 4 : thread->_poison_stack();
41 4 : thread->_run();
42 :
43 4 : if (thread->_auto_free) {
44 0 : delete thread;
45 : }
46 :
47 4 : return nullptr;
48 : }
49 :
50 2 : bool Thread::_run()
51 : {
52 2 : if (!_task) {
53 0 : return false;
54 : }
55 :
56 2 : _task();
57 :
58 2 : return true;
59 : }
60 :
61 : /* Round up to the specified alignment.
62 : *
63 : * Let u be the address p rounded up to the alignment a. Then:
64 : * u = p + a - 1 - r, where r = (p + a - 1) % a
65 : *
66 : * If p % a = 0, i.e. if p is already aligned, then:
67 : * r = a - 1 ==> u = p
68 : *
69 : * Otherwise:
70 : * r = p % a -1 ==> u = p + a - p % a
71 : *
72 : * p can be written p = q + p % a, where q is rounded down to the
73 : * alignment a. Then u = q + a.
74 : */
75 4 : static inline void *align_to(void *p, size_t align)
76 : {
77 4 : return (void *)(((uintptr_t)p + align - 1) & ~(align - 1));
78 : }
79 :
80 4 : void Thread::_poison_stack()
81 : {
82 : pthread_attr_t attr;
83 : size_t stack_size, guard_size;
84 : void *stackp;
85 : uint32_t *p, *curr, *begin, *end;
86 :
87 : // `pthread_self` should be used here since _ctx could be not initialized
88 : // in a race condition.
89 8 : if (pthread_getattr_np(pthread_self(), &attr) != 0 ||
90 8 : pthread_attr_getstack(&attr, &stackp, &stack_size) != 0 ||
91 4 : pthread_attr_getguardsize(&attr, &guard_size) != 0) {
92 0 : return;
93 : }
94 :
95 4 : stack_size /= sizeof(uint32_t);
96 4 : guard_size /= sizeof(uint32_t);
97 :
98 : /* The stack either grows upward or downard. The guard part always
99 : * protects the end */
100 4 : end = (uint32_t *)stackp;
101 4 : begin = end + stack_size;
102 4 : curr = (uint32_t *)align_to(alloca(sizeof(uint32_t)), alignof(uint32_t));
103 :
104 : /* if curr is closer to @end, the stack actually grows from low to high
105 : * virtual address: this is because this function should be executing very
106 : * early in the thread's life and close to the thread creation, assuming
107 : * the actual stack size is greater than the guard size and the stack
108 : * until now is resonably small */
109 4 : if (abs(curr - begin) > abs(curr - end)) {
110 0 : std::swap(end, begin);
111 0 : end -= guard_size;
112 :
113 0 : for (p = end; p > curr; p--) {
114 0 : *p = STACK_POISON;
115 : }
116 : } else {
117 4 : end += guard_size;
118 :
119 : /* we aligned curr to the up boundary, make sure this didn't cause us
120 : * to lose some bytes */
121 4 : curr--;
122 :
123 8379760 : for (p = end; p < curr; p++) {
124 8379756 : *p = STACK_POISON;
125 : }
126 : }
127 :
128 4 : _stack_debug.start = begin;
129 4 : _stack_debug.end = end;
130 : }
131 :
132 0 : size_t Thread::get_stack_usage()
133 : {
134 : uint32_t *p;
135 0 : size_t result = 0;
136 :
137 : /* Make sure we are tracking usage for this thread */
138 0 : if (_stack_debug.start == 0 || _stack_debug.end == 0) {
139 0 : return 0;
140 : }
141 :
142 0 : if (_stack_debug.start < _stack_debug.end) {
143 0 : for (p = _stack_debug.end; p > _stack_debug.start; p--) {
144 0 : if (*p != STACK_POISON) {
145 0 : break;
146 : }
147 : }
148 0 : result = p - _stack_debug.start;
149 : } else {
150 0 : for (p = _stack_debug.end; p < _stack_debug.start; p++) {
151 0 : if (*p != STACK_POISON) {
152 0 : break;
153 : }
154 : }
155 0 : result = _stack_debug.start - p;
156 : }
157 :
158 0 : return result;
159 : }
160 :
161 4 : bool Thread::start(const char *name, int policy, int prio)
162 : {
163 4 : if (_started) {
164 0 : return false;
165 : }
166 :
167 4 : struct sched_param param = { .sched_priority = prio };
168 : pthread_attr_t attr;
169 : int r;
170 :
171 4 : pthread_attr_init(&attr);
172 :
173 : /*
174 : we need to run as root to get realtime scheduling. Allow it to
175 : run as non-root for debugging purposes, plus to allow the Replay
176 : tool to run
177 : */
178 4 : if (geteuid() == 0) {
179 0 : if ((r = pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED)) != 0 ||
180 0 : (r = pthread_attr_setschedpolicy(&attr, policy)) != 0 ||
181 0 : (r = pthread_attr_setschedparam(&attr, ¶m)) != 0) {
182 0 : AP_HAL::panic("Failed to set attributes for thread '%s': %s",
183 : name, strerror(r));
184 : }
185 : }
186 :
187 4 : if (_stack_size) {
188 0 : if (pthread_attr_setstacksize(&attr, _stack_size) != 0) {
189 0 : return false;
190 : }
191 : }
192 :
193 4 : r = pthread_create(&_ctx, &attr, &Thread::_run_trampoline, this);
194 4 : if (r != 0) {
195 0 : AP_HAL::panic("Failed to create thread '%s': %s",
196 : name, strerror(r));
197 : }
198 4 : pthread_attr_destroy(&attr);
199 :
200 4 : if (name) {
201 0 : pthread_setname_np(_ctx, name);
202 : }
203 :
204 4 : _started = true;
205 :
206 4 : return true;
207 : }
208 :
209 0 : bool Thread::is_current_thread()
210 : {
211 0 : return pthread_equal(pthread_self(), _ctx);
212 : }
213 :
214 2 : bool Thread::join()
215 : {
216 : void *ret;
217 :
218 2 : if (_ctx == 0) {
219 0 : return false;
220 : }
221 :
222 4 : if (pthread_join(_ctx, &ret) != 0 ||
223 2 : (intptr_t)ret != 0) {
224 0 : return false;
225 : }
226 :
227 2 : return true;
228 : }
229 :
230 :
231 2 : bool PeriodicThread::set_rate(uint32_t rate_hz)
232 : {
233 2 : if (_started || rate_hz == 0) {
234 1 : return false;
235 : }
236 :
237 1 : _period_usec = hz_to_usec(rate_hz);
238 :
239 1 : return true;
240 : }
241 :
242 0 : bool Thread::set_stack_size(size_t stack_size)
243 : {
244 0 : if (_started) {
245 0 : return false;
246 : }
247 :
248 0 : _stack_size = MAX(stack_size, (size_t) PTHREAD_STACK_MIN);
249 :
250 0 : return true;
251 : }
252 :
253 1 : bool PeriodicThread::_run()
254 : {
255 1 : if (_period_usec == 0) {
256 0 : return false;
257 : }
258 :
259 1 : uint64_t next_run_usec = AP_HAL::micros64() + _period_usec;
260 :
261 10 : while (!_should_exit) {
262 9 : uint64_t dt = next_run_usec - AP_HAL::micros64();
263 9 : if (dt > _period_usec) {
264 : // we've lost sync - restart
265 0 : next_run_usec = AP_HAL::micros64();
266 : } else {
267 9 : Scheduler::from(hal.scheduler)->microsleep(dt);
268 : }
269 9 : next_run_usec += _period_usec;
270 :
271 9 : _task();
272 : }
273 :
274 1 : _started = false;
275 1 : _should_exit = false;
276 :
277 1 : return true;
278 : }
279 :
280 1 : bool PeriodicThread::stop()
281 : {
282 1 : if (!is_started()) {
283 0 : return false;
284 : }
285 :
286 1 : _should_exit = true;
287 :
288 1 : return true;
289 : }
290 :
291 : }
|
